Python 实现小乌龟的目标点运动(ROS)

小乌龟目标点运动代码如6.9所示。
Code 6.9: 小乌龟目标点运动（turtlesim_goal_motion.py）

#!/usr/bin/env python

import time
import rospy
import math
import sys

from geometry_msgs .msg import Twist
from turtlesim .msg import Pose

class TurtleSimMotion :
    def __init__(self):
        self.x, self.y, self.z, self.yaw = 0, 0, 0, 0

        self.velocity_publisher = rospy. Publisher ('/turtle1/cmd_vel',
            Twist , queue_size =10)

    def pose_callback (self , pose_message ):
        self.x = pose_message .x
        self.y = pose_message .y
        self.yaw = pose_message .theta

        rospy.loginfo('Pose Callback x = %f y = %f yaw = %f',
            self.x, self.y, self.yaw)

    def go_to_goal (self , x_goal , y_goal):
        velocity_message = Twist ()

        while True:
            # PID控 制 中 比 例 控 制 系 数
            K_linear = 0.5
            # 计 算 从 当 前 位 置 到 目 标 位 置 之 间 的 距 离
            distance = abs(math.sqrt(
                (x_goal - self.x) ** 2 + (y_goal - self.y) ** 2))
            # 通 过 比 例 系 数 调 整 线 速 度

            linear_speed = distance * K_linear

            # 与 线 速 度 中 的 比 例 控 制 系 统 作 用 相 同
            K_angular = 4.0
            desired_angle_goal = math.atan2(y_goal - self.y, x_goal - self.x)
            angular_speed = ( desired_angle_goal - self.yaw) * K_angular

            velocity_message.linear.x = linear_speed
            velocity_message.angular.z = angular_speed

        # 修 改 小 乌 龟 的 速 度
            self.velocity_publisher .publish( velocity_message )

        # 当 前 起 始 位 置 到 目 标 位 置 的 距 离 小 于 阈 值 是 表 示 运 行 到 目 标 位 置
            if distance < 0.01:
                break

if __name__ == '__main__':
    if len(sys.argv) == 3:
        x_goal = int(sys.argv [1])
        y_goal = int(sys.argv [2])
    else:
        print("%s [x_goal y_goal]" % sys.argv [0])
        sys.exit (1)

    rospy. init_node ('turtlesim_goal_motion', anonymous =True)

    turtle_sim_motion = TurtleSimMotion()
    pose_subscriber = rospy. Subscriber('/turtle1/pose', Pose ,
    turtle_sim_motion.pose_callback )
    
    time.sleep(2)

    turtle_sim_motion.go_to_goal (x_goal=x_goal , y_goal=y_goal)

第 31 行：与 PID 控制中的比例控制系数相同，用于动态控制小乌龟的速度。
第 33~34 行：计算当前位置到目标位置之间距离，该距离是动态控制小乌龟速度的依据。
第 50~51 行：当当前位置和目标位置之间的距离小于某个阈值时，则表明当前小乌龟已经到达
该目标位置。
执行如6.10所示的指令就可以实现小乌龟运动到目标点。

Code 6.10: 小乌龟目标点运动指令

roscore

 # 启 动 小 乌 龟
 rosrun turtlesim turtlesim_node

 # 使 小 乌 龟 运 动 到[10, 10]的 目 标 点
 rosrun hello_tutorials turtlesim_goal_motion.py 10 10