From 1bb7b6a17df4dbca3caa6e0287a095b866e3c08d Mon Sep 17 00:00:00 2001
From: Wojciech Kwolek <me@irth.pl>
Date: Fri, 19 Mar 2021 15:09:56 +0000
Subject: [PATCH] add a simple node

---
 src/ros2aria/CMakeLists.txt   |  5 +++
 src/ros2aria/package.xml      |  3 ++
 src/ros2aria/src/ros2aria.cpp | 61 ++++++++++++++++++++++++++++-------
 3 files changed, 58 insertions(+), 11 deletions(-)

diff --git a/src/ros2aria/CMakeLists.txt b/src/ros2aria/CMakeLists.txt
index 021ef11..039b025 100644
--- a/src/ros2aria/CMakeLists.txt
+++ b/src/ros2aria/CMakeLists.txt
@@ -17,17 +17,22 @@ endif()
 
 # find dependencies
 find_package(ament_cmake REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(std_srvs REQUIRED)
 # uncomment the following section in order to fill in
 # further dependencies manually.
 # find_package(<dependency> REQUIRED)
 
 
 add_executable(ros2aria src/ros2aria.cpp)
+ament_target_dependencies(ros2aria rclcpp)
+ament_target_dependencies(ros2aria std_srvs)
 target_include_directories(ros2aria PUBLIC
   $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
   $<INSTALL_INTERFACE:include>
   /usr/local/Aria/include)
 
+
 target_link_libraries(ros2aria /usr/local/Aria/lib/libAria.so)
 
 install(TARGETS ros2aria
diff --git a/src/ros2aria/package.xml b/src/ros2aria/package.xml
index 50e5751..a940bc3 100644
--- a/src/ros2aria/package.xml
+++ b/src/ros2aria/package.xml
@@ -12,6 +12,9 @@
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>ament_lint_common</test_depend>
 
+  <depend>rclcpp</depend>
+  <depend>std_srvs</depend>
+
   <export>
     <build_type>ament_cmake</build_type>
   </export>
diff --git a/src/ros2aria/src/ros2aria.cpp b/src/ros2aria/src/ros2aria.cpp
index aab96fb..dc2c494 100644
--- a/src/ros2aria/src/ros2aria.cpp
+++ b/src/ros2aria/src/ros2aria.cpp
@@ -1,7 +1,41 @@
 #include <cstdio>
+#include <memory>
 
 #include "Aria/Aria.h"
 
+#include "rclcpp/rclcpp.hpp"
+#include "std_msgs/msg/string.hpp"
+#include "std_srvs/srv/empty.hpp"
+using std::placeholders::_1;
+using std::placeholders::_2;
+
+class MinimalSubscriber : public rclcpp::Node
+{
+public:
+  MinimalSubscriber()
+      : Node("minimal_subscriber")
+  {
+    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));
+  }
+
+private:
+  void topic_callback(const std_msgs::msg::String::SharedPtr msg) const
+  {
+    RCLCPP_INFO(this->get_logger(), "I heard: '%s'", msg->data.c_str());
+  }
+  rclcpp::Subscription<std_msgs::msg::String>::SharedPtr subscription_;
+
+private:
+  void stop(const std::shared_ptr<std_srvs::srv::Empty::Request> request, std::shared_ptr<std_srvs::srv::Empty::Response> response) const
+  {
+    RCLCPP_INFO(this->get_logger(), "stop");
+    rclcpp::shutdown();
+  }
+  rclcpp::Service<std_srvs::srv::Empty>::SharedPtr service_;
+};
+
 int main(int argc, char **argv)
 {
   (void)argc;
@@ -16,9 +50,9 @@ int main(int argc, char **argv)
   args->add("-robotPort /dev/ttyS0");                       // pass robot's serial port to Aria
   // args->add("-robotLogPacketsReceived");                    // log received packets
   // args->add("-robotLogPacketsSent");                        // log sent packets
-  args->add("-robotLogVelocitiesReceived"); // log received velocities
-  args->add("-robotLogMovementSent");
-  args->add("-robotLogMovementReceived");
+  // args->add("-robotLogVelocitiesReceived"); // log received velocities
+  // args->add("-robotLogMovementSent");
+  // args->add("-robotLogMovementReceived");
   ArRobotConnector *conn = new ArRobotConnector(argparser, robot); // warning never freed
   if (!conn->connectRobot())
   {
@@ -30,17 +64,22 @@ int main(int argc, char **argv)
   printf("Enabling motors\n");
   robot->enableMotors();
 
-  printf("Enabled motors\n");
-  robot->setVel(2);
-  robot->move(500);
+  // printf("Enabled motors\n");
+  // robot->setVel(2);
+  // robot->move(500);
 
-  sleep(10);
+  // sleep(10);
 
-  robot->move(-500);
-  sleep(2);
-  conn->disconnectAll();
+  // robot->move(-500);
+  // sleep(2);
+  // conn->disconnectAll();
 
-  return 0;
+  // return 0;
+
+  // return 0;
+  rclcpp::init(argc, argv);
+  rclcpp::spin(std::make_shared<MinimalSubscriber>());
+  rclcpp::shutdown();
 
   return 0;
 }