Class containing primary business logic for the Light Controlled Intersection Tactical Plugin. More...

#include <light_controlled_intersection_tactical_plugin.hpp>

Collaboration diagram for light_controlled_intersection_tactical_plugin::LightControlledIntersectionTacticalPlugin:

Public Member Functions
	LightControlledIntersectionTacticalPlugin (carma_wm::WorldModelConstPtr wm, const Config &config, const std::string &plugin_name, std::shared_ptr< carma_ros2_utils::CarmaLifecycleNode > nh)
	LightControlledIntersectionTacticalPlugin constructor. More...

void	planTrajectoryCB (carma_planning_msgs::srv::PlanTrajectory::Request::SharedPtr req, carma_planning_msgs::srv::PlanTrajectory::Response::SharedPtr resp)
	Function to process the light controlled intersection tactical plugin service call for trajectory planning. More...

void	set_yield_client (carma_ros2_utils::ClientPtr< carma_planning_msgs::srv::PlanTrajectory > client)
	set the yield service More...

void	setConfig (const Config &config)
	Setter function to set a new config for this object. More...

Private Member Functions
void	applyTrajectorySmoothingAlgorithm (const carma_wm::WorldModelConstPtr &wm, std::vector< PointSpeedPair > &points_and_target_speeds, double start_dist, double remaining_dist, double starting_speed, double departure_speed, TrajectoryParams tsp)
	Creates a speed profile according to case one or two of the light controlled intersection, where the vehicle accelerates (then cruises if needed) and decelerates into the intersection. More...

void	applyOptimizedTargetSpeedProfile (const carma_planning_msgs::msg::Maneuver &maneuver, const double starting_speed, std::vector< PointSpeedPair > &points_and_target_speeds)
	Apply optimized target speeds to the trajectory determined for fixed-time and actuated signals. Based on TSMO USE CASE 2. Chapter 2. Trajectory Smoothing. More...

std::vector< PointSpeedPair >	createGeometryProfile (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 INTERSECTION_TRANSIT maneuver types (by converting it to LANE_FOLLOW) More...

double	findSpeedLimit (const lanelet::ConstLanelet &llt, const carma_wm::WorldModelConstPtr &wm) const
	Given a Lanelet, find it's associated Speed Limit. More...

	FRIEND_TEST (LCITacticalPluginTest, applyTrajectorySmoothingAlgorithm)

	FRIEND_TEST (LCITacticalPluginTest, applyOptimizedTargetSpeedProfile)

	FRIEND_TEST (LCITacticalPluginTest, createGeometryProfile)

	FRIEND_TEST (LCITacticalPluginTest, planTrajectoryCB)

	FRIEND_TEST (LCITacticalPluginTest, setConfig)

Private Attributes
carma_wm::WorldModelConstPtr	wm_

Config	config_

std::shared_ptr< carma_ros2_utils::CarmaLifecycleNode >	nh_

uint32_t	street_msg_timestamp_ = 0.0

uint32_t	scheduled_stop_time_ = 0.0

uint32_t	scheduled_enter_time_ = 0.0

uint32_t	scheduled_depart_time_ = 0.0

uint32_t	scheduled_latest_depart_time_ = 0.0

bool	is_allowed_int_ = false

double	speed_limit_ = 11.176

boost::optional< TSCase >	last_case_

boost::optional< bool >	is_last_case_successful_

carma_planning_msgs::msg::TrajectoryPlan	last_trajectory_

carma_ros2_utils::ClientPtr< carma_planning_msgs::srv::PlanTrajectory >	yield_client_

carma_planning_msgs::msg::VehicleState	ending_state_before_buffer_

double	epsilon_ = 0.001

double	current_downtrack_ = 0.0

double	last_successful_ending_downtrack_

double	last_successful_scheduled_entry_time_

std::string	plugin_name_

carma_debug_ros2_msgs::msg::TrajectoryCurvatureSpeeds	debug_msg_

std::vector< double >	last_final_speeds_

std::string	light_controlled_intersection_strategy_ = "Carma/signalized_intersection"

Detailed Description

Class containing primary business logic for the Light Controlled Intersection Tactical Plugin.

Definition at line 87 of file light_controlled_intersection_tactical_plugin.hpp.

Constructor & Destructor Documentation

◆ LightControlledIntersectionTacticalPlugin()

light_controlled_intersection_tactical_plugin::LightControlledIntersectionTacticalPlugin::LightControlledIntersectionTacticalPlugin	(	carma_wm::WorldModelConstPtr	wm,
		const Config &	config,
		const std::string &	plugin_name,
		std::shared_ptr< carma_ros2_utils::CarmaLifecycleNode >	nh
	)

LightControlledIntersectionTacticalPlugin constructor.

Definition at line 23 of file light_controlled_intersection_tactical_plugin.cpp.

        :wm_(wm), config_(config), plugin_name_(plugin_name), nh_(nh)
    {
    }

Member Function Documentation

◆ applyOptimizedTargetSpeedProfile()

void light_controlled_intersection_tactical_plugin::LightControlledIntersectionTacticalPlugin::applyOptimizedTargetSpeedProfile	(	const carma_planning_msgs::msg::Maneuver &	maneuver,
		const double	starting_speed,
		std::vector< PointSpeedPair > &	points_and_target_speeds
	)

private

Apply optimized target speeds to the trajectory determined for fixed-time and actuated signals. Based on TSMO USE CASE 2. Chapter 2. Trajectory Smoothing.

Parameters

maneuver	Maneuver associated that has starting downtrack and desired entry time
starting_speed	Starting speed of the vehicle
points	The set of points with raw speed limits whose speed profile to be changed. These points must be in the same lane as the vehicle and must extend in front of it though it is fine if they also extend behind it.

Definition at line 356 of file light_controlled_intersection_tactical_plugin.cpp.

    {
        if(GET_MANEUVER_PROPERTY(maneuver, parameters.float_valued_meta_data).size() < 9 ||
            GET_MANEUVER_PROPERTY(maneuver, parameters.int_valued_meta_data).size() < 2 ){
            throw std::invalid_argument("There must be 9 float_valued_meta_data and 2 int_valued_meta_data to apply algorithm's parameters.");
        }
 
        TrajectoryParams tsp;
 
        tsp.a1_ = GET_MANEUVER_PROPERTY(maneuver, parameters.float_valued_meta_data[0]);
        tsp.v1_ = GET_MANEUVER_PROPERTY(maneuver, parameters.float_valued_meta_data[1]);
        tsp.x1_ = GET_MANEUVER_PROPERTY(maneuver, parameters.float_valued_meta_data[2]);
 
        tsp.a2_ = GET_MANEUVER_PROPERTY(maneuver, parameters.float_valued_meta_data[3]);
        tsp.v2_ = GET_MANEUVER_PROPERTY(maneuver, parameters.float_valued_meta_data[4]);
        tsp.x2_ = GET_MANEUVER_PROPERTY(maneuver, parameters.float_valued_meta_data[5]);
 
        tsp.a3_ = GET_MANEUVER_PROPERTY(maneuver, parameters.float_valued_meta_data[6]);
        tsp.v3_ = GET_MANEUVER_PROPERTY(maneuver, parameters.float_valued_meta_data[7]);
        tsp.x3_ = GET_MANEUVER_PROPERTY(maneuver, parameters.float_valued_meta_data[8]);
 
        double starting_downtrack = GET_MANEUVER_PROPERTY(maneuver, start_dist);
        double ending_downtrack = GET_MANEUVER_PROPERTY(maneuver, end_dist);
        double departure_speed = GET_MANEUVER_PROPERTY(maneuver, end_speed);
        double scheduled_entry_time = rclcpp::Time(GET_MANEUVER_PROPERTY(maneuver, end_time)).seconds();
        double entry_dist = ending_downtrack - starting_downtrack;
 
        // change speed profile depending on algorithm case starting from maneuver start_dist
        applyTrajectorySmoothingAlgorithm(wm_, points_and_target_speeds, starting_downtrack, entry_dist, starting_speed,
                                                departure_speed, tsp);
    }

Referenced by planTrajectoryCB().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ applyTrajectorySmoothingAlgorithm()

void light_controlled_intersection_tactical_plugin::LightControlledIntersectionTacticalPlugin::applyTrajectorySmoothingAlgorithm	(	const carma_wm::WorldModelConstPtr &	wm,
		std::vector< PointSpeedPair > &	points_and_target_speeds,
		double	start_dist,
		double	remaining_dist,
		double	starting_speed,
		double	departure_speed,
		TrajectoryParams	tsp
	)

private

Creates a speed profile according to case one or two of the light controlled intersection, where the vehicle accelerates (then cruises if needed) and decelerates into the intersection.

Parameters

wm	world_model pointer
points_and_target_speeds	of centerline points paired with speed limits whose speeds are to be modified:
start_dist	starting downtrack of the maneuver to be planned (excluding buffer points) in m
remaining_dist	distance for the maneuver to be planned (excluding buffer points) in m
starting_speed	starting speed at the start of the maneuver in m/s
departure_speed	ending speed of the maneuver a.k.a entry speed into the intersection m/s
tsp	trajectory smoothing parameters NOTE: Cruising speed profile is applied (case 1) if speed before deceleration is higher than speed limit. Otherwise Case 2. NOTE: when applying the speed profile, the function ignores buffer points beyond start_dist and end_dist. Internally uses: config_.back_distance and speed_limit_

Definition at line 269 of file light_controlled_intersection_tactical_plugin.cpp.

    {
        if (points_and_target_speeds.empty())
        {
            throw std::invalid_argument("Point and target speed list is empty! Unable to apply case one speed profile...");
        }
 
        // Checking route geometry start against start_dist and adjust profile
        double planning_downtrack_start = wm->routeTrackPos(points_and_target_speeds[0].point).downtrack; // this can include buffered points earlier than maneuver start_dist
 
        //Check calculated total dist against maneuver limits
        double total_distance_needed = remaining_dist;
        double dist1 = tsp.x1_ - start_dist;
        double dist2 = tsp.x2_ - start_dist;
        double dist3 = tsp.x3_ - start_dist;
 
        RCLCPP_DEBUG_STREAM(rclcpp::get_logger("light_controlled_intersection_tactical_plugin"), "total_distance_needed: " << total_distance_needed << "\n" <<
                        "dist1: " << dist1 << "\n" <<
                        "dist2: " << dist2 << "\n" <<
                        "dist3: " << dist3);
        double algo_min_speed = std::min({tsp.v1_,tsp.v2_,tsp.v3_});
        double algo_max_speed = std::max({tsp.v1_,tsp.v2_,tsp.v3_});
 
        RCLCPP_DEBUG_STREAM(rclcpp::get_logger("light_controlled_intersection_tactical_plugin"), "found algo_minimum_speed: " << algo_min_speed << "\n" <<
                        "algo_max_speed: " << algo_max_speed);
 
        double total_dist_planned = 0; //Starting dist for maneuver treated as 0.0
 
        if (planning_downtrack_start < start_dist)
        {
            //Account for the buffer distance that is technically not part of this maneuver
 
            total_dist_planned = planning_downtrack_start - start_dist;
            RCLCPP_DEBUG_STREAM(rclcpp::get_logger("light_controlled_intersection_tactical_plugin"), "buffered section is present. Adjusted total_dist_planned to: " << total_dist_planned);
        }
 
        double prev_speed = starting_speed;
        auto prev_point = points_and_target_speeds.front();
 
        for(auto& p : points_and_target_speeds)
        {
            double delta_d = lanelet::geometry::distance2d(prev_point.point, p.point);
            total_dist_planned += delta_d;
 
            //Apply the speed from algorithm at dist covered
            //Kinematic: v_f = sqrt(v_o^2 + 2*a*d)
            double speed_i;
            if (total_dist_planned <= epsilon_)
            {
                //Keep target speed same for buffer distance portion
                speed_i = starting_speed;
            }
            else if(total_dist_planned <= dist1 + epsilon_){
                //First segment
                speed_i = sqrt(pow(starting_speed, 2) + 2 * tsp.a1_ * total_dist_planned);
            }
            else if(total_dist_planned > dist1 && total_dist_planned <= dist2 + epsilon_){
                //Second segment
                speed_i = sqrt(std::max(pow(tsp.v1_, 2) + 2 * tsp.a2_ * (total_dist_planned - dist1), 0.0)); //std::max to ensure negative value is not sqrt
            }
            else if (total_dist_planned > dist2 && total_dist_planned <= dist3 + epsilon_)
            {
                //Third segment
                speed_i = sqrt(std::max(pow(tsp.v2_, 2) + 2 * tsp.a3_ * (total_dist_planned - dist2), 0.0)); //std::max to ensure negative value is not sqrt
            }
            else
            {
                //buffer points that will be cut
                speed_i = prev_speed;
            }
 
            if (isnan(speed_i))
            {
                speed_i = std::max(config_.minimum_speed, algo_min_speed);
                RCLCPP_DEBUG_STREAM(rclcpp::get_logger("light_controlled_intersection_tactical_plugin"), "Detected nan number from equations. Set to " << speed_i);
            }
 
            p.speed = std::max({speed_i, config_.minimum_speed, algo_min_speed});
            p.speed = std::min({p.speed, speed_limit_, algo_max_speed});
            RCLCPP_DEBUG_STREAM(rclcpp::get_logger("light_controlled_intersection_tactical_plugin"), "Applied speed: " << p.speed << ", at dist: " << total_dist_planned);
 
            prev_point = p;
            prev_speed = p.speed;
        }
    }

Referenced by applyOptimizedTargetSpeedProfile().

Here is the caller graph for this function:

◆ createGeometryProfile()

std::vector< PointSpeedPair > light_controlled_intersection_tactical_plugin::LightControlledIntersectionTacticalPlugin::createGeometryProfile	(	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
	)

private

Creates geometry profile to return a point speed pair struct for INTERSECTION_TRANSIT maneuver types (by converting it to LANE_FOLLOW)

Parameters

maneuvers	The list of maneuvers to convert to geometry points and calculate associated raw speed limits
max_starting_downtrack	The maximum downtrack that is allowed for the first maneuver. This should be set to the vehicle position or earlier. If the first maneuver exceeds this then it's downtrack will be shifted to this value.
wm	Pointer to intialized world model for semantic map access
ending_state_before_buffer	reference to Vehicle state, which is state before applying extra points for curvature calculation that are removed later
state	The vehicle state at the time the function is called
general_config	Basic autonomy struct defined to load general config parameters from tactical plugins
detailed_config	Basic autonomy struct defined to load detailed config parameters from tactical plugins

Returns: A vector of point speed pair struct which contains geometry points as basicpoint::lanelet2d and speed as a double for the maneuver NOTE: This function is a slightly modified version of the same function in basic_autonomy library and currently only plans for the first maneuver

Definition at line 401 of file light_controlled_intersection_tactical_plugin.cpp.

    {
        std::vector<PointSpeedPair> points_and_target_speeds;
 
        bool first = true;
        std::unordered_set<lanelet::Id> visited_lanelets;
        std::vector<carma_planning_msgs::msg::Maneuver> processed_maneuvers;
        RCLCPP_DEBUG_STREAM(rclcpp::get_logger("light_controlled_intersection_tactical_plugin"), "VehDowntrack: "<<max_starting_downtrack);
 
        // Only one maneuver is expected in the received maneuver plan
        if(maneuvers.size() == 1)
        {
            auto maneuver = maneuvers.front();
 
            double starting_downtrack = GET_MANEUVER_PROPERTY(maneuver, start_dist);
 
            starting_downtrack = std::min(starting_downtrack, max_starting_downtrack);
 
            RCLCPP_DEBUG_STREAM(rclcpp::get_logger("light_controlled_intersection_tactical_plugin"), "Used downtrack: " << starting_downtrack);
 
            // check if required parameter from strategic planner is present
            if(GET_MANEUVER_PROPERTY(maneuver, parameters.float_valued_meta_data).empty())
            {
                throw std::invalid_argument("No time_to_schedule_entry is provided in float_valued_meta_data");
            }
 
            RCLCPP_DEBUG_STREAM(rclcpp::get_logger("light_controlled_intersection_tactical_plugin"), "Creating Lane Follow Geometry");
            std::vector<PointSpeedPair> lane_follow_points = basic_autonomy::waypoint_generation::create_lanefollow_geometry(maneuver, starting_downtrack, wm, general_config, detailed_config, visited_lanelets);
            points_and_target_speeds.insert(points_and_target_speeds.end(), lane_follow_points.begin(), lane_follow_points.end());
            processed_maneuvers.push_back(maneuver);
        }
        else
        {
            throw std::invalid_argument("Light Control Intersection Plugin can only create a geometry profile for one maneuver");
        }
 
        //Add buffer ending to lane follow points at the end of maneuver(s) end dist
        if(!processed_maneuvers.empty() && processed_maneuvers.back().type == carma_planning_msgs::msg::Maneuver::LANE_FOLLOWING){
            points_and_target_speeds = add_lanefollow_buffer(wm, points_and_target_speeds, processed_maneuvers, ending_state_before_buffer, detailed_config);
        }
 
        return points_and_target_speeds;
    }

References basic_autonomy::waypoint_generation::add_lanefollow_buffer(), basic_autonomy::waypoint_generation::create_lanefollow_geometry(), and GET_MANEUVER_PROPERTY.

Referenced by planTrajectoryCB().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ findSpeedLimit()

double light_controlled_intersection_tactical_plugin::LightControlledIntersectionTacticalPlugin::findSpeedLimit	(	const lanelet::ConstLanelet &	llt,
		const carma_wm::WorldModelConstPtr &	wm
	)		const

private

Given a Lanelet, find it's associated Speed Limit.

Parameters

llt	Constant Lanelet object

Exceptions

std::invalid_argument if the speed limit could not be retrieved

Returns: value of speed limit in mps

Definition at line 388 of file light_controlled_intersection_tactical_plugin.cpp.

    {
        lanelet::Optional<carma_wm::TrafficRulesConstPtr> traffic_rules = wm->getTrafficRules();
        if (traffic_rules)
        {
            return (*traffic_rules)->speedLimit(llt).speedLimit.value();
        }
        else
        {
            throw std::invalid_argument("Valid traffic rules object could not be built");
        }
    }

Referenced by planTrajectoryCB().

Here is the caller graph for this function:

◆ FRIEND_TEST() [1/5]

light_controlled_intersection_tactical_plugin::LightControlledIntersectionTacticalPlugin::FRIEND_TEST	(	LCITacticalPluginTest	,
		applyOptimizedTargetSpeedProfile
	)

private

◆ FRIEND_TEST() [2/5]

light_controlled_intersection_tactical_plugin::LightControlledIntersectionTacticalPlugin::FRIEND_TEST	(	LCITacticalPluginTest	,
		applyTrajectorySmoothingAlgorithm
	)

private

◆ FRIEND_TEST() [3/5]

light_controlled_intersection_tactical_plugin::LightControlledIntersectionTacticalPlugin::FRIEND_TEST	(	LCITacticalPluginTest	,
		createGeometryProfile
	)

private

◆ FRIEND_TEST() [4/5]

light_controlled_intersection_tactical_plugin::LightControlledIntersectionTacticalPlugin::FRIEND_TEST	(	LCITacticalPluginTest	,
		planTrajectoryCB
	)

private

◆ FRIEND_TEST() [5/5]

light_controlled_intersection_tactical_plugin::LightControlledIntersectionTacticalPlugin::FRIEND_TEST	(	LCITacticalPluginTest	,
		setConfig
	)

private

◆ planTrajectoryCB()

void light_controlled_intersection_tactical_plugin::LightControlledIntersectionTacticalPlugin::planTrajectoryCB	(	carma_planning_msgs::srv::PlanTrajectory::Request::SharedPtr	req,
		carma_planning_msgs::srv::PlanTrajectory::Response::SharedPtr	resp
	)

Function to process the light controlled intersection tactical plugin service call for trajectory planning.

Parameters

req	The service request
resp	The service response

Definition at line 29 of file light_controlled_intersection_tactical_plugin.cpp.

    {
        std::chrono::system_clock::time_point start_time = std::chrono::system_clock::now();  // Start timing the execution time for planning so it can be logged
 
        RCLCPP_DEBUG_STREAM(rclcpp::get_logger("light_controlled_intersection_tactical_plugin"), "Starting light controlled intersection trajectory planning");
 
        if(req->maneuver_index_to_plan >= req->maneuver_plan.maneuvers.size())
        {
            throw std::invalid_argument(
                "Light Control Intersection Plugin asked to plan invalid maneuver index: " + std::to_string(req->maneuver_index_to_plan) +
                " for plan of size: " + std::to_string(req->maneuver_plan.maneuvers.size()));
        }
 
        std::vector<carma_planning_msgs::msg::Maneuver> maneuver_plan;
        // Expecting only one maneuver for an intersection, which corresponds to the maneuver_index_to_plan value provided in the request
        if(req->maneuver_plan.maneuvers[req->maneuver_index_to_plan].type == carma_planning_msgs::msg::Maneuver::LANE_FOLLOWING
            && GET_MANEUVER_PROPERTY(req->maneuver_plan.maneuvers[req->maneuver_index_to_plan], parameters.string_valued_meta_data.front()) == light_controlled_intersection_strategy_)
        {
            maneuver_plan.push_back(req->maneuver_plan.maneuvers[req->maneuver_index_to_plan]);
            resp->related_maneuvers.push_back(req->maneuver_index_to_plan);
        }
        else
        {
            throw std::invalid_argument("Light Control Intersection Plugin asked to plan unsupported maneuver");
        }
 
        lanelet::BasicPoint2d veh_pos(req->vehicle_state.x_pos_global, req->vehicle_state.y_pos_global);
        RCLCPP_DEBUG_STREAM(rclcpp::get_logger("light_controlled_intersection_tactical_plugin"), "Planning state x:" << req->vehicle_state.x_pos_global << " , y: " << req->vehicle_state.y_pos_global);
 
        current_downtrack_ = wm_->routeTrackPos(veh_pos).downtrack;
 
        RCLCPP_DEBUG_STREAM(rclcpp::get_logger("light_controlled_intersection_tactical_plugin"), "Current_downtrack: "<< current_downtrack_);
 
        auto current_lanelets = wm_->getLaneletsFromPoint({req->vehicle_state.x_pos_global, req->vehicle_state.y_pos_global});
        lanelet::ConstLanelet current_lanelet;
 
        if (current_lanelets.empty())
        {
            RCLCPP_ERROR_STREAM(rclcpp::get_logger("light_controlled_intersection_tactical_plugin"), "Given vehicle position is not on the road! Returning...");
            return;
        }
 
        // get the lanelet that is on the route in case overlapping ones found
        auto llt_on_route_optional = wm_->getFirstLaneletOnShortestPath(current_lanelets);
 
        if (llt_on_route_optional){
            current_lanelet = llt_on_route_optional.value();
        }
        else{
        RCLCPP_WARN_STREAM(rclcpp::get_logger("light_controlled_intersection_tactical_plugin"), "When identifying the corresponding lanelet for requested trajectory plan's state, x: " 
            << req->vehicle_state.x_pos_global << ", y: " << req->vehicle_state.y_pos_global << ", no possible lanelet was found to be on the shortest path."
            << "Picking arbitrary lanelet: " << current_lanelets[0].id() << ", instead");
            current_lanelet = current_lanelets[0];
        }
 
        RCLCPP_DEBUG_STREAM(rclcpp::get_logger("light_controlled_intersection_tactical_plugin"), "Current_lanelet: " << current_lanelet.id());
 
        speed_limit_ = findSpeedLimit(current_lanelet, wm_);
 
        RCLCPP_DEBUG_STREAM(rclcpp::get_logger("light_controlled_intersection_tactical_plugin"), "speed_limit_: " << speed_limit_);
 
        DetailedTrajConfig wpg_detail_config;
        GeneralTrajConfig wpg_general_config;
 
        wpg_general_config = basic_autonomy:: waypoint_generation::compose_general_trajectory_config("intersection_transit",
                                                                                    config_.default_downsample_ratio,
                                                                                    config_.turn_downsample_ratio);
 
        wpg_detail_config = basic_autonomy:: waypoint_generation::compose_detailed_trajectory_config(config_.trajectory_time_length,
                                                                                config_.curve_resample_step_size, config_.minimum_speed,
                                                                                config_.vehicle_accel_limit,
                                                                                config_.lateral_accel_limit,
                                                                                config_.speed_moving_average_window_size,
                                                                                config_.curvature_moving_average_window_size, config_.back_distance,
                                                                                config_.buffer_ending_downtrack);
 
         // CHECK IF LAST TRAJECTORY WILL BE USED
 
        bool is_new_case_successful = GET_MANEUVER_PROPERTY(maneuver_plan.front(), parameters.int_valued_meta_data[1]);
        TSCase new_case = static_cast<TSCase>GET_MANEUVER_PROPERTY(maneuver_plan.front(), parameters.int_valued_meta_data[0]);
 
        if (is_last_case_successful_ != boost::none && last_case_ != boost::none)
        {
            RCLCPP_DEBUG_STREAM(rclcpp::get_logger("light_controlled_intersection_tactical_plugin"), "all variables are set!");
            RCLCPP_DEBUG_STREAM(rclcpp::get_logger("light_controlled_intersection_tactical_plugin"), "is_last_case_successful_.get(): " << (int)is_last_case_successful_.get());
            RCLCPP_DEBUG_STREAM(rclcpp::get_logger("light_controlled_intersection_tactical_plugin"), "evaluation distance: " << last_successful_ending_downtrack_ - current_downtrack_);
 
            RCLCPP_DEBUG_STREAM(rclcpp::get_logger("light_controlled_intersection_tactical_plugin"), "evaluation time: " << std::to_string(last_successful_scheduled_entry_time_ - rclcpp::Time(req->header.stamp).seconds()));
        }
        else
        {
            RCLCPP_DEBUG_STREAM(rclcpp::get_logger("light_controlled_intersection_tactical_plugin"), "Not all variables are set...");
        }
 
 
 
        RCLCPP_DEBUG_STREAM(rclcpp::get_logger("light_controlled_intersection_tactical_plugin"), "traj points size: " << last_trajectory_.trajectory_points.size() << ", last_final_speeds_ size: " <<
                            last_final_speeds_.size() );
 
        size_t idx_to_start_new_traj = 0;
        for (size_t i = 0; i < last_trajectory_.trajectory_points.size(); i++)
        {
            auto dist = sqrt(pow(veh_pos.x() - last_trajectory_.trajectory_points.at(i).x, 2) + std::pow(veh_pos.y() - last_trajectory_.trajectory_points.at(i).y, 2));
 
            if (dist < 2.0) // reduce until if 2 meters away
            {
                idx_to_start_new_traj = i;
                break;
            }
        }
 
        last_final_speeds_ = std::vector<double>(last_final_speeds_.begin() + idx_to_start_new_traj, last_final_speeds_.end());
        last_trajectory_.trajectory_points = std::vector<carma_planning_msgs::msg::TrajectoryPlanPoint>(last_trajectory_.trajectory_points.begin() + idx_to_start_new_traj, last_trajectory_.trajectory_points.end());
 
        if (is_last_case_successful_ != boost::none && last_case_ != boost::none
            && last_case_.get() == new_case
            && is_new_case_successful == true
            && last_trajectory_.trajectory_points.size() >= 2
            && rclcpp::Time(last_trajectory_.trajectory_points.back().target_time) > rclcpp::Time(req->header.stamp) + rclcpp::Duration(1 * 1e9)) // Duration is in nanoseconds
        {
            resp->trajectory_plan = last_trajectory_;
            RCLCPP_DEBUG_STREAM(rclcpp::get_logger("light_controlled_intersection_tactical_plugin"), "Last TRAJ's target time: " << std::to_string(rclcpp::Time(last_trajectory_.trajectory_points.back().target_time).seconds()) << ", and stamp:" << std::to_string(rclcpp::Time(req->header.stamp).seconds()));
            RCLCPP_DEBUG_STREAM(rclcpp::get_logger("light_controlled_intersection_tactical_plugin"), "USING LAST TRAJ: " << (int)last_case_.get());
        }
        else if (is_last_case_successful_ != boost::none && last_case_ != boost::none
            && is_last_case_successful_.get() == true
            && is_new_case_successful == false
            && last_successful_ending_downtrack_ - current_downtrack_ < config_.algorithm_evaluation_distance
            && last_successful_scheduled_entry_time_ - rclcpp::Time(req->header.stamp).seconds() < config_.algorithm_evaluation_period
            && last_trajectory_.trajectory_points.size() >= 2
            && rclcpp::Time(last_trajectory_.trajectory_points.back().target_time).seconds() > rclcpp::Time(req->header.stamp).seconds())
        {
            resp->trajectory_plan = last_trajectory_;
            RCLCPP_DEBUG_STREAM(rclcpp::get_logger("light_controlled_intersection_tactical_plugin"), "Last Traj's target time: " << std::to_string(rclcpp::Time(last_trajectory_.trajectory_points.back().target_time).seconds()) << ", and stamp:" << std::to_string(rclcpp::Time(req->header.stamp).seconds()) << ", and scheduled: " << std::to_string(last_successful_scheduled_entry_time_));
            RCLCPP_DEBUG_STREAM(rclcpp::get_logger("light_controlled_intersection_tactical_plugin"), "EDGE CASE: USING LAST TRAJ: " << (int)last_case_.get());
        }
 
        if (!resp->trajectory_plan.trajectory_points.empty()) // if has valid trajectory saved from before return
        {
            RCLCPP_DEBUG_STREAM(rclcpp::get_logger("light_controlled_intersection_tactical_plugin"), "Debug: new case:" << (int) new_case << ", is_new_case_successful: " << is_new_case_successful);
 
            resp->trajectory_plan.initial_longitudinal_velocity = last_final_speeds_.front();
 
            resp->maneuver_status.push_back(carma_planning_msgs::srv::PlanTrajectory::Response::MANEUVER_IN_PROGRESS);
 
            std::chrono::system_clock::time_point end_time = std::chrono::system_clock::now();  // Planning complete
 
            auto duration = end_time - start_time;
            RCLCPP_DEBUG_STREAM(rclcpp::get_logger("light_controlled_intersection_tactical_plugin"), "ExecutionTime Using New: " << std::chrono::duration<double>(duration).count());
 
            return;
        }
 
        // IF NOT USING LAST TRAJECTORY PLAN NEW TRAJECTORY
 
        // Create curve-fitting compatible trajectories (with extra back and front attached points) with raw speed limits from maneuver
        auto points_and_target_speeds = createGeometryProfile(maneuver_plan, std::max((double)0, current_downtrack_ - config_.back_distance),
                                                                                wm_, ending_state_before_buffer_, req->vehicle_state, wpg_general_config, wpg_detail_config);
 
        // Change raw speed limit values to target speeds specified by the algorithm
        applyOptimizedTargetSpeedProfile(maneuver_plan.front(), req->vehicle_state.longitudinal_vel, points_and_target_speeds);
 
        RCLCPP_DEBUG_STREAM(rclcpp::get_logger("light_controlled_intersection_tactical_plugin"), "points_and_target_speeds: " << points_and_target_speeds.size());
 
        RCLCPP_DEBUG_STREAM(rclcpp::get_logger("light_controlled_intersection_tactical_plugin"), "PlanTrajectory");
 
        //Trajectory Plan
        carma_planning_msgs::msg::TrajectoryPlan trajectory;
        trajectory.header.frame_id = "map";
        trajectory.header.stamp = req->header.stamp;
        trajectory.trajectory_id = boost::uuids::to_string(boost::uuids::random_generator()());
 
        // Compose smooth trajectory/speed by resampling
        trajectory.trajectory_points = basic_autonomy::waypoint_generation::compose_lanefollow_trajectory_from_path(points_and_target_speeds,
                                                                                    req->vehicle_state, req->header.stamp, wm_, ending_state_before_buffer_, debug_msg_,
                                                                                    wpg_detail_config); // Compute the trajectory
 
        // Set the planning plugin field name
        for (auto& p : trajectory.trajectory_points) {
            p.planner_plugin_name = plugin_name_;
        }
 
        if (trajectory.trajectory_points.size () < 2)
        {
            if (last_trajectory_.trajectory_points.size() >= 2
                && rclcpp::Time(last_trajectory_.trajectory_points.back().target_time) > rclcpp::Time(req->header.stamp))
            {
                resp->trajectory_plan = last_trajectory_;
                RCLCPP_WARN_STREAM(rclcpp::get_logger("light_controlled_intersection_tactical_plugin"), "Failed to generate new trajectory, so using last valid trajectory!");
            }
            else
            {
                resp->trajectory_plan = trajectory;
                RCLCPP_WARN_STREAM(rclcpp::get_logger("light_controlled_intersection_tactical_plugin"), "Failed to generate new trajectory or use old valid trajectory, so returning empty/invalid trajectory!");
            }
        }
        else
        {
            last_trajectory_ = trajectory;
            resp->trajectory_plan = trajectory;
            last_case_ = new_case;
            last_final_speeds_ = debug_msg_.velocity_profile;
            is_last_case_successful_ = is_new_case_successful;
            RCLCPP_DEBUG_STREAM(rclcpp::get_logger("light_controlled_intersection_tactical_plugin"), "USING NEW: Target time: " << std::to_string(rclcpp::Time(last_trajectory_.trajectory_points.back().target_time).seconds()) << ", and stamp:" << std::to_string(rclcpp::Time(req->header.stamp).seconds()));
            if (is_new_case_successful)
            {
                last_successful_ending_downtrack_ = GET_MANEUVER_PROPERTY(maneuver_plan.front(), end_dist);              // if algorithm was successful, this is traffic_light_downtrack
                last_successful_scheduled_entry_time_ = rclcpp::Time(GET_MANEUVER_PROPERTY(maneuver_plan.front(), end_time)).seconds();  // if algorithm was successful, this is also scheduled entry time (ET in TSMO UC2 Algo)
                RCLCPP_DEBUG_STREAM(rclcpp::get_logger("light_controlled_intersection_tactical_plugin"), "last_successful_ending_downtrack_:" << last_successful_ending_downtrack_ << ", last_successful_scheduled_entry_time_: " << std::to_string(last_successful_scheduled_entry_time_));
            }
            RCLCPP_DEBUG_STREAM(rclcpp::get_logger("light_controlled_intersection_tactical_plugin"), "USING NEW CASE!!! : " << (int)last_case_.get());
 
        }
        RCLCPP_DEBUG_STREAM(rclcpp::get_logger("light_controlled_intersection_tactical_plugin"), "Debug: new case:" << (int) new_case << ", is_new_case_successful: " << is_new_case_successful);
 
        resp->trajectory_plan.initial_longitudinal_velocity = last_final_speeds_.front();
 
        // Yield for potential obstacles in the road
        // Aside from the flag, yield_plugin should not be called on invalid trajectories
        if (config_.enable_object_avoidance && resp->trajectory_plan.trajectory_points.size() >= 2)
        {
            basic_autonomy::waypoint_generation::modify_trajectory_to_yield_to_obstacles(nh_, req, resp, yield_client_, config_.tactical_plugin_service_call_timeout);
        }
        else
        {
            RCLCPP_DEBUG(rclcpp::get_logger("light_controlled_intersection_tactical_plugin"), "Ignored Object Avoidance");
        }
 
        resp->maneuver_status.push_back(carma_planning_msgs::srv::PlanTrajectory::Response::MANEUVER_IN_PROGRESS);
 
        std::chrono::system_clock::time_point end_time = std::chrono::system_clock::now();  // Planning complete
 
        auto duration = end_time - start_time;
        RCLCPP_DEBUG_STREAM(rclcpp::get_logger("light_controlled_intersection_tactical_plugin"), "ExecutionTime Using Last: " << std::chrono::duration<double>(duration).count());
 
        return;
    }

Here is the call graph for this function:

◆ set_yield_client()

void light_controlled_intersection_tactical_plugin::LightControlledIntersectionTacticalPlugin::set_yield_client ( carma_ros2_utils::ClientPtr< carma_planning_msgs::srv::PlanTrajectory > client )

set the yield service

Parameters

yield_srv input yield service

Definition at line 452 of file light_controlled_intersection_tactical_plugin.cpp.

    {
        yield_client_ = client;
    }

References yield_client_.

◆ setConfig()

void light_controlled_intersection_tactical_plugin::LightControlledIntersectionTacticalPlugin::setConfig ( const Config & config )

Setter function to set a new config for this object.

Parameters

config The new config to be used by this object

Definition at line 447 of file light_controlled_intersection_tactical_plugin.cpp.

    {
        config_ = config;
    }

References config_.

Member Data Documentation

◆ config_

Config light_controlled_intersection_tactical_plugin::LightControlledIntersectionTacticalPlugin::config_

private

Definition at line 95 of file light_controlled_intersection_tactical_plugin.hpp.

Referenced by applyTrajectorySmoothingAlgorithm(), planTrajectoryCB(), and setConfig().

◆ current_downtrack_

double light_controlled_intersection_tactical_plugin::LightControlledIntersectionTacticalPlugin::current_downtrack_ = 0.0

private

Definition at line 124 of file light_controlled_intersection_tactical_plugin.hpp.

Referenced by planTrajectoryCB().

◆ debug_msg_

carma_debug_ros2_msgs::msg::TrajectoryCurvatureSpeeds light_controlled_intersection_tactical_plugin::LightControlledIntersectionTacticalPlugin::debug_msg_

private

Definition at line 130 of file light_controlled_intersection_tactical_plugin.hpp.

Referenced by planTrajectoryCB().

◆ ending_state_before_buffer_

carma_planning_msgs::msg::VehicleState light_controlled_intersection_tactical_plugin::LightControlledIntersectionTacticalPlugin::ending_state_before_buffer_

private

Definition at line 119 of file light_controlled_intersection_tactical_plugin.hpp.

Referenced by planTrajectoryCB().

◆ epsilon_

double light_controlled_intersection_tactical_plugin::LightControlledIntersectionTacticalPlugin::epsilon_ = 0.001

private

Definition at line 121 of file light_controlled_intersection_tactical_plugin.hpp.

Referenced by applyTrajectorySmoothingAlgorithm().

◆ is_allowed_int_

bool light_controlled_intersection_tactical_plugin::LightControlledIntersectionTacticalPlugin::is_allowed_int_ = false

private

Definition at line 111 of file light_controlled_intersection_tactical_plugin.hpp.

◆ is_last_case_successful_

boost::optional<bool> light_controlled_intersection_tactical_plugin::LightControlledIntersectionTacticalPlugin::is_last_case_successful_

private

Definition at line 115 of file light_controlled_intersection_tactical_plugin.hpp.

Referenced by planTrajectoryCB().

◆ last_case_

boost::optional<TSCase> light_controlled_intersection_tactical_plugin::LightControlledIntersectionTacticalPlugin::last_case_

private

Definition at line 114 of file light_controlled_intersection_tactical_plugin.hpp.

Referenced by planTrajectoryCB().

◆ last_final_speeds_

std::vector<double> light_controlled_intersection_tactical_plugin::LightControlledIntersectionTacticalPlugin::last_final_speeds_

private

Definition at line 131 of file light_controlled_intersection_tactical_plugin.hpp.

Referenced by planTrajectoryCB().

◆ last_successful_ending_downtrack_

double light_controlled_intersection_tactical_plugin::LightControlledIntersectionTacticalPlugin::last_successful_ending_downtrack_

private

Definition at line 126 of file light_controlled_intersection_tactical_plugin.hpp.

Referenced by planTrajectoryCB().

◆ last_successful_scheduled_entry_time_

double light_controlled_intersection_tactical_plugin::LightControlledIntersectionTacticalPlugin::last_successful_scheduled_entry_time_

private

Definition at line 127 of file light_controlled_intersection_tactical_plugin.hpp.

Referenced by planTrajectoryCB().

◆ last_trajectory_

carma_planning_msgs::msg::TrajectoryPlan light_controlled_intersection_tactical_plugin::LightControlledIntersectionTacticalPlugin::last_trajectory_

private

Definition at line 116 of file light_controlled_intersection_tactical_plugin.hpp.

Referenced by planTrajectoryCB().

◆ light_controlled_intersection_strategy_

std::string light_controlled_intersection_tactical_plugin::LightControlledIntersectionTacticalPlugin::light_controlled_intersection_strategy_ = "Carma/signalized_intersection"

private

Definition at line 133 of file light_controlled_intersection_tactical_plugin.hpp.

Referenced by planTrajectoryCB().

◆ nh_

std::shared_ptr<carma_ros2_utils::CarmaLifecycleNode> light_controlled_intersection_tactical_plugin::LightControlledIntersectionTacticalPlugin::nh_

private

Definition at line 97 of file light_controlled_intersection_tactical_plugin.hpp.

Referenced by planTrajectoryCB().

◆ plugin_name_

std::string light_controlled_intersection_tactical_plugin::LightControlledIntersectionTacticalPlugin::plugin_name_

private

Definition at line 129 of file light_controlled_intersection_tactical_plugin.hpp.

Referenced by planTrajectoryCB().

◆ scheduled_depart_time_

uint32_t light_controlled_intersection_tactical_plugin::LightControlledIntersectionTacticalPlugin::scheduled_depart_time_ = 0.0

private

Definition at line 107 of file light_controlled_intersection_tactical_plugin.hpp.

◆ scheduled_enter_time_

uint32_t light_controlled_intersection_tactical_plugin::LightControlledIntersectionTacticalPlugin::scheduled_enter_time_ = 0.0

private

Definition at line 105 of file light_controlled_intersection_tactical_plugin.hpp.

◆ scheduled_latest_depart_time_

uint32_t light_controlled_intersection_tactical_plugin::LightControlledIntersectionTacticalPlugin::scheduled_latest_depart_time_ = 0.0

private

Definition at line 109 of file light_controlled_intersection_tactical_plugin.hpp.

◆ scheduled_stop_time_

uint32_t light_controlled_intersection_tactical_plugin::LightControlledIntersectionTacticalPlugin::scheduled_stop_time_ = 0.0

private

Definition at line 103 of file light_controlled_intersection_tactical_plugin.hpp.

◆ speed_limit_

double light_controlled_intersection_tactical_plugin::LightControlledIntersectionTacticalPlugin::speed_limit_ = 11.176

private

Definition at line 113 of file light_controlled_intersection_tactical_plugin.hpp.

Referenced by applyTrajectorySmoothingAlgorithm(), and planTrajectoryCB().

◆ street_msg_timestamp_

uint32_t light_controlled_intersection_tactical_plugin::LightControlledIntersectionTacticalPlugin::street_msg_timestamp_ = 0.0

private

Definition at line 101 of file light_controlled_intersection_tactical_plugin.hpp.

◆ wm_

carma_wm::WorldModelConstPtr light_controlled_intersection_tactical_plugin::LightControlledIntersectionTacticalPlugin::wm_

private

Definition at line 92 of file light_controlled_intersection_tactical_plugin.hpp.

Referenced by applyOptimizedTargetSpeedProfile(), and planTrajectoryCB().

◆ yield_client_

carma_ros2_utils::ClientPtr<carma_planning_msgs::srv::PlanTrajectory> light_controlled_intersection_tactical_plugin::LightControlledIntersectionTacticalPlugin::yield_client_

private

Definition at line 117 of file light_controlled_intersection_tactical_plugin.hpp.

Referenced by planTrajectoryCB(), and set_yield_client().

The documentation for this class was generated from the following files:

light_controlled_intersection_tactical_plugin/include/light_controlled_intersection_tactical_plugin/light_controlled_intersection_tactical_plugin.hpp
light_controlled_intersection_tactical_plugin/src/light_controlled_intersection_tactical_plugin.cpp

Public Member Functions

Private Member Functions

Private Attributes

Detailed Description

Constructor & Destructor Documentation

◆ LightControlledIntersectionTacticalPlugin()

Member Function Documentation

◆ applyOptimizedTargetSpeedProfile()

◆ applyTrajectorySmoothingAlgorithm()

◆ createGeometryProfile()

◆ findSpeedLimit()

◆ FRIEND_TEST() [1/5]

◆ FRIEND_TEST() [2/5]

◆ FRIEND_TEST() [3/5]

◆ FRIEND_TEST() [4/5]

◆ FRIEND_TEST() [5/5]

◆ planTrajectoryCB()

◆ set_yield_client()

◆ setConfig()

Member Data Documentation

◆ config_

◆ current_downtrack_

◆ debug_msg_

◆ ending_state_before_buffer_

◆ epsilon_

◆ is_allowed_int_

◆ is_last_case_successful_

◆ last_case_

◆ last_final_speeds_

◆ last_successful_ending_downtrack_

◆ last_successful_scheduled_entry_time_

◆ last_trajectory_

◆ light_controlled_intersection_strategy_

◆ nh_

◆ plugin_name_

◆ scheduled_depart_time_

◆ scheduled_enter_time_

◆ scheduled_latest_depart_time_

◆ scheduled_stop_time_

◆ speed_limit_

◆ street_msg_timestamp_

◆ wm_

◆ yield_client_