Я новичок в ROS.Я пытаюсь заставить свою черепаху двигаться вперед, пока она не окажется на минимальном расстоянии от препятствия, затем повернуть, пока путь не станет свободным, затем снова двигаться вперед и так далее ...
Я написал этот код:
import rospy, sys
from stopper import Stopper
if __name__ == "__main__":
rospy.init_node("stopper_node", argv=sys.argv)
forward_speed = 0.5
angular_speed = 45
if rospy.has_param('~forward_speed'):
forward_speed = rospy.get_param('~forward_speed')
if rospy.has_param('~angular_speed'):
angular_speed = rospy.get_param('~angular_speed')
my_stopper = Stopper(forward_speed, angular_speed)
my_stopper.start_moving();
и это:
import rospy
import math
from geometry_msgs.msg import Twist
from sensor_msgs.msg import LaserScan
class Stopper(object):
def __init__(self, forward_speed, angular_speed):
self.forward_speed = forward_speed
self.angular_speed = angular_speed
self.min_scan_angle = -30 /180 * math.pi
self.max_scan_angle = 30 / 180 * math.pi
self.min_dist_from_obstacle = 1
self.keep_moving = True
self.keep_rotating = False
self.command_pub = rospy.Publisher("/cmd_vel_mux/input/teleop", Twist, queue_size=10)
self.laser_subscriber = rospy.Subscriber("scan",LaserScan, self.scan_callback, queue_size=1)
def start_moving(self):
print "started moving1"
rate = rospy.Rate(1)
rospy.loginfo("Starting to move")
while not rospy.is_shutdown() and self.keep_moving:
self.move_forward()
rate.sleep()
self.start_rotating()
def move_forward(self):
move_msg = Twist()
move_msg.linear.x = self.forward_speed
self.command_pub.publish(move_msg)
def start_rotating(self):
print "started rotating1"
rate = rospy.Rate(1)
rospy.loginfo("Starting to rotate")
while not rospy.is_shutdown() and self.keep_rotating:
self.rotate()
rate.sleep()
self.start_moving()
def rotate(self):
move_msg = Twist()
move_msg.angular.z = self.angular_speed * 2 * math.pi / 360
self.command_pub.publish(move_msg)
def scan_callback(self, scan_msg):
for dist in scan_msg.ranges:
print dist
if self.keep_moving and dist < self.min_dist_from_obstacle:
self.keep_moving = False
print "keep moving is now false"
self.keep_rotating = True
print "keep rotating is now true"
break
if self.keep_rotating and math.isnan(dist):
self.keep_rotating = False
print "keep rotating is now false"
self.keep_moving = True
print "keep moving is now true"
break
Но даже если я не могу найти в этом никаких логических ошибок, он просто не работает и иногда натыкается на вещи.Я использую симулятор беседки "turtlebot_world".Я хотел бы получить помощь.Спасибо!