Turbot与python编程-实现走方形

Turbot与Python教程-实现走方形

说明：

• 介绍如何实现通过python控制turbot实现走方形

代码：

• 参考代码：github
• 实现代码：

#!/usr/bin/env python
# An example of TurtleBot 2 drawing a 0.4 meter square.
# Written for indigo
# drawSquare.py

import rospy
from geometry_msgs.msg import Twist
from math import radians

class DrawASquare():
    def __init__(self):
        # initiliaze
        rospy.init_node('drawasquare', anonymous=False)

        # What to do you ctrl + c    
        rospy.on_shutdown(self.shutdown)
        
        self.cmd_vel = rospy.Publisher('cmd_vel_mux/input/navi', Twist, queue_size=10)
     
    # 5 HZ
        r = rospy.Rate(5);

    # create two different Twist() variables.  One for moving forward.  One for turning 45 degrees.

        # let's go forward at 0.2 m/s
        move_cmd = Twist()
        move_cmd.linear.x = 0.2
    # by default angular.z is 0 so setting this isn't required

        #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

    #two keep drawing squares.  Go forward for 2 seconds (10 x 5 HZ) then turn for 2 second
    count = 0
        while not rospy.is_shutdown():
        # go forward 0.4 m (2 seconds * 0.2 m / seconds)
        rospy.loginfo("Going Straight")
            for x in range(0,10):
                self.cmd_vel.publish(move_cmd)
                r.sleep()
        # turn 90 degrees
        rospy.loginfo("Turning")
            for x in range(0,10):
                self.cmd_vel.publish(turn_cmd)
                r.sleep()            
        count = count + 1
        if(count == 4): 
                count = 0
        if(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:
        DrawASquare()
    except:
        rospy.loginfo("node terminated.")

演示：

• 主机，新终端，启动底盘

$ roslaunch turbot_bringup minimal.launch

• 从机，新终端，启动脚本

$ rosrun turbot_code drawSquare.py