Carma-platform v4.2.0
CARMA Platform is built on robot operating system (ROS) and utilizes open source software (OSS) that enables Cooperative Driving Automation (CDA) features to allow Automated Driving Systems to interact and cooperate with infrastructure and other vehicles through communication.
basic_autonomy.hpp
Go to the documentation of this file.
1#pragma once
2
3/*
4 * Copyright (C) 2021-2022 LEIDOS.
5 *
6 * Licensed under the Apache License, Version 2.0 (the "License"); you may not
7 * use this file except in compliance with the License. You may obtain a copy of
8 * the License at
9 *
10 * http://www.apache.org/licenses/LICENSE-2.0
11 *
12 * Unless required by applicable law or agreed to in writing, software
13 * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
14 * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
15 * License for the specific language governing permissions and limitations under
16 * the License.
17 */
18
19#include <iostream>
20#include <vector>
21#include <rclcpp/rclcpp.hpp>
22#include <carma_planning_msgs/msg/trajectory_plan.hpp>
23#include <carma_planning_msgs/msg/trajectory_plan_point.hpp>
24#include <carma_planning_msgs/msg/plugin.h>
25#include <boost/shared_ptr.hpp>
26#include <carma_ros2_utils/containers/containers.hpp>
27#include <trajectory_utils/trajectory_utils.hpp>
28#include <trajectory_utils/conversions/conversions.hpp>
29#include <boost/geometry.hpp>
30#include <carma_wm/Geometry.hpp>
31#include <carma_planning_msgs/srv/plan_trajectory.hpp>
33#include <functional>
34#include <unordered_set>
35#include <autoware_msgs/msg/lane.h>
36#include <lanelet2_core/geometry/Point.h>
37#include <basic_autonomy/impl/basic_autonomy.tpp>
41#include <carma_debug_ros2_msgs/msg/trajectory_curvature_speeds.hpp>
42#include <autoware_auto_msgs/msg/trajectory.hpp>
43
50#define GET_MANEUVER_PROPERTY(mvr, property)\
51 (((mvr).type == carma_planning_msgs::msg::Maneuver::INTERSECTION_TRANSIT_LEFT_TURN ? (mvr).intersection_transit_left_turn_maneuver.property :\
52 ((mvr).type == carma_planning_msgs::msg::Maneuver::INTERSECTION_TRANSIT_RIGHT_TURN ? (mvr).intersection_transit_right_turn_maneuver.property :\
53 ((mvr).type == carma_planning_msgs::msg::Maneuver::INTERSECTION_TRANSIT_STRAIGHT ? (mvr).intersection_transit_straight_maneuver.property :\
54 ((mvr).type == carma_planning_msgs::msg::Maneuver::LANE_CHANGE ? (mvr).lane_change_maneuver.property :\
55 (mvr).type == carma_planning_msgs::msg::Maneuver::LANE_FOLLOWING ? (mvr).lane_following_maneuver.property :\
56 (mvr).type == carma_planning_msgs::msg::Maneuver::STOP_AND_WAIT ? (mvr).lane_following_maneuver.property :\
57 throw new std::invalid_argument("GET_MANEUVER_PROPERTY (property) called on maneuver with invalid type id"))))))
58
60{
61 namespace waypoint_generation
62 {
63 static const std::string BASIC_AUTONOMY_LOGGER = "basic_autonomy";
64
66 {
67 lanelet::BasicPoint2d point;
68 double speed = 0;
69 };
70
72 {
73 std::string trajectory_type = "inlanecruising";
74 int default_downsample_ratio = 36.0; // Amount to downsample input lanelet centerline data.
75 int turn_downsample_ratio = 20.0; // Amount to downsample input lanelet centerline data if the lanelet is marked as a turn
76 // Corresponds to saving each nth point
77 };
78
80 {
81 double trajectory_time_length = 6.0; // Trajectory length in seconds
82 double curve_resample_step_size = 1.0; // Curve re-sampling step size in m
83 double minimum_speed = 2.2352; // Minimum allowable speed in m/s
84 double max_accel = 3; // Maximum allowable longitudinal acceleration in m/s^2
85 double lateral_accel_limit = 2.5; // Maximum allowable lateral acceleration m/s^2
86 int speed_moving_average_window_size = 5; // Size of the window used in the moving average filter to smooth both the speed profile
87 int curvature_moving_average_window_size = 9; // Size of the window used in the moving average filter to smooth the curvature profile
88 // computed curvature and output speeds
89 double back_distance = 20; // Number of meters behind the first maneuver that need to be included in points for curvature calculation
90 double buffer_ending_downtrack = 20.0; //The additional downtrack beyond requested end dist used to fit points along spline
91 std::string desired_controller_plugin = "default"; //The desired controller plugin for the generated trajectory
92 };
93
94
103 std::vector<double> apply_speed_limits(const std::vector<double> speeds, const std::vector<double> speed_limits);
104
113 Eigen::Isometry2d compute_heading_frame(const lanelet::BasicPoint2d &p1, const lanelet::BasicPoint2d &p2);
114
123 std::vector<PointSpeedPair> constrain_to_time_boundary(const std::vector<PointSpeedPair> &points, double time_span);
124
134 std::vector<carma_planning_msgs::msg::TrajectoryPlanPoint> constrain_to_time_boundary(
135 const std::vector<carma_planning_msgs::msg::TrajectoryPlanPoint>& trajectory,
136 double time_span);
137
145 std::pair<double, size_t> min_with_exclusions(const std::vector<double> &values, const std::unordered_set<size_t> &excluded);
146
156 std::vector<double> optimize_speed(const std::vector<double> &downtracks, const std::vector<double> &curv_speeds, double accel_limit);
157
171 std::vector<carma_planning_msgs::msg::TrajectoryPlanPoint> trajectory_from_points_times_orientations(
172 const std::vector<lanelet::BasicPoint2d> &points, const std::vector<double> &times,
173 const std::vector<double> &yaws, rclcpp::Time startTime, const std::string &desired_controller_plugin);
174
186 std::vector<PointSpeedPair> attach_past_points(const std::vector<PointSpeedPair> &points_set, std::vector<PointSpeedPair> future_points,
187 const int nearest_pt_index, double back_distance);
188
195 std::unique_ptr<basic_autonomy::smoothing::SplineI> compute_fit(const std::vector<lanelet::BasicPoint2d> &basic_points);
196
204 double compute_curvature_at(const basic_autonomy::smoothing::SplineI &fit_curve, double step_along_the_curve);
205
218 std::vector<PointSpeedPair> create_geometry_profile(const std::vector<carma_planning_msgs::msg::Maneuver> &maneuvers, double max_starting_downtrack, const carma_wm::WorldModelConstPtr &wm,
219 carma_planning_msgs::msg::VehicleState &ending_state_before_buffer,
220 const carma_planning_msgs::msg::VehicleState& state,const GeneralTrajConfig &general_config,
221 const DetailedTrajConfig &detailed_config);
233 std::vector<PointSpeedPair> create_lanefollow_geometry(const carma_planning_msgs::msg::Maneuver &maneuver, double max_starting_downtrack,
234 const carma_wm::WorldModelConstPtr &wm, const GeneralTrajConfig &general_config,
235 const DetailedTrajConfig &detailed_config, std::unordered_set<lanelet::Id>& visited_lanelets);
236
248 std::vector<PointSpeedPair> add_lanefollow_buffer(const carma_wm::WorldModelConstPtr &wm, std::vector<PointSpeedPair>& points_and_target_speeds, const std::vector<carma_planning_msgs::msg::Maneuver> &maneuvers,
249 carma_planning_msgs::msg::VehicleState &ending_state_before_buffer, const DetailedTrajConfig &detailed_config);
250
260 std::vector<carma_planning_msgs::msg::TrajectoryPlanPoint>
261 compose_lanefollow_trajectory_from_path(const std::vector<PointSpeedPair> &points, const carma_planning_msgs::msg::VehicleState &state,
262 const rclcpp::Time &state_time, const carma_wm::WorldModelConstPtr &wm,
263 const carma_planning_msgs::msg::VehicleState &ending_state_before_buffer, carma_debug_ros2_msgs::msg::TrajectoryCurvatureSpeeds& debug_msg,
264 const DetailedTrajConfig &detailed_config);
265
266 //Functions specific to lane change
282 std::vector<PointSpeedPair> get_lanechange_points_from_maneuver(const carma_planning_msgs::msg::Maneuver &maneuver, double max_starting_downtrack,
283 const carma_wm::WorldModelConstPtr &wm, carma_planning_msgs::msg::VehicleState &ending_state_before_buffer,
284 const carma_planning_msgs::msg::VehicleState &state, const GeneralTrajConfig &general_config,const DetailedTrajConfig &detailed_config);
285
299 std::vector<lanelet::BasicPoint2d> create_lanechange_geometry(lanelet::Id starting_lane_id, lanelet::Id ending_lane_id, double starting_downtrack, double ending_downtrack,
300 const carma_wm::WorldModelConstPtr &wm, int downsample_ratio, double buffer_ending_downtrack);
301
302
312 std::vector<std::vector<lanelet::BasicPoint2d>> resample_linestring_pair_to_same_size(std::vector<lanelet::BasicPoint2d>& line_1, std::vector<lanelet::BasicPoint2d>& line_2);
313
326 std::vector<carma_planning_msgs::msg::TrajectoryPlanPoint> compose_lanechange_trajectory_from_path(
327 const std::vector<PointSpeedPair> &points, const carma_planning_msgs::msg::VehicleState &state, const rclcpp::Time &state_time,
328 const carma_wm::WorldModelConstPtr &wm, const carma_planning_msgs::msg::VehicleState &ending_state_before_buffer,
329 const DetailedTrajConfig &detailed_config);
330
339 std::vector<lanelet::BasicPoint2d> create_route_geom(double starting_downtrack, int starting_lane_id,
340 double ending_downtrack, const carma_wm::WorldModelConstPtr &wm);
341
349 lanelet::BasicLineString2d create_lanechange_path(const lanelet::ConstLanelet &start_lanelet, const lanelet::ConstLanelet &end_lanelet);
350
351 DetailedTrajConfig compose_detailed_trajectory_config(double trajectory_time_length,
352 double curve_resample_step_size,
353 double minimum_speed,
354 double max_accel,
355 double lateral_accel_limit,
356 int speed_moving_average_window_size,
357 int curvature_moving_average_window_size,
358 double back_distance,
359 double buffer_ending_downtrack,
360 std::string desired_controller_plugin = "default");
361
362 GeneralTrajConfig compose_general_trajectory_config(const std::string& trajectory_type,
363 int default_downsample_ratio,
364 int turn_downsample_ratio);
365
373 autoware_auto_msgs::msg::Trajectory process_trajectory_plan(const carma_planning_msgs::msg::TrajectoryPlan& tp, double vehicle_response_lag);
374
383 std::vector<double> apply_response_lag(const std::vector<double>& speeds, const std::vector<double> downtracks, double response_lag);
384
395 carma_planning_msgs::srv::PlanTrajectory::Response::SharedPtr modify_trajectory_to_yield_to_obstacles(
396 const std::shared_ptr<carma_ros2_utils::CarmaLifecycleNode>& node_handler,
397 const carma_planning_msgs::srv::PlanTrajectory::Request::SharedPtr& req,
398 const carma_planning_msgs::srv::PlanTrajectory::Response::SharedPtr& resp,
399 const carma_ros2_utils::ClientPtr<carma_planning_msgs::srv::PlanTrajectory>& yield_client,
400 int yield_plugin_service_call_timeout);
401
409 bool is_valid_yield_plan(const std::shared_ptr<carma_ros2_utils::CarmaLifecycleNode>& node_handler, const carma_planning_msgs::msg::TrajectoryPlan& yield_plan);
410
411 }
412
413} // basic_autonomy
Interface to a spline interpolator that can be used to smoothly interpolate between points.
Definition: SplineI.hpp:30
GeneralTrajConfig compose_general_trajectory_config(const std::string &trajectory_type, int default_downsample_ratio, int turn_downsample_ratio)
std::vector< std::vector< lanelet::BasicPoint2d > > resample_linestring_pair_to_same_size(std::vector< lanelet::BasicPoint2d > &line_1, std::vector< lanelet::BasicPoint2d > &line_2)
Resamples a pair of basicpoint2d lines to get lines of same number of points.
std::vector< lanelet::BasicPoint2d > create_lanechange_geometry(lanelet::Id starting_lane_id, lanelet::Id ending_lane_id, double starting_downtrack, double ending_downtrack, const carma_wm::WorldModelConstPtr &wm, int downsample_ratio, double buffer_ending_downtrack)
Creates a vector of lane change points using parameters defined.
carma_planning_msgs::srv::PlanTrajectory::Response::SharedPtr modify_trajectory_to_yield_to_obstacles(const std::shared_ptr< carma_ros2_utils::CarmaLifecycleNode > &node_handler, const carma_planning_msgs::srv::PlanTrajectory::Request::SharedPtr &req, const carma_planning_msgs::srv::PlanTrajectory::Response::SharedPtr &resp, const carma_ros2_utils::ClientPtr< carma_planning_msgs::srv::PlanTrajectory > &yield_client, int yield_plugin_service_call_timeout)
Applies a yield trajectory to the original trajectory set in response.
std::vector< double > apply_response_lag(const std::vector< double > &speeds, const std::vector< double > downtracks, double response_lag)
Applies a specified response lag in seconds to the trajectory shifting the whole thing by the specifi...
static const std::string BASIC_AUTONOMY_LOGGER
std::vector< carma_planning_msgs::msg::TrajectoryPlanPoint > compose_lanechange_trajectory_from_path(const std::vector< PointSpeedPair > &points, const carma_planning_msgs::msg::VehicleState &state, const rclcpp::Time &state_time, const carma_wm::WorldModelConstPtr &wm, const carma_planning_msgs::msg::VehicleState &ending_state_before_buffer, const DetailedTrajConfig &detailed_config)
Method converts a list of lanelet centerline points and current vehicle state into a usable list of t...
std::vector< double > optimize_speed(const std::vector< double > &downtracks, const std::vector< double > &curv_speeds, double accel_limit)
Applies the longitudinal acceleration limit to each point's speed.
DetailedTrajConfig compose_detailed_trajectory_config(double trajectory_time_length, double curve_resample_step_size, double minimum_speed, double max_accel, double lateral_accel_limit, int speed_moving_average_window_size, int curvature_moving_average_window_size, double back_distance, double buffer_ending_downtrack, std::string desired_controller_plugin="default")
std::vector< lanelet::BasicPoint2d > create_route_geom(double starting_downtrack, int starting_lane_id, double ending_downtrack, const carma_wm::WorldModelConstPtr &wm)
Creates a Lanelet2 Linestring from a vector or points along the geometry.
bool is_valid_yield_plan(const std::shared_ptr< carma_ros2_utils::CarmaLifecycleNode > &node_handler, const carma_planning_msgs::msg::TrajectoryPlan &yield_plan)
Helper function to verify if the input yield trajectory plan is valid.
std::pair< double, size_t > min_with_exclusions(const std::vector< double > &values, const std::unordered_set< size_t > &excluded)
Returns the min, and its idx, from the vector of values, excluding given set of values.
std::unique_ptr< basic_autonomy::smoothing::SplineI > compute_fit(const std::vector< lanelet::BasicPoint2d > &basic_points)
Computes a spline based on the provided points.
lanelet::BasicLineString2d create_lanechange_path(const lanelet::ConstLanelet &start_lanelet, const lanelet::ConstLanelet &end_lanelet)
Given a start and end point, create a vector of points fit through a spline between the points (using...
std::vector< PointSpeedPair > create_lanefollow_geometry(const carma_planning_msgs::msg::Maneuver &maneuver, double max_starting_downtrack, const carma_wm::WorldModelConstPtr &wm, const GeneralTrajConfig &general_config, const DetailedTrajConfig &detailed_config, std::unordered_set< lanelet::Id > &visited_lanelets)
Converts a set of requested LANE_FOLLOWING maneuvers to point speed limit pairs.
std::vector< double > apply_speed_limits(const std::vector< double > speeds, const std::vector< double > speed_limits)
Applies the provided speed limits to the provided speeds such that each element is capped at its corr...
std::vector< carma_planning_msgs::msg::TrajectoryPlanPoint > trajectory_from_points_times_orientations(const std::vector< lanelet::BasicPoint2d > &points, const std::vector< double > &times, const std::vector< double > &yaws, rclcpp::Time startTime, const std::string &desired_controller_plugin)
Method combines input points, times, orientations, and an absolute start time to form a valid carma p...
std::vector< PointSpeedPair > create_geometry_profile(const std::vector< carma_planning_msgs::msg::Maneuver > &maneuvers, double max_starting_downtrack, const carma_wm::WorldModelConstPtr &wm, carma_planning_msgs::msg::VehicleState &ending_state_before_buffer, const carma_planning_msgs::msg::VehicleState &state, const GeneralTrajConfig &general_config, const DetailedTrajConfig &detailed_config)
Creates geometry profile to return a point speed pair struct for LANE FOLLOW and LANE CHANGE maneuver...
std::vector< PointSpeedPair > add_lanefollow_buffer(const carma_wm::WorldModelConstPtr &wm, std::vector< PointSpeedPair > &points_and_target_speeds, const std::vector< carma_planning_msgs::msg::Maneuver > &maneuvers, carma_planning_msgs::msg::VehicleState &ending_state_before_buffer, const DetailedTrajConfig &detailed_config)
Adds extra centerline points beyond required message length to lane follow maneuver points so that th...
std::vector< carma_planning_msgs::msg::TrajectoryPlanPoint > compose_lanefollow_trajectory_from_path(const std::vector< PointSpeedPair > &points, const carma_planning_msgs::msg::VehicleState &state, const rclcpp::Time &state_time, const carma_wm::WorldModelConstPtr &wm, const carma_planning_msgs::msg::VehicleState &ending_state_before_buffer, carma_debug_ros2_msgs::msg::TrajectoryCurvatureSpeeds &debug_msg, const DetailedTrajConfig &detailed_config)
Method converts a list of lanelet centerline points and current vehicle state into a usable list of t...
Eigen::Isometry2d compute_heading_frame(const lanelet::BasicPoint2d &p1, const lanelet::BasicPoint2d &p2)
Returns a 2D coordinate frame which is located at p1 and oriented so p2 lies on the +X axis.
double compute_curvature_at(const basic_autonomy::smoothing::SplineI &fit_curve, double step_along_the_curve)
Given the curvature fit, computes the curvature at the given step along the curve.
autoware_auto_msgs::msg::Trajectory process_trajectory_plan(const carma_planning_msgs::msg::TrajectoryPlan &tp, double vehicle_response_lag)
Given a carma type of trajectory_plan, generate autoware type of trajectory accounting for speed_lag ...
std::vector< PointSpeedPair > constrain_to_time_boundary(const std::vector< PointSpeedPair > &points, double time_span)
Reduces the input points to only those points that fit within the provided time boundary.
std::vector< PointSpeedPair > get_lanechange_points_from_maneuver(const carma_planning_msgs::msg::Maneuver &maneuver, double max_starting_downtrack, const carma_wm::WorldModelConstPtr &wm, carma_planning_msgs::msg::VehicleState &ending_state_before_buffer, const carma_planning_msgs::msg::VehicleState &state, const GeneralTrajConfig &general_config, const DetailedTrajConfig &detailed_config)
Converts a set of requested LANE_CHANGE maneuvers to point speed limit pairs.
std::vector< PointSpeedPair > attach_past_points(const std::vector< PointSpeedPair > &points_set, std::vector< PointSpeedPair > future_points, const int nearest_pt_index, double back_distance)
Attaches back_distance length of points behind the future points.
std::shared_ptr< const WorldModel > WorldModelConstPtr
Definition: WorldModel.hpp:454