From 8d3662cccdd0666c645fa2cf8b21e9c7b728c98a Mon Sep 17 00:00:00 2001 From: Lennart Nachtigall Date: Wed, 8 Nov 2023 10:34:23 +0100 Subject: [PATCH 1/3] added option for setting speedscaling factor via topic --- .../scaled_joint_trajectory_controller.hpp | 8 ++++++- .../scaled_joint_trajectory_controller.cpp | 24 +++++++++++++------ ...oint_trajectory_controller_parameters.yaml | 5 ++++ 3 files changed, 29 insertions(+), 8 deletions(-) diff --git a/ur_controllers/include/ur_controllers/scaled_joint_trajectory_controller.hpp b/ur_controllers/include/ur_controllers/scaled_joint_trajectory_controller.hpp index 75a450547..d42bdc3fa 100644 --- a/ur_controllers/include/ur_controllers/scaled_joint_trajectory_controller.hpp +++ b/ur_controllers/include/ur_controllers/scaled_joint_trajectory_controller.hpp @@ -45,11 +45,15 @@ #include "rclcpp/duration.hpp" #include "scaled_joint_trajectory_controller_parameters.hpp" +#include + namespace ur_controllers { class ScaledJointTrajectoryController : public joint_trajectory_controller::JointTrajectoryController { public: + using ScalingFactorMsg = std_msgs::msg::Float64; + ScaledJointTrajectoryController() = default; ~ScaledJointTrajectoryController() override = default; @@ -73,11 +77,13 @@ class ScaledJointTrajectoryController : public joint_trajectory_controller::Join }; private: - double scaling_factor_{}; + double scaling_factor_{1.0}; realtime_tools::RealtimeBuffer time_data_; std::shared_ptr scaled_param_listener_; scaled_joint_trajectory_controller::Params scaled_params_; + + rclcpp::Subscription::SharedPtr scaling_factor_sub_; }; } // namespace ur_controllers diff --git a/ur_controllers/src/scaled_joint_trajectory_controller.cpp b/ur_controllers/src/scaled_joint_trajectory_controller.cpp index e7cee27de..9e3b71835 100644 --- a/ur_controllers/src/scaled_joint_trajectory_controller.cpp +++ b/ur_controllers/src/scaled_joint_trajectory_controller.cpp @@ -58,13 +58,21 @@ controller_interface::InterfaceConfiguration ScaledJointTrajectoryController::st { controller_interface::InterfaceConfiguration conf; conf = JointTrajectoryController::state_interface_configuration(); - conf.names.push_back(scaled_params_.speed_scaling_interface_name); + if(!scaled_params_.use_speed_scaling_topic_instead) + conf.names.push_back(scaled_params_.speed_scaling_interface_name); return conf; } controller_interface::CallbackReturn ScaledJointTrajectoryController::on_activate(const rclcpp_lifecycle::State& state) { + if(scaled_params_.use_speed_scaling_topic_instead){ + scaling_factor_sub_ = get_node()->create_subscription("~/speed_scaling_factor",10, [&](const ScalingFactorMsg& msg){ + scaling_factor_ = std::clamp( msg.data/100.0, 0.0,1.0); + }); + } + + TimeData time_data; time_data.time = get_node()->now(); time_data.period = rclcpp::Duration::from_nanoseconds(0); @@ -75,12 +83,14 @@ controller_interface::CallbackReturn ScaledJointTrajectoryController::on_activat controller_interface::return_type ScaledJointTrajectoryController::update(const rclcpp::Time& time, const rclcpp::Duration& period) -{ - if (state_interfaces_.back().get_name() == scaled_params_.speed_scaling_interface_name) { - scaling_factor_ = state_interfaces_.back().get_value(); - } else { - RCLCPP_ERROR(get_node()->get_logger(), "Speed scaling interface (%s) not found in hardware interface.", - scaled_params_.speed_scaling_interface_name.c_str()); +{ + if(!scaled_params_.use_speed_scaling_topic_instead){ + if (state_interfaces_.back().get_name() == scaled_params_.speed_scaling_interface_name) { + scaling_factor_ = state_interfaces_.back().get_value(); + } else { + RCLCPP_ERROR(get_node()->get_logger(), "Speed scaling interface (%s) not found in hardware interface.", + scaled_params_.speed_scaling_interface_name.c_str()); + } } if (get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) { diff --git a/ur_controllers/src/scaled_joint_trajectory_controller_parameters.yaml b/ur_controllers/src/scaled_joint_trajectory_controller_parameters.yaml index e72b0e58c..0c1c8cb35 100644 --- a/ur_controllers/src/scaled_joint_trajectory_controller_parameters.yaml +++ b/ur_controllers/src/scaled_joint_trajectory_controller_parameters.yaml @@ -4,3 +4,8 @@ scaled_joint_trajectory_controller: default_value: "speed_scaling/speed_scaling_factor", description: "Fully qualified name of the speed scaling interface name" } + use_speed_scaling_topic_instead: { + type: bool, + default_value: false, + description: "Instead of using the speed_scaling_interface_name listen on ~/speed_scaling_factor" + } From 61baffbf3933e793fafc93965577ed58ceda0ba4 Mon Sep 17 00:00:00 2001 From: Lennart Nachtigall Date: Wed, 8 Nov 2023 10:36:11 +0100 Subject: [PATCH 2/3] formatting --- .../scaled_joint_trajectory_controller.hpp | 4 ++-- .../src/scaled_joint_trajectory_controller.cpp | 17 ++++++++--------- 2 files changed, 10 insertions(+), 11 deletions(-) diff --git a/ur_controllers/include/ur_controllers/scaled_joint_trajectory_controller.hpp b/ur_controllers/include/ur_controllers/scaled_joint_trajectory_controller.hpp index d42bdc3fa..344cf1e94 100644 --- a/ur_controllers/include/ur_controllers/scaled_joint_trajectory_controller.hpp +++ b/ur_controllers/include/ur_controllers/scaled_joint_trajectory_controller.hpp @@ -52,7 +52,7 @@ namespace ur_controllers class ScaledJointTrajectoryController : public joint_trajectory_controller::JointTrajectoryController { public: - using ScalingFactorMsg = std_msgs::msg::Float64; + using ScalingFactorMsg = std_msgs::msg::Float64; ScaledJointTrajectoryController() = default; ~ScaledJointTrajectoryController() override = default; @@ -77,7 +77,7 @@ class ScaledJointTrajectoryController : public joint_trajectory_controller::Join }; private: - double scaling_factor_{1.0}; + double scaling_factor_{ 1.0 }; realtime_tools::RealtimeBuffer time_data_; std::shared_ptr scaled_param_listener_; diff --git a/ur_controllers/src/scaled_joint_trajectory_controller.cpp b/ur_controllers/src/scaled_joint_trajectory_controller.cpp index 9e3b71835..c28b07aae 100644 --- a/ur_controllers/src/scaled_joint_trajectory_controller.cpp +++ b/ur_controllers/src/scaled_joint_trajectory_controller.cpp @@ -58,7 +58,7 @@ controller_interface::InterfaceConfiguration ScaledJointTrajectoryController::st { controller_interface::InterfaceConfiguration conf; conf = JointTrajectoryController::state_interface_configuration(); - if(!scaled_params_.use_speed_scaling_topic_instead) + if (!scaled_params_.use_speed_scaling_topic_instead) conf.names.push_back(scaled_params_.speed_scaling_interface_name); return conf; @@ -66,13 +66,12 @@ controller_interface::InterfaceConfiguration ScaledJointTrajectoryController::st controller_interface::CallbackReturn ScaledJointTrajectoryController::on_activate(const rclcpp_lifecycle::State& state) { - if(scaled_params_.use_speed_scaling_topic_instead){ - scaling_factor_sub_ = get_node()->create_subscription("~/speed_scaling_factor",10, [&](const ScalingFactorMsg& msg){ - scaling_factor_ = std::clamp( msg.data/100.0, 0.0,1.0); - }); + if (scaled_params_.use_speed_scaling_topic_instead) { + scaling_factor_sub_ = get_node()->create_subscription( + "~/speed_scaling_factor", 10, + [&](const ScalingFactorMsg& msg) { scaling_factor_ = std::clamp(msg.data / 100.0, 0.0, 1.0); }); } - TimeData time_data; time_data.time = get_node()->now(); time_data.period = rclcpp::Duration::from_nanoseconds(0); @@ -83,13 +82,13 @@ controller_interface::CallbackReturn ScaledJointTrajectoryController::on_activat controller_interface::return_type ScaledJointTrajectoryController::update(const rclcpp::Time& time, const rclcpp::Duration& period) -{ - if(!scaled_params_.use_speed_scaling_topic_instead){ +{ + if (!scaled_params_.use_speed_scaling_topic_instead) { if (state_interfaces_.back().get_name() == scaled_params_.speed_scaling_interface_name) { scaling_factor_ = state_interfaces_.back().get_value(); } else { RCLCPP_ERROR(get_node()->get_logger(), "Speed scaling interface (%s) not found in hardware interface.", - scaled_params_.speed_scaling_interface_name.c_str()); + scaled_params_.speed_scaling_interface_name.c_str()); } } From 2d250a243a7843bb33d9fad81fb9a28ca622119f Mon Sep 17 00:00:00 2001 From: Lennart Nachtigall Date: Thu, 9 Nov 2023 09:18:37 +0100 Subject: [PATCH 3/3] Allow changing topic name, made topic transient_local --- ur_controllers/src/scaled_joint_trajectory_controller.cpp | 5 ++++- .../src/scaled_joint_trajectory_controller_parameters.yaml | 7 ++++++- 2 files changed, 10 insertions(+), 2 deletions(-) diff --git a/ur_controllers/src/scaled_joint_trajectory_controller.cpp b/ur_controllers/src/scaled_joint_trajectory_controller.cpp index c28b07aae..bd0451510 100644 --- a/ur_controllers/src/scaled_joint_trajectory_controller.cpp +++ b/ur_controllers/src/scaled_joint_trajectory_controller.cpp @@ -67,8 +67,11 @@ controller_interface::InterfaceConfiguration ScaledJointTrajectoryController::st controller_interface::CallbackReturn ScaledJointTrajectoryController::on_activate(const rclcpp_lifecycle::State& state) { if (scaled_params_.use_speed_scaling_topic_instead) { + auto qos = rclcpp::QoS(10); + qos.transient_local(); + scaling_factor_sub_ = get_node()->create_subscription( - "~/speed_scaling_factor", 10, + scaled_params_.speed_scaling_topic_name, qos, [&](const ScalingFactorMsg& msg) { scaling_factor_ = std::clamp(msg.data / 100.0, 0.0, 1.0); }); } diff --git a/ur_controllers/src/scaled_joint_trajectory_controller_parameters.yaml b/ur_controllers/src/scaled_joint_trajectory_controller_parameters.yaml index 0c1c8cb35..0e7785bb4 100644 --- a/ur_controllers/src/scaled_joint_trajectory_controller_parameters.yaml +++ b/ur_controllers/src/scaled_joint_trajectory_controller_parameters.yaml @@ -7,5 +7,10 @@ scaled_joint_trajectory_controller: use_speed_scaling_topic_instead: { type: bool, default_value: false, - description: "Instead of using the speed_scaling_interface_name listen on ~/speed_scaling_factor" + description: "Instead of using the speed_scaling_interface_name listen on " + } + speed_scaling_topic_name: { + type: string, + default_value: "~/speed_scaling_factor", + description: "Topic name for the speed scaling factor (if enabled)" }