Merge pull request ipa320#2 from iirob/raw_with_fts_arg_ctrllers

added arg for controllers to spawn
KITrobotics · Apr 29, 2019 · c61cdc7 · c61cdc7
2 parents af5294d + cd5f5ec
commit c61cdc7
Show file tree

Hide file tree

Showing 3 changed files with 19 additions and 3 deletions.
diff --git a/cob_bringup/components/raw3_base.launch b/cob_bringup/components/raw3_base.launch
@@ -6,18 +6,23 @@
 	<arg name="sim" default="false"/>
 
 	<arg name="driver_yaml" default="$(arg pkg_hardware_config)/robots/raw3/config/raw3_base_driver.yaml"/>
+        <arg name="canopen_driver_yaml" default="$(arg pkg_hardware_config)/robots/raw3-5/config/base_driver.yaml"/>
 	<arg name="controller_yaml" default="$(arg pkg_hardware_config)/robots/raw3/config/raw3_base_controller.yaml" />
 
+        <arg name="base_controllers_to_spawn" default="joint_state_controller odometry_controller"/>
+
 	<include file="$(find cob_bringup)/components/ros_control_base.launch">
 		<arg name="robot" value="$(arg robot)"/>
 		<arg name="pkg_hardware_config" value="$(arg pkg_hardware_config)"/>
 		<arg name="driver_yaml" value="$(arg driver_yaml)"/>
+		<arg name="canopen_driver_yaml" value="$(arg canopen_driver_yaml)"/>
 		<arg name="controller_yaml" value="$(arg controller_yaml)"/>
 		<arg name="reset_errors_before_recover" value="true"/>
 		<arg name="can_device" value="$(arg can_device)"/>
 	        <arg name="stuck_detector_timeout" value="4.0" />
 		<arg name="stuck_detector_threshold" value="0.5" />
 		<arg name="stuck_detector_shutdown" value="false" />
 		<arg name="sim" value="$(arg sim)"/>
+                <arg name="base_controllers_to_spawn" value="$(arg base_controllers_to_spawn)"/>
 	</include>
 </launch>
diff --git a/cob_bringup/components/ros_control_base.launch b/cob_bringup/components/ros_control_base.launch
@@ -2,6 +2,7 @@
 <launch>
 	<arg name="robot"/>
 	<arg name="driver_yaml"/>
+        <arg name="canopen_driver_yaml"/>
 	<arg name="controller_yaml"/>
 
 	<arg name="can_device" default="can0"/>
@@ -19,6 +20,12 @@
 	<arg name="stuck_detector_threshold" default="0.174533"/>
 	<arg name="stuck_detector_shutdown" default="false"/>
 
+        <arg name="base_controllers_to_spawn" default="joint_state_controller odometry_controller"/>
+
+        <arg name="velocity_smoother_config_file"   default="$(arg pkg_hardware_config)/robots/$(arg robot)/config/velocity_smoother.yaml"/>
+        <arg name="twist_mux_config_locks"          default="$(arg pkg_hardware_config)/robots/$(arg robot)/config/twist_mux_locks.yaml"/>
+        <arg name="twist_mux_config_topics"         default="$(arg pkg_hardware_config)/robots/$(arg robot)/config/twist_mux_topics.yaml"/>
+
 	<rosparam unless="$(arg sim)" ns="$(arg component_name)/driver" command="load" file="$(arg driver_yaml)"/>
 	<include unless="$(arg sim)" file="$(find cob_bringup)/drivers/canopen_402.launch">
 		<arg name="robot" value="$(arg robot)"/>
@@ -28,6 +35,7 @@
 		<arg name="use_external_sync" value="$(arg use_external_sync)"/>
 		<arg name="start_external_sync" value="$(arg start_external_sync)"/>
 		<arg name="reset_errors_before_recover" value="$(arg reset_errors_before_recover)"/>
+		<arg name="canopen_driver_yaml" value="$(arg canopen_driver_yaml)"/>
 	</include>
 
 	<group if="$(arg enable_stuck_detector)">
@@ -41,21 +49,23 @@
 
 	<group ns="base">
 		<rosparam command="load" file="$(arg controller_yaml)"/>
-		<node pkg="controller_manager" type="controller_manager" name="base_controller_spawner" args="spawn joint_state_controller twist_controller odometry_controller" respawn="false" output="screen"/>
+		<node pkg="controller_manager" type="controller_manager" name="base_controller_spawner" args="spawn $(arg base_controllers_to_spawn)" respawn="false" output="screen"/>
 	</group>
 
 	<group ns="$(arg component_name)">
 		<!-- twist mux -->
 		<include file="$(find cob_bringup)/tools/twist_mux.launch">
 			<arg name="robot" value="$(arg robot)"/>
+			<arg name="config_locks"  default="$(arg twist_mux_config_locks)"/>
+                        <arg name="config_topics" default="$(arg twist_mux_config_topics)"/>
 			<arg name="cmd_vel_out" default="velocity_smoother/command"/>
 			<arg name="marker_out" default="twist_mux/marker"/>
 		</include>
 
 		<!-- velocity smoother -->
 		<include file="$(find cob_bringup)/tools/velocity_smoother.launch">
 			<arg name="robot" value="$(arg robot)"/>
-			<arg name="config_file"           default="$(arg pkg_hardware_config)/robots/$(arg robot)/config/velocity_smoother.yaml"/>
+			<arg name="config_file"           default="$(arg velocity_smoother_config_file)"/>
 			<arg name="raw_cmd_vel_topic"     default="velocity_smoother/command"/>
 			<arg name="smooth_cmd_vel_topic"  default="twist_controller/command"/>
 			<arg name="robot_cmd_vel_topic"   default="twist_controller/command"/>

diff --git a/cob_bringup/drivers/canopen_402.launch b/cob_bringup/drivers/canopen_402.launch
@@ -10,6 +10,7 @@
 	<arg name="use_external_sync"/>
 	<arg name="start_external_sync"/>
 	<arg name="reset_errors_before_recover" default="false"/>
+	<arg name="canopen_driver_yaml" default="$(arg pkg_hardware_config)/robots/$(arg robot)/config/$(arg component_name)_driver.yaml"/>
 
 	<!-- start external sync node if requested -->
 	<node ns="$(arg component_name)" pkg="canopen_chain_node" type="canopen_sync_node" name="sync_$(arg can_device)" output="screen" if="$(arg start_external_sync)">
@@ -18,7 +19,7 @@
 
 	<!-- start canopen node -->
 	<node ns="$(arg component_name)" pkg="canopen_motor_node" type="canopen_motor_node" name="driver" output="screen">
-		<rosparam command="load" file="$(arg pkg_hardware_config)/robots/$(arg robot)/config/$(arg component_name)_driver.yaml"/>
+		<rosparam command="load" file="$(arg canopen_driver_yaml)"/>
 		<rosparam subst_value="True">{bus: { device: $(arg can_device) } }</rosparam>
 		<rosparam subst_value="True" if="$(arg use_external_sync)">{sync: { interval_ms: $(arg interval_ms), overflow: $(arg overflow) } }</rosparam>
 		<param name="bus/master_allocator" value="canopen::ExternalMaster::Allocator" if="$(arg use_external_sync)"/>