From c034da9010c652de1d0d236ab65fb95715936cee Mon Sep 17 00:00:00 2001 From: Wojciech Kwolek Date: Thu, 8 Apr 2021 12:37:48 +0000 Subject: [PATCH] make a RAII wrapper for ArRobot --- src/ros2aria/CMakeLists.txt | 2 +- src/ros2aria/src/raiibot.cpp | 73 +++++++++++++++++++++++++++++++++++ src/ros2aria/src/ros2aria.cpp | 54 ++++++++++---------------- 3 files changed, 94 insertions(+), 35 deletions(-) create mode 100644 src/ros2aria/src/raiibot.cpp diff --git a/src/ros2aria/CMakeLists.txt b/src/ros2aria/CMakeLists.txt index e13f990..3d859a6 100644 --- a/src/ros2aria/CMakeLists.txt +++ b/src/ros2aria/CMakeLists.txt @@ -25,7 +25,7 @@ find_package(geometry_msgs REQUIRED) # find_package( 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) diff --git a/src/ros2aria/src/raiibot.cpp b/src/ros2aria/src/raiibot.cpp new file mode 100644 index 0000000..9f83273 --- /dev/null +++ b/src/ros2aria/src/raiibot.cpp @@ -0,0 +1,73 @@ +#include +#include +#include + +#include "Aria/Aria.h" + +class RAIIBot +{ +public: + typedef std::shared_ptr 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; +}; \ No newline at end of file diff --git a/src/ros2aria/src/ros2aria.cpp b/src/ros2aria/src/ros2aria.cpp index 9c07684..7e6cafc 100644 --- a/src/ros2aria/src/ros2aria.cpp +++ b/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(); - - 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(); + this->robot = std::make_shared("/dev/ttyS0"); + RCLCPP_INFO(this->get_logger(), "starting subs"); cmd_vel_sub_ = this->create_subscription( "cmd_vel", 10, std::bind(&MinimalSubscriber::cmd_vel_callback, this, _1)); @@ -48,8 +33,7 @@ public: } private: - ArRobot *robot; - ArRobotConnector *robotConn; + RAIIBot::SharedPtr robot; 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(); - this->robot->setVel(0); - this->robot->setRotVel(0); - if (this->robot->hasLatVel()) - this->robot->setLatVel(0); - this->robot->unlock(); + std::lock_guard lock(*robot.get()); + auto r = robot->getRobot(); + r->setVel(0); + r->setRotVel(0); + if (r->hasLatVel()) + 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 lock(*robot.get()); + auto r = robot->getRobot(); + this->watchdog = this->clock->now(); - this->robot->setVel(x * 1e3); - if (robot->hasLatVel()) - robot->setLatVel(y * 1e3); - robot->setRotVel(z * 180 / M_PI); - robot->unlock(); + r->setVel(x * 1e3); + if (r->hasLatVel()) + r->setLatVel(y * 1e3); + r->setRotVel(z * 180 / M_PI); } + rclcpp::Subscription::SharedPtr cmd_vel_sub_; void stop(const std::shared_ptr request, std::shared_ptr response) const { RCLCPP_INFO(this->get_logger(), "stop"); - this->robotConn->disconnectAll(); rclcpp::shutdown(); } rclcpp::Service::SharedPtr stop_service_;