addressed humble compatability (#225 (comment))

lbr-stack · Nov 20, 2024 · 7da8f2a · 7da8f2a
1 parent cf4030e
commit 7da8f2a
Show file tree

Hide file tree

Showing 12 changed files with 114 additions and 70 deletions.
diff --git a/.github/workflows/backport.yaml b/.github/workflows/backport.yaml
diff --git a/lbr_bringup/launch/hardware.launch.py b/lbr_bringup/launch/hardware.launch.py
@@ -39,7 +39,9 @@ def generate_launch_description() -> LaunchDescription:
     ld.add_action(robot_state_publisher)
 
     # ros2 control node
-    ros2_control_node = LBRROS2ControlMixin.node_ros2_control(use_sim_time=False)
+    ros2_control_node = LBRROS2ControlMixin.node_ros2_control(
+        use_sim_time=False, robot_description=robot_description
+    )
     ld.add_action(ros2_control_node)
 
     # joint state broad caster and controller on ros2 control node start

diff --git a/lbr_bringup/launch/mock.launch.py b/lbr_bringup/launch/mock.launch.py
@@ -37,7 +37,9 @@ def generate_launch_description() -> LaunchDescription:
     ld.add_action(robot_state_publisher)
 
     # ros2 control node
-    ros2_control_node = LBRROS2ControlMixin.node_ros2_control(use_sim_time=False)
+    ros2_control_node = LBRROS2ControlMixin.node_ros2_control(
+        use_sim_time=False, robot_description=robot_description
+    )
     ld.add_action(ros2_control_node)
 
     # joint state broad caster and controller on ros2 control node start

diff --git a/lbr_bringup/lbr_bringup/ros2_control.py b/lbr_bringup/lbr_bringup/ros2_control.py
@@ -72,6 +72,9 @@ def node_ros2_control(
         use_sim_time: Optional[Union[LaunchConfiguration, bool]] = LaunchConfiguration(
             "use_sim_time", default="false"
         ),
+        robot_description: Optional[
+            Dict[str, str]
+        ] = {},  # required for certain ROS 2 controllers in Humble
         **kwargs,
     ) -> Node:
         return Node(
@@ -91,6 +94,7 @@ def node_ros2_control(
                         ),
                     ]
                 ),
+                robot_description,
             ],
             namespace=robot_name,
             remappings=[

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
@@ -52,6 +52,9 @@ class AdmittanceController : public controller_interface::ControllerInterface {
   void zero_all_values_();
   void log_info_() const;
 
+  // robot description
+  std::string robot_description_;
+
   // admittance
   bool initialized_ = false;
   std::unique_ptr<lbr_fri_ros2::AdmittanceImpl> admittance_impl_ptr_;

diff --git a/lbr_ros2_control/include/lbr_ros2_control/controllers/twist_controller.hpp b/lbr_ros2_control/include/lbr_ros2_control/controllers/twist_controller.hpp
@@ -54,6 +54,9 @@ class TwistController : public controller_interface::ControllerInterface {
   void configure_inv_jac_ctrl_impl_();
   void log_info_() const;
 
+  // robot description
+  std::string robot_description_;
+
   // some safety checks
   std::atomic<int> updates_since_last_command_ = 0;
   double timeout_ = 0.2;

diff --git a/lbr_ros2_control/src/controllers/admittance_controller.cpp b/lbr_ros2_control/src/controllers/admittance_controller.cpp
@@ -38,22 +38,51 @@ AdmittanceController::state_interface_configuration() const {
 
 controller_interface::CallbackReturn AdmittanceController::on_init() {
   try {
-    this->get_node()->declare_parameter("robot_name", "lbr");
-    this->get_node()->declare_parameter("admittance.mass",
-                                        std::vector<double>(lbr_fri_ros2::CARTESIAN_DOF, 1.0));
-    this->get_node()->declare_parameter("admittance.damping",
-                                        std::vector<double>(lbr_fri_ros2::CARTESIAN_DOF, 0.0));
-    this->get_node()->declare_parameter("admittance.stiffness",
-                                        std::vector<double>(lbr_fri_ros2::CARTESIAN_DOF, 0.0));
-    this->get_node()->declare_parameter("inv_jac_ctrl.chain_root", "lbr_link_0");
-    this->get_node()->declare_parameter("inv_jac_ctrl.chain_tip", "lbr_link_ee");
-    this->get_node()->declare_parameter("inv_jac_ctrl.damping", 0.2);
-    this->get_node()->declare_parameter("inv_jac_ctrl.max_linear_velocity", 0.1);
-    this->get_node()->declare_parameter("inv_jac_ctrl.max_angular_velocity", 0.1);
-    this->get_node()->declare_parameter("inv_jac_ctrl.joint_gains",
-                                        std::vector<double>(lbr_fri_ros2::N_JNTS, 0.0));
-    this->get_node()->declare_parameter("inv_jac_ctrl.cartesian_gains",
-                                        std::vector<double>(lbr_fri_ros2::CARTESIAN_DOF, 0.0));
+    if (!this->get_node()->has_parameter("robot_description")) {
+      this->get_node()->declare_parameter("robot_description", "");
+    }
+    if (!this->get_node()->has_parameter("robot_name")) {
+      this->get_node()->declare_parameter("robot_name", "lbr");
+    }
+    if (!this->get_node()->has_parameter("admittance.mass")) {
+      this->get_node()->declare_parameter("admittance.mass",
+                                          std::vector<double>(lbr_fri_ros2::CARTESIAN_DOF, 1.0));
+    }
+    if (!this->get_node()->has_parameter("admittance.damping")) {
+      this->get_node()->declare_parameter("admittance.damping",
+                                          std::vector<double>(lbr_fri_ros2::CARTESIAN_DOF, 0.0));
+    }
+    if (!this->get_node()->has_parameter("admittance.stiffness")) {
+      this->get_node()->declare_parameter("admittance.stiffness",
+                                          std::vector<double>(lbr_fri_ros2::CARTESIAN_DOF, 0.0));
+    }
+    if (!this->get_node()->has_parameter("inv_jac_ctrl.chain_root")) {
+      this->get_node()->declare_parameter("inv_jac_ctrl.chain_root", "lbr_link_0");
+    }
+    if (!this->get_node()->has_parameter("inv_jac_ctrl.chain_tip")) {
+      this->get_node()->declare_parameter("inv_jac_ctrl.chain_tip", "lbr_link_ee");
+    }
+    if (!this->get_node()->has_parameter("inv_jac_ctrl.damping")) {
+      this->get_node()->declare_parameter("inv_jac_ctrl.damping", 0.2);
+    }
+    if (!this->get_node()->has_parameter("inv_jac_ctrl.max_linear_velocity")) {
+      this->get_node()->declare_parameter("inv_jac_ctrl.max_linear_velocity", 0.1);
+    }
+    if (!this->get_node()->has_parameter("inv_jac_ctrl.max_angular_velocity")) {
+      this->get_node()->declare_parameter("inv_jac_ctrl.max_angular_velocity", 0.1);
+    }
+    if (!this->get_node()->has_parameter("inv_jac_ctrl.joint_gains")) {
+      this->get_node()->declare_parameter("inv_jac_ctrl.joint_gains",
+                                          std::vector<double>(lbr_fri_ros2::N_JNTS, 0.0));
+    }
+    if (!this->get_node()->has_parameter("inv_jac_ctrl.cartesian_gains")) {
+      this->get_node()->declare_parameter("inv_jac_ctrl.cartesian_gains",
+                                          std::vector<double>(lbr_fri_ros2::CARTESIAN_DOF, 0.0));
+    }
+    robot_description_ = this->get_node()->get_parameter("robot_description").as_string();
+    if (robot_description_.empty()) {
+      throw std::runtime_error("No robot description provided");
+    }
     configure_joint_names_();
     configure_admittance_impl_();
     configure_inv_jac_ctrl_impl_();
@@ -285,7 +314,7 @@ void AdmittanceController::configure_inv_jac_ctrl_impl_() {
         this->get_node()->get_parameter("inv_jac_ctrl.cartesian_gains").as_double_array()[i];
   }
   inv_jac_ctrl_impl_ptr_ = std::make_unique<lbr_fri_ros2::InvJacCtrlImpl>(
-      this->get_robot_description(),
+      robot_description_,
       lbr_fri_ros2::InvJacCtrlParameters{
           this->get_node()->get_parameter("inv_jac_ctrl.chain_root").as_string(),
           this->get_node()->get_parameter("inv_jac_ctrl.chain_tip").as_string(),

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
@@ -29,7 +29,9 @@ 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");
+    if (!this->get_node()->has_parameter("robot_name")) {
+      this->get_node()->declare_parameter("robot_name", "lbr");
+    }
     configure_joint_names_();
   } catch (const std::exception &e) {
     RCLCPP_ERROR(this->get_node()->get_logger(),

diff --git a/lbr_ros2_control/src/controllers/lbr_state_broadcaster.cpp b/lbr_ros2_control/src/controllers/lbr_state_broadcaster.cpp
@@ -21,7 +21,9 @@ controller_interface::CallbackReturn LBRStateBroadcaster::on_init() {
     rt_state_publisher_ptr_ =
         std::make_shared<realtime_tools::RealtimePublisher<lbr_fri_idl::msg::LBRState>>(
             state_publisher_ptr_);
-    this->get_node()->declare_parameter("robot_name", "lbr");
+    if (!this->get_node()->has_parameter("robot_name")) {
+      this->get_node()->declare_parameter("robot_name", "lbr");
+    }
     configure_joint_names_();
   } catch (const std::exception &e) {
     RCLCPP_ERROR(this->get_node()->get_logger(),

diff --git a/lbr_ros2_control/src/controllers/lbr_torque_command_controller.cpp b/lbr_ros2_control/src/controllers/lbr_torque_command_controller.cpp
@@ -28,7 +28,9 @@ 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");
+    if (!this->get_node()->has_parameter("robot_name")) {
+      this->get_node()->declare_parameter("robot_name", "lbr");
+    }
     configure_joint_names_();
   } catch (const std::exception &e) {
     RCLCPP_ERROR(this->get_node()->get_logger(),

diff --git a/lbr_ros2_control/src/controllers/lbr_wrench_command_controller.cpp b/lbr_ros2_control/src/controllers/lbr_wrench_command_controller.cpp
@@ -33,7 +33,9 @@ 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");
+    if (!this->get_node()->has_parameter("robot_name")) {
+      this->get_node()->declare_parameter("robot_name", "lbr");
+    }
     configure_joint_names_();
   } catch (const std::exception &e) {
     RCLCPP_ERROR(this->get_node()->get_logger(),

diff --git a/lbr_ros2_control/src/controllers/twist_controller.cpp b/lbr_ros2_control/src/controllers/twist_controller.cpp
@@ -34,18 +34,45 @@ controller_interface::CallbackReturn TwistController::on_init() {
           rt_twist_ptr_.writeFromNonRT(msg);
           updates_since_last_command_ = 0;
         });
-    this->get_node()->declare_parameter("robot_name", "lbr");
-    this->get_node()->declare_parameter("inv_jac_ctrl.chain_root", "lbr_link_0");
-    this->get_node()->declare_parameter("inv_jac_ctrl.chain_tip", "lbr_link_ee");
-    this->get_node()->declare_parameter("inv_jac_ctrl.twist_in_tip_frame", true);
-    this->get_node()->declare_parameter("inv_jac_ctrl.damping", 0.2);
-    this->get_node()->declare_parameter("inv_jac_ctrl.max_linear_velocity", 0.1);
-    this->get_node()->declare_parameter("inv_jac_ctrl.max_angular_velocity", 0.1);
-    this->get_node()->declare_parameter("inv_jac_ctrl.joint_gains",
-                                        std::vector<double>(lbr_fri_ros2::N_JNTS, 0.0));
-    this->get_node()->declare_parameter("inv_jac_ctrl.cartesian_gains",
-                                        std::vector<double>(lbr_fri_ros2::CARTESIAN_DOF, 0.0));
-    this->get_node()->declare_parameter("timeout", 0.2);
+    if (!this->get_node()->has_parameter("robot_description")) {
+      this->get_node()->declare_parameter("robot_description", "");
+    }
+    if (!this->get_node()->has_parameter("robot_name")) {
+      this->get_node()->declare_parameter("robot_name", "lbr");
+    }
+    if (!this->get_node()->has_parameter("inv_jac_ctrl.chain_root")) {
+      this->get_node()->declare_parameter("inv_jac_ctrl.chain_root", "lbr_link_0");
+    }
+    if (!this->get_node()->has_parameter("inv_jac_ctrl.chain_tip")) {
+      this->get_node()->declare_parameter("inv_jac_ctrl.chain_tip", "lbr_link_ee");
+    }
+    if (!this->get_node()->has_parameter("inv_jac_ctrl.twist_in_tip_frame")) {
+      this->get_node()->declare_parameter("inv_jac_ctrl.twist_in_tip_frame", true);
+    }
+    if (!this->get_node()->has_parameter("inv_jac_ctrl.damping")) {
+      this->get_node()->declare_parameter("inv_jac_ctrl.damping", 0.2);
+    }
+    if (!this->get_node()->has_parameter("inv_jac_ctrl.max_linear_velocity")) {
+      this->get_node()->declare_parameter("inv_jac_ctrl.max_linear_velocity", 0.1);
+    }
+    if (!this->get_node()->has_parameter("inv_jac_ctrl.max_angular_velocity")) {
+      this->get_node()->declare_parameter("inv_jac_ctrl.max_angular_velocity", 0.1);
+    }
+    if (!this->get_node()->has_parameter("inv_jac_ctrl.joint_gains")) {
+      this->get_node()->declare_parameter("inv_jac_ctrl.joint_gains",
+                                          std::vector<double>(lbr_fri_ros2::N_JNTS, 0.0));
+    }
+    if (!this->get_node()->has_parameter("inv_jac_ctrl.cartesian_gains")) {
+      this->get_node()->declare_parameter("inv_jac_ctrl.cartesian_gains",
+                                          std::vector<double>(lbr_fri_ros2::CARTESIAN_DOF, 0.0));
+    }
+    if (!this->get_node()->has_parameter("timeout")) {
+      this->get_node()->declare_parameter("timeout", 0.2);
+    }
+    robot_description_ = this->get_node()->get_parameter("robot_description").as_string();
+    if (robot_description_.empty()) {
+      throw std::runtime_error("No robot description provided");
+    }
     configure_joint_names_();
     configure_inv_jac_ctrl_impl_();
     log_info_();
@@ -200,7 +227,7 @@ void TwistController::configure_inv_jac_ctrl_impl_() {
         this->get_node()->get_parameter("inv_jac_ctrl.cartesian_gains").as_double_array()[i];
   }
   inv_jac_ctrl_impl_ptr_ = std::make_unique<lbr_fri_ros2::InvJacCtrlImpl>(
-      this->get_robot_description(),
+      robot_description_,
       lbr_fri_ros2::InvJacCtrlParameters{
           this->get_node()->get_parameter("inv_jac_ctrl.chain_root").as_string(),
           this->get_node()->get_parameter("inv_jac_ctrl.chain_tip").as_string(),