Refactor moveit config and launch

start-jsk · Jan 18, 2018 · a5eef1c · a5eef1c
1 parent 7b93999
commit a5eef1c
Show file tree

Hide file tree

Showing 22 changed files with 381 additions and 49 deletions.
diff --git a/jsk_arc2017_baxter/launch/setup/baxter.launch b/jsk_arc2017_baxter/launch/setup/baxter.launch
@@ -58,7 +58,7 @@
         pkg="jsk_baxter_startup" type="xdisplay_image_topic.py"
         args="/desktop_bg_publisher/output" />
 
-  <include file="$(find jsk_arc2017_baxter)/launch/setup/include/baxter_moveit.launch" if="$(arg moveit)">
+  <include file="$(find jsk_arc2017_baxter)/launch/setup/include/moveit/baxter/baxter_moveit.launch" if="$(arg moveit)">
     <arg name="load_robot_description" value="false" />
     <arg name="debug" value="$(arg debug)" />
   </include>

diff --git a/jsk_arc2017_baxter/launch/setup/baxterlgv7.launch b/jsk_arc2017_baxter/launch/setup/baxterlgv7.launch
@@ -58,7 +58,7 @@
         pkg="jsk_baxter_startup" type="xdisplay_image_topic.py"
         args="/desktop_bg_publisher/output" />
 
-  <include file="$(find jsk_arc2017_baxter)/launch/setup/include/baxterlgv7_moveit.launch" if="$(arg moveit)">
+  <include file="$(find jsk_arc2017_baxter)/launch/setup/include/moveit/baxterlgv7/baxterlgv7_moveit.launch" if="$(arg moveit)">
     <arg name="load_robot_description" value="false" />
     <arg name="debug" value="$(arg debug)" />
   </include>

diff --git a/jsk_arc2017_baxter/launch/setup/include/baxterlgv7_planning_context.launch b/jsk_arc2017_baxter/launch/setup/include/baxterlgv7_planning_context.launch
diff --git a/...launch/setup/include/baxter_moveit.launch → ...nclude/moveit/baxter/baxter_moveit.launch b/...launch/setup/include/baxter_moveit.launch → ...nclude/moveit/baxter/baxter_moveit.launch
@@ -4,12 +4,14 @@
   <arg name="debug" default="false" />
 
   <!-- Load the URDF, SRDF and other .yaml configuration files on the param server -->
-  <include file="$(find jsk_arc2017_baxter)/launch/setup/include/planning_context.launch">
+  <include file="$(find jsk_arc2017_baxter)/launch/setup/include/moveit/common/planning_context.launch">
+    <arg name="robot" value="baxter"/>
     <arg name="load_robot_description" value="$(arg load_robot_description)"/>
   </include>
 
   <!-- Run the main MoveIt executable without trajectory execution (we do not have controllers configured by default) -->
-  <include file="$(find jsk_arc2017_baxter)/launch/setup/include/move_group.launch">
+  <include file="$(find jsk_arc2017_baxter)/launch/setup/include/moveit/common/move_group.launch">
+    <arg name="robot" value="baxter"/>
     <arg name="allow_trajectory_execution" value="true"/>
     <arg name="info" value="$(arg debug)"/>
     <arg name="debug" value="$(arg debug)"/>

diff --git a/...p/include/trajectory_execution.launch.xml → ...er/baxter_trajectory_execution.launch.xml b/...p/include/trajectory_execution.launch.xml → ...er/baxter_trajectory_execution.launch.xml
@@ -13,6 +13,7 @@
 
   <!-- Load the robot specific controller manager; this sets the moveit_controller_manager ROS parameter -->
   <arg name="moveit_controller_manager" default="baxter" />
-  <include file="$(find jsk_arc2017_baxter)/launch/setup/include/$(arg moveit_controller_manager)_moveit_controller_manager.launch.xml" />
-
+  <include file="$(find jsk_arc2017_baxter)/launch/setup/include/moveit/common/moveit_controller_manager.launch.xml">
+    <arg name="robot" value="$(arg moveit_controller_manager)"/>
+  </include>
 </launch>
diff --git a/...ch/setup/include/baxterlgv7_moveit.launch → ...oveit/baxterlgv7/baxterlgv7_moveit.launch b/...ch/setup/include/baxterlgv7_moveit.launch → ...oveit/baxterlgv7/baxterlgv7_moveit.launch
@@ -4,12 +4,14 @@
   <arg name="debug" default="false" />
 
   <!-- Load the URDF, SRDF and other .yaml configuration files on the param server -->
-  <include file="$(find jsk_arc2017_baxter)/launch/setup/include/baxterlgv7_planning_context.launch">
+  <include file="$(find jsk_arc2017_baxter)/launch/setup/include/moveit/common/planning_context.launch">
+    <arg name="robot" value="baxterlgv7"/>
     <arg name="load_robot_description" value="$(arg load_robot_description)"/>
   </include>
 
   <!-- Run the main MoveIt executable without trajectory execution (we do not have controllers configured by default) -->
-  <include file="$(find jsk_arc2017_baxter)/launch/setup/include/move_group.launch">
+  <include file="$(find jsk_arc2017_baxter)/launch/setup/include/moveit/common/move_group.launch">
+    <arg name="robot" value="baxterlgv7"/>
     <arg name="allow_trajectory_execution" value="true"/>
     <arg name="info" value="$(arg debug)"/>
     <arg name="debug" value="$(arg debug)"/>

diff --git a/..._baxter/launch/setup/include/moveit/baxterlgv7/baxterlgv7_trajectory_execution.launch.xml b/..._baxter/launch/setup/include/moveit/baxterlgv7/baxterlgv7_trajectory_execution.launch.xml
@@ -0,0 +1,19 @@
+<launch>
+
+  <!-- This file makes it easy to include the settings for trajectory execution  -->
+
+  <!-- Flag indicating whether MoveIt! is allowed to load/unload  or switch controllers -->
+  <arg name="moveit_manage_controllers" default="true"/>
+  <param name="moveit_manage_controllers" value="$(arg moveit_manage_controllers)"/>
+
+  <!-- When determining the expected duration of a trajectory, this multiplicative factor is applied to get the allowed duration of execution -->
+  <param name="trajectory_execution/allowed_execution_duration_scaling" value="1.2"/> <!-- default 1.2 -->
+  <!-- Allow more than the expected execution time before triggering a trajectory cancel (applied after scaling) -->
+  <param name="trajectory_execution/allowed_goal_duration_margin" value="0.5"/> <!-- default 0.5 -->
+
+  <!-- Load the robot specific controller manager; this sets the moveit_controller_manager ROS parameter -->
+  <arg name="moveit_controller_manager" default="baxterlgv7" />
+  <include file="$(find jsk_arc2017_baxter)/launch/setup/include/moveit/common/moveit_controller_manager.launch.xml">
+    <arg name="robot" value="$(arg moveit_controller_manager)"/>
+  </include>
+</launch>
diff --git a/...er/launch/setup/include/move_group.launch → ...p/include/moveit/common/move_group.launch b/...er/launch/setup/include/move_group.launch → ...p/include/moveit/common/move_group.launch
@@ -1,5 +1,7 @@
 <launch>
 
+  <arg name="robot" default="baxter"/>
+
   <!-- GDB Debug Option -->
   <arg name="debug" default="false" />
   <arg unless="$(arg debug)" name="launch_prefix" value="" />
@@ -18,12 +20,12 @@
   <arg name="publish_monitored_planning_scene" default="true"/>
 
   <!-- Planning Functionality -->
-  <include ns="move_group" file="$(find jsk_arc2017_baxter)/launch/setup/include/ompl_planning_pipeline.launch" />
+  <include ns="move_group" file="$(find jsk_arc2017_baxter)/launch/setup/include/moveit/common/ompl_planning_pipeline.launch" />
 
   <!-- Trajectory Execution Functionality -->
-  <include ns="move_group" file="$(find jsk_arc2017_baxter)/launch/setup/include/trajectory_execution.launch.xml" if="$(arg allow_trajectory_execution)">
+  <include ns="move_group" file="$(find jsk_arc2017_baxter)/launch/setup/include/moveit/$(arg robot)/$(arg robot)_trajectory_execution.launch.xml" if="$(arg allow_trajectory_execution)">
     <arg name="moveit_manage_controllers" value="true" />
-    <arg name="moveit_controller_manager" value="baxter"/>
+    <arg name="moveit_controller_manager" value="$(arg robot)"/>
   </include>
 
   <!-- Start the actual move_group node/action server -->

diff --git a/...xter_moveit_controller_manager.launch.xml → ...mmon/moveit_controller_manager.launch.xml b/...xter_moveit_controller_manager.launch.xml → ...mmon/moveit_controller_manager.launch.xml
@@ -1,5 +1,7 @@
 <launch>
 
+  <arg name="robot" default="baxter"/>
+
   <!-- Set the param that trajectory_execution_manager needs to find the controller plugin -->
   <arg name="moveit_controller_manager" default="moveit_simple_controller_manager/MoveItSimpleControllerManager" />
   <param name="moveit_controller_manager" value="$(arg moveit_controller_manager)"/>
@@ -16,6 +18,6 @@
   <param name="use_controller_manager" value="$(arg use_controller_manager)" />
 
   <!-- load controller_list and controller_manager_ns -->
-  <rosparam file="$(find jsk_arc2017_baxter)/robots/moveit_config/baxter_controllers.yaml"/>
+  <rosparam file="$(find jsk_arc2017_baxter)/robots/moveit_config/$(arg robot)/controllers.yaml"/>
 
 </launch>
diff --git a/...tup/include/ompl_planning_pipeline.launch → ...veit/common/ompl_planning_pipeline.launch b/...tup/include/ompl_planning_pipeline.launch → ...veit/common/ompl_planning_pipeline.launch
@@ -18,6 +18,6 @@
   <param name="start_state_max_bounds_error" value="$(arg start_state_max_bounds_error)" />
 
   <rosparam command="load" file="$(find baxter_moveit_config)/config/kinematics.yaml"/>
-  <rosparam command="load" file="$(find jsk_arc2017_baxter)/robots/moveit_config/ompl_planning.yaml"/>
+  <rosparam command="load" file="$(find jsk_arc2017_baxter)/robots/moveit_config/common/ompl_planning.yaml"/>
 
 </launch>
diff --git a/...nch/setup/include/planning_context.launch → ...ude/moveit/common/planning_context.launch b/...nch/setup/include/planning_context.launch → ...ude/moveit/common/planning_context.launch
@@ -1,20 +1,23 @@
 <launch>
+
+  <arg name="robot" default="baxter"/>
+
   <!-- By default we do not overwrite the URDF. Change the following to true to change the default behavior -->
   <arg name="load_robot_description" default="false"/>
 
   <!-- The name of the parameter under which the URDF is loaded -->
   <arg name="robot_description" default="robot_description"/>
 
   <!-- Load universal robot description format (URDF) -->
-  <param if="$(arg load_robot_description)" name="$(arg robot_description)" command="rosrun xacro xacro --inorder $(find jsk_arc2017_baxter)/robots/baxter.xacro"/>
+  <param if="$(arg load_robot_description)" name="$(arg robot_description)" command="rosrun xacro xacro --inorder $(find jsk_arc2017_baxter)/robots/$(arg robot).xacro"/>
 
   <!-- The semantic description that corresponds to the URDF -->
   <param name="$(arg robot_description)_semantic"
-      command="rosrun xacro xacro --inorder $(find jsk_arc2017_baxter)/robots/moveit_config/baxter.srdf.xacro"/>
+      command="rosrun xacro xacro --inorder $(find jsk_arc2017_baxter)/robots/moveit_config/$(arg robot)/$(arg robot).srdf.xacro"/>
 
   <!-- Load updated joint limits (override information from URDF) -->
   <group ns="$(arg robot_description)_planning">
-    <rosparam command="load" file="$(find jsk_arc2017_baxter)/robots/moveit_config/joint_limits.yaml"/>
+    <rosparam command="load" file="$(find jsk_arc2017_baxter)/robots/moveit_config/$(arg robot)/joint_limits.yaml"/>
   </group>
 
   <!-- Load default settings for kinematics; these settings are overridden by settings in a node's namespace -->

diff --git a/...er/robots/moveit_config/baxter.srdf.xacro → ...ts/moveit_config/baxter/baxter.srdf.xacro b/...er/robots/moveit_config/baxter.srdf.xacro → ...ts/moveit_config/baxter/baxter.srdf.xacro
@@ -2,12 +2,12 @@
 <robot name="baxter" xmlns:xacro="http://www.ros.org/wiki/xacro">
   <xacro:arg name="left_tip_name" default="left_gripper_pad_with_base"/>
   <xacro:arg name="right_tip_name" default="right_gripper_pad_with_base"/>
-  <xacro:include filename="$(find jsk_arc2017_baxter)/robots/moveit_config/baxter_base.srdf.xacro" />
+  <xacro:include filename="$(find jsk_arc2017_baxter)/robots/moveit_config/baxter/baxter_base.srdf.xacro" />
   <xacro:baxter_base left_tip_name="$(arg left_tip_name)" right_tip_name="$(arg right_tip_name)"/>
   <!--Left End Effector Collisions-->
-  <xacro:include filename="$(find jsk_arc2017_baxter)/robots/moveit_config/gripper_v6.srdf.xacro" />
+  <xacro:include filename="$(find jsk_arc2017_baxter)/robots/moveit_config/common/gripper_v6.srdf.xacro" />
   <xacro:vacuum_gripper side="left"/>
   <!--Right End Effector Collisions-->
-  <xacro:include filename="$(find jsk_arc2017_baxter)/robots/moveit_config/gripper_v6.srdf.xacro" />
+  <xacro:include filename="$(find jsk_arc2017_baxter)/robots/moveit_config/common/gripper_v6.srdf.xacro" />
   <xacro:vacuum_gripper side="right"/>
 </robot>
diff --git a/...bots/moveit_config/baxter_base.srdf.xacro → ...veit_config/baxter/baxter_base.srdf.xacro b/...bots/moveit_config/baxter_base.srdf.xacro → ...veit_config/baxter/baxter_base.srdf.xacro
diff --git a/...ots/moveit_config/baxter_controllers.yaml → ...ots/moveit_config/baxter/controllers.yaml b/...ots/moveit_config/baxter_controllers.yaml → ...ots/moveit_config/baxter/controllers.yaml
diff --git a/...er/robots/moveit_config/joint_limits.yaml → ...ts/moveit_config/baxter/joint_limits.yaml b/...er/robots/moveit_config/joint_limits.yaml → ...ts/moveit_config/baxter/joint_limits.yaml
diff --git a/...obots/moveit_config/baxterlgv7.srdf.xacro → ...t_config/baxterlgv7/baxterlgv7.srdf.xacro b/...obots/moveit_config/baxterlgv7.srdf.xacro → ...t_config/baxterlgv7/baxterlgv7.srdf.xacro
@@ -1,13 +1,13 @@
 <?xml version="1.0" ?>
-<robot name="baxter" xmlns:xacro="http://www.ros.org/wiki/xacro">
+<robot name="baxterlgv7" xmlns:xacro="http://www.ros.org/wiki/xacro">
   <xacro:arg name="left_tip_name" default="left_gripper_pad_with_base"/>
   <xacro:arg name="right_tip_name" default="right_gripper_pad_with_base"/>
-  <xacro:include filename="$(find jsk_arc2017_baxter)/robots/moveit_config/baxter_base.srdf.xacro" />
-  <xacro:baxter_base left_tip_name="$(arg left_tip_name)" right_tip_name="$(arg right_tip_name)"/>
+  <xacro:include filename="$(find jsk_arc2017_baxter)/robots/moveit_config/baxterlgv7/baxterlgv7_base.srdf.xacro" />
+  <xacro:baxterlgv7_base left_tip_name="$(arg left_tip_name)" right_tip_name="$(arg right_tip_name)"/>
   <!--Left End Effector Collisions-->
-  <xacro:include filename="$(find jsk_arc2017_baxter)/robots/moveit_config/gripper_v7.srdf.xacro" />
+  <xacro:include filename="$(find jsk_arc2017_baxter)/robots/moveit_config/common/gripper_v7.srdf.xacro" />
   <xacro:vacuum_gripper side="left"/>
   <!--Right End Effector Collisions-->
-  <xacro:include filename="$(find jsk_arc2017_baxter)/robots/moveit_config/gripper_v6.srdf.xacro" />
+  <xacro:include filename="$(find jsk_arc2017_baxter)/robots/moveit_config/common/gripper_v6.srdf.xacro" />
   <xacro:vacuum_gripper side="right"/>
 </robot>