[sot_tiago_bringup] Facilitate usage of different tiago models

sot_tiago_bringup/CMakeLists.txt

@@ -11,23 +11,8 @@ catkin_package()
 
 #setup_project()
 
-INSTALL(
-  FILES 
-  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/config
-)
-
-INSTALL(
-  FILES
-  launch/geometric_simu.launch
-  launch/geometric_simu_context.launch
-  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch
-)
-
-INSTALL(
-  FILES
-  sot/tiago.yaml
-  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/sot
-)
+INSTALL(DIRECTORY launch
+  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})
 
 INSTALL(
   FILES

sot_tiago_bringup/launch/geometric_simu.launch

@@ -12,6 +12,7 @@
 
   <include file="$(find sot_tiago_bringup)/launch/geometric_simu_context.launch">
     <arg name="end_effector" value="$(arg end_effector)"/>
+    <arg name="use_mobile_base" value="$(arg use_mobile_base)"/>
   </include>
 
   <param name="/sot_controller/use_mobile_base" value="$(arg use_mobile_base)"/>
@@ -22,8 +23,7 @@
         pkg="dynamic_graph_bridge"
         type="geometric_simu"
         args=" --input-file $(arg libsot)"
-        launch-prefix="$(arg sot-launch-prefix)"
-	respawn="true">
+        launch-prefix="$(arg sot-launch-prefix)" >
     <param name="/sot/dg/geometric_simu" value="" />
   </node>
 

sot_tiago_bringup/launch/geometric_simu_context.launch

@@ -3,6 +3,7 @@
   -->
 <launch>
   <arg name="end_effector" doc="one of false, pal-gripper, pal-hey5, schunk-wsg"/>
+  <arg name="use_mobile_base" doc="Whether SoT should control the mobile base"/>
 
    <!-- Load robot model. -->
   <!--
@@ -12,11 +13,20 @@
   <param name="robot_description"
     command="$(find xacro)/xacro.py '$(find tiago_description)/robots/tiago.urdf.xacro' end_effector:=$(arg end_effector)"/>
 
-
-
-   <!-- Load robot sot params. -->
-  <rosparam command="load"
-	 file="$(find sot_tiago_bringup)/sot/tiago.yaml" />
+  <arg     if="$(arg use_mobile_base) " name="mobile_base_joint_names" value="'wheel_left_joint', 'wheel_right_joint', " />
+  <arg unless="$(arg use_mobile_base) " name="mobile_base_joint_names" value="" />
+  <arg if="$(eval arg('end_effector') == 'schunk-wsg')" name="gripper_joint_names" value="'gripper_finger_joint', " />
+  <arg if="$(eval arg('end_effector') == 'pal-hey5'  )" name="gripper_joint_names" value="'hand_index_joint', 'hand_mrl_joint', 'hand_thumb_joint', " />
+
+  <!-- Load robot sot params. -->
+  <group ns="sot">
+    <rosparam param="tf_base_link">base_footprint</rosparam>
+    <rosparam param="state_vector_map" subst_value="True">[ $(arg mobile_base_joint_names) "torso_lift_joint", "arm_1_joint", "arm_2_joint", "arm_3_joint", "arm_4_joint", "arm_5_joint", "arm_6_joint", "arm_7_joint", $(arg gripper_joint_names) "head_1_joint", "head_2_joint" ]</rosparam>
+  </group>
+  <group ns="sot_controller">
+    <rosparam param="joint_names" subst_value="True">[ $(arg mobile_base_joint_names) "torso_lift_joint", "arm_1_joint", "arm_2_joint", "arm_3_joint", "arm_4_joint", "arm_5_joint", "arm_6_joint", "arm_7_joint", $(arg gripper_joint_names) "head_1_joint", "head_2_joint" ]</rosparam>
+    <rosparam param="dt">0.01</rosparam>
+  </group>
   
   <machine name="geometric_simu_machine"
     address="localhost" />

sot_tiago_bringup/sot/tiago.yaml

-sot:
-  state_vector_map: [
-    wheel_left_joint, wheel_right_joint,
-    torso_lift_joint,
-    arm_1_joint, arm_2_joint, arm_3_joint, arm_4_joint, arm_5_joint, arm_6_joint, arm_7_joint, gripper_finger_joint,
-    head_1_joint,head_2_joint]
-
-sot_controller:
-  joint_names: [
-    wheel_left_joint, wheel_right_joint,
-    torso_lift_joint,
-    arm_1_joint, arm_2_joint, arm_3_joint, arm_4_joint, arm_5_joint, arm_6_joint, arm_7_joint, gripper_finger_joint,
-    head_1_joint,head_2_joint
-  ]
-  dt: 0.001