ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange
Ask Your Question

Unable to produce accurate length of path from NavFN or DWAPlanner

asked 2023-05-15 08:35:08 -0500

MartinBraas gravatar image


I have set up 5 individual simulated turtlebot3 robots, which are navigating the same environment Map in gazebo, running the navigation stack. In order to make a planning algorithm to assign which robot should go to which (simulated task), i want to know the length between each robot and all the tasks, so i can assign the closes robot.

In order to accomplish this i tried subscribeing to both tf_prefix/move_base/NavfnROS/plan and tf_prefix/move_base/DWAPlannerROS/global_plan.

Then use callback to calculate the length of the initial path. This is do by:

def path_callback(data): global total_distance prev_pose = None

for pose in data.poses:
    if prev_pose is not None:
        dx = pose.pose.position.x - prev_pose.pose.position.x
        dy = pose.pose.position.y - prev_pose.pose.position.y
        dist = (dx**2 + dy**2)**0.5
        total_distance += dist
    prev_pose = pose

and then:

def dist_calc(robot): global total_distance sub = rospy.Subscriber(robot +'/move_base/NavfnROS/plan', Path, path_callback)

rate = rospy.Rate(10) # 10 Hz
while not rospy.is_shutdown():
    if total_distance > 0:
        print("Total distance: %.2f meters" % total_distance)
        return total_distance

But i somehow get very varying and incorrect results. For example always calculating 1.55 meters, when there clearly is 4 meters. If i record the topics with rosbag, and convert it to a csv file, i can see that there is "multiple paths" listed in the csv file. I thought if i could differentiate these paths, i could pick a single path, for example path 0, and use that solely for the calculations. Does anyone know if it is possible to index the "Path" which i call my callback function with, or do any of you have other suggestions for how i might accomplish this?

edit retag flag offensive close merge delete

1 Answer

Sort by » oldest newest most voted

answered 2023-05-16 06:46:05 -0500

Mike Scheutzow gravatar image

updated 2023-05-16 07:02:43 -0500

There are a number of problems with your current implementation:

  1. Your code has a race condition, as the while loop may check the value of total_distance while it is still being calculated.
  2. You don't want to use the DWAPlanner global_plan for this; that one is related to the geometric intersection of the local_costmap with the navfn global plan (so the plan on this topic will always have a short length.)
  3. If you have move_base "retry" enabled, the Navfn global planner will be run again if DWAPlanner can't make progress, so this can also result in a short navfn plan if it occurs close to the goal you specified.

I suggest you enable the global plan visualization in rviz. That should make it very clear how navfn is behaving.

i can see that there is "multiple paths" listed in the csv file.

This should not happen if DWAPlanner had no failures (see #3 above) and you gave it only a single goal.

edit flag offensive delete link more


Thank you for your response. Your mentioning of something was wrong with the while loop got me looking into it. The solution in the end, was switching from rospy.subscriber to rospy.wait_for_message to know when exactly the paths got created, as prompting for multiple paths would mess up the calculations

Thank you for your help!

MartinBraas gravatar image MartinBraas  ( 2023-05-19 07:34:42 -0500 )edit

Question Tools


Asked: 2023-05-15 08:35:08 -0500

Seen: 25 times

Last updated: May 16