Walking in circles
#!/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
————————————————
Rectangular walk around and bumping into an obstacle stops.
#!/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
----------------
Disclaimer: This article is CSDN blogger "Wu stubborn" in the original article, follow the CC 4.0 BY-SA copyright agreement, reproduced, please attach the original source and this link statement.
Original link: https: //blog.csdn.net/blowfire123/article/details/102792073