Skip to content

Commit c64f72d

Browse files
christophfroehlichchristophfroehlich
authored
Don't use msg_ field of realtime publisher (ros-controls#1947)
1 parent 4b1f45e commit c64f72d

File tree

5 files changed

+34
-48
lines changed

5 files changed

+34
-48
lines changed

‎diff_drive_controller/src/diff_drive_controller.cpp‎

Lines changed: 5 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -438,25 +438,24 @@ controller_interface::CallbackReturn DiffDriveController::on_configure(
438438
const auto odom_frame_id = tf_prefix + params_.odom_frame_id;
439439
const auto base_frame_id = tf_prefix + params_.base_frame_id;
440440

441-
auto & odometry_message = realtime_odometry_publisher_->msg_;
442-
odometry_message.header.frame_id = odom_frame_id;
443-
odometry_message.child_frame_id = base_frame_id;
441+
odometry_message_.header.frame_id = odom_frame_id;
442+
odometry_message_.child_frame_id = base_frame_id;
444443

445444
// limit the publication on the topics /odom and /tf
446445
publish_rate_ = params_.publish_rate;
447446
publish_period_ = rclcpp::Duration::from_seconds(1.0 / publish_rate_);
448447

449448
// initialize odom values zeros
450-
odometry_message.twist =
449+
odometry_message_.twist =
451450
geometry_msgs::msg::TwistWithCovariance(rosidl_runtime_cpp::MessageInitialization::ALL);
452451

453452
constexpr size_t NUM_DIMENSIONS = 6;
454453
for (size_t index = 0; index < 6; ++index)
455454
{
456455
// 0, 7, 14, 21, 28, 35
457456
const size_t diagonal_index = NUM_DIMENSIONS * index + index;
458-
odometry_message.pose.covariance[diagonal_index] = params_.pose_covariance_diagonal[index];
459-
odometry_message.twist.covariance[diagonal_index] = params_.twist_covariance_diagonal[index];
457+
odometry_message_.pose.covariance[diagonal_index] = params_.pose_covariance_diagonal[index];
458+
odometry_message_.twist.covariance[diagonal_index] = params_.twist_covariance_diagonal[index];
460459
}
461460

462461
// initialize transform publisher and message

‎diff_drive_controller/test/test_diff_drive_controller.cpp‎

Lines changed: 19 additions & 33 deletions
Original file line numberDiff line numberDiff line change
@@ -73,7 +73,13 @@ class TestableDiffDriveController : public diff_drive_controller::DiffDriveContr
7373
{
7474
return realtime_odometry_publisher_;
7575
}
76-
76+
// Declare these tests as friends so we can access odometry_message_
77+
FRIEND_TEST(TestDiffDriveController, configure_succeeds_tf_test_prefix_false_no_namespace);
78+
FRIEND_TEST(TestDiffDriveController, configure_succeeds_tf_test_prefix_true_no_namespace);
79+
FRIEND_TEST(TestDiffDriveController, configure_succeeds_tf_blank_prefix_true_no_namespace);
80+
FRIEND_TEST(TestDiffDriveController, configure_succeeds_tf_test_prefix_false_set_namespace);
81+
FRIEND_TEST(TestDiffDriveController, configure_succeeds_tf_test_prefix_true_set_namespace);
82+
FRIEND_TEST(TestDiffDriveController, configure_succeeds_tf_blank_prefix_true_set_namespace);
7783
// Declare these tests as friends so we can access controller_->reference_interfaces_
7884
FRIEND_TEST(TestDiffDriveController, chainable_controller_unchained_mode);
7985
FRIEND_TEST(TestDiffDriveController, chainable_controller_chained_mode);
@@ -311,12 +317,9 @@ TEST_F(TestDiffDriveController, configure_succeeds_tf_test_prefix_false_no_names
311317

312318
ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS);
313319

314-
auto odometry_message = controller_->get_rt_odom_publisher()->msg_;
315-
std::string test_odom_frame_id = odometry_message.header.frame_id;
316-
std::string test_base_frame_id = odometry_message.child_frame_id;
317320
/* tf_frame_prefix_enable is false so no modifications to the frame id's */
318-
ASSERT_EQ(test_odom_frame_id, odom_id);
319-
ASSERT_EQ(test_base_frame_id, base_link_id);
321+
ASSERT_EQ(controller_->odometry_message_.header.frame_id, odom_id);
322+
ASSERT_EQ(controller_->odometry_message_.child_frame_id, base_link_id);
320323
}
321324

322325
TEST_F(TestDiffDriveController, configure_succeeds_tf_test_prefix_true_no_namespace)
@@ -336,14 +339,10 @@ TEST_F(TestDiffDriveController, configure_succeeds_tf_test_prefix_true_no_namesp
336339

337340
ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS);
338341

339-
auto odometry_message = controller_->get_rt_odom_publisher()->msg_;
340-
std::string test_odom_frame_id = odometry_message.header.frame_id;
341-
std::string test_base_frame_id = odometry_message.child_frame_id;
342-
343342
/* tf_frame_prefix_enable is true and frame_prefix is not blank so should be appended to the frame
344343
* id's */
345-
ASSERT_EQ(test_odom_frame_id, frame_prefix + "/" + odom_id);
346-
ASSERT_EQ(test_base_frame_id, frame_prefix + "/" + base_link_id);
344+
ASSERT_EQ(controller_->odometry_message_.header.frame_id, frame_prefix + "/" + odom_id);
345+
ASSERT_EQ(controller_->odometry_message_.child_frame_id, frame_prefix + "/" + base_link_id);
347346
}
348347

349348
TEST_F(TestDiffDriveController, configure_succeeds_tf_blank_prefix_true_no_namespace)
@@ -363,13 +362,10 @@ TEST_F(TestDiffDriveController, configure_succeeds_tf_blank_prefix_true_no_names
363362

364363
ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS);
365364

366-
auto odometry_message = controller_->get_rt_odom_publisher()->msg_;
367-
std::string test_odom_frame_id = odometry_message.header.frame_id;
368-
std::string test_base_frame_id = odometry_message.child_frame_id;
369365
/* tf_frame_prefix_enable is true but frame_prefix is blank so should not be appended to the frame
370366
* id's */
371-
ASSERT_EQ(test_odom_frame_id, odom_id);
372-
ASSERT_EQ(test_base_frame_id, base_link_id);
367+
ASSERT_EQ(controller_->odometry_message_.header.frame_id, odom_id);
368+
ASSERT_EQ(controller_->odometry_message_.child_frame_id, base_link_id);
373369
}
374370

375371
TEST_F(TestDiffDriveController, configure_succeeds_tf_test_prefix_false_set_namespace)
@@ -392,12 +388,9 @@ TEST_F(TestDiffDriveController, configure_succeeds_tf_test_prefix_false_set_name
392388

393389
ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS);
394390

395-
auto odometry_message = controller_->get_rt_odom_publisher()->msg_;
396-
std::string test_odom_frame_id = odometry_message.header.frame_id;
397-
std::string test_base_frame_id = odometry_message.child_frame_id;
398391
/* tf_frame_prefix_enable is false so no modifications to the frame id's */
399-
ASSERT_EQ(test_odom_frame_id, odom_id);
400-
ASSERT_EQ(test_base_frame_id, base_link_id);
392+
ASSERT_EQ(controller_->odometry_message_.header.frame_id, odom_id);
393+
ASSERT_EQ(controller_->odometry_message_.child_frame_id, base_link_id);
401394
}
402395

403396
TEST_F(TestDiffDriveController, configure_succeeds_tf_test_prefix_true_set_namespace)
@@ -420,14 +413,10 @@ TEST_F(TestDiffDriveController, configure_succeeds_tf_test_prefix_true_set_names
420413

421414
ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS);
422415

423-
auto odometry_message = controller_->get_rt_odom_publisher()->msg_;
424-
std::string test_odom_frame_id = odometry_message.header.frame_id;
425-
std::string test_base_frame_id = odometry_message.child_frame_id;
426-
427416
/* tf_frame_prefix_enable is true and frame_prefix is not blank so should be appended to the frame
428417
* id's instead of the namespace*/
429-
ASSERT_EQ(test_odom_frame_id, frame_prefix + "/" + odom_id);
430-
ASSERT_EQ(test_base_frame_id, frame_prefix + "/" + base_link_id);
418+
ASSERT_EQ(controller_->odometry_message_.header.frame_id, frame_prefix + "/" + odom_id);
419+
ASSERT_EQ(controller_->odometry_message_.child_frame_id, frame_prefix + "/" + base_link_id);
431420
}
432421

433422
TEST_F(TestDiffDriveController, configure_succeeds_tf_blank_prefix_true_set_namespace)
@@ -449,14 +438,11 @@ TEST_F(TestDiffDriveController, configure_succeeds_tf_blank_prefix_true_set_name
449438

450439
ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS);
451440

452-
auto odometry_message = controller_->get_rt_odom_publisher()->msg_;
453-
std::string test_odom_frame_id = odometry_message.header.frame_id;
454-
std::string test_base_frame_id = odometry_message.child_frame_id;
455441
std::string ns_prefix = test_namespace.erase(0, 1) + "/";
456442
/* tf_frame_prefix_enable is true but frame_prefix is blank so namespace should be appended to the
457443
* frame id's */
458-
ASSERT_EQ(test_odom_frame_id, ns_prefix + odom_id);
459-
ASSERT_EQ(test_base_frame_id, ns_prefix + base_link_id);
444+
ASSERT_EQ(controller_->odometry_message_.header.frame_id, ns_prefix + odom_id);
445+
ASSERT_EQ(controller_->odometry_message_.child_frame_id, ns_prefix + base_link_id);
460446
}
461447

462448
TEST_F(TestDiffDriveController, activate_fails_without_resources_assigned)

‎force_torque_sensor_broadcaster/src/force_torque_sensor_broadcaster.cpp‎

Lines changed: 6 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -241,32 +241,32 @@ ForceTorqueSensorBroadcaster::on_export_state_interfaces()
241241
if (!force_names[0].empty())
242242
{
243243
exported_state_interfaces.emplace_back(
244-
export_prefix, force_names[0], &realtime_raw_publisher_->msg_.wrench.force.x);
244+
export_prefix, force_names[0], &wrench_raw_.wrench.force.x);
245245
}
246246
if (!force_names[1].empty())
247247
{
248248
exported_state_interfaces.emplace_back(
249-
export_prefix, force_names[1], &realtime_raw_publisher_->msg_.wrench.force.y);
249+
export_prefix, force_names[1], &wrench_raw_.wrench.force.y);
250250
}
251251
if (!force_names[2].empty())
252252
{
253253
exported_state_interfaces.emplace_back(
254-
export_prefix, force_names[2], &realtime_raw_publisher_->msg_.wrench.force.z);
254+
export_prefix, force_names[2], &wrench_raw_.wrench.force.z);
255255
}
256256
if (!torque_names[0].empty())
257257
{
258258
exported_state_interfaces.emplace_back(
259-
export_prefix, torque_names[0], &realtime_raw_publisher_->msg_.wrench.torque.x);
259+
export_prefix, torque_names[0], &wrench_raw_.wrench.torque.x);
260260
}
261261
if (!torque_names[1].empty())
262262
{
263263
exported_state_interfaces.emplace_back(
264-
export_prefix, torque_names[1], &realtime_raw_publisher_->msg_.wrench.torque.y);
264+
export_prefix, torque_names[1], &wrench_raw_.wrench.torque.y);
265265
}
266266
if (!torque_names[2].empty())
267267
{
268268
exported_state_interfaces.emplace_back(
269-
export_prefix, torque_names[2], &realtime_raw_publisher_->msg_.wrench.torque.z);
269+
export_prefix, torque_names[2], &wrench_raw_.wrench.torque.z);
270270
}
271271
return exported_state_interfaces;
272272
}

‎joint_trajectory_controller/test/test_trajectory_controller_utils.hpp‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -204,7 +204,7 @@ class TestableJointTrajectoryController
204204

205205
control_msgs::msg::JointTrajectoryControllerState get_state_msg()
206206
{
207-
return state_publisher_->msg_;
207+
return state_publisher_->get_msg();
208208
}
209209

210210
/**

‎tricycle_controller/include/tricycle_controller/tricycle_controller.hpp‎

Lines changed: 3 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -33,8 +33,8 @@
3333
#include "geometry_msgs/msg/twist_stamped.hpp"
3434
#include "nav_msgs/msg/odometry.hpp"
3535
#include "rclcpp_lifecycle/state.hpp"
36-
#include "realtime_tools/realtime_box.hpp"
3736
#include "realtime_tools/realtime_publisher.hpp"
37+
#include "realtime_tools/realtime_thread_safe_box.hpp"
3838
#include "std_srvs/srv/empty.hpp"
3939
#include "tf2_msgs/msg/tf_message.hpp"
4040

@@ -127,7 +127,8 @@ class TricycleController : public controller_interface::ControllerInterface
127127
bool subscriber_is_active_ = false;
128128
rclcpp::Subscription<TwistStamped>::SharedPtr velocity_command_subscriber_ = nullptr;
129129

130-
realtime_tools::RealtimeBox<std::shared_ptr<TwistStamped>> received_velocity_msg_ptr_{nullptr};
130+
realtime_tools::RealtimeThreadSafeBox<std::shared_ptr<TwistStamped>> received_velocity_msg_ptr_{
131+
nullptr};
131132
std::shared_ptr<TwistStamped> last_command_msg_;
132133

133134
rclcpp::Service<std_srvs::srv::Empty>::SharedPtr reset_odom_service_;

0 commit comments

Comments
 (0)