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

Properly disconnect from the robot when stopping the service

Browse Source
This commit is contained in:
Wojciech Kwolek 2021-03-25 11:00:04 +00:00
parent 1bb7b6a17d
commit d120d122a4
1 changed files with 24 additions and 23 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

47
src/ros2aria/src/ros2aria.cpp
View File

@ -15,22 +15,45 @@ public:
MinimalSubscriber() MinimalSubscriber()
: Node("minimal_subscriber") : 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>( subscription_ = this->create_subscription<std_msgs::msg::String>(
"topic", 10, std::bind(&MinimalSubscriber::topic_callback, this, _1)); "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)); service_ = this->create_service<std_srvs::srv::Empty>("stop", std::bind(&MinimalSubscriber::stop, this, _1, _2));
} }
private: private:
ArRobot *robot;
ArRobotConnector *robotConn;
void topic_callback(const std_msgs::msg::String::SharedPtr msg) const void topic_callback(const std_msgs::msg::String::SharedPtr msg) const
{ {
RCLCPP_INFO(this->get_logger(), "I heard: '%s'", msg->data.c_str()); RCLCPP_INFO(this->get_logger(), "I heard: '%s'", msg->data.c_str());
} }
rclcpp::Subscription<std_msgs::msg::String>::SharedPtr subscription_; 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 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"); RCLCPP_INFO(this->get_logger(), "stop");
this->robotConn->disconnectAll();
rclcpp::shutdown(); rclcpp::shutdown();
} }
rclcpp::Service<std_srvs::srv::Empty>::SharedPtr service_; rclcpp::Service<std_srvs::srv::Empty>::SharedPtr service_;
@ -42,28 +65,6 @@ int main(int argc, char **argv)
(void)argv; (void)argv;
printf("hello world ros2aria package\n"); 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"); // printf("Enabled motors\n");
// robot->setVel(2); // robot->setVel(2);
// robot->move(500); // robot->move(500);