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 DebugPublisher &debug_publisher, 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 its associated Speed Limit. More...

void	logDebugInfoAboutPreviousTrajectory ()
	Logs debug information about the previously planned trajectory. More...

bool	shouldUseLastTrajectory (TSCase new_case, bool is_new_case_successful, const rclcpp::Time &current_time)
	Determines whether the last trajectory should be reused based on the planning case. Should use last case if 1) last traj is valid AND last case is the same as new case, OR 2) last traj is valid AND within certain evaluation zone before the intersection where the new case is not successful but last case is. More...

void	planTrajectorySmoothing (carma_planning_msgs::srv::PlanTrajectory::Request::SharedPtr req, carma_planning_msgs::srv::PlanTrajectory::Response::SharedPtr resp)
	Smooths the trajectory as part of the trajectory planning process. More...

carma_planning_msgs::msg::TrajectoryPlan	generateNewTrajectory (const std::vector< carma_planning_msgs::msg::Maneuver > &maneuver_plan, const carma_planning_msgs::srv::PlanTrajectory::Request::SharedPtr &req, std::vector< double > &final_speeds)
	Generates a new trajectory plan based on the provided maneuver plan and request. NOTE: This function plans for the entire maneuver input and doesn't time bound it. More...

bool	isLastTrajectoryValid (const rclcpp::Time &current_time, double min_remaining_time_seconds=0.0) const
	Checks if the last trajectory plan remains valid based on the current time. More...

	FRIEND_TEST (LCITacticalPluginTest, applyTrajectorySmoothingAlgorithm)

	FRIEND_TEST (LCITacticalPluginTest, applyOptimizedTargetSpeedProfile)

	FRIEND_TEST (LCITacticalPluginTest, planTrajectorySmoothing)

	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_time_unbound_

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

const std::string	LCI_TACTICAL_LOGGER = "light_controlled_intersection_tactical_plugin"

carma_planning_msgs::msg::VehicleState	ending_state_before_buffer_

rclcpp::Time	latest_traj_request_header_stamp_

double	epsilon_ = 0.001

double	current_downtrack_ = 0.0

double	last_successful_ending_downtrack_

double	last_successful_scheduled_entry_time_

std::string	plugin_name_

DebugPublisher	debug_publisher_

carma_debug_ros2_msgs::msg::TrajectoryCurvatureSpeeds	debug_msg_

std::vector< double >	last_speeds_time_unbound_

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 105 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 DebugPublisher &	debug_publisher,
		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), nh_(nh), plugin_name_(plugin_name),
        debug_publisher_(debug_publisher)
    {
    }

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 519 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 generateNewTrajectory().

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 432 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(LCI_TACTICAL_LOGGER), "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(LCI_TACTICAL_LOGGER), "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(LCI_TACTICAL_LOGGER), "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(LCI_TACTICAL_LOGGER), "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(LCI_TACTICAL_LOGGER), "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 564 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(LCI_TACTICAL_LOGGER), "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(LCI_TACTICAL_LOGGER), "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(LCI_TACTICAL_LOGGER), "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 Tactical Plugin currently 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(), GET_MANEUVER_PROPERTY, and LCI_TACTICAL_LOGGER.

Referenced by generateNewTrajectory().

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 its associated Speed Limit.

Parameters

llt	Constant Lanelet object.
wm	World model pointer to query additional information.

Exceptions

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

Returns: Speed limit in meters per second.

Definition at line 551 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 planTrajectorySmoothing().

Here is the caller graph for this function:

◆ FRIEND_TEST() [1/6]

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

private

◆ FRIEND_TEST() [2/6]

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

private

◆ FRIEND_TEST() [3/6]

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

private

◆ FRIEND_TEST() [4/6]

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

private

◆ FRIEND_TEST() [5/6]

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

private

◆ FRIEND_TEST() [6/6]

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

private

◆ generateNewTrajectory()

carma_planning_msgs::msg::TrajectoryPlan light_controlled_intersection_tactical_plugin::LightControlledIntersectionTacticalPlugin::generateNewTrajectory	(	const std::vector< carma_planning_msgs::msg::Maneuver > &	maneuver_plan,
		const carma_planning_msgs::srv::PlanTrajectory::Request::SharedPtr &	req,
		std::vector< double > &	final_speeds
	)

private

Generates a new trajectory plan based on the provided maneuver plan and request. NOTE: This function plans for the entire maneuver input and doesn't time bound it.

Parameters

maneuver_plan	A vector of maneuver messages defining the planned maneuvers. The first maneuver is expected to have all the necessary TS parameters.
req	Shared pointer to the trajectory planning request message.
final_speeds	A vector that will be populated with the final speeds for each point as it is useful for late operations or debugging

Returns: A newly generated trajectory plan message.

Definition at line 105 of file light_controlled_intersection_tactical_plugin.cpp.

    {
        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(
            99.0, // trajectory time length in duration (seconds) is arbitrarily selected
                  // high to generate all at once
            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);
 
        // Create trajectory 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);
 
        // Apply optimized speed profile
        applyOptimizedTargetSpeedProfile(maneuver_plan.front(), req->vehicle_state.longitudinal_vel,
            points_and_target_speeds);
 
        // Create new trajectory
        carma_planning_msgs::msg::TrajectoryPlan new_trajectory;
        new_trajectory.header.frame_id = "map";
        new_trajectory.header.stamp = req->header.stamp;
        new_trajectory.trajectory_id = boost::uuids::to_string(boost::uuids::random_generator()());
 
        // Generate points
        new_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);
 
        // Save final speeds
        final_speeds = debug_msg_.velocity_profile;
 
        return new_trajectory;
    }

Referenced by planTrajectorySmoothing().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ isLastTrajectoryValid()

bool light_controlled_intersection_tactical_plugin::LightControlledIntersectionTacticalPlugin::isLastTrajectoryValid	(	const rclcpp::Time &	current_time,
		double	min_remaining_time_seconds = `0.0`
	)		const

private

Checks if the last trajectory plan remains valid based on the current time.

This function verifies that the previously generated trajectory still meets timing and duration requirements. It can also enforce a minimum remaining duration if specified.

Parameters

current_time	The current time stamp.
min_remaining_time_seconds	Optional parameter specifying the minimum required remaining time (in seconds) for the trajectory to be considered valid.

Returns: True if the last trajectory is valid, false otherwise.

Definition at line 34 of file light_controlled_intersection_tactical_plugin.cpp.

    {
        // Check if we have at least 2 points in the trajectory
        if (last_trajectory_time_unbound_.trajectory_points.size() < 2)
        {
            return false;
        }
 
        // Check if the last point's time is sufficiently in the future
        auto last_point_time = rclcpp::Time(last_trajectory_time_unbound_.trajectory_points.back().target_time);
 
 
        if (rclcpp::Duration min_time_remaining =
            rclcpp::Duration::from_seconds(min_remaining_time_seconds);
            last_point_time <= current_time + min_time_remaining)
        {
            return false;
        }
 
        // Check if we have case information from previous planning
        if (is_last_case_successful_ == boost::none || last_case_ == boost::none)
        {
            return false;
        }
 
        // Ensure we have consistent speed data
        if (last_speeds_time_unbound_.size() != last_trajectory_time_unbound_.trajectory_points.size())
        {
            return false;
        }
 
        return true;
    }

References is_last_case_successful_, last_case_, last_speeds_time_unbound_, and last_trajectory_time_unbound_.

Referenced by shouldUseLastTrajectory().

Here is the caller graph for this function:

◆ logDebugInfoAboutPreviousTrajectory()

void light_controlled_intersection_tactical_plugin::LightControlledIntersectionTacticalPlugin::logDebugInfoAboutPreviousTrajectory ( )

private

Logs debug information about the previously planned trajectory.

This helper function outputs detailed internal state and trajectory information that can be used for debugging trajectory planning issues.

Definition at line 158 of file light_controlled_intersection_tactical_plugin.cpp.

    {
        if (is_last_case_successful_ != boost::none && last_case_ != boost::none)
        {
            RCLCPP_DEBUG_STREAM(rclcpp::get_logger(LCI_TACTICAL_LOGGER),
                "all variables are set!");
            RCLCPP_DEBUG_STREAM(rclcpp::get_logger(LCI_TACTICAL_LOGGER),
                "is_last_case_successful_.get(): " << (int)is_last_case_successful_.get());
            RCLCPP_DEBUG_STREAM(rclcpp::get_logger(LCI_TACTICAL_LOGGER),
                "evaluation distance: " << last_successful_ending_downtrack_ - current_downtrack_);
            RCLCPP_DEBUG_STREAM(rclcpp::get_logger(LCI_TACTICAL_LOGGER),
                "evaluation time: " << std::to_string(last_successful_scheduled_entry_time_ -
                latest_traj_request_header_stamp_.seconds()));
        }
        else
        {
            RCLCPP_DEBUG_STREAM(rclcpp::get_logger(LCI_TACTICAL_LOGGER),
                "Not all variables are set...");
        }
 
        RCLCPP_DEBUG_STREAM(rclcpp::get_logger(LCI_TACTICAL_LOGGER),
            "traj points size: " << last_trajectory_time_unbound_.trajectory_points.size() <<
            ", last_speeds_time_unbound_ size: " << last_speeds_time_unbound_.size());
    }

References current_downtrack_, is_last_case_successful_, last_case_, last_speeds_time_unbound_, last_successful_ending_downtrack_, last_successful_scheduled_entry_time_, last_trajectory_time_unbound_, latest_traj_request_header_stamp_, LCI_TACTICAL_LOGGER, and carma_cooperative_perception::to_string().

Referenced by planTrajectorySmoothing().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ 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 400 of file light_controlled_intersection_tactical_plugin.cpp.

    {
        std::chrono::system_clock::time_point start_time = std::chrono::system_clock::now();
 
        latest_traj_request_header_stamp_ = rclcpp::Time(req->header.stamp); //for debugging
 
        RCLCPP_DEBUG_STREAM(rclcpp::get_logger(LCI_TACTICAL_LOGGER),
            "Starting light controlled intersection trajectory planning");
 
        // Call the function to plan trajectory without yield
        planTrajectorySmoothing(req, resp);
 
        // Yield for potential obstacles in the road
        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(LCI_TACTICAL_LOGGER), "Ignored Object Avoidance");
        }
 
        std::chrono::system_clock::time_point end_time = std::chrono::system_clock::now();
        auto duration = end_time - start_time;
 
        RCLCPP_DEBUG_STREAM(rclcpp::get_logger(LCI_TACTICAL_LOGGER),
            "ExecutionTime: " << std::chrono::duration<double>(duration).count());
    }

References config_, light_controlled_intersection_tactical_plugin::Config::enable_object_avoidance, latest_traj_request_header_stamp_, LCI_TACTICAL_LOGGER, basic_autonomy::waypoint_generation::modify_trajectory_to_yield_to_obstacles(), nh_, planTrajectorySmoothing(), light_controlled_intersection_tactical_plugin::Config::tactical_plugin_service_call_timeout, and yield_client_.

Here is the call graph for this function:

◆ planTrajectorySmoothing()

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

private

Smooths the trajectory as part of the trajectory planning process.

This function takes a trajectory planning request, applies smoothing algorithms to refine the trajectory, and populates the response with the smoothed trajectory. NOTE: Function is called by planTrajectoryCB and doesn't use yield client

Parameters

req	Shared pointer to the trajectory planning request message.
resp	Shared pointer to the trajectory planning response message to be filled.

Definition at line 183 of file light_controlled_intersection_tactical_plugin.cpp.

    {
        // Validate request
        if(req->maneuver_index_to_plan >= req->maneuver_plan.maneuvers.size())
        {
            throw std::invalid_argument(
                "Light Control Intersection Tactical Plugin was 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()));
        }
 
        // Extract maneuver plan
        std::vector<carma_planning_msgs::msg::Maneuver> maneuver_plan;
        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 Tactical Plugin "
                "was asked to plan unsupported maneuver");
        }
 
        // Get vehicle position and update tracking variables
        lanelet::BasicPoint2d veh_pos(req->vehicle_state.x_pos_global,
            req->vehicle_state.y_pos_global);
        RCLCPP_DEBUG_STREAM(rclcpp::get_logger(LCI_TACTICAL_LOGGER),
            "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(LCI_TACTICAL_LOGGER),
            "Current_downtrack: "<< current_downtrack_);
 
        // Find current lanelet
        auto current_lanelets = wm_->getLaneletsFromPoint({req->vehicle_state.x_pos_global,
            req->vehicle_state.y_pos_global});
 
        RCLCPP_DEBUG_STREAM(rclcpp::get_logger(LCI_TACTICAL_LOGGER), "size: "
            << current_lanelets.size());
 
        lanelet::ConstLanelet current_lanelet;
 
        if (current_lanelets.empty())
        {
            RCLCPP_ERROR_STREAM(rclcpp::get_logger(LCI_TACTICAL_LOGGER),
                "Given vehicle position is not on the road! Returning...");
            return;
        }
 
        // Get the lanelet that is on the route in case overlapping ones found
        if (auto llt_on_route_optional = wm_->getFirstLaneletOnShortestPath(current_lanelets);
            llt_on_route_optional)
        {
            current_lanelet = llt_on_route_optional.value();
        }
        else
        {
            RCLCPP_WARN_STREAM(rclcpp::get_logger(LCI_TACTICAL_LOGGER),
                "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(LCI_TACTICAL_LOGGER),
            "Current_lanelet: " << current_lanelet.id());
 
        speed_limit_ = findSpeedLimit(current_lanelet, wm_);
        RCLCPP_DEBUG_STREAM(rclcpp::get_logger(LCI_TACTICAL_LOGGER),
            "speed_limit_: " << speed_limit_);
 
        // Get new case parameters
        bool is_new_case_successful =
            GET_MANEUVER_PROPERTY(maneuver_plan.front(), parameters.int_valued_meta_data[1]);
 
        auto new_case =
            static_cast<TSCase>GET_MANEUVER_PROPERTY(
                maneuver_plan.front(), parameters.int_valued_meta_data[0]);
 
        // Log debug info about previous trajectory
        logDebugInfoAboutPreviousTrajectory();
 
        // Find closest point in last trajectory to current vehicle position
        size_t idx_to_start_new_traj =
            basic_autonomy::waypoint_generation::get_nearest_point_index(
                last_trajectory_time_unbound_.trajectory_points,
                veh_pos);
 
        // Update last trajectory to start from closest point (remove passed points)
        if (!last_trajectory_time_unbound_.trajectory_points.empty()) {
            last_speeds_time_unbound_ = std::vector<double>(
                last_speeds_time_unbound_.begin() + idx_to_start_new_traj,
                last_speeds_time_unbound_.end());
            last_trajectory_time_unbound_.trajectory_points =
                std::vector<carma_planning_msgs::msg::TrajectoryPlanPoint>
                (last_trajectory_time_unbound_.trajectory_points.begin() + idx_to_start_new_traj,
                last_trajectory_time_unbound_.trajectory_points.end());
        }
 
        // Check if we should use the last trajectory completely
        auto current_time = rclcpp::Time(req->header.stamp);
 
 
        auto last_trajectory_time_bound =
            basic_autonomy::waypoint_generation::constrain_to_time_boundary(
                last_trajectory_time_unbound_.trajectory_points, config_.trajectory_time_length);
        if (shouldUseLastTrajectory(new_case, is_new_case_successful, current_time))
        {
            resp->trajectory_plan = last_trajectory_time_unbound_;
            resp->trajectory_plan.trajectory_points = last_trajectory_time_bound;
 
            RCLCPP_DEBUG_STREAM(rclcpp::get_logger(LCI_TACTICAL_LOGGER),
                "USING LAST TRAJ WITH CASE: " << (int)last_case_.get());
 
            resp->trajectory_plan.initial_longitudinal_velocity = last_speeds_time_unbound_.front();
 
            // Set the planning plugin field name
            for (auto& p : resp->trajectory_plan.trajectory_points) {
                p.planner_plugin_name = plugin_name_;
            }
 
            debug_msg_.trajectory_plan = resp->trajectory_plan;
            debug_msg_.velocity_profile = last_speeds_time_unbound_;
            debug_publisher_(debug_msg_);
            return;
        }
 
        // Reaching here means the plugin needs to generate a new trajectory
        std::vector<double> new_final_speeds;
        if (carma_planning_msgs::msg::TrajectoryPlan new_trajectory =
            generateNewTrajectory(maneuver_plan, req, new_final_speeds);
            new_trajectory.trajectory_points.size() >= 2)
        {
            // New trajectory, by default, extends for the whole maneuver duration, so time bound it
            auto new_trajectory_time_bound =
            basic_autonomy::waypoint_generation::constrain_to_time_boundary(
                new_trajectory.trajectory_points, config_.trajectory_time_length);
 
            resp->trajectory_plan = new_trajectory;
            resp->trajectory_plan.trajectory_points = new_trajectory_time_bound;
 
            // Update stored trajectories
            last_trajectory_time_unbound_ = new_trajectory;
            last_speeds_time_unbound_ = new_final_speeds;
 
            RCLCPP_DEBUG_STREAM(rclcpp::get_logger(LCI_TACTICAL_LOGGER),
                "USING NEW TRAJECTORY for case: " << (int)new_case);
        }
        // Fall back to last trajectory if new is invalid but last is valid
        else if (last_trajectory_time_unbound_.trajectory_points.size() >= 2 &&
                rclcpp::Time(last_trajectory_time_unbound_.trajectory_points.back().target_time) > current_time)
        {
            resp->trajectory_plan = last_trajectory_time_unbound_;
            resp->trajectory_plan.trajectory_points = last_trajectory_time_bound;
 
            RCLCPP_WARN_STREAM(rclcpp::get_logger(LCI_TACTICAL_LOGGER),
                "Failed to generate a new trajectory, so using last valid trajectory!");
        }
        // Last resort - return the invalid new trajectory
        else
        {
            resp->trajectory_plan = new_trajectory;
            RCLCPP_WARN_STREAM(rclcpp::get_logger(LCI_TACTICAL_LOGGER),
                "Failed to generate a new trajectory or use old valid trajectory, "
                "so returning empty/invalid trajectory!");
        }
 
        // Update stored case information
 
        last_case_ = new_case;
        is_last_case_successful_ = is_new_case_successful;
 
        // Update variables for next evaluation
        if (is_new_case_successful) {
            // LCI Tactical plugin receives only single maneuver that can span multiple lanelets
            // end of the maneuver is expected to be the start of the intersection
            last_successful_ending_downtrack_ =
                GET_MANEUVER_PROPERTY(maneuver_plan.front(), end_dist);
 
            // Entry time to the intersection
            last_successful_scheduled_entry_time_ =
                rclcpp::Time(GET_MANEUVER_PROPERTY(maneuver_plan.front(), end_time)).seconds();
 
            RCLCPP_DEBUG_STREAM(rclcpp::get_logger(LCI_TACTICAL_LOGGER),
                "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(LCI_TACTICAL_LOGGER),
            "Debug: new case:" << (int) new_case << ", is_new_case_successful: "
            << is_new_case_successful);
 
        resp->maneuver_status.push_back(
            carma_planning_msgs::srv::PlanTrajectory::Response::MANEUVER_IN_PROGRESS);
        resp->trajectory_plan.initial_longitudinal_velocity = last_speeds_time_unbound_.front();
 
        // Set the planning plugin field name
        for (auto& p : resp->trajectory_plan.trajectory_points) {
            p.planner_plugin_name = plugin_name_;
        }
 
        debug_msg_.trajectory_plan = resp->trajectory_plan;
        debug_msg_.velocity_profile = last_speeds_time_unbound_;
        debug_publisher_(debug_msg_);
    }

References config_, basic_autonomy::waypoint_generation::constrain_to_time_boundary(), current_downtrack_, debug_msg_, debug_publisher_, findSpeedLimit(), generateNewTrajectory(), GET_MANEUVER_PROPERTY, basic_autonomy::waypoint_generation::get_nearest_point_index(), is_last_case_successful_, last_case_, last_speeds_time_unbound_, last_successful_ending_downtrack_, last_successful_scheduled_entry_time_, last_trajectory_time_unbound_, LCI_TACTICAL_LOGGER, light_controlled_intersection_strategy_, logDebugInfoAboutPreviousTrajectory(), plugin_name_, shouldUseLastTrajectory(), speed_limit_, carma_cooperative_perception::to_string(), light_controlled_intersection_tactical_plugin::Config::trajectory_time_length, and wm_.

Referenced by planTrajectoryCB().

Here is the call graph for this function:

Here is the caller 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 616 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 611 of file light_controlled_intersection_tactical_plugin.cpp.

    {
        config_ = config;
    }

References config_.

◆ shouldUseLastTrajectory()

bool light_controlled_intersection_tactical_plugin::LightControlledIntersectionTacticalPlugin::shouldUseLastTrajectory	(	TSCase	new_case,
		bool	is_new_case_successful,
		const rclcpp::Time &	current_time
	)

private

Determines whether the last trajectory should be reused based on the planning case. Should use last case if 1) last traj is valid AND last case is the same as new case, OR 2) last traj is valid AND within certain evaluation zone before the intersection where the new case is not successful but last case is.

Parameters

new_case	An enumerated type representing the new trajectory planning case.
is_new_case_successful	A boolean flag indicating if the new trajectory planning was successful.
current_time	The current time stamp.

Returns: True if the last trajectory should be used, false otherwise.

Definition at line 70 of file light_controlled_intersection_tactical_plugin.cpp.

    {
        // Validate trajectory with 1 second minimum remaining time or vehicle_response_lag seconds
        // This is because the vehicle executes speed command of vehicle_response_lag secs ahead
        // Therefore, the trajectory should be enough to cover the vehicle_response_lag
        if (!isLastTrajectoryValid(current_time, std::max(config_.vehicle_response_lag, 1.0)))
        {
            return false;
        }
 
        // New case is successful and is same as the last case
        if (last_case_.get() == new_case && is_new_case_successful == true)
        {
            return true;
        }
 
        // Edge case - TS Alorithm successful to unsuccessful transition (Case (1-7) to 8)
        // near the intersection. The vehicle should "lock in" to the last trajectory as it is
        // expected for the TS algorithm to be unsuccessful near the intersection.
        if (is_last_case_successful_.get() == true &&
            is_new_case_successful == false &&
            last_successful_ending_downtrack_ - current_downtrack_ <
                config_.dist_before_intersection_to_force_last_traj &&
            last_successful_scheduled_entry_time_ - current_time.seconds() <
                config_.period_before_intersection_to_force_last_traj)
        {
            return true;
        }
 
        // Returning false here means that it should use the new trajectory because:
        // New case is not same as last case and not within the "lock in" distance
        return false;
    }

References config_, current_downtrack_, light_controlled_intersection_tactical_plugin::Config::dist_before_intersection_to_force_last_traj, is_last_case_successful_, isLastTrajectoryValid(), last_case_, last_successful_ending_downtrack_, last_successful_scheduled_entry_time_, light_controlled_intersection_tactical_plugin::Config::period_before_intersection_to_force_last_traj, and light_controlled_intersection_tactical_plugin::Config::vehicle_response_lag.

Referenced by planTrajectorySmoothing().

Here is the call graph for this function:

Here is the caller graph for this function:

Member Data Documentation

◆ config_

Config light_controlled_intersection_tactical_plugin::LightControlledIntersectionTacticalPlugin::config_

private

Definition at line 113 of file light_controlled_intersection_tactical_plugin.hpp.

Referenced by applyTrajectorySmoothingAlgorithm(), generateNewTrajectory(), planTrajectoryCB(), planTrajectorySmoothing(), setConfig(), and shouldUseLastTrajectory().

◆ current_downtrack_

double light_controlled_intersection_tactical_plugin::LightControlledIntersectionTacticalPlugin::current_downtrack_ = 0.0

private

Definition at line 142 of file light_controlled_intersection_tactical_plugin.hpp.

Referenced by generateNewTrajectory(), logDebugInfoAboutPreviousTrajectory(), planTrajectorySmoothing(), and shouldUseLastTrajectory().

◆ debug_msg_

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

private

Definition at line 149 of file light_controlled_intersection_tactical_plugin.hpp.

Referenced by generateNewTrajectory(), and planTrajectorySmoothing().

◆ debug_publisher_

DebugPublisher light_controlled_intersection_tactical_plugin::LightControlledIntersectionTacticalPlugin::debug_publisher_

private

Definition at line 148 of file light_controlled_intersection_tactical_plugin.hpp.

Referenced by planTrajectorySmoothing().

◆ ending_state_before_buffer_

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

private

Definition at line 137 of file light_controlled_intersection_tactical_plugin.hpp.

Referenced by generateNewTrajectory().

◆ epsilon_

double light_controlled_intersection_tactical_plugin::LightControlledIntersectionTacticalPlugin::epsilon_ = 0.001

private

Definition at line 139 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 129 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 133 of file light_controlled_intersection_tactical_plugin.hpp.

Referenced by isLastTrajectoryValid(), logDebugInfoAboutPreviousTrajectory(), planTrajectorySmoothing(), and shouldUseLastTrajectory().

◆ last_case_

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

private

Definition at line 132 of file light_controlled_intersection_tactical_plugin.hpp.

Referenced by isLastTrajectoryValid(), logDebugInfoAboutPreviousTrajectory(), planTrajectorySmoothing(), and shouldUseLastTrajectory().

◆ last_speeds_time_unbound_

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

private

Definition at line 150 of file light_controlled_intersection_tactical_plugin.hpp.

Referenced by isLastTrajectoryValid(), logDebugInfoAboutPreviousTrajectory(), and planTrajectorySmoothing().

◆ last_successful_ending_downtrack_

double light_controlled_intersection_tactical_plugin::LightControlledIntersectionTacticalPlugin::last_successful_ending_downtrack_

private

Definition at line 144 of file light_controlled_intersection_tactical_plugin.hpp.

Referenced by logDebugInfoAboutPreviousTrajectory(), planTrajectorySmoothing(), and shouldUseLastTrajectory().

◆ last_successful_scheduled_entry_time_

double light_controlled_intersection_tactical_plugin::LightControlledIntersectionTacticalPlugin::last_successful_scheduled_entry_time_

private

Definition at line 145 of file light_controlled_intersection_tactical_plugin.hpp.

Referenced by logDebugInfoAboutPreviousTrajectory(), planTrajectorySmoothing(), and shouldUseLastTrajectory().

◆ last_trajectory_time_unbound_

carma_planning_msgs::msg::TrajectoryPlan light_controlled_intersection_tactical_plugin::LightControlledIntersectionTacticalPlugin::last_trajectory_time_unbound_

private

Definition at line 134 of file light_controlled_intersection_tactical_plugin.hpp.

Referenced by isLastTrajectoryValid(), logDebugInfoAboutPreviousTrajectory(), and planTrajectorySmoothing().

◆ latest_traj_request_header_stamp_

rclcpp::Time light_controlled_intersection_tactical_plugin::LightControlledIntersectionTacticalPlugin::latest_traj_request_header_stamp_

private

Definition at line 138 of file light_controlled_intersection_tactical_plugin.hpp.

Referenced by logDebugInfoAboutPreviousTrajectory(), and planTrajectoryCB().

◆ LCI_TACTICAL_LOGGER

const std::string light_controlled_intersection_tactical_plugin::LightControlledIntersectionTacticalPlugin::LCI_TACTICAL_LOGGER = "light_controlled_intersection_tactical_plugin"

private

Definition at line 136 of file light_controlled_intersection_tactical_plugin.hpp.

Referenced by applyTrajectorySmoothingAlgorithm(), createGeometryProfile(), logDebugInfoAboutPreviousTrajectory(), planTrajectoryCB(), and planTrajectorySmoothing().

◆ light_controlled_intersection_strategy_

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

private

Definition at line 152 of file light_controlled_intersection_tactical_plugin.hpp.

Referenced by planTrajectorySmoothing().

◆ nh_

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

private

Definition at line 115 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 147 of file light_controlled_intersection_tactical_plugin.hpp.

Referenced by planTrajectorySmoothing().

◆ scheduled_depart_time_

uint32_t light_controlled_intersection_tactical_plugin::LightControlledIntersectionTacticalPlugin::scheduled_depart_time_ = 0.0

private

Definition at line 125 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 123 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 127 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 121 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 131 of file light_controlled_intersection_tactical_plugin.hpp.

Referenced by applyTrajectorySmoothingAlgorithm(), and planTrajectorySmoothing().

◆ street_msg_timestamp_

uint32_t light_controlled_intersection_tactical_plugin::LightControlledIntersectionTacticalPlugin::street_msg_timestamp_ = 0.0

private

Definition at line 119 of file light_controlled_intersection_tactical_plugin.hpp.

◆ wm_

carma_wm::WorldModelConstPtr light_controlled_intersection_tactical_plugin::LightControlledIntersectionTacticalPlugin::wm_

private

Definition at line 110 of file light_controlled_intersection_tactical_plugin.hpp.

Referenced by applyOptimizedTargetSpeedProfile(), generateNewTrajectory(), and planTrajectorySmoothing().

◆ yield_client_

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

private

Definition at line 135 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/6]

◆ FRIEND_TEST() [2/6]

◆ FRIEND_TEST() [3/6]

◆ FRIEND_TEST() [4/6]

◆ FRIEND_TEST() [5/6]

◆ FRIEND_TEST() [6/6]

◆ generateNewTrajectory()

◆ isLastTrajectoryValid()

◆ logDebugInfoAboutPreviousTrajectory()

◆ planTrajectoryCB()

◆ planTrajectorySmoothing()

◆ set_yield_client()

◆ setConfig()

◆ shouldUseLastTrajectory()

Member Data Documentation

◆ config_

◆ current_downtrack_

◆ debug_msg_

◆ debug_publisher_

◆ ending_state_before_buffer_

◆ epsilon_

◆ is_allowed_int_

◆ is_last_case_successful_

◆ last_case_

◆ last_speeds_time_unbound_

◆ last_successful_ending_downtrack_

◆ last_successful_scheduled_entry_time_

◆ last_trajectory_time_unbound_

◆ latest_traj_request_header_stamp_

◆ LCI_TACTICAL_LOGGER

◆ 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_