basic_autonomy.hpp

Go to the documentation of this file.

#pragma once
 
/*
 * Copyright (C) 2021-2022 LEIDOS.
 *
 * Licensed under the Apache License, Version 2.0 (the "License"); you may not
 * use this file except in compliance with the License. You may obtain a copy of
 * the License at
 *
 * http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
 * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
 * License for the specific language governing permissions and limitations under
 * the License.
 */
 
#include <iostream>
#include <vector>
#include <rclcpp/rclcpp.hpp>
#include <carma_planning_msgs/msg/trajectory_plan.hpp>
#include <carma_planning_msgs/msg/trajectory_plan_point.hpp>
#include <carma_planning_msgs/msg/plugin.h>
#include <boost/shared_ptr.hpp>
#include <carma_ros2_utils/containers/containers.hpp>
#include <trajectory_utils/trajectory_utils.hpp>
#include <trajectory_utils/conversions/conversions.hpp>
#include <boost/geometry.hpp>
#include <carma_wm/Geometry.hpp>
#include <carma_planning_msgs/srv/plan_trajectory.hpp>
#include <carma_wm/WMListener.hpp>
#include <functional>
#include <unordered_set>
#include <autoware_msgs/msg/lane.h>
#include <lanelet2_core/geometry/Point.h>
#include <basic_autonomy/impl/basic_autonomy.tpp>
#include <basic_autonomy/smoothing/SplineI.hpp>
#include <basic_autonomy/smoothing/BSpline.hpp>
#include <basic_autonomy/smoothing/filters.hpp>
#include <carma_debug_ros2_msgs/msg/trajectory_curvature_speeds.hpp>
#include <autoware_auto_msgs/msg/trajectory.hpp>
 
#define GET_MANEUVER_PROPERTY(mvr, property)\
        (((mvr).type == carma_planning_msgs::msg::Maneuver::INTERSECTION_TRANSIT_LEFT_TURN ? (mvr).intersection_transit_left_turn_maneuver.property :\
            ((mvr).type == carma_planning_msgs::msg::Maneuver::INTERSECTION_TRANSIT_RIGHT_TURN ? (mvr).intersection_transit_right_turn_maneuver.property :\
                ((mvr).type == carma_planning_msgs::msg::Maneuver::INTERSECTION_TRANSIT_STRAIGHT ? (mvr).intersection_transit_straight_maneuver.property :\
                    ((mvr).type == carma_planning_msgs::msg::Maneuver::LANE_CHANGE ? (mvr).lane_change_maneuver.property :\
                        (mvr).type == carma_planning_msgs::msg::Maneuver::LANE_FOLLOWING ? (mvr).lane_following_maneuver.property :\
                        (mvr).type == carma_planning_msgs::msg::Maneuver::STOP_AND_WAIT ? (mvr).lane_following_maneuver.property :\
                        throw new std::invalid_argument("GET_MANEUVER_PROPERTY (property) called on maneuver with invalid type id"))))))
 
namespace basic_autonomy
{
    namespace waypoint_generation
    {
        struct PointSpeedPair
        {
            lanelet::BasicPoint2d point;
            double speed = 0;
        };
 
        struct GeneralTrajConfig
        {
            std::string trajectory_type = "inlanecruising";
            int default_downsample_ratio = 36.0; // Amount to downsample input lanelet centerline data.
            int turn_downsample_ratio = 20.0;    // Amount to downsample input lanelet centerline data if the lanelet is marked as a turn
                                                 // Corresponds to saving each nth point
        };
 
        struct DetailedTrajConfig
        {
            double trajectory_time_length = 6.0;          // Trajectory length in seconds
            double curve_resample_step_size = 1.0;        // Curve re-sampling step size in m
            double minimum_speed = 2.2352;                // Minimum allowable speed in m/s
            double max_accel = 3;                         // Maximum allowable longitudinal acceleration in m/s^2
            double lateral_accel_limit = 2.5;             // Maximum allowable lateral acceleration m/s^2
            int speed_moving_average_window_size = 5;     // Size of the window used in the moving average filter to smooth both the speed profile
            int curvature_moving_average_window_size = 9; // Size of the window used in the moving average filter to smooth the curvature profile
                                                          // computed curvature and output speeds
            double back_distance = 20;                    // Number of meters behind the first maneuver that need to be included in points for curvature calculation
            double buffer_ending_downtrack = 20.0;        //The additional downtrack beyond requested end dist used to fit points along spline
            std::string desired_controller_plugin = "default"; //The desired controller plugin for the generated trajectory
        };
 
 
        std::vector<double> apply_speed_limits(const std::vector<double> speeds, const std::vector<double> speed_limits);
 
        Eigen::Isometry2d compute_heading_frame(const lanelet::BasicPoint2d &p1, const lanelet::BasicPoint2d &p2);
 
        std::vector<PointSpeedPair> constrain_to_time_boundary(const std::vector<PointSpeedPair> &points, double time_span);
 
        std::vector<carma_planning_msgs::msg::TrajectoryPlanPoint> constrain_to_time_boundary(
            const std::vector<carma_planning_msgs::msg::TrajectoryPlanPoint>& trajectory,
            double time_span);
 
        std::pair<double, size_t> min_with_exclusions(const std::vector<double> &values, const std::unordered_set<size_t> &excluded);
 
        std::vector<double> optimize_speed(const std::vector<double> &downtracks, const std::vector<double> &curv_speeds, double accel_limit);
 
        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);
 
        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);
 
        std::unique_ptr<basic_autonomy::smoothing::SplineI> compute_fit(const std::vector<lanelet::BasicPoint2d> &basic_points);
 
        double compute_curvature_at(const basic_autonomy::smoothing::SplineI &fit_curve, double step_along_the_curve);
 
        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);
        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);
 
        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);
 
        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);
 
       //Functions specific to lane change
        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);
 
        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);
 
 
        std::vector<std::vector<lanelet::BasicPoint2d>> resample_linestring_pair_to_same_size(std::vector<lanelet::BasicPoint2d>& line_1, std::vector<lanelet::BasicPoint2d>& line_2);
 
        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);
 
        std::vector<lanelet::BasicPoint2d> create_route_geom(double starting_downtrack, int starting_lane_id,
                                                             double ending_downtrack, const carma_wm::WorldModelConstPtr &wm);
 
        lanelet::BasicLineString2d create_lanechange_path(const lanelet::ConstLanelet &start_lanelet, const lanelet::ConstLanelet &end_lanelet);
 
        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");
 
        GeneralTrajConfig compose_general_trajectory_config(const std::string& trajectory_type,
                                                            int default_downsample_ratio,
                                                            int turn_downsample_ratio);
 
        autoware_auto_msgs::msg::Trajectory process_trajectory_plan(const carma_planning_msgs::msg::TrajectoryPlan& tp, double vehicle_response_lag);
 
        std::vector<double> apply_response_lag(const std::vector<double>& speeds, const std::vector<double> downtracks, double response_lag);
 
        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);
 
        bool is_valid_yield_plan(const std::shared_ptr<carma_ros2_utils::CarmaLifecycleNode>& node_handler, const carma_planning_msgs::msg::TrajectoryPlan& yield_plan);
 
    }
 
} // basic_autonomy