Add spawn flag for fullbody controller

iory · Dec 4, 2024 · aa53235 · aa53235
1 parent 4f11852
commit aa53235
Show file tree

Hide file tree

Showing 3 changed files with 10 additions and 2 deletions.
diff --git a/ros/kxr_controller/launch/kxr_controller.launch b/ros/kxr_controller/launch/kxr_controller.launch
@@ -18,6 +18,7 @@
   <arg name="temperature_limit" default="80" doc="Temperature limit in celsius" />
   <arg name="read_current" default="false" />
   <arg name="read_temperature" default="false" />
+  <arg name="spawn_fullbody_controller" default="true" doc="Internally spawn fullbody_controller" />
 
   <group if="$(eval len(arg('namespace')) > 0)" ns="$(arg namespace)" >
     <param name="control_loop_rate" value="$(arg control_loop_rate)" />
@@ -52,6 +53,7 @@
         temperature_limit: $(arg temperature_limit)
         read_current: $(arg read_current)
         read_temperature: $(arg read_temperature)
+        spawn_fullbody_controller: $(arg spawn_fullbody_controller)
       </rosparam>
     </node>
     <node name="urdf_model_server"
@@ -92,6 +94,7 @@
         temperature_limit: $(arg temperature_limit)
         read_current: $(arg read_current)
         read_temperature: $(arg read_temperature)
+        spawn_fullbody_controller: $(arg spawn_fullbody_controller)
       </rosparam>
     </node>
 

diff --git a/ros/kxr_controller/launch/kxr_controller_for_wheel.launch b/ros/kxr_controller/launch/kxr_controller_for_wheel.launch
@@ -15,6 +15,7 @@
   <arg name="temperature_limit" default="80" doc="Temperature limit in celsius" />
   <arg name="read_current" default="false" />
   <arg name="read_temperature" default="false" />
+  <arg name="spawn_fullbody_controller" default="true" doc="Internally spawn fullbody_controller" />
 
   <include file="$(find kxr_controller)/launch/kxr_controller.launch" >
     <arg name="urdf_path" value="$(arg urdf_path)" />
@@ -32,6 +33,7 @@
     <arg name="temperature_limit" value="$(arg temperature_limit)" />
     <arg name="read_current" value="$(arg read_current)" />
     <arg name="read_temperature" value="$(arg read_temperature)" />
+    <arg name="spawn_fullbody_controller" value="$(arg spawn_fullbody_controller)" />
   </include>
 
   <group if="$(eval len(arg('namespace')) > 0)" ns="$(arg namespace)" >

diff --git a/ros/kxr_controller/node_scripts/rcb4_ros_bridge.py b/ros/kxr_controller/node_scripts/rcb4_ros_bridge.py
@@ -177,6 +177,8 @@ def setup_paths_and_params(self):
         self.joint_name_to_id, self.servo_infos = load_yaml(servo_config_path)
         self.urdf_path = rospy.get_param("~urdf_path", None)
         self.use_rcb4 = rospy.get_param("~use_rcb4", False)
+        self.spawn_fullbody_controller = rospy.get_param(
+            "~spawn_fullbody_controller", True)
         self.control_pressure = rospy.get_param("~control_pressure", False)
         self.read_temperature = rospy.get_param("~read_temperature", False) and not self.use_rcb4
         self.read_current = rospy.get_param("~read_current", False) and not self.use_rcb4
@@ -204,8 +206,9 @@ def setup_urdf_and_model(self):
 
     def setup_ros_parameters(self):
         """Configure joint state and full body controllers."""
-        set_joint_state_controler()
-        self.set_fullbody_controller()
+        if self.spawn_fullbody_controller:
+            set_joint_state_controler()
+            self.set_fullbody_controller()
 
     def setup_publishers_and_servers(self):
         """Set up ROS publishers and action servers."""