ROS programming: Trying to make my turtlebot wander without running into obstacles

Question

I'm new to ROS. I'm trying to make my turtlebot move forward until it is within a minimal distance from an obstacle, then rotate until the path is clear, then move forward again and so on...

I wrote this code:

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();

and this:

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

But even though I can't find any logical mistakes in it, it just does not work and occasionaly bumps into stuff. I am running it with the gazebo simulation "turtlebot_world". I would love to get some help. Thanks!

Answer 1

Okay, I've implemented something that should work pretty well.

I have 3 files that make the wander turtlebot work as you asked.

1. wander_bot.launch Responsibility: This file is a launch file that will run the gazebo world and will store the configurable parameters.

2. turtlebot_node.py Responsibility: This python file will initialize the ROS node and it will load the configurable parameters, and then it will run the code that makes the turtlebot move.

3. turtlebot_logic.py Responsibility: This python file will listen to the laser, it will implement all the logic stuff to make the wander bot work.

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

---

The laser looks at 180 degrees, the right angle is index 0 at ranges array, forward is index 359 at ranges array, and left is index 719 at ranges array, ranges array is coming from the subscriber:

rospy.Subscriber("scan", LaserScan, self.scanCallback, queue_size=10)

The callback function is scanCallback

---

Well, that kind of straight forward, I don't think that I need to explain the code, it's well documented and easy to understand, for any further questions please comment here.

Answer 2

I have a solution with the bugs navigation algorithms, hope help you up:

with these repositories (agn_gazebo, bugs), you could bring up a Gazebo simulation with a bounded map and some obstacle and a mobile wheeled robot platform (Pioneer P3dx) which is equipped with a laser scanner (Hokuyo URG) for environmental perception.

---

USAGE:

• After cloning from these repositories in ~/catkin_ws/src using:

  git clone https://github.com/agn-7/agn_gazebo.git
  git clone https://github.com/agn-7/bugs.git
  

• Then build them with catkin_make in your catkin workspace.

• After package building, you need to change the agn_gazebo/worlds/final.world file:

  Open it and change all /home/agn/catkin_ws/src/... with your target path with Ctrl + F or Ctrl + H

  I've opened an issue to make it as a dynamic path instead of static, but currently, I couldn't do it.

• Then Bring up the simulator: roslaunch agn_gazebo gazebo.launch

• Running any bugs algorithm with a position target:

  rosrun bugs bug.py bug0 11 0
  or
  rosrun bugs bug.py bug1 11 0
  or
  rosrun bugs bug.py bug2 11 0