ROS计算导航路径路程（导航前计算距离）python

在网络上查找的代码都是些要么用A*算法实现路径规划（时间太久不说【长达18分钟】， 还需要将导航过程中的点与点之间的距离相加这又不知道浪费多少时间～），要么必须导航到目标点才能知道路程多大。

然而，目的地的远近通常也是机器人是否考虑过去的一个重要指标，比如还有另一个更近的机器人，为何不让那个更近的去呢？为此，我们试图解决在未导航前就知道距离目的地有多远。

当然，此处的路径长度当然不是简单的欧式距离，是根据地图避障以后的路程大小哦～

主函数里只需要修改成你的目的地地点坐标就可以了啦！

这个问题困扰了我两个星期且网上都是理论知识实属太难了，希望有困扰的小伙伴能免受这种痛苦～

#!/usr/bin/env python
import math
import roslib, rospy
from nav_msgs.msg import Path
from geometry_msgs.msg import PoseWithCovarianceStamped
from geometry_msgs.msg import PoseStamped
from move_base_msgs.msg import MoveBaseActionGoal
from actionlib_msgs.msg import GoalID


subscriber = None


def calculate_path_distance(goal_position_x, goal_position_y, goal_orientation_z, goal_orientation_w):
    global subscriber
    topic = '/move_base/GlobalPlanner/plan'
    rospy.init_node('calculate_path_distance', anonymous=True)
    pub_goal = rospy.Publisher('move_base_simple/goal', PoseStamped, queue_size=10)
    #  geometry_msgs/PoseWithCovarianceStamped
    pub_start = rospy.wait_for_message('/amcl_pose', PoseWithCovarianceStamped, timeout=None)
    subscriber = rospy.Subscriber(topic, Path, printPath)


    start_point = PoseWithCovarianceStamped()
    # start point position x
    start_point.pose.pose.position.x = pub_start.pose.pose.position.x
    # start point position y
    start_point.pose.pose.position.y = pub_start.pose.pose.position.y
    start_point.header.stamp = rospy.Time.now()
    start_point.pose.pose.orientation.z = pub_start.pose.pose.orientation.z
    start_point.pose.pose.orientation.w = pub_start.pose.pose.orientation.w
    start_point.header.frame_id = 'map'
    rospy.sleep(1)
    # pub_start.publish(start_point)
    # print("Start Point:")
    # print(start_point)
    # print("--------------------")
    goal_point = PoseStamped()
    # goal point position x
    goal_point.pose.position.x = goal_position_x
    # goal point Y
    goal_point.pose.position.y = goal_position_y
    goal_point.header.stamp = rospy.Time.now()
    goal_point.pose.orientation.z = goal_orientation_z
    goal_point.pose.orientation.w = goal_orientation_w
    goal_point.header.frame_id = 'map'
    rospy.sleep(2)
    pub_goal.publish(goal_point)
    # print("Goal Point:")
    # print(goal_point)
    # print("--------------------")

    # print("Listening to " + topic)
    rospy.spin()


def printPath(path):
    global subscriber
    first_time = True
    prev_x = 0.0
    prev_y = 0.0
    total_distance = 0.0
    if len(path.poses) > 0:
        # pub_stop = rospy.Publisher('move_base/cancel', GoalID, queue_size=10)
        rospy.sleep(1)
        # pub_stop.publish(GoalID())
    for current_point in path.poses:
        x = current_point.pose.position.x
        y = current_point.pose.position.y
        if not first_time:
            total_distance += math.hypot(prev_x - x, prev_y - y)
        else:
            first_time = False
        prev_x = x
        prev_y = y
    subscriber.unregister()
    print("Total Distance = " + str(total_distance) + " meters")
    # print("Press Ctrl+C to exit.")


if __name__ == '__main__':
    calculate_path_distance(-9.12162303925, 6.68007469177,  0.343458574332, 0.939167827237)