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 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()
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)