其他分享
首页 > 其他分享> > rospy 让机器人绕圈、矩形行走(碰到障碍物停止)

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

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

 

标签:__,stop,desired,rospy,绕圈,import,velocity,矩形
来源: https://www.cnblogs.com/sqzqllaozhuo/p/11781370.html