rospy 让小车前进

1. goforward.py

#!/usr/bin/env python
# coding=utf-8
# A very basic TurtleBot script that moves TurtleBot forward indefinitely. 
# Press CTRL + C to stop.  To run:
# On TurtleBot:
# roslaunch turtlebot_bringup minimal.launch
# On work station:
# python goforward.py

import rospy
from geometry_msgs.msg import Twist  # velocity


class GoForward:
    def __init__(self):
        # 1.定义 node: GoForward
        rospy.init_node('GoForward', anonymous=False)

        # tell user how to stop TurtleBot
        rospy.loginfo("To stop TurtleBot CTRL + C")  # rospy INFO 打印信息

        # 2.定义 Topic: cmd_vel, 此 Topic 由 GoForward node 来 publish
        # name: str, 'cmd_vel_mux/input/navi', 这是一个自带的 rostopic list
        # data_class: message class for serialization，这里传入 Twist 速度的类
        # queue size: used for asynchronously publishing messages 异步通信
        self.cmd_vel = rospy.Publisher('cmd_vel_mux/input/navi', Twist, queue_size=10)

        # 3.设置 Topic 信息发送频率
        r = rospy.Rate(10)  # TurtleBot will stop if we don't keep telling it to move.

        # 4.设置 msg 内容
        move_cmd = Twist()  # Twist is a datatype for velocity    
        move_cmd.linear.x = 0.2  # go forward at 0.2 m/s
        move_cmd.angular.z = 0  # w = 0

        # 5.设置关闭函数，一定要在下面 while死循环之前
        rospy.on_shutdown(self.shutdown)  # 传入1个函数

        # 6.as long as you haven't ctrl + c keeping doing...
        while not rospy.is_shutdown():
            self.cmd_vel.publish(move_cmd)  # Topic publish msg
            r.sleep()  # sleep 0.1 seconds (10 HZ) and publish again

    # stop turtlebot
    def shutdown(self):
        rospy.loginfo("Stop TurtleBot")
        self.cmd_vel.publish(Twist())  # a default Twist has linear.x of 0 and angular.z of 0. So it'll stop TurtleBot
        rospy.sleep(1)  # sleep just makes sure TurtleBot receives the stop command prior to shutting down the script


if __name__ == '__main__':
    try:
        GoForward()
    except:
        rospy.loginfo("GoForward node terminated.")

2. gocircle.py

#!/usr/bin/env python
# coding=utf-8
import rospy
from geometry_msgs.msg import Twist


class GoCircle:
    def __init__(self):
        # 1.定义 Node
        rospy.init_node('GoCircle', anonymous=False)
        rospy.loginfo("To stop TurtleBot CTRL + C")

        # 2.关闭函数
        rospy.on_shutdown(self.shutdown)

        # 3.定义 Topic
        self.cmd_vel = rospy.Publisher('cmd_vel_mux/input/navi', Twist, queue_size=10)

        # 4.publish 信息频率
        r = rospy.Rate(10)

        # 5.定义 msg
        move_cmd = Twist()
        move_cmd.linear.x = 0
        move_cmd.angular.z = -0.5  # +逆时针，-顺时针

        # 6.循环执行
        while not rospy.is_shutdown():
            self.cmd_vel.publish(move_cmd)
            r.sleep()

    def shutdown(self):
        rospy.loginfo("Stop TurtleBot")
        self.cmd_vel.publish(Twist())
        rospy.sleep(1)


if __name__ == '__main__':
    try:
        GoCircle()
    except:
        rospy.loginfo("GoCircle node terminated.")

3. gosquare.py

#!/usr/bin/env python
# coding=utf-8

import rospy
from geometry_msgs.msg import Twist
from math import radians  # 弧度


class GoSquare:
    def __init__(self):
        # 1.定义 Node
        rospy.init_node('GoSquare', anonymous=False)  # 这里的名字可以不和类名一样，但最好一样

        # 2.关闭函数  
        rospy.on_shutdown(self.shutdown)

        # 3.定义 Topic
        self.cmd_vel = rospy.Publisher('cmd_vel_mux/input/navi', Twist, queue_size=10)

        # 4.设置频率 5hz/s
        r = rospy.Rate(5)  # 可理解为循环体内每隔5次执行一次速度

        # 5.定义2个速度，1个前进，1个原地旋转45°，也就是 msg

        # let's go forward at 0.2 m/s
        move_cmd = Twist()
        move_cmd.linear.x = 0.2  # 默认 z=0 所以不用设置

        # let's turn at 45 deg/s
        turn_cmd = Twist()
        turn_cmd.linear.x = 0
        turn_cmd.angular.z = radians(45)  # 45 deg/s in radians/s

        # 6.执行循环
        count = 0
        while not rospy.is_shutdown():
            rospy.loginfo("Going Straight")  # 前进
            for x in range(0, 10):  # 10Hz = 2s，0.2x2
                self.cmd_vel.publish(move_cmd)
                r.sleep() # 5hz，即每 0.2s 执行循环体一次，10次循环 = 2s
                
            rospy.loginfo("Turning")  # 旋转
            for x in range(0, 10):
                self.cmd_vel.publish(turn_cmd)
                r.sleep() # 45°/s 所以2s旋转90°
                
            count += 1
            if count == 4:
                count = 0
                rospy.loginfo("TurtleBot should be close to the original starting position (but it's probably way off)")

    def shutdown(self):
        # stop turtlebot
        rospy.loginfo("Stop Drawing Squares")
        self.cmd_vel.publish(Twist())
        rospy.sleep(1)


if __name__ == '__main__':
    try:
        GoSquare()
    except:
        rospy.loginfo("node terminated.")