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::SharedPtr publisher_; + rclcpp::Subscription::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 @@ rclcpp sample_msgs geometry_msgs + nav_msgs tf2 tf2_ros tf2_geometry_msgs 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("/example_string", 20); + + subscriber_ = this -> create_subscription( + "/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)