add files

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<nav_msgs::msg::Odometry>::SharedPtr sub_;
+    rclcpp::TimerBase::SharedPtr timer_;
     Odometry::ConstSharedPtr current_kinematics_;
 
   };

planning/kinematic_reader/src/kinematic_reader.cpp

@@ -1,4 +1,7 @@
 #include "kinematic_reader/kinematic_reader.hpp"
+#include <chrono>
+
+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<nav_msgs::msg::Odometry>(
-        "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