def inputToPWM():
# initialize node
rospy.init_node('inputToPWM', anonymous=True)
global pubname, newECU, subname, move, still_moving
newECU = ECU()
newECU.motor = 1500
newECU.servo = 1550
move = False
still_moving = False
#print("1")
#print(move)
# topic subscriptions / publications
pubname = rospy.Publisher('ecu_pwm', ECU, queue_size=2)
rospy.Subscriber('turtle1/cmd_vel', Twist, start_callback)
rospy.Subscriber('encoder', Encoder, enc_callback) # Line Added
subname = rospy.Subscriber('uOpt', Input, callback_function)
rospy.Subscriber('moving', Moving, moving_callback_function)
# set node rate
loop_rate = 40
ts = 1.0 / loop_rate
rate = rospy.Rate(loop_rate)
t0 = time.time()
while not rospy.is_shutdown():
# calculate acceleration from PID controller.
motor_pwm = PID_control.acc_calculate(v_ref, v_meas) + motor_pwm_offset
# publish control command
ecu_pub.publish(ECU(motor_pwm, servo_pwm))
# wait
rate.sleep()
def inputToPWM():
# initialize node
rospy.init_node('inputToPWM', anonymous=True)
global pubname , newECU , subname, move , still_moving
newECU = ECU()
newECU.motor = 1500
newECU.servo = 1550
move = False
still_moving = False
#print("1")
#print(move)
# topic subscriptions / publications
pubname = rospy.Publisher('ecu_pwm',ECU, queue_size = 2)
rospy.Subscriber('turtle1/cmd_vel', Twist, start_callback)
subname = rospy.Subscriber('uOpt', Input, callback_function)
rospy.Subscriber('moving', Moving, moving_callback_function)
# set node rate
loop_rate = 40
ts = 1.0 / loop_rate
rate = rospy.Rate(loop_rate)
t0 = time.time()
rospy.spin()
def arduino_interface():
global ecu_pub, motor_pwm, servo_pwm
init_node('arduino_interface')
# set node rate
loop_rate = 50
dt = 1.0 / loop_rate
rate = rospy.Rate(loop_rate)
time_prev = time.time()
ecu_pub = Publisher('ecu_pwm', ECU, queue_size=10)
while not rospy.is_shutdown():
if time.time() >= time_prev and time.time() < time_prev + 6.5:
motor_pwm = 1650
if time.time() >= time_prev + 6.5:
motor_pwm = 1500 #1465
#if time.time() >= time_prev + 5 and time.time() < time_prev + 10:
# motor_pwm = 84.0
#if time.time() >= time_prev + 12 and time.time() < time_prev + 17:
# motor_pwm = 86.0
ecu_cmd = ECU(motor_pwm, servo_pwm)
ecu_pub.publish(ecu_cmd)
break
ecu_cmd = ECU(motor_pwm, servo_pwm)
ecu_pub.publish(ecu_cmd)
# wait
rate.sleep()
def arduino_interface():
global ecu_pub, motor_pwm, servo_pwm
init_node('arduino_interface')
# set node rate
loop_rate = 50
dt = 1.0 / loop_rate
rate = rospy.Rate(loop_rate)
time_prev = time.time()
ecu_pub = Publisher('ecu_pwm', ECU, queue_size = 10)
motor_pwm = 1600
servo_pwm = 1400
flag = 0
while not rospy.is_shutdown():
# if time.time()-time_prev>=3:
# servo_pwm = 1540
if time.time()-time_prev >= 8:
motor_pwm = 1500
ecu_cmd = ECU(motor_pwm, servo_pwm)
ecu_pub.publish(ecu_cmd)
break
ecu_cmd = ECU(motor_pwm, servo_pwm)
ecu_pub.publish(ecu_cmd)
# wait
rate.sleep()
def inputToPWM():
global motor_pwm, servo_pwm, motor_pwm_offset, servo_pwm_offset
global v_ref, v_meas
# initialize node
rospy.init_node('inputToPWM', anonymous=True)
global pubname, newECU, subname, move, still_moving
newECU = ECU()
newECU.motor = 1500
newECU.servo = 1550
move = False
still_moving = False
#print("1")
#print(move)
# topic subscriptions / publications
pubname = rospy.Publisher('ecu_pwm', ECU, queue_size=2)
rospy.Subscriber('turtle1/cmd_vel', Twist, start_callback)
subname = rospy.Subscriber('uOpt', Input, callback_function)
rospy.Subscriber('moving', Moving, moving_callback_function)
rospy.Subscriber('hold_previous_turn', Bool, hold_turn_function)
rospy.Subscriber('encoder', Encoder, enc_callback)
# set node rate
loop_rate = 40
ts = 1.0 / loop_rate
rate = rospy.Rate(loop_rate)
t0 = time.time()
# Initialize the PID controller
longitudinal_control = PID(kp=30, ki=3, kd=0)
maxspeed = 1620
minspeed = 1400
while not rospy.is_shutdown():
try:
# acceleration calculated from PID controller
motor_pwm = longitudinal_control.acc_calculate(
v_ref, v_meas) + motor_pwm_offset
if (motor_pwm < minspeed):
motor_pwm = minspeed
elif (motor_pwm > maxspeed):
motor_pwm = maxspeed
if (move == False):
motor_pwm = 1500
servo_pwm = 1550
# publish information
pubname.publish(ECU(motor_pwm, servo_pwm))
# wait
rate.sleep()
except:
pass
def move(current):
msg = ECU()
msg.motor = 1500
msg.servo = 1500
if 'a' in current:
msg.servo -= 300
elif 'd' in current:
msg.servo += 300
if 'w' in current:
msg.motor += 100
elif 's' in current:
msg.motor -= 200
return msg
def main_auto():
# initialize ROS node
init_node('auto_mode', anonymous=True)
nh = Publisher('ecu', ECU, queue_size=10)
# set node rate
rateHz = get_param("controller/rate")
rate = Rate(rateHz)
dt = 1.0 / rateHz
t_i = 0
# get experiment parameters
t_0 = get_param("controller/t_0") # time to start test
t_f = get_param("controller/t_f") # time to end test
FxR_target = get_param("controller/FxR_target")
d_f_target = pi / 180 * get_param("controller/d_f_target")
# main loop
while not is_shutdown():
# get command signal
(FxR, d_f) = circular(t_i, t_0, t_f, d_f_target, FxR_target)
# send command signal
ecu_cmd = ECU(FxR, d_f)
nh.publish(ecu_cmd)
# wait
t_i += dt
rate.sleep()
def controller():
# initialize node
rospy.init_node('vehicle_simulator', anonymous=True)
# topic subscriptions / publications
rospy.Subscriber('state_vehicle_simulator', Z_DynBkMdl, measurements_callback)
rospy.Subscriber('state_vehicle_error_frame_simulator', Z_DynBkMdlErrorFrame, measurements_error_frame_callback)
state_pub = rospy.Publisher('ecu', ECU, queue_size = 10)
# set node rate
loop_rate = 50
dt = 1.0 / loop_rate
rate = rospy.Rate(loop_rate)
t0 = time.time()
# set initial conditions
d_f = 0
acc = 0
while not rospy.is_shutdown():
# publish state estimate
acc = (1 - v_x)
d_f = 0.1*(0 - ey) + (0 - epsi)
# publish information
state_pub.publish( ECU(acc, d_f) )
# wait
rate.sleep()
def controller():
global motor_pwm, servo_pwm, motor_pwm_offset
global v_ref, v_meas
# Initialize node:
rospy.init_node('simulationGain', anonymous=True)
# TODO: Add your necessary topic subscriptions / publications, depending on your preferred method of velocity estimation
ecu_pub = rospy.Publisher('ecu_pwm', ECU, queue_size = 10)
# Set node rate
loop_rate = 50
rate = rospy.Rate(loop_rate)
# TODO: Initialize your PID controller here, with your chosen PI gains
PID_control = PID(kp = 1, ki = 1, kd = 0)
while not rospy.is_shutdown():
# calculate acceleration from PID controller.
motor_pwm = PID_control.acc_calculate(v_ref, v_meas) + motor_pwm_offset
# publish control command
ecu_pub.publish( ECU(motor_pwm, servo_pwm) )
# wait
rate.sleep()
def arduino_interface():
global ecu_pub, motor_pwm, servo_pwm
init_node('arduino_interface')
# set node rate
loop_rate = 50
dt = 1.0 / loop_rate
rate = rospy.Rate(loop_rate)
time_prev = time.time()
ecu_pub = Publisher('ecu_pwm', ECU, queue_size=10)
while not rospy.is_shutdown():
if time.time() >= time_prev and time.time() < time_prev + 4:
motor_pwm = 1580.0
if time.time() >= time_prev + 4 and time.time() < time_prev + 6:
motor_pwm = 1620.0
if time.time() >= time_prev + 6 and time.time() < time_prev + 8:
motor_pwm = 1600.0
if time.time() >= time_prev + 8 and time.time() < time_prev + 10:
motor_pwm = 1500.0
if time.time() >= time_prev + 10:
break
ecu_cmd = ECU(motor_pwm, servo_pwm)
ecu_pub.publish(ecu_cmd)
# wait
rate.sleep()
def controller():
global motor_pwm, servo_pwm, motor_pwm_offset
global v_ref, v_meas
# initialize node:
rospy.init_node('simulationGain', anonymous=True)
# topic subscriptions / publications
rospy.Subscriber('encoder', Encoder, enc_callback)
ecu_pub = rospy.Publisher('ecu_pwm', ECU, queue_size = 10)
# set node rate
loop_rate = 50
rate = rospy.Rate(loop_rate)
# Initialize the PID controller
PID_control = PID(kp=200, ki=0, kd=0.0)
while not rospy.is_shutdown():
# acceleration calculated from PID controller.
motor_pwm = PID_control.acc_calculate(v_ref, v_meas) + motor_pwm_offset
rospy.logwarn("pwm = {}".format(motor_pwm))
# publish information
ecu_pub.publish( ECU(motor_pwm, servo_pwm) )
# wait
rate.sleep()
def controller():
global motor_pwm, servo_pwm, motor_pwm_offset, servo_pwm_offset
global v_ref, v_meas, t_start
# initialize node
rospy.init_node('CorneringStiffnessTest', anonymous=True)
# topic subscriptions / publications
rospy.Subscriber('encoder', Encoder, enc_callback)
ecu_pub = rospy.Publisher('ecu_pwm', ECU, queue_size=10)
# set node rate
loop_rate = 50
rate = rospy.Rate(loop_rate)
# Initialize the PID controller
PID_control = PID(kp=55.0, ki=20.0, kd=0.0)
while not rospy.is_shutdown():
# acceleration calculated from PID controller.
motor_pwm = PID_control.acc_calculate(v_ref, v_meas) + motor_pwm_offset
# rospy.logwarn("pwm = {}".format(motor_pwm))
t_now = time.time()
servo_pwm = 200 * np.sin(pi * (t_now - t_start)) + servo_pwm_offset
rospy.logwarn("pwm = {}".format(servo_pwm))
# publish information
ecu_pub.publish(ECU(motor_pwm, servo_pwm))
# wait
rate.sleep()
def controller():
global motor_pwm, servo_pwm, motor_pwm_offset, servo_pwm_offset
global v_ref, v_meas
global lateral_error, x_est, y_est
# initialize node:
rospy.init_node('lateral_control', anonymous=True)
# topic subscriptions / publications
rospy.Subscriber('encoder', Encoder, enc_callback)
rospy.Subscriber('state_estimate', Z_KinBkMdl, state_callback)
ecu_pub = rospy.Publisher('ecu_pwm', ECU, queue_size = 10)
# set node rate
loop_rate = 50
rate = rospy.Rate(loop_rate)
# Initialize the PID controller
longitudinal_control = PID(kp=200, ki=0, kd=0.0)
lateral_control = PID(kp=50, ki=0, kd=0.0)
while not rospy.is_shutdown():
# acceleration calculated from PID controller.
motor_pwm = longitudinal_control.acc_calculate(v_ref, v_meas) + motor_pwm_offset
servo_pwm = lateral_control.acc_calculate(0, lateral_error) + servo_pwm_offset
rospy.logwarn("pwm = {}".format(motor_pwm))
# publish information
ecu_pub.publish( ECU(motor_pwm, servo_pwm) )
# wait
rate.sleep()
def arduino_interface():
global ecu_pub, motor_pwm, servo_pwm
# initialize node
init_node('arduino_interface')
# setup publishers and subscribers
loop_rate = 50
dt = 1.0 / loop_rate
rate = rospy.Rate(loop_rate)
time_prev = time.time()
ecu_pub = Publisher('ecu_pwm', ECU, queue_size=10)
v_meas_pub = rospy.Publisher('v_meas', Float32, queue_size=10)
rospy.Subscriber('encoder', Encoder, enc_callback)
while not rospy.is_shutdown():
if time.time() >= time_prev and time.time() < time_prev + 4:
motor_pwm = 1650
if time.time() >= time_prev + 4:
motor_pwm = 1500
ecu_cmd = ECU(motor_pwm, servo_pwm)
ecu_pub.publish(ecu_cmd)
v_meas_pub.publish(Float32(v_meas))
# wait
rate.sleep()
def callback(uOptdata):
pub = rospy.Publisher('ecu', ECU, queue_size=10)
servo = (uOptdata.delta - 71.0977) / -0.0461
#motor= 1550
motor = 1550
out = ECU(motor, servo)
pub.publish(out)
def main_auto():
# initialize ROS node
rospy.init_node('auto_mode', anonymous=True)
nh = rospy.Publisher('ecu', ECU, queue_size=10)
steering_offset_subscriber = rospy.Subscriber("lane_offset", Float32,
offset_callback)
# set node rate
rateHz = 50
rate = rospy.Rate(rateHz)
# set start time
t0 = time.time()
# specify test and test options
v_x_pwm_1 = rospy.get_param("steering_controller/v_x_pwm_1")
v_x_pwm_2 = rospy.get_param("steering_controller/v_x_pwm_2")
tf = rospy.get_param("steering_controller/tf")
# Get Controller Parameters
Kp = rospy.get_param("steering_controller/Kp")
Kd = rospy.get_param("steering_controller/Kd")
Ki = rospy.get_param("steering_controller/Ki")
# main loop
while not rospy.is_shutdown():
# get command signal
servoCMD, steering_angle = steering_command(rateHz, Kp, Kd, Ki)
v_x_pwm = speed_command(t0, tf, v_x_pwm_1, v_x_pwm_2)
# send command signal
ecu_cmd = ECU(v_x_pwm, servoCMD)
nh.publish(ecu_cmd)
# wait
rate.sleep()
def main_auto():
global nh, v_x_curr, max_torque
# initialize ROS node
rospy.init_node('auto_mode', anonymous=True)
nh = rospy.Publisher('ecu', ECU, queue_size = 10)
steering_offset_subscriber = rospy.Subscriber("lane_offset", Float32, offset_callback)
rospy.Subscriber('encoder', Encoder, enc_callback)
#XXX:DATALOGGING
torque_pub = rospy.Publisher('torque', Float32, queue_size = 10)
steering_angle_pub = rospy.Publisher('steering_angle', Float32, queue_size = 10)
speed_pub = rospy.Publisher('speed', Float32, queue_size = 10)
# set node rate
rateHz = 50
rate = rospy.Rate(rateHz)
# Get Desired Speed
speed_desired = rospy.get_param("speed_controller/speed_desired")
max_torque = rospy.get_param("speed_controller/max_torque")
# Get Speed Controller Parameters
KpS = rospy.get_param("speed_controller/KpS")
KdS = rospy.get_param("speed_controller/KdS")
KiS = rospy.get_param('speed_controller/KiS')
# Get Turning Controller Parameters
Kp = rospy.get_param("speed_controller/Kp")
Kd = rospy.get_param("speed_controller/Kd")
Ki = rospy.get_param("speed_controller/Ki")
# get test time
tf = rospy.get_param('speed_controller/test_time')
# set start time
t0 = time.time()
# main loop
while not rospy.is_shutdown():
# get speed command signal
speedCMD = speed_command(speed_desired, rateHz, KpS, KdS, KiS)
if ((time.time()-t0)>tf):
speedCMD = 40
# get steering command signal
servoCMD, angle = steering_command(rateHz, Kp, Kd, Ki)
# send command signal
ecu_cmd = ECU(speedCMD, servoCMD)
nh.publish(ecu_cmd)
#XXX:DATALOGGING
torque_pub.publish(speedCMD)
steering_angle_pub.publish(angle)
speed_pub.publish(v_x_curr)
# wait
rate.sleep()
def controller():
global motor_pwm, servo_pwm, motor_pwm_offset
global v_ref, v_meas
# Initialize node:
rospy.init_node('simulationGain', anonymous=True)
# topic subscriptions / publications
rospy.Subscriber('encoder', Encoder, enc_callback)
# TODO: Add your necessary topic subscriptions / publications, depending on your preferred method of velocity estimation
ecu_pub = rospy.Publisher('ecu_pwm', ECU, queue_size=10)
velarr_pub = rospy.Publisher('velarr', ECU, queue_size=10)
# Set node rate
loop_rate = 50
rate = rospy.Rate(loop_rate)
# TODO: Initialize your PID controller here, with your chosen PI gains
PID_control = PID(kp=20, ki=5, kd=0)
while not rospy.is_shutdown():
#rospy.sleep(2)
# calculate acceleration from PID controller.
arr_vel.append(v_meas)
arr_vel.pop(0)
v_meas = sum(arr_vel) / len(arr_vel)
motor_pwm = PID_control.acc_calculate(v_ref, v_meas) + motor_pwm_offset
if motor_pwm > 1650:
motor_pwm = 1650
arr_pwm.append(motor_pwm)
arr_pwm.pop(0)
motor_pwm = sum(arr_pwm) / len(arr_pwm)
# publish information
# publish control command
ecu_pub.publish(ECU(motor_pwm, servo_pwm))
velarr_pub.publish(ECU(v_meas, motor_pwm))
# wait
rate.sleep()
class low_level_control(object):
motor_pwm = 90
servo_pwm = 90
str_ang_max = 35
str_ang_min = -35
ecu_pub = 0
ecu_cmd = ECU()
def pwm_converter_callback(self, msg):
# translate from SI units in vehicle model
# to pwm angle units (i.e. to send command signal to actuators)
# convert desired steering angle to degrees, saturate based on input limits
# Old servo control:
# self.servo_pwm = 91.365 + 105.6*float(msg.servo)
# New servo Control
# if msg.servo < 0.0: # right curve
# self.servo_pwm = 95.5 + 118.8*float(msg.servo)
# elif msg.servo > 0.0: # left curve
# self.servo_pwm = 90.8 + 78.9*float(msg.servo)
self.servo_pwm = 90.0 + 89.0 * float(msg.servo)
# compute motor command
FxR = float(msg.motor)
if FxR == 0:
self.motor_pwm = 90.0
elif FxR > 0:
#self.motor_pwm = max(94,91 + 6.5*FxR) # using writeMicroseconds() in Arduino
self.motor_pwm = 91 + 6.5 * FxR # using writeMicroseconds() in Arduino
# self.motor_pwm = max(94,90.74 + 6.17*FxR)
#self.motor_pwm = 90.74 + 6.17*FxR
#self.motor_pwm = max(94,90.12 + 5.24*FxR)
#self.motor_pwm = 90.12 + 5.24*FxR
# Note: Barc doesn't move for u_pwm < 93
else: # motor break / slow down
self.motor_pwm = 93.5 + 46.73 * FxR
# self.motor_pwm = 98.65 + 67.11*FxR
#self.motor = 69.95 + 68.49*FxR
self.update_arduino()
def neutralize(self):
self.motor_pwm = 40 # slow down first
self.servo_pwm = 90
self.update_arduino()
rospy.sleep(1) # slow down for 1 sec
self.motor_pwm = 90
self.update_arduino()
def update_arduino(self):
self.ecu_cmd.header.stamp = get_rostime()
self.ecu_cmd.motor = self.motor_pwm
self.ecu_cmd.servo = self.servo_pwm
self.ecu_pub.publish(self.ecu_cmd)
def _publish_ecu(self):
# assemble the message to publish
# acceleration command from keyboard
acc_cmd = motor
# steering angle command from keyboard
d_f_cmd = servo
ecu_out = ECU(acc_cmd, d_f_cmd)
self.pub.publish(ecu_out)
def main_auto():
global nh, v_x_enc
# initialize ROS node
rospy.init_node('auto_mode', anonymous=True)
rospy.on_shutdown(shutdown_func)
nh = rospy.Publisher('ecu', ECU, queue_size=10)
steering_offset_subscriber = rospy.Subscriber("lane_offset", Float32,
offset_callback)
rospy.Subscriber('encoder', Encoder, enc_callback)
rospy.Subscriber('cv_abort', Int32, cv_fail_callback)
#XXX:DATALOGGING
torque_pub = rospy.Publisher('torque', Float32, queue_size=10)
steering_angle_pub = rospy.Publisher('steering_angle',
Float32,
queue_size=10)
speed_pub = rospy.Publisher('speed', Float32, queue_size=10)
# set node rate
rateHz = 50
rate = rospy.Rate(rateHz)
# Get Desired Speed
speed_desired = rospy.get_param("speed_controller/speed_desired")
# Get Speed Controller Parameters
KpS = rospy.get_param("speed_controller/KpS")
KdS = rospy.get_param("speed_controller/KdS")
KiS = rospy.get_param('speed_controller/KiS')
# Get Turning Controller Parameters
Kp = rospy.get_param("speed_controller/Kp")
Kd = rospy.get_param("speed_controller/Kd")
Ki = rospy.get_param("speed_controller/Ki")
# main loop
while not rospy.is_shutdown():
# get command signal
speedCMD = speed_command(speed_desired, rateHz, KpS, KdS, KiS)
servoCMD, angle = steering_command(rateHz, Kp, Kd, Ki)
# send command signal
ecu_cmd = ECU(speedCMD, servoCMD)
nh.publish(ecu_cmd)
#XXX:DATALOGGING
torque_pub.publish(speedCMD)
steering_angle_pub.publish(angle)
speed_pub.publish(v_x_enc)
# wait
rate.sleep()
def main_auto():
# initialize ROS node
rospy.init_node('auto_mode', anonymous=True)
nh = rospy.Publisher('ecu', ECU, queue_size=10)
rospy.Subscriber('imu', TimeData, imu_callback)
# set node rate
rateHz = 50
dt = 1.0 / rateHz
rate = rospy.Rate(rateHz)
t_i = 0
# specify test and test options
experiment_opt = {
0: CircularTest,
1: Straight,
2: SineSweep,
3: DoubleLaneChange,
4: CoastDown
}
experiment_sel = rospy.get_param("controller/experiment_sel")
v_x_pwm = rospy.get_param("controller/v_x_pwm")
t_0 = rospy.get_param("controller/t_0")
t_exp = rospy.get_param("controller/t_exp")
str_ang = rospy.get_param("controller/steering_angle")
test_mode = experiment_opt.get(experiment_sel)
opt = TestSettings(SPD=v_x_pwm, turn=str_ang, dt=t_exp)
opt.t_0 = t_0
# use simple pid control to keep steering straight
p = rospy.get_param("controller/p")
i = rospy.get_param("controller/i")
d = rospy.get_param("controller/d")
pid = PID(P=p, I=i, D=d)
# main loop
while not rospy.is_shutdown():
# get steering wheel command
u = pid.update(err, dt)
servoCMD = angle_2_servo(u)
# get command signal
(motorCMD, _) = test_mode(opt, rateHz, t_i)
# send command signal
ecu_cmd = ECU(motorCMD, servoCMD)
nh.publish(ecu_cmd)
# wait
t_i += 1
rate.sleep()
def __init__(self, namespace, plotter_params, visualizer_params):
"""Initialization
Arguments:
"""
plot_sim = visualizer_params.plot_sim
plot_est = visualizer_params.plot_est
plot_sensor = visualizer_params.plot_sensor
plot_gps = visualizer_params.plot_gps
plot_state = plotter_params.plot_state
plot_score = plotter_params.plot_score
plot_pred = plotter_params.plot_pred
plot_ss = plotter_params.plot_ss
if plot_sim:
rospy.Subscriber(namespace + '/sim_states', States,
self.simState_callback)
if plot_sensor:
rospy.Subscriber(namespace + '/mea_states', States,
self.meaState_callback)
if plot_est:
rospy.Subscriber(namespace + '/est_states', States,
self.estState_callback)
if plot_pred:
rospy.Subscriber(namespace + '/pred_states', Prediction,
self.predState_callback)
if plot_ss:
rospy.Subscriber(namespace + '/sel_ss', Prediction,
self.SS_callback)
if plot_gps:
rospy.Subscriber(namespace + '/gps_pos', States, self.gps_callback)
if plot_state:
rospy.Subscriber(namespace + '/fsm_state', FSMState,
self.fsm_state_callback)
if plot_score:
rospy.Subscriber(namespace + '/strategy_scores', Scores,
self.scores_callback)
rospy.Subscriber(namespace + '/ecu', ECU, self.ecu_callback)
self.sim_states = States()
self.mea_states = States()
self.est_states = States()
self.pred_states = Prediction()
self.ss_states = Prediction()
self.gps = States()
self.input = ECU()
self.fsm_state = 'Start'
self.scores = [0, 0, 0]
def image_node():
global error
error = 0
param_names = [
'lowerY', 'upperY', 'display_image', 'display_processed_image',
'debug_info', 'publish_processed_image'
]
params = {}
for param in param_names:
params[param] = rospy.get_param(param)
print(params)
rospy.init_node('Test_Imager')
img_pub = rospy.Publisher('/processed_image', Image, queue_size=1)
#params['img_pub'] = img_pub
rospy.Subscriber('/image_raw/compressed', CompressedImage, image_process,
params)
pub = rospy.Publisher('ecu_pwm', ECU, queue_size=10)
rate = rospy.Rate(10)
K = 400
Ki = 20
integral = 0
while not rospy.is_shutdown():
msg = ECU()
# if params['debug_info']:
# print(error)
msg.motor = 1590
msg.servo = 1500 + K * error # Ki*integral
pub.publish(msg)
integral += (1 / 10) * error
if integral > 20:
integral = 20
if integral < -20:
integral = -20
rate.sleep()
def callback_data(self, data):
global ang_o
global ang_n
ang_o = self.angle
# Do all computation from the incoming message to the output message here
self.angle = data.angle
self.displacement = data.horizontal_displacement
if self.angle > 0:
self.angle = self.angle - pi
ang_ctrl = Kp_angle * (self.angle) + Kp_disp * (self.displacement)
self.publisher.publish(ECU(6.5, ang_ctrl + pi))
def keyRelay():
rospy.init_node('keyRelay')
rate = rospy.Rate(50)
state_pub = rospy.Publisher('ecu', ECU, queue_size = 10)
acc = 0
d_f = 0
while not rospy.is_shutdown():
key = getKey()
print(key)
acc_d, d_f_d = keyToData(key)
acc = acc + acc_d
d_f = d_f + d_f_d
state_pub.publish( ECU(acc, d_f) )
rate.sleep()
def main_auto():
global read_yaw0, yaw_local
# initialize ROS node
init_node('auto_mode', anonymous=True)
ecu_pub = Publisher('ecu', ECU, queue_size=10)
se_sub = Subscriber('imu', Imu, imu_callback)
# set node rate
rateHz = get_param("controller/rate")
rate = Rate(rateHz)
dt = 1.0 / rateHz
# get PID parameters
p = get_param("controller/p")
i = get_param("controller/i")
d = get_param("controller/d")
pid = PID(P=p, I=i, D=d)
setReference = False
# get experiment parameters
t_0 = get_param("controller/t_0") # time to start test
t_f = get_param("controller/t_f") # time to end test
FxR_target = get_param("controller/FxR_target")
t_params = (t_0, t_f, dt)
while not is_shutdown():
# OPEN LOOP
if read_yaw0:
# set reference angle
if not setReference:
pid.setPoint(yaw_local)
setReference = True
t_i = 0.0
print('yaw_local', yaw_local)
# apply open loop command
else:
(FxR, d_f) = straight(t_i, pid, t_params, FxR_target)
ecu_cmd = ECU(FxR, -d_f)
ecu_pub.publish(ecu_cmd)
t_i += dt
# wait
rate.sleep()
def TwistToECU():
# initialize node
rospy.init_node('TwistToECU', anonymous=True)
global pubname, newECU
newECU = ECU()
# topic subscriptions / publications
pubname = rospy.Publisher('ecu', ECU, queue_size=100)
rospy.Subscriber('turtle1/cmd_vel', Twist, callback_function)
# set node rate
loop_rate = 50
ts = 1.0 / loop_rate
rate = rospy.Rate(loop_rate)
t0 = time.time()
rospy.spin()
def controller():
# initialize node
rospy.init_node('vehicle_simulator', anonymous=True)
# topic subscriptions / publications
rospy.Subscriber('z_vhcl', Z_DynBkMdl, measurements_callback)
rospy.Subscriber('ez_vhcl', eZ_DynBkMdl, measurements_error_frame_callback)
state_pub = rospy.Publisher('ecu', ECU, queue_size=10)
# set node rate
loop_rate = 50
dt = 1.0 / loop_rate
rate = rospy.Rate(loop_rate)
t0 = time.time()
# set initial conditions
d_f = 0
acc = 0
# reference speed
v_ref = 8 # reference speed is 8 m/s
# Initialize the PID controller
# =====================================tune the gains for PID controller=================================#
PID_control = PID(kp=1, ki=1, kd=1)
# =======================================================================================================#
while not rospy.is_shutdown():
# acceleration calculated from PID controller.
acc = PID_control.acc_calculate(v_ref, v_x)
# steering angle
d_f = 0.0
# publish information
state_pub.publish(ECU(acc, d_f))
# wait
rate.sleep()
def main_auto():
# initialize ROS node
init_node('auto_mode', anonymous=True)
ecu_pub = Publisher('ecu', ECU, queue_size=10)
# set node rate
rateHz = get_param("controller/rate")
rate = Rate(rateHz)
dt = 1.0 / rateHz
# get experiment parameters
FxR_target = get_param("controller/FxR_target")
while not is_shutdown():
# OPEN LOOP
ecu_cmd = ECU(FxR_target, 0)
ecu_pub.publish(ecu_cmd)
# wait
rate.sleep()