Skip to content

Commit 3a13376

Browse files
christophfroehlichchristophfroehlich
authored
Update API for realtime publisher (ros-controls#1830)
1 parent 2186969 commit 3a13376

File tree

29 files changed

+328
-385
lines changed

29 files changed

+328
-385
lines changed

‎admittance_controller/include/admittance_controller/admittance_controller.hpp‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -121,6 +121,7 @@ class AdmittanceController : public controller_interface::ChainableControllerInt
121121
input_joint_command_;
122122
realtime_tools::RealtimeThreadSafeBox<geometry_msgs::msg::WrenchStamped> input_wrench_command_;
123123
std::unique_ptr<realtime_tools::RealtimePublisher<ControllerStateMsg>> state_publisher_;
124+
ControllerStateMsg state_msg_;
124125

125126
// save the last commands in case of unable to get value from box
126127
trajectory_msgs::msg::JointTrajectoryPoint joint_command_msg_;

‎admittance_controller/src/admittance_controller.cpp‎

Lines changed: 6 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -332,9 +332,7 @@ controller_interface::CallbackReturn AdmittanceController::on_configure(
332332
std::make_unique<realtime_tools::RealtimePublisher<ControllerStateMsg>>(s_publisher_);
333333

334334
// Initialize state message
335-
state_publisher_->lock();
336-
state_publisher_->msg_ = admittance_->get_controller_state();
337-
state_publisher_->unlock();
335+
state_msg_ = admittance_->get_controller_state();
338336

339337
// Initialize FTS semantic semantic_component
340338
force_torque_sensor_ = std::make_unique<semantic_components::ForceTorqueSensor>(
@@ -457,9 +455,11 @@ controller_interface::return_type AdmittanceController::update_and_write_command
457455
write_state_to_hardware(reference_admittance_);
458456

459457
// Publish controller state
460-
state_publisher_->lock();
461-
state_publisher_->msg_ = admittance_->get_controller_state();
462-
state_publisher_->unlockAndPublish();
458+
if (state_publisher_)
459+
{
460+
state_msg_ = admittance_->get_controller_state();
461+
state_publisher_->try_publish(state_msg_);
462+
}
463463

464464
return controller_interface::return_type::OK;
465465
}

‎diff_drive_controller/include/diff_drive_controller/diff_drive_controller.hpp‎

Lines changed: 3 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -113,11 +113,13 @@ class DiffDriveController : public controller_interface::ChainableControllerInte
113113
std::shared_ptr<rclcpp::Publisher<nav_msgs::msg::Odometry>> odometry_publisher_ = nullptr;
114114
std::shared_ptr<realtime_tools::RealtimePublisher<nav_msgs::msg::Odometry>>
115115
realtime_odometry_publisher_ = nullptr;
116+
nav_msgs::msg::Odometry odometry_message_;
116117

117118
std::shared_ptr<rclcpp::Publisher<tf2_msgs::msg::TFMessage>> odometry_transform_publisher_ =
118119
nullptr;
119120
std::shared_ptr<realtime_tools::RealtimePublisher<tf2_msgs::msg::TFMessage>>
120121
realtime_odometry_transform_publisher_ = nullptr;
122+
tf2_msgs::msg::TFMessage odometry_transform_message_;
121123

122124
bool subscriber_is_active_ = false;
123125
rclcpp::Subscription<TwistStamped>::SharedPtr velocity_command_subscriber_ = nullptr;
@@ -136,6 +138,7 @@ class DiffDriveController : public controller_interface::ChainableControllerInte
136138
std::shared_ptr<rclcpp::Publisher<TwistStamped>> limited_velocity_publisher_ = nullptr;
137139
std::shared_ptr<realtime_tools::RealtimePublisher<TwistStamped>>
138140
realtime_limited_velocity_publisher_ = nullptr;
141+
TwistStamped limited_velocity_message_;
139142

140143
rclcpp::Time previous_update_timestamp_{0};
141144

‎diff_drive_controller/src/diff_drive_controller.cpp‎

Lines changed: 27 additions & 30 deletions
Original file line numberDiff line numberDiff line change
@@ -231,32 +231,31 @@ controller_interface::return_type DiffDriveController::update_and_write_commands
231231

232232
if (should_publish)
233233
{
234-
if (realtime_odometry_publisher_->trylock())
234+
if (realtime_odometry_publisher_)
235235
{
236-
auto & odometry_message = realtime_odometry_publisher_->msg_;
237-
odometry_message.header.stamp = time;
238-
odometry_message.pose.pose.position.x = odometry_.getX();
239-
odometry_message.pose.pose.position.y = odometry_.getY();
240-
odometry_message.pose.pose.orientation.x = orientation.x();
241-
odometry_message.pose.pose.orientation.y = orientation.y();
242-
odometry_message.pose.pose.orientation.z = orientation.z();
243-
odometry_message.pose.pose.orientation.w = orientation.w();
244-
odometry_message.twist.twist.linear.x = odometry_.getLinear();
245-
odometry_message.twist.twist.angular.z = odometry_.getAngular();
246-
realtime_odometry_publisher_->unlockAndPublish();
236+
odometry_message_.header.stamp = time;
237+
odometry_message_.pose.pose.position.x = odometry_.getX();
238+
odometry_message_.pose.pose.position.y = odometry_.getY();
239+
odometry_message_.pose.pose.orientation.x = orientation.x();
240+
odometry_message_.pose.pose.orientation.y = orientation.y();
241+
odometry_message_.pose.pose.orientation.z = orientation.z();
242+
odometry_message_.pose.pose.orientation.w = orientation.w();
243+
odometry_message_.twist.twist.linear.x = odometry_.getLinear();
244+
odometry_message_.twist.twist.angular.z = odometry_.getAngular();
245+
realtime_odometry_publisher_->try_publish(odometry_message_);
247246
}
248247

249-
if (params_.enable_odom_tf && realtime_odometry_transform_publisher_->trylock())
248+
if (params_.enable_odom_tf && realtime_odometry_transform_publisher_)
250249
{
251-
auto & transform = realtime_odometry_transform_publisher_->msg_.transforms.front();
250+
auto & transform = odometry_transform_message_.transforms.front();
252251
transform.header.stamp = time;
253252
transform.transform.translation.x = odometry_.getX();
254253
transform.transform.translation.y = odometry_.getY();
255254
transform.transform.rotation.x = orientation.x();
256255
transform.transform.rotation.y = orientation.y();
257256
transform.transform.rotation.z = orientation.z();
258257
transform.transform.rotation.w = orientation.w();
259-
realtime_odometry_transform_publisher_->unlockAndPublish();
258+
realtime_odometry_transform_publisher_->try_publish(odometry_transform_message_);
260259
}
261260
}
262261

@@ -271,17 +270,16 @@ controller_interface::return_type DiffDriveController::update_and_write_commands
271270
previous_two_commands_.push({{linear_command, angular_command}});
272271

273272
// Publish limited velocity
274-
if (publish_limited_velocity_ && realtime_limited_velocity_publisher_->trylock())
275-
{
276-
auto & limited_velocity_command = realtime_limited_velocity_publisher_->msg_;
277-
limited_velocity_command.header.stamp = time;
278-
limited_velocity_command.twist.linear.x = linear_command;
279-
limited_velocity_command.twist.linear.y = 0.0;
280-
limited_velocity_command.twist.linear.z = 0.0;
281-
limited_velocity_command.twist.angular.x = 0.0;
282-
limited_velocity_command.twist.angular.y = 0.0;
283-
limited_velocity_command.twist.angular.z = angular_command;
284-
realtime_limited_velocity_publisher_->unlockAndPublish();
273+
if (publish_limited_velocity_ && realtime_limited_velocity_publisher_)
274+
{
275+
limited_velocity_message_.header.stamp = time;
276+
limited_velocity_message_.twist.linear.x = linear_command;
277+
limited_velocity_message_.twist.linear.y = 0.0;
278+
limited_velocity_message_.twist.linear.z = 0.0;
279+
limited_velocity_message_.twist.angular.x = 0.0;
280+
limited_velocity_message_.twist.angular.y = 0.0;
281+
limited_velocity_message_.twist.angular.z = angular_command;
282+
realtime_limited_velocity_publisher_->try_publish(limited_velocity_message_);
285283
}
286284

287285
// Compute wheels velocities:
@@ -469,10 +467,9 @@ controller_interface::CallbackReturn DiffDriveController::on_configure(
469467
odometry_transform_publisher_);
470468

471469
// keeping track of odom and base_link transforms only
472-
auto & odometry_transform_message = realtime_odometry_transform_publisher_->msg_;
473-
odometry_transform_message.transforms.resize(1);
474-
odometry_transform_message.transforms.front().header.frame_id = odom_frame_id;
475-
odometry_transform_message.transforms.front().child_frame_id = base_frame_id;
470+
odometry_transform_message_.transforms.resize(1);
471+
odometry_transform_message_.transforms.front().header.frame_id = odom_frame_id;
472+
odometry_transform_message_.transforms.front().child_frame_id = base_frame_id;
476473

477474
previous_update_timestamp_ = get_node()->get_clock()->now();
478475
return controller_interface::CallbackReturn::SUCCESS;

‎force_torque_sensor_broadcaster/src/force_torque_sensor_broadcaster.cpp‎

Lines changed: 5 additions & 19 deletions
Original file line numberDiff line numberDiff line change
@@ -137,17 +137,6 @@ controller_interface::CallbackReturn ForceTorqueSensorBroadcaster::on_configure(
137137
wrench_raw_.header.frame_id = params_.frame_id;
138138
wrench_filtered_.header.frame_id = params_.frame_id;
139139

140-
realtime_raw_publisher_->lock();
141-
realtime_raw_publisher_->msg_.header.frame_id = params_.frame_id;
142-
realtime_raw_publisher_->unlock();
143-
144-
if (has_filter_chain_)
145-
{
146-
realtime_filtered_publisher_->lock();
147-
realtime_filtered_publisher_->msg_.header.frame_id = params_.frame_id;
148-
realtime_filtered_publisher_->unlock();
149-
}
150-
151140
RCLCPP_INFO(get_node()->get_logger(), "configure successful");
152141
return controller_interface::CallbackReturn::SUCCESS;
153142
}
@@ -193,22 +182,19 @@ controller_interface::return_type ForceTorqueSensorBroadcaster::update_and_write
193182
this->apply_sensor_offset(params_, wrench_raw_);
194183
this->apply_sensor_multiplier(params_, wrench_raw_);
195184

196-
if (realtime_raw_publisher_ && realtime_raw_publisher_->trylock())
185+
if (realtime_raw_publisher_)
197186
{
198-
realtime_raw_publisher_->msg_.header.stamp = time;
199-
realtime_raw_publisher_->msg_.wrench = wrench_raw_.wrench;
200-
realtime_raw_publisher_->unlockAndPublish();
187+
realtime_raw_publisher_->try_publish(wrench_raw_);
201188
}
202189

203190
if (has_filter_chain_)
204191
{
205192
// Filter sensor data, if no filter chain config was specified, wrench_filtered_ = wrench_raw_
206193
auto filtered = filter_chain_->update(wrench_raw_, wrench_filtered_);
207-
if (filtered && realtime_filtered_publisher_ && realtime_filtered_publisher_->trylock())
194+
if (filtered && realtime_filtered_publisher_)
208195
{
209-
realtime_filtered_publisher_->msg_.header.stamp = time;
210-
realtime_filtered_publisher_->msg_.wrench = wrench_filtered_.wrench;
211-
realtime_filtered_publisher_->unlockAndPublish();
196+
wrench_filtered_.header.stamp = time;
197+
realtime_filtered_publisher_->try_publish(wrench_filtered_);
212198
}
213199
}
214200

‎gpio_controllers/include/gpio_controllers/gpio_command_controller.hpp‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -100,6 +100,7 @@ class GpioCommandController : public controller_interface::ControllerInterface
100100

101101
std::shared_ptr<rclcpp::Publisher<StateType>> gpio_state_publisher_{};
102102
std::shared_ptr<realtime_tools::RealtimePublisher<StateType>> realtime_gpio_state_publisher_{};
103+
StateType gpio_state_msg_;
103104

104105
std::shared_ptr<gpio_command_controller_parameters::ParamListener> param_listener_{};
105106
gpio_command_controller_parameters::Params params_;

‎gpio_controllers/src/gpio_command_controller.cpp‎

Lines changed: 13 additions & 15 deletions
Original file line numberDiff line numberDiff line change
@@ -252,19 +252,18 @@ void GpioCommandController::store_state_interface_types()
252252

253253
void GpioCommandController::initialize_gpio_state_msg()
254254
{
255-
auto & gpio_state_msg = realtime_gpio_state_publisher_->msg_;
256-
gpio_state_msg.header.stamp = get_node()->now();
257-
gpio_state_msg.interface_groups.resize(params_.gpios.size());
258-
gpio_state_msg.interface_values.resize(params_.gpios.size());
255+
gpio_state_msg_.header.stamp = get_node()->now();
256+
gpio_state_msg_.interface_groups.resize(params_.gpios.size());
257+
gpio_state_msg_.interface_values.resize(params_.gpios.size());
259258

260259
for (std::size_t gpio_index = 0; gpio_index < params_.gpios.size(); ++gpio_index)
261260
{
262261
const auto gpio_name = params_.gpios[gpio_index];
263-
gpio_state_msg.interface_groups[gpio_index] = gpio_name;
264-
gpio_state_msg.interface_values[gpio_index].interface_names =
262+
gpio_state_msg_.interface_groups[gpio_index] = gpio_name;
263+
gpio_state_msg_.interface_values[gpio_index].interface_names =
265264
get_gpios_state_interfaces_names(gpio_name);
266-
gpio_state_msg.interface_values[gpio_index].values = std::vector<double>(
267-
gpio_state_msg.interface_values[gpio_index].interface_names.size(),
265+
gpio_state_msg_.interface_values[gpio_index].values = std::vector<double>(
266+
gpio_state_msg_.interface_values[gpio_index].interface_names.size(),
268267
std::numeric_limits<double>::quiet_NaN());
269268
}
270269
}
@@ -391,24 +390,23 @@ void GpioCommandController::apply_command(
391390

392391
void GpioCommandController::update_gpios_states()
393392
{
394-
if (!realtime_gpio_state_publisher_ || !realtime_gpio_state_publisher_->trylock())
393+
if (!realtime_gpio_state_publisher_)
395394
{
396395
return;
397396
}
398397

399-
auto & gpio_state_msg = realtime_gpio_state_publisher_->msg_;
400-
gpio_state_msg.header.stamp = get_node()->now();
401-
for (std::size_t gpio_index = 0; gpio_index < gpio_state_msg.interface_groups.size();
398+
gpio_state_msg_.header.stamp = get_node()->now();
399+
for (std::size_t gpio_index = 0; gpio_index < gpio_state_msg_.interface_groups.size();
402400
++gpio_index)
403401
{
404402
for (std::size_t interface_index = 0;
405-
interface_index < gpio_state_msg.interface_values[gpio_index].interface_names.size();
403+
interface_index < gpio_state_msg_.interface_values[gpio_index].interface_names.size();
406404
++interface_index)
407405
{
408-
apply_state_value(gpio_state_msg, gpio_index, interface_index);
406+
apply_state_value(gpio_state_msg_, gpio_index, interface_index);
409407
}
410408
}
411-
realtime_gpio_state_publisher_->unlockAndPublish();
409+
realtime_gpio_state_publisher_->try_publish(gpio_state_msg_);
412410
}
413411

414412
void GpioCommandController::apply_state_value(

‎gps_sensor_broadcaster/include/gps_sensor_broadcaster/gps_sensor_broadcaster.hpp‎

Lines changed: 3 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -57,8 +57,11 @@ class GPSSensorBroadcaster : public controller_interface::ControllerInterface
5757
callback_return_type setup_publisher();
5858

5959
GPSSensorVariant gps_sensor_;
60+
6061
rclcpp::Publisher<sensor_msgs::msg::NavSatFix>::SharedPtr sensor_state_publisher_;
6162
std::unique_ptr<StatePublisher> realtime_publisher_;
63+
sensor_msgs::msg::NavSatFix state_message_;
64+
6265
std::shared_ptr<gps_sensor_broadcaster::ParamListener> param_listener_{};
6366
gps_sensor_broadcaster::Params params_;
6467
std::vector<std::string> state_names_;

‎gps_sensor_broadcaster/src/gps_sensor_broadcaster.cpp‎

Lines changed: 8 additions & 10 deletions
Original file line numberDiff line numberDiff line change
@@ -82,10 +82,8 @@ callback_return_type GPSSensorBroadcaster::setup_publisher()
8282
sensor_state_publisher_ = get_node()->create_publisher<sensor_msgs::msg::NavSatFix>(
8383
"~/gps/fix", rclcpp::SystemDefaultsQoS());
8484
realtime_publisher_ = std::make_unique<StatePublisher>(sensor_state_publisher_);
85-
realtime_publisher_->lock();
86-
realtime_publisher_->msg_.header.frame_id = params_.frame_id;
85+
state_message_.header.frame_id = params_.frame_id;
8786
setup_covariance();
88-
realtime_publisher_->unlock();
8987

9088
return callback_return_type::SUCCESS;
9189
}
@@ -102,14 +100,14 @@ void GPSSensorBroadcaster::setup_covariance()
102100
{
103101
if (params_.read_covariance_from_interface)
104102
{
105-
realtime_publisher_->msg_.position_covariance_type = COVARIANCE_TYPE_DIAGONAL_KNOWN;
103+
state_message_.position_covariance_type = COVARIANCE_TYPE_DIAGONAL_KNOWN;
106104
return;
107105
}
108106

109107
for (size_t i = 0; i < COVARIANCE_3x3_SIZE; ++i)
110108
{
111-
realtime_publisher_->msg_.position_covariance[i] = params_.static_position_covariance[i];
112-
realtime_publisher_->msg_.position_covariance_type = COVARIANCE_TYPE_KNOWN;
109+
state_message_.position_covariance[i] = params_.static_position_covariance[i];
110+
state_message_.position_covariance_type = COVARIANCE_TYPE_KNOWN;
113111
}
114112
}
115113

@@ -152,19 +150,19 @@ callback_return_type GPSSensorBroadcaster::on_deactivate(const rclcpp_lifecycle:
152150
controller_interface::return_type GPSSensorBroadcaster::update(
153151
const rclcpp::Time &, const rclcpp::Duration &)
154152
{
155-
if (realtime_publisher_ && realtime_publisher_->trylock())
153+
if (realtime_publisher_)
156154
{
157-
realtime_publisher_->msg_.header.stamp = get_node()->now();
155+
state_message_.header.stamp = get_node()->now();
158156
std::visit(
159157
Visitor{
160158
[this](auto & sensor)
161159
{
162160
sensor_msgs::msg::NavSatFix message;
163-
sensor.get_values_as_message(realtime_publisher_->msg_);
161+
sensor.get_values_as_message(state_message_);
164162
},
165163
[](std::monostate &) {}},
166164
gps_sensor_);
167-
realtime_publisher_->unlockAndPublish();
165+
realtime_publisher_->try_publish(state_message_);
168166
}
169167
return controller_interface::return_type::OK;
170168
}

‎imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.cpp‎

Lines changed: 0 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -40,7 +40,6 @@ using testing::SizeIs;
4040
namespace
4141
{
4242
constexpr auto NODE_SUCCESS = controller_interface::CallbackReturn::SUCCESS;
43-
constexpr auto NODE_ERROR = controller_interface::CallbackReturn::ERROR;
4443
} // namespace
4544

4645
void IMUSensorBroadcasterTest::SetUpTestCase() {}

0 commit comments

Comments
 (0)