From c1e0a1bf5ecbb8b19d5ac68c11fd711f0f48f53c Mon Sep 17 00:00:00 2001
From: akilpath <akilpath@gmail.com>
Date: Sat, 11 May 2024 00:23:40 +0000
Subject: [PATCH] Deliverables #5

---
 src/robot/control/CMakeLists.txt           |  2 ++
 src/robot/control/include/control_node.hpp |  7 ++++++-
 src/robot/control/package.xml              |  1 +
 src/robot/control/src/control_node.cpp     | 21 +++++++++++++++++++--
 4 files changed, 28 insertions(+), 3 deletions(-)

diff --git a/src/robot/control/CMakeLists.txt b/src/robot/control/CMakeLists.txt
index 827a2b3..33935d5 100644
--- a/src/robot/control/CMakeLists.txt
+++ b/src/robot/control/CMakeLists.txt
@@ -14,6 +14,7 @@ endif()
 find_package(ament_cmake REQUIRED) # ROS2 build tool
 find_package(rclcpp REQUIRED)      # ROS2 C++ package
 find_package(sample_msgs REQUIRED) # Custom package containing ROS2 messages
+find_package(nav_msgs REQUIRED)
 find_package(geometry_msgs REQUIRED)
 find_package(tf2 REQUIRED)
 find_package(tf2_ros REQUIRED)
@@ -37,6 +38,7 @@ ament_target_dependencies(control_lib
   rclcpp
   sample_msgs
   geometry_msgs
+  nav_msgs
   tf2
   tf2_ros
   tf2_geometry_msgs
diff --git a/src/robot/control/include/control_node.hpp b/src/robot/control/include/control_node.hpp
index 84539ec..ee2fd31 100644
--- a/src/robot/control/include/control_node.hpp
+++ b/src/robot/control/include/control_node.hpp
@@ -11,7 +11,8 @@
 #include "tf2_ros/transform_listener.h"
 #include "tf2_ros/buffer.h"
 #include "tf2_geometry_msgs/tf2_geometry_msgs.h"
-#include "std_msgs/msg/string.h"
+#include "std_msgs/msg/string.hpp"
+#include "nav_msgs/msg/odometry.hpp"
 
 class ControlNode : public rclcpp::Node {
   public:
@@ -20,7 +21,11 @@ class ControlNode : public rclcpp::Node {
   private:
     robot::ControlCore control_;
     rclcpp::TimerBase::SharedPtr timer_;
+    rclcpp::Publisher<std_msgs::msg::String>::SharedPtr publisher_;
+    rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr subscriber_;
     void timer_callback();
+    void subscription_callback(const nav_msgs::msg::Odometry::SharedPtr msg);
+
 
 };
 
diff --git a/src/robot/control/package.xml b/src/robot/control/package.xml
index 434289c..6056928 100644
--- a/src/robot/control/package.xml
+++ b/src/robot/control/package.xml
@@ -12,6 +12,7 @@
   <depend>rclcpp</depend>
   <depend>sample_msgs</depend>
   <depend>geometry_msgs</depend>
+  <depend>nav_msgs</depend>
   <depend>tf2</depend>
   <depend>tf2_ros</depend>
   <depend>tf2_geometry_msgs</depend>
diff --git a/src/robot/control/src/control_node.cpp b/src/robot/control/src/control_node.cpp
index 5ea9158..e270216 100644
--- a/src/robot/control/src/control_node.cpp
+++ b/src/robot/control/src/control_node.cpp
@@ -2,13 +2,30 @@
 
 #include "control_node.hpp"
 
+#define timer_delay 1500
 ControlNode::ControlNode(): Node("control"), control_(robot::ControlCore())
 {
-  timer_ = this->create_wall_timer(std::chrono::milliseconds(1500), std::bind(&ControlNode::timer_callback, this));
+  timer_ = this->create_wall_timer(std::chrono::milliseconds(timer_delay), std::bind(&ControlNode::timer_callback, this));
+  publisher_ = this->create_publisher<std_msgs::msg::String>("/example_string", 20);
+
+  subscriber_ = this -> create_subscription<nav_msgs::msg::Odometry>(
+                        "/model/robot/odometry", 20, 
+                        std::bind(&ControlNode::subscription_callback, this, std::placeholders::_1));
 }
 
 void ControlNode::timer_callback(){
-  RCLCPP_INFO(this->get_logger(), "Timer callbakc");
+  auto message = std_msgs::msg::String();
+  message.data = "Published message";
+  publisher_ -> publish(message);
+  RCLCPP_INFO(this->get_logger(), "Deliverables");
+}
+
+void ControlNode::subscription_callback(const nav_msgs::msg::Odometry::SharedPtr msg)
+{
+  float x = msg -> pose.pose.position.x;
+  float y = msg -> pose.pose.position.y;
+  float z = msg -> pose.pose.position.z;
+  RCLCPP_INFO(this->get_logger(), "Coordinates (x,y,z): (%f, %f, %f)", x,y,z);
 }
 
 int main(int argc, char ** argv)