add sensor update callback (and ensure clean exit still)

src/ros2aria/src/raiibot.cpp

@@ -21,7 +21,7 @@ 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
+        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,6 +75,7 @@ public:
 
     void lock()
     {
+        if (this->robot != nullptr)
             this->robot->lock();
     }
 

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;