From 7c11cb16ef780fd1cdadf1d35b9a85cac441bfa8 Mon Sep 17 00:00:00 2001 From: Omer Waxman Date: Tue, 18 May 2021 14:55:45 +0000 Subject: [PATCH 1/3] move_base: Publish message: path and time left to the goal. The message is published in context of feedback action. --- move_base/CMakeLists.txt | 16 ++++++++++++++++ move_base/include/move_base/move_base.h | 4 +++- move_base/msg/Path.msg | 4 ++++ move_base/src/move_base.cpp | 22 ++++++++++++++++++++++ 4 files changed, 45 insertions(+), 1 deletion(-) create mode 100644 move_base/msg/Path.msg diff --git a/move_base/CMakeLists.txt b/move_base/CMakeLists.txt index 6be107a148..fd633d471d 100644 --- a/move_base/CMakeLists.txt +++ b/move_base/CMakeLists.txt @@ -26,6 +26,21 @@ find_package(catkin REQUIRED find_package(Eigen3 REQUIRED) add_definitions(${EIGEN3_DEFINITIONS}) +############################################################################### +# MESSAGES, SERVICES AND ACTIONS +############################################################################### + +add_message_files( + DIRECTORY msg + FILES + Path.msg +) + +generate_messages( + DEPENDENCIES + geometry_msgs +) + # dynamic reconfigure generate_dynamic_reconfigure_options( cfg/MoveBase.cfg @@ -38,6 +53,7 @@ catkin_package( move_base_msgs nav_msgs roscpp + message_runtime ) include_directories( diff --git a/move_base/include/move_base/move_base.h b/move_base/include/move_base/move_base.h index f1bedc2d73..65ac0c3561 100644 --- a/move_base/include/move_base/move_base.h +++ b/move_base/include/move_base/move_base.h @@ -169,6 +169,8 @@ namespace move_base { double distance(const geometry_msgs::PoseStamped& p1, const geometry_msgs::PoseStamped& p2); geometry_msgs::PoseStamped goalToGlobalFrame(const geometry_msgs::PoseStamped& goal_pose_msg); + + void publishPathMsg(const geometry_msgs::PoseStamped& current_position, const geometry_msgs::PoseStamped& goal); /** * @brief This is used to wake the planner at periodic intervals. @@ -195,7 +197,7 @@ namespace move_base { int32_t max_planning_retries_; uint32_t planning_retries_; double conservative_reset_dist_, clearing_radius_; - ros::Publisher current_goal_pub_, vel_pub_, action_goal_pub_, recovery_status_pub_; + ros::Publisher current_goal_pub_, vel_pub_, action_goal_pub_, recovery_status_pub_, move_base_simple_pub_; ros::Subscriber goal_sub_; ros::ServiceServer make_plan_srv_, clear_costmaps_srv_; bool shutdown_costmaps_, clearing_rotation_allowed_, recovery_behavior_enabled_; diff --git a/move_base/msg/Path.msg b/move_base/msg/Path.msg new file mode 100644 index 0000000000..406efc8528 --- /dev/null +++ b/move_base/msg/Path.msg @@ -0,0 +1,4 @@ +geometry_msgs/PoseStamped current_pose # Current position +geometry_msgs/PoseStamped goal_pose # Goal +float32 dist_to_goal # Path distance left to goal (m) +float32 time_to_goal # Time left to goal to navigate along path (s) diff --git a/move_base/src/move_base.cpp b/move_base/src/move_base.cpp index fdd5c92e4d..6dbba02a02 100644 --- a/move_base/src/move_base.cpp +++ b/move_base/src/move_base.cpp @@ -45,6 +45,7 @@ #include #include +#include namespace move_base { @@ -96,6 +97,9 @@ namespace move_base { vel_pub_ = nh.advertise("cmd_vel", 1); current_goal_pub_ = private_nh.advertise("current_goal", 0 ); + //to publish path length and time left to goal + move_base_simple_pub_ = private_nh.advertise("move_base_simple/feedback", 1); + ros::NodeHandle action_nh("move_base"); action_goal_pub_ = action_nh.advertise("goal", 1); recovery_status_pub_= action_nh.advertise("recovery_status", 1); @@ -815,6 +819,9 @@ namespace move_base { feedback.base_position = current_position; as_->publishFeedback(feedback); + //publish path left and time left to the goal + publishPathMsg(current_position, goal); + //check to see if we've moved far enough to reset our oscillation timeout if(distance(current_position, oscillation_pose_) >= oscillation_distance_) { @@ -1200,4 +1207,19 @@ namespace move_base { return true; } + + // publish in context of action feedback, path left and time left to the goal + void MoveBase::publishPathMsg(const geometry_msgs::PoseStamped& current_position, const geometry_msgs::PoseStamped& goal) + { + move_base::Path msg; + msg.current_pose = current_position; + msg.goal_pose = goal; + msg.dist_to_goal = distance(current_position, goal); + geometry_msgs::Twist cmd_vel; + tc_->computeVelocityCommands(cmd_vel); + msg.time_to_goal = msg.dist_to_goal/(cmd_vel.linear.x ? cmd_vel.linear.x : 1.0); + move_base_simple_pub_.publish(msg); + + } + }; From 91f1f4e11a353ec322618ec9ea4d44a348c01d72 Mon Sep 17 00:00:00 2001 From: Omer Waxman Date: Tue, 18 May 2021 15:03:49 +0000 Subject: [PATCH 2/3] move_base: Fix tabulation --- move_base/include/move_base/move_base.h | 2 +- move_base/src/move_base.cpp | 12 ++++++------ 2 files changed, 7 insertions(+), 7 deletions(-) diff --git a/move_base/include/move_base/move_base.h b/move_base/include/move_base/move_base.h index 65ac0c3561..3adf62bb50 100644 --- a/move_base/include/move_base/move_base.h +++ b/move_base/include/move_base/move_base.h @@ -170,7 +170,7 @@ namespace move_base { geometry_msgs::PoseStamped goalToGlobalFrame(const geometry_msgs::PoseStamped& goal_pose_msg); - void publishPathMsg(const geometry_msgs::PoseStamped& current_position, const geometry_msgs::PoseStamped& goal); + void publishPathMsg(const geometry_msgs::PoseStamped& current_position, const geometry_msgs::PoseStamped& goal); /** * @brief This is used to wake the planner at periodic intervals. diff --git a/move_base/src/move_base.cpp b/move_base/src/move_base.cpp index 6dbba02a02..4673d237ca 100644 --- a/move_base/src/move_base.cpp +++ b/move_base/src/move_base.cpp @@ -97,8 +97,8 @@ namespace move_base { vel_pub_ = nh.advertise("cmd_vel", 1); current_goal_pub_ = private_nh.advertise("current_goal", 0 ); - //to publish path length and time left to goal - move_base_simple_pub_ = private_nh.advertise("move_base_simple/feedback", 1); + //to publish path length and time left to goal + move_base_simple_pub_ = private_nh.advertise("move_base_simple/feedback", 1); ros::NodeHandle action_nh("move_base"); action_goal_pub_ = action_nh.advertise("goal", 1); @@ -819,8 +819,8 @@ namespace move_base { feedback.base_position = current_position; as_->publishFeedback(feedback); - //publish path left and time left to the goal - publishPathMsg(current_position, goal); + //publish path left and time left to the goal + publishPathMsg(current_position, goal); //check to see if we've moved far enough to reset our oscillation timeout if(distance(current_position, oscillation_pose_) >= oscillation_distance_) @@ -1219,7 +1219,7 @@ namespace move_base { tc_->computeVelocityCommands(cmd_vel); msg.time_to_goal = msg.dist_to_goal/(cmd_vel.linear.x ? cmd_vel.linear.x : 1.0); move_base_simple_pub_.publish(msg); - + } - + }; From d1b3e1c0aaf8b94cb072aac14f2b75d5b0e64de2 Mon Sep 17 00:00:00 2001 From: Michael Prujan <49917340+michaelblindnology@users.noreply.github.com> Date: Wed, 19 May 2021 08:10:33 +0300 Subject: [PATCH 3/3] move_base: Fix Tabulation --- move_base/CMakeLists.txt | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/move_base/CMakeLists.txt b/move_base/CMakeLists.txt index fd633d471d..6a518771f7 100644 --- a/move_base/CMakeLists.txt +++ b/move_base/CMakeLists.txt @@ -31,14 +31,14 @@ add_definitions(${EIGEN3_DEFINITIONS}) ############################################################################### add_message_files( - DIRECTORY msg - FILES - Path.msg + DIRECTORY msg + FILES + Path.msg ) generate_messages( - DEPENDENCIES - geometry_msgs + DEPENDENCIES + geometry_msgs ) # dynamic reconfigure @@ -53,7 +53,7 @@ catkin_package( move_base_msgs nav_msgs roscpp - message_runtime + message_runtime ) include_directories(