test_accel.py
Go to the documentation of this file.
00001 #!/usr/bin/python
00002 #
00003 #  send pilot commands and update state
00004 #
00005 #   Copyright (C) 2011 Austin Robot Technology
00006 #   License: Modified BSD Software License Agreement
00007 #
00008 # $Id: pilot_cmd.py 1321 2011-04-19 20:23:37Z jack.oquin $
00009 
00010 PKG_NAME = 'art_pilot'
00011 
00012 # ROS node setup
00013 import roslib;
00014 roslib.load_manifest(PKG_NAME)
00015 import rospy
00016 
00017 # ROS messages
00018 from art_msgs.msg import ArtVehicle
00019 from art_msgs.msg import CarDriveStamped
00020 from art_msgs.msg import CarDrive
00021 from art_msgs.msg import DriverState
00022 from art_msgs.msg import Epsilon
00023 from art_msgs.msg import Gear
00024 from art_msgs.msg import PilotState
00025 
00026 def clamp(minimum, value, maximum):
00027     "constrain value to the range [minimum .. maximum]"
00028     return max(minimum, min(value, maximum))
00029 
00030 class PilotCommand():
00031     "ART pilot command interface."
00032 
00033     def __init__(self, maxspeed=6.0, minspeed=-3.0):
00034         "PilotCommand constructor"
00035         self.reconfigure(maxspeed, minspeed)
00036         self.pstate = PilotState()
00037         self.car_ctl = CarDrive()
00038         self.car_msg = CarDriveStamped()
00039         self.pub = rospy.Publisher('pilot/drive', CarDriveStamped)
00040         self.sub = rospy.Subscriber('pilot/state', PilotState,
00041                                     self.pilotCallback)
00042 
00043     def accelerate(self, dv):
00044         "accelerate dv meters/second^2"
00045         rospy.logdebug('acceleration: ' + str(dv))
00046 
00047         self.car_ctl.acceleration = dv
00048 
00049         # update speed
00050         dv *= 0.05                      # scale by cycle duration (dt)
00051         vabs = abs(self.car_ctl.speed)
00052 
00053         # never shift gears via acceleration, stop at zero
00054         if -dv > vabs:
00055             vabs = 0.0
00056         else:
00057             vabs += dv
00058 
00059         if self.car_ctl.gear.value == Gear.Drive:
00060             self.car_ctl.speed = vabs
00061         elif self.car_ctl.gear.value == Gear.Reverse:
00062             self.car_ctl.speed = -vabs
00063         else:                   # do nothing in Park
00064             self.car_ctl.speed = 0.0
00065             self.car_ctl.acceleration = 0.0
00066 
00067         # never exceeded forward or reverse speed limits
00068         self.car_ctl.speed = clamp(self.minspeed,
00069                                    self.car_ctl.speed,
00070                                    self.maxspeed)
00071 
00072     def command(self, speed, accel):
00073         "set pilot command parameters"
00074         self.car_ctl.speed = speed
00075         self.car_ctl.acceleration = accel
00076 
00077     def halt(self):
00078         "halt car immediately"
00079         self.car_ctl.speed = 0.0
00080         self.car_ctl.acceleration = 0.0
00081 
00082     def is_running(self):
00083         "return True if pilot node is RUNNING"
00084         # TODO check that time stamp is not too old
00085         return (self.pstate.pilot.state == DriverState.RUNNING)
00086 
00087     def is_stopped(self):
00088         "return True if vehicle is stopped"
00089         return (abs(self.car_ctl.speed) < Epsilon.speed)
00090 
00091     def pilotCallback(self, pstate):
00092         "handle pilot state message"
00093         self.pstate = pstate
00094         #rospy.loginfo(str(pstate))
00095         # Base future commands on current state, not target.
00096         self.car_ctl = pstate.current
00097 
00098     def publish(self):
00099         "publish pilot command message"
00100         self.car_msg.header.stamp = rospy.Time.now()
00101         self.car_msg.control = self.car_ctl
00102         self.pub.publish(self.car_msg)
00103 
00104     def reconfigure(self, maxspeed, minspeed):
00105         "reconfigure forward and reverse speed limits"
00106         self.maxspeed = maxspeed
00107         self.minspeed = minspeed
00108 
00109     def shift(self, gear):
00110         "set gear request (only if stopped)"
00111         if self.is_stopped():
00112             self.car_ctl.gear.value = gear
00113 
00114     def steer(self, angle):
00115         "set wheel angle (radians)"
00116         # ensure maximum wheel angle never exceeded
00117         self.car_ctl.steering_angle = clamp(-ArtVehicle.max_steer_radians,
00118                                              angle,
00119                                              ArtVehicle.max_steer_radians)
00120 
00121 
00122 def test(pilot):
00123 
00124     # list of tuples: (speed, acceleration, duration)
00125     requests = [(3.0, 20.0, 8.0),
00126                 (6.0, 20.0, 8.0),
00127                 (9.0, 20.0, 8.0),
00128                 (6.0, 20.0, 8.0),
00129                 (3.0, 20.0, 8.0),
00130                 (0.0, 20.0, 4.0),
00131                 (3.0, 0.5, 16.0),
00132                 (6.0, 0.5, 16.0),
00133                 (9.0, 0.5, 16.0),
00134                 (6.0, 0.5, 16.0),
00135                 (3.0, 0.5, 16.0),
00136                 (0.0, 0.5, 16.0)]
00137 
00138     while not rospy.is_shutdown():
00139 
00140         if pilot.is_running():
00141             # start running the test
00142 
00143             if len(requests) == 0:
00144                 # stop car after test completed
00145                 vel = 0.0
00146                 accel = 0.0
00147                 duration = 0.0
00148             else:
00149                 # get first tuple in the requests list
00150                 (vel, accel, duration) = requests.pop(0)
00151 
00152             rospy.loginfo('requesting v: ' + str(vel)
00153                           + ' a: ' + str(accel)
00154                           + ' t: ' + str(duration))
00155 
00156             if pilot.is_stopped():
00157                 pilot.shift(Gear.Drive)
00158 
00159             pilot.command(vel, accel)
00160             pilot.publish()
00161 
00162             if duration == 0.0:
00163                 break           # test finished
00164 
00165             rospy.sleep(duration)
00166 
00167         else:
00168             # wait until pilot is running
00169             rospy.loginfo('waiting for pilot to run')
00170             rospy.sleep(1.0)
00171 
00172 if __name__ == '__main__':
00173     rospy.init_node('pilot_cmd')
00174     rospy.loginfo('starting acceleration test')
00175     pilot = PilotCommand(10.0)
00176     try:
00177         test(pilot)
00178     except rospy.ROSInterruptException: pass
00179 
00180     rospy.loginfo('acceleration test completed')
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Properties Friends Defines


art_pilot
Author(s): Austin Robot Technology, Jack O'Quin
autogenerated on Tue Sep 24 2013 10:44:02