From 460ad6ceb85b779b2b7224a3573c3bdf4917da62 Mon Sep 17 00:00:00 2001 From: Iori Yanokura Date: Fri, 6 Dec 2024 00:49:17 +0900 Subject: [PATCH] Use default_controller param in default_controller --- .../python/kxr_controller/kxr_interface.py | 16 +++++++++++++++- 1 file changed, 15 insertions(+), 1 deletion(-) diff --git a/ros/kxr_controller/python/kxr_controller/kxr_interface.py b/ros/kxr_controller/python/kxr_controller/kxr_interface.py index 546dda6b..26940c26 100644 --- a/ros/kxr_controller/python/kxr_controller/kxr_interface.py +++ b/ros/kxr_controller/python/kxr_controller/kxr_interface.py @@ -210,4 +210,18 @@ def fullbody_controller(self): } def default_controller(self): - return [self.fullbody_controller] + controller_names = rospy.get_param( + self.namespace + 'default_controller', None) + if controller_names is None: + return [self.fullbody_controller] + controllers = [] + for cont_name in controller_names: + controllers.append({ + "controller_type": cont_name, + "controller_action": cont_name + "/follow_joint_trajectory", + "controller_state": cont_name + "/state", + "action_type": control_msgs.msg.FollowJointTrajectoryAction, + "joint_names": rospy.get_param( + self.namespace + cont_name + '/joints'), + }) + return controllers