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

Slow control loop with dwa_local_planner (melodic) #956

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

Slow control loop with dwa_local_planner (melodic) #956

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
484b326
LocalPlannerUtil keeps time for TF transforms.
pavloblindnology Mar 16, 2018
cdbbfad
goal_functions: Replace redundant advanced TF transforms' calls.
pavloblindnology Mar 16, 2018
9fa7b9e
dwa_local_planner: Fix multiple waitForTransform delays.
pavloblindnology Mar 16, 2018
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
3 changes: 3 additions & 0 deletions base_local_planner/include/base_local_planner/goal_functions.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -102,6 +102,7 @@ namespace base_local_planner {
const geometry_msgs::PoseStamped& global_robot_pose,
const costmap_2d::Costmap2D& costmap,
const std::string& global_frame,
const ros::Time& time,
Copy link
Contributor

Choose a reason for hiding this comment

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

I've spent quite a bit of time looking at this now - I really don't like littering extra params all over, and having all this book-keeping going on as to what time we are using for things (it seems like this will be very easy to use incorrectly).

std::vector<geometry_msgs::PoseStamped>& transformed_plan);

/**
Expand All @@ -115,6 +116,7 @@ namespace base_local_planner {
bool getGoalPose(const tf2_ros::Buffer& tf,
const std::vector<geometry_msgs::PoseStamped>& global_plan,
const std::string& global_frame,
const ros::Time& time,
geometry_msgs::PoseStamped &goal_pose);

/**
Expand All @@ -134,6 +136,7 @@ namespace base_local_planner {
const std::vector<geometry_msgs::PoseStamped>& global_plan,
const costmap_2d::Costmap2D& costmap,
const std::string& global_frame,
const ros::Time& time,
geometry_msgs::PoseStamped& global_pose,
const nav_msgs::Odometry& base_odom,
double rot_stopped_vel, double trans_stopped_vel,
Expand Down
9 changes: 8 additions & 1 deletion base_local_planner/include/base_local_planner/local_planner_util.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -64,6 +64,9 @@ class LocalPlannerUtil {
costmap_2d::Costmap2D* costmap_;
tf2_ros::Buffer* tf_;

ros::Time time_; // The time used for TF transformations
bool time_set_; // Whether time was set


std::vector<geometry_msgs::PoseStamped> global_plan_;

Expand All @@ -74,14 +77,16 @@ class LocalPlannerUtil {
LocalPlannerLimits limits_;
bool initialized_;

ros::Time getTime();

public:

/**
* @brief Callback to update the local planner's parameters
*/
void reconfigureCB(LocalPlannerLimits &config, bool restore_defaults);

LocalPlannerUtil() : initialized_(false) {}
LocalPlannerUtil() : initialized_(false), time_set_(false) {}

~LocalPlannerUtil() {
}
Expand All @@ -90,6 +95,8 @@ class LocalPlannerUtil {
costmap_2d::Costmap2D* costmap,
std::string global_frame);

bool setTime(ros::Time time) { time_ = time; time_set_ = true; }

bool getGoal(geometry_msgs::PoseStamped& goal_pose);

bool setPlan(const std::vector<geometry_msgs::PoseStamped>& orig_global_plan);
Expand Down
13 changes: 6 additions & 7 deletions base_local_planner/src/goal_functions.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -99,6 +99,7 @@ namespace base_local_planner {
const geometry_msgs::PoseStamped& global_pose,
const costmap_2d::Costmap2D& costmap,
const std::string& global_frame,
const ros::Time& time,
std::vector<geometry_msgs::PoseStamped>& transformed_plan){
transformed_plan.clear();

Expand All @@ -110,8 +111,7 @@ namespace base_local_planner {
const geometry_msgs::PoseStamped& plan_pose = global_plan[0];
try {
// get plan_to_global_transform from plan frame to global_frame
geometry_msgs::TransformStamped plan_to_global_transform = tf.lookupTransform(global_frame, ros::Time(),
plan_pose.header.frame_id, plan_pose.header.stamp, plan_pose.header.frame_id, ros::Duration(0.5));
geometry_msgs::TransformStamped plan_to_global_transform = tf.lookupTransform(global_frame, plan_pose.header.frame_id, time, ros::Duration(0.5));
Copy link
Contributor

Choose a reason for hiding this comment

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

here, it seems the correct thing to use is not a passed in time, but rather the timestamp of global_pose (which will usually have just been determined by a call to getRobotPose().

Copy link
Contributor Author

@pavloblindnology pavloblindnology May 22, 2020 •
edited
Loading

Choose a reason for hiding this comment

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

Agree, it'll be more consistent, though global_pose's timestamp can be quite in the past (tens of ms).


//let's get the pose of the robot in the frame of the plan
geometry_msgs::PoseStamped robot_pose;
Expand Down Expand Up @@ -173,7 +173,7 @@ namespace base_local_planner {

bool getGoalPose(const tf2_ros::Buffer& tf,
const std::vector<geometry_msgs::PoseStamped>& global_plan,
const std::string& global_frame, geometry_msgs::PoseStamped &goal_pose) {
const std::string& global_frame, const ros::Time& time, geometry_msgs::PoseStamped &goal_pose) {
if (global_plan.empty())
{
ROS_ERROR("Received plan with zero length");
Expand All @@ -182,9 +182,7 @@ namespace base_local_planner {

const geometry_msgs::PoseStamped& plan_goal_pose = global_plan.back();
try{
geometry_msgs::TransformStamped transform = tf.lookupTransform(global_frame, ros::Time(),
plan_goal_pose.header.frame_id, plan_goal_pose.header.stamp,
plan_goal_pose.header.frame_id, ros::Duration(0.5));
geometry_msgs::TransformStamped transform = tf.lookupTransform(global_frame, plan_goal_pose.header.frame_id, time, ros::Duration(0.5));
Copy link
Contributor

Choose a reason for hiding this comment

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

why is ros::Time() a problem here? that should simply give us the latest transform - we don't even appear to be ever using the timestamp of the goal_pose returned, so?

Copy link
Contributor Author

@pavloblindnology pavloblindnology May 22, 2020 •
edited
Loading

Choose a reason for hiding this comment

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

We don't use the timestamp of returned goal_pose, but the timestamp is used to define transformation itself. And it's better to be consistent with the rest of computations. That is why I still propose to pass timestamp here and pass robot's pose timestamp (in LatchedStopRotateController).
And we can define this timestamp argument's default value to be ros::Time() in order not to break other local planners.


tf2::doTransform(plan_goal_pose, goal_pose, transform);
}
Expand All @@ -210,14 +208,15 @@ namespace base_local_planner {
const std::vector<geometry_msgs::PoseStamped>& global_plan,
const costmap_2d::Costmap2D& costmap GOAL_ATTRIBUTE_UNUSED,
const std::string& global_frame,
const ros::Time& time,
geometry_msgs::PoseStamped& global_pose,
const nav_msgs::Odometry& base_odom,
double rot_stopped_vel, double trans_stopped_vel,
double xy_goal_tolerance, double yaw_goal_tolerance){

//we assume the global goal is the last point in the global plan
geometry_msgs::PoseStamped goal_pose;
getGoalPose(tf, global_plan, global_frame, goal_pose);
getGoalPose(tf, global_plan, global_frame, time, goal_pose);

double goal_x = goal_pose.pose.position.x;
double goal_y = goal_pose.pose.position.y;
Expand Down
11 changes: 11 additions & 0 deletions base_local_planner/src/local_planner_util.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -79,12 +79,22 @@ LocalPlannerLimits LocalPlannerUtil::getCurrentLimits() {
return limits_;
}

inline ros::Time LocalPlannerUtil::getTime() {
//use current time if not set
ros::Time time;
if (time_set_)
time = time_;
else
time = ros::Time::now();
return time;
}

bool LocalPlannerUtil::getGoal(geometry_msgs::PoseStamped& goal_pose) {
//we assume the global goal is the last point in the global plan
return base_local_planner::getGoalPose(*tf_,
global_plan_,
global_frame_,
getTime(),
goal_pose);
}

Expand All @@ -110,6 +120,7 @@ bool LocalPlannerUtil::getLocalPlan(const geometry_msgs::PoseStamped& global_pos
global_pose,
*costmap_,
global_frame_,
getTime(),
transformed_plan)) {
ROS_WARN("Could not transform the global plan to the frame of the controller");
return false;
Expand Down
2 changes: 1 addition & 1 deletion base_local_planner/src/trajectory_planner_ros.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -406,7 +406,7 @@ namespace base_local_planner {

std::vector<geometry_msgs::PoseStamped> transformed_plan;
//get the global plan in our frame
if (!transformGlobalPlan(*tf_, global_plan_, global_pose, *costmap_, global_frame_, transformed_plan)) {
if (!transformGlobalPlan(*tf_, global_plan_, global_pose, *costmap_, global_frame_, ros::Time::now(), transformed_plan)) {
Copy link
Contributor

Choose a reason for hiding this comment

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

and here, now() is later than the stamp we just got on the global_pose, so again, it would seem that approach would be reasonable.

ROS_WARN("Could not transform the global plan to the frame of the controller");
return false;
}
Expand Down
13 changes: 9 additions & 4 deletions dwa_local_planner/src/dwa_planner_ros.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -161,6 +161,15 @@ namespace dwa_local_planner {
return false;
}

// Set current time that will be used by LocalPlannerUtil for TF extraction
// during current move_base control loop cycle.
// If not set, LocalPlannerUtil will wait for TF transform each time
// getGoal or getLocalPlan is called incurring multiple delays.
// Current robot pose must be obtained prior to this (and not updated later)
// to avoid possible TF extrapolation error,
// as getLocalPlan will also try to transform at its time stamp.
planner_util_.setTime(ros::Time::now());

if(latchedStopRotateController_.isGoalReached(&planner_util_, odom_helper_, current_pose_)) {
ROS_INFO("Goal reached");
return true;
Expand Down Expand Up @@ -263,10 +272,6 @@ namespace dwa_local_planner {

bool DWAPlannerROS::computeVelocityCommands(geometry_msgs::Twist& cmd_vel) {
// dispatches to either dwa sampling control or stop and rotate control, depending on whether we have been close enough to goal
if ( ! costmap_ros_->getRobotPose(current_pose_)) {
ROS_ERROR("Could not get robot pose");
return false;
}
Copy link
Contributor

Choose a reason for hiding this comment

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

while we currently call isGoalReached() right before computeVelocityCommands(), I really don't think we want to make this API harder to use -- using the timestamp of the current_pose_ rather than an extra timestamp allows this to stay and avoids extra bookkeeping.

Copy link
Contributor Author

Choose a reason for hiding this comment

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

Agree.

std::vector<geometry_msgs::PoseStamped> transformed_plan;
if ( ! planner_util_.getLocalPlan(current_pose_, transformed_plan)) {
ROS_ERROR("Could not get local plan");
Expand Down