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::SharedPtr cmd_vel_sub_;