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

小芳矶

于 2023-02-28 14:41:23 发布

阅读量1.3k

点赞数 9

分类专栏： ROS 文章标签： python 人工智能开发语言

本文链接：https://blog.csdn.net/weixin_44734413/article/details/129260894

版权

ROS 专栏收录该内容

1 篇文章

订阅专栏

文章介绍了一种在ROS环境中，使用Python预计算机器人从当前位置到目标位置的导航路径距离的方法，避免了A*算法的长时间计算和必须实际导航才能获取距离的问题。通过发布目标位置并监听路径规划结果，计算避障后的实际路程，适用于需要快速评估多个目标距离的场景。

摘要生成于 C知道，由 DeepSeek-R1 满血版支持，前往体验 >

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)