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

add sensor update callback (and ensure clean exit still)

Browse Source
This commit is contained in:
Wojciech Kwolek 2021-04-16 13:54:11 +00:00
parent 7851d903d5
commit 0910b1ee3f
2 changed files with 38 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

19
src/ros2aria/src/raiibot.cpp
View File

@ -21,7 +21,7 @@ public:
args = new ArArgumentBuilder(); args = new ArArgumentBuilder();
this->argparser = new ArArgumentParser(args); 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) 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("-robotLogPacketsReceived"); // log received packets
// args->add("-robotLogPacketsSent"); // log sent packets // args->add("-robotLogPacketsSent"); // log sent packets
// args->add("-robotLogVelocitiesReceived"); // log received velocities // args->add("-robotLogVelocitiesReceived"); // log received velocities
@ -41,9 +41,23 @@ public:
~RAIIBot() ~RAIIBot()
{ {
std::cout << std::endl 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) if (this->robotConn != nullptr)
{
std::cout << "disconnecting" << std::endl;
this->robotConn->disconnectAll(); this->robotConn->disconnectAll();
std::cout << "disconnected" << std::endl;
}
if (this->args != nullptr) if (this->args != nullptr)
delete this->args; delete this->args;
if (this->argparser != nullptr) if (this->argparser != nullptr)
@ -61,6 +75,7 @@ public:
void lock() void lock()
{ {
if (this->robot != nullptr)
this->robot->lock(); this->robot->lock();
} }

20
src/ros2aria/src/ros2aria.cpp
View File

@ -18,7 +18,8 @@ class Ros2Aria : public rclcpp::Node
{ {
public: public:
Ros2Aria() Ros2Aria()
: Node("minimal_subscriber") : Node("ros2aria"),
sensorCb(this, &Ros2Aria::publish)
{ {
this->robot = std::make_shared<RAIIBot>(this, "/dev/ttyS0"); 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_sub_ = this->create_subscription<geometry_msgs::msg::Twist>(
"cmd_vel", 10, std::bind(&Ros2Aria::cmd_vel_callback, this, _1)); "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)); 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: private:
RAIIBot::SharedPtr robot; RAIIBot::SharedPtr robot;
ArFunctorC<Ros2Aria> sensorCb;
void cmd_vel_callback(const geometry_msgs::msg::Twist::SharedPtr msg) void cmd_vel_callback(const geometry_msgs::msg::Twist::SharedPtr msg)
{ {
float x, y, z; float x, y, z;