移动机器人点对点判断是否到位代码

以下是一个简单的移动机器人点对点判断是否到位的示例代码，你可以根据你所使用的机器人和控制系统进行适当的修改：

import rospy
from geometry_msgs.msg import PoseStamped, Twist

class PointToPointController:
    def __init__(self):
        self.target_pose = None
        self.current_pose = None
        self.vel_publisher = rospy.Publisher('/cmd_vel', Twist, queue_size=1)
        rospy.Subscriber('/target_pose', PoseStamped, self.target_pose_callback)
        rospy.Subscriber('/current_pose', PoseStamped, self.current_pose_callback)

    def target_pose_callback(self, msg):
        self.target_pose = msg

    def current_pose_callback(self, msg):
        self.current_pose = msg

    def run(self):
        rate = rospy.Rate(10)
        while not rospy.is_shutdown():
            if self.target_pose is not None and self.current_pose is not None:
                # Calculate the distance between the target and current pose
                dx = self.target_pose.pose.position.x - self.current_pose.pose.position.x
                dy = self.target_pose.pose.position.y - self.current_pose.pose.position.y
                dz = self.target_pose.pose.position.z - self.current_pose.pose.position.z
                distance = (dx * dx + dy * dy + dz * dz) ** 0.5

                # If the distance is less than a threshold, stop the robot
                if distance < 0.1:
                    self.vel_publisher.publish(Twist())
                else:
                    # Calculate the direction to move towards the target
                    direction = Twist()
                    direction.linear.x = dx / distance
                    direction.linear.y = dy / distance
                    direction.linear.z = dz / distance

                    # Publish the velocity command
                    self.vel_publisher.publish(direction)

            rate.sleep()

if __name__ == '__main__':
    rospy.init_node('point_to_point_controller')
    controller = PointToPointController()
    controller.run()

这个示例代码订阅了 `/target_pose` 和 `/current_pose` 两个话题，分别用于获取目标位姿和当前位姿。在每个循环中，它会计算当前位姿与目标位姿之间的距离，如果距离小于一个阈值，则停止机器人运动，否则计算机器人移动的方向，并发布速度命令。你可以根据你的实际需求修改这个代码，并且你需要确保机器人的传感器和运动控制系统正常工作。