$search
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 1543 2011-05-10 22:39:28Z jack.oquin $ 00009 00010 PKG_NAME = 'art_teleop' 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 CarDrive 00020 from art_msgs.msg import CarDriveStamped 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, limit_forward=6.0, limit_reverse=3.0): 00034 "PilotCommand constructor" 00035 self.reconfigure(limit_forward, limit_reverse) 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 = abs(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 # never exceeded forward or reverse speed limits 00060 if self.car_ctl.gear.value == Gear.Drive: 00061 if vabs > self.limit_forward: 00062 vabs = self.limit_forward 00063 elif self.car_ctl.gear.value == Gear.Reverse: 00064 if vabs > self.limit_reverse: 00065 vabs = self.limit_reverse 00066 else: # do nothing in Park or Neutral 00067 vabs = 0.0 00068 self.car_ctl.acceleration = 0.0 00069 00070 self.car_ctl.speed = vabs 00071 00072 def halt(self): 00073 "halt car immediately" 00074 self.car_ctl.speed = 0.0 00075 self.car_ctl.acceleration = 0.0 00076 00077 def is_running(self): 00078 "return True if pilot node is RUNNING" 00079 # TODO check that time stamp is not too old 00080 return (self.pstate.pilot.state == DriverState.RUNNING) 00081 00082 def is_stopped(self): 00083 "return True if vehicle is stopped" 00084 return (abs(self.car_ctl.speed) < Epsilon.speed) 00085 00086 def pilotCallback(self, pstate): 00087 "handle pilot state message" 00088 self.pstate = pstate 00089 # Base future commands on current state, not target. 00090 self.car_ctl = pstate.current 00091 00092 def publish(self): 00093 "publish pilot command message" 00094 self.car_msg.header.stamp = rospy.Time.now() 00095 self.car_msg.control = self.car_ctl 00096 self.pub.publish(self.car_msg) 00097 00098 def reconfigure(self, limit_forward, limit_reverse): 00099 "reconfigure forward and reverse speed limits" 00100 self.limit_forward = limit_forward 00101 self.limit_reverse = abs(limit_reverse) 00102 00103 def shift(self, gear): 00104 "set gear request (only if stopped)" 00105 if self.is_stopped(): 00106 self.car_ctl.gear.value = gear 00107 00108 def steer(self, angle): 00109 "set wheel angle (radians)" 00110 # ensure maximum wheel angle never exceeded 00111 self.car_ctl.steering_angle = clamp(-ArtVehicle.max_steer_radians, 00112 angle, 00113 ArtVehicle.max_steer_radians) 00114 00115 00116 # standalone test of package 00117 if __name__ == '__main__': 00118 00119 rospy.init_node('pilot_cmd') 00120 00121 # make an instance 00122 pilot = PilotCommand() 00123 00124 # run the program 00125 try: 00126 rospy.spin() 00127 except rospy.ROSInterruptException: pass