Merge pull request #439 from PickNikRobotics/record-traj-wp

Moved waypoint and planes for square trajectory

Author: MikeWrock

src/lab_sim/objectives/move_along_square.xml

@@ -1,20 +1,23 @@
+<?xml version="1.0" encoding="UTF-8" ?>
 <root BTCPP_format="4" main_tree_to_execute="Move Along Square">
-  <!-- ////////// -->
+  <!--//////////-->
   <BehaviorTree ID="Move Along Square" _description="Move along a square path">
     <Control ID="Sequence">
       <Action
         ID="InitializeMTCTask"
         controller_names="joint_trajectory_controller"
         task="{mtc_task}"
         task_id="move_along_axis"
+        timeout="-1.000000"
+        trajectory_monitoring="false"
       />
       <Action ID="SetupMTCCurrentState" task="{mtc_task}" />
       <Action
         ID="SetupMTCMoveAlongFrameAxis"
         acceleration_scale="1.0"
         axis_frame="world"
-        axis_y="0.0"
-        axis_z="1.0"
+        axis_y="1.0"
+        axis_z="0.0"
         hand_frame="tool0"
         ignore_environment_collisions="false"
         max_distance="0.2"
@@ -43,8 +46,8 @@
         ID="SetupMTCMoveAlongFrameAxis"
         acceleration_scale="1.0"
         axis_frame="world"
-        axis_y="0.0"
-        axis_z="-1.0"
+        axis_y="-1.0"
+        axis_z="0.0"
         hand_frame="tool0"
         ignore_environment_collisions="false"
         max_distance="0.2"

src/lab_sim/objectives/record_square_trajectory.xml

@@ -33,21 +33,27 @@
             keep_orientation_tolerance="0.05"
             link_padding="0.01"
             velocity_scale_factor="1.0"
-            waypoint_name="Flask 3a - Pre Pick"
+            waypoint_name="Look at Base"
+            seed="0"
           />
           <Action
             ID="CallTriggerService"
             service_name="/start_recording_joint_trajectory"
+            response_timeout="3.000000"
+            wait_for_server_available_timeout="3.000000"
           />
           <SubTree ID="Move Along Square" _collapsed="true" />
           <Action
             ID="CallTriggerService"
             service_name="/stop_recording_joint_trajectory"
+            response_timeout="3.000000"
+            wait_for_server_available_timeout="3.000000"
           />
           <Action
             ID="SaveJointTrajectoryToYaml"
             message="{joint_trajectory_msg}"
             yaml_filename="~/user_ws/src/lab_sim/objectives/square_traj"
+            overwrite="true"
           />
         </Control>
       </Control>