def run_step(self, target_speed): """ Execute one step of local planning which involves running the longitudinal and lateral PID controllers to follow the waypoints trajectory. """ if not self._waypoints_queue: control = CarlaEgoVehicleControl() control.steer = 0.0 control.throttle = 0.0 control.brake = 1.0 control.hand_brake = False control.manual_gear_shift = False return control # Buffering the waypoints if not self._waypoint_buffer: for i in range(self._buffer_size): if self._waypoints_queue: self._waypoint_buffer.append( self._waypoints_queue.popleft()) else: break # current vehicle waypoint self._current_waypoint = self.get_waypoint(self._current_pose.position) # target waypoint target_route_point = self._waypoint_buffer[0] # for us redlight-detection self.target_waypoint = self.get_waypoint(target_route_point.position) target_point = PointStamped() target_point.header.frame_id = "map" target_point.point.x = target_route_point.position.x target_point.point.y = target_route_point.position.y target_point.point.z = target_route_point.position.z self._target_point_publisher.publish(target_point) # move using PID controllers control = self._vehicle_controller.run_step(target_speed, self._current_speed, self._current_pose, target_route_point) # purge the queue of obsolete waypoints max_index = -1 sampling_radius = target_speed * 1 / 3.6 # 1 seconds horizon min_distance = sampling_radius * self.MIN_DISTANCE_PERCENTAGE for i, route_point in enumerate(self._waypoint_buffer): if distance_vehicle(route_point, self._current_pose.position) < min_distance: max_index = i if max_index >= 0: for i in range(max_index + 1): self._waypoint_buffer.popleft() return control
def run_step(self, target_speed, current_speed, current_pose, waypoint): """ Execute one step of control invoking both lateral and longitudinal PID controllers to reach a target waypoint at a given target_speed. :param target_speed: desired vehicle speed :param waypoint: target location encoded as a waypoint :return: distance (in meters) to the waypoint """ current_time = rospy.get_time() dt = current_time - self._last_control_time if dt == 0.0: dt = 0.000001 control = CarlaEgoVehicleControl() throttle = self._lon_controller.run_step(target_speed, current_speed, dt) steering = self._lat_controller.run_step(current_pose, waypoint, dt) self._last_control_time = current_time control.steer = steering control.throttle = throttle control.brake = 0.0 control.hand_brake = False control.manual_gear_shift = False return control
def emergency_stop(self): control_msg = CarlaEgoVehicleControl() control_msg.steer = 0.0 control_msg.throttle = 0.0 control_msg.brake = 1.0 control_msg.hand_brake = False control_msg.manual_gear_shift = False self._control_cmd_publisher.publish(control_msg)
def control_callback(msg): data = CarlaEgoVehicleControl() data.throttle = msg.velocity data.steer = msg.angle data.brake = 0 data.gear = 1 data.reverse = False data.manual_gear_shift = True pub.publish(data) vehicle_control_manual_override_publisher.publish(vehicle_control_manual_override) auto_pilot_enable_publisher.publish(autopilot_enabled)
def run_step(self, target_speed, current_speed, current_pose, waypoint): """ Execute one step of control invoking both lateral and longitudinal PID controllers to reach a target waypoint at a given target_speed. :param target_speed: desired vehicle speed :param waypoint: target location encoded as a waypoint :return: control signal (throttle and steering) """ control = CarlaEgoVehicleControl() throttle = self._lon_controller.run_step(target_speed, current_speed) steering = self._lat_controller.run_step(current_pose, waypoint) control.steer = -steering control.throttle = throttle control.brake = 0.0 control.hand_brake = False control.manual_gear_shift = False return control
def Control(self): rate = rospy.Rate(10) lon_param = { 'K_P': 0.5, 'K_I': 0.5, 'K_D': 0 } # Set PID values for longitudinal controller lat_param = { 'K_P': 0.5, 'K_I': 0.3, 'K_D': 0 } # Set PID values for lateral controller vehicle_controller = VehiclePIDController( self.vehicle, lon_param, lat_param) # Calling vehicle controller class from controller.py i = 0 for k in range(1, len(self.current_route) ): # Iterate through all the waypoints in the route self.Distance(self.current_route[i][0], self.veh_pos.transform) self.Waypoints() rospy.Subscriber( '/machine_learning/output', Int16, self.Detection ) # Subscribes to topic for Stop sign detection. Need to run carla_detect_objects.py script to obtain detection while self.distance > 0.5: # Control the vehicle until the distance of the next waypoint and the vehicle is less than 0.5 m self.Actor( ) # Call Actor function to update vehicle's location control = vehicle_controller.run_step( 15, self.current_route[i] [0]) # Feeds the controller the waypoints one by one self.velocity = self.vehicle.get_velocity( ) # Get vehicle velocity if self.detection == 11: # Stop sign detection(apply brakes). Our ML has a class ID of 11 for stop signs print('Object detected, apply brakes') msg = CarlaEgoVehicleControl( ) # Ego vehicle's control message msg.throttle = 0 msg.steer = control.steer msg.brake = 1 msg.hand_brake = control.hand_brake msg.reverse = control.reverse msg.gear = 1 msg.manual_gear_shift = control.manual_gear_shift self.detection = None elif len(self.current_route) - 5 <= k <= len( self.current_route ): # If the ith waypoint is between the last waypoint minus five apply brakes msg = CarlaEgoVehicleControl() msg.throttle = 0 msg.steer = control.steer msg.brake = 1 msg.hand_brake = control.hand_brake msg.reverse = control.reverse msg.gear = 1 msg.manual_gear_shift = control.manual_gear_shift print('You arrived to your destination!!') else: # If neither scenario happen, keep driving msg = CarlaEgoVehicleControl() msg.throttle = control.throttle msg.steer = control.steer msg.brake = control.brake msg.hand_brake = control.hand_brake msg.reverse = control.reverse msg.gear = 1 msg.manual_gear_shift = control.manual_gear_shift self.Publisher(msg) rate.sleep() self.Distance( self.current_route[i][0], self.veh_pos.transform ) # Calculates the Euclidean distance between the vehicle and the next waypoint in every iteration i += 1