motion_wander.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 #       
00003 # License: BSD
00004 #   https://raw.github.com/yujinrobot/kobuki/hydro-devel/kobuki_testsuite/LICENSE
00005 #
00006 ##############################################################################
00007 # Python Imports
00008 ##############################################################################
00009 import random
00010 from math import degrees, radians
00011 
00012 ##############################################################################
00013 # Ros Imports
00014 ##############################################################################
00015 import rospy
00016 from tf.transformations import euler_from_quaternion
00017 from geometry_msgs.msg import Twist
00018 from nav_msgs.msg import Odometry
00019 from kobuki_msgs.msg import BumperEvent, CliffEvent
00020 
00021 ##############################################################################
00022 # Local imports
00023 ##############################################################################
00024 import utils
00025 
00026 '''
00027   Implements a safe wandering random motion using bump and cliff sensors. 
00028 
00029       API:
00030         init(speed,distance) : (re)initialise parameters 
00031         stop()  - stop.
00032         execute() - pass this to a thread to run
00033         shutdown() - cleanup
00034       
00035       @param topic names
00036       @type strings
00037 
00038 '''
00039 class SafeWandering(object):
00040     '''
00041       Initialise everything, then starts with start()
00042 
00043       API:
00044         start() - start to wander. 
00045         stop()  - stop wandering.
00046         set_vels(lin_xvel,stepback_xvel,ang_zvel)
00047       
00048       @param topic names
00049       @type strings
00050     '''
00051     def __init__(self, cmd_vel_topic, odom_topic, bumper_topic, cliff_topic ):
00052 
00053         self.bumper_subscriber = rospy.Subscriber(bumper_topic, BumperEvent, self.bumper_event_callback)
00054         self.cliff_subscriber = rospy.Subscriber(cliff_topic, CliffEvent, self.cliff_event_callback)
00055         self.odom_subscriber = rospy.Subscriber(odom_topic, Odometry, self.odometry_callback)
00056         self.cmd_vel_publisher = rospy.Publisher(cmd_vel_topic, Twist, queue_size=10)
00057         self.rate = rospy.Rate(50)
00058 
00059         self.ok = True
00060         self.theta = 0.0
00061         self.theta_goal = 0.0
00062         self._stop = False
00063         self._running = False
00064 
00065         self._lin_xvel = 0.18
00066         self._stepback_xvel = -0.1
00067         self._ang_zvel = 1.8
00068 
00069     def init(self, speed, stepback_speed, angular_speed):
00070         self._lin_xvel = speed
00071         self._stepback_xvel = stepback_speed
00072         self._ang_zvel = angular_speed
00073 
00074         
00075     def shutdown(self):
00076         self.stop()
00077         while self._running:
00078             self.rate.sleep()
00079         #self.cmd_vel_publisher.unregister() This one creates an error for some reason, probably because climbing_frame already unregisters.
00080         self.odom_subscriber.unregister()
00081         self.bumper_subscriber.unregister()
00082         self.cliff_subscriber.unregister()
00083 
00084     def command(self, twist):
00085         '''
00086           Lowest level of command - i.e. this is where the fine grained looping happens
00087           so we check for aborts here. Don't waste time doing anything if this is the case.
00088         '''
00089         if self._stop or rospy.is_shutdown():
00090             return False
00091         self.cmd_vel_publisher.publish(twist)
00092         self.rate.sleep()
00093         return True
00094 
00095     def go(self):
00096         twist = Twist()
00097         while self.ok:
00098             twist.linear.x = self._lin_xvel
00099             if not self.command(twist):
00100                 return False
00101         return True
00102 
00103     def stepback(self):
00104         twist = Twist()
00105         for i in range(0,35): 
00106             twist.linear.x = self._stepback_xvel
00107             if not self.command(twist):
00108                 return False
00109         return True
00110  
00111     def turn(self):
00112         twist = Twist()
00113         while not self.reached():
00114             twist.angular.z = self._ang_zvel * utils.sign(utils.wrap_to_pi(self.theta_goal - self.theta))
00115             if not self.command(twist):
00116                 return False
00117         self.ok = True
00118         return True
00119 
00120     def reached(self):
00121         if abs(utils.wrap_to_pi(self.theta_goal - self.theta)) < radians(5.0):
00122             return True
00123         else:
00124             return False
00125 
00126     def stop(self):
00127         self._stop = True
00128 
00129     def execute(self):
00130         if self._running:
00131             rospy.logerr("Kobuki TestSuite: already executing wandering, ignoring the request")
00132             return
00133         self._stop = False
00134         self._running = True
00135         while True:
00136             if not self.go() or not self.stepback() or not self.turn():
00137                 break
00138         self._running = False
00139         if not rospy.is_shutdown():
00140             cmd = Twist()
00141             cmd.linear.x = 0.0
00142             self.cmd_vel_publisher.publish(cmd)
00143 
00144     ##########################################################################
00145     # Callbacks
00146     ##########################################################################
00147 
00148     def odometry_callback(self, data):
00149         quat = data.pose.pose.orientation
00150         q = [quat.x, quat.y, quat.z, quat.w]
00151         roll, pitch, yaw = euler_from_quaternion(q)
00152         self.theta = yaw
00153 
00154       
00155     def bumper_event_callback(self, data):
00156         if data.state == BumperEvent.PRESSED:
00157             self.ok = False
00158             if   data.bumper == BumperEvent.LEFT:
00159                 self.theta_goal = self.theta - 3.141592*random.uniform(0.2, 1.0)
00160             elif data.bumper == BumperEvent.RIGHT:
00161                 self.theta_goal = self.theta + 3.141592*random.uniform(0.2, 1.0)
00162             else:
00163                 self.theta_goal = utils.wrap_to_pi(self.theta + 3.141592*random.uniform(-1.0, 1.0))
00164 
00165     def cliff_event_callback(self, data):
00166         if data.state == CliffEvent.CLIFF:
00167             self.ok = False
00168             # print "Cliff event: %s,%s"%(str(data.sensor),str(data.state))
00169             if   data.sensor == CliffEvent.LEFT:
00170                 self.theta_goal = self.theta - 3.141592*random.uniform(0.2, 1.0)
00171             elif data.sensor == CliffEvent.RIGHT:
00172                 self.theta_goal = self.theta + 3.141592*random.uniform(0.2, 1.0)
00173             else:
00174                 self.theta_goal = utils.wrap_to_pi(self.theta + 3.141592*random.uniform(-1.0, 1.0))
00175  


kobuki_testsuite
Author(s): Jorge Santos Simon
autogenerated on Thu Jun 6 2019 17:38:11