2019-03-31 15:27

# 修改了程序都draw a square 但是运行不出来？

``````# -*- coding: UTF-8 -*-
#!/usr/bin/env python

#前进1m，画边长（ 0.2 ）m的五边形

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

class ROUTE():
def __init__(self):
# 节点初始化
rospy.init_node('route')

# 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);

# 两个不同的Twist()函数。一个向前直走，一个转角度

# 向前走 0.2 m/s
move_cmd = Twist()
move_cmd.linear.x = 0.2

#转54度
turn_cmd = Twist()
turn_cmd.linear.x = 0
turn_cmd.angular.z = radians(18); #直线不动，只转圈

count = 0
#到达指定起点
rospy.loginfo("前进ing")
for x in range(0,20):
self.cmd_vel.publosh(move_cmd)
r.sleep()
#第一条边，须先转54度
rospy.loginfo("进入正轨ing")
for x in range(0,15):
self.cmd_vel.publosh(turn_cmd)
r.sleep()
while not rospy.is_shutdown():
# 向前0.2m
rospy.loginfo("正在走ing")
for x in range(0,5):
self.cmd_vel.publish(move_cmd)
r.sleep()
#准备拍照,转234度正对物体（13*18）
rospy.loginfo("准备拍照ing")
for x in range(0,65):
self.cmd_vel.publish(turn_cmd)
r.sleep()
# 转54度(3*18）继续前进
rospy.loginfo("继续前进ing")
for x in range(0,15):
self.cmd_vel.publish(turn_cmd)
r.sleep()
count = count + 1
if(count == 5):
count = 0
if(count == 0):
rospy.loginfo("好像走完了，嘻嘻")

def shutdown(self):
# stop turtlebot
rospy.loginfo("结束了，嘤嘤嘤")
self.cmd_vel.publish(Twist())
rospy.sleep(1)

if __name__ == '__main__':
try:
ROUTE()
except:
rospy.loginfo("停了啊。")

``````
``````
**hazel@hazel-Lenovo-Legion-Y7000:~/helloworld/turtlebot\$ python route.py
[INFO] [1554016863.412115]: 前进ing
[INFO] [1554016863.412651]: 停了啊。
[INFO] [1554016863.413453]: 结束了，嘤嘤嘤**

``````
• 点赞
• 写回答
• 关注问题
• 收藏
• 邀请回答