basic cmd_vel

.vscode/settings.json

@@ -1,5 +1,8 @@
 {
     "python.autoComplete.extraPaths": [
         "/opt/ros/foxy/lib/python3.8/site-packages"
-    ]
+    ],
+    "files.associations": {
+        "functional": "cpp"
+    }
 }

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>

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>

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,30 @@ public:
     this->robot->runAsync(true);
     this->robot->enableMotors();
 
-    subscription_ = this->create_subscription<std_msgs::msg::String>(
+    cmd_vel_sub_ = this->create_subscription<geometry_msgs::msg::Twist>(
-        "topic", 10, std::bind(&MinimalSubscriber::topic_callback, this, _1));
+        "cmd_vel", 10, std::bind(&MinimalSubscriber::cmd_vel_callback, this, _1));
-    service_ = this->create_service<std_srvs::srv::Empty>("stop", std::bind(&MinimalSubscriber::stop, this, _1, _2));
+    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);
+    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<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 +68,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)