From f9edc41a451877e292b206069aa2e8bfd25148c0 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Wed, 4 Dec 2024 11:16:28 +0100 Subject: [PATCH 1/2] Fix RealtimeBox API changes (#1385) --- .../diff_drive_controller.hpp | 1 + .../src/diff_drive_controller.cpp | 29 ++++++++++--------- .../test/test_diff_drive_controller.cpp | 3 +- .../tricycle_controller.hpp | 1 + .../src/tricycle_controller.cpp | 26 +++++++++-------- .../test/test_tricycle_controller.cpp | 3 +- 6 files changed, 35 insertions(+), 28 deletions(-) diff --git a/diff_drive_controller/include/diff_drive_controller/diff_drive_controller.hpp b/diff_drive_controller/include/diff_drive_controller/diff_drive_controller.hpp index 74526b199c..84712fe748 100644 --- a/diff_drive_controller/include/diff_drive_controller/diff_drive_controller.hpp +++ b/diff_drive_controller/include/diff_drive_controller/diff_drive_controller.hpp @@ -131,6 +131,7 @@ class DiffDriveController : public controller_interface::ControllerInterface rclcpp::Subscription::SharedPtr velocity_command_subscriber_ = nullptr; realtime_tools::RealtimeBox> received_velocity_msg_ptr_{nullptr}; + std::shared_ptr last_command_msg_; std::queue previous_commands_; // last two commands diff --git a/diff_drive_controller/src/diff_drive_controller.cpp b/diff_drive_controller/src/diff_drive_controller.cpp index d21cac602d..d710c81257 100644 --- a/diff_drive_controller/src/diff_drive_controller.cpp +++ b/diff_drive_controller/src/diff_drive_controller.cpp @@ -111,26 +111,27 @@ controller_interface::return_type DiffDriveController::update( return controller_interface::return_type::OK; } - std::shared_ptr last_command_msg; - received_velocity_msg_ptr_.get(last_command_msg); + // if the mutex is unable to lock, last_command_msg_ won't be updated + received_velocity_msg_ptr_.try_get([this](const std::shared_ptr & msg) + { last_command_msg_ = msg; }); - if (last_command_msg == nullptr) + if (last_command_msg_ == nullptr) { RCLCPP_WARN(logger, "Velocity message received was a nullptr."); return controller_interface::return_type::ERROR; } - const auto age_of_last_command = time - last_command_msg->header.stamp; + const auto age_of_last_command = time - last_command_msg_->header.stamp; // Brake if cmd_vel has timeout, override the stored command if (age_of_last_command > cmd_vel_timeout_) { - last_command_msg->twist.linear.x = 0.0; - last_command_msg->twist.angular.z = 0.0; + last_command_msg_->twist.linear.x = 0.0; + last_command_msg_->twist.angular.z = 0.0; } // command may be limited further by SpeedLimit, // without affecting the stored twist command - TwistStamped command = *last_command_msg; + TwistStamped command = *last_command_msg_; double & linear_command = command.twist.linear.x; double & angular_command = command.twist.angular.z; @@ -325,12 +326,12 @@ controller_interface::CallbackReturn DiffDriveController::on_configure( limited_velocity_publisher_); } - const TwistStamped empty_twist; - received_velocity_msg_ptr_.set(std::make_shared(empty_twist)); - + last_command_msg_ = std::make_shared(); + received_velocity_msg_ptr_.set([this](std::shared_ptr & stored_value) + { stored_value = last_command_msg_; }); // Fill last two commands with default constructed commands - previous_commands_.emplace(empty_twist); - previous_commands_.emplace(empty_twist); + previous_commands_.emplace(*last_command_msg_); + previous_commands_.emplace(*last_command_msg_); // initialize command subscriber velocity_command_subscriber_ = get_node()->create_subscription( @@ -350,7 +351,8 @@ controller_interface::CallbackReturn DiffDriveController::on_configure( "time, this message will only be shown once"); msg->header.stamp = get_node()->get_clock()->now(); } - received_velocity_msg_ptr_.set(std::move(msg)); + received_velocity_msg_ptr_.set([msg](std::shared_ptr & stored_value) + { stored_value = std::move(msg); }); }); // initialize odometry publisher and message @@ -476,7 +478,6 @@ controller_interface::CallbackReturn DiffDriveController::on_cleanup( return controller_interface::CallbackReturn::ERROR; } - received_velocity_msg_ptr_.set(std::make_shared()); return controller_interface::CallbackReturn::SUCCESS; } diff --git a/diff_drive_controller/test/test_diff_drive_controller.cpp b/diff_drive_controller/test/test_diff_drive_controller.cpp index 72ae4dbfd7..e29c6bbfee 100644 --- a/diff_drive_controller/test/test_diff_drive_controller.cpp +++ b/diff_drive_controller/test/test_diff_drive_controller.cpp @@ -49,7 +49,8 @@ class TestableDiffDriveController : public diff_drive_controller::DiffDriveContr std::shared_ptr getLastReceivedTwist() { std::shared_ptr ret; - received_velocity_msg_ptr_.get(ret); + received_velocity_msg_ptr_.get( + [&ret](const std::shared_ptr & msg) { ret = msg; }); return ret; } diff --git a/tricycle_controller/include/tricycle_controller/tricycle_controller.hpp b/tricycle_controller/include/tricycle_controller/tricycle_controller.hpp index 010a890f52..1f9361dadd 100644 --- a/tricycle_controller/include/tricycle_controller/tricycle_controller.hpp +++ b/tricycle_controller/include/tricycle_controller/tricycle_controller.hpp @@ -139,6 +139,7 @@ class TricycleController : public controller_interface::ControllerInterface rclcpp::Subscription::SharedPtr velocity_command_subscriber_ = nullptr; realtime_tools::RealtimeBox> received_velocity_msg_ptr_{nullptr}; + std::shared_ptr last_command_msg_; rclcpp::Service::SharedPtr reset_odom_service_; diff --git a/tricycle_controller/src/tricycle_controller.cpp b/tricycle_controller/src/tricycle_controller.cpp index ec7ca7bd5e..1d6daba958 100644 --- a/tricycle_controller/src/tricycle_controller.cpp +++ b/tricycle_controller/src/tricycle_controller.cpp @@ -95,25 +95,27 @@ controller_interface::return_type TricycleController::update( } return controller_interface::return_type::OK; } - std::shared_ptr last_command_msg; - received_velocity_msg_ptr_.get(last_command_msg); - if (last_command_msg == nullptr) + + // if the mutex is unable to lock, last_command_msg_ won't be updated + received_velocity_msg_ptr_.try_get([this](const std::shared_ptr & msg) + { last_command_msg_ = msg; }); + if (last_command_msg_ == nullptr) { RCLCPP_WARN(get_node()->get_logger(), "Velocity message received was a nullptr."); return controller_interface::return_type::ERROR; } - const auto age_of_last_command = time - last_command_msg->header.stamp; + const auto age_of_last_command = time - last_command_msg_->header.stamp; // Brake if cmd_vel has timeout, override the stored command if (age_of_last_command > cmd_vel_timeout_) { - last_command_msg->twist.linear.x = 0.0; - last_command_msg->twist.angular.z = 0.0; + last_command_msg_->twist.linear.x = 0.0; + last_command_msg_->twist.angular.z = 0.0; } // command may be limited further by Limiters, // without affecting the stored twist command - TwistStamped command = *last_command_msg; + TwistStamped command = *last_command_msg_; double & linear_command = command.twist.linear.x; double & angular_command = command.twist.angular.z; double Ws_read = traction_joint_[0].velocity_state.get().get_value(); // in radians/s @@ -271,9 +273,9 @@ CallbackReturn TricycleController::on_configure(const rclcpp_lifecycle::State & return CallbackReturn::ERROR; } - const TwistStamped empty_twist; - received_velocity_msg_ptr_.set(std::make_shared(empty_twist)); - + last_command_msg_ = std::make_shared(); + received_velocity_msg_ptr_.set([this](std::shared_ptr & stored_value) + { stored_value = last_command_msg_; }); // Fill last two commands with default constructed commands const AckermannDrive empty_ackermann_drive; previous_commands_.emplace(empty_ackermann_drive); @@ -307,7 +309,8 @@ CallbackReturn TricycleController::on_configure(const rclcpp_lifecycle::State & "time, this message will only be shown once"); msg->header.stamp = get_node()->get_clock()->now(); } - received_velocity_msg_ptr_.set(std::move(msg)); + received_velocity_msg_ptr_.set([msg](std::shared_ptr & stored_value) + { stored_value = std::move(msg); }); }); // initialize odometry publisher and message @@ -397,7 +400,6 @@ CallbackReturn TricycleController::on_cleanup(const rclcpp_lifecycle::State &) return CallbackReturn::ERROR; } - received_velocity_msg_ptr_.set(std::make_shared()); return CallbackReturn::SUCCESS; } diff --git a/tricycle_controller/test/test_tricycle_controller.cpp b/tricycle_controller/test/test_tricycle_controller.cpp index 9d43c2590d..5e868ebeea 100644 --- a/tricycle_controller/test/test_tricycle_controller.cpp +++ b/tricycle_controller/test/test_tricycle_controller.cpp @@ -54,7 +54,8 @@ class TestableTricycleController : public tricycle_controller::TricycleControlle std::shared_ptr getLastReceivedTwist() { std::shared_ptr ret; - received_velocity_msg_ptr_.get(ret); + received_velocity_msg_ptr_.get( + [&ret](const std::shared_ptr & msg) { ret = msg; }); return ret; } From cd730558b2c449568c627f066cdf8bfa8bc4c3bf Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Wed, 4 Dec 2024 15:06:45 +0100 Subject: [PATCH 2/2] Use the .hpp headers from `realtime_tools` package (#1406) --- .../include/admittance_controller/admittance_controller.hpp | 4 ++-- .../include/diff_drive_controller/diff_drive_controller.hpp | 4 ++-- .../force_torque_sensor_broadcaster.hpp | 2 +- .../forward_command_controller/forward_controllers_base.hpp | 2 +- .../include/gpio_controllers/gpio_command_controller.hpp | 4 ++-- .../gripper_controllers/gripper_action_controller.hpp | 4 ++-- .../imu_sensor_broadcaster/imu_sensor_broadcaster.hpp | 2 +- .../joint_state_broadcaster/joint_state_broadcaster.hpp | 2 +- .../joint_trajectory_controller.hpp | 6 +++--- .../mecanum_drive_controller/mecanum_drive_controller.hpp | 4 ++-- .../include/mecanum_drive_controller/odometry.hpp | 4 ++-- .../parallel_gripper_action_controller.hpp | 4 ++-- pid_controller/include/pid_controller/pid_controller.hpp | 4 ++-- .../include/pose_broadcaster/pose_broadcaster.hpp | 2 +- .../range_sensor_broadcaster/range_sensor_broadcaster.hpp | 2 +- .../steering_controllers_library.hpp | 4 ++-- .../include/tricycle_controller/tricycle_controller.hpp | 4 ++-- 17 files changed, 29 insertions(+), 29 deletions(-) diff --git a/admittance_controller/include/admittance_controller/admittance_controller.hpp b/admittance_controller/include/admittance_controller/admittance_controller.hpp index 9be6c3298c..89cd195ce4 100644 --- a/admittance_controller/include/admittance_controller/admittance_controller.hpp +++ b/admittance_controller/include/admittance_controller/admittance_controller.hpp @@ -32,8 +32,8 @@ #include "rclcpp/duration.hpp" #include "rclcpp/time.hpp" #include "rclcpp_lifecycle/state.hpp" -#include "realtime_tools/realtime_buffer.h" -#include "realtime_tools/realtime_publisher.h" +#include "realtime_tools/realtime_buffer.hpp" +#include "realtime_tools/realtime_publisher.hpp" #include "semantic_components/force_torque_sensor.hpp" namespace admittance_controller diff --git a/diff_drive_controller/include/diff_drive_controller/diff_drive_controller.hpp b/diff_drive_controller/include/diff_drive_controller/diff_drive_controller.hpp index 84712fe748..4fc4d37d4b 100644 --- a/diff_drive_controller/include/diff_drive_controller/diff_drive_controller.hpp +++ b/diff_drive_controller/include/diff_drive_controller/diff_drive_controller.hpp @@ -34,8 +34,8 @@ #include "nav_msgs/msg/odometry.hpp" #include "odometry.hpp" #include "rclcpp_lifecycle/state.hpp" -#include "realtime_tools/realtime_box.h" -#include "realtime_tools/realtime_publisher.h" +#include "realtime_tools/realtime_box.hpp" +#include "realtime_tools/realtime_publisher.hpp" #include "tf2_msgs/msg/tf_message.hpp" // auto-generated by generate_parameter_library diff --git a/force_torque_sensor_broadcaster/include/force_torque_sensor_broadcaster/force_torque_sensor_broadcaster.hpp b/force_torque_sensor_broadcaster/include/force_torque_sensor_broadcaster/force_torque_sensor_broadcaster.hpp index e5a5349c32..2364dd7c8b 100644 --- a/force_torque_sensor_broadcaster/include/force_torque_sensor_broadcaster/force_torque_sensor_broadcaster.hpp +++ b/force_torque_sensor_broadcaster/include/force_torque_sensor_broadcaster/force_torque_sensor_broadcaster.hpp @@ -28,7 +28,7 @@ #include "force_torque_sensor_broadcaster_parameters.hpp" #include "geometry_msgs/msg/wrench_stamped.hpp" #include "rclcpp_lifecycle/state.hpp" -#include "realtime_tools/realtime_publisher.h" +#include "realtime_tools/realtime_publisher.hpp" #include "semantic_components/force_torque_sensor.hpp" namespace force_torque_sensor_broadcaster diff --git a/forward_command_controller/include/forward_command_controller/forward_controllers_base.hpp b/forward_command_controller/include/forward_command_controller/forward_controllers_base.hpp index 4858e5ab64..fce7941d8a 100644 --- a/forward_command_controller/include/forward_command_controller/forward_controllers_base.hpp +++ b/forward_command_controller/include/forward_command_controller/forward_controllers_base.hpp @@ -23,7 +23,7 @@ #include "forward_command_controller/visibility_control.h" #include "rclcpp/subscription.hpp" #include "rclcpp_lifecycle/state.hpp" -#include "realtime_tools/realtime_buffer.h" +#include "realtime_tools/realtime_buffer.hpp" #include "std_msgs/msg/float64_multi_array.hpp" namespace forward_command_controller diff --git a/gpio_controllers/include/gpio_controllers/gpio_command_controller.hpp b/gpio_controllers/include/gpio_controllers/gpio_command_controller.hpp index 5149c74a1c..febac1294e 100644 --- a/gpio_controllers/include/gpio_controllers/gpio_command_controller.hpp +++ b/gpio_controllers/include/gpio_controllers/gpio_command_controller.hpp @@ -26,8 +26,8 @@ #include "gpio_controllers/visibility_control.h" #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" #include "rclcpp_lifecycle/state.hpp" -#include "realtime_tools/realtime_buffer.h" -#include "realtime_tools/realtime_publisher.h" +#include "realtime_tools/realtime_buffer.hpp" +#include "realtime_tools/realtime_publisher.hpp" namespace gpio_controllers { diff --git a/gripper_controllers/include/gripper_controllers/gripper_action_controller.hpp b/gripper_controllers/include/gripper_controllers/gripper_action_controller.hpp index 38e0bd8191..478168391b 100644 --- a/gripper_controllers/include/gripper_controllers/gripper_action_controller.hpp +++ b/gripper_controllers/include/gripper_controllers/gripper_action_controller.hpp @@ -37,8 +37,8 @@ #include "gripper_controllers/visibility_control.hpp" #include "hardware_interface/loaned_command_interface.hpp" #include "hardware_interface/loaned_state_interface.hpp" -#include "realtime_tools/realtime_buffer.h" -#include "realtime_tools/realtime_server_goal_handle.h" +#include "realtime_tools/realtime_buffer.hpp" +#include "realtime_tools/realtime_server_goal_handle.hpp" // Project #include "gripper_controllers/hardware_interface_adapter.hpp" diff --git a/imu_sensor_broadcaster/include/imu_sensor_broadcaster/imu_sensor_broadcaster.hpp b/imu_sensor_broadcaster/include/imu_sensor_broadcaster/imu_sensor_broadcaster.hpp index 1ba0b032ac..449020b6e2 100644 --- a/imu_sensor_broadcaster/include/imu_sensor_broadcaster/imu_sensor_broadcaster.hpp +++ b/imu_sensor_broadcaster/include/imu_sensor_broadcaster/imu_sensor_broadcaster.hpp @@ -26,7 +26,7 @@ // auto-generated by generate_parameter_library #include "imu_sensor_broadcaster_parameters.hpp" #include "rclcpp_lifecycle/state.hpp" -#include "realtime_tools/realtime_publisher.h" +#include "realtime_tools/realtime_publisher.hpp" #include "semantic_components/imu_sensor.hpp" #include "sensor_msgs/msg/imu.hpp" diff --git a/joint_state_broadcaster/include/joint_state_broadcaster/joint_state_broadcaster.hpp b/joint_state_broadcaster/include/joint_state_broadcaster/joint_state_broadcaster.hpp index ecc3c767f6..7ac98eccfb 100644 --- a/joint_state_broadcaster/include/joint_state_broadcaster/joint_state_broadcaster.hpp +++ b/joint_state_broadcaster/include/joint_state_broadcaster/joint_state_broadcaster.hpp @@ -25,7 +25,7 @@ #include "joint_state_broadcaster/visibility_control.h" // auto-generated by generate_parameter_library #include "joint_state_broadcaster_parameters.hpp" -#include "realtime_tools/realtime_publisher.h" +#include "realtime_tools/realtime_publisher.hpp" #include "sensor_msgs/msg/joint_state.hpp" #include "urdf/model.h" diff --git a/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp b/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp index 2377b2c0b6..324ccfe4f1 100644 --- a/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp +++ b/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp @@ -36,9 +36,9 @@ #include "rclcpp/timer.hpp" #include "rclcpp_action/server.hpp" #include "rclcpp_lifecycle/state.hpp" -#include "realtime_tools/realtime_buffer.h" -#include "realtime_tools/realtime_publisher.h" -#include "realtime_tools/realtime_server_goal_handle.h" +#include "realtime_tools/realtime_buffer.hpp" +#include "realtime_tools/realtime_publisher.hpp" +#include "realtime_tools/realtime_server_goal_handle.hpp" #include "trajectory_msgs/msg/joint_trajectory.hpp" #include "trajectory_msgs/msg/joint_trajectory_point.hpp" diff --git a/mecanum_drive_controller/include/mecanum_drive_controller/mecanum_drive_controller.hpp b/mecanum_drive_controller/include/mecanum_drive_controller/mecanum_drive_controller.hpp index 0fd7f6af29..f835752d43 100644 --- a/mecanum_drive_controller/include/mecanum_drive_controller/mecanum_drive_controller.hpp +++ b/mecanum_drive_controller/include/mecanum_drive_controller/mecanum_drive_controller.hpp @@ -29,8 +29,8 @@ #include "mecanum_drive_controller_parameters.hpp" #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" #include "rclcpp_lifecycle/state.hpp" -#include "realtime_tools/realtime_buffer.h" -#include "realtime_tools/realtime_publisher.h" +#include "realtime_tools/realtime_buffer.hpp" +#include "realtime_tools/realtime_publisher.hpp" #include "std_srvs/srv/set_bool.hpp" #include "control_msgs/msg/mecanum_drive_controller_state.hpp" diff --git a/mecanum_drive_controller/include/mecanum_drive_controller/odometry.hpp b/mecanum_drive_controller/include/mecanum_drive_controller/odometry.hpp index ac1ad060dd..241809e8c2 100644 --- a/mecanum_drive_controller/include/mecanum_drive_controller/odometry.hpp +++ b/mecanum_drive_controller/include/mecanum_drive_controller/odometry.hpp @@ -16,8 +16,8 @@ #define MECANUM_DRIVE_CONTROLLER__ODOMETRY_HPP_ #include "geometry_msgs/msg/twist.hpp" -#include "realtime_tools/realtime_buffer.h" -#include "realtime_tools/realtime_publisher.h" +#include "realtime_tools/realtime_buffer.hpp" +#include "realtime_tools/realtime_publisher.hpp" #define PLANAR_POINT_DIM 3 diff --git a/parallel_gripper_controller/include/parallel_gripper_controller/parallel_gripper_action_controller.hpp b/parallel_gripper_controller/include/parallel_gripper_controller/parallel_gripper_action_controller.hpp index d303990b89..739d1c570a 100644 --- a/parallel_gripper_controller/include/parallel_gripper_controller/parallel_gripper_action_controller.hpp +++ b/parallel_gripper_controller/include/parallel_gripper_controller/parallel_gripper_action_controller.hpp @@ -37,8 +37,8 @@ #include "hardware_interface/loaned_command_interface.hpp" #include "hardware_interface/loaned_state_interface.hpp" #include "parallel_gripper_controller/visibility_control.hpp" -#include "realtime_tools/realtime_buffer.h" -#include "realtime_tools/realtime_server_goal_handle.h" +#include "realtime_tools/realtime_buffer.hpp" +#include "realtime_tools/realtime_server_goal_handle.hpp" // Project #include "parallel_gripper_action_controller_parameters.hpp" diff --git a/pid_controller/include/pid_controller/pid_controller.hpp b/pid_controller/include/pid_controller/pid_controller.hpp index ce66fd06d4..0aba6cf849 100644 --- a/pid_controller/include/pid_controller/pid_controller.hpp +++ b/pid_controller/include/pid_controller/pid_controller.hpp @@ -29,8 +29,8 @@ #include "pid_controller/visibility_control.h" #include "pid_controller_parameters.hpp" #include "rclcpp_lifecycle/state.hpp" -#include "realtime_tools/realtime_buffer.h" -#include "realtime_tools/realtime_publisher.h" +#include "realtime_tools/realtime_buffer.hpp" +#include "realtime_tools/realtime_publisher.hpp" #include "std_srvs/srv/set_bool.hpp" namespace pid_controller diff --git a/pose_broadcaster/include/pose_broadcaster/pose_broadcaster.hpp b/pose_broadcaster/include/pose_broadcaster/pose_broadcaster.hpp index 621a90cc85..9317fce5e1 100644 --- a/pose_broadcaster/include/pose_broadcaster/pose_broadcaster.hpp +++ b/pose_broadcaster/include/pose_broadcaster/pose_broadcaster.hpp @@ -25,7 +25,7 @@ #include "pose_broadcaster_parameters.hpp" #include "rclcpp/publisher.hpp" #include "rclcpp_lifecycle/state.hpp" -#include "realtime_tools/realtime_publisher.h" +#include "realtime_tools/realtime_publisher.hpp" #include "semantic_components/pose_sensor.hpp" #include "tf2_msgs/msg/tf_message.hpp" diff --git a/range_sensor_broadcaster/include/range_sensor_broadcaster/range_sensor_broadcaster.hpp b/range_sensor_broadcaster/include/range_sensor_broadcaster/range_sensor_broadcaster.hpp index 5a93f95982..2e4e47b018 100644 --- a/range_sensor_broadcaster/include/range_sensor_broadcaster/range_sensor_broadcaster.hpp +++ b/range_sensor_broadcaster/include/range_sensor_broadcaster/range_sensor_broadcaster.hpp @@ -25,7 +25,7 @@ #include "range_sensor_broadcaster/visibility_control.h" #include "range_sensor_broadcaster_parameters.hpp" #include "rclcpp_lifecycle/state.hpp" -#include "realtime_tools/realtime_publisher.h" +#include "realtime_tools/realtime_publisher.hpp" #include "semantic_components/range_sensor.hpp" #include "sensor_msgs/msg/range.hpp" diff --git a/steering_controllers_library/include/steering_controllers_library/steering_controllers_library.hpp b/steering_controllers_library/include/steering_controllers_library/steering_controllers_library.hpp index 84a892d79e..49236986ee 100644 --- a/steering_controllers_library/include/steering_controllers_library/steering_controllers_library.hpp +++ b/steering_controllers_library/include/steering_controllers_library/steering_controllers_library.hpp @@ -23,8 +23,8 @@ #include "controller_interface/chainable_controller_interface.hpp" #include "hardware_interface/handle.hpp" #include "rclcpp_lifecycle/state.hpp" -#include "realtime_tools/realtime_buffer.h" -#include "realtime_tools/realtime_publisher.h" +#include "realtime_tools/realtime_buffer.hpp" +#include "realtime_tools/realtime_publisher.hpp" #include "steering_controllers_library/steering_odometry.hpp" #include "steering_controllers_library/visibility_control.h" #include "steering_controllers_library_parameters.hpp" diff --git a/tricycle_controller/include/tricycle_controller/tricycle_controller.hpp b/tricycle_controller/include/tricycle_controller/tricycle_controller.hpp index 1f9361dadd..4d64549613 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.h" -#include "realtime_tools/realtime_publisher.h" +#include "realtime_tools/realtime_box.hpp" +#include "realtime_tools/realtime_publisher.hpp" #include "std_srvs/srv/empty.hpp" #include "tf2_msgs/msg/tf_message.hpp" #include "tricycle_controller/odometry.hpp"