From 4e6d400ca86c13a037aec876ba6dde12225e6f31 Mon Sep 17 00:00:00 2001 From: "pinloon.lee" Date: Tue, 18 May 2021 17:54:20 +0800 Subject: [PATCH 1/2] fixed odom frame name --- scout_base/src/scout_messenger.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/scout_base/src/scout_messenger.cpp b/scout_base/src/scout_messenger.cpp index 23fb1a6..47011e0 100644 --- a/scout_base/src/scout_messenger.cpp +++ b/scout_base/src/scout_messenger.cpp @@ -276,7 +276,7 @@ namespace westonrobot // publish odometry and tf messages nav_msgs::Odometry odom_msg; odom_msg.header.stamp = current_time_; - odom_msg.header.frame_id = "testes"; + odom_msg.header.frame_id = odom_frame_; odom_msg.child_frame_id = base_frame_; odom_msg.pose.pose.position.x = position_x_; From 53c687f9ff216aa2f4863ec4158402541c95706c Mon Sep 17 00:00:00 2001 From: "pinloon.lee" Date: Tue, 18 May 2021 17:55:40 +0800 Subject: [PATCH 2/2] added signal to close node properly --- scout_base/src/scout_base_node.cpp | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/scout_base/src/scout_base_node.cpp b/scout_base/src/scout_base_node.cpp index 8567f16..7827ff7 100644 --- a/scout_base/src/scout_base_node.cpp +++ b/scout_base/src/scout_base_node.cpp @@ -11,12 +11,17 @@ using namespace westonrobot; std::shared_ptr 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(); 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 {