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

绕圆圈行走 

#!/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