我是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"。我很想得到一些帮助。谢谢
我有一个bugs导航算法的解决方案，希望能帮助你：
有了这些存储库(agn_gazebo，bugs(，您可以使用有界地图和一些障碍物以及移动轮式机器人平台(Pioneer P3dx(进行gazebo模拟，该平台配备了用于环境感知的激光扫描仪(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工作空间中使用
catkin_make构建它们。 -
包构建后，您需要更改
agn_gazebo/worlds/final.world文件：打开它并使用Ctrl+F或<kbd]Ctrl>+
H我已经打开了一个问题，将其作为动态路径而不是静态路径，但目前，我无法做到这一点。
-
然后打开模拟器：
roslaunch agn_gazebo gazebo.launch -
使用位置目标运行任何错误算法：
rosrun bugs bug.py bug0 11 0
或rosrun bugs bug.py bug1 11 0
，或rosrun bugs bug.py bug2 11 0
好的，我已经实现了一些应该可以很好地工作的东西。
我有三个文件，使流浪的骗子工作如你所要求的。
-
wander_bot.launch职责：该文件是一个启动文件，将运行gazebo世界并存储可配置的参数。
-
turtlebot_node.py职责：这个python文件将初始化ROS节点，并加载可配置的参数，然后运行使turtlebot移动的代码。
-
turtlebot_logic.py职责：这个python文件将监听激光器，它将实现所有的逻辑内容，使漫游机器人工作。
wander_bot.启动
<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在范围阵列处，前是范围阵列处的索引359，左是范围阵列的索引719，范围阵列来自订户：
rospy.Subscriber("scan", LaserScan, self.scanCallback, queue_size=10)
回调函数为scanCallback
好吧，这是直截了当的，我认为我不需要解释代码，它有很好的文档记录，很容易理解，如果有任何进一步的问题，请在这里评论。