Unable to produce accurate length of path from NavFN or DWAPlanner
Hello,
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:
sub.unregister()
print("Total distance: %.2f meters" % total_distance)
rate.sleep()
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?
