diff --git a/move_base/CMakeLists.txt b/move_base/CMakeLists.txt index 6be107a148..6a518771f7 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..3adf62bb50 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..4673d237ca 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); + + } + };