def sendNoBrake(enable_pub, brake_pub):
enable = Bool()
enable.data = True
enable_pub.publish(enable)
brake = PacmodCmd()
brake.enable = True
brake.f64_cmd = 0.0
brake_pub.publish(brake)
def constVelocity():
global currentVelocity, previousVelocity, targetVelocity, error, integralError, derivativeError, previousError, previousIntegralError, Kp, Ki, Kd, throttle, prevTime, time, controlMax, controlMin, MPH2MPS, integralMax
rospy.init_node('speed_controller', anonymous=True)
accel_pub = rospy.Publisher('pacmod/as_rx/accel_cmd',
PacmodCmd,
queue_size=10)
brake_pub = rospy.Publisher('/pacmod/as_rx/brake_cmd',
PacmodCmd,
queue_size=10)
rospy.Subscriber('/pacmod/as_tx/vehicle_speed', Float64, speedCallBack)
rospy.Sublisher('/speed_applied', Float64, whiteLineCallBack)
rospy.Sublisher('/stop_speed', Float64, whiteLineCallBack)
rospy.Subscriber('/selfdrive/state', Int8, state_callback)
accel = PacmodCmd()
accel.f64_cmd = 0
brake = PacmodCmd()
brake.f64_cmd = 0
rate = rospy.Rate(10)
while not rospy.is_shutdown():
if state and not (10 <= state < 20 or state == 3):
brake.f64_cmd = 0
brake_pub.publish(brake)
accel.f64_cmd = velController()
accel_pub.publish(accel)
else:
integralError = 0
rate.sleep()
def stop_vehicle():
throttle = PacmodCmd()
throttle.f64_cmd = 0.0
brake = PacmodCmd()
global speed
kp = 0.25
rate = rospy.Rate(10)
while speed > 0.01:
throttle_pub.publish(throttle)
brake_val = kp * speed
#if brake_val > 0.8:
# brake_val = 0.8
#if brake_val < 0.60:
# brake_val = 0.60
brake_val = 0.75
brake.f64_cmd = brake_val
brake_pub.publish(brake)
rate.sleep()
brake.f64_cmd = 0.65
brake_pub.publish(brake)
return
import time
from std_msgs.msg import String, Bool, Float32, Float64
from pacmod_msgs.msg import PositionWithSpeed, PacmodCmd
from pynput.keyboard import Key, Listener, KeyCode
steer_pub = rospy.Publisher('/pacmod/as_rx/steer_cmd',
PositionWithSpeed,
queue_size=1)
gear_pub = rospy.Publisher('/pacmod/as_rx/shift_cmd', PacmodCmd, queue_size=1)
enable_pub = rospy.Publisher('/pacmod/as_rx/enable', Bool, queue_size=1)
accel_pub = rospy.Publisher('/pacmod/as_rx/accel_cmd', PacmodCmd, queue_size=1)
brake_pub = rospy.Publisher('/pacmod/as_rx/brake_cmd', PacmodCmd, queue_size=1)
enabled = False
accel_flag = False
gear_cmd = PacmodCmd()
accel_cmd = PacmodCmd()
brake_cmd = PacmodCmd()
class PID_controller:
def __init__(self,
p=0.0,
i=0.0,
d=0.0,
wg=20.0,
avl=2.5,
max_steering_angle=0.8,
sp_p=0.0,
sp_d=0.0,
sp_i=0.0,
#!/usr/bin/env python
import rospy
from std_msgs.msg import Float64, Int8, Bool
from pacmod_msgs.msg import PacmodCmd, PositionWithSpeed
turn_cmd = PacmodCmd()
shift_cmd = PacmodCmd()
steer_cmd = PositionWithSpeed()
### Base settings for switching modes ###
def config(opmode=0):
global enabled
enable_pub.publish(True)
## turn off turn blinkers ##
turn_cmd.ui16_cmd = 1
turn_sig_pub.publish(turn_cmd)
shift_cmd.ui16_cmd = 2
shift_pub.publish(shift_cmd)
steer_cmd.angular_position = 0.0
steer_pub.publish(steer_cmd)
## disable Pacmod ##
if opmode:
tm = rospy.get_time()
while not enabled:
if rospy.get_time() - tm > 1.0:
print "\r\t** error: Pacmod refusing to enable..."
raise Exception
else:
enable_pub.publish(False)
return
#!/usr/bin/env python import rospy, sys from std_msgs.msg import Int16, Int8, Float64 from pacmod_msgs.msg import PacmodCmd brake_cmd = PacmodCmd() throttle_cmd = PacmodCmd() state = 0 VC = [50.0, 75.0] C = [50.0, 75.0, 100.0] M = [75.0, 150.0, 200.0] F = [150.0, 200.0, 250.0] VF = [200.0, 250.0] deltaST = [1.0, 2.0] deltaVS = [1.0, 2.0, 8.0] deltaS = [2.0, 8.0, 15.0] deltaF = [8.0, 15.0, 20.0] deltaVF = [20.0, 30.0] NOSPEED = 0.0 QUARTERSPEED = 0.5588 HALFSPEED = 1.1176 FULLSPEED = 2.2352 membership = [0.0, 0.0, 0.0, 0.0, 0.0] deltaMembership = [0.0, 0.0, 0.0, 0.0, 0.0]
#!/usr/bin/env python
import rospy, sys
from std_msgs.msg import Int8, UInt16MultiArray, Float64, Bool
from AY20_IGVC.msg import VisualObject, VisualObjectArray
from pacmod_msgs.msg import PacmodCmd, PositionWithSpeed, SystemRptInt
shift_cmd = PacmodCmd()
brake_cmd = PacmodCmd()
run_modes = {'\n':[0,"Regular Self-drive"],'q1':[1,'E-Stop Qualification'],'q2':[2,'Straight Lane Keeping Qualification'],\
'q3':[3,'Left Turn Qualification'],'q4':[4,'Right Turn Qualification'], 'q5':[5,'Stop Sign Detection Qualification']}
def mode_config(opt=0):
pass #...
return
def show_options():
print "\r\t\tOPTIONS:\
\n\r\t\t * E-Stop Qualification: 'q1'\
\n\r\t\t * Straight Lane Keeping Qualification: 'q2'\
\n\r\t\t * Left Turn Qualification: 'q3'\
\n\r\t\t * Right Turn Qualification: 'q4'\
\n\r\t\t * Stop Sign Detection Qualification 'q5'"
return
def countdown_to_run(mode_nm):
global run_status, shift_cmd, brake_cmd, tgt_shift
print "\r\t-- running {} in\n\r\t\t5...".format(mode_nm)
tgt_shift = 3 # put in forward drive
shift_cmd.ui16_cmd = tgt_shift