diff --git a/planning/kinematic_reader/include/kinematic_reader/kinematic_reader.hpp b/planning/kinematic_reader/include/kinematic_reader/kinematic_reader.hpp index c2a708538c612..d8f0d3dca6cce 100644 --- a/planning/kinematic_reader/include/kinematic_reader/kinematic_reader.hpp +++ b/planning/kinematic_reader/include/kinematic_reader/kinematic_reader.hpp @@ -19,10 +19,10 @@ using nav_msgs::msg::Odometry; { public: explicit KinematicReader(const rclcpp::NodeOptions & options); - void odomCallback(const nav_msgs::msg::Odometry::SharedPtr msg); - + void timer_callback(); private: rclcpp::Subscription::SharedPtr sub_; + rclcpp::TimerBase::SharedPtr timer_; Odometry::ConstSharedPtr current_kinematics_; }; diff --git a/planning/kinematic_reader/src/kinematic_reader.cpp b/planning/kinematic_reader/src/kinematic_reader.cpp index 2d560896ff6e7..38b98a92f63a3 100644 --- a/planning/kinematic_reader/src/kinematic_reader.cpp +++ b/planning/kinematic_reader/src/kinematic_reader.cpp @@ -1,4 +1,7 @@ #include "kinematic_reader/kinematic_reader.hpp" +#include + +using namespace std::chrono_literals; namespace kinematic_reader { @@ -7,16 +10,35 @@ KinematicReader::KinematicReader(const rclcpp::NodeOptions & options) { // Create a subscription to the odometry topic - + auto no_executed = [this](const nav_msgs::msg::Odometry::ConstSharedPtr msg) -> void { + assert(false); + }; + + rclcpp::CallbackGroup::SharedPtr cb_group_noexec = this->create_callback_group( + rclcpp::CallbackGroupType::MutuallyExclusive, false); + auto subscription_options = rclcpp::SubscriptionOptions(); + subscription_options.callback_group = cb_group_noexec; + + sub_ = this->create_subscription( - "odom", 10, std::bind(&KinematicReader::odomCallback, this, std::placeholders::_1)); + "odom", 1, no_executed); + + timer_ = this->create_wall_timer( + 500ms, std::bind(&KinematicReader::timer_callback, this)); } -void KinematicReader::odomCallback(const nav_msgs::msg::Odometry::SharedPtr msg) +void KinematicReader::timer_callback() { - // Store the linear and angular velocities - linear_velocity_ = msg->twist.twist.linear.x; - angular_velocity_ = msg->twist.twist.angular.z; + // Do something with the data + nav_msgs::msg::Odometry odom_msg; + rclcpp::MessageInfo msg_info; + + if (sub_->take(odom_msg, msg_info)) { + RCLCPP_INFO(this->get_logger(), "Received odometry message"); + } + else { + RCLCPP_INFO(this->get_logger(), "No odometry message received"); + } } } // namespace kinematic_reader