From 50767ebc272b6601051471f415b4617fbdfc3b24 Mon Sep 17 00:00:00 2001
From: Wojciech Kwolek <me@irth.pl>
Date: Thu, 25 Mar 2021 11:31:44 +0000
Subject: [PATCH] basic cmd_vel

---
 src/ros2aria/src/ros2aria.cpp | 10 ++++++++++
 1 file changed, 10 insertions(+)

diff --git a/src/ros2aria/src/ros2aria.cpp b/src/ros2aria/src/ros2aria.cpp
index 24db7a5..23fe0c4 100644
--- a/src/ros2aria/src/ros2aria.cpp
+++ b/src/ros2aria/src/ros2aria.cpp
@@ -49,6 +49,16 @@ private:
   void cmd_vel_callback(const geometry_msgs::msg::Twist::SharedPtr msg) const
   {
     RCLCPP_INFO(this->get_logger(), "cmd_vel: %f", msg->linear.x);
+    float x, y, z;
+    x = msg->linear.x;
+    y = msg->linear.y;
+    z = msg->angular.z;
+    this->robot->lock();
+    this->robot->setVel(x * 1e3);
+    if (robot->hasLatVel())
+      robot->setLatVel(y * 1e3);
+    robot->setRotVel(z * 180 / M_PI);
+    robot->unlock();
   }
   rclcpp::Subscription<geometry_msgs::msg::Twist>::SharedPtr cmd_vel_sub_;