Skip to content

Commit 90d289d

Browse files
mergify[bot]firesurfer
mergify[bot]
and
firesurfer
authored
Ported controllers to generate_parameters libary and added prefix for controllers (Multiarm part 2) (#594) (#695)
* This commits adds additional configuration parameters needed for multiarm support. Additionally is cleans up the parameters parsing in the on_activate method. Added parameters: - trajectory_port - script_command_port - keep_alive_count See Universal_Robots_ROS2_Description for more explainations * Port gpio_controller to generate_parameters. Added prefix to gpio_controller and speed_scaling_state_broad_caster (and the hardware interface where applicable) * fix remaining issues after rebasing onto main * using tf_prefix instead of prefix * reverted prefixing of gpio controller topics. Topics will now use the standard ros2 control scheme: <controller name>/<topic name> * Readded scaled JTC and adapted it for multiarm setup * added empty tf_prefix workaround * Implemented suggested description renaming * Readded startRTDECommunication * Code formatting * replaced prefix with tf_prefix * remove old flag * readd scaled jtc to CMakeLists * Renamed more prefix to tf_prefix occurences * removed old tf_prefix line * Code formatting Fixed EOF in yaml files * Updated example controllers with tf_prefix --------- Co-authored-by: Lennart Nachtigall <[email protected]> Co-authored-by: Lennart Nachtigall <[email protected]> Co-authored-by: Lennart Nachtigall <[email protected]> Co-authored-by: Felix Exner <[email protected]> (cherry picked from commit d8b2964) # Conflicts: # ur_controllers/src/speed_scaling_state_broadcaster.cpp Co-authored-by: Lennart Nachtigall <[email protected]>
1 parent b1bd00b commit 90d289d

12 files changed

+227
-98
lines changed

‎ur_controllers/CMakeLists.txt‎

Lines changed: 25 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -18,6 +18,7 @@ find_package(std_msgs REQUIRED)
1818
find_package(std_srvs REQUIRED)
1919
find_package(ur_dashboard_msgs REQUIRED)
2020
find_package(ur_msgs REQUIRED)
21+
find_package(generate_parameter_library REQUIRED)
2122

2223
set(THIS_PACKAGE_INCLUDE_DEPENDS
2324
angles
@@ -32,10 +33,27 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS
3233
std_srvs
3334
ur_dashboard_msgs
3435
ur_msgs
36+
generate_parameter_library
3537
)
3638

3739
include_directories(include)
3840

41+
42+
generate_parameter_library(
43+
gpio_controller_parameters
44+
src/gpio_controller_parameters.yaml
45+
)
46+
47+
generate_parameter_library(
48+
speed_scaling_state_broadcaster_parameters
49+
src/speed_scaling_state_broadcaster_parameters.yaml
50+
)
51+
52+
generate_parameter_library(
53+
scaled_joint_trajectory_controller_parameters
54+
src/scaled_joint_trajectory_controller_parameters.yaml
55+
)
56+
3957
add_library(${PROJECT_NAME} SHARED
4058
src/scaled_joint_trajectory_controller.cpp
4159
src/speed_scaling_state_broadcaster.cpp
@@ -44,11 +62,17 @@ add_library(${PROJECT_NAME} SHARED
4462
target_include_directories(${PROJECT_NAME} PRIVATE
4563
include
4664
)
47-
65+
target_link_libraries(${PROJECT_NAME}
66+
gpio_controller_parameters
67+
speed_scaling_state_broadcaster_parameters
68+
scaled_joint_trajectory_controller_parameters
69+
)
4870
ament_target_dependencies(${PROJECT_NAME}
4971
${THIS_PACKAGE_INCLUDE_DEPENDS}
5072
)
5173

74+
target_compile_options(${PROJECT_NAME} PRIVATE -Wpedantic -Werror=return-type)
75+
5276
# prevent pluginlib from using boost
5377
target_compile_definitions(${PROJECT_NAME} PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS")
5478
pluginlib_export_plugin_description_file(controller_interface controller_plugins.xml)

‎ur_controllers/include/ur_controllers/gpio_controller.hpp‎

Lines changed: 5 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -56,6 +56,7 @@
5656
#include "rclcpp/time.hpp"
5757
#include "rclcpp/duration.hpp"
5858
#include "std_msgs/msg/bool.hpp"
59+
#include "gpio_controller_parameters.hpp"
5960

6061
namespace ur_controllers
6162
{
@@ -175,6 +176,10 @@ class GPIOController : public controller_interface::ControllerInterface
175176
ur_dashboard_msgs::msg::SafetyMode safety_mode_msg_;
176177
std_msgs::msg::Bool program_running_msg_;
177178

179+
// Parameters from ROS for gpio_controller
180+
std::shared_ptr<gpio_controller::ParamListener> param_listener_;
181+
gpio_controller::Params params_;
182+
178183
static constexpr double ASYNC_WAITING = 2.0;
179184
// TODO(anyone) publishers to add: tcp_pose_pub_
180185
// TODO(anyone) subscribers to add: script_command_sub_

‎ur_controllers/include/ur_controllers/scaled_joint_trajectory_controller.hpp‎

Lines changed: 7 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -43,6 +43,7 @@
4343
#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp"
4444
#include "rclcpp/time.hpp"
4545
#include "rclcpp/duration.hpp"
46+
#include "scaled_joint_trajectory_controller_parameters.hpp"
4647

4748
namespace ur_controllers
4849
{
@@ -58,6 +59,8 @@ class ScaledJointTrajectoryController : public joint_trajectory_controller::Join
5859

5960
controller_interface::return_type update(const rclcpp::Time& time, const rclcpp::Duration& period) override;
6061

62+
CallbackReturn on_init() override;
63+
6164
protected:
6265
struct TimeData
6366
{
@@ -70,8 +73,11 @@ class ScaledJointTrajectoryController : public joint_trajectory_controller::Join
7073
};
7174

7275
private:
73-
double scaling_factor_;
76+
double scaling_factor_{};
7477
realtime_tools::RealtimeBuffer<TimeData> time_data_;
78+
79+
std::shared_ptr<scaled_joint_trajectory_controller::ParamListener> scaled_param_listener_;
80+
scaled_joint_trajectory_controller::Params scaled_params_;
7581
};
7682
} // namespace ur_controllers
7783

‎ur_controllers/include/ur_controllers/speed_scaling_state_broadcaster.hpp‎

Lines changed: 5 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -48,6 +48,7 @@
4848
#include "rclcpp/time.hpp"
4949
#include "rclcpp/duration.hpp"
5050
#include "std_msgs/msg/float64.hpp"
51+
#include "speed_scaling_state_broadcaster_parameters.hpp"
5152

5253
namespace ur_controllers
5354
{
@@ -76,6 +77,10 @@ class SpeedScalingStateBroadcaster : public controller_interface::ControllerInte
7677

7778
std::shared_ptr<rclcpp::Publisher<std_msgs::msg::Float64>> speed_scaling_state_publisher_;
7879
std_msgs::msg::Float64 speed_scaling_state_msg_;
80+
81+
// Parameters from ROS for SpeedScalingStateBroadcaster
82+
std::shared_ptr<speed_scaling_state_broadcaster::ParamListener> param_listener_;
83+
speed_scaling_state_broadcaster::Params params_;
7984
};
8085
} // namespace ur_controllers
8186
#endif // UR_CONTROLLERS__SPEED_SCALING_STATE_BROADCASTER_HPP_

‎ur_controllers/src/gpio_controller.cpp‎

Lines changed: 60 additions & 38 deletions
Original file line numberDiff line numberDiff line change
@@ -43,7 +43,16 @@ namespace ur_controllers
4343
{
4444
controller_interface::CallbackReturn GPIOController::on_init()
4545
{
46-
initMsgs();
46+
try {
47+
initMsgs();
48+
// Create the parameter listener and get the parameters
49+
param_listener_ = std::make_shared<gpio_controller::ParamListener>(get_node());
50+
params_ = param_listener_->get_params();
51+
52+
} catch (const std::exception& e) {
53+
fprintf(stderr, "Exception thrown during init stage with message: %s \n", e.what());
54+
return CallbackReturn::ERROR;
55+
}
4756

4857
return controller_interface::CallbackReturn::SUCCESS;
4958
}
@@ -53,40 +62,38 @@ controller_interface::InterfaceConfiguration GPIOController::command_interface_c
5362
controller_interface::InterfaceConfiguration config;
5463
config.type = controller_interface::interface_configuration_type::INDIVIDUAL;
5564

65+
const std::string tf_prefix = params_.tf_prefix;
66+
RCLCPP_INFO(get_node()->get_logger(), "Configure UR gpio controller with tf_prefix: %s", tf_prefix.c_str());
67+
5668
for (size_t i = 0; i < 18; ++i) {
57-
config.names.emplace_back("gpio/standard_digital_output_cmd_" + std::to_string(i));
69+
config.names.emplace_back(tf_prefix + "gpio/standard_digital_output_cmd_" + std::to_string(i));
5870
}
5971

6072
for (size_t i = 0; i < 2; ++i) {
61-
config.names.emplace_back("gpio/standard_analog_output_cmd_" + std::to_string(i));
73+
config.names.emplace_back(tf_prefix + "gpio/standard_analog_output_cmd_" + std::to_string(i));
6274
}
75+
config.names.emplace_back(tf_prefix + "gpio/tool_voltage_cmd");
6376

64-
config.names.emplace_back("gpio/tool_voltage_cmd");
77+
config.names.emplace_back(tf_prefix + "gpio/io_async_success");
6578

66-
config.names.emplace_back("gpio/io_async_success");
79+
config.names.emplace_back(tf_prefix + "speed_scaling/target_speed_fraction_cmd");
6780

68-
config.names.emplace_back("speed_scaling/target_speed_fraction_cmd");
81+
config.names.emplace_back(tf_prefix + "speed_scaling/target_speed_fraction_async_success");
6982

70-
config.names.emplace_back("speed_scaling/target_speed_fraction_async_success");
83+
config.names.emplace_back(tf_prefix + "resend_robot_program/resend_robot_program_cmd");
7184

72-
config.names.emplace_back("resend_robot_program/resend_robot_program_cmd");
73-
74-
config.names.emplace_back("resend_robot_program/resend_robot_program_async_success");
85+
config.names.emplace_back(tf_prefix + "resend_robot_program/resend_robot_program_async_success");
7586

7687
// payload stuff
77-
config.names.emplace_back("payload/mass");
78-
config.names.emplace_back("payload/cog.x");
79-
config.names.emplace_back("payload/cog.y");
80-
config.names.emplace_back("payload/cog.z");
81-
config.names.emplace_back("payload/payload_async_success");
82-
83-
// zero ft sensor
84-
config.names.emplace_back("zero_ftsensor/zero_ftsensor_cmd");
85-
config.names.emplace_back("zero_ftsensor/zero_ftsensor_async_success");
88+
config.names.emplace_back(tf_prefix + "payload/mass");
89+
config.names.emplace_back(tf_prefix + "payload/cog.x");
90+
config.names.emplace_back(tf_prefix + "payload/cog.y");
91+
config.names.emplace_back(tf_prefix + "payload/cog.z");
92+
config.names.emplace_back(tf_prefix + "payload/payload_async_success");
8693

8794
// hand back control --> make UR-program return
88-
config.names.emplace_back("hand_back_control/hand_back_control_cmd");
89-
config.names.emplace_back("hand_back_control/hand_back_control_async_success");
95+
config.names.emplace_back(tf_prefix + "hand_back_control/hand_back_control_cmd");
96+
config.names.emplace_back(tf_prefix + "hand_back_control/hand_back_control_async_success");
9097

9198
return config;
9299
}
@@ -96,56 +103,58 @@ controller_interface::InterfaceConfiguration ur_controllers::GPIOController::sta
96103
controller_interface::InterfaceConfiguration config;
97104
config.type = controller_interface::interface_configuration_type::INDIVIDUAL;
98105

106+
const std::string tf_prefix = params_.tf_prefix;
107+
99108
// digital io
100109
for (size_t i = 0; i < 18; ++i) {
101-
config.names.emplace_back("gpio/digital_output_" + std::to_string(i));
110+
config.names.emplace_back(tf_prefix + "gpio/digital_output_" + std::to_string(i));
102111
}
103112

104113
for (size_t i = 0; i < 18; ++i) {
105-
config.names.emplace_back("gpio/digital_input_" + std::to_string(i));
114+
config.names.emplace_back(tf_prefix + "gpio/digital_input_" + std::to_string(i));
106115
}
107116

108117
// analog io
109118
for (size_t i = 0; i < 2; ++i) {
110-
config.names.emplace_back("gpio/standard_analog_output_" + std::to_string(i));
119+
config.names.emplace_back(tf_prefix + "gpio/standard_analog_output_" + std::to_string(i));
111120
}
112121

113122
for (size_t i = 0; i < 2; ++i) {
114-
config.names.emplace_back("gpio/standard_analog_input_" + std::to_string(i));
123+
config.names.emplace_back(tf_prefix + "gpio/standard_analog_input_" + std::to_string(i));
115124
}
116125

117126
for (size_t i = 0; i < 4; ++i) {
118-
config.names.emplace_back("gpio/analog_io_type_" + std::to_string(i));
127+
config.names.emplace_back(tf_prefix + "gpio/analog_io_type_" + std::to_string(i));
119128
}
120129

121130
// tool
122-
config.names.emplace_back("gpio/tool_mode");
123-
config.names.emplace_back("gpio/tool_output_voltage");
124-
config.names.emplace_back("gpio/tool_output_current");
125-
config.names.emplace_back("gpio/tool_temperature");
131+
config.names.emplace_back(tf_prefix + "gpio/tool_mode");
132+
config.names.emplace_back(tf_prefix + "gpio/tool_output_voltage");
133+
config.names.emplace_back(tf_prefix + "gpio/tool_output_current");
134+
config.names.emplace_back(tf_prefix + "gpio/tool_temperature");
126135

127136
for (size_t i = 0; i < 2; ++i) {
128-
config.names.emplace_back("gpio/tool_analog_input_" + std::to_string(i));
137+
config.names.emplace_back(tf_prefix + "gpio/tool_analog_input_" + std::to_string(i));
129138
}
130139
for (size_t i = 0; i < 2; ++i) {
131-
config.names.emplace_back("gpio/tool_analog_input_type_" + std::to_string(i));
140+
config.names.emplace_back(tf_prefix + "gpio/tool_analog_input_type_" + std::to_string(i));
132141
}
133142

134143
// robot
135-
config.names.emplace_back("gpio/robot_mode");
144+
config.names.emplace_back(tf_prefix + "gpio/robot_mode");
136145
for (size_t i = 0; i < 4; ++i) {
137-
config.names.emplace_back("gpio/robot_status_bit_" + std::to_string(i));
146+
config.names.emplace_back(tf_prefix + "gpio/robot_status_bit_" + std::to_string(i));
138147
}
139148

140149
// safety
141-
config.names.emplace_back("gpio/safety_mode");
150+
config.names.emplace_back(tf_prefix + "gpio/safety_mode");
142151
for (size_t i = 0; i < 11; ++i) {
143-
config.names.emplace_back("gpio/safety_status_bit_" + std::to_string(i));
152+
config.names.emplace_back(tf_prefix + "gpio/safety_status_bit_" + std::to_string(i));
144153
}
145-
config.names.emplace_back("system_interface/initialized");
154+
config.names.emplace_back(tf_prefix + "system_interface/initialized");
146155

147156
// program running
148-
config.names.emplace_back("gpio/program_running");
157+
config.names.emplace_back(tf_prefix + "gpio/program_running");
149158

150159
return config;
151160
}
@@ -164,6 +173,19 @@ controller_interface::return_type ur_controllers::GPIOController::update(const r
164173
controller_interface::CallbackReturn
165174
ur_controllers::GPIOController::on_configure(const rclcpp_lifecycle::State& /*previous_state*/)
166175
{
176+
const auto logger = get_node()->get_logger();
177+
178+
if (!param_listener_) {
179+
RCLCPP_ERROR(get_node()->get_logger(), "Error encountered during init");
180+
return controller_interface::CallbackReturn::ERROR;
181+
}
182+
183+
// update the dynamic map parameters
184+
param_listener_->refresh_dynamic_parameters();
185+
186+
// get parameters from the listener in case they were updated
187+
params_ = param_listener_->get_params();
188+
167189
return LifecycleNodeInterface::CallbackReturn::SUCCESS;
168190
}
169191

‎ur_controllers/src/gpio_controller_parameters.yaml‎

Lines changed: 6 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,6 @@
1+
gpio_controller:
2+
tf_prefix: {
3+
type: string,
4+
default_value: "",
5+
description: "Urdf prefix of the corresponding arm"
6+
}

‎ur_controllers/src/scaled_joint_trajectory_controller.cpp‎

Lines changed: 15 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -44,11 +44,22 @@
4444

4545
namespace ur_controllers
4646
{
47+
48+
controller_interface::CallbackReturn ScaledJointTrajectoryController::on_init()
49+
{
50+
// Create the parameter listener and get the parameters
51+
scaled_param_listener_ = std::make_shared<scaled_joint_trajectory_controller::ParamListener>(get_node());
52+
scaled_params_ = scaled_param_listener_->get_params();
53+
54+
return JointTrajectoryController::on_init();
55+
}
56+
4757
controller_interface::InterfaceConfiguration ScaledJointTrajectoryController::state_interface_configuration() const
4858
{
4959
controller_interface::InterfaceConfiguration conf;
5060
conf = JointTrajectoryController::state_interface_configuration();
51-
conf.names.push_back("speed_scaling/speed_scaling_factor");
61+
conf.names.push_back(scaled_params_.speed_scaling_interface_name);
62+
5263
return conf;
5364
}
5465

@@ -65,10 +76,11 @@ controller_interface::CallbackReturn ScaledJointTrajectoryController::on_activat
6576
controller_interface::return_type ScaledJointTrajectoryController::update(const rclcpp::Time& time,
6677
const rclcpp::Duration& /*period*/)
6778
{
68-
if (state_interfaces_.back().get_interface_name() == "speed_scaling_factor") {
79+
if (state_interfaces_.back().get_name() == scaled_params_.speed_scaling_interface_name) {
6980
scaling_factor_ = state_interfaces_.back().get_value();
7081
} else {
71-
RCLCPP_ERROR(get_node()->get_logger(), "Speed scaling interface not found in hardware interface.");
82+
RCLCPP_ERROR(get_node()->get_logger(), "Speed scaling interface (%s) not found in hardware interface.",
83+
scaled_params_.speed_scaling_interface_name.c_str());
7284
}
7385

7486
if (get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) {

‎ur_controllers/src/scaled_joint_trajectory_controller_parameters.yaml‎

Lines changed: 6 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,6 @@
1+
scaled_joint_trajectory_controller:
2+
speed_scaling_interface_name: {
3+
type: string,
4+
default_value: "speed_scaling/speed_scaling_factor",
5+
description: "Fully qualified name of the speed scaling interface name"
6+
}

‎ur_controllers/src/speed_scaling_state_broadcaster.cpp‎

Lines changed: 22 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -61,7 +61,13 @@ SpeedScalingStateBroadcaster::SpeedScalingStateBroadcaster()
6161
controller_interface::CallbackReturn SpeedScalingStateBroadcaster::on_init()
6262
{
6363
try {
64-
auto_declare<double>("state_publish_rate", 100.0);
64+
// Create the parameter listener and get the parameters
65+
param_listener_ = std::make_shared<speed_scaling_state_broadcaster::ParamListener>(get_node());
66+
params_ = param_listener_->get_params();
67+
68+
RCLCPP_INFO(get_node()->get_logger(), "Loading UR SpeedScalingStateBroadcaster with tf_prefix: %s",
69+
params_.tf_prefix.c_str());
70+
6571
} catch (std::exception& e) {
6672
fprintf(stderr, "Exception thrown during init stage with message: %s \n", e.what());
6773
return controller_interface::CallbackReturn::ERROR;
@@ -79,20 +85,30 @@ controller_interface::InterfaceConfiguration SpeedScalingStateBroadcaster::state
7985
{
8086
controller_interface::InterfaceConfiguration config;
8187
config.type = controller_interface::interface_configuration_type::INDIVIDUAL;
82-
config.names.push_back("speed_scaling/speed_scaling_factor");
88+
89+
const std::string tf_prefix = params_.tf_prefix;
90+
config.names.push_back(tf_prefix + "speed_scaling/speed_scaling_factor");
8391
return config;
8492
}
8593

8694
controller_interface::CallbackReturn
8795
SpeedScalingStateBroadcaster::on_configure(const rclcpp_lifecycle::State& /*previous_state*/)
8896
{
89-
if (!get_node()->get_parameter("state_publish_rate", publish_rate_)) {
90-
RCLCPP_INFO(get_node()->get_logger(), "Parameter 'state_publish_rate' not set");
97+
if (!param_listener_) {
98+
RCLCPP_ERROR(get_node()->get_logger(), "Error encountered during init");
9199
return controller_interface::CallbackReturn::ERROR;
92-
} else {
93-
RCLCPP_INFO(get_node()->get_logger(), "Publisher rate set to : %.1f Hz", publish_rate_);
94100
}
95101

102+
// update the dynamic map parameters
103+
param_listener_->refresh_dynamic_parameters();
104+
105+
// get parameters from the listener in case they were updated
106+
params_ = param_listener_->get_params();
107+
108+
publish_rate_ = params_.state_publish_rate;
109+
110+
RCLCPP_INFO(get_node()->get_logger(), "Publisher rate set to : %.1f Hz", publish_rate_);
111+
96112
try {
97113
speed_scaling_state_publisher_ =
98114
get_node()->create_publisher<std_msgs::msg::Float64>("~/speed_scaling", rclcpp::SystemDefaultsQoS());

0 commit comments

Comments
 (0)