Simulation branch

.gitignore

@@ -1,3 +1,5 @@
+.vscode/
+*.pyc
 match_amc/scripts/path_files/__pycache__/circle_mirx.cpython-38.pyc
 match_amc/scripts/path_files/__pycache__/circle_miry.cpython-38.pyc
 match_amc/scripts/path_files/__pycache__/circle_urx.cpython-38.pyc

journal_experiments/TODO.md

@@ -0,0 +1,26 @@
+# TODO for SImualtion of journal_experiments
+
+## Launch
+
+```bash
+roslaunch journal_experiments mur_sim.launch
+roslaunch journal_experiments start_experiment.launch
+```
+
+wait for URs to reach home pose, then:
+
+```bash
+roslaunch journal_experiments twist_sim.launch
+```
+
+## Problems
+- Twist Controller: orientation control correct?
+
+- No PointCloud!
+  - lateral_nozzle_pose_override is set to 0 in control_ur
+
+## Future
+- All in namespace like move_to_start_pose
+- Include speeds
+  - calc speed mir/ur in Parse Path
+  - change ur_control accordingly

journal_experiments/launch/mur_sim.launch

@@ -0,0 +1,13 @@
+<launch>
+    <arg name="gui" default="false"/>
+
+    <include file="$(find match_gazebo)/launch/big_square.launch" >
+        <arg name="gui" value="$(arg gui)"/>
+    </include>
+
+    <include file="$(find mur_launch_sim)/launch/mur_620.launch">
+        <arg name="tf_prefix" value="mur620c"/>
+    </include>
+
+    <!-- Launch twist_sim as well (after homing finished), or set controllers at launch with args in mur620.launch-->
+</launch>

journal_experiments/launch/start_experiment.launch

@@ -1,5 +1,36 @@
 <launch>
+<arg name="sim" default="true" />
 
+<group if="$(arg sim)">
+    <rosparam>
+        mur620c/move_to_start_pose/mir_cmd_vel_topic: "/mur620c/mobile_base_controller/cmd_vel"
+        move_ur_to_start_pose/ur_command_topic: "/mur620c/UR10_l/twist_controller/command_safe"
+        move_ur_to_start_pose/ur_pose_topic: "/mur620c/UR10_l/ur_calibrated_pose"
+        move_ur_to_start_pose/ur_base_link_frame_id: "mur620c/UR10_l/base_link"
+
+        move_ur_to_start_pose/ur_target_tolerance_trans: 0.1
+        move_ur_to_start_pose/ur_target_tolerance_rot: 0.5
+
+        move_ur_to_start_pose/Kpx: 0.5
+        move_ur_to_start_pose/Kpy: 0.5
+        move_ur_to_start_pose/Kpz: 0.5
+        move_ur_to_start_pose/Kp_phi: 0.1
+
+        control_ur/mir_cmd_vel_topic: "/mur620c/mobile_base_controller/cmd_vel"
+        control_ur/ur_command_topic: "/mur620c/UR10_l/twist_controller/command_safe"
+        control_ur/mir_pose_topic : /mur620c/mir_pose_simple
+        control_ur/ur_pose_topic: "/mur620c/UR10_l/ur_calibrated_pose"
+        control_ur/ur_base_link_frame_id: "mur620c/UR10_l/base_link"
+
+        control_mir/mir_cmd_vel_topic_relative: "mobile_base_controller/cmd_vel"
+
+        calibrated_pose_fake/namespace: "mur620c/UR10_l/"
+    </rosparam>
+    <node name="calibrated_pose_fake" pkg="journal_experiments" type="calibrated_pose_fake.py" output="screen" />
+
+    <node pkg="tf2_ros" type="static_transform_publisher" name="world_transform" args="0 0 0 0 0 0 mur620c/UR10_l/tool0 sensor_frame" output="screen"/>
+
+</group>
 <!-- start the state machine -->
 <node pkg="journal_experiments" type="state_machine.py" name="state_machine" output="screen" />
 

journal_experiments/launch/twist_sim.launch

@@ -0,0 +1,25 @@
+<launch>
+    <group ns="mur620c">
+    <!-- spawn right controller for twist_controller: -->
+    <node name="controller_stop" pkg="controller_manager" type="controller_manager" respawn="false" output="screen" args="stop UR10_l/arm_controller" />
+    <node name="controller_start" pkg="controller_manager" type="controller_manager" respawn="false" output="screen" args="start joint_group_vel_controller_l/unsafe" />
+
+
+    <node name="twist_controller" pkg="ur_controllers_match" type="twist_controller.py" output="screen">
+        <param name="ur_ns" type="string" value="mur620c" />
+        <param name="prefix_ur" type="string" value="UR10_l/" />
+        <param name="prefix_mir" type="string" value="" />
+
+        <remap from="twist_command" to="/mur620c/UR10_l/twist_controller/command_safe" />
+        <remap from="/mur620c/UR10_l/joint_group_vel_controller/command" to="/mur620c/joint_group_vel_controller_l/unsafe/command" />
+    </node>
+    </group>
+
+    <!-- set parameters for state_machine: -->
+    <rosparam>
+        ur_command_topic: "/mur620c/UR10_l/twist_command"
+        mir_pose_topic: "/mur620c/mir_pose_simple"
+        mir_cmd_vel_topic: "/mur620c/mobile_base_controller/cmd_vel"
+    </rosparam>
+
+</launch>