Obtaining path length of path from global planner
I am currently working on a paper investigating the combination of global and local path planning techniques. For this, i would like to be able to calculate or in some way recieve the length of a plan made from the global_costmap
from the ROS navigation stack.
Currently, i am subscribing to the move_base/DWAPlannerROS/global_plan
and retrieving data in the form of the Path
class. However, when calculating the length of the path using euclidian distance calculations such as here, i get results that are not compliant with the dimensions of my simulation environment, i.e. the calculated distance does not match up with the distance one would assume the robot would have to move (moving from coordinates (0,0,0)
to (0,3,0)
while keeping the original orientation of the robot is supposedly equivalent to 1,55
meters). Also, it seems that the topic reference in this aforementioned post is not availiable in my environment/distro.
While something could potentially be wrong with the calculations, i dont suspect this to be the case. Rather, i think it has something to do with the data i recieve from the move_base/DWAPlannerROS/global_plan
topic.
Currently, i am subscribing with the following call:
rospy.subscriber(robot + '/move_base/DWAPlannnerROS/global_plan', Path, path_callback)
The path_callback
function takes a parameter data
and looks like this:
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
However, when inspecting the .bag
file containing the data extravcted from calling this, it seems to log multiple instances of the 'pose' data in the form
field.poses0.pose.position.x
field.pose1.pose.position.x
field.pose2.pose.position.x
And so on.
My question is this: How can i go about obtaining the distance of the path calculated by the global planner before traversing it? Is there another topic better suited for obtaining data to calculate this?
I am working in ROS Noetic in Ubuntu 20.04