diff --git a/.github/workflows/backport.yaml b/.github/workflows/backport.yaml deleted file mode 100644 index daacbb31..00000000 --- a/.github/workflows/backport.yaml +++ /dev/null @@ -1,34 +0,0 @@ -name: Backport merged pull request -on: - issue_comment: - types: [created] -permissions: - contents: write # so it can comment - pull-requests: write # so it can create pull requests -jobs: - backport: - name: Backport pull request - runs-on: ubuntu-latest - - # Only run when pull request is merged - # or when a comment starting with `/backport` is created by someone other than the - # https://github.com/backport-action bot user (user id: 97796249). Note that if you use your - # own PAT as `github_token`, that you should replace this id with yours. - if: > - ( - github.event_name == 'pull_request_target' && - github.event.pull_request.merged - ) || ( - github.event_name == 'issue_comment' && - github.event.issue.pull_request && - github.event.comment.user.id != 97796249 && - startsWith(github.event.comment.body, '/backport') - ) - steps: - - uses: actions/checkout@v4 - - name: Create backport pull requests - uses: korthout/backport-action@v3 - with: - label_pattern: "" - target_branches: humble - merge_commits: skip diff --git a/lbr_bringup/launch/hardware.launch.py b/lbr_bringup/launch/hardware.launch.py index d6ec6b9b..cffdf59b 100644 --- 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 index a87be2d4..fc7c24f7 100644 --- 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 index f98ef127..00f3a30e 100644 --- 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 index 17143903..7488d6b2 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 @@ -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 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 index f0a1fea4..6f202520 100644 --- 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 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 index bbe950b6..2b6c0b45 100644 --- 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(lbr_fri_ros2::CARTESIAN_DOF, 1.0)); - this->get_node()->declare_parameter("admittance.damping", - std::vector(lbr_fri_ros2::CARTESIAN_DOF, 0.0)); - this->get_node()->declare_parameter("admittance.stiffness", - std::vector(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(lbr_fri_ros2::N_JNTS, 0.0)); - this->get_node()->declare_parameter("inv_jac_ctrl.cartesian_gains", - std::vector(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(lbr_fri_ros2::CARTESIAN_DOF, 1.0)); + } + if (!this->get_node()->has_parameter("admittance.damping")) { + this->get_node()->declare_parameter("admittance.damping", + std::vector(lbr_fri_ros2::CARTESIAN_DOF, 0.0)); + } + if (!this->get_node()->has_parameter("admittance.stiffness")) { + this->get_node()->declare_parameter("admittance.stiffness", + std::vector(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(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(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( - 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 index 232acf93..5556159f 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,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 index 765bdd5b..cafd0a63 100644 --- 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>( 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 index b56bd062..62d027e4 100644 --- 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 index 37a278d9..87e669e4 100644 --- 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 index d851f165..379e89f6 100644 --- 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(lbr_fri_ros2::N_JNTS, 0.0)); - this->get_node()->declare_parameter("inv_jac_ctrl.cartesian_gains", - std::vector(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(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(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( - 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(),