rospy 让机器人绕圈矩形行走（碰到障碍物停止）

Posted 2020-12-01 sqzqllaozhuo

绕圆圈行走 

#!/usr/bin/env python

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

def publisher():
　　pub = rospy.Publisher(‘mobile_base/commands/velocity‘, Twist)
　　rospy.init_node(‘Walker‘, anonymous=True)
　　rate = rospy.Rate(10) #10hz
　　r = 0.1 # 半径
　　speed_lin = 0.5 #线速度
　　speed_ang = speed_lin/(2*pi*r) #角速度
　　desired_velocity = Twist()
　　desired_velocity.linear.x = speed_lin
　　desired_velocity.angular.z = speed_ang
　　while True:
　　pub.publish(desired_velocity)
　　rate.sleep()


if __name__ == "__main__":
try:
　　publisher()
except rospy.ROSInterruptException:
　　pass
————————————————

 

 

绕矩形行走，并且碰到障碍物停止。

#!/usr/bin/env python

import rospy
from geometry_msgs.msg import Twist
from kobuki_msgs.msg import BumperEvent
from math import pi
import time


stop = False

def callback(data, pub):
global stop
if data.state == 1:
stop = True
print(‘Robot stop!‘)

def publisher():
global stop
pub = rospy.Publisher(‘mobile_base/commands/velocity‘, Twist)
sub = rospy.Subscriber(‘mobile_base/events/bumper‘, BumperEvent, callback=callback, callback_args=pub)
rospy.init_node(‘Walker‘, anonymous=True)
rate = rospy.Rate(10) #10hz 10/s
desired_velocity = Twist()
desired_velocity.linear.x = 0
desired_velocity.angular.z = 0

while True:
desired_velocity.linear.x = 0.3
desired_velocity.angular.z = 0
# go straigth
for i in range(40):
pub.publish(desired_velocity)
if stop:
return
rate.sleep()
# turn direct
desired_velocity.linear.x = 0
desired_velocity.angular.z = pi/2
for i in range(10):
pub.publish(desired_velocity)
if stop:
return
rate.sleep()

if __name__ == "__main__":
try:
publisher()
except rospy.ROSInterruptException:
pass

————————————————
版权声明：本文为CSDN博主「武倔」的原创文章，遵循 CC 4.0 BY-SA 版权协议，转载请附上原文出处链接及本声明。
原文链接：https://blog.csdn.net/blowfire123/article/details/102792073