Turtlebot- Random Wandering with Collision Avoidance

edit

This answer is a bit of a copy-paste. The task you describe is the first assignment in Brown's CS148 Introduction to Robotics course. Here's a solution that isn't exactly pretty but presumably, at some point, did work... :P

#!/usr/bin/env python
import roslib; roslib.load_manifest('enclosure_escape')
import rospy
import random
from geometry_msgs.msg import Twist
from turtlebot_node.msg import TurtlebotSensorState

# global variables
bump = False

action_duration = 0.25
movement_speed = 0.15
turn_speed = 0.5
min_turn_duration = 3.5
max_turn_duration = 10.5

# listen (adapted from line_follower
def processSensing(TurtlebotSensorState):
     global bump
     bump = TurtlebotSensorState.bumps_wheeldrops
     #newInfo = True


def hello_create():
     pub = rospy.Publisher('/turtlebot_node/cmd_vel', Twist)
     rospy.Subscriber('/turtlebot_node/sensor_state', TurtlebotSensorState, processSensing)
     rospy.init_node('hello_create')
     #listen
     global bump
     twist = Twist()
     while not rospy.is_shutdown():
         if bump==1:
             str = "right bumper, turning left %s"%rospy.get_time()
             rospy.loginfo(str)
             turn(pub, random_duration(), turn_speed)
         elif bump==2:
             str = "left bumper, turning right %s"%rospy.get_time()
             rospy.loginfo(str)
             turn(pub, random_duration(), -turn_speed)
         elif bump==3:
             str = "both bumpers, turning left %s"%rospy.get_time()
             rospy.loginfo(str)
             turn(pub, random_duration(), turn_speed)
         else:
             str = "moving straight ahead %s"%rospy.get_time()
             rospy.loginfo(str)
             twist.linear.x = movement_speed; twist.linear.y = 0; twist.linear.z = 0
             twist.angular.x = 0; twist.angular.y = 0; twist.angular.z = 0
             bump = False
         pub.publish(twist)
         rospy.sleep(action_duration)

def random_duration():
    # Calculates a random amount of time for the Roomba to turn for
    duration = min_turn_duration + random.random() * (max_turn_duration - min_turn_duration)
    str = "Random duration: %s"%duration
    rospy.loginfo(str)
    return duration

def turn(pub, duration, weight):
    twist = Twist()

    # First, back up slightly from the wall
    twist.linear.x = -movement_speed; twist.linear.y = 0; twist.linear.z = 0
    twist.angular.x = 0; twist.angular.y = 0; twist.angular.z = 0
    pub.publish(twist)
    rospy.sleep(action_duration)

    # Now, keep turning until the end of the specified duration
    currentTime = rospy.get_time();
    stopTime = rospy.get_time() + duration;
    while (rospy.get_time() < stopTime):
         str = "turning %s"%rospy.get_time()
         rospy.loginfo(str)
         twist.linear.x = 0.0; twist.linear.y = 0; twist.linear.z = 0
         twist.angular.x = 0; twist.angular.y = 0; twist.angular.z = weight
         pub.publish(twist)
         rospy.sleep(action_duration)


if __name__ == '__main__':
     try:
         hello_create()
     except rospy.ROSInterruptException: pass