L1.5/ros2aria
L1.5/ros2aria
7
0
Fork 0
Code Issues 5 Pull Requests Projects 1 Releases Wiki Activity

make a RAII wrapper for ArRobot

Browse Source
This commit is contained in:
Wojciech Kwolek 2021-04-08 12:37:48 +00:00
parent cc053a899f
commit c034da9010
3 changed files with 94 additions and 35 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
src/ros2aria/CMakeLists.txt
View File

@ -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)

73
src/ros2aria/src/raiibot.cpp Normal file
View File

@ -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;
};

54
src/ros2aria/src/ros2aria.cpp
View File

@ -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<RAIIBot>("/dev/ttyS0");
RCLCPP_INFO(this->get_logger(), "starting subs");
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;
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<RAIIBot> 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<RAIIBot> 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<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_;