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
获取最新文章: 扫一扫右上角的二维码加入“创客智造”公众号