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!

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