From 7681468e07290f13a87bb0c88807e43e817e3d54 Mon Sep 17 00:00:00 2001
From: Iori Yanokura <ab.ioryz@gmail.com>
Date: Wed, 4 Dec 2024 12:44:37 +0900
Subject: [PATCH] Add spawn flag for fullbody controller

---
 .../launch/kxr_controller.launch              |  3 +++
 .../launch/kxr_controller_for_wheel.launch    |  2 ++
 .../node_scripts/rcb4_ros_bridge.py           | 19 +++++++++++--------
 3 files changed, 16 insertions(+), 8 deletions(-)

diff --git a/ros/kxr_controller/launch/kxr_controller.launch b/ros/kxr_controller/launch/kxr_controller.launch
index 2722dc9e..a784b1fc 100644
--- 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
index 52b99831..e18e82df 100644
--- 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
index a01fd8b8..5ef617d1 100644
--- 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
@@ -419,14 +421,15 @@ def log_error_and_close_interface(function_name):
         return True
 
     def run_ros_robot_controllers(self):
-        self.proc_controller_spawner = subprocess.Popen(
-            [
-                f'/opt/ros/{os.environ["ROS_DISTRO"]}/bin/rosrun',
-                "controller_manager",
-                "spawner",
-            ]
-            + ["joint_state_controller", "fullbody_controller"]
-        )
+        if self.spawn_fullbody_controller:
+            self.proc_controller_spawner = subprocess.Popen(
+                [
+                    f'/opt/ros/{os.environ["ROS_DISTRO"]}/bin/rosrun',
+                    "controller_manager",
+                    "spawner",
+                ]
+                + ["joint_state_controller", "fullbody_controller"]
+            )
         self.proc_robot_state_publisher = run_robot_state_publisher(self.base_namespace)
         self.proc_kxr_controller = run_kxr_controller(namespace=self.base_namespace)