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

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

@ -20,8 +20,8 @@ public:
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
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 // TODO: use `port` variable.
// args->add("-robotLogPacketsReceived"); // log received packets
// args->add("-robotLogPacketsSent"); // log sent packets
// args->add("-robotLogVelocitiesReceived"); // log received velocities
@ -41,9 +41,23 @@ public:
~RAIIBot()
{
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)
{
std::cout << "disconnecting" << std::endl;
this->robotConn->disconnectAll();
std::cout << "disconnected" << std::endl;
}
if (this->args != nullptr)
delete this->args;
if (this->argparser != nullptr)
@ -61,7 +75,8 @@ public:
void lock()
{
this->robot->lock();
if (this->robot != nullptr)
this->robot->lock();
}
void unlock()

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

@ -18,7 +18,8 @@ class Ros2Aria : public rclcpp::Node
{
public:
Ros2Aria()
: Node("minimal_subscriber")
: Node("ros2aria"),
sensorCb(this, &Ros2Aria::publish)
{
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", 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));
// 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:
RAIIBot::SharedPtr robot;
ArFunctorC<Ros2Aria> sensorCb;
void cmd_vel_callback(const geometry_msgs::msg::Twist::SharedPtr msg)
{
float x, y, z;