def _init_controller(self, opt_dict):
"""
Controller initialization.
:param opt_dict: dictionary of arguments.
:return:
"""
# default params
self._current_speed = 0.0 # Km/h
self._current_pose = Pose()
args_lateral_dict = {'K_P': 1.95, 'K_D': 0.01, 'K_I': 1.4}
args_longitudinal_dict = {'K_P': 0.2, 'K_D': 0.05, 'K_I': 0.1}
# parameters overload
if opt_dict:
if 'lateral_control_dict' in opt_dict:
args_lateral_dict = opt_dict['lateral_control_dict']
if 'longitudinal_control_dict' in opt_dict:
args_longitudinal_dict = opt_dict['longitudinal_control_dict']
self._current_waypoint = self.get_waypoint(self._current_pose.position)
self._vehicle_controller = VehiclePIDController(
args_lateral=args_lateral_dict,
args_longitudinal=args_longitudinal_dict)
self._global_plan = False
def __init__(self):
super(LocalPlanner, self).__init__('local_planner')
# ros parameters
role_name = self.get_param("role_name", "ego_vehicle")
self.control_time_step = self.get_param("control_time_step", 0.05)
args_lateral_dict = {}
args_lateral_dict['K_P'] = self.get_param("Kp_lateral", 0.9)
args_lateral_dict['K_I'] = self.get_param("Ki_lateral", 0.0)
args_lateral_dict['K_D'] = self.get_param("Kd_lateral", 0.0)
args_longitudinal_dict = {}
args_longitudinal_dict['K_P'] = self.get_param("Kp_longitudinal",
0.206)
args_longitudinal_dict['K_I'] = self.get_param("Ki_longitudinal",
0.0206)
args_longitudinal_dict['K_D'] = self.get_param("Kd_longitudinal",
0.515)
self.target_route_point = None
self._vehicle_controller = None
self._waypoints_queue = deque(maxlen=20000)
self._buffer_size = 5
self._waypoint_buffer = deque(maxlen=self._buffer_size)
self.path_received = False
self._current_speed = None
self._current_pose = None
self._target_speed = 0.0
# subscribers
self._odometry_subscriber = self.create_subscriber(
Odometry, "/carla/{}/odometry".format(role_name),
self.odometry_updated)
self._path_subscriber = self.create_subscriber(
Path, "/carla/{}/waypoints".format(role_name), self.path_updated,
QoSProfile(depth=1, durability=True))
self._target_speed_subscriber = self.create_subscriber(
Float64, "/carla/{}/speed_command".format(role_name),
self.target_speed_updated, QoSProfile(depth=1, durability=True))
# publishers
self._target_point_publisher = self.new_publisher(
Marker, "/carla/{}/next_target".format(role_name),
QoSProfile(depth=10, durability=False))
self._control_cmd_publisher = self.new_publisher(
CarlaEgoVehicleControl,
"/carla/{}/vehicle_control_cmd".format(role_name),
QoSProfile(depth=1, durability=False))
# initializing controller
self._vehicle_controller = VehiclePIDController(
self,
args_lateral=args_lateral_dict,
args_longitudinal=args_longitudinal_dict)
# wait for required messages
self.loginfo('Local planner waiting for a path and target speed...')
while self._current_pose is None or self._target_speed is None or self.path_received is False:
time.sleep(0.05)
if ROS_VERSION == 2:
rclpy.spin_once(self, timeout_sec=0)
def _init_controller(self, opt_dict):
"""
Controller initialization.
:param opt_dict: dictionary of arguments.
:return:
"""
# default params
args_lateral_dict = {
'K_P': 1.95,
'K_D': 0.01,
'K_I': 1.4}
args_longitudinal_dict = {
'K_P': 0.2,
'K_D': 0.05,
'K_I': 0.1}
# parameters overload
if opt_dict:
if 'lateral_control_dict' in opt_dict:
args_lateral_dict = opt_dict['lateral_control_dict']
if 'longitudinal_control_dict' in opt_dict:
args_longitudinal_dict = opt_dict['longitudinal_control_dict']
self._vehicle_controller = VehiclePIDController(args_lateral=args_lateral_dict,
args_longitudinal=args_longitudinal_dict)
class LocalPlanner(object):
"""
LocalPlanner implements the basic behavior of following a trajectory of waypoints that is
generated on-the-fly. The low-level motion of the vehicle is computed by using two PID
controllers, one is used for the lateral control and the other for the longitudinal
control (cruise speed).
When multiple paths are available (intersections) this local planner makes a random choice.
"""
# minimum distance to target waypoint as a percentage (e.g. within 90% of
# total distance)
MIN_DISTANCE_PERCENTAGE = 0.9
def __init__(self, role_name, opt_dict=None):
"""
:param vehicle: actor to apply to local planner logic onto
:param opt_dict: dictionary of arguments with the following semantics:
target_speed -- desired cruise speed in Km/h
sampling_radius -- search radius for next waypoints in seconds: e.g. 0.5 seconds ahead
lateral_control_dict -- dictionary of arguments to setup the lateral PID controller
{'K_P':, 'K_D':, 'K_I'}
longitudinal_control_dict -- dictionary of arguments to setup the longitudinal
PID controller
{'K_P':, 'K_D':, 'K_I'}
"""
self._current_waypoint = None
self.target_waypoint = None
self._vehicle_controller = None
self._global_plan = None
self._waypoints_queue = deque(maxlen=20000)
self._buffer_size = 5
self._waypoint_buffer = deque(maxlen=self._buffer_size)
self._vehicle_yaw = None
self._current_speed = None
self._current_pose = None
self._target_point_publisher = rospy.Publisher("/next_target",
PointStamped,
queue_size=1)
rospy.wait_for_service(
'/carla_waypoint_publisher/{}/get_waypoint'.format(role_name))
self._get_waypoint_client = rospy.ServiceProxy(
'/carla_waypoint_publisher/{}/get_waypoint'.format(role_name),
GetWaypoint)
# initializing controller
self._init_controller(opt_dict)
def get_waypoint(self, location):
"""
Helper to get waypoint from a ros service
"""
try:
response = self._get_waypoint_client(location)
return response.waypoint
except (rospy.ServiceException, rospy.ROSInterruptException) as e:
if not rospy.is_shutdown:
rospy.logwarn("Service call failed: {}".format(e))
def odometry_updated(self, odo):
"""
Callback on new odometry
"""
self._current_speed = math.sqrt(odo.twist.twist.linear.x**2 +
odo.twist.twist.linear.y**2 +
odo.twist.twist.linear.z**2) * 3.6
self._current_pose = odo.pose.pose
quaternion = (odo.pose.pose.orientation.x, odo.pose.pose.orientation.y,
odo.pose.pose.orientation.z, odo.pose.pose.orientation.w)
_, _, self._vehicle_yaw = euler_from_quaternion(quaternion)
def _init_controller(self, opt_dict):
"""
Controller initialization.
:param opt_dict: dictionary of arguments.
:return:
"""
# default params
self._current_speed = 0.0 # Km/h
self._current_pose = Pose()
args_lateral_dict = {'K_P': 1.95, 'K_D': 0.01, 'K_I': 1.4}
args_longitudinal_dict = {'K_P': 0.2, 'K_D': 0.05, 'K_I': 0.1}
# parameters overload
if opt_dict:
if 'lateral_control_dict' in opt_dict:
args_lateral_dict = opt_dict['lateral_control_dict']
if 'longitudinal_control_dict' in opt_dict:
args_longitudinal_dict = opt_dict['longitudinal_control_dict']
self._current_waypoint = self.get_waypoint(self._current_pose.position)
self._vehicle_controller = VehiclePIDController(
args_lateral=args_lateral_dict,
args_longitudinal=args_longitudinal_dict)
self._global_plan = False
def set_global_plan(self, current_plan):
"""
set a global plan to follow
"""
self._waypoints_queue.clear()
self._waypoint_buffer.clear()
for elem in current_plan:
self._waypoints_queue.append(elem.pose)
self._global_plan = True
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
class LocalPlanner(object):
"""
LocalPlanner implements the basic behavior of following a trajectory of waypoints that is
generated on-the-fly. The low-level motion of the vehicle is computed by using two PID
controllers, one is used for the lateral control and the other for the longitudinal
control (cruise speed).
When multiple paths are available (intersections) this local planner makes a random choice.
"""
# minimum distance to target waypoint as a percentage (e.g. within 90% of
# total distance)
MIN_DISTANCE_PERCENTAGE = 0.9
def __init__(self, opt_dict=None):
"""
:param vehicle: actor to apply to local planner logic onto
:param opt_dict: dictionary of arguments with the following semantics:
target_speed -- desired cruise speed in Km/h
sampling_radius -- search radius for next waypoints in seconds: e.g. 0.5 seconds ahead
lateral_control_dict -- dictionary of arguments to setup the lateral PID controller
{'K_P':, 'K_D':, 'K_I'}
longitudinal_control_dict -- dictionary of arguments to setup the longitudinal
PID controller
{'K_P':, 'K_D':, 'K_I'}
"""
self.target_route_point = None
self._vehicle_controller = None
self._waypoints_queue = deque(maxlen=20000)
self._buffer_size = 5
self._waypoint_buffer = deque(maxlen=self._buffer_size)
self._target_point_publisher = rospy.Publisher("/next_target",
PointStamped,
queue_size=1)
# initializing controller
self._init_controller(opt_dict)
def _init_controller(self, opt_dict):
"""
Controller initialization.
:param opt_dict: dictionary of arguments.
:return:
"""
# default params
args_lateral_dict = {'K_P': 1.95, 'K_D': 0.01, 'K_I': 1.4}
args_longitudinal_dict = {'K_P': 0.2, 'K_D': 0.05, 'K_I': 0.1}
# parameters overload
if opt_dict:
if 'lateral_control_dict' in opt_dict:
args_lateral_dict = opt_dict['lateral_control_dict']
if 'longitudinal_control_dict' in opt_dict:
args_longitudinal_dict = opt_dict['longitudinal_control_dict']
self._vehicle_controller = VehiclePIDController(
args_lateral=args_lateral_dict,
args_longitudinal=args_longitudinal_dict)
def set_global_plan(self, current_plan):
"""
set a global plan to follow
"""
self.target_route_point = None
self._waypoint_buffer.clear()
self._waypoints_queue.clear()
for elem in current_plan:
self._waypoints_queue.append(elem.pose)
def run_step(self, target_speed, current_speed, current_pose):
"""
Execute one step of local planning which involves running the longitudinal
and lateral PID controllers to follow the waypoints trajectory.
"""
if not self._waypoint_buffer and 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
rospy.loginfo("Route finished.")
return control, True
# 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
# target waypoint
self.target_route_point = self._waypoint_buffer[0]
target_point = PointStamped()
target_point.header.frame_id = "map"
target_point.point.x = self.target_route_point.position.x
target_point.point.y = self.target_route_point.position.y
target_point.point.z = self.target_route_point.position.z
self._target_point_publisher.publish(target_point)
# move using PID controllers
control = self._vehicle_controller.run_step(target_speed,
current_speed,
current_pose,
self.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,
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, False
class LocalPlanner(CompatibleNode):
"""
LocalPlanner implements the basic behavior of following a trajectory of waypoints that is
generated on-the-fly. The low-level motion of the vehicle is computed by using two PID
controllers, one is used for the lateral control and the other for the longitudinal
control (cruise speed).
When multiple paths are available (intersections) this local planner makes a random choice.
"""
# minimum distance to target waypoint as a percentage (e.g. within 90% of
# total distance)
MIN_DISTANCE_PERCENTAGE = 0.9
def __init__(self):
super(LocalPlanner, self).__init__('local_planner')
# ros parameters
role_name = self.get_param("role_name", "ego_vehicle")
self.control_time_step = self.get_param("control_time_step", 0.05)
args_lateral_dict = {}
args_lateral_dict['K_P'] = self.get_param("Kp_lateral", 0.9)
args_lateral_dict['K_I'] = self.get_param("Ki_lateral", 0.0)
args_lateral_dict['K_D'] = self.get_param("Kd_lateral", 0.0)
args_longitudinal_dict = {}
args_longitudinal_dict['K_P'] = self.get_param("Kp_longitudinal",
0.206)
args_longitudinal_dict['K_I'] = self.get_param("Ki_longitudinal",
0.0206)
args_longitudinal_dict['K_D'] = self.get_param("Kd_longitudinal",
0.515)
self.target_route_point = None
self._vehicle_controller = None
self._waypoints_queue = deque(maxlen=20000)
self._buffer_size = 5
self._waypoint_buffer = deque(maxlen=self._buffer_size)
self.path_received = False
self._current_speed = None
self._current_pose = None
self._target_speed = 0.0
# subscribers
self._odometry_subscriber = self.create_subscriber(
Odometry, "/carla/{}/odometry".format(role_name),
self.odometry_updated)
self._path_subscriber = self.create_subscriber(
Path, "/carla/{}/waypoints".format(role_name), self.path_updated,
QoSProfile(depth=1, durability=True))
self._target_speed_subscriber = self.create_subscriber(
Float64, "/carla/{}/speed_command".format(role_name),
self.target_speed_updated, QoSProfile(depth=1, durability=True))
# publishers
self._target_point_publisher = self.new_publisher(
Marker, "/carla/{}/next_target".format(role_name),
QoSProfile(depth=10, durability=False))
self._control_cmd_publisher = self.new_publisher(
CarlaEgoVehicleControl,
"/carla/{}/vehicle_control_cmd".format(role_name),
QoSProfile(depth=1, durability=False))
# initializing controller
self._vehicle_controller = VehiclePIDController(
self,
args_lateral=args_lateral_dict,
args_longitudinal=args_longitudinal_dict)
# wait for required messages
self.loginfo('Local planner waiting for a path and target speed...')
while self._current_pose is None or self._target_speed is None or self.path_received is False:
time.sleep(0.05)
if ROS_VERSION == 2:
rclpy.spin_once(self, timeout_sec=0)
def odometry_updated(self, new_pose):
"""
callback on new odometry
"""
self._current_speed = math.sqrt(new_pose.twist.twist.linear.x**2 +
new_pose.twist.twist.linear.y**2 +
new_pose.twist.twist.linear.z**2) * 3.6
self._current_pose = new_pose.pose.pose
def target_speed_updated(self, new_target_speed):
self._target_speed = new_target_speed.data
def path_updated(self, new_path):
"""
set a global plan to follow
"""
self.loginfo('New path to follow received.')
self.path_received = True
self.target_route_point = None
self._waypoint_buffer.clear()
self._waypoints_queue.clear()
for elem in new_path.poses:
self._waypoints_queue.append(elem.pose)
def run_step(self):
"""
Execute one step of local planning which involves running the longitudinal
and lateral PID controllers to follow the waypoints trajectory.
"""
if self.path_received is False:
return
if not self._waypoint_buffer and not self._waypoints_queue:
self.emergency_stop()
self.loginfo("Route finished. Waiting for a new one.")
self.path_received = False
return
# When target speed is 0, brake
if self._target_speed == 0.0:
self.emergency_stop()
return
# 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
# target waypoint
self.target_route_point = self._waypoint_buffer[0]
target_point = Marker()
target_point.type = 0
target_point.header.frame_id = "map"
target_point.pose = self.target_route_point
target_point.scale.x = 1.0
target_point.scale.y = 0.2
target_point.scale.z = 0.2
target_point.color.r = 255.0
target_point.color.a = 1.0
self._target_point_publisher.publish(target_point)
# move using PID controllers
control = self._vehicle_controller.run_step(self._target_speed,
self._current_speed,
self._current_pose,
self.target_route_point)
# purge the queue of obsolete waypoints
max_index = -1
sampling_radius = self._target_speed * 1 / 3.6 # search radius for next waypoints in seconds
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()
self._control_cmd_publisher.publish(control)
return
def emergency_stop(self):
control = CarlaEgoVehicleControl()
control.steer = 0.0
control.throttle = 0.0
control.brake = 1.0
control.hand_brake = False
control.manual_gear_shift = False
self._control_cmd_publisher.publish(control)