From d120d122a4afbb22071dcabf83d3ef30b5365084 Mon Sep 17 00:00:00 2001
From: Wojciech Kwolek <me@irth.pl>
Date: Thu, 25 Mar 2021 11:00:04 +0000
Subject: [PATCH] Properly disconnect from the robot when stopping the service

---
 src/ros2aria/src/ros2aria.cpp | 47 ++++++++++++++++++-----------------
 1 file changed, 24 insertions(+), 23 deletions(-)

diff --git a/src/ros2aria/src/ros2aria.cpp b/src/ros2aria/src/ros2aria.cpp
index dc2c494..ebb1600 100644
--- a/src/ros2aria/src/ros2aria.cpp
+++ b/src/ros2aria/src/ros2aria.cpp
@@ -15,22 +15,45 @@ public:
   MinimalSubscriber()
       : Node("minimal_subscriber")
   {
+    this->robot = new ArRobot();
+
+    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();
+
     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:
+  ArRobot *robot;
+  ArRobotConnector *robotConn;
+
   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");
+    this->robotConn->disconnectAll();
     rclcpp::shutdown();
   }
   rclcpp::Service<std_srvs::srv::Empty>::SharedPtr service_;
@@ -42,28 +65,6 @@ int main(int argc, char **argv)
   (void)argv;
 
   printf("hello world ros2aria package\n");
-  ArRobot *robot = new ArRobot();
-
-  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");
-  ArRobotConnector *conn = new ArRobotConnector(argparser, robot); // warning never freed
-  if (!conn->connectRobot())
-  {
-    printf("RosAria: ARIA could not connect to robot! (Check ~port parameter is correct, and permissions on port device, or any errors reported above)\n");
-    return 1;
-  }
-  robot->runAsync(true);
-  sleep(3);
-  printf("Enabling motors\n");
-  robot->enableMotors();
-
   // printf("Enabled motors\n");
   // robot->setVel(2);
   // robot->move(500);