Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Published a new message that contains: #1119

Open
wants to merge 3 commits into
base: melodic-devel
Choose a base branch
from
Open

Published a new message that contains: #1119

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
7c11cb1
move_base: Publish message: path and time left
omerwaxman May 18, 2021
91f1f4e
move_base: Fix tabulation
omerwaxman May 18, 2021
d1b3e1c
move_base: Fix Tabulation
michaelblindnology May 19, 2021
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Jump to
Jump to file
Failed to load files.
Diff view
Diff view
16 changes: 16 additions & 0 deletions move_base/CMakeLists.txt
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand All @@ -38,6 +53,7 @@ catkin_package(
move_base_msgs
nav_msgs
roscpp
message_runtime
)

include_directories(
Expand Down
4 changes: 3 additions & 1 deletion move_base/include/move_base/move_base.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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.
Expand All @@ -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_;
Expand Down
4 changes: 4 additions & 0 deletions move_base/msg/Path.msg
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -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)
22 changes: 22 additions & 0 deletions move_base/src/move_base.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -45,6 +45,7 @@
#include <geometry_msgs/Twist.h>

#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
#include <move_base/Path.h>
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

we should not be adding messages to move_base - there is a move_base_msgs package in a separate repo specifically so that users of nav do not have to build all of nav on remote machines that are monitoring outputs.

There is also a standard path message in nav_msgs, why not use that?

Copy link
Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Hi,
I used my msg instead of nav_msg/Path, since I need

float32 dist_to_goal # Path distance left to goal (m)
float32 time_to_goal # Time left to goal to navigate along path (s)


namespace move_base {

Expand Down Expand Up @@ -96,6 +97,9 @@ namespace move_base {
vel_pub_ = nh.advertise<geometry_msgs::Twist>("cmd_vel", 1);
current_goal_pub_ = private_nh.advertise<geometry_msgs::PoseStamped>("current_goal", 0 );

//to publish path length and time left to goal
move_base_simple_pub_ = private_nh.advertise<move_base::Path>("move_base_simple/feedback", 1);

ros::NodeHandle action_nh("move_base");
action_goal_pub_ = action_nh.advertise<move_base_msgs::MoveBaseActionGoal>("goal", 1);
recovery_status_pub_= action_nh.advertise<move_base_msgs::RecoveryStatus>("recovery_status", 1);
Expand Down Expand Up @@ -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_)
{
Expand Down Expand Up @@ -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);

}

};