def __init__(self, city_name):
self.timer = Timer()
# Map setup
self._map = CarlaMap(city_name)
self._centerlines = Centerlines(city_name)
# Agent Setup
Agent.__init__(self)
self._neural_net = CAL_network()
self._seq_len = 5 #self._neural_net.model.max_input_shape
self._state = VehicleState()
self._agents_present = False
# Controller setup
param_path = os.path.dirname(__file__) + '/controller/params/'
cruise_params = get_params_from_txt(param_path + 'cruise_params.txt')
self._PID_cruise = PID(*cruise_params)
follow_params = get_params_from_txt(param_path + 'follow_params.txt')
self._PID_follow = PID(*follow_params)
# General Parameter setup
general_params = get_params_from_txt(param_path + 'general_params.txt')
self.c, self.d = general_params[0], general_params[1]
self.Kl_STANLEY = general_params[2]
self.Kr_STANLEY = general_params[3]
self.K0_STANLEY = general_params[4]
self.curve_slowdown = general_params[5]
self.DELTAl = general_params[6]
self.DELTAr = general_params[7]
self.DELTA0 = general_params[8]
self.EXP_DECAY = general_params[9]
def __init__(self):
# Create a node that
# Subscribes to the laser scan topic,
# Publishes to drive topic - to move the vehicle.
# Initialize subscriber to laser scan.
rospy.Subscriber(self.SCAN_TOPIC, LaserScan, self.LaserCb)
# Initialize a publisher of drive commands.
self.drive_pub = rospy.Publisher(self.DRIVE_TOPIC,
Twist,
queue_size=10)
# Variables to keep track of drive commands being sent to robot.
self.seq_id = 0
# Class variables for following.
self.side_angle_window_fwd_ = math.pi * 0.1
self.side_angle_window_bwd_ = math.pi - math.pi * 0.3
self.point_buffer_x_ = np.array([])
self.point_buffer_y_ = np.array([])
self.num_readings_in_buffer_ = 0
self.num_readings_for_fit_ = 2
self.reject_dist = 0.7
self.steer_cmd = 0
self.vel_cmd = self.VELOCITY
self.pid = PID()
def __init__(self):
super().__init__(0.25)
self.pid = PID(60, 30, 20, -50, 50)
self.pid.difference = compass.angleDifference
self.drive_forward = False
self.drive_backward = False
self.t = 0
self.last = 0
self.r_speed = 100
self.l_speed = 100
def apply_takeoff_controls():
# set wind environment
wind_speed, wind_heading = set_winds()
throttle_controller = PID(2.0, 0.0, 1.0, 10.0, sample_time)
rudder_controller = PID(0.3, 0.4, 1.5, 10.0, sample_time)
read_drefs = XPlaneDefs.control_dref + XPlaneDefs.position_dref
maximum_cle = 0
controls = None
start = time.time()
for t in range(int(simulation_steps // receding_horizon)):
gs, psi, throttle, x, _, z = xp_client.getDREFs(read_drefs)
gs, psi, throttle, x, z = gs[0], psi[0], throttle[0], x[0], z[0]
if TAKEOFF:
break
# cle = rejection_dist(desired_x, desired_z, x - origin_x, z - origin_z)
# maximum_cle = max(cld, maximum_cle)
dist = signed_rejection_dist(center_line, x - origin_x, z - origin_z)
dist = confine_bins(dist, c_bins)
psi = confine_bins(psi - runway_heading, h_bins)
gs = confine_bins(gs, v_bins)
wind_speed = confine_bins(wind_speed, wsp_bins)
wind_heading = confine_bins(wind_heading, whe_bins)
controls, cost = table[(dist, psi, gs, wind_speed, wind_heading)]
# change wind conditions every second
if time.time() - start > 1:
wind_speed, wind_heading = set_winds()
start = time.time()
# let PID controller take over
apply_lookup_controls(xp_client, throttle_controller,
rudder_controller, controls, sample_time,
receding_horizon)
return maximum_cle
def __init__(self):
super().__init__(0.25)
self.pid = PID(60, 30, 20, -50, 50)
self.pid.difference = compass.angleDifference
self.drive_forward = False
self.drive_backward = False
self.t = 0
self.last = 0
self.r_speed = 100
self.l_speed = 100
def turn_to(heading, error=math.radians(4)):
pid = PID(3, 0.1, 5, -50, 50)
pid.set_target(heading)
pid.difference = compass.angleDifference
h = compass.getHeading()
while True:
ret = pid.update(h)
if ret < 0:
motors.right(50-ret)
elif ret > 0:
motors.left(50+ret)
if abs(compass.angleDifference(h, heading)) <= error:
motors.stop()
time.sleep(0.5)
if abs(compass.angleDifference(compass.getHeading(), heading)) <= error:
break
h = compass.getHeading()
time.sleep(0.05)
def test_pid(self):
pid_settings = PIDSettings()
pid_settings.B = 1
pid_settings.Ki = 5
pid_settings.Kd = 0.2
pid_settings.Kp = 19
pid_settings.Tt = 1 / pid_settings.Ki
pid_settings.Ts = 1
pid_settings.YMin = -10
pid_settings.YMax = 10
self.do_test(PID(), pid_settings, PIDInput, PIDParameters)
class _MotorController(Timer):
def __init__(self):
super().__init__(0.25)
self.pid = PID(60, 30, 20, -50, 50)
self.pid.difference = compass.angleDifference
self.drive_forward = False
self.drive_backward = False
self.t = 0
self.last = 0
self.r_speed = 100
self.l_speed = 100
def forward(self):
self.drive_backward = False
self.drive_forward = True
def backward(self):
self.drive_backward = True
self.drive_forward = False
def run(self):
dt = time.time() - self.t
self.t = time.time()
direction = compass.getHeading()
ret = self.pid.update(direction, dt)
if ret < 0:
self.r_speed = 100 + ret
self.l_speed = 100
if ret > 0:
self.r_speed = 100
self.l_speed = 100 - ret
if self.drive_forward:
LEFT.forward(self.l_speed)
RIGHT.forward(self.r_speed)
elif self.drive_backward:
LEFT.backward(self.l_speed)
RIGHT.backward(self.r_speed)
def start(self):
self.last = compass.getHeading()
self.t = time.time()
super().start()
class _MotorController(Timer):
def __init__(self):
super().__init__(0.25)
self.pid = PID(60, 30, 20, -50, 50)
self.pid.difference = compass.angleDifference
self.drive_forward = False
self.drive_backward = False
self.t = 0
self.last = 0
self.r_speed = 100
self.l_speed = 100
def forward(self):
self.drive_backward = False
self.drive_forward = True
def backward(self):
self.drive_backward = True
self.drive_forward = False
def run(self):
dt = time.time() - self.t
self.t = time.time()
direction = compass.getHeading()
ret = self.pid.update(direction, dt)
if ret < 0:
self.r_speed = 100 + ret
self.l_speed = 100
if ret > 0:
self.r_speed = 100
self.l_speed = 100 - ret
if self.drive_forward:
LEFT.forward(self.l_speed)
RIGHT.forward(self.r_speed)
elif self.drive_backward:
LEFT.backward(self.l_speed)
RIGHT.backward(self.r_speed)
def start(self):
self.last = compass.getHeading()
self.t = time.time()
super().start()
def turn_to(heading, error=math.radians(4)):
pid = PID(3, 0.1, 5, -50, 50)
pid.set_target(heading)
pid.difference = compass.angleDifference
h = compass.getHeading()
while True:
ret = pid.update(h)
if ret < 0:
motors.right(50 - ret)
elif ret > 0:
motors.left(50 + ret)
if abs(compass.angleDifference(h, heading)) <= error:
motors.stop()
time.sleep(0.5)
if abs(compass.angleDifference(compass.getHeading(),
heading)) <= error:
break
h = compass.getHeading()
time.sleep(0.05)
s2.Kp = 5 s2.Ki = 16 s2.Kd = 3 s2.Ts = 0.002 # they all must be the same!!! s2.Tt = 1 / s1.Ki s2.YMin = 0 s2.YMax = 10 # PID parameters using each individual setting params = PIDParameters() params.setSettings([s1, s2]) params.wrap = False params.saturate = True params.integration_method = IntegrationMethod.TUSTIN # configure PID instance controller = PID() controller.configParameters(params) # ... code is running ... # ... inside a mainloop to update the controller ... ctrl_in = PIDInput() ctrl_in.time = LinearSystem.getTimeFromSeconds(0.2) ctrl_in.ref = [0.1, 0.2] ctrl_in.sig = [0.3, 0.4] ctrl_in.dref = [0, 0] ctrl_in.dsig = [0, 0] ctrl_in.sat = [0, 0] # last PID output after saturation out = controller.update(ctrl_in) print(out.getValue())
model_name = sys.argv[1]
target_name = sys.argv[2]
target_func = load_target_func(target_name)
elif len(sys.argv) == 4:
model_name = sys.argv[1]
target_name = sys.argv[2]
target_func = load_target_func(target_name)
num_steps = int(sys.argv[3])
if model_name in ['pd', 'pid', 'pidt', 'pidtf']:
# The controller does not use a Nengo Model
if model_name == 'pd':
cont = PD(target_func=target_func)
elif model_name == 'pid':
cont = PID(target_func=target_func)
elif model_name == 'pidt':
cont = PIDt(target_func=target_func)
elif model_name == 'pidtf':
cont = PIDt(fast_i=True, target_func=target_func)
else:
cont = PIDt(target_func=target_func)
state = []
count = 0
# if no target is given, run 'forever'
# also don't bother saving anything
if not recording:
while True:
cont.control_step()
while done is False:
a = controller.calculate(s, 0.02)
s_, reward, done = env.step(a)
# add record data
if episode % record_period == 0:
episode_record.add(np.hstack((s, a, [reward], s_))) # add data
s = s_
step += 1
if done:
# record
if episode % record_period == 0:
episode_record.save() # save data
if __name__ == '__main__':
path = "/data/control/" + getTime()
record_path = path + "/record/"
create_dir(path)
PID_controller = PID(kp=0.5, ki=0, kd=0.1, dss_bound=10)
##### select env #################
# track_env = Env(is_bldc2_control=False,is_compare=True) # for random cruve compare
# track_env = Env(is_bldc2_control=True,is_compare=True) # for random motor compare
# track_env = Env(is_bldc2_control=False,is_compare=False) # for random curve single control
track_env = Env(is_bldc2_control=True,
is_compare=False) # for random motor single control
run_task(track_env, PID_controller, 500, record_path=record_path)
class CAL(Agent):
def __init__(self, city_name):
self.timer = Timer()
# Map setup
self._map = CarlaMap(city_name)
self._centerlines = Centerlines(city_name)
# Agent Setup
Agent.__init__(self)
self._neural_net = CAL_network()
self._seq_len = 5 #self._neural_net.model.max_input_shape
self._state = VehicleState()
self._agents_present = False
# Controller setup
param_path = os.path.dirname(__file__) + '/controller/params/'
cruise_params = get_params_from_txt(param_path + 'cruise_params.txt')
self._PID_cruise = PID(*cruise_params)
follow_params = get_params_from_txt(param_path + 'follow_params.txt')
self._PID_follow = PID(*follow_params)
# General Parameter setup
general_params = get_params_from_txt(param_path + 'general_params.txt')
self.c, self.d = general_params[0], general_params[1]
self.Kl_STANLEY = general_params[2]
self.Kr_STANLEY = general_params[3]
self.K0_STANLEY = general_params[4]
self.curve_slowdown = general_params[5]
self.DELTAl = general_params[6]
self.DELTAr = general_params[7]
self.DELTA0 = general_params[8]
self.EXP_DECAY = general_params[9]
def reset_state(self):
""" for resetting at the start of a new episode"""
self._state = VehicleState()
def modified_run_step(self, measurements, sensor_data, carla_direction,
target):
curr_time = time.time()
# update the vehicle state
#self._state.speed = measurements.player_measurements.forward_speed*3.6
# get the current location and orientation of the agent in the world COS
location, psi = self._get_location_and_orientation(measurements)
# check if there are other cars or pedestrians present
# speed up the benchmark, because we dont need to stop for red lights
#self._agents_present = any([agent.HasField('vehicle') for agent in measurements.non_player_agents])
# get the possible directions (at position of the front axle)
front_axle_pos = self._get_front_axle(location, psi)
try:
directions_list_new = self._centerlines.get_directions(
front_axle_pos)
except:
directions_list_new = {}
# determine the current direction
if directions_list_new:
self._set_current_direction(directions_list_new, carla_direction)
#### SIGNALS FOR EVALUATION ####
# set the correct center line and get the ground truth
self._state.center_distance_GT = self._centerlines.get_center_distance(
front_axle_pos)
self._state.long_accel, self._state.lat_accel = self._get_accel(
measurements, psi)
################################
with open(
"/home/self-driving/Desktop/CARLA_0.8.2/Indranil/CAL-master/PythonClient/_benchmarks_results/latestData.csv",
"a+") as myf:
wrs=str(self._state.center_distance_GT)+","+str(self._state.long_accel)+","+str(self._state.lat_accel)+","+\
str(measurements.game_timestamp)+","+str(self._state.direction)
myf.write(wrs + "\n")
def run_step(self, measurements, sensor_data, carla_direction, target):
curr_time = time.time()
# update the vehicle state
self._state.speed = measurements.player_measurements.forward_speed * 3.6
# get the current location and orientation of the agent in the world COS
location, psi = self._get_location_and_orientation(measurements)
# check if there are other cars or pedestrians present
# speed up the benchmark, because we dont need to stop for red lights
self._agents_present = any([
agent.HasField('vehicle')
for agent in measurements.non_player_agents
])
# get the possible directions (at position of the front axle)
front_axle_pos = self._get_front_axle(location, psi)
try:
directions_list_new = self._centerlines.get_directions(
front_axle_pos)
except:
directions_list_new = {}
# determine the current direction
if directions_list_new:
self._set_current_direction(directions_list_new, carla_direction)
#### SIGNALS FOR EVALUATION ####
# set the correct center line and get the ground truth
self._state.center_distance_GT = self._centerlines.get_center_distance(
front_axle_pos)
self._state.long_accel, self._state.lat_accel = self._get_accel(
measurements, psi)
################################
with open(
"/home/self-driving/Desktop/CARLA_0.8.2/Indranil/latestData.csv",
"a+") as myf:
wrs=str(self._state.center_distance_GT)+","+str(self._state.long_accel)+","+str(self._state.lat_accel)+","+\
str(measurements.game_timestamp)+","+str(self._state.direction)
myf.write(wrs + "\n")
# cycle the image sequence
new_im = sensor_data['CameraRGB'].data
if self._state.image_hist is None:
im0 = self._neural_net.preprocess(new_im, sequence=True)
print("1 works")
self._state.image_hist = im0.repeat_interleave(self._seq_len,
dim=1)
print("2 works")
else:
# get the newest entry
im0 = self._neural_net.preprocess(new_im, sequence=True)
print("2.5 works")
# drop the oldelst entry
self._state.image_hist = self._state.image_hist[:, 1:, :, :]
# add new entry
print("3 works")
self._state.image_hist = np.concatenate(
(self._state.image_hist, im0), axis=1)
print("4 works")
# calculate the control
control, prediction = self._compute_action(carla_direction,
self._state.direction)
return control, prediction
#edited ash
def getMetData(self):
tempDict = {}
tempDict['direction'] = self._state.direction
tempDict['centerDist'] = self._state.center_distance_GT
tempDict['long_accel'], tempDict[
'lat_accel'] = self._state.long_accel, self._state.lat_accel
return tempDict
def _set_current_direction(self, directions_list_new, carla_direction):
if carla_direction == 3.0:
self._state.direction = -1
if carla_direction == 4.0:
self._state.direction = 1
if carla_direction == 2.0 or len(directions_list_new) == 1:
self._state.direction = list(directions_list_new)[0]
if carla_direction == 5.0 or carla_direction == 0.0:
self._state.direction = 0
### set the correct GT centerline maps
# set the centerlines according to the decided direction
# and to the current street type
direction = self._state.direction
is_straight = direction == 0 or directions_list_new == {0}
is_c1 = (direction == -1 and directions_list_new == {0,-1}) or \
(direction == 1 and directions_list_new == {1,-1}) or \
directions_list_new == {-1} or directions_list_new == {1}
is_c2 = (direction == 1 and directions_list_new == {0,1}) or \
(direction ==-1 and directions_list_new == {1,-1})
# set the centerlines accordingly
if is_straight: self._centerlines.set_centerlines('straight')
if is_c1: self._centerlines.set_centerlines('c1')
if is_c2: self._centerlines.set_centerlines('c2')
def _compute_action(self, carla_direction, direction):
start = time.time()
# Predict the intermediate representations
prediction = self._neural_net.predict(self._state.image_hist,
[direction])
logging.info("Time for prediction: {}".format(time.time() - start))
logging.info("CARLA Direction {}, Real Direction {}".format(
carla_direction, direction))
# update the speed limit if a speed limit sign is detected
if prediction['speed_sign'][0] != -1:
self._state.speed_limit = prediction['speed_sign']
# Calculate the control
control = Control()
control.throttle, control.brake = self._longitudinal_control(
prediction, direction)
control.steer = self._lateral_control(prediction)
return control, prediction
def _longitudinal_control(self, prediction, direction):
"""
calculate the _longitudinal_control
the constants (c, d, curve_slowdown) are defined on top of the file
"""
### Variable setup
throttle = 0
brake = 0
# unpack the state variables
speed = self._state.speed
limit = self._state.speed_limit
# uncomment for similar conditions to carla paper
limit = 30
# determine whether to use other states than cruising
cruising_only = not self._agents_present
# brake when driving into a curve
if direction: limit -= self.curve_slowdown
# get the distance to the leading car
veh_distance = prediction['veh_distance']
# half of the speed indicator
is_following = veh_distance < np.clip(limit, 30, 50)
# optimal car following model
following_speed = limit * (
1 - np.exp(-self.c / limit * veh_distance - self.d))
### State machine
if prediction['hazard_stop'][0] \
and prediction['hazard_stop'][1] > 0.9 \
and self._agents_present:
state_name = 'hazard_stop'
prediction_proba = prediction['hazard_stop'][1]
brake = 1
elif prediction['red_light'][0] \
and prediction['red_light'][1] > 0.98 \
and self._agents_present:
state_name = 'red_light'
prediction_proba = prediction['red_light'][1]
throttle = 0
if speed > 5:
# brake if driving to fast
brake = 0.8 * (speed / 30.0)
else:
# fully brake if close to standing still
brake = 1.0
elif is_following and self._agents_present:
state_name = 'following'
prediction_proba = 1.0
desired_speed = following_speed
self._PID_follow.update(desired_speed - speed)
throttle = -self._PID_follow.output
else: # is cruising
state_name = 'cruising'
prediction_proba = 1.0
desired_speed = limit
self._PID_cruise.update(desired_speed - speed)
throttle = -self._PID_cruise.output
logging.info('STATE: {}, PROBA: {:.4f} %'.format(
state_name, prediction_proba * 100))
### Additional Corrections
# slow down speed limit is exceeded
if speed > limit + 10: brake = 0.3 * (speed / 30)
# clipping
throttle = np.clip(throttle, 0, 0.8)
brake = np.clip(brake, 0, 1)
if brake: throttle = 0
return throttle, brake
def _lateral_control(self, prediction):
"""
function implements the lateral control algorithm
input:
- vehicle speed
- front axle position
- vehicle yaw
- distance to closest pixel on center line [with correct sign]
- yaw in closest pixel on center line
output:
- delta signal in [-1,1]
"""
# vehicle state
v = self._state.speed
# when standing don't steer
if abs(v) <= 0.1: return 0
# choose value for k and d depending on the street
if self._state.direction == 0:
k = self.K0_STANLEY
d = self.DELTA0
elif self._state.direction == -1:
k = self.Kl_STANLEY
d = self.DELTAl
elif self._state.direction == 1:
k = self.Kr_STANLEY
d = self.DELTAr
# stanley control
theta_e = prediction['relative_angle']
theta_d = math.atan2(k * prediction['center_distance'], v)
delta = theta_e + theta_d
# normalize delta
delta /= MAX_STEER
# get delta sign, damping is calculed using the absolute delta
delta_sign = np.sign(delta)
delta = abs(delta)
# add exponential damping
decay = d * math.exp(-self.EXP_DECAY * delta)
logging.info("DECAY: {}".format(decay))
delta -= decay
delta = np.clip(delta, 0, 1)
# return the signed delta
return delta_sign * delta
def _get_location_and_orientation(self, measurements):
# get the location
location_world = [
measurements.player_measurements.transform.location.x,
measurements.player_measurements.transform.location.y,
measurements.player_measurements.transform.location.z
]
location_map = self._map.convert_to_pixel(location_world)
# get the orientation
veh_ori_x = measurements.player_measurements.transform.orientation.x,
veh_ori_y = measurements.player_measurements.transform.orientation.y,
psi = math.atan2(veh_ori_y[0], veh_ori_x[0]) # angle in the world COS
return location_map[:2], psi
def _get_front_axle(self, location, psi):
# calculate the position of the front axle
point = self._vehicle_to_world_COS(location, (0, 8.7644), psi)
return (int(point[0]), int(point[1]))
def _vehicle_to_world_COS(self, origin, point, psi):
"""
transform a 2d point from the vehicle COS to the world COS
"""
x_new = origin[0] - point[0] * math.sin(psi) + point[1] * math.cos(psi)
y_new = origin[1] + point[0] * math.cos(psi) + point[1] * math.sin(psi)
return (x_new, y_new)
def _get_accel(self, measurements, psi):
# get the absolute aceleration in the cars COS
acceleration = measurements.player_measurements.acceleration
a_x, a_y = acceleration.x, acceleration.y
# calculate in the relative COS
a_x_rel = a_x * math.cos(psi) + a_y * math.sin(psi)
a_y_rel = -a_x * math.sin(psi) + a_y * math.cos(psi)
return a_x_rel, a_y_rel
def get_GT(self):
""""
This functions returns the current distance to the center line
and the current directions_list_new
"""
d = {}
d['center_distance'] = self._state.center_distance_GT
d['direction'] = self._state.direction
d['long_accel'] = self._state.long_accel
d['lat_accel'] = self._state.lat_accel
return d
class WallFollower:
# Import ROS parameters from the "params.yaml" file.
# Access these variables in class functions with self:
# i.e. self.CONSTANT
SCAN_TOPIC = "/scan"
DRIVE_TOPIC = "cmd_vel"
SIDE = -1 # -1 right is and +1 is left
VELOCITY = 1.6
DESIRED_DISTANCE = 0.5
def __init__(self):
# Create a node that
# Subscribes to the laser scan topic,
# Publishes to drive topic - to move the vehicle.
# Initialize subscriber to laser scan.
rospy.Subscriber(self.SCAN_TOPIC, LaserScan, self.LaserCb)
# Initialize a publisher of drive commands.
self.drive_pub = rospy.Publisher(self.DRIVE_TOPIC,
Twist,
queue_size=10)
# Variables to keep track of drive commands being sent to robot.
self.seq_id = 0
# Class variables for following.
self.side_angle_window_fwd_ = math.pi * 0.1
self.side_angle_window_bwd_ = math.pi - math.pi * 0.3
self.point_buffer_x_ = np.array([])
self.point_buffer_y_ = np.array([])
self.num_readings_in_buffer_ = 0
self.num_readings_for_fit_ = 2
self.reject_dist = 0.7
self.steer_cmd = 0
self.vel_cmd = self.VELOCITY
self.pid = PID()
def GetLocalSideWallCoords(self, ranges, angle_min, angle_max, angle_step):
# Slice out the interesting samples from our scan. pi/2 radians from pi/4 to (pi - pi/4) radians for the right side.
positive_start_angle = self.side_angle_window_fwd_
positive_end_angle = self.side_angle_window_bwd_
if self.SIDE == -1: #"right":
start_angle = -positive_start_angle
end_angle = -positive_end_angle
elif self.SIDE == 1: #"left":
start_angle = positive_start_angle
end_angle = positive_end_angle
start_ix_ = int((-angle_min + start_angle) / angle_step)
end_ix_ = int((-angle_min + end_angle) / angle_step)
start_ix = min(start_ix_, end_ix_)
end_ix = max(start_ix_, end_ix_)
side_ranges = ranges[min(start_ix, end_ix):max(start_ix, end_ix)]
x_values = np.array([
ranges[i] * math.cos(angle_min + i * angle_step)
if i < len(ranges) else ranges[(i - len(ranges))] *
math.cos(angle_min + (i - len(ranges)) * angle_step)
for i in range(start_ix, end_ix)
])
y_values = np.array([
ranges[i] * math.sin(angle_min + i * angle_step)
if i < len(ranges) else ranges[i - len(ranges)] *
math.sin(angle_min + (i - len(ranges)) * angle_step)
for i in range(start_ix, end_ix)
])
# Check that the values for the points are within 1 meter from each other. Discard any point that is not within one meter form the one before it.
out_x = []
out_y = []
for ix in range(0, len(x_values)):
new_point = (x_values[ix], y_values[ix])
# This conditional handles points with infinite value.
if side_ranges[ix] < 2.5 and side_ranges[ix] > 0 and abs(
new_point[1]) < 7000 and abs(new_point[0]) < 7000:
out_x.append(new_point[0])
out_y.append(new_point[1])
return np.array(out_x), np.array(out_y)
def LaserCb(self, scan_data):
# This function is called every time we get a laser scan.
# This is the plan:
# * Get scan data.
# * Convert it to x,y coordinates in the local frame of the robot.
# * Find a least squares - best fit line through those points with numpy.
# Consider using data from multiple scans in one least-squares fit cycle.
# This is a line equation, with respect to the car at (0,0), with the x axis being the heading.
# Get vector theta for the line, and theta_0 as the y intersection.
# * Find the distance from the line to the origin with ( theta_T dot [[0],[0]] + theta_0 ) / (norm theta)
# TLDR, We have a vector theta for the line we have found, and a distance to that wall.
# TODO(yorai): Handle erroneous scan values. If one is too big, or too small, use past value.
# Do not do this for too many in a row, maybe just throw scan away if too many corrections.
angle_step = scan_data.angle_increment
angle_min = scan_data.angle_min
angle_max = scan_data.angle_max
ranges = scan_data.ranges
# Get data for side ranges. Add to buffer.
wall_coords_local = self.GetLocalSideWallCoords(
ranges, angle_min, angle_max, angle_step)
#######
#Find mean and throw out everything that is 1 meter away from mean distance, no outliers.
# If one differs by more than a meter from the previous one, gets thrown out from both x and y. Distnaces as we go along vector of points.
# but print the things first.
#######
self.point_buffer_x_ = np.append(self.point_buffer_x_,
wall_coords_local[0])
self.point_buffer_y_ = np.append(self.point_buffer_y_,
wall_coords_local[1])
self.num_readings_in_buffer_ += 1
# If we have enough data, then find line of best fit.
if self.num_readings_in_buffer_ >= self.num_readings_for_fit_:
# Find line of best fit.
# self.point_buffer_x_ = np.array([0, 1, 2, 3])
# self.point_buffer_y_ = np.array([-1, 0.2, 0.9, 2.1])
A = np.vstack(
[self.point_buffer_x_,
np.ones(len(self.point_buffer_x_))]).T
m, c = np.linalg.lstsq(A, self.point_buffer_y_, rcond=0.001)[0]
# Find angle from heading to wall.
# Vector of wall. Call wall direction vector theta.
th = np.array([[m], [1]])
th /= np.linalg.norm(th)
# Scalar to define the (hyper) plane
th_0 = c
# Distance to wall is (th.T dot x_0 + th_0)/(norm(th))
dist_to_wall = abs(c / np.linalg.norm(th))
# Angle between heading and wall.
angle_to_wall = math.atan2(m, 1)
# Clear scan buffers.
self.point_buffer_x_ = np.array([])
self.point_buffer_y_ = np.array([])
self.num_readings_in_buffer_ = 0
# Simple Proportional controller.
# Feeding the current angle ERROR(with target 0), and the distance ERROR to wall. Desired error to be 0.
print("ANGLE", angle_to_wall, "DIST", dist_to_wall)
steer = self.pid.GetControl(0.0 - angle_to_wall,
self.DESIRED_DISTANCE - dist_to_wall,
self.SIDE)
# Publish control to /drive topic.
drive_msg = Twist()
# drive_msg.header.seq = self.seq_id
# self.seq_id += 1
# Populate the command itself.
drive_msg.linear.x = self.VELOCITY
drive_msg.angular.z = steer
# drive_msg.drive.steering_angle = steer
# drive_msg.drive.steering_angle_velocity = 0.1
# drive_msg.drive.speed = self.VELOCITY
# drive_msg.drive.acceleration = 1
# drive_msg.drive.acceleration = 0.5
self.drive_pub.publish(drive_msg)
else:
handler = logging.StreamHandler()
formatter = logging.Formatter('%(name)-12s %(levelname)5s: %(message)s')
handler.setFormatter(formatter)
root_logger.addHandler(handler)
working = True
def sigterm_handler(signum, frame):
global working
working = False
root_logger.info("Exiting due to SIGTERM")
signal.signal(signal.SIGTERM, sigterm_handler)
with PID(proportional = args.proportional, integrative = args.integrative, derivative = args.derivative,
integral_minimum = args.integral_minimum, integral_maximum = args.integral_maximum, minimal_control = args.minimal_control, force_shutdown_error = args.force_shutdown_error) as controller:
with TargetThermometer(minimal = args.minimal_temperature, maximal = args.maximal_temperature) as element:
with TargetDriver() as driver:
with ClosedLoop(controller = controller, element = element, driver = driver) as system:
while working:
try:
system.step()
time.sleep(args.update_delay)
except KeyboardInterrupt:
break
logging.shutdown()
# uncertainty for prediction
Q = np.eye(4) * 10000
# covariant noise
N = np.random.randint(1, 100, size=(4, 4))
np.fill_diagonal(N, 0)
Q = Q + N
# uncertainty for measurements
R = np.eye(4) * 5
xp_client = XPlaneConnect()
end_x, end_z = runway['terminate_X'], runway['terminate_Z']
origin_x, origin_z = runway['origin_X'], runway['origin_Z']
starting_elevation = runway['starting_elevation']
throttle_controller = PID(2.0, 0.0, 1.0, 10.0, config['sample_time'])
rudder_controller = PID(0.3, 0.4, 1.5, 10.0, config['sample_time'])
num_samples = config['environment_samples']
ws_bound = config['windspeed_bounds']
wh_bound = config['wind_heading_bounds']
center_line = np.array([end_x - origin_x, end_z - origin_z])
kf = KFilter(F, B, Q, H, R)
no_sim_runs = 60
choose_num_samples = [0, 5, 8]
takeoff_successes = [0, 0, 0]
log = open("sample_0.csv", "w")
xp_client.sendDREFs(XPlaneDefs.position_dref, [origin_x, start_y, origin_z])
# want to end here at t = sim_time
desired_x -= origin_x
desired_z -= origin_z
time.sleep(1)
xp_client.sendCOMM("sim/operation/fix_all_systems")
# release park brake
xp_client.sendDREF("sim/flightmodel/controls/parkbrake", 0)
time.sleep(1)
controls = None
throttle_controller = PID(2.0, 0.0, 1.0, 10.0, sample_time)
rudder_controller = PID(0.3, 0.4, 1.5, 10.0, sample_time)
cle = 0
start = time.time()
for t in range(int(simulation_steps // receding_horizon)):
if time.time() - start > 1:
wind_speed = np.random.randint(ws_bins[0], ws_bins[1])
wind_degrees = np.random.randint(wh_bins[0], wh_bins[1])
wind_direction = runway_heading + wind_degrees
xp_wind_direction = -1 * wind_degrees + runway_heading # since positive rotation to right
xp_wind_direction += 180 # wind is counter clockwise of true north
if len(sys.argv) == 2:
model_name = sys.argv[1] #diff model
if len(sys.argv) == 3:
model_name = sys.argv[1]
control_name = sys.argv[2] #diff control method 1 key 2 gesture 3 ??
print('control', control_name, 'model', model_name)
if model_name in ['pd', 'pid', 'pidt']:
if model_name == 'pd':
control = PD(target_func = target_func)
elif model_name == 'pid':
control = PIDt(target_func = target_func)
else:
control = PID(target_func = target_func)
state = []
count = 0
if control_name == 'key':
key_cont.start()
obj = key_cont
print("key board control")
elif control_name == 'hand':
obj = gesture_cont
elif control_name == 'control':
obj = server_cont
while True:
control.control_step(obj, control_name)
