From 95f81b71c5839b053a1ecbaf55f6af5fc67cfbab Mon Sep 17 00:00:00 2001
From: Wojciech Kwolek <me@irth.pl>
Date: Thu, 25 Mar 2021 11:14:39 +0000
Subject: [PATCH] Set up a listener for cmd_vel

---
 .vscode/settings.json         |  5 ++++-
 src/ros2aria/CMakeLists.txt   |  2 ++
 src/ros2aria/package.xml      |  1 +
 src/ros2aria/src/ros2aria.cpp | 16 +++++++++-------
 4 files changed, 16 insertions(+), 8 deletions(-)

diff --git a/.vscode/settings.json b/.vscode/settings.json
index 6b40311..b01d19d 100644
--- a/.vscode/settings.json
+++ b/.vscode/settings.json
@@ -1,5 +1,8 @@
 {
     "python.autoComplete.extraPaths": [
         "/opt/ros/foxy/lib/python3.8/site-packages"
-    ]
+    ],
+    "files.associations": {
+        "functional": "cpp"
+    }
 }
\ No newline at end of file
diff --git a/src/ros2aria/CMakeLists.txt b/src/ros2aria/CMakeLists.txt
index 039b025..e13f990 100644
--- a/src/ros2aria/CMakeLists.txt
+++ b/src/ros2aria/CMakeLists.txt
@@ -19,6 +19,7 @@ endif()
 find_package(ament_cmake REQUIRED)
 find_package(rclcpp REQUIRED)
 find_package(std_srvs REQUIRED)
+find_package(geometry_msgs REQUIRED)
 # uncomment the following section in order to fill in
 # further dependencies manually.
 # find_package(<dependency> REQUIRED)
@@ -27,6 +28,7 @@ find_package(std_srvs REQUIRED)
 add_executable(ros2aria src/ros2aria.cpp)
 ament_target_dependencies(ros2aria rclcpp)
 ament_target_dependencies(ros2aria std_srvs)
+ament_target_dependencies(ros2aria geometry_msgs)
 target_include_directories(ros2aria PUBLIC
   $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
   $<INSTALL_INTERFACE:include>
diff --git a/src/ros2aria/package.xml b/src/ros2aria/package.xml
index a940bc3..ac2ad23 100644
--- a/src/ros2aria/package.xml
+++ b/src/ros2aria/package.xml
@@ -14,6 +14,7 @@
 
   <depend>rclcpp</depend>
   <depend>std_srvs</depend>
+  <depend>geometry_msgs</depend>
 
   <export>
     <build_type>ament_cmake</build_type>
diff --git a/src/ros2aria/src/ros2aria.cpp b/src/ros2aria/src/ros2aria.cpp
index ebb1600..24db7a5 100644
--- a/src/ros2aria/src/ros2aria.cpp
+++ b/src/ros2aria/src/ros2aria.cpp
@@ -6,6 +6,8 @@
 #include "rclcpp/rclcpp.hpp"
 #include "std_msgs/msg/string.hpp"
 #include "std_srvs/srv/empty.hpp"
+#include "geometry_msgs/msg/twist.hpp"
+
 using std::placeholders::_1;
 using std::placeholders::_2;
 
@@ -35,20 +37,20 @@ public:
     this->robot->runAsync(true);
     this->robot->enableMotors();
 
-    subscription_ = this->create_subscription<std_msgs::msg::String>(
-        "topic", 10, std::bind(&MinimalSubscriber::topic_callback, this, _1));
-    service_ = this->create_service<std_srvs::srv::Empty>("stop", std::bind(&MinimalSubscriber::stop, this, _1, _2));
+    cmd_vel_sub_ = this->create_subscription<geometry_msgs::msg::Twist>(
+        "cmd_vel", 10, std::bind(&MinimalSubscriber::cmd_vel_callback, this, _1));
+    stop_service_ = this->create_service<std_srvs::srv::Empty>("stop", std::bind(&MinimalSubscriber::stop, this, _1, _2));
   }
 
 private:
   ArRobot *robot;
   ArRobotConnector *robotConn;
 
-  void topic_callback(const std_msgs::msg::String::SharedPtr msg) const
+  void cmd_vel_callback(const geometry_msgs::msg::Twist::SharedPtr msg) const
   {
-    RCLCPP_INFO(this->get_logger(), "I heard: '%s'", msg->data.c_str());
+    RCLCPP_INFO(this->get_logger(), "cmd_vel: %f", msg->linear.x);
   }
-  rclcpp::Subscription<std_msgs::msg::String>::SharedPtr subscription_;
+  rclcpp::Subscription<geometry_msgs::msg::Twist>::SharedPtr cmd_vel_sub_;
 
   void stop(const std::shared_ptr<std_srvs::srv::Empty::Request> request, std::shared_ptr<std_srvs::srv::Empty::Response> response) const
   {
@@ -56,7 +58,7 @@ private:
     this->robotConn->disconnectAll();
     rclcpp::shutdown();
   }
-  rclcpp::Service<std_srvs::srv::Empty>::SharedPtr service_;
+  rclcpp::Service<std_srvs::srv::Empty>::SharedPtr stop_service_;
 };
 
 int main(int argc, char **argv)