diff --git a/diff_drive_controller/src/diff_drive_controller.cpp b/diff_drive_controller/src/diff_drive_controller.cpp index e49fce44b5..4057121895 100644 --- a/diff_drive_controller/src/diff_drive_controller.cpp +++ b/diff_drive_controller/src/diff_drive_controller.cpp @@ -438,16 +438,15 @@ controller_interface::CallbackReturn DiffDriveController::on_configure( const auto odom_frame_id = tf_prefix + params_.odom_frame_id; const auto base_frame_id = tf_prefix + params_.base_frame_id; - auto & odometry_message = realtime_odometry_publisher_->msg_; - odometry_message.header.frame_id = odom_frame_id; - odometry_message.child_frame_id = base_frame_id; + odometry_message_.header.frame_id = odom_frame_id; + odometry_message_.child_frame_id = base_frame_id; // limit the publication on the topics /odom and /tf publish_rate_ = params_.publish_rate; publish_period_ = rclcpp::Duration::from_seconds(1.0 / publish_rate_); // initialize odom values zeros - odometry_message.twist = + odometry_message_.twist = geometry_msgs::msg::TwistWithCovariance(rosidl_runtime_cpp::MessageInitialization::ALL); constexpr size_t NUM_DIMENSIONS = 6; @@ -455,8 +454,8 @@ controller_interface::CallbackReturn DiffDriveController::on_configure( { // 0, 7, 14, 21, 28, 35 const size_t diagonal_index = NUM_DIMENSIONS * index + index; - odometry_message.pose.covariance[diagonal_index] = params_.pose_covariance_diagonal[index]; - odometry_message.twist.covariance[diagonal_index] = params_.twist_covariance_diagonal[index]; + odometry_message_.pose.covariance[diagonal_index] = params_.pose_covariance_diagonal[index]; + odometry_message_.twist.covariance[diagonal_index] = params_.twist_covariance_diagonal[index]; } // initialize transform publisher and message diff --git a/diff_drive_controller/test/test_diff_drive_controller.cpp b/diff_drive_controller/test/test_diff_drive_controller.cpp index a180b3cef7..250d434405 100644 --- a/diff_drive_controller/test/test_diff_drive_controller.cpp +++ b/diff_drive_controller/test/test_diff_drive_controller.cpp @@ -73,7 +73,13 @@ class TestableDiffDriveController : public diff_drive_controller::DiffDriveContr { return realtime_odometry_publisher_; } - + // Declare these tests as friends so we can access odometry_message_ + FRIEND_TEST(TestDiffDriveController, configure_succeeds_tf_test_prefix_false_no_namespace); + FRIEND_TEST(TestDiffDriveController, configure_succeeds_tf_test_prefix_true_no_namespace); + FRIEND_TEST(TestDiffDriveController, configure_succeeds_tf_blank_prefix_true_no_namespace); + FRIEND_TEST(TestDiffDriveController, configure_succeeds_tf_test_prefix_false_set_namespace); + FRIEND_TEST(TestDiffDriveController, configure_succeeds_tf_test_prefix_true_set_namespace); + FRIEND_TEST(TestDiffDriveController, configure_succeeds_tf_blank_prefix_true_set_namespace); // Declare these tests as friends so we can access controller_->reference_interfaces_ FRIEND_TEST(TestDiffDriveController, chainable_controller_unchained_mode); FRIEND_TEST(TestDiffDriveController, chainable_controller_chained_mode); @@ -311,12 +317,9 @@ TEST_F(TestDiffDriveController, configure_succeeds_tf_test_prefix_false_no_names ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); - auto odometry_message = controller_->get_rt_odom_publisher()->msg_; - std::string test_odom_frame_id = odometry_message.header.frame_id; - std::string test_base_frame_id = odometry_message.child_frame_id; /* tf_frame_prefix_enable is false so no modifications to the frame id's */ - ASSERT_EQ(test_odom_frame_id, odom_id); - ASSERT_EQ(test_base_frame_id, base_link_id); + ASSERT_EQ(controller_->odometry_message_.header.frame_id, odom_id); + ASSERT_EQ(controller_->odometry_message_.child_frame_id, base_link_id); } 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 ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); - auto odometry_message = controller_->get_rt_odom_publisher()->msg_; - std::string test_odom_frame_id = odometry_message.header.frame_id; - std::string test_base_frame_id = odometry_message.child_frame_id; - /* tf_frame_prefix_enable is true and frame_prefix is not blank so should be appended to the frame * id's */ - ASSERT_EQ(test_odom_frame_id, frame_prefix + "/" + odom_id); - ASSERT_EQ(test_base_frame_id, frame_prefix + "/" + base_link_id); + ASSERT_EQ(controller_->odometry_message_.header.frame_id, frame_prefix + "/" + odom_id); + ASSERT_EQ(controller_->odometry_message_.child_frame_id, frame_prefix + "/" + base_link_id); } 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 ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); - auto odometry_message = controller_->get_rt_odom_publisher()->msg_; - std::string test_odom_frame_id = odometry_message.header.frame_id; - std::string test_base_frame_id = odometry_message.child_frame_id; /* tf_frame_prefix_enable is true but frame_prefix is blank so should not be appended to the frame * id's */ - ASSERT_EQ(test_odom_frame_id, odom_id); - ASSERT_EQ(test_base_frame_id, base_link_id); + ASSERT_EQ(controller_->odometry_message_.header.frame_id, odom_id); + ASSERT_EQ(controller_->odometry_message_.child_frame_id, base_link_id); } 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 ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); - auto odometry_message = controller_->get_rt_odom_publisher()->msg_; - std::string test_odom_frame_id = odometry_message.header.frame_id; - std::string test_base_frame_id = odometry_message.child_frame_id; /* tf_frame_prefix_enable is false so no modifications to the frame id's */ - ASSERT_EQ(test_odom_frame_id, odom_id); - ASSERT_EQ(test_base_frame_id, base_link_id); + ASSERT_EQ(controller_->odometry_message_.header.frame_id, odom_id); + ASSERT_EQ(controller_->odometry_message_.child_frame_id, base_link_id); } 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 ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); - auto odometry_message = controller_->get_rt_odom_publisher()->msg_; - std::string test_odom_frame_id = odometry_message.header.frame_id; - std::string test_base_frame_id = odometry_message.child_frame_id; - /* tf_frame_prefix_enable is true and frame_prefix is not blank so should be appended to the frame * id's instead of the namespace*/ - ASSERT_EQ(test_odom_frame_id, frame_prefix + "/" + odom_id); - ASSERT_EQ(test_base_frame_id, frame_prefix + "/" + base_link_id); + ASSERT_EQ(controller_->odometry_message_.header.frame_id, frame_prefix + "/" + odom_id); + ASSERT_EQ(controller_->odometry_message_.child_frame_id, frame_prefix + "/" + base_link_id); } 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 ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); - auto odometry_message = controller_->get_rt_odom_publisher()->msg_; - std::string test_odom_frame_id = odometry_message.header.frame_id; - std::string test_base_frame_id = odometry_message.child_frame_id; std::string ns_prefix = test_namespace.erase(0, 1) + "/"; /* tf_frame_prefix_enable is true but frame_prefix is blank so namespace should be appended to the * frame id's */ - ASSERT_EQ(test_odom_frame_id, ns_prefix + odom_id); - ASSERT_EQ(test_base_frame_id, ns_prefix + base_link_id); + ASSERT_EQ(controller_->odometry_message_.header.frame_id, ns_prefix + odom_id); + ASSERT_EQ(controller_->odometry_message_.child_frame_id, ns_prefix + base_link_id); } TEST_F(TestDiffDriveController, activate_fails_without_resources_assigned) diff --git a/force_torque_sensor_broadcaster/src/force_torque_sensor_broadcaster.cpp b/force_torque_sensor_broadcaster/src/force_torque_sensor_broadcaster.cpp index cf346ea438..a8d1ddf1f0 100644 --- a/force_torque_sensor_broadcaster/src/force_torque_sensor_broadcaster.cpp +++ b/force_torque_sensor_broadcaster/src/force_torque_sensor_broadcaster.cpp @@ -241,32 +241,32 @@ ForceTorqueSensorBroadcaster::on_export_state_interfaces() if (!force_names[0].empty()) { exported_state_interfaces.emplace_back( - export_prefix, force_names[0], &realtime_raw_publisher_->msg_.wrench.force.x); + export_prefix, force_names[0], &wrench_raw_.wrench.force.x); } if (!force_names[1].empty()) { exported_state_interfaces.emplace_back( - export_prefix, force_names[1], &realtime_raw_publisher_->msg_.wrench.force.y); + export_prefix, force_names[1], &wrench_raw_.wrench.force.y); } if (!force_names[2].empty()) { exported_state_interfaces.emplace_back( - export_prefix, force_names[2], &realtime_raw_publisher_->msg_.wrench.force.z); + export_prefix, force_names[2], &wrench_raw_.wrench.force.z); } if (!torque_names[0].empty()) { exported_state_interfaces.emplace_back( - export_prefix, torque_names[0], &realtime_raw_publisher_->msg_.wrench.torque.x); + export_prefix, torque_names[0], &wrench_raw_.wrench.torque.x); } if (!torque_names[1].empty()) { exported_state_interfaces.emplace_back( - export_prefix, torque_names[1], &realtime_raw_publisher_->msg_.wrench.torque.y); + export_prefix, torque_names[1], &wrench_raw_.wrench.torque.y); } if (!torque_names[2].empty()) { exported_state_interfaces.emplace_back( - export_prefix, torque_names[2], &realtime_raw_publisher_->msg_.wrench.torque.z); + export_prefix, torque_names[2], &wrench_raw_.wrench.torque.z); } return exported_state_interfaces; } diff --git a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp index 3756c59d22..5cae33323e 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp +++ b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp @@ -204,7 +204,7 @@ class TestableJointTrajectoryController control_msgs::msg::JointTrajectoryControllerState get_state_msg() { - return state_publisher_->msg_; + return state_publisher_->get_msg(); } /** diff --git a/tricycle_controller/include/tricycle_controller/tricycle_controller.hpp b/tricycle_controller/include/tricycle_controller/tricycle_controller.hpp index 004f7d867b..81a62b989c 100644 --- a/tricycle_controller/include/tricycle_controller/tricycle_controller.hpp +++ b/tricycle_controller/include/tricycle_controller/tricycle_controller.hpp @@ -33,8 +33,8 @@ #include "geometry_msgs/msg/twist_stamped.hpp" #include "nav_msgs/msg/odometry.hpp" #include "rclcpp_lifecycle/state.hpp" -#include "realtime_tools/realtime_box.hpp" #include "realtime_tools/realtime_publisher.hpp" +#include "realtime_tools/realtime_thread_safe_box.hpp" #include "std_srvs/srv/empty.hpp" #include "tf2_msgs/msg/tf_message.hpp" @@ -127,7 +127,8 @@ class TricycleController : public controller_interface::ControllerInterface bool subscriber_is_active_ = false; rclcpp::Subscription::SharedPtr velocity_command_subscriber_ = nullptr; - realtime_tools::RealtimeBox> received_velocity_msg_ptr_{nullptr}; + realtime_tools::RealtimeThreadSafeBox> received_velocity_msg_ptr_{ + nullptr}; std::shared_ptr last_command_msg_; rclcpp::Service::SharedPtr reset_odom_service_;