实例 3——Python 实现小乌龟的旋转运动(ROS)

该实例包含两个功能，一个是小乌龟旋转指定角度（rotate），另一个则是小乌龟旋转到指定角
度（set_desired_orientation）。小乌龟目标点运动代码如6.11所示。
Code 6.11: 小乌龟旋转运动（turtlesim_rotate_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
from distutils .util import strtobool

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

 # angular_speed_degree ： 旋 转 的 角 速 度
 # relative_angle_degree ： 相 对 于 当 前 位 置 旋 转 的 角 度
 # clockwise ： 顺 时 针 还 是 逆 时 针 旋 转
  def rotate(self , angular_speed_degree , relative_angle_degree , clockwise ):
 # 初 始 化 小 乌 龟 速 度 对 象
     velocity_message = Twist ()
     velocity_message .linear.x = 0
     velocity_message .linear.y = 0
     velocity_message .linear.z = 0
     velocity_message .angular.x = 0
     velocity_message .angular.y = 0
     velocity_message .angular.z = 0

 # 把 旋 转 角 速 度 转 化 为 弧 度
     angular_speed = math.radians(abs( angular_speed_degree ))

 # 设 置 小 乌 龟 角 速 度 的 值
     if clockwise :
        velocity_message .angular.z = -1 * abs( angular_speed )
     else:
        velocity_message .angular.z = abs( angular_speed )

     
    loop_rate = rospy.Rate (10)

    # 记 录 当 前 时 间
    t_0 = rospy.Time.now ().to_sec ()
    while True:
        rospy.loginfo('Turtlesim rotates')
    # 发 送 小 乌 龟 的 速 度 值
        self. velocity_publisher .publish( velocity_message )

        t_1 = rospy.Time.now ().to_sec ()
 # 计 算 已 经 转 过 的 角 度
        current_angle_degree = (t_1 - t_0) * angular_speed_degree
        loop_rate .sleep ()

 # 如 果 转 过 的 角 度 大 于 需 要 旋 转 的 角 度， 那 么 表 示 到 达
        if current_angle_degree > relative_angle_degree :
            rospy.loginfo('Reached')
            break

 # 停 止 旋 转
    velocity_message .angular.z = 0
    self. velocity_publisher .publish( velocity_message )

 # desired_angle_degree ： 相 对 于0位 置 需 要 转 动 的 角 度
 def set_desired_orientation (self , desired_angle_degree ):
 # 把 目 标 角 度 转 化 为 弧 度
    desired_angle_radians = math.radians(abs( desired_angle_degree ))

 # 如 果 目 标 弧 度 小 于 当 前 弧 度 则 正 转， 否 则 就 反 转
    relative_angle_degree = desired_angle_radians - self.yaw
    if relative_angle_degree < 0:
        clockwise = 1
    else:
        clockwise = 0

    self.rotate (30, math.degrees(abs( relative_angle_degree )), clockwise )


if __name__ == '__main__':
    type = int(sys.argv [1])

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

  if type == 1:
    if len(sys.argv) == 5:
        angular_speed_degree = int(sys.argv [2])
        relative_angle_degree = int(sys.argv [3])
        clockwise = strtobool (sys.argv [4])

        turtle_sim_motion .rotate( angular_speed_degree ,
            relative_angle_degree , clockwise )
    else:
        print("%s Type = %d Parameter error" % (sys.argv [0], type))
        sys.exit (1)
  elif type == 2:
    if len(sys.argv) == 3:
        desired_degree = int(sys.argv [2])

        turtle_sim_motion . set_desired_orientation ( desired_degree )
    else:
        print("%s Type = %d Parameter error" % (sys.argv [0], type))
        sys.exit (1)

第 29~35 行：因为小乌龟只需要执行旋转操作，因此需要把角速度和线速度都设置为 0。
第 38 行：把角速度从角度值转化为弧度值。
第 41~44 行：如果角速度大于 0，则逆时针旋转，否则就顺时针旋转。
第 49 行：记录当前时间作为起始时间。
第 57 行：计算从起始时间开始到当前时间，小乌龟转过的角度。
第 61~63 行：当前旋转的角度大于要旋转的角度，那么就表明到达。
第 72 行：把要旋转到的目标角度转化为弧度。
第 75~79：根据目标角度与当前角度的差值判断是正转还是反转。
执行如6.12所示的指令就可以实现小乌龟的旋转。

Code 6.12: 小乌龟的旋转指令

roscore

 # 启 动 小 乌 龟
 rosrun turtlesim turtlesim_node

 # 使 小 乌 龟 顺 时 针 旋 转45度
 rosrun hello_tutorials turtlesim_rotate_motion.py 1  20  45  1

 # 使 小 乌 龟 旋 转 到270度
 rosrun hello_tutorials turtlesim_rotate_motion.py 2  270