mirror of
https://github.com/westonrobot/scout_ros.git
synced 2026-01-12 11:20:02 +08:00
added signal to close node properly
This commit is contained in:
@@ -11,12 +11,17 @@
|
||||
using namespace westonrobot;
|
||||
|
||||
std::shared_ptr<ScoutBase> robot;
|
||||
bool keep_run = true;
|
||||
|
||||
void DetachRobot(int signal) { keep_run = false; }
|
||||
|
||||
int main(int argc, char **argv) {
|
||||
// setup ROS node
|
||||
ros::init(argc, argv, "scout_node");
|
||||
ros::NodeHandle node(""), private_node("~");
|
||||
|
||||
std::signal(SIGINT, DetachRobot);
|
||||
|
||||
// instantiate a robot object
|
||||
robot = std::make_shared<ScoutBase>();
|
||||
ScoutROSMessenger messenger(robot.get(), &node);
|
||||
@@ -46,7 +51,7 @@ int main(int argc, char **argv) {
|
||||
|
||||
// publish robot state at 50Hz while listening to twist commands
|
||||
ros::Rate rate(50);
|
||||
while (true) {
|
||||
while (keep_run) {
|
||||
if (!messenger.simulated_robot_) {
|
||||
messenger.PublishStateToROS();
|
||||
} else {
|
||||
|
||||
Reference in New Issue
Block a user