def init():
pub_time = rospy.Publisher('start_time', Float64, queue_size=10)
rospy.init_node('keyboard_input', anonymous=True)
rate = rospy.Rate(20)
msg = Control()
is_auto=1
estop=0
gear=0
speed=0.0 #m/s
steer=0.0 #degree
brake=0
time = rospy.get_param('/time', 0) + 0.3 #seconds
init = rospy.Time.now()
while not rospy.is_shutdown():
cur = rospy.Time.now()
if time!=0 :
t = cur-init
key = getKey()
print("press the key" + key)
if key == 'c':
time = Float64()
time.data = rospy.get_time()
pub_time.publish(time)
def __init__(self):
self.pub_rate = 20.0 #Hz
self.time_revise = 10 #Hz
self.buff_size = 25
self.speed_slope = 0.2 #m/s/conf
self.delta_y_max = 0.3 #m maximum error
self.speed_max = 2.1 #m/s
self.speed_min = 0.5
self.speed_plan = 1.0
self.speed_delta = 0.1
self.p_gain = 0.5
self.emergency_stop_keep = 10 #sec
self.map_resolution = 0.03
self.s_response = 10 #se
self.brake_max = 100
# -------------^CONSTANT^--------------
self.L = 1.54 #m distance between two wheel axis
self.lr = 0.53 #m distance between cm and rear wheel
self.cencorr = -0.04 #m origin distance between cenPoint and sPath
#car coordinate origin will be the geometrical center of ca0r frame, except bumper.
#goalpoint should be cenPoint.y_waypoint + self.cencorr
# ----------------------------------^HARDWARE_CONSTANT^------------------------------
self.control = Control() #my order to the car
self.goalpoint = CenPoint(
) #data of goal point - sPath origin placed in center, cenPath origin placed 770 from front wheel
self.cont_buff = np.empty(self.buff_size,
dtype=(type([Control(), float()
]))) #past buff of the ordering
self.updated_sPath = PathArray()
#time variables _ time when this node get sPath or cenPoint
self.stime = 0
self.ctime = 0
self.sttime = 0
self.estoptime = -10
#-----------------------^DETERMINED^---------------------
self.sPath = PathArray()
self.cPoint = CenPoint()
self.sDelay = 0.0
self.cDelay = 0.0
self.state_buff = np.empty(self.buff_size,
dtype=type([VehicleState(),
float()]))
self.sPath_n = 0
def __init__(self):
# Control constants
self.MAX_speed = 5.5 # [m/s]
self.MIN_speed = 0. # [m/s]
#self.MAX_steer = 2000/float(71) # [degree]
#self.MIN_steer = -2000/float(71) # [degree]
self.MAX_steer = 180.
self.MIN_steer = -180.
self.MAX_brake = 150
self.MIN_brake = 1
# Control states
self.NO_CONTROL = 0
self.EMERGENCY_BRAKE = 1
self.NORMAL_TRACKING = 2
self.ESTIMATE_TRACKING = 3
# Platform constants
self.wheelbase = 1.02 #[m] distance between two wheel axis
self.cm2rear = 0.51 #[m] distance between center of mass and rear axis
# Control variable
self.control = Control()
self.control_mode = self.NO_CONTROL
self.control_write = 0
self.control_count = 0
self.control_buff = []
self.control_time_buff = []
self.control_buff_size = 200
self.look_ahead_distance = 20
self.look_ahead_point = PoseStamped()
# Input manage
self.vehicle_state = Control()
self.latest_generated_path = Path()
self.current_path = Path()
self.update_path = False
self.latest_velocity_level = VelocityLevel()
self.velocity_level = VelocityLevel()
self.update_velocity_level = False
self.curvature = Curvature()
def mainloop():
'''
code for activate and deactivate the node
'''
global veloicty
global steer
# global motion_state
nodename = 'model_estimator'
mainloop.active = True
def signalResponse(data):
mainloop.active
if 'zero_monitor' in data.active_nodes:
if nodename in data.active_nodes:
mainloop.active = True
else:
mainloop.active = False
else:
rospy.signal_shutdown('no monitor')
rospy.Subscriber('/active_nodes', ActiveNode, signalResponse)
'''
...
'''
rospy.Subscriber("/imu_speed", ImuSpeed, icb)
rospy.Subscriber("/curvature", Curvature, ccb)
rospy.Subscriber("/velocity_level", VelocityLevel, vcb)
# rospy.Subscriber("/motion_state", MotionState, mcb)
pub = rospy.Publisher('/calibrated_control', Control, queue_size=10)
rospy.init_node(nodename, anonymous=True)
rate = rospy.Rate(10) # 10hz
control = Control()
control.is_auto = True
control.header.frame_id = 'car_frame'
i = 0
while not rospy.is_shutdown():
control.header.stamp = rospy.Time.now()
control.header.seq = i
i = i + 1
control.speed = velocity
control.steer = steer
#if motion_state == 'HALT':
# control.speed = 0.
if mainloop.active:
pub.publish(control)
rate.sleep()
def mainloop():
'''
code for activate and deactivate the node
'''
nodename = 'path_tracker'
mainloop.active = True
def signalResponse(data):
mainloop.active
if 'zero_monitor' in data.active_nodes:
if nodename in data.active_nodes:
mainloop.active = True
else:
mainloop.active = False
else:
rospy.signal_shutdown('no monitor')
rospy.Subscriber('/active_nodes', ActiveNode, signalResponse)
'''
...
'''
rospy.Subscriber("/velocity_level", VelocityLevel, vcb)
rospy.Subscriber("/path", Path, pcb)
pub = rospy.Publisher('/ideal_control', Control, queue_size=10)
rospy.init_node(nodename, anonymous=True)
rate = rospy.Rate(10) # 10hz
control = Control()
control.header.frame_id = 'car_frame'
i = 0
while not rospy.is_shutdown():
control.header.stamp = rospy.Time.now()
control.header.seq = i
i = i + 1
if mainloop.active:
pub.publish(control)
rate.sleep()
def mainloop():
'''
code for activate and deactivate the node
'''
nodename = 'model_estimator'
mainloop.active = True
def signalResponse(data):
mainloop.active
if 'zero_monitor' in data.active_nodes:
if nodename in data.active_nodes:
mainloop.active = True
else:
mainloop.active = False
else:
rospy.signal_shutdown('no monitor')
rospy.Subscriber('/active_nodes', ActiveNode, signalResponse)
'''
...
'''
rospy.Subscriber("/imu_speed", ImuSpeed, icb)
rospy.Subscriber("/ideal_control", Control, ccb)
pub = rospy.Publisher('/calibrated_control', Control, queue_size=10)
rospy.init_node(nodename, anonymous=True)
rate = rospy.Rate(10) # 10hz
control = Control()
control.header.frame_id = 'car_frame'
i = 0
while not rospy.is_shutdown():
control.header.stamp = rospy.Time.now()
control.header.seq = i
i = i + 1
if mainloop.active:
pub.publish(control)
rate.sleep()
def init():
pub = rospy.Publisher('control', Control, queue_size=10)
rospy.init_node('control_sender', anonymous=True)
rate = rospy.Rate(20)
msg = Control()
is_auto=1
estop=0
gear=0
speed=0 #m/s
steer=0 #degree
brake=1
while not rospy.is_shutdown():
key = getKey()
print("key" + key)
if key == 'q':
break
elif key == 'a':
steer = steer - 1
if steer <= -30 :
steer = -30
elif key == 'd':
steer = steer + 1
if steer >= 30 :
steer = 30
elif key == 'w':
speed = speed + 0.1
if speed >= 3:
speed = 3
elif key == 's':
speed = speed - 0.1
if speed < 0:
speed = 0
elif key == 'e':
if gear == 0:
gear = 2
elif gear == 2:
gear = 0
elif key == 'b':
if brake == 1:
brake = 50
elif brake == 50:
brake = 1
rospy.loginfo(msg)
msg.is_auto = is_auto
msg.estop = estop
msg.gear = gear
msg.brake = brake
msg.speed = round(speed,3)
msg.steer = round(steer,3)
msg.encoder = 100
pub.publish(msg)
rate.sleep()
def init():
pub = rospy.Publisher('/car_signal', Control, queue_size=10)
rospy.init_node('keyboard_input', anonymous=True)
rate = rospy.Rate(20)
msg = Control()
is_auto = 1
estop = 0
gear = 0
speed = rospy.get_param('/speed', 0) #m/s
steer = rospy.get_param('/steer', 0) #degree
brake = 0
time = rospy.get_param('/time', 0) + 0.3 #seconds
init = rospy.Time.now()
while not rospy.is_shutdown():
cur = rospy.Time.now()
if time != 0:
t = cur - init
key = getKey()
print("key" + key)
if key == 'q':
break
elif key == 'a':
steer = steer - 1
if steer <= -28:
steer = -28
elif key == 'd':
steer = steer + 1
if steer >= 28:
steer = 28
elif key == 'w':
speed = speed + 0.1
if speed >= 3:
speed = 3
elif key == 's':
speed = speed - 0.1
if speed < -3:
speed = -3
elif key == 'e':
if gear == 0:
gear = 2
elif gear == 2:
gear = 0
elif key == 'b':
if brake == 1:
brake = 50
elif brake == 50:
brake = 1
rospy.loginfo(msg)
print("steer_degree : ", steer)
msg.is_auto = is_auto
msg.estop = estop
msg.gear = gear
msg.brake = brake
msg.speed = round(speed, 3)
msg.steer = round(steer, 3)
msg.header.stamp = rospy.Time.now()
pub.publish(msg)
rate.sleep()
def main_control_loop(self) :
'''
1. Initialization
'''
self.control = Control()
temp_control_mode = Control_mode.NO_CONTROL
self.control.is_auto = True
self.control.estop = 0
self.control.gear = 0 #0: front 1: neutral 2: rear
self.control.speed = 0
self.control.steer = 0 #in degree
self.control.brake = 1
#TODO: change this to server client later
#uturn = 0
uturn_mode = rospy.get_param('uturn_mode') ###################### added
park_mode = rospy.get_param('park_mode')
current_t = self.current_state.header.stamp.to_sec()
'''
2. Updating the s-path and c-point according to vehicle odometry
'''
#update s-path
#erase s-path to empty object after a certain period of time
#JUNHO: this does not work well in rosbag mdode because current_t is much more current than the stamp
#IMPORTANT: emergency_brake case have move to the top for fast response
if current_t - self.estoptime < self.emergency_stop_keep :
if Z_DEBUG:
print "control mode: emergency braking"
temp_control_mode = Control_mode.EMERGENCY_BRAKE
if temp_control_mode is not Control_mode.EMERGENCY_BRAKE:
if current_t - self.sPath.header.stamp.to_sec() > self.spath_time_threshold:
self.sPath.pathpoints = []
self.updated_sPath.pathpoints = []
# print "path erased"
is_updated_spath= self.update_spath(current_t)
# if Z_DEBUG and not is_updated_spath :
# print "s-path is not updated and is empty"
#update c-point
is_updated_cpoint = self.update_cpoint(current_t)
# if Z_DEBUG and not is_updated_cpoint :
# print "c-point is not updated due to short state buffer"
if park_mode ==2:
is_updated_ppoint = self.update_ppoint(current_t)
elif park_mode ==3:
is_updated_rpath = self.update_rpath(current_t)
'''
3. Deciding the control mode
IMPORTANT: This is the most upper level logic of low-level controller!!
'''
# emergency stop case, emergency stop occurs by the e_stop message
if temp_control_mode == Control_mode.EMERGENCY_BRAKE:
if Z_DEBUG:
print "control mode: emergency braking"
elif uturn_mode >= 1: ##################################### added
if uturn_mode == 1:
if self.uturn_stop_toggle:
self.uturn_stop_start_time = rospy.Time.now().to_sec()
self.uturn_stop_toggle = True
if Z_DEBUG:
print "control mode: u-turning enter, C point tracking with low speed"
temp_control_mode = Control_mode.SLOW_C
elif uturn_mode == 2:
##TODO: upgrade second logic of if statement
# if self.uturn_angle > self.uturn_end and self.cpoint.header.stamp.to_sec() > self.control_time_buff[-1]:
if self.uturn_angle > self.uturn_end:
if Z_DEBUG:
print "control mode: u-turning end, C point tracking start"
uturn_mode = 0
rospy.set_param("uturn_mode", 0)
# self.uturn_angle = 0
temp_control_mode = Control_mode.NORMAL_C
else :
# if Z_DEBUG:
# print "control mode: u-turning"
temp_control_mode = Control_mode.U_TURN
elif park_mode >= 1:
if park_mode ==1:
temp_control_mode = Control_mode.SLOW_C
if self.park_semi_stop_toggle:
self.park_semi_stop_start_time = rospy.Time.now().to_sec()
self.park_semi_stop_toggle = False
elif park_mode ==2:
temp_control_mode = Control_mode.PARK_FORWARD
elif park_mode ==3:
temp_control_mode = Control_mode.PARK_REVERSE
if self.park_stop_toggle:
self.park_stop_start_time = rospy.Time.now().to_sec()
self.park_stop_toggle = False
# normal s path tracking, when the updated_sPath is still long enough to follow, the vehicle follows it.
# This situation occurs during inside missions with rubber cones, and also when the vehicle is exiting the mission.
elif len(self.updated_sPath.pathpoints) > self.spath_length_threshold :
if Z_DEBUG:
print "control mode: Normal S path tracking"
temp_control_mode = Control_mode.NORMAL_S
# c point tracking with low speed
# This situation occurs when sPath is not created but there are obstacles
# Usually occurs when entering into the mission with rubber cones, or when the sPath keeps fail updating during the mission.
elif self.flag_obstacle > 0:
if Z_DEBUG:
print "control mode: C point tracking with low speed, there still are obstacles."
temp_control_mode = Control_mode.SLOW_C
else: #when flag_obstacle == 0
# if Z_DEBUG:
# print "control mode: Normal C point tracking"
temp_control_mode = Control_mode.NORMAL_C
#TODO add cases for uturn and parking control
#TODO add case to deal with when cPoint is also empty
self.control_mode = temp_control_mode
'''
4. Decide steering angle depending on each control mode
1) updtate goalpoint
if control mode is NORMAL_S, decide goalpoint according to sPath_n and updated_sPath
if control mode is NORMAL_C or SLOW_C, decide goalpoint according and updated_cPoint
2) pure pursuit logic
'''
# 1) update goalpoint
# self.show_cPoint()
if self.control_mode == Control_mode.NORMAL_S:
#TODO: activate this
# self.decide_sPath_n() ########################################### added
if len(self.updated_sPath.pathpoints) <= self.sPath_n :
self.goalpoint.x_waypoint = self.map_resolution*(200-self.updated_sPath.pathpoints[0].y)
self.goalpoint.y_waypoint = self.map_resolution*(100-self.updated_sPath.pathpoints[0].x)
else:
self.goalpoint.x_waypoint = self.map_resolution*(200-self.updated_sPath.pathpoints[-self.sPath_n].y)
self.goalpoint.y_waypoint = self.map_resolution*(100-self.updated_sPath.pathpoints[-self.sPath_n].x)
elif self.control_mode == Control_mode.NORMAL_C or self.control_mode == Control_mode.SLOW_C:
self.goalpoint.x_waypoint = self.updated_cPoint.x_waypoint
self.goalpoint.y_waypoint = self.updated_cPoint.y_waypoint
self.goalpoint.confidence = self.updated_cPoint.confidence
elif self.control_mode == Control_mode.PARK_FORWARD:
self.goalpoint.x_waypoint = self.updated_pPoint.goal_point.x
self.goalpoint.y_waypoint = self.updated_pPoint.goal_point.y
# print self.goalpoint
if self.goalpoint.x_waypoint < -0.2:
print "goal point passed"
if self.updated_pPoint.goal_point.x >= -1.0 and ( self.distance_to_point(self.goalpoint) < self.goal_reach_distance or (self.goalpoint.x_waypoint < -0.2 and abs(self.goalpoint.y_waypoint) < 0.5)) :
rospy.set_param('park_mode',3)
# self.make_reverse_path()
elif self.control_mode == Control_mode.PARK_REVERSE:
if current_t - self.park_reverse_start_time > self.park_reverse_duration and self.park_reverse_start_time is not 0:
print "park reverse count finish"
rospy.set_param('park_mode', 0)
# in other control_mode, goalpoint do not need to be set again because the control logic does not depend on the goalpoint.
# 2) deciding PURE PURSUIT OUTPUT based on goalpoint
if self.control_mode == Control_mode.EMERGENCY_BRAKE:
delta = 0
elif self.control_mode == Control_mode.NORMAL_S or self.control_mode == Control_mode.NORMAL_C or self.control_mode == Control_mode.SLOW_C or self.control_mode == Control_mode.PARK_FORWARD:
ld = math.sqrt(self.goalpoint.x_waypoint*self.goalpoint.x_waypoint + self.goalpoint.y_waypoint*self.goalpoint.y_waypoint)
if ld < 0.2:
delta = 0
else:
delta = -np.arctan(2 * self.L * (self.goalpoint.y_waypoint / ld) / (ld + 2 * self.lr * self.goalpoint.x_waypoint/ld)) # ld is lookahead distance
self.control.steer = delta / np.pi * 180 #in degree
# print "first steer", self.control.steer
#################### added
elif self.control_mode == Control_mode.U_TURN :
self.control.steer = -self.steer_max
#################### added
elif self.control_mode == Control_mode.PARK_REVERSE:
self.control.steer = self.steer_max/2
else:
self.control.steer = 0.0
#TODO: set steer for u-turn and reverse
if self.control_mode == Control_mode.PARK_REVERSE:
self.control.gear = 2
else:
self.control.gear = 0
'''
5. Decide speed depending on each control mode
'''
if self.control_mode == Control_mode.EMERGENCY_BRAKE:
self.control.speed = 0
self.control.brake = self.brake_max
elif park_mode == 1:
if current_t - self.park_semi_stop_start_time <= self.park_semi_stop_duration:
self.control.speed = 0
self.control.brake = self.brake_max
elif self.park_semi_stop_start_time is not 0:
self.control.speed = self.speed_min
elif self.control_mode == Control_mode.NORMAL_C:
self.control.speed = self.goalpoint.confidence * self.speed_slope
# print(self.goalpoint.confidence)
elif self.control_mode == Control_mode.NO_CONTROL:
self.control.speed = 0
#TODO: set speed for u-turn and reverse
elif self.control_mode == Control_mode.U_TURN:
if current_t - self.uturn_stop_start_time <= self.uturn_stop_duration:
self.control.speed = 0
self.control.brake = self.brake_max
# elif self.uturn_stop_start_time is not 0:
else:
self.control.speed = self.speed_uturn
elif self.control_mode == Control_mode.PARK_REVERSE:
if current_t - self.park_stop_start_time <= self.park_stop_duration:
self.control.speed = 0
self.control.brake = self.brake_max
elif self.park_stop_start_time is not 0:
if self.park_reverse_toggle:
self.park_reverse_start_time = rospy.Time.now().to_sec()
self.park_reverse_toggle = False
self.control.speed = self.speed_min
else:
self.control.speed = self.speed_min
'''
6. Additional Post processing on Control commands
1) Add P gains to Steer and Speed command and Decide brake
2) Apply low pass filter
3) Updtate Control buffer
'''
#final decision making for control variables (adding p gains and decide brake value)
self.update_con_pid()
#applying low pass filter to finally decided control variable
self.update_lpf()
#updating control buffer
# if Z_DEBUG:
# print "length of control time buffer is ", len(self.control_time_buff)
if len(self.control_buff) >= self.control_buff_size:
self.control_buff[0:-1]=self.control_buff[1:]
self.control_buff[-1] = self.control
self.control_time_buff[0:-1] = self.control_time_buff[1:]
self.control_time_buff[-1] = rospy.Time.now().to_sec()
else:
self.control_buff.append(self.control)
self.control_time_buff = np.append(self.control_time_buff, rospy.Time.now().to_sec())
self.control.control_mode = self.control_mode
self.control_count +=1
rospy.set_param('control_count',self.control_count)
return self.control #need to publish self.control as control message
def __init__ (self) :
# -------------^CONSTANT^--------------
self.pub_rate = 20.0 #Hz #IMPORTANT!!! Main control rate
self.access_wait_rate = 500 #Hz
self.speed_max = 2.2 #m/s
# self.speed_max = 2.0
self.speed_min = 0.45
self.speed_slope = self.speed_max / 10 #m/s/conf
self.path_error_max = 0.3 #m maximum error ############################# added : name
self.speed_uturn = 0.4
self.speed_delta = 0.1
self.p_gain = 0.3
self.steer_p_final = 1.5
self.steer_p_final_max = 1.8
self.emergency_stop_keep = 3 #sec #time keeping for emergency stop after receiving the message
self.map_resolution = 0.03
self.map_height = 200
self.map_width = 200
self.brake_max = 200 #this is for emergency braking(maximum braking)
self.brake_adjust_max = 50 #this is for normal velocity adjustment
self.brake_slope = 50
self.lpf_dt_cutoff = 10
self.steer_max = 28.1 #degree, this is for uturning ################### added
# self.uturn_end = 100
self.uturn_end = 280 #degree, turn 180 degree in uturning ####################### added
self.goal_reach_distance = 0.5
#JUNHO
self.spath_length_threshold = 20 #TODO Tune this
self.spath_time_threshold = 15 #sec
# if Z_DEBUG:
# self.spath_time_threshold =1
# ----------------------------------^HARDWARE_CONSTANT^------------------------------
self.L = 1.02 #m distance between two wheel axis
self.lr = 0.51 #m distance between cm and rear wheel
# JUNHO ??
self.cencorr = -0.04 #m origin distance between cenPoint and sPath
#car coordinate origin will be the geometrical center of car frame, except bumper.
#goalpoint should be cenPoint.y_waypoint + self.cencorr
self.control = Control() #my order to the car
self.control_mode = Control_mode.NO_CONTROL
self.goalpoint = CenPoint() #data of goal point - sPath origin placed in center, cenPath origin placed 770 from front wheel
self.sPath = PathArray()
self.cPoint = CenPoint()
self.pPoint = ParkPoints() #for parking mission
self.updated_sPath = PathArray()
self.updated_cPoint = CenPoint()
self.updated_pPoint = ParkPoints() #for parking mission
self.updated_pPoint.goal_point.x = -1.0 #needed for safe initialization
self.rPath = PathArray()
self.updated_rPath = PathArray()
self.uturn_angle = 0 ################################ added
self.estoptime = -10
self.accessing_state = 0
self.accessing_sPath = 0
self.writing_state = 0
#-----------------------^DETERMINED^---------------------
self.flag_obstacle = 0
self.current_state = VehicleState()
self.state_buff = []
self.state_time_buff = np.empty(0)
self.state_buff_park = []
self.state_time_buff_park = np.empty(0)
self.control_buff = []
self.control_time_buff = np.empty(0)
self.state_buff_size = 600
self.control_buff_size = 600 #buff_sizes are only used when updating the buffer
self.control_count = 0
# self.park_stop_count = 12*self.pub_rate
# self.park_reverse_count = 13*self.pub_rate
# self.uturn_stop_count = 4*self.pub_rate
# self.park_semi_stop_count = 4*self.pub_rate
self.park_stop_duration = 12
self.park_reverse_duration = 14
self.uturn_stop_duration = 2
self.park_semi_stop_duration = 2
self.park_stop_start_time= 0
self.park_reverse_start_time =0
self.uturn_stop_start_time= 0
self.park_semi_stop_start_time= 0
self.park_stop_toggle = True
self.park_reverse_toggle = True
self.uturn_stop_toggle = True
self.park_semi_stop_toggle = True
self.sPath_n = 10
self.occupancy_map = np.zeros((self.map_height,self.map_width), dtype='uint8')
def calc_get_cont(self):
self.control = Control()
self.control.is_auto = True
self.control.estop = 0
self.control.gear = 0
self.control.speed = 0
self.control.steer = 0 #in degree
self.control.brake = 1
pttype = 'n'
t = rospy.Time.now().to_sec()
if t - self.estoptime < 10:
print "estop"
self.control.speed = 0
self.control.brake = self.brake_max
self.cont_buff[0:-1] = self.cont_buff[1:self.buff_size]
self.cont_buff[self.buff_size -
1] = [self.control,
rospy.Time.now().to_sec()]
return self.control
if self.stime > 0 and len(self.sPath.pathpoints) and pttype == 'n' > 0:
while True:
self.updated_sPath = PathArray()
st = self.stime
for i in self.sPath.pathpoints:
self.goalpoint.x_waypoint = self.map_resolution * (
200 - i.y) #0.3m per pixel
self.goalpoint.y_waypoint = self.map_resolution * (100 -
i.x)
self.update_goalpoint(st, t)
v = Vector3()
v.z = 0
v.y = 200 - self.goalpoint.x_waypoint / self.map_resolution
v.x = 100 - self.goalpoint.y_waypoint / self.map_resolution
self.updated_sPath.pathpoints.append(v)
self.updated_sPath.header.stamp = self.sPath.header.stamp
#if self.decide_sPath_n(self.speed_plan) > self.delta_y_max :
# while self.decide_sPath_n(self.speed_plan) > self.delta_y_max :
# self.speed_plan -= self.speed_delta
# if self.speed_plan > self.speed_min :
# print "speed should be much slower"
# break
# elif self.decide_sPath_n(self.speed_plan + self.speed_delta) < self.delta_y_max :
# self.speed_plan += self.speed_delta
# else :
# self.decide_sPath_n(self.speed_plan)
self.sPath_n = 20 #############################
self.control.speed = self.speed_min
self.goalpoint.x_waypoint = self.map_resolution * (
200 - self.updated_sPath.pathpoints[self.sPath_n].y)
self.goalpoint.y_waypoint = self.map_resolution * (
100 - self.updated_sPath.pathpoints[self.sPath_n].x)
if rospy.Time.now().to_sec() - self.stime < self.s_response:
if self.stime == st:
print "publish based on sPath"
pttype = 's'
break
else:
break
print "Time Problem occured."
rtmp = rospy.Rate(main_track.time_revise)
rtmp.sleep()
if self.ctime > 0 and pttype == 'n':
print "publish based on cPath"
pttype = 'c'
self.goalpoint.x_waypoint = self.cPoint.x_waypoint - self.cencorr
self.goalpoint.y_waypoint = self.cPoint.y_waypoint
self.goalpoint.confidence = self.cPoint.confidence
self.update_goalpoint(self.ctime, t)
v = Vector3()
v.z = 0
v.y = 200 - self.goalpoint.x_waypoint / self.map_resolution
v.x = 100 - self.goalpoint.y_waypoint / self.map_resolution
self.updated_sPath.pathpoints.append(v)
tmpspd = self.goalpoint.confidence * self.speed_slope
if tmpspd > self.speed_plan:
self.speed_plan += self.speed_delta
elif tmpspd < self.speed_plan:
self.speed_plan -= self.speed_delta
if pttype == 'n':
print "no input"
return self.control
ld = math.sqrt(self.goalpoint.x_waypoint * self.goalpoint.x_waypoint +
self.goalpoint.y_waypoint * self.goalpoint.y_waypoint)
delta = -np.arctan(2 * self.L * self.goalpoint.y_waypoint / ld /
(ld + 2 * self.lr * self.goalpoint.x_waypoint / ld)
) # ld is lookahead distance
self.control.speed = self.speed_plan
self.control.steer = 1 * delta / np.pi * 180 #in degree
self.cont_buff[0:-1] = self.cont_buff[1:self.buff_size]
self.cont_buff[self.buff_size -
1] = [self.control,
rospy.Time.now().to_sec()]
self.update_con_pid()
return self.control #need to update self.control
def main_control_loop(self):
'''
1. Initialization
'''
self.control = Control()
# Update velocity level
if self.update_velocity_level == True:
self.velocity_level = copy.deepcopy(self.latest_velocity_level)
else:
pass
self.update_velocity_level = False
# Initialize contorl variables
self.control.is_auto = False
self.control.estop = False
self.control.gear = self.gear_level
self.control.speed = self.velocity_level.velocity_level
self.control.steer = 0
self.control.brake = self.MAX_brake
temp_control_mode = self.NO_CONTROL
initialize_time = self.vehicle_state.header.stamp.to_sec()
'''
2. Checking E-stop and Path update
1) Check E-stop condition. If E-stop all control variable would be controlled for E-stop (Q: What is E-stop condition?)
2) If it is okay, then path should be updated.
2-1) Update path from planner
2-2) Or estimate new path by transforming latest path
'''
if self.update_path == True:
temp_control_mode = self.Path_update()
else:
if len(self.control_buff) <= 0:
temp_control_mode = self.EMERGENCY_BRAKE
else:
temp_control_mode = self.Path_estimate(initialize_time)
self.update_path = False
'''
3. Deciding new curvature
1) By path and control mode, we calculate appropriate steering angle.
'''
if temp_control_mode == self.EMERGENCY_BRAKE:
pass
else:
temp_control_mode = self.Set_look_ahead_point(temp_control_mode)
temp_control_mode = self.Deicide_curvature(temp_control_mode)
'''
4. Post process of main_control_loop
1) Save new control variable values at the buffer with current time.
2) Return new curvature.
'''
self.Control_post_processing(temp_control_mode)
if len(self.control_buff) >= self.control_buff_size:
self.control_buff[0:-1] = self.control_buff[1:]
self.control_buff[-1] = self.control
self.control_time_buff[0:-1] = self.control_time_buff[1:]
self.control_time_buff[-1] = rospy.get_rostime().to_sec()
else:
self.control_buff.append(self.control)
self.control_time_buff = np.append(self.control_time_buff,
rospy.get_time())
self.control_count += 1
print("One main tracking cycle is compeleted.")
return self.curvature