diff --git a/lbr_demos/lbr_demos_advanced_cpp/config/admittance_control.yaml b/lbr_demos/lbr_demos_advanced_cpp/config/admittance_control.yaml index 7df54a6d..b356195d 100644 --- a/lbr_demos/lbr_demos_advanced_cpp/config/admittance_control.yaml +++ b/lbr_demos/lbr_demos_advanced_cpp/config/admittance_control.yaml @@ -1,7 +1,7 @@ /**/admittance_control: ros__parameters: - base_link: "link_0" - end_effector_link: "link_ee" + base_link: "lbr_link_0" + end_effector_link: "lbr_link_ee" f_ext_th: [2., 2., 2., 0.5, 0.5, 0.5] dq_gains: [4.0, 4.0, 4.0, 4.0, 4.0, 4.0, 4.0] dx_gains: [0.1, 0.1, 0.1, 0.1, 0.1, 0.1] diff --git a/lbr_demos/lbr_demos_advanced_cpp/src/pose_control_node.cpp b/lbr_demos/lbr_demos_advanced_cpp/src/pose_control_node.cpp index 8d38376a..47602410 100644 --- a/lbr_demos/lbr_demos_advanced_cpp/src/pose_control_node.cpp +++ b/lbr_demos/lbr_demos_advanced_cpp/src/pose_control_node.cpp @@ -32,8 +32,8 @@ class PoseControlNode : public LBRBasePositionCommandNode { return; } - this->declare_parameter("base_link", "link_0"); - this->declare_parameter("end_effector_link", "link_ee"); + this->declare_parameter("base_link", "lbr_link_0"); + this->declare_parameter("end_effector_link", "lbr_link_ee"); std::string root_link = this->get_parameter("base_link").as_string(); std::string tip_link = this->get_parameter("end_effector_link").as_string(); diff --git a/lbr_demos/lbr_demos_advanced_py/config/admittance_control.yaml b/lbr_demos/lbr_demos_advanced_py/config/admittance_control.yaml index 4146d02f..1dbf503e 100644 --- a/lbr_demos/lbr_demos_advanced_py/config/admittance_control.yaml +++ b/lbr_demos/lbr_demos_advanced_py/config/admittance_control.yaml @@ -1,7 +1,7 @@ /**/admittance_control: ros__parameters: - base_link: "link_0" - end_effector_link: "link_ee" + base_link: "lbr_link_0" + end_effector_link: "lbr_link_ee" f_ext_th: [2., 2., 2., 0.5, 0.5, 0.5] dq_gains: [1., 1., 1., 1., 1., 1., 1.] dx_gains: [0.1, 0.1, 0.1, 0.1, 0.1, 0.1] diff --git a/lbr_demos/lbr_demos_advanced_py/config/admittance_rcm_control.yaml b/lbr_demos/lbr_demos_advanced_py/config/admittance_rcm_control.yaml index 901d1d87..0f95f796 100644 --- a/lbr_demos/lbr_demos_advanced_py/config/admittance_rcm_control.yaml +++ b/lbr_demos/lbr_demos_advanced_py/config/admittance_rcm_control.yaml @@ -1,7 +1,7 @@ /**/admittance_rcm_control: ros__parameters: - base_link: "link_0" - end_effector_link: "link_ee" + base_link: "lbr_link_0" + end_effector_link: "lbr_link_ee" f_ext_th: [4.0, 4.0, 4.0, 1.0, 1.0, 1.0] dq_gains: [1., 1., 1., 1., 1., 1., 1.] dx_gains: [0.1, 0.1, 0.1, 0.1, 0.1, 0.1] diff --git a/lbr_demos/lbr_demos_advanced_py/lbr_demos_advanced_py/admittance_control_node.py b/lbr_demos/lbr_demos_advanced_py/lbr_demos_advanced_py/admittance_control_node.py index eaccee81..d1aae155 100755 --- a/lbr_demos/lbr_demos_advanced_py/lbr_demos_advanced_py/admittance_control_node.py +++ b/lbr_demos/lbr_demos_advanced_py/lbr_demos_advanced_py/admittance_control_node.py @@ -12,8 +12,8 @@ def __init__(self, node_name: str = "admittance_control") -> None: super().__init__(node_name=node_name) # parameters - self.declare_parameter("base_link", "link_0") - self.declare_parameter("end_effector_link", "link_ee") + self.declare_parameter("base_link", "lbr_link_0") + self.declare_parameter("end_effector_link", "lbr_link_ee") self.declare_parameter("f_ext_th", [2.0, 2.0, 2.0, 0.5, 0.5, 0.5]) self.declare_parameter("dq_gains", [1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0]) self.declare_parameter("dx_gains", [0.1, 0.1, 0.1, 0.1, 0.1, 0.1]) diff --git a/lbr_demos/lbr_demos_advanced_py/lbr_demos_advanced_py/admittance_controller.py b/lbr_demos/lbr_demos_advanced_py/lbr_demos_advanced_py/admittance_controller.py index 39aece33..d6875744 100644 --- a/lbr_demos/lbr_demos_advanced_py/lbr_demos_advanced_py/admittance_controller.py +++ b/lbr_demos/lbr_demos_advanced_py/lbr_demos_advanced_py/admittance_controller.py @@ -8,8 +8,8 @@ class AdmittanceController(object): def __init__( self, robot_description: str, - base_link: str = "link_0", - end_effector_link: str = "link_ee", + base_link: str = "lbr_link_0", + end_effector_link: str = "lbr_link_ee", f_ext_th: np.ndarray = np.array([2.0, 2.0, 2.0, 0.5, 0.5, 0.5]), dq_gains: np.ndarray = np.array([1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0]), dx_gains: np.ndarray = np.array([0.1, 0.1, 0.1, 0.1, 0.1, 0.1]), diff --git a/lbr_demos/lbr_demos_advanced_py/lbr_demos_advanced_py/admittance_rcm_control_node.py b/lbr_demos/lbr_demos_advanced_py/lbr_demos_advanced_py/admittance_rcm_control_node.py index 55eb5bac..cc99dfbd 100644 --- a/lbr_demos/lbr_demos_advanced_py/lbr_demos_advanced_py/admittance_rcm_control_node.py +++ b/lbr_demos/lbr_demos_advanced_py/lbr_demos_advanced_py/admittance_rcm_control_node.py @@ -12,8 +12,8 @@ def __init__(self, node_name: str = "admittance_rcm_control") -> None: super().__init__(node_name) # declare and get parameters - self.declare_parameter("base_link", "link_0") - self.declare_parameter("end_effector_link", "link_ee") + self.declare_parameter("base_link", "lbr_link_0") + self.declare_parameter("end_effector_link", "lbr_link_ee") self.declare_parameter("f_ext_th", [4.0, 4.0, 4.0, 1.0, 1.0, 1.0]) self.declare_parameter("dq_gains", [1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0]) self.declare_parameter("dx_gains", [0.1, 0.1, 0.1, 0.1, 0.1, 0.1]) diff --git a/lbr_demos/lbr_demos_advanced_py/lbr_demos_advanced_py/admittance_rcm_controller.py b/lbr_demos/lbr_demos_advanced_py/lbr_demos_advanced_py/admittance_rcm_controller.py index 95fd3b18..4c9b5486 100644 --- a/lbr_demos/lbr_demos_advanced_py/lbr_demos_advanced_py/admittance_rcm_controller.py +++ b/lbr_demos/lbr_demos_advanced_py/lbr_demos_advanced_py/admittance_rcm_controller.py @@ -6,8 +6,8 @@ class AdmittanceRCMController: def __init__( self, robot_description: str, - base_link: str = "link_0", - end_effector_link: str = "link_ee", + base_link: str = "lbr_link_0", + end_effector_link: str = "lbr_link_ee", ): self._robot = optas.RobotModel( urdf_string=robot_description, time_derivs=[0, 1] diff --git a/lbr_description/ros2_control/lbr_system_interface.xacro b/lbr_description/ros2_control/lbr_system_interface.xacro index 9b578f7f..9a61a792 100644 --- a/lbr_description/ros2_control/lbr_system_interface.xacro +++ b/lbr_description/ros2_control/lbr_system_interface.xacro @@ -1,7 +1,7 @@ + params="robot_name mode joint_limits system_parameters_path"> diff --git a/lbr_description/ros2_control/lbr_system_parameters.yaml b/lbr_description/ros2_control/lbr_system_parameters.yaml index 4ae4cbf2..b55b0aef 100644 --- a/lbr_description/ros2_control/lbr_system_parameters.yaml +++ b/lbr_description/ros2_control/lbr_system_parameters.yaml @@ -19,8 +19,8 @@ hardware: open_loop: true # KUKA works the best in open_loop control mode estimated_ft_sensor: # estimates the external force-torque from the external joint torque values - chain_root: link_0 - chain_tip: link_ee + chain_root: lbr_link_0 + chain_tip: lbr_link_ee damping: 0.2 # damping factor for the pseudo-inverse of the Jacobian force_x_th: 2.0 # x-force threshold. Only if the force exceeds this value, the force will be considered force_y_th: 2.0 # y-force threshold. Only if the force exceeds this value, the force will be considered diff --git a/lbr_description/urdf/iiwa14/iiwa14_description.xacro b/lbr_description/urdf/iiwa14/iiwa14_description.xacro index 00eba77b..b4eb7212 100644 --- a/lbr_description/urdf/iiwa14/iiwa14_description.xacro +++ b/lbr_description/urdf/iiwa14/iiwa14_description.xacro @@ -22,6 +22,7 @@ diff --git a/lbr_description/urdf/iiwa7/iiwa7_description.xacro b/lbr_description/urdf/iiwa7/iiwa7_description.xacro index c9234c68..089ffe65 100644 --- a/lbr_description/urdf/iiwa7/iiwa7_description.xacro +++ b/lbr_description/urdf/iiwa7/iiwa7_description.xacro @@ -22,6 +22,7 @@ diff --git a/lbr_description/urdf/med14/med14_description.xacro b/lbr_description/urdf/med14/med14_description.xacro index fd78c8da..e76b72f9 100644 --- a/lbr_description/urdf/med14/med14_description.xacro +++ b/lbr_description/urdf/med14/med14_description.xacro @@ -22,6 +22,7 @@ diff --git a/lbr_description/urdf/med7/med7_description.xacro b/lbr_description/urdf/med7/med7_description.xacro index d7b6abfa..675b1a2c 100644 --- a/lbr_description/urdf/med7/med7_description.xacro +++ b/lbr_description/urdf/med7/med7_description.xacro @@ -22,6 +22,7 @@ diff --git a/lbr_fri_ros2/include/lbr_fri_ros2/ft_estimator.hpp b/lbr_fri_ros2/include/lbr_fri_ros2/ft_estimator.hpp index b4257a9e..8ef59405 100644 --- a/lbr_fri_ros2/include/lbr_fri_ros2/ft_estimator.hpp +++ b/lbr_fri_ros2/include/lbr_fri_ros2/ft_estimator.hpp @@ -35,8 +35,8 @@ class FTEstimator { using cart_array_t_ref = cart_array_t &; using const_cart_array_t_ref = const cart_array_t &; - FTEstimator(const std::string &robot_description, const std::string &chain_root = "link_0", - const std::string &chain_tip = "link_ee", + FTEstimator(const std::string &robot_description, const std::string &chain_root = "lbr_link_0", + const std::string &chain_tip = "lbr_link_ee", const_cart_array_t_ref f_ext_th = {2., 2., 2., 0.5, 0.5, 0.5}); void compute(const_jnt_pos_array_t_ref measured_joint_position, const_ext_tau_array_t_ref external_torque, cart_array_t_ref f_ext, diff --git a/lbr_fri_ros2/test/test_position_command.cpp b/lbr_fri_ros2/test/test_position_command.cpp index 22cd7348..e2f7830d 100644 --- a/lbr_fri_ros2/test/test_position_command.cpp +++ b/lbr_fri_ros2/test/test_position_command.cpp @@ -24,7 +24,8 @@ int main() { pid_params.p = 1.0; - cmd_guard_params.joint_names = {"A1", "A2", "A3", "A4", "A5", "A6", "A7"}; + cmd_guard_params.joint_names = {"lbr_A1", "lbr_A2", "lbr_A3", "lbr_A4", + "lbr_A5", "lbr_A6", "lbr_A7"}; cmd_guard_params.max_positions = {170. * M_PI / 180., 120. * M_PI / 180., 170. * M_PI / 180., 120. * M_PI / 180., 170. * M_PI / 180., 120. * M_PI / 180., 175. * M_PI / 180.}; diff --git a/lbr_fri_ros2/test/test_torque_command.cpp b/lbr_fri_ros2/test/test_torque_command.cpp index 2af7f93a..9c580828 100644 --- a/lbr_fri_ros2/test/test_torque_command.cpp +++ b/lbr_fri_ros2/test/test_torque_command.cpp @@ -24,7 +24,8 @@ int main() { pid_params.p = 10.0; - cmd_guard_params.joint_names = {"A1", "A2", "A3", "A4", "A5", "A6", "A7"}; + cmd_guard_params.joint_names = {"lbr_A1", "lbr_A2", "lbr_A3", "lbr_A4", + "lbr_A5", "lbr_A6", "lbr_A7"}; cmd_guard_params.max_positions = {170. * M_PI / 180., 120. * M_PI / 180., 170. * M_PI / 180., 120. * M_PI / 180., 170. * M_PI / 180., 120. * M_PI / 180., 175. * M_PI / 180.}; diff --git a/lbr_fri_ros2/test/test_wrench_command.cpp b/lbr_fri_ros2/test/test_wrench_command.cpp index de88e0e6..9ad8b315 100644 --- a/lbr_fri_ros2/test/test_wrench_command.cpp +++ b/lbr_fri_ros2/test/test_wrench_command.cpp @@ -24,7 +24,8 @@ int main() { pid_params.p = 10.0; - cmd_guard_params.joint_names = {"A1", "A2", "A3", "A4", "A5", "A6", "A7"}; + cmd_guard_params.joint_names = {"lbr_A1", "lbr_A2", "lbr_A3", "lbr_A4", + "lbr_A5", "lbr_A6", "lbr_A7"}; cmd_guard_params.max_positions = {170. * M_PI / 180., 120. * M_PI / 180., 170. * M_PI / 180., 120. * M_PI / 180., 170. * M_PI / 180., 120. * M_PI / 180., 175. * M_PI / 180.}; diff --git a/lbr_ros2_control/config/lbr_controllers.yaml b/lbr_ros2_control/config/lbr_controllers.yaml index f8fc7aad..e6936732 100644 --- a/lbr_ros2_control/config/lbr_controllers.yaml +++ b/lbr_ros2_control/config/lbr_controllers.yaml @@ -32,15 +32,21 @@ lbr_wrench_command_controller: type: lbr_ros2_control/LBRWrenchCommandController - # Admittance controller admittance_controller: type: lbr_ros2_control/AdmittanceController +# ROS 2 control broadcasters /**/force_torque_broadcaster: ros__parameters: frame_id: lbr_link_ee # namespace: https://github.com/ros2/rviz/issues/1103 sensor_name: estimated_ft_sensor +# LBR ROS 2 control broadcasters +/**/lbr_state_broadcaster: + ros__parameters: + robot_name: lbr + +# ROS 2 control controllers /**/joint_trajectory_controller: ros__parameters: joints: @@ -70,3 +76,16 @@ - lbr_A6 - lbr_A7 interface_name: position + +# LBR ROS 2 control controllers +/**/lbr_joint_position_command_controller: + ros__parameters: + robot_name: lbr + +/**/lbr_torque_command_controller: + ros__parameters: + robot_name: lbr + +/**/lbr_wrench_command_controller: + ros__parameters: + robot_name: lbr diff --git a/lbr_ros2_control/include/lbr_ros2_control/controllers/admittance_controller.hpp b/lbr_ros2_control/include/lbr_ros2_control/controllers/admittance_controller.hpp index 6d28e3a1..1a75be92 100644 --- a/lbr_ros2_control/include/lbr_ros2_control/controllers/admittance_controller.hpp +++ b/lbr_ros2_control/include/lbr_ros2_control/controllers/admittance_controller.hpp @@ -50,9 +50,9 @@ class AdmittanceController : public controller_interface::ControllerInterface { bool reference_state_interfaces_(); void clear_command_interfaces_(); void clear_state_interfaces_(); + void configure_joint_names_(); - std::array joint_names_ = { - "A1", "A2", "A3", "A4", "A5", "A6", "A7"}; + std::array joint_names_; std::vector> joint_position_command_interfaces_; diff --git a/lbr_ros2_control/include/lbr_ros2_control/controllers/lbr_joint_position_command_controller.hpp b/lbr_ros2_control/include/lbr_ros2_control/controllers/lbr_joint_position_command_controller.hpp index 46eebecd..929dcf63 100644 --- a/lbr_ros2_control/include/lbr_ros2_control/controllers/lbr_joint_position_command_controller.hpp +++ b/lbr_ros2_control/include/lbr_ros2_control/controllers/lbr_joint_position_command_controller.hpp @@ -40,8 +40,9 @@ class LBRJointPositionCommandController : public controller_interface::Controlle on_deactivate(const rclcpp_lifecycle::State &previous_state) override; protected: - std::array joint_names_ = { - "A1", "A2", "A3", "A4", "A5", "A6", "A7"}; + void configure_joint_names_(); + + std::array joint_names_; realtime_tools::RealtimeBuffer rt_lbr_joint_position_command_ptr_; diff --git a/lbr_ros2_control/include/lbr_ros2_control/controllers/lbr_state_broadcaster.hpp b/lbr_ros2_control/include/lbr_ros2_control/controllers/lbr_state_broadcaster.hpp index 877f71e6..70efcdb3 100644 --- a/lbr_ros2_control/include/lbr_ros2_control/controllers/lbr_state_broadcaster.hpp +++ b/lbr_ros2_control/include/lbr_ros2_control/controllers/lbr_state_broadcaster.hpp @@ -47,9 +47,10 @@ class LBRStateBroadcaster : public controller_interface::ControllerInterface { protected: void init_state_interface_map_(); void init_state_msg_(); + void configure_joint_names_(); std::array joint_names_ = { - "A1", "A2", "A3", "A4", "A5", "A6", "A7"}; + "lbr_A1", "lbr_A2", "lbr_A3", "lbr_A4", "lbr_A5", "lbr_A6", "lbr_A7"}; std::unordered_map> state_interface_map_; rclcpp::Publisher::SharedPtr state_publisher_ptr_; diff --git a/lbr_ros2_control/include/lbr_ros2_control/controllers/lbr_torque_command_controller.hpp b/lbr_ros2_control/include/lbr_ros2_control/controllers/lbr_torque_command_controller.hpp index 7a4c5f7b..65d6f0a8 100644 --- a/lbr_ros2_control/include/lbr_ros2_control/controllers/lbr_torque_command_controller.hpp +++ b/lbr_ros2_control/include/lbr_ros2_control/controllers/lbr_torque_command_controller.hpp @@ -44,9 +44,9 @@ class LBRTorqueCommandController : public controller_interface::ControllerInterf protected: bool reference_command_interfaces_(); void clear_command_interfaces_(); + void configure_joint_names_(); - std::array joint_names_ = { - "A1", "A2", "A3", "A4", "A5", "A6", "A7"}; + std::array joint_names_; std::vector> joint_position_command_interfaces_, torque_command_interfaces_; diff --git a/lbr_ros2_control/include/lbr_ros2_control/controllers/lbr_wrench_command_controller.hpp b/lbr_ros2_control/include/lbr_ros2_control/controllers/lbr_wrench_command_controller.hpp index 52b76f35..c145f043 100644 --- a/lbr_ros2_control/include/lbr_ros2_control/controllers/lbr_wrench_command_controller.hpp +++ b/lbr_ros2_control/include/lbr_ros2_control/controllers/lbr_wrench_command_controller.hpp @@ -47,9 +47,9 @@ class LBRWrenchCommandController : public controller_interface::ControllerInterf protected: bool reference_command_interfaces_(); void clear_command_interfaces_(); + void configure_joint_names_(); - std::array joint_names_ = { - "A1", "A2", "A3", "A4", "A5", "A6", "A7"}; + std::array joint_names_; std::vector> joint_position_command_interfaces_, wrench_command_interfaces_; diff --git a/lbr_ros2_control/include/lbr_ros2_control/system_interface.hpp b/lbr_ros2_control/include/lbr_ros2_control/system_interface.hpp index ceb5157c..42a4e77a 100644 --- a/lbr_ros2_control/include/lbr_ros2_control/system_interface.hpp +++ b/lbr_ros2_control/include/lbr_ros2_control/system_interface.hpp @@ -55,8 +55,8 @@ struct SystemInterfaceParameters { }; struct EstimatedFTSensorParameters { - std::string chain_root{"link_0"}; - std::string chain_tip{"link_ee"}; + std::string chain_root{"lbr_link_0"}; + std::string chain_tip{"lbr_link_ee"}; double damping{0.2}; double force_x_th{2.0}; double force_y_th{2.0}; diff --git a/lbr_ros2_control/src/controllers/admittance_controller.cpp b/lbr_ros2_control/src/controllers/admittance_controller.cpp index 147ad742..8c1a94f4 100644 --- a/lbr_ros2_control/src/controllers/admittance_controller.cpp +++ b/lbr_ros2_control/src/controllers/admittance_controller.cpp @@ -74,4 +74,18 @@ void AdmittanceController::clear_command_interfaces_() { void AdmittanceController::clear_state_interfaces_() { estimated_ft_sensor_state_interface_.clear(); } + +void AdmittanceController::configure_joint_names_() { + if (joint_names_.size() != KUKA::FRI::LBRState::NUMBER_OF_JOINTS) { + RCLCPP_ERROR( + this->get_node()->get_logger(), + "Number of joint names (%ld) does not match the number of joints in the robot (%d).", + joint_names_.size(), KUKA::FRI::LBRState::NUMBER_OF_JOINTS); + throw std::runtime_error("Failed to configure joint names."); + } + std::string robot_name = this->get_node()->get_parameter("robot_name").as_string(); + for (int i = 0; i < KUKA::FRI::LBRState::NUMBER_OF_JOINTS; i++) { + joint_names_[i] = robot_name + "_A" + std::to_string(i + 1); + } +} } // namespace lbr_ros2_control diff --git a/lbr_ros2_control/src/controllers/lbr_joint_position_command_controller.cpp b/lbr_ros2_control/src/controllers/lbr_joint_position_command_controller.cpp index f5217b82..380827ec 100644 --- a/lbr_ros2_control/src/controllers/lbr_joint_position_command_controller.cpp +++ b/lbr_ros2_control/src/controllers/lbr_joint_position_command_controller.cpp @@ -29,6 +29,8 @@ controller_interface::CallbackReturn LBRJointPositionCommandController::on_init( [this](const lbr_fri_idl::msg::LBRJointPositionCommand::SharedPtr msg) { rt_lbr_joint_position_command_ptr_.writeFromNonRT(msg); }); + this->get_node()->declare_parameter("robot_name", "lbr"); + configure_joint_names_(); } catch (const std::exception &e) { RCLCPP_ERROR(this->get_node()->get_logger(), "Failed to initialize LBR position command controller with: %s.", e.what()); @@ -66,6 +68,20 @@ controller_interface::CallbackReturn LBRJointPositionCommandController::on_deact const rclcpp_lifecycle::State & /*previous_state*/) { return controller_interface::CallbackReturn::SUCCESS; } + +void LBRJointPositionCommandController::configure_joint_names_() { + if (joint_names_.size() != KUKA::FRI::LBRState::NUMBER_OF_JOINTS) { + RCLCPP_ERROR( + this->get_node()->get_logger(), + "Number of joint names (%ld) does not match the number of joints in the robot (%d).", + joint_names_.size(), KUKA::FRI::LBRState::NUMBER_OF_JOINTS); + throw std::runtime_error("Failed to configure joint names."); + } + std::string robot_name = this->get_node()->get_parameter("robot_name").as_string(); + for (int i = 0; i < KUKA::FRI::LBRState::NUMBER_OF_JOINTS; i++) { + joint_names_[i] = robot_name + "_A" + std::to_string(i + 1); + } +} } // namespace lbr_ros2_control #include "pluginlib/class_list_macros.hpp" diff --git a/lbr_ros2_control/src/controllers/lbr_state_broadcaster.cpp b/lbr_ros2_control/src/controllers/lbr_state_broadcaster.cpp index d858d220..b8d9f733 100644 --- a/lbr_ros2_control/src/controllers/lbr_state_broadcaster.cpp +++ b/lbr_ros2_control/src/controllers/lbr_state_broadcaster.cpp @@ -21,13 +21,8 @@ controller_interface::CallbackReturn LBRStateBroadcaster::on_init() { rt_state_publisher_ptr_ = std::make_shared>( state_publisher_ptr_); - if (joint_names_.size() != KUKA::FRI::LBRState::NUMBER_OF_JOINTS) { - RCLCPP_ERROR( - this->get_node()->get_logger(), - "Number of joint names (%ld) does not match the number of joints in the robot (%d).", - joint_names_.size(), KUKA::FRI::LBRState::NUMBER_OF_JOINTS); - return controller_interface::CallbackReturn::ERROR; - } + this->get_node()->declare_parameter("robot_name", "lbr"); + configure_joint_names_(); } catch (const std::exception &e) { RCLCPP_ERROR(this->get_node()->get_logger(), "Failed to initialize LBR state broadcaster with: %s.", e.what()); @@ -154,6 +149,20 @@ void LBRStateBroadcaster::init_state_msg_() { rt_state_publisher_ptr_->msg_.time_stamp_sec = std::numeric_limits::quiet_NaN(); rt_state_publisher_ptr_->msg_.tracking_performance = std::numeric_limits::quiet_NaN(); } + +void LBRStateBroadcaster::configure_joint_names_() { + if (joint_names_.size() != KUKA::FRI::LBRState::NUMBER_OF_JOINTS) { + RCLCPP_ERROR( + this->get_node()->get_logger(), + "Number of joint names (%ld) does not match the number of joints in the robot (%d).", + joint_names_.size(), KUKA::FRI::LBRState::NUMBER_OF_JOINTS); + throw std::runtime_error("Failed to configure joint names."); + } + std::string robot_name = this->get_node()->get_parameter("robot_name").as_string(); + for (int i = 0; i < KUKA::FRI::LBRState::NUMBER_OF_JOINTS; i++) { + joint_names_[i] = robot_name + "_A" + std::to_string(i + 1); + } +} } // namespace lbr_ros2_control #include "pluginlib/class_list_macros.hpp" diff --git a/lbr_ros2_control/src/controllers/lbr_torque_command_controller.cpp b/lbr_ros2_control/src/controllers/lbr_torque_command_controller.cpp index a4f674f6..22a341d2 100644 --- a/lbr_ros2_control/src/controllers/lbr_torque_command_controller.cpp +++ b/lbr_ros2_control/src/controllers/lbr_torque_command_controller.cpp @@ -28,6 +28,8 @@ controller_interface::CallbackReturn LBRTorqueCommandController::on_init() { "command/torque", 1, [this](const lbr_fri_idl::msg::LBRTorqueCommand::SharedPtr msg) { rt_lbr_torque_command_ptr_.writeFromNonRT(msg); }); + this->get_node()->declare_parameter("robot_name", "lbr"); + configure_joint_names_(); } catch (const std::exception &e) { RCLCPP_ERROR(this->get_node()->get_logger(), "Failed to initialize LBR torque command controller with: %s.", e.what()); @@ -102,6 +104,20 @@ void LBRTorqueCommandController::clear_command_interfaces_() { joint_position_command_interfaces_.clear(); torque_command_interfaces_.clear(); } + +void LBRTorqueCommandController::configure_joint_names_() { + if (joint_names_.size() != KUKA::FRI::LBRState::NUMBER_OF_JOINTS) { + RCLCPP_ERROR( + this->get_node()->get_logger(), + "Number of joint names (%ld) does not match the number of joints in the robot (%d).", + joint_names_.size(), KUKA::FRI::LBRState::NUMBER_OF_JOINTS); + throw std::runtime_error("Failed to configure joint names."); + } + std::string robot_name = this->get_node()->get_parameter("robot_name").as_string(); + for (int i = 0; i < KUKA::FRI::LBRState::NUMBER_OF_JOINTS; i++) { + joint_names_[i] = robot_name + "_A" + std::to_string(i + 1); + } +} } // namespace lbr_ros2_control #include "pluginlib/class_list_macros.hpp" diff --git a/lbr_ros2_control/src/controllers/lbr_wrench_command_controller.cpp b/lbr_ros2_control/src/controllers/lbr_wrench_command_controller.cpp index 53445f1f..59763270 100644 --- a/lbr_ros2_control/src/controllers/lbr_wrench_command_controller.cpp +++ b/lbr_ros2_control/src/controllers/lbr_wrench_command_controller.cpp @@ -33,6 +33,8 @@ controller_interface::CallbackReturn LBRWrenchCommandController::on_init() { "command/wrench", 1, [this](const lbr_fri_idl::msg::LBRWrenchCommand::SharedPtr msg) { rt_lbr_wrench_command_ptr_.writeFromNonRT(msg); }); + this->get_node()->declare_parameter("robot_name", "lbr"); + configure_joint_names_(); } catch (const std::exception &e) { RCLCPP_ERROR(this->get_node()->get_logger(), "Failed to initialize LBR wrench command controller with: %s.", e.what()); @@ -108,6 +110,20 @@ void LBRWrenchCommandController::clear_command_interfaces_() { joint_position_command_interfaces_.clear(); wrench_command_interfaces_.clear(); } + +void LBRWrenchCommandController::configure_joint_names_() { + if (joint_names_.size() != KUKA::FRI::LBRState::NUMBER_OF_JOINTS) { + RCLCPP_ERROR( + this->get_node()->get_logger(), + "Number of joint names (%ld) does not match the number of joints in the robot (%d).", + joint_names_.size(), KUKA::FRI::LBRState::NUMBER_OF_JOINTS); + throw std::runtime_error("Failed to configure joint names."); + } + std::string robot_name = this->get_node()->get_parameter("robot_name").as_string(); + for (int i = 0; i < KUKA::FRI::LBRState::NUMBER_OF_JOINTS; i++) { + joint_names_[i] = robot_name + "_A" + std::to_string(i + 1); + } +} } // namespace lbr_ros2_control #include "pluginlib/class_list_macros.hpp"