From eeb8a8f6858e4c4b0b215f5669bb74db51bd6e60 Mon Sep 17 00:00:00 2001
From: Aoi Nakane <nakane@jsk.imi.i.u-tokyo.ac.jp>
Date: Sun, 3 Nov 2024 14:24:40 +0900
Subject: [PATCH] Allow empty urdf_path

---
 ros/kxr_controller/launch/kxr_controller.launch    | 6 ++++--
 ros/kxr_controller/node_scripts/rcb4_ros_bridge.py | 1 +
 2 files changed, 5 insertions(+), 2 deletions(-)

diff --git a/ros/kxr_controller/launch/kxr_controller.launch b/ros/kxr_controller/launch/kxr_controller.launch
index d6d5919a..390df2eb 100644
--- a/ros/kxr_controller/launch/kxr_controller.launch
+++ b/ros/kxr_controller/launch/kxr_controller.launch
@@ -33,8 +33,9 @@
           output="screen"
           required="true" >
       <param name="device" type="str" value="$(arg device)" />
+      <param name="urdf_path" type="str" value="$(arg urdf_path)"
+             if="$(eval len(arg('urdf_path')) > 0)"/>
       <rosparam subst_value="true" >
-        urdf_path: $(arg urdf_path)
         servo_config_path: $(arg servo_config_path)
         publish_imu: $(arg publish_imu)
         publish_sensor: $(arg publish_sensor)
@@ -68,8 +69,9 @@
           output="screen"
           required="true" >
       <param name="device" type="str" value="$(arg device)" />
+      <param name="urdf_path" type="str" value="$(arg urdf_path)"
+             if="$(eval len(arg('urdf_path')) > 0)"/>
       <rosparam subst_value="true" >
-        urdf_path: $(arg urdf_path)
         servo_config_path: $(arg servo_config_path)
         publish_imu: $(arg publish_imu)
         publish_sensor: $(arg publish_sensor)
diff --git a/ros/kxr_controller/node_scripts/rcb4_ros_bridge.py b/ros/kxr_controller/node_scripts/rcb4_ros_bridge.py
index 8d561fd7..720d6f6e 100644
--- a/ros/kxr_controller/node_scripts/rcb4_ros_bridge.py
+++ b/ros/kxr_controller/node_scripts/rcb4_ros_bridge.py
@@ -177,6 +177,7 @@ def setup_paths_and_params(self):
 
     def setup_urdf_and_model(self):
         robot_model = RobotModel()
+        rospy.loginfo(f"[setup_urdf_and_model] Loading URDF File. {self.urdf_path}")
         if self.urdf_path is None:
             self.urdf_path = make_urdf_file(self.joint_name_to_id)
             rospy.loginfo("Use temporary URDF")