python - ROS编程: Trying to make my turtlebot wander without running into obstacles

Question

我是 ROS 新手。我试图让我的 turtlebot 向前移动，直到它与障碍物的距离在最小距离内，然后旋转直到路径畅通，然后再次向前移动等等...

我写了这段代码:

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”。我很想得到一些帮助。谢谢!

Answer 1

我有一个解决错误导航算法的方法，希望能帮助你:

使用这些存储库(agn_gazebo、bugs)，您可以调出一个带有边界 map 和一些障碍物的 Gazebo 模拟以及一个移动轮式机器人平台 (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 工作区中使用 catkin_make 构建它们。

• 构建包后，您需要更改agn_gazebo/worlds/final.world 文件:

  打开它并使用 Ctrl + F 或更改所有 /home/agn/catkin_ws/src/... Ctrl + H

  我打开了一个 issue使其成为动态路径而不是静态路径，但目前我做不到。

• 然后启动模拟器:roslaunch agn_gazebo gazebo.launch

• 使用位置目标运行任何错误算法:

  rosrun 错误 bug.py bug0 11 0
  或者
  rosrun 错误 bug.py bug1 11 0
  或者
  rosrun 错误 bug.py bug2 11 0