motion_travel_forward.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 # Imports
00008 ##############################################################################
00009 
00010 import math
00011 import rospy
00012 from geometry_msgs.msg import Twist, Pose
00013 from nav_msgs.msg import Odometry
00014 from kobuki_msgs.msg import CliffEvent
00015 # Local imports
00016 import utils
00017 
00018 ##############################################################################
00019 # Classes
00020 ##############################################################################
00021     
00022 '''
00023   Travels forward a set distance. 
00024 
00025       API:
00026         init(speed,distance) : (re)initialise parameters 
00027         stop()  - stop.
00028         execute() - pass this to a thread to run
00029         shutdown() - cleanup
00030 '''
00031 class TravelForward(object):
00032     '''
00033       Initialise everything
00034       
00035       @param topic names
00036       @type strings
00037     '''
00038     def __init__(self, cmd_vel_topic, odom_topic, cliff_sensor_topic):
00039         self.odom_subscriber = rospy.Subscriber(odom_topic, Odometry, self.odometry_callback)
00040         self.cmd_vel_publisher = rospy.Publisher(cmd_vel_topic, Twist, queue_size=10)
00041         self.cliff_sensor_subscriber = rospy.Subscriber(cliff_sensor_topic, CliffEvent , self.cliff_sensor_callback)
00042         self.speed = 0.7
00043         self.distance = 1.0
00044         self._current_pose = Pose()
00045         self._starting_pose = Pose()
00046         self._stop = False
00047         self._running = False
00048     
00049     def init(self, speed, distance):
00050         self.speed = speed
00051         self.distance = distance
00052         
00053     def shutdown(self):
00054         self.stop()
00055         while self._running:
00056             rospy.sleep(0.05)
00057         self.cmd_vel_publisher.unregister()
00058         self.odom_subscriber.unregister()
00059     
00060     def stop(self):
00061         self._stop = True
00062 
00063     def execute(self):
00064         '''
00065           Drop this into threading.Thread or QThread for execution
00066         '''
00067         if self._running:
00068             rospy.logerr("Kobuki TestSuite: already executing a motion, ignoring the request")
00069             return
00070         self._stop = False
00071         self._running = True
00072         rate = rospy.Rate(10)
00073         self._current_speed = 0.0
00074         current_distance_sq = 0.0
00075         distance_sq = self.distance*self.distance
00076         self._starting_pose = self._current_pose
00077         while not self._stop and not rospy.is_shutdown():
00078             if current_distance_sq > distance_sq:
00079                 break
00080             else:
00081                 current_distance_sq = (self._current_pose.position.x - self._starting_pose.position.x)*(self._current_pose.position.x - self._starting_pose.position.x) + \
00082                                    (self._current_pose.position.y - self._starting_pose.position.y)*(self._current_pose.position.y - self._starting_pose.position.y)
00083                 #current_distance_sq += 0.01 # uncomment this and comment above for debugging
00084                 print("Distance %s"%math.sqrt(current_distance_sq))
00085                 if self.speed > 0:
00086                     if self._current_speed < self.speed:
00087                         self._current_speed += 0.01
00088                 else:
00089                     if self._current_speed > self.speed:
00090                         self._current_speed -= 0.01
00091                 cmd = Twist()
00092                 cmd.linear.x = self._current_speed
00093                 self.cmd_vel_publisher.publish(cmd)
00094             rate.sleep()
00095         if not rospy.is_shutdown():
00096             cmd = Twist()
00097             cmd.linear.x = 0.0
00098             self.cmd_vel_publisher.publish(cmd)
00099         self._running = False
00100         
00101     ##########################################################################
00102     # Ros Callbacks
00103     ##########################################################################
00104 
00105     def cliff_sensor_callback(self, data):
00106         rospy.loginfo("Kobuki Testsuite: cliff event on sensor [%s]"%str(data.sensor))
00107         if data.state == CliffEvent.CLIFF:
00108             if not rospy.is_shutdown():
00109                 cmd = Twist()
00110                 cmd.linear.x = 0.0
00111                 self.cmd_vel_publisher.publish(cmd)
00112             self.stop()
00113     
00114     def odometry_callback(self, data):
00115         self._current_pose = data.pose.pose


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