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):
"""
Execute one step of navigation.
"""
control = CarlaEgoVehicleControl()
control.steer = 0.0
control.throttle = 0.0
control.brake = 0.0
control.hand_brake = False
if not self._agent:
rospy.loginfo("Waiting for ego vehicle...")
return control
if not self._route_assigned and self._global_plan:
rospy.loginfo("Assigning plan...")
self._agent._local_planner.set_global_plan( # pylint: disable=protected-access
self._global_plan.poses)
self._route_assigned = True
else:
control, finished = self._agent.run_step(self._target_speed)
if finished:
self._global_plan = None
self._route_assigned = False
return control
def tick(self):
target_speed = self._get_target_speed()
speed_err = target_speed - self.speed
cte = self._calc_lateral_error() # cross-track error
max_delta = 100
steer = clamp(-1, self.steer_pid.update(cte), 1)
# steer = clamp(self.prev_steer - max_delta, steer, self.prev_steer + max_delta)
self.prev_steer = steer
brake = 0
throttle = clamp(-1, self.throttle_pid.update(speed_err), 1)
if throttle < 0:
throttle = 0
brake = -throttle
# public control command
control = CarlaEgoVehicleControl()
control.throttle = throttle
control.steer = steer
control.brake = brake
control.hand_brake = False
self._vehicle_control_publisher.publish(control)
# publish debug info
debug_info_msg = msg.ControllerDebugInfo()
debug_info_msg.cross_track_error = cte
debug_info_msg.speed_error = speed_err
debug_info_msg.target_speed = target_speed
self._debug_info_publisher.publish(debug_info_msg)
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 emergency_stop(self): # pylint: disable=no-self-use
"""
Send an emergency stop command to the vehicle
:return:
"""
control = CarlaEgoVehicleControl()
control.steer = 0.0
control.throttle = 0.0
control.brake = 1.0
control.hand_brake = False
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: 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
