我是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”。我很想得到一些帮助。 谢谢！
答案 0 :(得分：0)
我有一个bugs导航算法的解决方案，希望对您有所帮助：
使用这些存储库（agn_gazebo，bugs），您可以调出带有边界图和某些障碍物以及移动轮式机器人平台（ Pioneer P3dx ），并装有激光扫描仪（ Hokuyo URG ）以实现环境感知。
使用以下方法从~/catkin_ws/src中的这些存储库克隆后：
git clone https://github.com/agn-7/agn_gazebo.git
git clone https://github.com/agn-7/bugs.git
然后在柳絮工作区中使用catkin_make构建它们。
构建软件包后，您需要更改agn_gazebo/worlds/final.world文件：
打开它，并用 Ctrl + F 或 Ctrl + H < / kbd>
我已经打开issue使其成为动态路径而不是静态路径，但是目前我无法做到这一点。
然后启动模拟器：/home/agn/catkin_ws/src/...
使用定位目标运行任何错误算法：
roslaunch agn_gazebo gazebo.launch
或
rosrun bugs bug.py bug0 11 0
或
rosrun bugs bug.py bug1 11 0
答案 1 :(得分：0)
好的，我已经实现了一些应该可以正常工作的东西。
我有3个文件，可以按照您的要求使漫游机器人工作。
wander_bot.launch 职责：该文件是一个启动文件，它将运行凉亭世界并存储可配置的参数。
turtlebot_node.py 职责：该python文件将初始化ROS节点，并加载可配置的参数，然后运行使Turtlebot移动的代码。
turtlebot_logic.py 责任：这个python文件将监听激光，它将执行所有逻辑工作以使漫游器机器人正常工作。
wander_bot.launch
<launch>
<param name="/use_sim_time" value="true" />
<!-- Launch turtle bot world -->
<include file="$(find turtlebot_gazebo)/launch/turtlebot_world.launch"/>
<!-- Launch stopper node -->
<node name="wander_bot" pkg="wander_bot" type="turtlebot_node.py" output="screen">
<param name="forward_speed" type="double" value="0.2"/>
<param name="minimum_distance_from_obstacle" type="double" value="1.0"/>
<param name="rotation_speed" type="int" value="2"/>
<param name="minimum_angle" type="int" value="-30"/>
<param name="maximum_angle" type="int" value="30"/>
</node>
</launch>
turtlebot_node.py
#!/usr/bin/python
import rospy
import sys
from turtlebot_logic import RosTurtlebotLogic
def loadParams():
forwardSpeed = 0.2
rotationSpeed = 2
minDistanceFromObstacle = 1.0
minimumAngle = -30
maximumAngle = 30
if rospy.has_param('~forward_speed'):
forwardSpeed = rospy.get_param('~forward_speed')
if rospy.has_param('~rotation_speed'):
rotationSpeed = rospy.get_param('~rotation_speed')
if rospy.has_param('~minimum_distance_from_obstacle'):
minDistanceFromObstacle = rospy.get_param('~minimum_distance_from_obstacle')
if rospy.has_param('~minimum_angle'):
minimumAngle = rospy.get_param('~minimum_angle')
if rospy.has_param('~maximum_angle'):
maximumAngle = rospy.get_param('~maximum_angle')
return forwardSpeed, rotationSpeed, minDistanceFromObstacle, minimumAngle, maximumAngle
if __name__ == "__main__":
rospy.loginfo("Started program.")
rospy.init_node("stopper_node", argv=sys.argv)
forwardSpeed, rotationSpeed, minDistanceFromObstacle, minimumAngle, maximumAngle = loadParams()
rospy.loginfo("Finished import parameters.")
my_stopper = RosTurtlebotLogic(forwardSpeed, rotationSpeed, minDistanceFromObstacle, minimumAngle, maximumAngle)
my_stopper.startMoving()
turtlebot_logic.py
#!/usr/bin/python
import math
import rospy
from geometry_msgs.msg import Twist
from sensor_msgs.msg import LaserScan
class RosTurtlebotLogic(object):
def __init__(self, forwardSpeed, rotationSpeed, minDistanceFromObstacle, minimumAngle, maximumAngle):
self.forwardSpeed = forwardSpeed
self.rotationSpeed = rotationSpeed
self.minDistanceFromObstacle = minDistanceFromObstacle
# Keeps the current minimum distance from obstacle from laser.
self.minimumRangeAhead = 5
# In which direction to rotate, left or right.
self.rotationDirection = 0
self.minimumIndexLaser = 360 * (90 + minimumAngle) / 90
self.maximumIndexLaser = 360 * (90 + maximumAngle) / 90 - 1
self.keepMoving = True
self.commandPub = rospy.Publisher("/cmd_vel_mux/input/teleop", Twist, queue_size=10)
rospy.Subscriber("scan", LaserScan, self.scanCallback, queue_size=10)
def startMoving(self):
rate = rospy.Rate(10)
while(not rospy.is_shutdown()):
if(self.keepMoving):
if (self.minimumRangeAhead < self.minDistanceFromObstacle):
self.keepMoving = False
else:
if(self.minimumRangeAhead > self.minDistanceFromObstacle):
self.keepMoving = True
twist = Twist()
if(self.keepMoving):
twist.linear.x = self.forwardSpeed
else:
twist.angular.z = self.rotationDirection
self.commandPub.publish(twist)
rate.sleep()
def scanCallback(self, scan_msg):
minimum = 100
index = 0
# Find the minimum distance to obstacle and we keep the minimum distance and the index.
# We need the minimum distance in order to know if we can go forward or not.
# We need the index to know in which direction to rotate.
if(not math.isnan(scan_msg.ranges[self.minimumIndexLaser])):
minimum = scan_msg.ranges[self.minimumIndexLaser]
for i in range(self.minimumIndexLaser, self.maximumIndexLaser + 1):
if(not math.isnan(scan_msg.ranges[i]) and scan_msg.ranges[i] < minimum):
minimum = scan_msg.ranges[i]
index = i
self.minimumRangeAhead = minimum
if(index <= 359):
self.rotationDirection = self.rotationSpeed
else:
self.rotationDirection = -self.rotationSpeed
激光器注视180度，直角为索引0 在ranges数组中，forward是ranges数组中的索引359，左边是ranges数组中的索引719，ranges数组来自订户：
rospy.Subscriber("scan", LaserScan, self.scanCallback, queue_size=10)
回调函数为scanCallback
嗯，这种简单明了，我认为我不需要解释代码，它有充分的文档记录和易于理解，如有任何其他问题，请在此处评论。