stop_controlled_intersection_tactical_plugin::StopControlledIntersectionTacticalPlugin Class Reference

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

#include <stop_controlled_intersection_plugin.hpp>

Inheritance diagram for stop_controlled_intersection_tactical_plugin::StopControlledIntersectionTacticalPlugin:

Collaboration diagram for stop_controlled_intersection_tactical_plugin::StopControlledIntersectionTacticalPlugin:

Public Member Functions
	StopControlledIntersectionTacticalPlugin (const rclcpp::NodeOptions &options)
std::vector< PointSpeedPair >	maneuvers_to_points (const std::vector< carma_planning_msgs::msg::Maneuver > &maneuvers, const carma_wm::WorldModelConstPtr &wm, const carma_planning_msgs::msg::VehicleState &state)
	Converts a set of requested stop controlled intersection maneuvers to point speed limit pairs. More...
std::vector< PointSpeedPair >	create_case_one_speed_profile (const carma_wm::WorldModelConstPtr &wm, const carma_planning_msgs::msg::Maneuver &maneuver, std::vector< lanelet::BasicPoint2d > &route_geometry_points, double starting_speed, const carma_planning_msgs::msg::VehicleState &states)
	Creates a speed profile according to case one of the stop controlled intersection, where the vehicle accelerates and then decelerates to a stop. More...
std::vector< PointSpeedPair >	create_case_two_speed_profile (const carma_wm::WorldModelConstPtr &wm, const carma_planning_msgs::msg::Maneuver &maneuver, std::vector< lanelet::BasicPoint2d > &route_geometry_points, double starting_speed)
	Creates a speed profile according to case two of the stop controlled intersection, where the vehicle first accelerates then cruises and finally decelerates to a stop. More...
std::vector< PointSpeedPair >	create_case_three_speed_profile (const carma_wm::WorldModelConstPtr &wm, const carma_planning_msgs::msg::Maneuver &maneuver, std::vector< lanelet::BasicPoint2d > &route_geometry_points, double starting_speed)
	Creates a speed profile according to case three of the stop controlled intersection, where the vehicle continuously decelerates to a stop. More...
std::vector< carma_planning_msgs::msg::TrajectoryPlanPoint >	compose_trajectory_from_centerline (const std::vector< PointSpeedPair > &points, const carma_planning_msgs::msg::VehicleState &state, const rclcpp::Time &state_time)
	Method converts a list of lanelet centerline points and current vehicle state into a usable list of trajectory points for trajectory planning. More...
carma_ros2_utils::CallbackReturn	on_configure_plugin () override
	Method which is triggered when this plugin is moved from the UNCONFIGURED to INACTIVE states. This method should be used to load parameters and is required to be implemented. More...
bool	get_availability ()
	Get the availability status of this plugin based on the current operating environment. Method must be overriden by extending classes. More...
std::string	get_version_id ()
	Returns the version id of this plugin. More...
rcl_interfaces::msg::SetParametersResult	parameter_update_callback (const std::vector< rclcpp::Parameter > &parameters)
void	plan_trajectory_callback (std::shared_ptr< rmw_request_id_t >, carma_planning_msgs::srv::PlanTrajectory::Request::SharedPtr req, carma_planning_msgs::srv::PlanTrajectory::Response::SharedPtr resp) override
	Extending class provided callback which should return a planned trajectory based on the provided trajectory planning request. More...
Public Member Functions inherited from carma_guidance_plugins::TacticalPlugin
	TacticalPlugin (const rclcpp::NodeOptions &)
	TacticalPlugin constructor. More...
virtual	~TacticalPlugin ()=default
	Virtual destructor for safe deletion. More...
virtual void	plan_trajectory_callback (std::shared_ptr< rmw_request_id_t > srv_header, carma_planning_msgs::srv::PlanTrajectory::Request::SharedPtr req, carma_planning_msgs::srv::PlanTrajectory::Response::SharedPtr resp)=0
	Extending class provided callback which should return a planned trajectory based on the provided trajectory planning request. More...
std::string	get_capability () override
	Get the capability string representing this plugins capabilities Method must be overriden by extending classes. Expectation is that abstract plugin type parent classes will provide a default implementation. More...
uint8_t	get_type () override final
	Returns the type of this plugin according to the carma_planning_msgs::Plugin type enum. Extending classes for the specific type should override this method. More...
carma_ros2_utils::CallbackReturn	handle_on_configure (const rclcpp_lifecycle::State &) override final
carma_ros2_utils::CallbackReturn	handle_on_activate (const rclcpp_lifecycle::State &) override final
carma_ros2_utils::CallbackReturn	handle_on_deactivate (const rclcpp_lifecycle::State &) override final
carma_ros2_utils::CallbackReturn	handle_on_cleanup (const rclcpp_lifecycle::State &) override final
carma_ros2_utils::CallbackReturn	handle_on_shutdown (const rclcpp_lifecycle::State &) override final
carma_ros2_utils::CallbackReturn	handle_on_error (const rclcpp_lifecycle::State &, const std::string &exception_string) override final
Public Member Functions inherited from carma_guidance_plugins::PluginBaseNode
	PluginBaseNode (const rclcpp::NodeOptions &)
	PluginBaseNode constructor. More...
virtual	~PluginBaseNode ()=default
	Virtual destructor for safe deletion. More...
virtual std::shared_ptr< carma_wm::WMListener >	get_world_model_listener () final
	Method to return the default world model listener provided as a convience by this base class If this method or get_world_model() are not called then the world model remains uninitialized and will not create unnecessary subscriptions. More...
virtual carma_wm::WorldModelConstPtr	get_world_model () final
	Method to return the default world model provided as a convience by this base class If this method or get_world_model_listener() are not called then the world model remains uninitialized and will not create unnecessary subscriptions. More...
virtual bool	get_activation_status () final
	Returns the activation status of this plugin. The plugins API callbacks will only be triggered when this method returns true. More...
virtual uint8_t	get_type ()
	Returns the type of this plugin according to the carma_planning_msgs::Plugin type enum. Extending classes for the specific type should override this method. More...
std::string	get_plugin_name_and_ns () const
	Return the name of this plugin with namespace. NOTE: If only the name of the plugin is required, use get_plugin_name() More...
std::string	get_plugin_name () const
	Return the name of this plugin. More...
virtual bool	get_availability ()=0
	Get the availability status of this plugin based on the current operating environment. Method must be overriden by extending classes. More...
virtual std::string	get_capability ()=0
	Get the capability string representing this plugins capabilities Method must be overriden by extending classes. Expectation is that abstract plugin type parent classes will provide a default implementation. More...
virtual std::string	get_version_id ()=0
	Returns the version id of this plugin. More...
virtual carma_ros2_utils::CallbackReturn	on_configure_plugin ()=0
	Method which is triggered when this plugin is moved from the UNCONFIGURED to INACTIVE states. This method should be used to load parameters and is required to be implemented. More...
virtual carma_ros2_utils::CallbackReturn	on_activate_plugin ()
	Method which is triggered when this plugin is moved from the INACTIVE to ACTIVE states. This method should be used to prepare for future callbacks for plugin's capabilites. More...
virtual carma_ros2_utils::CallbackReturn	on_deactivate_plugin ()
	Method which is triggered when this plugin is moved from the ACTIVE to INACTIVE states. This method should be used to disable any functionality which should cease execution when plugin is inactive. More...
virtual carma_ros2_utils::CallbackReturn	on_cleanup_plugin ()
	Method which is triggered when this plugin is moved from the INACTIVE to UNCONFIGURED states. This method should be used to fully reset the plugin such that a future call to on_configure_plugin would leave the plugin in a fresh state as though just launched. More...
virtual carma_ros2_utils::CallbackReturn	on_shutdown_plugin ()
	Method which is triggered when this plugin is moved from any state to FINALIZED This method should be used to generate any shutdown logs or final cleanup. More...
virtual carma_ros2_utils::CallbackReturn	on_error_plugin (const std::string &exception_string)
	Method which is triggered when an unhandled exception occurs in this plugin This method should be used to cleanup such that the plugin could be moved to UNCONFIGURED state if possible. More...
carma_ros2_utils::CallbackReturn	handle_on_configure (const rclcpp_lifecycle::State &) override
carma_ros2_utils::CallbackReturn	handle_on_activate (const rclcpp_lifecycle::State &) override
carma_ros2_utils::CallbackReturn	handle_on_deactivate (const rclcpp_lifecycle::State &) override
carma_ros2_utils::CallbackReturn	handle_on_cleanup (const rclcpp_lifecycle::State &) override
carma_ros2_utils::CallbackReturn	handle_on_shutdown (const rclcpp_lifecycle::State &) override
carma_ros2_utils::CallbackReturn	handle_on_error (const rclcpp_lifecycle::State &, const std::string &exception_string) override
	FRIEND_TEST (carma_guidance_plugins_test, connections_test)

Private Member Functions
	FRIEND_TEST (StopControlledIntersectionTacticalPlugin, TestSCIPlanning_case_one)
	FRIEND_TEST (StopControlledIntersectionTacticalPlugin, TestSCIPlanning_case_two)
	FRIEND_TEST (StopControlledIntersectionTacticalPlugin, TestSCIPlanning_case_three)

Private Attributes
carma_wm::WorldModelConstPtr	wm_
StopControlledIntersectionTacticalPluginConfig	config_
std::string	stop_controlled_intersection_strategy_ = "Carma/stop_controlled_intersection"
double	epsilon_ = 0.001

Detailed Description

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

Definition at line 63 of file stop_controlled_intersection_plugin.hpp.

Constructor & Destructor Documentation

StopControlledIntersectionTacticalPlugin()

stop_controlled_intersection_tactical_plugin::StopControlledIntersectionTacticalPlugin::StopControlledIntersectionTacticalPlugin ( const rclcpp::NodeOptions &  options )

explicit

Definition at line 51 of file stop_controlled_intersection_tactical_plugin.cpp.

  : carma_guidance_plugins::TacticalPlugin(options), config_(StopControlledIntersectionTacticalPluginConfig()) 
{
    basic_autonomy::set_logger(get_logger().get_child("basic_autonomy"));
    // Declare parameters
    config_.trajectory_time_length = declare_parameter<double>("trajectory_time_length",   config_.trajectory_time_length);
    config_.curve_resample_step_size = declare_parameter<double>("curve_resample_step_size",   config_.curve_resample_step_size);
    config_.centerline_sampling_spacing = declare_parameter<double>("centerline_sampling_spacing",   config_.centerline_sampling_spacing);
    config_.curvature_moving_average_window_size = declare_parameter<int>("curvature_moving_average_window_size",   config_.curvature_moving_average_window_size);
    config_.lateral_accel_limit = declare_parameter<double>("lateral_accel_limit",   config_.lateral_accel_limit);
    config_.speed_moving_average_window_size = declare_parameter<int>("speed_moving_average_window_size",   config_.speed_moving_average_window_size);
    config_.back_distance = declare_parameter<double>("back_distance",   config_.back_distance);
}

References stop_controlled_intersection_tactical_plugin::StopControlledIntersectionTacticalPluginConfig::back_distance, stop_controlled_intersection_tactical_plugin::StopControlledIntersectionTacticalPluginConfig::centerline_sampling_spacing, config_, stop_controlled_intersection_tactical_plugin::StopControlledIntersectionTacticalPluginConfig::curvature_moving_average_window_size, stop_controlled_intersection_tactical_plugin::StopControlledIntersectionTacticalPluginConfig::curve_resample_step_size, basic_autonomy::get_logger(), stop_controlled_intersection_tactical_plugin::StopControlledIntersectionTacticalPluginConfig::lateral_accel_limit, basic_autonomy::set_logger(), stop_controlled_intersection_tactical_plugin::StopControlledIntersectionTacticalPluginConfig::speed_moving_average_window_size, and stop_controlled_intersection_tactical_plugin::StopControlledIntersectionTacticalPluginConfig::trajectory_time_length.

Here is the call graph for this function:

Member Function Documentation

compose_trajectory_from_centerline()

std::vector< carma_planning_msgs::msg::TrajectoryPlanPoint > stop_controlled_intersection_tactical_plugin::StopControlledIntersectionTacticalPlugin::compose_trajectory_from_centerline	(	const std::vector< PointSpeedPair > &	points,
		const carma_planning_msgs::msg::VehicleState &	state,
		const rclcpp::Time &	state_time
	)

Method converts a list of lanelet centerline points and current vehicle state into a usable list of trajectory points for trajectory planning.

Parameters

points	The set of points that define the current lane the vehicle is in and are defined based on the request planning maneuvers. 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.
state	The current state of the vehicle
state_time	The abosolute time which the provided vehicle state corresponds to

Returns

A list of trajectory points to send to the carma planning stack

Definition at line 478 of file stop_controlled_intersection_tactical_plugin.cpp.

                                                                                                                           {
    
    std::vector<carma_planning_msgs::msg::TrajectoryPlanPoint> trajectory;
    RCLCPP_DEBUG_STREAM(rclcpp::get_logger("stop_controlled_intersection_tactical_plugin"), "VehicleState: "
                        << " x: " << state.x_pos_global << " y: " << state.y_pos_global << " yaw: " << state.orientation
                        << " speed: " << state.longitudinal_vel);
    
    int nearest_pt_index = basic_autonomy::waypoint_generation::get_nearest_point_index(points, state);
    RCLCPP_DEBUG_STREAM(rclcpp::get_logger("stop_controlled_intersection_tactical_plugin"), "Nearest pt index: "<<nearest_pt_index);
    std::vector<PointSpeedPair> future_points(points.begin() + nearest_pt_index + 1, points.end()); //Points in front of current vehicle position
    RCLCPP_DEBUG_STREAM(rclcpp::get_logger("stop_controlled_intersection_tactical_plugin"), "Future points size: "<<future_points.size());
    auto time_bound_points = basic_autonomy::waypoint_generation::constrain_to_time_boundary(future_points, config_.trajectory_time_length);
    RCLCPP_DEBUG_STREAM(rclcpp::get_logger("stop_controlled_intersection_tactical_plugin"), "Got time bound points with size:" << time_bound_points.size());
 
    //Attach past points
    std::vector<PointSpeedPair> back_and_future = attach_past_points(points, time_bound_points, nearest_pt_index, config_.back_distance);
    RCLCPP_DEBUG_STREAM(rclcpp::get_logger("stop_controlled_intersection_tactical_plugin"), "Got back_and_future points with size: "<<back_and_future.size());
 
    std::vector<double> speed_limits;
    std::vector<lanelet::BasicPoint2d> curve_points;
    split_point_speed_pairs(time_bound_points, &curve_points, &speed_limits);
 
    std::unique_ptr<basic_autonomy::smoothing::SplineI> fit_curve = basic_autonomy::waypoint_generation::compute_fit(curve_points); //Compute splines based on curve points
    if(!fit_curve)
    {
        throw std::invalid_argument("Could not fit a spline curve along the trajectory!");
    }
 
    RCLCPP_DEBUG_STREAM(rclcpp::get_logger("stop_controlled_intersection_tactical_plugin"), "Got fit");
    RCLCPP_DEBUG_STREAM(rclcpp::get_logger("stop_controlled_intersection_tactical_plugin"), "Speed_limits.size(): "<<speed_limits.size());
 
    std::vector<lanelet::BasicPoint2d> all_sampling_points;
    all_sampling_points.reserve(1 + curve_points.size() * 2);
 
    std::vector<double> distributed_speed_limits;
    distributed_speed_limits.reserve(1+ curve_points.size() * 2);
 
    //Compute total length of the trajectory to get correct number of points
    // we expect using curve resample step size
    std::vector<double> downtracks_raw = carma_wm::geometry::compute_arc_lengths(curve_points);
 
    auto total_step_along_curve = static_cast<int>(downtracks_raw.back() / config_.curve_resample_step_size);
 
    int current_speed_index = 0;
    size_t total_point_size = curve_points.size();
 
    double step_threshold_for_next_speed = (double)total_step_along_curve / (double)total_point_size;
    double scaled_steps_along_curve = 0.0; // from 0 (start) to 1 (end) for the whole trajectory
    std::vector<double> better_curvature;
    better_curvature.reserve(1 + curve_points.size() * 2);
 
    for (size_t steps_along_curve = 0; steps_along_curve < total_step_along_curve; steps_along_curve++) // Resample curve at tighter resolution
    {
        lanelet::BasicPoint2d p = (*fit_curve)(scaled_steps_along_curve);
        all_sampling_points.push_back(p);
        double c = basic_autonomy::waypoint_generation::compute_curvature_at((*fit_curve), scaled_steps_along_curve);
        better_curvature.push_back(c);
 
        if((double) steps_along_curve > step_threshold_for_next_speed)
        {
            step_threshold_for_next_speed += (double)total_step_along_curve / (double)total_point_size;
            current_speed_index++;
        }
        distributed_speed_limits.push_back(speed_limits[current_speed_index]);  //Identify speed limits for resampled points
        scaled_steps_along_curve += 1.0 / total_step_along_curve;               //adding steps_along_curve_step_size
    }
 
    RCLCPP_DEBUG_STREAM(rclcpp::get_logger("stop_controlled_intersection_tactical_plugin"), "Got sampled points with size:" << all_sampling_points.size());
 
    std::vector<double> final_yaw_values = carma_wm::geometry::compute_tangent_orientations(all_sampling_points);
 
    std::vector<double> curvatures = basic_autonomy::smoothing::moving_average_filter(better_curvature, config_.curvature_moving_average_window_size, false);
    std::vector<double> ideal_speeds =
        trajectory_utils::constrained_speeds_for_curvatures(curvatures, config_.lateral_accel_limit);
 
    std::vector<double> constrained_speed_limits = basic_autonomy::waypoint_generation::apply_speed_limits(ideal_speeds, distributed_speed_limits); //Speed min(ideal, calculated)
    RCLCPP_DEBUG_STREAM(rclcpp::get_logger("stop_controlled_intersection_tactical_plugin"), "Processed all points in computed fit");
    std::vector<double> final_actual_speeds = constrained_speed_limits;
 
    if (all_sampling_points.empty())
    {
        RCLCPP_WARN_STREAM(rclcpp::get_logger("stop_controlled_intersection_tactical_plugin"), "No trajectory points could be generated");
        return {};
    }
 
    //Drop Past points
    nearest_pt_index = basic_autonomy::waypoint_generation::get_nearest_index_by_downtrack(all_sampling_points, wm_, state);
    std::vector<lanelet::BasicPoint2d> future_basic_points(all_sampling_points.begin() + nearest_pt_index + 1,
                                                            all_sampling_points.end());
    std::vector<double> future_speeds(final_actual_speeds.begin() + nearest_pt_index + 1, 
                                    final_actual_speeds.end());                                                            
    std::vector<double> future_yaw(final_yaw_values.begin() + nearest_pt_index + 1,
                                    final_yaw_values.end());
 
    // Add current vehicle point to front of the trajectory
    lanelet::BasicPoint2d cur_veh_point(state.x_pos_global, state.y_pos_global);
 
    future_basic_points.insert(future_basic_points.begin(),
                                cur_veh_point); // Add current vehicle position to front of sample points
    future_speeds.insert(future_speeds.begin(), state.longitudinal_vel);
    future_yaw.insert(future_yaw.begin(), state.orientation);
 
    // Compute points to local downtracks
    std::vector<double> downtracks = carma_wm::geometry::compute_arc_lengths(future_basic_points);
 
    final_actual_speeds = basic_autonomy::smoothing::moving_average_filter(future_speeds, config_.speed_moving_average_window_size);
 
    // Convert speeds to times
    std::vector<double> times;
 
    //Force last point speed to 0.0 if close to end
    if(lanelet::geometry::distance2d(future_basic_points.back(), points.back().point) < epsilon_){
        final_actual_speeds.back() = 0.0;
    }
 
    trajectory_utils::conversions::speed_to_time(downtracks, final_actual_speeds, &times);
 
    // Build trajectory points
    std::vector<carma_planning_msgs::msg::TrajectoryPlanPoint> traj_points =
        basic_autonomy::waypoint_generation::trajectory_from_points_times_orientations(future_basic_points, times, future_yaw, state_time, "default");
 
    return traj_points;
}

References basic_autonomy::waypoint_generation::apply_speed_limits(), basic_autonomy::waypoint_generation::attach_past_points(), stop_controlled_intersection_tactical_plugin::StopControlledIntersectionTacticalPluginConfig::back_distance, process_traj_logs::c, carma_wm::geometry::compute_arc_lengths(), basic_autonomy::waypoint_generation::compute_curvature_at(), basic_autonomy::waypoint_generation::compute_fit(), carma_wm::geometry::compute_tangent_orientations(), config_, basic_autonomy::waypoint_generation::constrain_to_time_boundary(), stop_controlled_intersection_tactical_plugin::StopControlledIntersectionTacticalPluginConfig::curvature_moving_average_window_size, stop_controlled_intersection_tactical_plugin::StopControlledIntersectionTacticalPluginConfig::curve_resample_step_size, epsilon_, basic_autonomy::get_logger(), basic_autonomy::waypoint_generation::get_nearest_index_by_downtrack(), basic_autonomy::waypoint_generation::get_nearest_point_index(), stop_controlled_intersection_tactical_plugin::StopControlledIntersectionTacticalPluginConfig::lateral_accel_limit, basic_autonomy::smoothing::moving_average_filter(), stop_controlled_intersection_tactical_plugin::StopControlledIntersectionTacticalPluginConfig::speed_moving_average_window_size, basic_autonomy::waypoint_generation::split_point_speed_pairs(), basic_autonomy::waypoint_generation::trajectory_from_points_times_orientations(), stop_controlled_intersection_tactical_plugin::StopControlledIntersectionTacticalPluginConfig::trajectory_time_length, and wm_.

Referenced by plan_trajectory_callback().

Here is the call graph for this function:

Here is the caller graph for this function:

create_case_one_speed_profile()

std::vector< PointSpeedPair > stop_controlled_intersection_tactical_plugin::StopControlledIntersectionTacticalPlugin::create_case_one_speed_profile	(	const carma_wm::WorldModelConstPtr &	wm,
		const carma_planning_msgs::msg::Maneuver &	maneuver,
		std::vector< lanelet::BasicPoint2d > &	route_geometry_points,
		double	starting_speed,
		const carma_planning_msgs::msg::VehicleState &	states
	)

Creates a speed profile according to case one of the stop controlled intersection, where the vehicle accelerates and then decelerates to a stop.

Parameters

wm	Pointer to intialized world model for semantic map access
maneuvers	The maneuver to being planned for. Maneuver meta-dara parameters are used to create the trajectory profile.
route_geometry_points	The geometry points along the route which are associated with a speed in this method.
starting_speed	The current speed of the vehicle at the time of the trajectory planning request

Returns

List of centerline points paired with target speeds

Definition at line 233 of file stop_controlled_intersection_tactical_plugin.cpp.

                                                                                                                                                                              {
 
    RCLCPP_DEBUG_STREAM(rclcpp::get_logger("stop_controlled_intersection_tactical_plugin"), "Planning for Case One");
    //Derive meta data values from maneuver message - Using order in sci_strategic_plugin
    double a_acc = GET_MANEUVER_PROPERTY(maneuver, parameters.float_valued_meta_data[0]);
    double a_dec = GET_MANEUVER_PROPERTY(maneuver, parameters.float_valued_meta_data[1]); //a_dec is a -ve value
    double t_acc = GET_MANEUVER_PROPERTY(maneuver, parameters.float_valued_meta_data[2]);
    double t_dec = GET_MANEUVER_PROPERTY(maneuver, parameters.float_valued_meta_data[3]);
    double speed_before_decel = GET_MANEUVER_PROPERTY(maneuver, parameters.float_valued_meta_data[4]);
    RCLCPP_DEBUG_STREAM(rclcpp::get_logger("stop_controlled_intersection_tactical_plugin"), "a_acc received: "<< a_acc);
    RCLCPP_DEBUG_STREAM(rclcpp::get_logger("stop_controlled_intersection_tactical_plugin"), "a_dec received: "<< a_dec);
    RCLCPP_DEBUG_STREAM(rclcpp::get_logger("stop_controlled_intersection_tactical_plugin"), "t_acc received: "<< t_acc);
    RCLCPP_DEBUG_STREAM(rclcpp::get_logger("stop_controlled_intersection_tactical_plugin"), "t_dec received: "<< t_dec);
    RCLCPP_DEBUG_STREAM(rclcpp::get_logger("stop_controlled_intersection_tactical_plugin"), "speed before decel received: "<< speed_before_decel);
    
    //Derive start and end dist from maneuver
    double start_dist = GET_MANEUVER_PROPERTY(maneuver, start_dist);
    double end_dist = GET_MANEUVER_PROPERTY(maneuver, end_dist);
 
    RCLCPP_DEBUG_STREAM(rclcpp::get_logger("stop_controlled_intersection_tactical_plugin"), "Maneuver starting downtrack: "<< start_dist);
    RCLCPP_DEBUG_STREAM(rclcpp::get_logger("stop_controlled_intersection_tactical_plugin"), "Maneuver ending downtrack: "<< end_dist);
    //Checking state against start_dist and adjust profile
    lanelet::BasicPoint2d state_point(state.x_pos_global, state.y_pos_global);
    double route_starting_downtrack = wm->routeTrackPos(state_point).downtrack;  //Starting downtrack based on geometry points
    double dist_acc;        //Distance for which acceleration lasts
 
    if(route_starting_downtrack < start_dist){
        //Update parameters
        //Keeping the deceleration part the same
        RCLCPP_DEBUG_STREAM(rclcpp::get_logger("stop_controlled_intersection_tactical_plugin"), "Starting distance is less than maneuver start, updating parameters");
        double dist_decel = pow(speed_before_decel, 2)/(2*std::abs(a_dec));
 
        dist_acc = end_dist - dist_decel;
        a_acc = (pow(speed_before_decel, 2) - pow(starting_speed,2))/(2*dist_acc);
        RCLCPP_DEBUG_STREAM(rclcpp::get_logger("stop_controlled_intersection_tactical_plugin"), "Updated a_acc: "<< a_acc);
    }
    else{
        //Use parameters from maneuver message
        dist_acc = (pow(speed_before_decel, 2) - pow(starting_speed, 2))/(2*a_acc);
    }
 
    std::vector<PointSpeedPair> points_and_target_speeds;
    PointSpeedPair first_point;
    first_point.point = state_point;
    first_point.speed = starting_speed;
    points_and_target_speeds.push_back(first_point);
 
    lanelet::BasicPoint2d prev_point = state_point;
    double total_dist_covered = 0;                  //Starting dist for maneuver treated as 0.0
 
    for(size_t i = 1; i < route_geometry_points.size(); i++){
        lanelet::BasicPoint2d current_point = route_geometry_points[i];
        double delta_d = lanelet::geometry::distance2d(prev_point, current_point);
        total_dist_covered += delta_d;      
        //Find speed at dist covered
        double speed_i; 
        if(total_dist_covered <= dist_acc){
            //Acceleration part
            speed_i = sqrt(pow(starting_speed,2) + 2*a_acc*total_dist_covered);
        }
        else{
            //Deceleration part
            speed_i = sqrt(std::max(pow(speed_before_decel,2) + 2*a_dec*(total_dist_covered - dist_acc),0.0)); //std::max to ensure negative value is not sqrt
            if(speed_i < epsilon_){
                speed_i = 0.0;
            }
        }
 
        PointSpeedPair p;
        if(speed_i < epsilon_){
            p.point = prev_point;
            p.speed = 0.0;
        }
        else{
            p.point = route_geometry_points[i];
            p.speed = speed_i;
            prev_point = current_point; //Advance prev point if speed changes
        }
        points_and_target_speeds.push_back(p);
 
    }
 
    return points_and_target_speeds;
 
}

References epsilon_, process_bag::first_point, basic_autonomy::get_logger(), GET_MANEUVER_PROPERTY, process_bag::i, basic_autonomy::waypoint_generation::PointSpeedPair::point, and basic_autonomy::waypoint_generation::PointSpeedPair::speed.

Referenced by maneuvers_to_points().

Here is the call graph for this function:

Here is the caller graph for this function:

create_case_three_speed_profile()

std::vector< PointSpeedPair > stop_controlled_intersection_tactical_plugin::StopControlledIntersectionTacticalPlugin::create_case_three_speed_profile	(	const carma_wm::WorldModelConstPtr &	wm,
		const carma_planning_msgs::msg::Maneuver &	maneuver,
		std::vector< lanelet::BasicPoint2d > &	route_geometry_points,
		double	starting_speed
	)

Creates a speed profile according to case three of the stop controlled intersection, where the vehicle continuously decelerates to a stop.

Parameters

wm	Pointer to intialized world model for semantic map access
maneuvers	The maneuver to being planned for. Maneuver meta-dara parameters are used to create the trajectory profile.
route_geometry_points	The geometry points along the route which are associated with a speed in this method.
starting_speed	The current speed of the vehicle at the time of the trajectory planning request

Returns

List of centerline points paired with speed limits

Definition at line 421 of file stop_controlled_intersection_tactical_plugin.cpp.

                                                                                                                             {
    RCLCPP_DEBUG_STREAM(rclcpp::get_logger("stop_controlled_intersection_tactical_plugin"), "Planning for Case three");
    //Derive meta data values from maneuver message - Using order in sci_strategic_plugin
    double a_dec = GET_MANEUVER_PROPERTY(maneuver, parameters.float_valued_meta_data[0]);
 
    //Derive start and end dist from maneuver
    double start_dist = GET_MANEUVER_PROPERTY(maneuver, start_dist);
    double end_dist = GET_MANEUVER_PROPERTY(maneuver, end_dist);
    RCLCPP_DEBUG_STREAM(rclcpp::get_logger("stop_controlled_intersection_tactical_plugin"), "Maneuver starting downtrack: "<< start_dist);
    RCLCPP_DEBUG_STREAM(rclcpp::get_logger("stop_controlled_intersection_tactical_plugin"), "Maneuver ending downtrack: "<< end_dist);
 
    //Checking route geometry start against start_dist and adjust profile
    double route_starting_downtrack = wm->routeTrackPos(route_geometry_points[0]).downtrack;  //Starting downtrack based on geometry points
 
    if(route_starting_downtrack < start_dist){
        //update parameter
        RCLCPP_DEBUG_STREAM(rclcpp::get_logger("stop_controlled_intersection_tactical_plugin"), "Starting distance is less than maneuver start, updating parameters");
        a_dec = pow(starting_speed, 2)/(2*(end_dist - route_starting_downtrack));
    }
 
    std::vector<PointSpeedPair> points_and_target_speeds;
    PointSpeedPair first_point;
    first_point.point = route_geometry_points[0];
    first_point.speed = starting_speed;
    points_and_target_speeds.push_back(first_point);
 
    lanelet::BasicPoint2d prev_point = route_geometry_points[0];
    double total_dist_covered = 0;          //Starting dist for maneuver treated as 0.0
 
    for(size_t i = 0;i < route_geometry_points.size(); i++){
        lanelet::BasicPoint2d current_point = route_geometry_points[i];
        double delta_d = lanelet::geometry::distance2d(prev_point, current_point);
        total_dist_covered +=delta_d;
        //Find speed at dist covered
        double speed_i = sqrt(std::max(pow(starting_speed,2) + 2 * a_dec * total_dist_covered, 0.0)); //std::max to ensure negative value is not sqrt
        
        PointSpeedPair p;
        
        if(speed_i < epsilon_){
            p.point = prev_point;
            p.speed = 0.0;
        }
        else{
            p.point = route_geometry_points[i];
            p.speed = speed_i;
            prev_point = current_point; //Advance prev point if speed changes
        }
        
        points_and_target_speeds.push_back(p);
 
        
    }
 
    return points_and_target_speeds;
}

References epsilon_, process_bag::first_point, basic_autonomy::get_logger(), GET_MANEUVER_PROPERTY, process_bag::i, basic_autonomy::waypoint_generation::PointSpeedPair::point, and basic_autonomy::waypoint_generation::PointSpeedPair::speed.

Referenced by maneuvers_to_points().

Here is the call graph for this function:

Here is the caller graph for this function:

create_case_two_speed_profile()

std::vector< PointSpeedPair > stop_controlled_intersection_tactical_plugin::StopControlledIntersectionTacticalPlugin::create_case_two_speed_profile	(	const carma_wm::WorldModelConstPtr &	wm,
		const carma_planning_msgs::msg::Maneuver &	maneuver,
		std::vector< lanelet::BasicPoint2d > &	route_geometry_points,
		double	starting_speed
	)

Creates a speed profile according to case two of the stop controlled intersection, where the vehicle first accelerates then cruises and finally decelerates to a stop.

Parameters

wm	Pointer to intialized world model for semantic map access
maneuvers	The maneuver to being planned for. Maneuver meta-dara parameters are used to create the trajectory profile.
route_geometry_points	The geometry points along the route which are associated with a speed in this method.
starting_speed	The current speed of the vehicle at the time of the trajectory planning request

Returns

List of centerline points paired with speed limits

Definition at line 320 of file stop_controlled_intersection_tactical_plugin.cpp.

                                                                                                                             {
    RCLCPP_DEBUG_STREAM(rclcpp::get_logger("stop_controlled_intersection_tactical_plugin"), "Planning for Case Two");
    //Derive meta data values from maneuver message - Using order in sci_strategic_plugin
    double a_acc = GET_MANEUVER_PROPERTY(maneuver, parameters.float_valued_meta_data[0]);
    double a_dec = GET_MANEUVER_PROPERTY(maneuver, parameters.float_valued_meta_data[1]); //a_dec is a -ve value
    double t_acc = GET_MANEUVER_PROPERTY(maneuver, parameters.float_valued_meta_data[2]);
    double t_dec = GET_MANEUVER_PROPERTY(maneuver, parameters.float_valued_meta_data[3]);
    double t_cruise = GET_MANEUVER_PROPERTY(maneuver, parameters.float_valued_meta_data[4]);
    double speed_before_decel = GET_MANEUVER_PROPERTY(maneuver, parameters.float_valued_meta_data[5]);
 
    //Derive start and end dist from maneuver
    double start_dist = GET_MANEUVER_PROPERTY(maneuver, start_dist);
    double end_dist = GET_MANEUVER_PROPERTY(maneuver, end_dist);
    RCLCPP_DEBUG_STREAM(rclcpp::get_logger("stop_controlled_intersection_tactical_plugin"), "Maneuver starting downtrack: "<< start_dist);
    RCLCPP_DEBUG_STREAM(rclcpp::get_logger("stop_controlled_intersection_tactical_plugin"), "Maneuver ending downtrack: "<< end_dist);
 
    //Checking route geometry start against start_dist and adjust profile
    double route_starting_downtrack = wm->routeTrackPos(route_geometry_points[0]).downtrack;  //Starting downtrack based on geometry points
    double dist_acc;        //Distance over which acceleration happens
    double dist_cruise;     //Distance over which cruising happens
    double dist_decel;      //Distance over which deceleration happens
 
    if(route_starting_downtrack < start_dist){
        //update parameters
        //Keeping acceleration and deceleration part same as planned in strategic plugin
        RCLCPP_DEBUG_STREAM(rclcpp::get_logger("stop_controlled_intersection_tactical_plugin"), "Starting distance is less than maneuver start, updating parameters");
        dist_acc = starting_speed*t_acc + 0.5 * a_acc * pow(t_acc,2);
        dist_decel = speed_before_decel*t_dec + 0.5 * a_dec * pow(t_dec,2);
        dist_cruise = end_dist - route_starting_downtrack - (dist_acc + dist_decel);
    }
    else{   
        //Use maneuver parameters to create speed profile
        dist_acc = starting_speed*t_acc + 0.5 * a_acc * pow(t_acc,2);
        dist_cruise = speed_before_decel*t_cruise;
        dist_decel = speed_before_decel*t_dec + 0.5 * a_dec * pow(t_dec,2);
    }
 
    //Check calculated total dist against maneuver limits
    double total_distance_needed = dist_acc + dist_cruise + dist_decel;
    if(total_distance_needed - (end_dist - start_dist) > epsilon_ ){
        //Requested maneuver needs to be modified to meet start and end dist req
        //Sacrifice on cruising and then acceleration if needed
        RCLCPP_DEBUG_STREAM(rclcpp::get_logger("stop_controlled_intersection_tactical_plugin"), "Updating maneuver to meet start and end dist req.");
        double delta_total_dist = total_distance_needed - (end_dist - start_dist);
        dist_cruise -= delta_total_dist;
        if(dist_cruise < 0){
            dist_acc += dist_cruise;
            dist_cruise = 0;
        }
        //Not considering dist_acc < 0 after this.
    }
 
    std::vector<PointSpeedPair> points_and_target_speeds;
    PointSpeedPair first_point;
    first_point.point = route_geometry_points[0];
    first_point.speed = starting_speed;
    points_and_target_speeds.push_back(first_point);
 
    lanelet::BasicPoint2d prev_point = route_geometry_points.front();
    double total_dist_planned = 0;                  //Starting dist for maneuver treated as 0.0
    double prev_speed = starting_speed;
    for(auto route_point : route_geometry_points){
        lanelet::BasicPoint2d current_point = route_point;
        double delta_d = lanelet::geometry::distance2d(prev_point, current_point);
        total_dist_planned += delta_d;  
 
        //Find speed at dist covered
        double speed_i;
        if(total_dist_planned < dist_acc){
            //Acceleration part
            speed_i = sqrt(pow(starting_speed,2) + 2*a_acc*total_dist_planned);
        }
        else if(dist_cruise > 0 && total_dist_planned >= dist_acc && total_dist_planned <= (dist_acc + dist_cruise)){
            //Cruising part
            speed_i = prev_speed;
        }
        else{
            //Deceleration part
            speed_i = sqrt(std::max(pow(speed_before_decel,2) + 2*a_dec*(total_dist_planned - dist_acc - dist_cruise),0.0));//std::max to ensure negative value is not sqrt
        }
        
        PointSpeedPair p;
        if(speed_i < epsilon_){
            p.point = prev_point;
            p.speed = 0.0;
        }
        else{
            p.point = route_point;
            p.speed = std::min(speed_i,speed_before_decel);
            prev_point = current_point; //Advance prev point if speed changes
        }
        points_and_target_speeds.push_back(p);
 
        prev_speed = speed_i;
    }
 
    return points_and_target_speeds;
 
}

References epsilon_, process_bag::first_point, basic_autonomy::get_logger(), GET_MANEUVER_PROPERTY, basic_autonomy::waypoint_generation::PointSpeedPair::point, and basic_autonomy::waypoint_generation::PointSpeedPair::speed.

Referenced by maneuvers_to_points().

Here is the call graph for this function:

Here is the caller graph for this function:

FRIEND_TEST() [1/3]

stop_controlled_intersection_tactical_plugin::StopControlledIntersectionTacticalPlugin::FRIEND_TEST ( StopControlledIntersectionTacticalPlugin  , TestSCIPlanning_case_one    )

private

FRIEND_TEST() [2/3]

stop_controlled_intersection_tactical_plugin::StopControlledIntersectionTacticalPlugin::FRIEND_TEST ( StopControlledIntersectionTacticalPlugin  , TestSCIPlanning_case_three    )

private

FRIEND_TEST() [3/3]

stop_controlled_intersection_tactical_plugin::StopControlledIntersectionTacticalPlugin::FRIEND_TEST ( StopControlledIntersectionTacticalPlugin  , TestSCIPlanning_case_two    )

private

get_availability()

bool stop_controlled_intersection_tactical_plugin::StopControlledIntersectionTacticalPlugin::get_availability ( )

virtual

Get the availability status of this plugin based on the current operating environment. Method must be overriden by extending classes.

Returns

This method should return true if the plugin's current understanding of the world means it would be capable of planning or executing its capability.

Implements carma_guidance_plugins::PluginBaseNode.

Definition at line 603 of file stop_controlled_intersection_tactical_plugin.cpp.

{
  return true;
}

get_version_id()

std::string stop_controlled_intersection_tactical_plugin::StopControlledIntersectionTacticalPlugin::get_version_id ( )

virtual

Returns the version id of this plugin.

Returns

The version id represented as a string

Implements carma_guidance_plugins::PluginBaseNode.

Definition at line 608 of file stop_controlled_intersection_tactical_plugin.cpp.

{
  return "v1.0";
}

maneuvers_to_points()

std::vector< PointSpeedPair > stop_controlled_intersection_tactical_plugin::StopControlledIntersectionTacticalPlugin::maneuvers_to_points	(	const std::vector< carma_planning_msgs::msg::Maneuver > &	maneuvers,
		const carma_wm::WorldModelConstPtr &	wm,
		const carma_planning_msgs::msg::VehicleState &	state
	)

Converts a set of requested stop controlled intersection maneuvers to point speed limit pairs.

Parameters

maneuvers	The list of maneuvers to convert
wm	Pointer to intialized world model for semantic map access
state	The current state of the vehicle

Returns

List of centerline points paired with target speeds

Definition at line 171 of file stop_controlled_intersection_tactical_plugin.cpp.

{
    std::vector<PointSpeedPair> points_and_target_speeds;
    std::unordered_set<lanelet::Id> visited_lanelets;
 
    lanelet::BasicPoint2d veh_pos(state.x_pos_global, state.y_pos_global);
    double max_starting_downtrack = wm_->routeTrackPos(veh_pos).downtrack; //The vehicle position
    double starting_speed = state.longitudinal_vel;
 
    bool first = true;    
    double starting_downtrack;
    for (const auto& maneuver : maneuvers)
    {
        if(maneuver.type != carma_planning_msgs::msg::Maneuver::LANE_FOLLOWING && maneuver.type != carma_planning_msgs::msg::Maneuver::INTERSECTION_TRANSIT_STRAIGHT && maneuver.type != carma_planning_msgs::msg::Maneuver::INTERSECTION_TRANSIT_LEFT_TURN
        && maneuver.type !=carma_planning_msgs::msg::Maneuver::INTERSECTION_TRANSIT_RIGHT_TURN ){
            throw std::invalid_argument("Stop Controlled Intersection Tactical Plugin does not support this maneuver type");
        }
    
        if(first)
        {
            starting_downtrack = GET_MANEUVER_PROPERTY(maneuver, start_dist);
            if (starting_downtrack > max_starting_downtrack)
            {
                starting_downtrack = max_starting_downtrack;
            }
            first = false;
        }
 
        // Sample the lanelet centerline at fixed increments.
        // std::min call here is a guard against starting_downtrack being within 1m of the maneuver end_dist
        // in this case the sampleRoutePoints method will return a single point allowing execution to continue
        std::vector<lanelet::BasicPoint2d> route_points = wm->sampleRoutePoints(
            std::min(starting_downtrack + config_.centerline_sampling_spacing, GET_MANEUVER_PROPERTY(maneuver,end_dist)),
            GET_MANEUVER_PROPERTY(maneuver, end_dist), config_.centerline_sampling_spacing);
        
        route_points.insert(route_points.begin(), veh_pos);
        RCLCPP_DEBUG_STREAM(rclcpp::get_logger("stop_controlled_intersection_tactical_plugin"), "Route geometery points size: "<<route_points.size());
        //get case num from maneuver parameters
        if(GET_MANEUVER_PROPERTY(maneuver,parameters.int_valued_meta_data).empty()){
            throw std::invalid_argument("No case number specified for stop controlled intersection maneuver");
        }
        
        int case_num = GET_MANEUVER_PROPERTY(maneuver,parameters.int_valued_meta_data[0]);
        if(case_num == 1){
            points_and_target_speeds = create_case_one_speed_profile(wm, maneuver, route_points, starting_speed, state);
        }
        else if(case_num == 2){
            points_and_target_speeds = create_case_two_speed_profile(wm, maneuver, route_points, starting_speed);
        }
        else if(case_num == 3)
        {
            points_and_target_speeds = create_case_three_speed_profile(wm, maneuver, route_points, starting_speed);
        }
        else{
            throw std::invalid_argument("The stop controlled intersection tactical plugin doesn't handle the case number requested");
        }
    }
 
    return points_and_target_speeds;
}

References stop_controlled_intersection_tactical_plugin::StopControlledIntersectionTacticalPluginConfig::centerline_sampling_spacing, config_, create_case_one_speed_profile(), create_case_three_speed_profile(), create_case_two_speed_profile(), basic_autonomy::get_logger(), GET_MANEUVER_PROPERTY, and wm_.

Referenced by plan_trajectory_callback().

Here is the call graph for this function:

Here is the caller graph for this function:

on_configure_plugin()

carma_ros2_utils::CallbackReturn stop_controlled_intersection_tactical_plugin::StopControlledIntersectionTacticalPlugin::on_configure_plugin ( )

overridevirtual

Method which is triggered when this plugin is moved from the UNCONFIGURED to INACTIVE states. This method should be used to load parameters and is required to be implemented.

Returns

SUCCESS, FAILURE, or ERROR. Transition to INACTIVE will only complete on SUCCESS.

Implements carma_guidance_plugins::PluginBaseNode.

Definition at line 87 of file stop_controlled_intersection_tactical_plugin.cpp.

{
    config_ = StopControlledIntersectionTacticalPluginConfig();
 
     // Declare parameters
    get_parameter<double>("trajectory_time_length",   config_.trajectory_time_length);
    get_parameter<double>("curve_resample_step_size",   config_.curve_resample_step_size);
    get_parameter<double>("centerline_sampling_spacing",   config_.centerline_sampling_spacing);
    get_parameter<int>("curvature_moving_average_window_size",   config_.curvature_moving_average_window_size);
    get_parameter<double>("lateral_accel_limit",   config_.lateral_accel_limit);
    get_parameter<int>("speed_moving_average_window_size",   config_.speed_moving_average_window_size);
    get_parameter<double>("back_distance",   config_.back_distance);
 
    // Register runtime parameter update callback
    add_on_set_parameters_callback(std::bind(&StopControlledIntersectionTacticalPlugin::parameter_update_callback, this, std_ph::_1));
 
    RCLCPP_INFO_STREAM(rclcpp::get_logger("stop_controlled_intersection_tactical_plugin"),"Done loading parameters: " << config_);
 
    // set world model pointer
    wm_ = get_world_model();
 
    // Return success if everything initialized successfully
    return CallbackReturn::SUCCESS;
}

References stop_controlled_intersection_tactical_plugin::StopControlledIntersectionTacticalPluginConfig::back_distance, stop_controlled_intersection_tactical_plugin::StopControlledIntersectionTacticalPluginConfig::centerline_sampling_spacing, config_, stop_controlled_intersection_tactical_plugin::StopControlledIntersectionTacticalPluginConfig::curvature_moving_average_window_size, stop_controlled_intersection_tactical_plugin::StopControlledIntersectionTacticalPluginConfig::curve_resample_step_size, basic_autonomy::get_logger(), carma_guidance_plugins::PluginBaseNode::get_world_model(), stop_controlled_intersection_tactical_plugin::StopControlledIntersectionTacticalPluginConfig::lateral_accel_limit, parameter_update_callback(), stop_controlled_intersection_tactical_plugin::StopControlledIntersectionTacticalPluginConfig::speed_moving_average_window_size, stop_controlled_intersection_tactical_plugin::StopControlledIntersectionTacticalPluginConfig::trajectory_time_length, and wm_.

Here is the call graph for this function:

parameter_update_callback()

rcl_interfaces::msg::SetParametersResult stop_controlled_intersection_tactical_plugin::StopControlledIntersectionTacticalPlugin::parameter_update_callback

(

const std::vector< rclcpp::Parameter > &

parameters

)

Definition at line 65 of file stop_controlled_intersection_tactical_plugin.cpp.

{
  auto error_double = update_params<double>({
    {"trajectory_time_length", config_.trajectory_time_length},
    {"curve_resample_step_size", config_.curve_resample_step_size},
    {"centerline_sampling_spacing", config_.centerline_sampling_spacing},
    {"lateral_accel_limit", config_.lateral_accel_limit},
    {"back_distance", config_.back_distance}
  }, parameters); 
 
    auto error_int = update_params<int>({
    {"curvature_moving_average_window_size", config_.curvature_moving_average_window_size},
    {"speed_moving_average_window_size", config_.speed_moving_average_window_size}
  }, parameters); 
 
  rcl_interfaces::msg::SetParametersResult result;
 
  result.successful = !error_double && !error_int;
 
  return result;
}

References stop_controlled_intersection_tactical_plugin::StopControlledIntersectionTacticalPluginConfig::back_distance, stop_controlled_intersection_tactical_plugin::StopControlledIntersectionTacticalPluginConfig::centerline_sampling_spacing, config_, stop_controlled_intersection_tactical_plugin::StopControlledIntersectionTacticalPluginConfig::curvature_moving_average_window_size, stop_controlled_intersection_tactical_plugin::StopControlledIntersectionTacticalPluginConfig::curve_resample_step_size, stop_controlled_intersection_tactical_plugin::StopControlledIntersectionTacticalPluginConfig::lateral_accel_limit, stop_controlled_intersection_tactical_plugin::StopControlledIntersectionTacticalPluginConfig::speed_moving_average_window_size, and stop_controlled_intersection_tactical_plugin::StopControlledIntersectionTacticalPluginConfig::trajectory_time_length.

Referenced by on_configure_plugin().

Here is the caller graph for this function:

plan_trajectory_callback()

void stop_controlled_intersection_tactical_plugin::StopControlledIntersectionTacticalPlugin::plan_trajectory_callback ( std::shared_ptr< rmw_request_id_t >  srv_header, carma_planning_msgs::srv::PlanTrajectory::Request::SharedPtr  req, carma_planning_msgs::srv::PlanTrajectory::Response::SharedPtr  resp  )

overridevirtual

Extending class provided callback which should return a planned trajectory based on the provided trajectory planning request.

Parameters

srv_header	RCL header for services calls. Can usually be ignored by implementers.
req	The service request containing the maneuvers to plan trajectories for and current vehicle state
resp	The response containing the planned trajectory

Implements carma_guidance_plugins::TacticalPlugin.

Definition at line 112 of file stop_controlled_intersection_tactical_plugin.cpp.

{
    RCLCPP_DEBUG_STREAM(rclcpp::get_logger("stop_controlled_intersection_tactical_plugin"), "Starting stop controlled intersection trajectory planning");
    
    if(req->maneuver_index_to_plan >= req->maneuver_plan.maneuvers.size())
    {
    throw std::invalid_argument(
        "Stop 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;
    for(size_t i = req->maneuver_index_to_plan; i < req->maneuver_plan.maneuvers.size(); i++){
        
        if((req->maneuver_plan.maneuvers[i].type == carma_planning_msgs::msg::Maneuver::LANE_FOLLOWING || req->maneuver_plan.maneuvers[i].type == carma_planning_msgs::msg::Maneuver::INTERSECTION_TRANSIT_STRAIGHT
        || req->maneuver_plan.maneuvers[i].type == carma_planning_msgs::msg::Maneuver::INTERSECTION_TRANSIT_LEFT_TURN || req->maneuver_plan.maneuvers[i].type ==carma_planning_msgs::msg::Maneuver::INTERSECTION_TRANSIT_RIGHT_TURN) 
        && GET_MANEUVER_PROPERTY(req->maneuver_plan.maneuvers[i], parameters.string_valued_meta_data.front()) == stop_controlled_intersection_strategy_)
        {
            maneuver_plan.push_back(req->maneuver_plan.maneuvers[i]);
            resp->related_maneuvers.push_back(req->maneuver_plan.maneuvers[i].type);
        }
        else
        {
            break;
        }
    }
 
    lanelet::BasicPoint2d veh_pos(req->vehicle_state.x_pos_global, req->vehicle_state.y_pos_global);
    RCLCPP_DEBUG_STREAM(rclcpp::get_logger("stop_controlled_intersection_tactical_plugin"), "Planning state x:"<<req->vehicle_state.x_pos_global <<" , y: " << req->vehicle_state.y_pos_global);
 
    double current_downtrack = wm_->routeTrackPos(veh_pos).downtrack;
    RCLCPP_DEBUG_STREAM(rclcpp::get_logger("stop_controlled_intersection_tactical_plugin"), "Current_downtrack"<< current_downtrack);
 
    std::vector<PointSpeedPair> points_and_target_speeds = maneuvers_to_points( maneuver_plan, wm_, req->vehicle_state);
    RCLCPP_DEBUG_STREAM(rclcpp::get_logger("stop_controlled_intersection_tactical_plugin"), "Maneuver to points size:"<< points_and_target_speeds.size());
    // RCLCPP_DEBUG_STREAM(rclcpp::get_logger("stop_controlled_intersection_tactical_plugin"), "Printing points: ");
    // TODO: add print logic
    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()());
 
    //Add compose trajectory from centerline
    trajectory.trajectory_points = compose_trajectory_from_centerline(points_and_target_speeds, req->vehicle_state, req->header.stamp);
    trajectory.initial_longitudinal_velocity = req->vehicle_state.longitudinal_vel;
 
    // Set the planning plugin field name
    for (auto& p : trajectory.trajectory_points) {
        p.planner_plugin_name = get_plugin_name();
        // p.controller_plugin_name = "PurePursuit";
    }
 
    resp->trajectory_plan = trajectory;
    
    resp->maneuver_status.push_back(carma_planning_msgs::srv::PlanTrajectory::Response::MANEUVER_IN_PROGRESS);
}

References compose_trajectory_from_centerline(), basic_autonomy::get_logger(), GET_MANEUVER_PROPERTY, carma_guidance_plugins::PluginBaseNode::get_plugin_name(), process_bag::i, maneuvers_to_points(), stop_controlled_intersection_strategy_, carma_cooperative_perception::to_string(), and wm_.

Here is the call graph for this function:

Member Data Documentation

config_

StopControlledIntersectionTacticalPluginConfig stop_controlled_intersection_tactical_plugin::StopControlledIntersectionTacticalPlugin::config_

private

Definition at line 158 of file stop_controlled_intersection_plugin.hpp.

Referenced by StopControlledIntersectionTacticalPlugin(), compose_trajectory_from_centerline(), maneuvers_to_points(), on_configure_plugin(), and parameter_update_callback().

epsilon_

double stop_controlled_intersection_tactical_plugin::StopControlledIntersectionTacticalPlugin::epsilon_ = 0.001

private

Definition at line 162 of file stop_controlled_intersection_plugin.hpp.

Referenced by compose_trajectory_from_centerline(), create_case_one_speed_profile(), create_case_three_speed_profile(), and create_case_two_speed_profile().

stop_controlled_intersection_strategy_

std::string stop_controlled_intersection_tactical_plugin::StopControlledIntersectionTacticalPlugin::stop_controlled_intersection_strategy_ = "Carma/stop_controlled_intersection"

private

Definition at line 160 of file stop_controlled_intersection_plugin.hpp.

Referenced by plan_trajectory_callback().

wm_

carma_wm::WorldModelConstPtr stop_controlled_intersection_tactical_plugin::StopControlledIntersectionTacticalPlugin::wm_

private

Definition at line 157 of file stop_controlled_intersection_plugin.hpp.

Referenced by compose_trajectory_from_centerline(), maneuvers_to_points(), on_configure_plugin(), and plan_trajectory_callback().

---

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

• stop_controlled_intersection_tactical_plugin/include/stop_controlled_intersection_plugin.hpp
• stop_controlled_intersection_tactical_plugin/src/stop_controlled_intersection_tactical_plugin.cpp