make a RAII wrapper for ArRobot

src/ros2aria/CMakeLists.txt

@@ -25,7 +25,7 @@ find_package(geometry_msgs REQUIRED)
 # find_package(<dependency> REQUIRED)
 
 
-add_executable(ros2aria src/ros2aria.cpp)
+add_executable(ros2aria src/ros2aria.cpp src/raiibot.cpp)
 ament_target_dependencies(ros2aria rclcpp)
 ament_target_dependencies(ros2aria std_srvs)
 ament_target_dependencies(ros2aria geometry_msgs)

src/ros2aria/src/raiibot.cpp

@@ -0,0 +1,73 @@
+#include <string>
+#include <memory>
+#include <iostream>
+
+#include "Aria/Aria.h"
+
+class RAIIBot
+{
+public:
+    typedef std::shared_ptr<RAIIBot> SharedPtr;
+
+    RAIIBot(std::string port)
+    {
+        this->robot = new ArRobot();
+
+        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("-robotLogPacketsReceived");                    // log received packets
+        // args->add("-robotLogPacketsSent");                        // log sent packets
+        // args->add("-robotLogVelocitiesReceived"); // log received velocities
+        // args->add("-robotLogMovementSent");
+        // args->add("-robotLogMovementReceived");
+        this->robotConn = new ArRobotConnector(argparser, robot);
+        if (!this->robotConn->connectRobot())
+        {
+
+            // RCLCPP_INFO(this->get_logger(), "RosAria: ARIA could not connect to robot! (Check ~port parameter is correct, and permissions on port device, or any errors reported above)");
+            // rclcpp::shutdown();
+        }
+        this->robot->runAsync(true);
+        this->robot->enableMotors();
+    }
+
+    ~RAIIBot()
+    {
+        std::cout << std::endl
+                  << "RAIIBOT DESTRUCTOR" << std::endl;
+        if (this->robotConn != nullptr)
+            this->robotConn->disconnectAll();
+        if (this->args != nullptr)
+            delete this->args;
+        if (this->argparser != nullptr)
+            delete this->argparser;
+        if (this->robotConn != nullptr)
+            delete this->robotConn;
+        if (this->robot != nullptr)
+            delete this->robot;
+    }
+
+    ArRobot *getRobot()
+    {
+        return this->robot;
+    }
+
+    void lock()
+    {
+        this->robot->lock();
+    }
+
+    void unlock()
+    {
+        if (this->robot != nullptr)
+            this->robot->unlock();
+    }
+
+private:
+    ArRobot *robot = nullptr;
+    ArRobotConnector *robotConn = nullptr;
+    ArArgumentBuilder *args = nullptr;
+    ArArgumentParser *argparser = nullptr;
+};

src/ros2aria/src/ros2aria.cpp

@@ -8,6 +8,8 @@
 #include "std_srvs/srv/empty.hpp"
 #include "geometry_msgs/msg/twist.hpp"
 
+#include "./raiibot.cpp"
+
 using std::placeholders::_1;
 using std::placeholders::_2;
 using namespace std::chrono_literals;
@@ -18,25 +20,8 @@ public:
   MinimalSubscriber()
       : Node("minimal_subscriber")
   {
-    this->robot = new ArRobot();
+    this->robot = std::make_shared<RAIIBot>("/dev/ttyS0");
-
+    RCLCPP_INFO(this->get_logger(), "starting subs");
-    ArArgumentBuilder *args = new ArArgumentBuilder();        //  never freed
-    ArArgumentParser *argparser = new ArArgumentParser(args); // Warning never freed
-    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
-    // args->add("-robotLogPacketsReceived");                    // log received packets
-    // args->add("-robotLogPacketsSent");                        // log sent packets
-    // args->add("-robotLogVelocitiesReceived"); // log received velocities
-    // args->add("-robotLogMovementSent");
-    // args->add("-robotLogMovementReceived");
-    this->robotConn = new ArRobotConnector(argparser, robot); // warning never freed
-    if (!this->robotConn->connectRobot())
-    {
-      RCLCPP_INFO(this->get_logger(), "RosAria: ARIA could not connect to robot! (Check ~port parameter is correct, and permissions on port device, or any errors reported above)");
-      rclcpp::shutdown();
-    }
-    this->robot->runAsync(true);
-    this->robot->enableMotors();
 
     cmd_vel_sub_ = this->create_subscription<geometry_msgs::msg::Twist>(
         "cmd_vel", 10, std::bind(&MinimalSubscriber::cmd_vel_callback, this, _1));
@@ -48,8 +33,7 @@ public:
   }
 
 private:
-  ArRobot *robot;
+  RAIIBot::SharedPtr robot;
-  ArRobotConnector *robotConn;
 
   rclcpp::Clock::SharedPtr clock;
   rclcpp::TimerBase::SharedPtr timer;
@@ -59,12 +43,12 @@ private:
     auto now = this->clock->now();
     if (now - this->watchdog > 1s)
     {
-      this->robot->lock();
+      std::lock_guard<RAIIBot> lock(*robot.get());
-      this->robot->setVel(0);
+      auto r = robot->getRobot();
-      this->robot->setRotVel(0);
+      r->setVel(0);
-      if (this->robot->hasLatVel())
+      r->setRotVel(0);
-        this->robot->setLatVel(0);
+      if (r->hasLatVel())
-      this->robot->unlock();
+        r->setLatVel(0);
     }
   }
 
@@ -75,20 +59,22 @@ private:
     x = msg->linear.x;
     y = msg->linear.y;
     z = msg->angular.z;
-    this->robot->lock();
+
+    std::lock_guard<RAIIBot> lock(*robot.get());
+    auto r = robot->getRobot();
+
     this->watchdog = this->clock->now();
-    this->robot->setVel(x * 1e3);
+    r->setVel(x * 1e3);
-    if (robot->hasLatVel())
+    if (r->hasLatVel())
-      robot->setLatVel(y * 1e3);
+      r->setLatVel(y * 1e3);
-    robot->setRotVel(z * 180 / M_PI);
+    r->setRotVel(z * 180 / M_PI);
-    robot->unlock();
   }
+
   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
   {
     RCLCPP_INFO(this->get_logger(), "stop");
-    this->robotConn->disconnectAll();
     rclcpp::shutdown();
   }
   rclcpp::Service<std_srvs::srv::Empty>::SharedPtr stop_service_;