From 0910b1ee3f5ebf4fb0929f770048ad658f856291 Mon Sep 17 00:00:00 2001
From: Wojciech Kwolek <me@irth.pl>
Date: Fri, 16 Apr 2021 13:54:11 +0000
Subject: [PATCH] add sensor update callback (and ensure clean exit still)

---
 src/ros2aria/src/raiibot.cpp  | 23 +++++++++++++++++++----
 src/ros2aria/src/ros2aria.cpp | 20 +++++++++++++++++++-
 2 files changed, 38 insertions(+), 5 deletions(-)

diff --git a/src/ros2aria/src/raiibot.cpp b/src/ros2aria/src/raiibot.cpp
index 275a785..faa68f6 100644
--- a/src/ros2aria/src/raiibot.cpp
+++ b/src/ros2aria/src/raiibot.cpp
@@ -20,8 +20,8 @@ public:
 
         args = new ArArgumentBuilder();
         this->argparser = new ArArgumentParser(args);
-        argparser->loadDefaultArguments(); // adds any arguments given in /etc/Aria.args.  Useful on robots with unusual serial port or baud rate (e.g. pioneer lx)
-        args->add("-robotPort ", port);    // pass robot's serial port to Aria
+        argparser->loadDefaultArguments();  // adds any arguments given in /etc/Aria.args.  Useful on robots with unusual serial port or baud rate (e.g. pioneer lx)
+        args->add("-robotPort /dev/ttyS0"); // pass robot's serial port to Aria // TODO: use `port` variable.
         // args->add("-robotLogPacketsReceived");                    // log received packets
         // args->add("-robotLogPacketsSent");                        // log sent packets
         // args->add("-robotLogVelocitiesReceived"); // log received velocities
@@ -41,9 +41,23 @@ public:
     ~RAIIBot()
     {
         std::cout << std::endl
-                  << "RAIIBOT DESTRUCTOR" << std::endl;
+                  << "RAIIBOT DESTRUCTOR RUNNING!" << std::endl;
+
+        if (this->robot != nullptr)
+        {
+            this->robot->lock();
+            std::cout << "disabling motors" << std::endl;
+            this->robot->disableMotors();
+            std::cout << "disabled motors" << std::endl;
+            Aria::shutdown();
+        }
         if (this->robotConn != nullptr)
+        {
+            std::cout << "disconnecting" << std::endl;
             this->robotConn->disconnectAll();
+            std::cout << "disconnected" << std::endl;
+        }
+
         if (this->args != nullptr)
             delete this->args;
         if (this->argparser != nullptr)
@@ -61,7 +75,8 @@ public:
 
     void lock()
     {
-        this->robot->lock();
+        if (this->robot != nullptr)
+            this->robot->lock();
     }
 
     void unlock()
diff --git a/src/ros2aria/src/ros2aria.cpp b/src/ros2aria/src/ros2aria.cpp
index 3cc0e5d..24ef17c 100644
--- a/src/ros2aria/src/ros2aria.cpp
+++ b/src/ros2aria/src/ros2aria.cpp
@@ -18,7 +18,8 @@ class Ros2Aria : public rclcpp::Node
 {
 public:
   Ros2Aria()
-      : Node("minimal_subscriber")
+      : Node("ros2aria"),
+        sensorCb(this, &Ros2Aria::publish)
   {
     this->robot = std::make_shared<RAIIBot>(this, "/dev/ttyS0");
 
@@ -26,11 +27,28 @@ public:
     cmd_vel_sub_ = this->create_subscription<geometry_msgs::msg::Twist>(
         "cmd_vel", 10, std::bind(&Ros2Aria::cmd_vel_callback, this, _1));
     stop_service_ = this->create_service<std_srvs::srv::Empty>("stop", std::bind(&Ros2Aria::stop, this, _1, _2));
+
+    // listen to sensors
+    auto r = robot->getRobot();
+    r->addSensorInterpTask("ROSPublishingTask", 100, &this->sensorCb);
+  }
+
+  ~Ros2Aria()
+  {
+    auto r = robot->getRobot();
+    r->remSensorInterpTask("ROSPublishingTask");
+  }
+
+  void publish()
+  {
+    RCLCPP_INFO(this->get_logger(), "publish");
   }
 
 private:
   RAIIBot::SharedPtr robot;
 
+  ArFunctorC<Ros2Aria> sensorCb;
+
   void cmd_vel_callback(const geometry_msgs::msg::Twist::SharedPtr msg)
   {
     float x, y, z;