Slow control loop with dwa_local_planner (melodic) #956

pavloblindnology · 2020-01-10T13:17:46Z

Fix for #679. Replication of #688 for melodic.

pavloblindnology · 2020-02-28T09:25:37Z

Any updates on this?

mikeferguson

I've finally spent an appropriate amount of time looking this one (and the kinetic version) over -- I see the issues you're trying to fix, and very much appreciate your attempt to fix them - but I'm really not thrilled with the extra bookkeeping -- I'm convinced there has to be a better/simpler approach. Please let me know what you think of the proposed alternatives.

mikeferguson · 2020-03-03T17:48:59Z

base_local_planner/include/base_local_planner/goal_functions.h

@@ -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,


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).

mikeferguson · 2020-03-03T17:49:45Z

base_local_planner/src/goal_functions.cpp

@@ -105,8 +106,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));


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().

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

mikeferguson · 2020-03-03T17:55:30Z

base_local_planner/src/goal_functions.cpp

-      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));


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?

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.

mikeferguson · 2020-03-03T17:56:25Z

base_local_planner/src/trajectory_planner_ros.cpp

@@ -387,7 +387,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)) {


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.

mikeferguson · 2020-03-03T17:57:39Z

dwa_local_planner/src/dwa_planner_ros.cpp

-    if ( ! costmap_ros_->getRobotPose(current_pose_)) {
-      ROS_ERROR("Could not get robot pose");
-      return false;
-    }


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.

pavloblindnology · 2020-05-22T07:48:23Z

@mikeferguson The whole idea was to keep all transformations done during one local planner cycle consistent. I agree it can be a little bit confusing. I see your point and agree that this consistent time is better to be the latest robot pose's time, though it can be quite in the past (tens of ms). This way we know that we're using the same time when making computations based on both robot's pose and transformed global plan. The only inconsistency left is goal computations, which, if using ros::Time() instead of book-kept time are not tied to robot's pose timestamp. Though goal computations seem less important than transformed plan computations, we still can fix it by passing robot's pose timestamp to LocalPlannerUtil::getGoal and goal_functions::getGoalPose:

navigation/base_local_planner/src/local_planner_util.cpp

Line 83 in 9368db2

bool LocalPlannerUtil::getGoal(geometry_msgs::PoseStamped& goal_pose) {

and

navigation/base_local_planner/src/goal_functions.cpp

Line 174 in 9368db2

bool getGoalPose(const tf2_ros::Buffer& tf,

BTW, this will be consistent with TrajectoryPlannerROS, which has its transformed_plan and goal timestamps consistent.

pavloblindnology force-pushed the melodic-wft-fix branch from afc5d08 to f735843 Compare February 11, 2020 15:08

mikeferguson requested changes Mar 3, 2020

View reviewed changes

pavloblindnology force-pushed the melodic-wft-fix branch from f735843 to a567163 Compare May 25, 2020 09:44

DLu added melodic planners labels Jul 3, 2020

pavloblindnology force-pushed the melodic-wft-fix branch from a567163 to 65734c5 Compare August 5, 2020 09:25

pavloblindnology force-pushed the melodic-wft-fix branch from 65734c5 to ab12fe9 Compare November 12, 2020 17:27

pavloblindnology force-pushed the melodic-wft-fix branch from ab12fe9 to d4d30dd Compare December 8, 2020 17:46

pavloblindnology force-pushed the melodic-wft-fix branch from d4d30dd to d90ed9a Compare May 19, 2021 08:31

pavloblindnology added 3 commits May 28, 2021 19:10

LocalPlannerUtil keeps time for TF transforms.

484b326

goal_functions: Replace redundant advanced TF transforms' calls.

cdbbfad

dwa_local_planner: Fix multiple waitForTransform delays.

9fa7b9e

pavloblindnology force-pushed the melodic-wft-fix branch from d90ed9a to 9fa7b9e Compare May 28, 2021 16:10

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Slow control loop with dwa_local_planner (melodic) #956

Slow control loop with dwa_local_planner (melodic) #956

pavloblindnology commented Jan 10, 2020

pavloblindnology commented Feb 28, 2020

mikeferguson left a comment

mikeferguson Mar 3, 2020

mikeferguson Mar 3, 2020

pavloblindnology May 22, 2020 •

edited

mikeferguson Mar 3, 2020

pavloblindnology May 22, 2020 •

edited

mikeferguson Mar 3, 2020

mikeferguson Mar 3, 2020

pavloblindnology May 22, 2020

pavloblindnology commented May 22, 2020 •

edited

Slow control loop with dwa_local_planner (melodic) #956

Are you sure you want to change the base?

Slow control loop with dwa_local_planner (melodic) #956

Conversation

pavloblindnology commented Jan 10, 2020

pavloblindnology commented Feb 28, 2020

mikeferguson left a comment

Choose a reason for hiding this comment

mikeferguson Mar 3, 2020

Choose a reason for hiding this comment

mikeferguson Mar 3, 2020

Choose a reason for hiding this comment

pavloblindnology May 22, 2020 • edited

Choose a reason for hiding this comment

mikeferguson Mar 3, 2020

Choose a reason for hiding this comment

pavloblindnology May 22, 2020 • edited

Choose a reason for hiding this comment

mikeferguson Mar 3, 2020

Choose a reason for hiding this comment

mikeferguson Mar 3, 2020

Choose a reason for hiding this comment

pavloblindnology May 22, 2020

Choose a reason for hiding this comment

pavloblindnology commented May 22, 2020 • edited

pavloblindnology May 22, 2020 •

edited

pavloblindnology May 22, 2020 •

edited

pavloblindnology commented May 22, 2020 •

edited