yield_plugin::YieldPlugin Class Reference

Class containing primary business logic for the In-Lane Cruising Plugin. More...

#include <yield_plugin.hpp>

Collaboration diagram for yield_plugin::YieldPlugin:

Public Member Functions
	YieldPlugin (std::shared_ptr< carma_ros2_utils::CarmaLifecycleNode > nh, carma_wm::WorldModelConstPtr wm, YieldPluginConfig config, MobilityResponseCB mobility_response_publisher, LaneChangeStatusCB lc_status_publisher)
	Constructor. More...
void	plan_trajectory_callback (carma_planning_msgs::srv::PlanTrajectory::Request::SharedPtr req, carma_planning_msgs::srv::PlanTrajectory::Response::SharedPtr resp)
	Service callback for trajectory planning. More...
carma_planning_msgs::msg::TrajectoryPlan	update_traj_for_object (const carma_planning_msgs::msg::TrajectoryPlan &original_tp, const std::vector< carma_perception_msgs::msg::ExternalObject > &external_objects, double initial_velocity)
	trajectory is modified to safely avoid obstacles on the road More...
double	polynomial_calc (std::vector< double > coeff, double x) const
	calculate quintic polynomial equation for a given x More...
double	polynomial_calc_d (std::vector< double > coeff, double x) const
	calculate derivative of quintic polynomial equation for a given x More...
double	max_trajectory_speed (const std::vector< carma_planning_msgs::msg::TrajectoryPlanPoint > &trajectory_points, double timestamp_in_sec_to_search_until) const
	calculates the maximum speed in a set of tajectory points More...
std::vector< double >	get_relative_downtracks (const carma_planning_msgs::msg::TrajectoryPlan &trajectory_plan) const
	calculates distance between trajectory points in a plan More...
void	mobilityrequest_cb (const carma_v2x_msgs::msg::MobilityRequest::UniquePtr msg)
	callback for mobility request More...
void	bsm_cb (const carma_v2x_msgs::msg::BSM::UniquePtr msg)
	callback for bsm message More...
std::vector< lanelet::BasicPoint2d >	convert_eceftrajectory_to_mappoints (const carma_v2x_msgs::msg::Trajectory &ecef_trajectory) const
	convert a carma trajectory from ecef frame to map frame ecef trajectory consists of the point and a set of offsets with reference to the point More...
lanelet::BasicPoint2d	ecef_to_map_point (const carma_v2x_msgs::msg::LocationECEF &ecef_point) const
	convert a point in ecef frame (in cm) into map frame (in meters) More...
carma_v2x_msgs::msg::MobilityResponse	compose_mobility_response (const std::string &resp_recipient_id, const std::string &req_plan_id, bool response) const
	compose a mobility response message More...
carma_planning_msgs::msg::TrajectoryPlan	generate_JMT_trajectory (const carma_planning_msgs::msg::TrajectoryPlan &original_tp, double initial_pos, double goal_pos, double initial_velocity, double goal_velocity, double planning_time, double original_max_speed)
	generate a Jerk Minimizing Trajectory(JMT) with the provided start and end conditions More...
carma_planning_msgs::msg::TrajectoryPlan	update_traj_for_cooperative_behavior (const carma_planning_msgs::msg::TrajectoryPlan &original_tp, double current_speed)
	update trajectory for yielding to an incoming cooperative behavior More...
std::vector< std::pair< int, lanelet::BasicPoint2d > >	detect_trajectories_intersection (std::vector< lanelet::BasicPoint2d > self_trajectory, std::vector< lanelet::BasicPoint2d > incoming_trajectory) const
	detect intersection point(s) of two trajectories More...
void	set_incoming_request_info (std::vector< lanelet::BasicPoint2d > req_trajectory, double req_speed, double req_planning_time, double req_timestamp)
	set values for member variables related to cooperative behavior More...
void	lookup_ecef_to_map_transform ()
	Looks up the transform between map and earth frames, and sets the member variable. More...
double	check_traj_for_digital_min_gap (const carma_planning_msgs::msg::TrajectoryPlan &original_tp) const
	checks trajectory for minimum gap associated with it from the road More...
void	set_georeference_string (const std::string &georeference)
	Setter for map projection string to define lat/lon -> map conversion. More...
void	set_external_objects (const std::vector< carma_perception_msgs::msg::ExternalObject > &object_list)
	Setter for external objects with predictions in the environment. More...
void	update_route_llt_cache ()
	Rebuild the cached route lanelet polygons and IDs from the current route. Should be called once whenever the route changes via setRouteCallback(). More...
std::optional< GetCollisionResult >	get_collision (const carma_planning_msgs::msg::TrajectoryPlan &ego_trajectory, const std::vector< carma_perception_msgs::msg::PredictedState > &object_predictions, double collision_radius, double ego_max_speed)
	Return naive collision time and locations based on collision radius given two trajectories with one being obstacle's predicted steps. More...
std::optional< rclcpp::Time >	get_collision_time (const carma_planning_msgs::msg::TrajectoryPlan &original_tp, const carma_perception_msgs::msg::ExternalObject &curr_obstacle, double original_tp_max_speed)
	Return collision time given two trajectories with one being external object with predicted steps. More...
bool	is_object_behind_vehicle (uint32_t object_id, const rclcpp::Time &collision_time, double vehicle_point, double object_downtrack)
	Check if object location is behind the vehicle using estimates of the vehicle's length and route downtracks. More...
std::optional< std::pair< carma_perception_msgs::msg::ExternalObject, double > >	get_earliest_collision_object_and_time (const carma_planning_msgs::msg::TrajectoryPlan &original_tp, const std::vector< carma_perception_msgs::msg::ExternalObject > &external_objects)
	Return the earliest collision object and time of collision pair from the given trajectory and list of external objects with predicted states. Function first filters obstacles based on whether if their any of predicted state will be on the route. Only then, the logic compares trajectory and predicted states. More...
std::unordered_map< uint32_t, rclcpp::Time >	get_collision_times_concurrently (const carma_planning_msgs::msg::TrajectoryPlan &original_tp, const std::vector< carma_perception_msgs::msg::ExternalObject > &external_objects, double original_tp_max_speed)
	Given the list of objects with predicted states, get all collision times concurrently using multi-threading. More...
double	get_predicted_velocity_at_time (const geometry_msgs::msg::Twist &object_velocity_in_map_frame, const carma_planning_msgs::msg::TrajectoryPlan &original_tp, double timestamp_in_sec_to_predict)
	Given the object velocity in map frame with x,y components, this function returns the projected velocity along the trajectory at given time. More...

Private Member Functions
std::unordered_map< uint32_t, rclcpp::Time >	get_collision_times_concurrently_cpu (const carma_planning_msgs::msg::TrajectoryPlan &original_tp, const std::vector< carma_perception_msgs::msg::ExternalObject > &external_objects, double original_tp_max_speed)
	CPU implementation of get_collision_times_concurrently. Launches one thread per external object and computes get_collision_time for each in parallel. More...
std::unordered_map< uint32_t, rclcpp::Time >	get_collision_times_concurrently_cuda (const carma_planning_msgs::msg::TrajectoryPlan &original_tp, const std::vector< carma_perception_msgs::msg::ExternalObject > &external_objects, double original_tp_max_speed)
	CUDA implementation of get_collision_times_concurrently. Filters objects to those with on-route predictions, then batches the collision check across all objects on the GPU. Falls back to get_collision_times_concurrently_cpu if no CUDA device is available or on error. More...
std::pair< bool, int >	find_on_route_in_predictions (const std::vector< carma_perception_msgs::msg::PredictedState > &predictions, int stride, bool zero_speed) const
	Check whether any predicted state of an object falls within the route lanelet polygons. More...
std::string	bsmIDtoString (carma_v2x_msgs::msg::BSMCoreData bsm_core)

Private Attributes
carma_wm::WorldModelConstPtr	wm_
YieldPluginConfig	config_
MobilityResponseCB	mobility_response_publisher_
LaneChangeStatusCB	lc_status_publisher_
std::shared_ptr< carma_ros2_utils::CarmaLifecycleNode >	nh_
std::set< lanelet::Id >	route_llt_ids_
std::vector< lanelet::ConstLanelet >	route_llt_polygons_
lanelet::Id	previous_llt_id_
std::vector< carma_perception_msgs::msg::ExternalObject >	external_objects_
std::unordered_map< uint32_t, int >	consecutive_clearance_count_for_obstacles_
bool	cooperative_request_acceptable_ = false
std::vector< lanelet::BasicPoint2d >	req_trajectory_points_
double	req_target_speed_ = 0
double	req_timestamp_ = 0
double	req_target_plan_time_ = 0
int	timesteps_since_last_req_ = 0
int	clc_urgency_ = 0
std::optional< double >	last_speed_ = std::nullopt
std::optional< rclcpp::Time >	last_speed_time_ = std::nullopt
std::optional< rclcpp::Time >	first_time_stopped_to_prevent_collision_ = std::nullopt
std::optional< carma_planning_msgs::msg::TrajectoryPlan >	last_traj_plan_committed_to_stopping_ = std::nullopt
double	ecef_traj_timestep_ = 0.1
geometry_msgs::msg::Vector3	host_vehicle_size
double	current_speed_
std::string	host_bsm_id_
std::string	georeference_ {""}
std::shared_ptr< lanelet::projection::LocalFrameProjector >	map_projector_

Detailed Description

Class containing primary business logic for the In-Lane Cruising Plugin.

Definition at line 77 of file yield_plugin.hpp.

Constructor & Destructor Documentation

YieldPlugin()

yield_plugin::YieldPlugin::YieldPlugin	(	std::shared_ptr< carma_ros2_utils::CarmaLifecycleNode >	nh,
		carma_wm::WorldModelConstPtr	wm,
		YieldPluginConfig	config,
		MobilityResponseCB	mobility_response_publisher,
		LaneChangeStatusCB	lc_status_publisher
	)

Constructor.

Parameters

wm	Pointer to intialized instance of the carma world model for accessing semantic map data
config	The configuration to be used for this object
mobility_response_publisher	Callback which will publish the mobility response
lc_status_publisher	Callback which will publish the cooperative lane change status

Definition at line 49 of file yield_plugin.cpp.

    : nh_(nh), wm_(wm), config_(config),mobility_response_publisher_(mobility_response_publisher), lc_status_publisher_(lc_status_publisher)
  {
 
  }

Member Function Documentation

bsm_cb()

void yield_plugin::YieldPlugin::bsm_cb

(

const carma_v2x_msgs::msg::BSM::UniquePtr

msg

)

callback for bsm message

Parameters

msg	mobility bsm message

Definition at line 250 of file yield_plugin.cpp.

  {
    carma_v2x_msgs::msg::BSMCoreData bsm_core_ = msg->core_data;
    host_bsm_id_ = bsmIDtoString(bsm_core_);
  }

References bsmIDtoString(), and host_bsm_id_.

Referenced by yield_plugin::YieldPluginNode::on_configure_plugin().

Here is the call graph for this function:

Here is the caller graph for this function:

bsmIDtoString()

std::string yield_plugin::YieldPlugin::bsmIDtoString ( carma_v2x_msgs::msg::BSMCoreData  bsm_core )

inlineprivate

Definition at line 394 of file yield_plugin.hpp.

  {
    std::string res = "";
    for (size_t i=0; i<bsm_core.id.size(); i++)
    {
      res+=std::to_string(bsm_core.id[i]);
    }
      return res;
  }

References process_bag::i, and carma_cooperative_perception::to_string().

Referenced by bsm_cb().

Here is the call graph for this function:

Here is the caller graph for this function:

check_traj_for_digital_min_gap()

double yield_plugin::YieldPlugin::check_traj_for_digital_min_gap

(

const carma_planning_msgs::msg::TrajectoryPlan &

original_tp

)

const

checks trajectory for minimum gap associated with it from the road

Parameters

original_tp

original trajectory plan

Returns

minumum required min_gap from the road, if none exists, return default minimum_safety_gap_in_meters

Definition at line 1441 of file yield_plugin.cpp.

  {
    double desired_gap = 0;
    if (!wm_->getRoute()) return desired_gap;
 
    const lanelet::BasicPoint2d traj_start(original_tp.trajectory_points.front().x,
                                           original_tp.trajectory_points.front().y);
    const lanelet::BasicPoint2d traj_end(original_tp.trajectory_points.back().x,
                                         original_tp.trajectory_points.back().y);
 
    // Fetch up to 4 candidates per endpoint — a boundary point can lie on
    // overlapping lanelets and a single result may be the wrong one.
    auto start_llts = wm_->getLaneletsFromPoint(traj_start, 4);
    auto end_llts   = wm_->getLaneletsFromPoint(traj_end,   4);
 
    if (start_llts.empty() || end_llts.empty())
    {
      // Trajectory generation may place a point off-road in rare edge cases
      // (see https://github.com/usdot-fhwa-stol/carma-platform/issues/2503)
      RCLCPP_WARN_STREAM(nh_->get_logger(), "check_traj_for_digital_min_gap: trajectory endpoint "
        "not on a lanelet, skipping digital gap check.");
      return desired_gap;
    }
 
    std::unordered_set<lanelet::Id> start_ids;
    std::unordered_set<lanelet::Id> end_ids;
    for (const auto& llt : start_llts) start_ids.insert(llt.id());
    for (const auto& llt : end_llts)   end_ids.insert(llt.id());
 
    // One pass: first index matching any start candidate, last index matching any end candidate.
    // Taking "last" for end covers all overlapping lanelets at the trajectory's back boundary.
    std::optional<size_t> start_pos;
    std::optional<size_t> end_pos;
    size_t pos = 0;
    const auto& path = wm_->getRoute()->shortestPath();
    for (const auto& llt : path)
    {
      if (!start_pos.has_value() && start_ids.count(llt.id())) start_pos = pos;
      if (end_ids.count(llt.id()))                 end_pos   = pos;
      ++pos;
    }
 
    if (!start_pos || !end_pos || *start_pos > *end_pos)
    {
      RCLCPP_WARN_STREAM(nh_->get_logger(), "check_traj_for_digital_min_gap: trajectory endpoints "
        "not found on route shortest path, skipping digital gap check.");
      return desired_gap;
    }
 
    pos = 0;
    for (const auto& llt : path)
    {
      if (pos > *end_pos) break;
      if (pos >= *start_pos)
      {
        auto digital_min_gap = llt.regulatoryElementsAs<lanelet::DigitalMinimumGap>();
        if (!digital_min_gap.empty())
        {
          double digital_gap = digital_min_gap[0]->getMinimumGap();
          RCLCPP_DEBUG_STREAM(nh_->get_logger(), "Digital Gap found with value: " << digital_gap);
          desired_gap = std::max(desired_gap, digital_gap);
        }
      }
      ++pos;
    }
    return desired_gap;
  }

References nh_, and wm_.

Referenced by update_traj_for_cooperative_behavior(), and update_traj_for_object().

Here is the caller graph for this function:

compose_mobility_response()

carma_v2x_msgs::msg::MobilityResponse yield_plugin::YieldPlugin::compose_mobility_response	(	const std::string &	resp_recipient_id,
		const std::string &	req_plan_id,
		bool	response
	)		const

compose a mobility response message

Parameters

resp_recipient_id	vehicle id of the recipient of the message
req_plan_id	plan id from the requested message
response	accept/reject to the response based on conditions

Returns

filled mobility response

Definition at line 137 of file yield_plugin.cpp.

  {
    carma_v2x_msgs::msg::MobilityResponse out_mobility_response;
    out_mobility_response.m_header.sender_id = config_.vehicle_id;
    out_mobility_response.m_header.recipient_id = resp_recipient_id;
    out_mobility_response.m_header.sender_bsm_id = host_bsm_id_;
    out_mobility_response.m_header.plan_id = req_plan_id;
    out_mobility_response.m_header.timestamp = nh_->now().seconds()*1000;
 
 
    if (config_.always_accept_mobility_request && response)
    {
      out_mobility_response.is_accepted = true;
    }
    else out_mobility_response.is_accepted = false;
 
    return out_mobility_response;
  }

References YieldPluginConfig::always_accept_mobility_request, config_, host_bsm_id_, nh_, and YieldPluginConfig::vehicle_id.

Referenced by mobilityrequest_cb().

Here is the caller graph for this function:

convert_eceftrajectory_to_mappoints()

std::vector< lanelet::BasicPoint2d > yield_plugin::YieldPlugin::convert_eceftrajectory_to_mappoints

(

const carma_v2x_msgs::msg::Trajectory &

ecef_trajectory

)

const

convert a carma trajectory from ecef frame to map frame ecef trajectory consists of the point and a set of offsets with reference to the point

Parameters

ecef_trajectory

carma trajectory (ecef frame)

Returns

vector of 2d points in map frame

Definition at line 98 of file yield_plugin.cpp.

  {
    carma_planning_msgs::msg::TrajectoryPlan trajectory_plan;
    std::vector<lanelet::BasicPoint2d> map_points;
 
    lanelet::BasicPoint2d first_point = ecef_to_map_point(ecef_trajectory.location);
 
    map_points.push_back(first_point);
    auto curr_point = ecef_trajectory.location;
 
    for (size_t i = 0; i<ecef_trajectory.offsets.size(); i++)
    {
      lanelet::BasicPoint2d offset_point;
      curr_point.ecef_x += ecef_trajectory.offsets.at(i).offset_x;
      curr_point.ecef_y += ecef_trajectory.offsets.at(i).offset_y;
      curr_point.ecef_z += ecef_trajectory.offsets.at(i).offset_z;
 
      offset_point = ecef_to_map_point(curr_point);
 
      map_points.push_back(offset_point);
    }
 
    return map_points;
  }

References ecef_to_map_point(), process_bag::first_point, and process_bag::i.

Referenced by mobilityrequest_cb().

Here is the call graph for this function:

Here is the caller graph for this function:

detect_trajectories_intersection()

std::vector< std::pair< int, lanelet::BasicPoint2d > > yield_plugin::YieldPlugin::detect_trajectories_intersection	(	std::vector< lanelet::BasicPoint2d >	self_trajectory,
		std::vector< lanelet::BasicPoint2d >	incoming_trajectory
	)		const

detect intersection point(s) of two trajectories

Parameters

self_trajectory	vector of 2d trajectory points
incoming_trajectory	vector of 2d trajectory points

Returns

vector of pairs of 2d intersection points and index of the point in trajectory array

Definition at line 77 of file yield_plugin.cpp.

  {
    std::vector<std::pair<int, lanelet::BasicPoint2d>> intersection_points;
    boost::geometry::model::linestring<lanelet::BasicPoint2d> self_traj;
    for (auto tpp:self_trajectory)
    {
      boost::geometry::append(self_traj, tpp);
    }
    // distance to consider trajectories colliding (chosen based on lane width and vehicle size)
    for (size_t i=0; i<incoming_trajectory.size(); i++)
    {
      double res = boost::geometry::distance(incoming_trajectory.at(i), self_traj);
 
      if (fabs(res) <= config_.intervehicle_collision_distance_in_m)
      {
         intersection_points.push_back(std::make_pair(i, incoming_trajectory.at(i)));
      }
    }
    return intersection_points;
  }

References config_, process_bag::i, and YieldPluginConfig::intervehicle_collision_distance_in_m.

Referenced by update_traj_for_cooperative_behavior().

Here is the caller graph for this function:

ecef_to_map_point()

lanelet::BasicPoint2d yield_plugin::YieldPlugin::ecef_to_map_point

(

const carma_v2x_msgs::msg::LocationECEF &

ecef_point

)

const

convert a point in ecef frame (in cm) into map frame (in meters)

Parameters

ecef_point	carma point ecef frame in cm
map_in_earth	translate frame

Returns

2d point in map frame

Definition at line 123 of file yield_plugin.cpp.

  {
 
    if (!map_projector_) {
        throw std::invalid_argument("No map projector available for ecef conversion");
    }
 
    lanelet::BasicPoint3d map_point = map_projector_->projectECEF( { static_cast<double>(ecef_point.ecef_x)/100.0, static_cast<double>(ecef_point.ecef_y)/100.0, static_cast<double>(ecef_point.ecef_z)/100.0 } , 1);
 
    return lanelet::traits::to2D(map_point);
  }

References map_projector_.

Referenced by convert_eceftrajectory_to_mappoints().

Here is the caller graph for this function:

find_on_route_in_predictions()

std::pair< bool, int > yield_plugin::YieldPlugin::find_on_route_in_predictions ( const std::vector< carma_perception_msgs::msg::PredictedState > &  predictions, int  stride, bool  zero_speed  ) const

private

Check whether any predicted state of an object falls within the route lanelet polygons.

Parameters

predictions	list of predicted states for an object
stride	step size used to iterate over predictions
zero_speed	if true, only the first prediction is checked (object is stationary)

Returns

pair of (true, index of first on-route prediction) if found, otherwise (false, 0)

Definition at line 910 of file yield_plugin.cpp.

  {
    for (size_t j = 0; j < predictions.size(); j += stride) {
      const lanelet::BasicPoint2d point(predictions[j].predicted_position.position.x,
                                        predictions[j].predicted_position.position.y);
      for (const auto& llt : route_llt_polygons_) {
        if (boost::geometry::within(point, llt.polygon2d())) {
          return {true, static_cast<int>(j)};
        }
      }
      if (object_has_zero_speed) break;
    }
    return {false, 0};
  }

References process_traj_logs::point, and route_llt_polygons_.

Referenced by get_collision_times_concurrently_cuda().

Here is the caller graph for this function:

generate_JMT_trajectory()

carma_planning_msgs::msg::TrajectoryPlan yield_plugin::YieldPlugin::generate_JMT_trajectory	(	const carma_planning_msgs::msg::TrajectoryPlan &	original_tp,
		double	initial_pos,
		double	goal_pos,
		double	initial_velocity,
		double	goal_velocity,
		double	planning_time,
		double	original_max_speed
	)

generate a Jerk Minimizing Trajectory(JMT) with the provided start and end conditions

Parameters

original_tp	original trajectory plan
intial_pos	start position
goal_pos	final position
initial_velocity	start velocity
goal_velocity	end velocity
planning_time	time duration of the planning
original	original_max_speed from original_tp up until the goal_pos portion NOTE: the function would generate trajectory duration arbitrarily high if stopping motion is needed. This is to keep the original trajectory's shape in terms of location so that the vehicle steers toward the direction of travel even when stopping.

Returns

updated JMT trajectory

Definition at line 469 of file yield_plugin.cpp.

  {
    carma_planning_msgs::msg::TrajectoryPlan jmt_trajectory;
    std::vector<carma_planning_msgs::msg::TrajectoryPlanPoint> jmt_trajectory_points;
    jmt_trajectory_points.push_back(original_tp.trajectory_points[0]);
 
    std::vector<double> original_traj_relative_downtracks = get_relative_downtracks(original_tp);
    std::vector<double> calculated_speeds = {};
    std::vector<double> new_relative_downtracks = {};
    new_relative_downtracks.push_back(0.0);
    calculated_speeds.push_back(initial_velocity);
    double new_traj_accumulated_downtrack = 0.0;
    double original_traj_accumulated_downtrack = original_traj_relative_downtracks.at(1);
 
    // Up until goal_pos (which also can be until end of the entire original trajectory), generate new speeds at
    // or near original trajectory points by generating them at a fixed time interval using the JMT polynomial equation
    const double initial_time = 0;
    double initial_accel = 0;
    if (last_speed_ && last_speed_time_)
    {
      initial_accel = (initial_velocity - last_speed_.value()) /
        (nh_->now() - last_speed_time_.value()).seconds();
 
      if (!std::isnormal(initial_accel))
      {
        RCLCPP_DEBUG_STREAM(rclcpp::get_logger("yield_plugin"),"Detecting nan initial_accel set to 0");
        initial_accel = 0.0;
      }
 
      RCLCPP_DEBUG_STREAM(rclcpp::get_logger("yield_plugin"),"Detecting initial_accel: " << initial_accel
        << ", initial_velocity:" << initial_velocity
        << ", last_speed_: " << last_speed_.value()
        << ", nh_->now(): " << nh_->now().seconds()
        << ", last_speed_time_.get(): " << last_speed_time_.value().seconds());
    }
 
    const double goal_accel = 0;
    RCLCPP_DEBUG_STREAM(rclcpp::get_logger("yield_plugin"),"Following parameters used for JMT: "
      "\ninitial_pos: " << initial_pos <<
      "\ngoal_pos: " << goal_pos <<
      "\ninitial_velocity: " << initial_velocity <<
      "\ngoal_velocity: " << goal_velocity <<
      "\ninitial_accel: " << initial_accel <<
      "\ngoal_accel: " << goal_accel <<
      "\nplanning_time: " << planning_time <<
      "\noriginal_max_speed: " << original_max_speed);
 
    // Get the polynomial solutions used to generate the trajectory
    std::vector<double> polynomial_coefficients = quintic_coefficient_calculator::quintic_coefficient_calculator(initial_pos,
                                                                                                goal_pos,
                                                                                                initial_velocity,
                                                                                                goal_velocity,
                                                                                                initial_accel,
                                                                                                goal_accel,
                                                                                                initial_time,
                                                                                                planning_time);
    RCLCPP_DEBUG_STREAM(rclcpp::get_logger("yield_plugin"),"Used original_max_speed: " << original_max_speed);
    for (size_t i = 0; i < polynomial_coefficients.size(); i++) {
      RCLCPP_DEBUG_STREAM(rclcpp::get_logger("yield_plugin"),"Coefficient " << i << ": " << polynomial_coefficients[i]);
    }
    // Cap at 0.1 s: finer steps gave no accuracy benefit but caused O(1/dt) loop blow-up
    // when upstream planners produced sub-10 ms trajectory timesteps.
    const double smallest_time_step = std::max(get_smallest_time_step_of_traj(original_tp), 0.1);
    int new_traj_idx = 1;
    int original_traj_idx = 1;
    while (new_traj_accumulated_downtrack < goal_pos - EPSILON && original_traj_idx < original_traj_relative_downtracks.size())
    {
      const double target_time = new_traj_idx * smallest_time_step;
      const double downtrack_at_target_time = polynomial_calc(polynomial_coefficients, target_time);
      double velocity_at_target_time = polynomial_calc_d(polynomial_coefficients, target_time);
 
      // if the speed becomes negative, the downtrack starts reversing to negative as well
      // which will never reach the goal_pos, so break here.
      if (velocity_at_target_time < 0.0)
      {
        break;
      }
 
      // Cannot have a negative speed or have a higher speed than that of the original trajectory
      velocity_at_target_time = std::clamp(velocity_at_target_time, 0.0, original_max_speed);
 
      // Pick the speed if it matches with the original downtracks
      if (downtrack_at_target_time >= original_traj_accumulated_downtrack)
      {
        // velocity_at_target_time doesn't exactly correspond to original_traj_accumulated_downtrack but does for new_traj_accumulated_downtrack.
        // however, the logic is assuming they are close enough that the speed is usable
        calculated_speeds.push_back(velocity_at_target_time);
        original_traj_accumulated_downtrack += original_traj_relative_downtracks.at(original_traj_idx);
        original_traj_idx ++;
      }
      new_traj_accumulated_downtrack = downtrack_at_target_time;
      new_traj_idx++;
 
    }
 
    // if the loop above finished prematurely due to negative speed, fill with 0.0 speeds
    // since the speed crossed 0.0 and algorithm indicates stopping
    std::fill_n(std::back_inserter(calculated_speeds),
                std::size(original_traj_relative_downtracks) - std::size(calculated_speeds),
                0.0);
 
    // Moving average filter to smoothen the speeds
    std::vector<double> filtered_speeds = basic_autonomy::smoothing::moving_average_filter(calculated_speeds, config_.speed_moving_average_window_size);
    // Replace the original trajectory's associated timestamps based on the newly calculated speeds
    double prev_speed = filtered_speeds.at(0);
    last_speed_ = prev_speed;
    last_speed_time_ = nh_->now();
 
    for(size_t i = 1; i < original_tp.trajectory_points.size(); i++)
    {
      carma_planning_msgs::msg::TrajectoryPlanPoint jmt_tpp = original_tp.trajectory_points.at(i);
 
      // In case only subset of original trajectory needs modification,
      // the rest of the points should keep the last speed to cruise
      double current_speed = goal_velocity;
 
      if (i < filtered_speeds.size())
      {
        current_speed = filtered_speeds.at(i);
      }
 
      //Force the speed to 0 if below configured value for more control over stopping behavior
      if (current_speed < config_.max_stop_speed_in_ms)
      {
        current_speed = 0;
      }
 
      // Derived from constant accelaration kinematic equation: (vi + vf) / 2 * dt = d_dist
      // This also handles a case correctly when current_speed is 0, but prev_speed is not 0 yet
      const double dt = (2 * original_traj_relative_downtracks.at(i)) / (current_speed + prev_speed);
      jmt_tpp.target_time =  rclcpp::Time(jmt_trajectory_points.back().target_time) + rclcpp::Duration::from_nanoseconds(dt*1e9);
 
      if (prev_speed < EPSILON) // Handle a special case if prev_speed (thus current_speed too) is 0
      {
        // NOTE: Assigning arbitrary 100 mins dt between points where normally dt is only 1 sec to model a stopping behavior.
        // Another way to model it is to keep the trajectory point at a same location and increment time slightly. However,
        // if the vehicle goes past the point, it may cruise toward undesirable location (for example into the intersection).
        // Keeping the points help the controller steer the vehicle toward direction of travel even when stopping.
        // Only downside is the trajectory plan is huge where only 15 sec is expected, but since this is stopping case, it shouldn't matter.
        jmt_tpp.target_time = rclcpp::Time(jmt_trajectory_points.back().target_time) + rclcpp::Duration::from_nanoseconds(6000 * 1e9);
      }
 
 
      jmt_trajectory_points.push_back(jmt_tpp);
      prev_speed = current_speed;
    }
 
    jmt_trajectory.header = original_tp.header;
    jmt_trajectory.trajectory_id = original_tp.trajectory_id;
    jmt_trajectory.trajectory_points = jmt_trajectory_points;
    jmt_trajectory.initial_longitudinal_velocity = initial_velocity;
    return jmt_trajectory;
  }

References config_, EPSILON, get_relative_downtracks(), yield_plugin::get_smallest_time_step_of_traj(), process_bag::i, last_speed_, last_speed_time_, YieldPluginConfig::max_stop_speed_in_ms, basic_autonomy::smoothing::moving_average_filter(), nh_, polynomial_calc(), polynomial_calc_d(), and YieldPluginConfig::speed_moving_average_window_size.

Referenced by update_traj_for_cooperative_behavior(), and update_traj_for_object().

Here is the call graph for this function:

Here is the caller graph for this function:

get_collision()

std::optional< GetCollisionResult > yield_plugin::YieldPlugin::get_collision	(	const carma_planning_msgs::msg::TrajectoryPlan &	ego_trajectory,
		const std::vector< carma_perception_msgs::msg::PredictedState > &	object_predictions,
		double	collision_radius,
		double	ego_max_speed
	)

Return naive collision time and locations based on collision radius given two trajectories with one being obstacle's predicted steps.

Parameters

ego_trajectory	trajectory of the ego vehicle
object_predictions	trajectory of predicted steps
collision_radius	a distance to check between two trajectory points at a same timestamp that is considered a collision
ego_max_speed	max speed of the ego_trajectory to efficiently traverse through possible collision combination of the two trajectories NOTE: Currently object_predictions is assumed to be a simple cv model to save computational performance NOTE: Collisions are based on only collision radius at the same predicted time even if ego vehicle maybe past the obstacle. To filter these cases, see is_object_behind_vehicle()

Returns

data of time of collision if detected, otherwise, std::nullopt

Definition at line 624 of file yield_plugin.cpp.

  {
 
    // Iterate through each pair of consecutive points in the trajectories
    RCLCPP_DEBUG_STREAM(nh_->get_logger(), "Starting a new collision detection, trajectory size: "
      << ego_trajectory.trajectory_points.size() << ". prediction size: " << object_predictions.size());
 
    // Iterate through the object to check if it's on the route
    bool on_route = false;
    int on_route_idx = 0;
 
    // A flag to stop searching more than one lanelet if the object has no velocity
    const auto object_speed{std::hypot(object_predictions.front().predicted_velocity.linear.x,
                                  object_predictions.front().predicted_velocity.linear.y)};
    bool object_has_zero_speed = object_speed < config_.obstacle_zero_speed_threshold_in_ms;
 
    if (object_predictions.size() < 2)
    {
      throw std::invalid_argument("Object on ther road doesn't have enough predicted states! Please check motion_computation is correctly applying predicted states");
    }
    const double object_prediction_step_duration = (rclcpp::Time(object_predictions.at(1).header.stamp) - rclcpp::Time(object_predictions.front().header.stamp)).seconds();
    const double object_prediction_total_duration = get_trajectory_duration(object_predictions);
 
    if (object_prediction_step_duration < 0.0)
    {
      throw std::invalid_argument("Predicted states of the object is malformed. Detected trajectory going backwards in time!");
    }
 
    // In order to optimize the for loops for comparing two trajectories, following logic skips every iteration_stride-th points of the object_predictions.
    // Since skipping number of points from the object_predictions may result in ignoring potential collisions, its value is dependent on two
    // trajectories' speeds and intervehicle_collision_distance_in_m radius.
    // Therefore, the derivation first calculates the max time, t, that both actors can move while still being in collision radius:
    // sqrt( (v1 * t / 2)^2 + (v2 * t / 2)^2 ) = collision_radius. Here v1 and v2 are assumed to be perpendicular to each other and
    // intersecting at t/2 to get max possible collision_radius. Solving for t gives following:
    double iteration_stride_max_time_s = 2 * config_.intervehicle_collision_distance_in_m / sqrt(pow(object_speed, 2) + pow(ego_max_speed, 2));
    int iteration_stride = std::max(1, static_cast<int>(iteration_stride_max_time_s / object_prediction_step_duration));
 
    RCLCPP_DEBUG_STREAM(nh_->get_logger(), "Determined iteration_stride: " << iteration_stride
      << ", with object_speed: " << object_speed
      << ", with ego_max_speed: " << ego_max_speed
      << ", with object_prediction_step_duration: " << object_prediction_step_duration
      << ", iteration_stride_max_time_s: " << iteration_stride_max_time_s);
 
    for (size_t j = 0; j < object_predictions.size(); j += iteration_stride)
    {
      const lanelet::BasicPoint2d point(object_predictions.at(j).predicted_position.position.x,
                                        object_predictions.at(j).predicted_position.position.y);
      for (const auto& llt : route_llt_polygons_)
      {
        if (boost::geometry::within(point, llt.polygon2d()))
        {
          on_route = true;
          on_route_idx = j;
          break;
        }
      }
      if (on_route || object_has_zero_speed)
        break;
    }
 
    RCLCPP_DEBUG_STREAM(rclcpp::get_logger("yield_plugin"), "[CPU] on_route=" << on_route
      << " on_route_idx=" << on_route_idx
      << " speed=" << object_speed
      << " stride=" << iteration_stride);
 
    if (!on_route)
    {
      RCLCPP_DEBUG(rclcpp::get_logger("yield_plugin"), "[CPU] Object not on route — skipping");
      return std::nullopt;
    }
 
    double smallest_dist = std::numeric_limits<double>::infinity();
    for (size_t i = 0; i < ego_trajectory.trajectory_points.size() - 1; ++i)
    {
      auto ego_seg_start = ego_trajectory.trajectory_points.at(i);
      auto ego_seg_end = ego_trajectory.trajectory_points.at(i + 1);
      double previous_distance = std::numeric_limits<double>::infinity();
      for (size_t j = on_route_idx; j < object_predictions.size() - 1; j += iteration_stride)
      {
        auto object_seg_start = object_predictions.at(j);
        auto object_seg_end = object_predictions.at(j + 1);
        double ego_seg_start_time = rclcpp::Time(ego_seg_start.target_time).seconds();
        double ego_seg_end_time = rclcpp::Time(ego_seg_end.target_time).seconds();
        double object_seg_start_time = rclcpp::Time(object_seg_start.header.stamp).seconds();
        double object_seg_end_time = rclcpp::Time(object_seg_end.header.stamp).seconds();
 
        RCLCPP_DEBUG_STREAM(nh_->get_logger(), "ego_seg_start.target_time: " << std::to_string(ego_seg_start_time) << ", ego_seg_end.target_time: " << std::to_string(ego_seg_end_time));
        RCLCPP_DEBUG_STREAM(nh_->get_logger(), "object_seg_start.target_time: " << std::to_string(object_seg_start_time) << ", object_seg_end.target_time: " << std::to_string(object_seg_end_time));
        RCLCPP_DEBUG_STREAM(nh_->get_logger(), "ego_seg_start.x: " << ego_seg_start.x << ", ego_seg_start.y: " << ego_seg_start.y);
        RCLCPP_DEBUG_STREAM(nh_->get_logger(), "ego_seg_end.x: " << ego_seg_end.x << ", ego_seg_end.y: " << ego_seg_end.y);
 
        RCLCPP_DEBUG_STREAM(nh_->get_logger(), "object_seg_start.x: " << object_seg_start.predicted_position.position.x << ", object_seg_start.y: " << object_seg_start.predicted_position.position.y);
        RCLCPP_DEBUG_STREAM(nh_->get_logger(), "object_seg_end.x: " << object_seg_end.predicted_position.position.x << ", object_seg_end.y: " << object_seg_end.predicted_position.position.y);
 
        // Linearly interpolate positions at a common timestamp for both trajectories
        double interp_ratio = (object_seg_start_time - ego_seg_start_time) / (ego_seg_end_time - ego_seg_start_time);
        // if negative extrapolation, skip because car wouldn't go backwards
        if (interp_ratio < 0)
        {
          RCLCPP_DEBUG_STREAM(nh_->get_logger(),
            "Negative extrapolation, skipping this pair of points. object_seg_start_time: "
            << std::to_string(object_seg_start_time) << ", ego_seg_start_time: "
            << std::to_string(ego_seg_start_time));
          continue;
        }
        double ego_interp_x = ego_seg_start.x + interp_ratio * (ego_seg_end.x - ego_seg_start.x);
        double ego_interp_y = ego_seg_start.y + interp_ratio * (ego_seg_end.y - ego_seg_start.y);
        double object_x = object_seg_start.predicted_position.position.x;
        double object_y = object_seg_start.predicted_position.position.y;
 
        // Calculate the distance between the two interpolated points
        const auto distance{std::hypot(ego_interp_x - object_x, ego_interp_y - object_y)};
 
        smallest_dist = std::min(distance, smallest_dist);
 
        // Following "if logic" assumes the object_predictions is a simple cv model, aka, object_predictions point is a straight line over time.
        // And current ego_trajectory point is fixed in this iteration.
        // Then once the distance between the two start to increase over object_predictions iteration,
        // the distance will always increase and it's unnecessary to continue the logic to find the smallest_dist
        if (previous_distance < distance)
        {
          RCLCPP_DEBUG_STREAM(nh_->get_logger(), "Stopping search here because the distance between predictions started to increase");
          break;
        }
        previous_distance = distance;
 
        if (i == 0 && j == 0 && distance > config_.collision_check_radius_in_m)
        {
          RCLCPP_DEBUG(nh_->get_logger(), "Too far away" );
          return std::nullopt;
        }
 
        if (distance > collision_radius)
        {
          // continue searching for collision
          continue;
        }
 
        GetCollisionResult collision_result;
        collision_result.ego_point = lanelet::BasicPoint2d(ego_interp_x, ego_interp_y);
        collision_result.object_point = lanelet::BasicPoint2d(object_x, object_y);
        collision_result.collision_time = rclcpp::Time(object_seg_start.header.stamp);
        return collision_result;
      }
    }
    RCLCPP_DEBUG_STREAM(
      rclcpp::get_logger("yield_plugin"),
      "Was not able to find collision: smallest_dist: " << smallest_dist);
 
    // No collision detected
    return std::nullopt;
  }

References YieldPluginConfig::collision_check_radius_in_m, yield_plugin::GetCollisionResult::collision_time, config_, yield_plugin::GetCollisionResult::ego_point, yield_plugin::get_trajectory_duration(), process_bag::i, YieldPluginConfig::intervehicle_collision_distance_in_m, nh_, yield_plugin::GetCollisionResult::object_point, YieldPluginConfig::obstacle_zero_speed_threshold_in_ms, process_traj_logs::point, route_llt_polygons_, and carma_cooperative_perception::to_string().

Referenced by get_collision_time().

Here is the call graph for this function:

Here is the caller graph for this function:

get_collision_time()

std::optional< rclcpp::Time > yield_plugin::YieldPlugin::get_collision_time	(	const carma_planning_msgs::msg::TrajectoryPlan &	original_tp,
		const carma_perception_msgs::msg::ExternalObject &	curr_obstacle,
		double	original_tp_max_speed
	)

Return collision time given two trajectories with one being external object with predicted steps.

Parameters

original_tp	trajectory of the ego vehicle
curr_obstacle	trajectory of the obstacle
original_tp_max_speed	max speed of the original_tp to efficiently traverse through possible collision combination of the two trajectories NOTE: Currently curr_obstacle is assumed to be using a simple cv model to save computational performance

Returns

time_of_collision if collision detected, otherwise, std::nullopt

Definition at line 805 of file yield_plugin.cpp.

  {
    auto plan_start_time = get_trajectory_start_time(original_tp);
 
    RCLCPP_DEBUG_STREAM(nh_->get_logger(), "Object's back time: " << std::to_string(rclcpp::Time(curr_obstacle.predictions.back().header.stamp).seconds())
      << ", plan_start_time: " << std::to_string(plan_start_time));
 
    // do not process outdated objects
    if (rclcpp::Time(curr_obstacle.predictions.back().header.stamp).seconds() <= plan_start_time)
    {
      return std::nullopt;
    }
 
    std::vector<carma_perception_msgs::msg::PredictedState> new_list;
    carma_perception_msgs::msg::PredictedState curr_state;
    // artificially include current position as one of the predicted states
    curr_state.header.stamp = curr_obstacle.header.stamp;
    curr_state.predicted_position.position.x = curr_obstacle.pose.pose.position.x;
    curr_state.predicted_position.position.y = curr_obstacle.pose.pose.position.y;
    // NOTE: predicted_velocity is not used for collision calculation, but timestamps
    curr_state.predicted_velocity.linear.x = curr_obstacle.velocity.twist.linear.x;
    curr_state.predicted_velocity.linear.y = curr_obstacle.velocity.twist.linear.y;
    RCLCPP_DEBUG_STREAM(nh_->get_logger(), "Object: " << curr_obstacle.id <<", type: " << static_cast<int>(curr_obstacle.object_type)
      << ", speed_x: " << curr_obstacle.velocity.twist.linear.x  << ", speed_y: " << curr_obstacle.velocity.twist.linear.y);
    new_list.push_back(curr_state);
    new_list.insert(new_list.end(), curr_obstacle.predictions.cbegin(), curr_obstacle.predictions.cend());
 
    const auto collision_result = get_collision(original_tp, new_list, config_.intervehicle_collision_distance_in_m, original_tp_max_speed);
 
    if (!collision_result)
    {
      // reset the consecutive clearance counter because no collision was detected at this iteration
      consecutive_clearance_count_for_obstacles_[curr_obstacle.id] = 0;
      RCLCPP_DEBUG_STREAM(rclcpp::get_logger("yield_plugin"), "[CPU] obj=" << curr_obstacle.id << " no collision detected");
      return std::nullopt;
    }
 
    // if within collision radius, it is not a collision if obstacle is behind the vehicle despite being in collision radius
    const double vehicle_downtrack = wm_->routeTrackPos(collision_result.value().ego_point).downtrack;
    const double object_downtrack = wm_->routeTrackPos(collision_result.value().object_point).downtrack;
 
    if (is_object_behind_vehicle(curr_obstacle.id, collision_result.value().collision_time, vehicle_downtrack, object_downtrack))
    {
      RCLCPP_INFO_STREAM(nh_->get_logger(), "Confirmed that the object: " << curr_obstacle.id << " is behind the vehicle at timestamp " << std::to_string(collision_result.value().collision_time.seconds()));
      return std::nullopt;
    }
 
    const auto distance{std::hypot(
      collision_result.value().ego_point.x() - collision_result.value().object_point.x(),
      collision_result.value().ego_point.y() - collision_result.value().object_point.y()
    )}; //for debug
 
    RCLCPP_WARN_STREAM(nh_->get_logger(), "Collision detected for object: " << curr_obstacle.id << ", at timestamp " << std::to_string(collision_result.value().collision_time.seconds()) <<
      ", x: " << collision_result.value().ego_point.x() << ", y: " << collision_result.value().ego_point.y() <<
      ", within actual downtrack distance: " << object_downtrack - vehicle_downtrack <<
      ", and collision distance: " << distance);
    RCLCPP_DEBUG_STREAM(rclcpp::get_logger("yield_plugin"), "[CPU] obj=" << curr_obstacle.id << " collision at t=" << collision_result.value().collision_time.seconds());
 
    return collision_result.value().collision_time;
  }

References config_, consecutive_clearance_count_for_obstacles_, get_collision(), yield_plugin::get_trajectory_start_time(), YieldPluginConfig::intervehicle_collision_distance_in_m, is_object_behind_vehicle(), nh_, carma_cooperative_perception::to_string(), and wm_.

Referenced by get_collision_times_concurrently_cpu().

Here is the call graph for this function:

Here is the caller graph for this function:

get_collision_times_concurrently()

std::unordered_map< uint32_t, rclcpp::Time > yield_plugin::YieldPlugin::get_collision_times_concurrently	(	const carma_planning_msgs::msg::TrajectoryPlan &	original_tp,
		const std::vector< carma_perception_msgs::msg::ExternalObject > &	external_objects,
		double	original_tp_max_speed
	)

Given the list of objects with predicted states, get all collision times concurrently using multi-threading.

Parameters

original_tp	trajectory of the ego vehicle
external_objects	list of external objects with predicted states
original_tp_max_speed	max speed of the original_tp to efficiently traverse through possible collision combination of the two trajectories

Returns

mapping of objects' ids and their corresponding collision times (non-colliding objects are omitted)

Definition at line 927 of file yield_plugin.cpp.

  {
    if (!cuda_is_available())
      return get_collision_times_concurrently_cpu(original_tp, external_objects, original_tp_max_speed);
    return get_collision_times_concurrently_cuda(original_tp, external_objects, original_tp_max_speed);
  }

References get_collision_times_concurrently_cpu(), and get_collision_times_concurrently_cuda().

Referenced by get_earliest_collision_object_and_time().

Here is the call graph for this function:

Here is the caller graph for this function:

get_collision_times_concurrently_cpu()

std::unordered_map< uint32_t, rclcpp::Time > yield_plugin::YieldPlugin::get_collision_times_concurrently_cpu ( const carma_planning_msgs::msg::TrajectoryPlan &  original_tp, const std::vector< carma_perception_msgs::msg::ExternalObject > &  external_objects, double  original_tp_max_speed  )

private

CPU implementation of get_collision_times_concurrently. Launches one thread per external object and computes get_collision_time for each in parallel.

Parameters

original_tp	trajectory of the ego vehicle
external_objects	list of external objects with predicted states
original_tp_max_speed	max speed of the original_tp to efficiently traverse through possible collision combination of the two trajectories

Returns

mapping of objects' ids and their corresponding collision times (non-colliding objects are omitted)

Definition at line 937 of file yield_plugin.cpp.

  {
    std::unordered_map<uint32_t, std::future<std::optional<rclcpp::Time>>> futures;
    std::unordered_map<uint32_t, rclcpp::Time> collision_times;
    RCLCPP_DEBUG_STREAM(rclcpp::get_logger("yield_plugin"),
      "[CPU] Launching " << external_objects.size() << " async get_collision_time tasks");
    std::vector<std::thread> threads;
    threads.reserve(external_objects.size());
    for (const auto& object : external_objects) {
      std::packaged_task<std::optional<rclcpp::Time>()> task(
        [this, &original_tp, &object, &original_tp_max_speed] {
          return get_collision_time(original_tp, object, original_tp_max_speed);
        });
      futures[object.id] = task.get_future();
      threads.emplace_back(std::move(task));
    }
    for (auto& t : threads) t.join();
    for (const auto& object : external_objects) {
      if (const auto collision_time{futures.at(object.id).get()}) {
        collision_times[object.id] = collision_time.value();
      }
    }
    RCLCPP_DEBUG_STREAM(rclcpp::get_logger("yield_plugin"),
      "[CPU] Done — " << collision_times.size() << " collision(s) confirmed");
    return collision_times;
  }

References get_collision_time().

Referenced by get_collision_times_concurrently(), and get_collision_times_concurrently_cuda().

Here is the call graph for this function:

Here is the caller graph for this function:

get_collision_times_concurrently_cuda()

std::unordered_map< uint32_t, rclcpp::Time > yield_plugin::YieldPlugin::get_collision_times_concurrently_cuda ( const carma_planning_msgs::msg::TrajectoryPlan &  original_tp, const std::vector< carma_perception_msgs::msg::ExternalObject > &  external_objects, double  original_tp_max_speed  )

private

CUDA implementation of get_collision_times_concurrently. Filters objects to those with on-route predictions, then batches the collision check across all objects on the GPU. Falls back to get_collision_times_concurrently_cpu if no CUDA device is available or on error.

Parameters

original_tp	trajectory of the ego vehicle
external_objects	list of external objects with predicted states
original_tp_max_speed	max speed of the original_tp to efficiently traverse through possible collision combination of the two trajectories

Returns

mapping of objects' ids and their corresponding collision times (non-colliding objects are omitted)

Definition at line 967 of file yield_plugin.cpp.

  {
    std::unordered_map<uint32_t, rclcpp::Time> collision_times;
 
    if (original_tp.trajectory_points.size() < 2) return collision_times;
 
    const double plan_start_time = get_trajectory_start_time(original_tp);
 
    // Timestamps as absolute doubles; reference used to normalise into float32.
    const double ref_time = plan_start_time;
 
    // Build ego SoA (structure of array) with normalised timestamps.
    const auto num_ego_points = static_cast<int>(original_tp.trajectory_points.size());
    std::vector<CudaPoint> ego_pts;
    ego_pts.reserve(num_ego_points);
    for (const auto& tp : original_tp.trajectory_points) {
      ego_pts.push_back({
        static_cast<float>(tp.x),
        static_cast<float>(tp.y),
        static_cast<float>(rclcpp::Time(tp.target_time).seconds() - ref_time)
      });
    }
 
    // Per-object data accumulated for the CUDA batch.
    struct ActiveObject {
      uint32_t id;
      // Prediction list with current position prepended (same as get_collision_time builds).
      std::vector<carma_perception_msgs::msg::PredictedState> predictions;
      int on_route_idx;  // first prediction index known to be on the route
    };
 
    RCLCPP_DEBUG_STREAM(rclcpp::get_logger("yield_plugin"),
      "[GPU] Processing " << external_objects.size() << " external objects");
 
    std::vector<ActiveObject>  active;
    std::vector<CudaPoint>     obs_flat;
    std::vector<int>           obs_offsets;
    std::vector<int>           obs_sizes;
 
    for (const auto& obj : external_objects) {
      // Skip objects whose entire prediction horizon is before the plan start.
      if (rclcpp::Time(obj.predictions.back().header.stamp).seconds() <= plan_start_time) {
        RCLCPP_DEBUG_STREAM(rclcpp::get_logger("yield_plugin"),
          "[GPU] obj=" << obj.id << " skipped — predictions expired before plan start");
        continue;
      }
 
      // Build prediction list: prepend current position (mirrors get_collision_time).
      std::vector<carma_perception_msgs::msg::PredictedState> pred_list;
      pred_list.reserve(obj.predictions.size() + 1);
      {
        carma_perception_msgs::msg::PredictedState curr;
        curr.header.stamp = obj.header.stamp;
        curr.predicted_position.position.x = obj.pose.pose.position.x;
        curr.predicted_position.position.y = obj.pose.pose.position.y;
        curr.predicted_velocity.linear.x   = obj.velocity.twist.linear.x;
        curr.predicted_velocity.linear.y   = obj.velocity.twist.linear.y;
        pred_list.push_back(curr);
      }
      pred_list.insert(pred_list.end(), obj.predictions.cbegin(), obj.predictions.cend());
 
      if (pred_list.size() < 2) continue;
 
      const double object_prediction_step_duration =
        (rclcpp::Time(pred_list.at(1).header.stamp) - rclcpp::Time(pred_list.front().header.stamp)).seconds();
      if (object_prediction_step_duration < 0.0) continue;
 
      // Quick spatial pre-filter: if the object starts beyond collision_check_radius_in_m
      // it cannot collide with the ego at the start of the trajectory.
      {
        const double ego_to_object_dx = ego_pts[0].x - obj.pose.pose.position.x;
        const double ego_to_object_dy = ego_pts[0].y - obj.pose.pose.position.y;
        const double ego_to_object_dist = std::hypot(ego_to_object_dx, ego_to_object_dy);
        if (ego_to_object_dist > config_.collision_check_radius_in_m) {
          RCLCPP_DEBUG_STREAM(rclcpp::get_logger("yield_plugin"),
            "[GPU] obj=" << obj.id << " skipped — dist_from_ego=" << ego_to_object_dist
            << " > radius=" << config_.collision_check_radius_in_m);
          consecutive_clearance_count_for_obstacles_[obj.id] = 0;
          continue;
        }
      }
 
      // On-route check — stride logic as in get_collision, but uses pre-computed
      // per-lanelet bounding boxes instead of getLaneletsFromPoint (O(1) vs spatial query).
      const double object_speed = std::hypot(
        pred_list.front().predicted_velocity.linear.x,
        pred_list.front().predicted_velocity.linear.y);
      const bool object_has_zero_speed = object_speed < config_.obstacle_zero_speed_threshold_in_ms;
 
      const double stride_max_t = 2.0 * config_.intervehicle_collision_distance_in_m /
        std::sqrt(std::pow(object_speed, 2) + std::pow(original_tp_max_speed, 2));
      const int iteration_stride = std::max(1, static_cast<int>(stride_max_t / object_prediction_step_duration));
 
      const auto [on_route, on_route_idx] = find_on_route_in_predictions(pred_list, iteration_stride, object_has_zero_speed);
 
      RCLCPP_DEBUG_STREAM(rclcpp::get_logger("yield_plugin"),
        "[GPU] obj=" << obj.id << " on_route=" << on_route
        << " on_route_idx=" << on_route_idx
        << " speed=" << object_speed
        << " stride=" << iteration_stride);
 
      if (!on_route) {
        consecutive_clearance_count_for_obstacles_[obj.id] = 0;
        continue;
      }
 
      // Pack the on-route portion of the prediction into the flat obstacle buffer.
      obs_offsets.push_back(static_cast<int>(obs_flat.size()));
      int count = 0;
      for (int j = on_route_idx; j < static_cast<int>(pred_list.size()); ++j) {
        obs_flat.push_back({
          static_cast<float>(pred_list[j].predicted_position.position.x),
          static_cast<float>(pred_list[j].predicted_position.position.y),
          static_cast<float>(rclcpp::Time(pred_list[j].header.stamp).seconds() - ref_time)
        });
        ++count;
      }
      obs_sizes.push_back(count);
      active.push_back({obj.id, std::move(pred_list), on_route_idx});
    }
 
    if (active.empty()) return collision_times;
 
    // -----------------------------------------------------------------------
    // GPU: exact, continuous-time segment-pair collision detection.
    // -----------------------------------------------------------------------
    auto _t0_cuda = std::chrono::steady_clock::now();
    try {
    const auto cuda_results = cuda_check_all_collisions(
      ego_pts, obs_flat, obs_offsets, obs_sizes,
      static_cast<float>(config_.intervehicle_collision_distance_in_m));
    const double _cuda_ms = std::chrono::duration<double, std::milli>(std::chrono::steady_clock::now() - _t0_cuda).count();
    RCLCPP_DEBUG_STREAM(rclcpp::get_logger("yield_plugin"),
      "[timing] cuda_check_all_collisions: " << _cuda_ms << " ms");
 
    // -----------------------------------------------------------------------
    // Post-process: recover collision positions and run behind-vehicle check.
    // -----------------------------------------------------------------------
    for (size_t k = 0; k < active.size(); ++k) {
      const auto& object_result = cuda_results[k];
 
      if (!object_result.has_collision) {
        RCLCPP_DEBUG_STREAM(rclcpp::get_logger("yield_plugin"),
          "[GPU] obj=" << active[k].id << " no collision detected");
        consecutive_clearance_count_for_obstacles_[active[k].id] = 0;
        continue;
      }
 
      const double collision_time_abs = static_cast<double>(object_result.collision_t_norm) + ref_time;
      const rclcpp::Time collision_time(static_cast<int64_t>(collision_time_abs * 1e9));
 
      const lanelet::BasicPoint2d ego_collision_point = interp_trajectory_pt_at_time(collision_time_abs, ego_pts, num_ego_points, original_tp);
      const lanelet::BasicPoint2d object_collision_point = interp_predicted_pt_at_time(collision_time_abs, active[k].predictions, active[k].on_route_idx);
 
      const double vehicle_downtrack = wm_->routeTrackPos(ego_collision_point).downtrack;
      const double object_downtrack  = wm_->routeTrackPos(object_collision_point).downtrack;
 
      if (is_object_behind_vehicle(active[k].id, collision_time,
                                   vehicle_downtrack, object_downtrack)) {
        RCLCPP_INFO_STREAM(nh_->get_logger(),
          "Confirmed that the object: " << active[k].id
          << " is behind the vehicle at timestamp "
          << std::to_string(collision_time.seconds()));
        continue;
      }
 
      RCLCPP_WARN_STREAM(nh_->get_logger(),
        "Collision detected for object: " << active[k].id
        << ", at timestamp " << std::to_string(collision_time.seconds())
        << ", x: " << ego_collision_point.x() << ", y: " << ego_collision_point.y()
        << ", within actual downtrack distance: "
        << object_downtrack - vehicle_downtrack);
 
      RCLCPP_DEBUG_STREAM(rclcpp::get_logger("yield_plugin"),
        "[GPU] obj=" << active[k].id
        << " collision at t=" << collision_time_abs
        << " ego=(" << ego_collision_point.x() << "," << ego_collision_point.y() << ")"
        << " obs=(" << object_collision_point.x() << "," << object_collision_point.y() << ")"
        << " downtrack_gap=" << (object_downtrack - vehicle_downtrack));
      collision_times[active[k].id] = collision_time;
    }
 
    RCLCPP_DEBUG_STREAM(rclcpp::get_logger("yield_plugin"),
      "[GPU] Done — " << collision_times.size() << " collision(s) confirmed");
 
    } catch (const std::runtime_error& e) {
      std::string error_msg(e.what());
      // Detect CUDA-specific errors
      bool is_cuda_error = (error_msg.find("CUDA") != std::string::npos ||
                            error_msg.find("cuda") != std::string::npos ||
                            error_msg.find("driver version") != std::string::npos ||
                            error_msg.find("runtime version") != std::string::npos ||
                            error_msg.find("GPU") != std::string::npos);
 
      if (is_cuda_error) {
        RCLCPP_WARN_STREAM_ONCE(rclcpp::get_logger("yield_plugin"),
          "[GPU] CUDA unavailable (" << e.what() << "), please make sure GPU is accessible for this node or container. Using CPU fallback");
      } else {
        RCLCPP_ERROR_STREAM_ONCE(rclcpp::get_logger("yield_plugin"),
          "[GPU] Unexpected error during GPU collision detection: " << e.what() << ", using CPU fallback");
      }
      return get_collision_times_concurrently_cpu(original_tp, external_objects, original_tp_max_speed);
    } catch (const std::exception& e) {
      RCLCPP_ERROR_STREAM_ONCE(rclcpp::get_logger("yield_plugin"),
        "[GPU] Unexpected exception during GPU collision detection: " << typeid(e).name() << " - " << e.what() << ", using CPU fallback");
      return get_collision_times_concurrently_cpu(original_tp, external_objects, original_tp_max_speed);
    }
    return collision_times;
  }

References YieldPluginConfig::collision_check_radius_in_m, config_, consecutive_clearance_count_for_obstacles_, find_on_route_in_predictions(), get_collision_times_concurrently_cpu(), yield_plugin::get_trajectory_start_time(), yield_plugin::interp_predicted_pt_at_time(), yield_plugin::interp_trajectory_pt_at_time(), YieldPluginConfig::intervehicle_collision_distance_in_m, is_object_behind_vehicle(), nh_, YieldPluginConfig::obstacle_zero_speed_threshold_in_ms, carma_cooperative_perception::to_string(), and wm_.

Referenced by get_collision_times_concurrently().

Here is the call graph for this function:

Here is the caller graph for this function:

get_earliest_collision_object_and_time()

std::optional< std::pair< carma_perception_msgs::msg::ExternalObject, double > > yield_plugin::YieldPlugin::get_earliest_collision_object_and_time	(	const carma_planning_msgs::msg::TrajectoryPlan &	original_tp,
		const std::vector< carma_perception_msgs::msg::ExternalObject > &	external_objects
	)

Return the earliest collision object and time of collision pair from the given trajectory and list of external objects with predicted states. Function first filters obstacles based on whether if their any of predicted state will be on the route. Only then, the logic compares trajectory and predicted states.

Parameters

original_tp	trajectory of the ego vehicle
external_objects	list of external objects with predicted states

Returns

earliest collision object and its collision time if collision detected. std::nullopt if no collision is detected or if route is not available.

Definition at line 1180 of file yield_plugin.cpp.

  {
    RCLCPP_DEBUG_STREAM(rclcpp::get_logger("yield_plugin"), "ExternalObjects size: " << external_objects.size());
 
    if (!wm_->getRoute())
    {
      RCLCPP_WARN(nh_->get_logger(), "Yield plugin was not able to analyze collision since route is not available! Please check if route is set");
      return std::nullopt;
    }
 
    // Populate route_llt_polygons_ if empty but route is actually available.
    if (route_llt_polygons_.empty())
    {
      update_route_llt_cache();
    }
 
    RCLCPP_DEBUG_STREAM(nh_->get_logger(),"External Object List (external_objects) size: " << external_objects.size());
    const double original_max_speed = max_trajectory_speed(original_tp.trajectory_points, get_trajectory_end_time(original_tp));
    auto _t0_conc = std::chrono::steady_clock::now();
    std::unordered_map<uint32_t, rclcpp::Time> collision_times = get_collision_times_concurrently(original_tp,external_objects, original_max_speed);
    const double _conc_ms = std::chrono::duration<double, std::milli>(std::chrono::steady_clock::now() - _t0_conc).count();
    RCLCPP_DEBUG_STREAM(rclcpp::get_logger("yield_plugin"),
      "[timing] get_collision_times_concurrently: " << _conc_ms << " ms");
 
    if (collision_times.empty()) { return std::nullopt; }
 
    const auto earliest_colliding_object_id{std::min_element(
      std::cbegin(collision_times), std::cend(collision_times),
      [](const auto & a, const auto & b){ return a.second < b.second; })->first};
 
    const auto earliest_colliding_object{std::find_if(
      std::cbegin(external_objects), std::cend(external_objects),
      [&earliest_colliding_object_id](const auto & object) { return object.id == earliest_colliding_object_id; })};
 
    RCLCPP_DEBUG_STREAM(nh_->get_logger(),"earliest object x: " << earliest_colliding_object->velocity.twist.linear.x
        << ", y: " << earliest_colliding_object->velocity.twist.linear.y);
    return std::make_pair(*earliest_colliding_object, collision_times.at(earliest_colliding_object_id).seconds());
 
  }

References get_collision_times_concurrently(), yield_plugin::get_trajectory_end_time(), max_trajectory_speed(), nh_, route_llt_polygons_, update_route_llt_cache(), and wm_.

Referenced by update_traj_for_object().

Here is the call graph for this function:

Here is the caller graph for this function:

get_predicted_velocity_at_time()

double yield_plugin::YieldPlugin::get_predicted_velocity_at_time	(	const geometry_msgs::msg::Twist &	object_velocity_in_map_frame,
		const carma_planning_msgs::msg::TrajectoryPlan &	original_tp,
		double	timestamp_in_sec_to_predict
	)

Given the object velocity in map frame with x,y components, this function returns the projected velocity along the trajectory at given time.

Parameters

object_velocity_in_map_frame	trajectory of the ego vehicle
original_tp	trajectory of the ego vehicle
timestamp_in_sec_to_predict	timestamp in seconds along the trajectory to return the projected velocity NOTE: returns the last point's speed if the specified time is past the trajectory's planning time

Returns

get_predicted_velocity_at_time

Definition at line 1221 of file yield_plugin.cpp.

  {
    RCLCPP_DEBUG_STREAM(nh_->get_logger(), "timestamp_in_sec_to_predict: " << std::to_string(timestamp_in_sec_to_predict) <<
      ", trajectory_end_time: " << std::to_string(get_trajectory_end_time(original_tp)));
 
    double point_b_time = 0.0;
    carma_planning_msgs::msg::TrajectoryPlanPoint point_a;
    carma_planning_msgs::msg::TrajectoryPlanPoint point_b;
 
    // trajectory points' time is guaranteed to be increasing
    // then find the corresponding point at timestamp_in_sec_to_predict
    for (size_t i = 0; i < original_tp.trajectory_points.size() - 1; ++i)
    {
      point_a = original_tp.trajectory_points.at(i);
      point_b = original_tp.trajectory_points.at(i + 1);
      point_b_time = rclcpp::Time(point_b.target_time).seconds();
      if (point_b_time >= timestamp_in_sec_to_predict)
      {
        break;
      }
    }
 
    auto dx = point_b.x - point_a.x;
    auto dy = point_b.y - point_a.y;
    const tf2::Vector3 trajectory_direction(dx, dy, 0);
 
    RCLCPP_DEBUG_STREAM(nh_->get_logger(), "timestamp_in_sec_to_predict: " << std::to_string(timestamp_in_sec_to_predict)
      << ", point_b_time: " << std::to_string(point_b_time)
      << ", dx: " << dx << ", dy: " << dy << ", "
      << ", object_velocity_in_map_frame.x: " << object_velocity_in_map_frame.linear.x
      << ", object_velocity_in_map_frame.y: " << object_velocity_in_map_frame.linear.y);
 
    if (trajectory_direction.length() < 0.001) //EPSILON
    {
      return 0.0;
    }
 
    const tf2::Vector3 object_direction(object_velocity_in_map_frame.linear.x, object_velocity_in_map_frame.linear.y, 0);
 
    return tf2::tf2Dot(object_direction, trajectory_direction) / trajectory_direction.length();
  }

References visualize_xodr::dx, visualize_xodr::dy, yield_plugin::get_trajectory_end_time(), process_bag::i, nh_, and carma_cooperative_perception::to_string().

Referenced by update_traj_for_object().

Here is the call graph for this function:

Here is the caller graph for this function:

get_relative_downtracks()

std::vector< double > yield_plugin::YieldPlugin::get_relative_downtracks

(

const carma_planning_msgs::msg::TrajectoryPlan &

trajectory_plan

)

const

calculates distance between trajectory points in a plan

Parameters

trajectory_plan

input trajectory plan

Returns

vector of relative distances between trajectory points

Definition at line 1381 of file yield_plugin.cpp.

  {
    std::vector<double> downtracks;
    downtracks.reserve(trajectory_plan.trajectory_points.size());
    // relative downtrack distance of the fist Point is 0.0
    downtracks.push_back(0.0);
    for (size_t i=1; i < trajectory_plan.trajectory_points.size(); i++){
 
      double dx = trajectory_plan.trajectory_points.at(i).x - trajectory_plan.trajectory_points.at(i-1).x;
      double dy = trajectory_plan.trajectory_points.at(i).y - trajectory_plan.trajectory_points.at(i-1).y;
      downtracks.push_back(sqrt(dx*dx + dy*dy));
    }
    return downtracks;
  }

References visualize_xodr::dx, visualize_xodr::dy, and process_bag::i.

Referenced by generate_JMT_trajectory().

Here is the caller graph for this function:

is_object_behind_vehicle()

bool yield_plugin::YieldPlugin::is_object_behind_vehicle	(	uint32_t	object_id,
		const rclcpp::Time &	collision_time,
		double	vehicle_point,
		double	object_downtrack
	)

Check if object location is behind the vehicle using estimates of the vehicle's length and route downtracks.

Parameters

object_id	object id to use for the consecutive_clearance_count_for_obstacles_
collision_time	predicted time of collision
vehicle_downtrack	at the time of collision
object_downtrack	at the time of collision NOTE: Uses internal counter low pass filter to confirm the object is behind only if it counted continuously above config_.consecutive_clearance_count_for_passed_obstacles_threshold

Returns

return true if object is behind the vehicle

Definition at line 779 of file yield_plugin.cpp.

  {
    const auto previous_clearance_count = consecutive_clearance_count_for_obstacles_[object_id];
    // if the object's location is half a length of the vehicle past its rear-axle, it is considered behind
    // half a length of the vehicle to conservatively estimate the rear axle to rear bumper length
    if (object_downtrack < vehicle_downtrack - config_.vehicle_length / 2)
    {
      consecutive_clearance_count_for_obstacles_[object_id] = std::min(consecutive_clearance_count_for_obstacles_[object_id] + 1, config_.consecutive_clearance_count_for_passed_obstacles_threshold);
      RCLCPP_INFO_STREAM(nh_->get_logger(), "Detected an object nearby might be behind the vehicle at timestamp: " << std::to_string(collision_time.seconds()) <<
        ", and consecutive_clearance_count_for obstacle: " <<  object_id << ", is: " << consecutive_clearance_count_for_obstacles_[object_id]);
    }
    // confirmed false positive for a collision
    if (consecutive_clearance_count_for_obstacles_[object_id] >= config_.consecutive_clearance_count_for_passed_obstacles_threshold)
    {
      return true;
    }
    // if the clearance counter didn't increase by this point, true collision was detected
    // therefore reset the consecutive clearance counter as it is no longer consecutive
    if (consecutive_clearance_count_for_obstacles_[object_id] == previous_clearance_count)
    {
      consecutive_clearance_count_for_obstacles_[object_id] = 0;
    }
 
    return false;
  }

References config_, consecutive_clearance_count_for_obstacles_, YieldPluginConfig::consecutive_clearance_count_for_passed_obstacles_threshold, nh_, carma_cooperative_perception::to_string(), and YieldPluginConfig::vehicle_length.

Referenced by get_collision_time(), and get_collision_times_concurrently_cuda().

Here is the call graph for this function:

Here is the caller graph for this function:

lookup_ecef_to_map_transform()

void yield_plugin::YieldPlugin::lookup_ecef_to_map_transform

(

)

Looks up the transform between map and earth frames, and sets the member variable.

max_trajectory_speed()

double yield_plugin::YieldPlugin::max_trajectory_speed	(	const std::vector< carma_planning_msgs::msg::TrajectoryPlanPoint > &	trajectory_points,
		double	timestamp_in_sec_to_search_until
	)		const

calculates the maximum speed in a set of tajectory points

Parameters

trajectory_points

trajectory points

\param timestamp_in_sec_to_search_until before which to look for max trajectory speed

Returns

maximum speed

Definition at line 1418 of file yield_plugin.cpp.

  {
    double max_speed = 0;
    for(size_t i = 0; i < trajectory_points.size() - 2; i++ )
    {
      double dx = trajectory_points.at(i + 1).x - trajectory_points.at(i).x;
      double dy = trajectory_points.at(i + 1).y - trajectory_points.at(i).y;
      double d = sqrt(dx*dx + dy*dy);
      double t = (rclcpp::Time(trajectory_points.at(i + 1).target_time).seconds() - rclcpp::Time(trajectory_points.at(i).target_time).seconds());
      double v = d/t;
      if(v > max_speed)
      {
        max_speed = v;
      }
      if (rclcpp::Time(trajectory_points.at(i + 1).target_time).seconds() >= timestamp_in_sec_to_search_until)
      {
        break;
      }
 
    }
    return max_speed;
  }

References visualize_xodr::dx, visualize_xodr::dy, and process_bag::i.

Referenced by get_earliest_collision_object_and_time(), update_traj_for_cooperative_behavior(), and update_traj_for_object().

Here is the caller graph for this function:

mobilityrequest_cb()

void yield_plugin::YieldPlugin::mobilityrequest_cb

(

const carma_v2x_msgs::msg::MobilityRequest::UniquePtr

msg

)

callback for mobility request

Parameters

msg	mobility request message

Definition at line 157 of file yield_plugin.cpp.

  {
    carma_v2x_msgs::msg::MobilityRequest incoming_request = *msg;
    carma_planning_msgs::msg::LaneChangeStatus lc_status_msg;
    if (incoming_request.strategy == "carma/cooperative-lane-change")
    {
      if (!map_projector_) {
        RCLCPP_ERROR(nh_->get_logger(),"Cannot process mobility request as map projection is not yet set!");
        return;
      }
      if (incoming_request.plan_type.type == carma_v2x_msgs::msg::PlanType::CHANGE_LANE_LEFT || incoming_request.plan_type.type == carma_v2x_msgs::msg::PlanType::CHANGE_LANE_RIGHT)
      {
        RCLCPP_DEBUG(nh_->get_logger(),"Cooperative Lane Change Request Received");
        lc_status_msg.status = carma_planning_msgs::msg::LaneChangeStatus::REQUEST_RECEIVED;
        lc_status_msg.description = "Received lane merge request";
 
        if (incoming_request.m_header.recipient_id == config_.vehicle_id)
        {
          RCLCPP_DEBUG(nh_->get_logger(),"CLC Request correctly received");
        }
 
        // extract mobility header
        std::string req_sender_id = incoming_request.m_header.sender_id;
        std::string req_plan_id = incoming_request.m_header.plan_id;
        // extract mobility request
        carma_v2x_msgs::msg::LocationECEF ecef_location = incoming_request.location;
        carma_v2x_msgs::msg::Trajectory incoming_trajectory = incoming_request.trajectory;
        std::string req_strategy_params = incoming_request.strategy_params;
        clc_urgency_ = incoming_request.urgency;
        RCLCPP_DEBUG_STREAM(nh_->get_logger(),"received urgency: " << clc_urgency_);
 
        // Parse strategy parameters
        using boost::property_tree::ptree;
        ptree pt;
        std::istringstream strstream(req_strategy_params);
        boost::property_tree::json_parser::read_json(strstream, pt);
        int req_traj_speed_full = pt.get<int>("s");
        int req_traj_fractional = pt.get<int>("f");
        int start_lanelet_id = pt.get<int>("sl");
        int end_lanelet_id = pt.get<int>("el");
        double req_traj_speed = static_cast<double>(req_traj_speed_full) + static_cast<double>(req_traj_fractional)/10.0;
        RCLCPP_DEBUG_STREAM(nh_->get_logger(),"req_traj_speed" << req_traj_speed);
        RCLCPP_DEBUG_STREAM(nh_->get_logger(),"start_lanelet_id" << start_lanelet_id);
        RCLCPP_DEBUG_STREAM(nh_->get_logger(),"end_lanelet_id" << end_lanelet_id);
 
        std::vector<lanelet::BasicPoint2d> req_traj_plan = {};
 
        req_traj_plan = convert_eceftrajectory_to_mappoints(incoming_trajectory);
 
        double req_expiration_sec = static_cast<double>(incoming_request.expiration);
        double current_time_sec = nh_->now().seconds();
 
        bool response_to_clc_req = false;
        // ensure there is enough time for the yield
        double req_plan_time = req_expiration_sec - current_time_sec;
        double req_timestamp = static_cast<double>(incoming_request.m_header.timestamp) / 1000.0 - current_time_sec;
        set_incoming_request_info(req_traj_plan, req_traj_speed, req_plan_time, req_timestamp);
 
 
        if (req_expiration_sec - current_time_sec >= config_.min_obj_avoidance_plan_time_in_s && cooperative_request_acceptable_)
        {
          timesteps_since_last_req_ = 0;
          lc_status_msg.status = carma_planning_msgs::msg::LaneChangeStatus::REQUEST_ACCEPTED;
          lc_status_msg.description = "Accepted lane merge request";
          response_to_clc_req = true;
          RCLCPP_DEBUG(nh_->get_logger(),"CLC accepted");
        }
        else
        {
          lc_status_msg.status = carma_planning_msgs::msg::LaneChangeStatus::REQUEST_REJECTED;
          lc_status_msg.description = "Rejected lane merge request";
          response_to_clc_req = false;
          RCLCPP_DEBUG(nh_->get_logger(),"CLC rejected");
        }
        carma_v2x_msgs::msg::MobilityResponse outgoing_response = compose_mobility_response(req_sender_id, req_plan_id, response_to_clc_req);
        mobility_response_publisher_(outgoing_response);
        lc_status_msg.status = carma_planning_msgs::msg::LaneChangeStatus::RESPONSE_SENT;
        RCLCPP_DEBUG(nh_->get_logger(),"response sent");
      }
    }
    lc_status_publisher_(lc_status_msg);
 
  }

References clc_urgency_, compose_mobility_response(), config_, convert_eceftrajectory_to_mappoints(), cooperative_request_acceptable_, lc_status_publisher_, map_projector_, YieldPluginConfig::min_obj_avoidance_plan_time_in_s, mobility_response_publisher_, nh_, set_incoming_request_info(), timesteps_since_last_req_, and YieldPluginConfig::vehicle_id.

Referenced by yield_plugin::YieldPluginNode::on_configure_plugin().

Here is the call graph for this function:

Here is the caller graph for this function:

plan_trajectory_callback()

void yield_plugin::YieldPlugin::plan_trajectory_callback	(	carma_planning_msgs::srv::PlanTrajectory::Request::SharedPtr	req,
		carma_planning_msgs::srv::PlanTrajectory::Response::SharedPtr	resp
	)

Service callback for trajectory planning.

Parameters

srv_header	header
req	The service request
resp	The service response

Definition at line 256 of file yield_plugin.cpp.

{
    RCLCPP_DEBUG(nh_->get_logger(),"Yield_plugin was called!");
    if (req->initial_trajectory_plan.trajectory_points.size() < 2){
      throw std::invalid_argument("Empty Trajectory received by Yield");
    }
    rclcpp::Clock system_clock(RCL_SYSTEM_TIME);
    rclcpp::Time start_time = system_clock.now();  // Start timing the execution time for planning so it can be logged
 
    carma_planning_msgs::msg::TrajectoryPlan original_trajectory = req->initial_trajectory_plan;
    carma_planning_msgs::msg::TrajectoryPlan yield_trajectory;
 
    // if ego is not stopped and we committed to stopping, use the last committed trajectory
    if (req->vehicle_state.longitudinal_vel > EPSILON &&
      last_traj_plan_committed_to_stopping_.has_value())
    {
      RCLCPP_DEBUG(nh_->get_logger(), "Using last committed trajectory to stopping");
      lanelet::BasicPoint2d veh_pos(req->vehicle_state.x_pos_global,
            req->vehicle_state.y_pos_global);
      auto updated_trajectory = last_traj_plan_committed_to_stopping_.value();
 
      // 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(
              updated_trajectory.trajectory_points,
              veh_pos);
 
      // Update last trajectory to start from closest point (remove passed points)
      if (!updated_trajectory.trajectory_points.empty()) {
          updated_trajectory.trajectory_points =
              std::vector<carma_planning_msgs::msg::TrajectoryPlanPoint>
              (updated_trajectory.trajectory_points.begin() + idx_to_start_new_traj,
              updated_trajectory.trajectory_points.end());
      }
      last_traj_plan_committed_to_stopping_ = updated_trajectory;
      resp->trajectory_plan = last_traj_plan_committed_to_stopping_.value();
 
      rclcpp::Time end_time = system_clock.now();  // Planning complete
 
      auto duration = end_time - start_time;
      RCLCPP_DEBUG_STREAM(rclcpp::get_logger("yield_plugin"),
        "ExecutionTime Yield: " << std::to_string(duration.seconds()));
      return;
    }
 
    // Otherwise, we are planning a new trajectory by checking collision
    try
    {
      // NOTE: Wrapping entire plan_trajectory logic with try catch because there is intermittent
      // open issue of which cause is uncertain:
      // https://github.com/usdot-fhwa-stol/carma-platform/issues/2501
 
      double initial_velocity = req->vehicle_state.longitudinal_vel;
      // If vehicle_state is stopped, non-zero velocity from the trajectory
      // should be used. Otherwise, vehicle will not move.
      if (initial_velocity < EPSILON)
      {
        initial_velocity = original_trajectory.initial_longitudinal_velocity;
        // Record the time when vehicle was stopped first due to collision avoidance
        if (last_traj_plan_committed_to_stopping_.has_value() &&
          !first_time_stopped_to_prevent_collision_.has_value())
        {
          first_time_stopped_to_prevent_collision_ = nh_->now();
          RCLCPP_DEBUG(nh_->get_logger(), "First time stopped to prevent collision: %f",
            first_time_stopped_to_prevent_collision_.value().seconds());
        }
      }
 
      // seperating cooperative yield with regular object detection for better performance.
      if (config_.enable_cooperative_behavior && clc_urgency_ > config_.acceptable_urgency)
      {
        RCLCPP_DEBUG(nh_->get_logger(),"Only consider high urgency clc");
        if (timesteps_since_last_req_ < config_.acceptable_passed_timesteps)
        {
          RCLCPP_DEBUG(nh_->get_logger(),"Yield for CLC. We haven't received an updated negotiation this timestep");
          yield_trajectory = update_traj_for_cooperative_behavior(original_trajectory, initial_velocity);
          timesteps_since_last_req_++;
        }
        else
        {
          RCLCPP_DEBUG(nh_->get_logger(),"unreliable CLC communication, switching to object avoidance");
          yield_trajectory = update_traj_for_object(original_trajectory, external_objects_, initial_velocity); // Compute the trajectory
        }
      }
      else
      {
        RCLCPP_DEBUG(nh_->get_logger(),"Yield for object avoidance");
        auto _t0_upd = std::chrono::steady_clock::now();
        yield_trajectory = update_traj_for_object(original_trajectory, external_objects_, initial_velocity); // Compute the trajectory
        const double _upd_ms = std::chrono::duration<double, std::milli>(std::chrono::steady_clock::now() - _t0_upd).count();
        RCLCPP_DEBUG_STREAM(rclcpp::get_logger("yield_plugin"),
          "[timing] update_traj_for_object: " << _upd_ms << " ms");
      }
 
 
      // return original trajectory if no difference in trajectory points a.k.a no collision
      if (fabs(get_trajectory_end_time(original_trajectory) - get_trajectory_end_time(yield_trajectory)) < EPSILON)
      {
 
        resp->trajectory_plan = original_trajectory;
        // Reset the collision prevention variables
        first_time_stopped_to_prevent_collision_ = std::nullopt;
        last_traj_plan_committed_to_stopping_ = std::nullopt;
 
      }
      else
      {
        yield_trajectory.header.frame_id = "map";
        yield_trajectory.header.stamp = nh_->now();
        yield_trajectory.trajectory_id = original_trajectory.trajectory_id;
        resp->trajectory_plan = yield_trajectory;
      }
    }
    catch(const std::runtime_error& e) {
      RCLCPP_WARN_STREAM(nh_->get_logger(), "Yield Plugin failed to plan trajectory due to known negative time issue: " << e.what());
      RCLCPP_WARN_STREAM(nh_->get_logger(), "Returning the original trajectory, and retrying at the next call.");
      resp->trajectory_plan = original_trajectory;
    }
 
    rclcpp::Time end_time = system_clock.now();  // Planning complete
 
    auto duration = end_time - start_time;
    RCLCPP_DEBUG_STREAM(rclcpp::get_logger("yield_plugin"),
      "ExecutionTime Yield: " << std::to_string(duration.seconds()));
  }

References YieldPluginConfig::acceptable_passed_timesteps, YieldPluginConfig::acceptable_urgency, clc_urgency_, config_, YieldPluginConfig::enable_cooperative_behavior, EPSILON, external_objects_, first_time_stopped_to_prevent_collision_, basic_autonomy::waypoint_generation::get_nearest_point_index(), yield_plugin::get_trajectory_end_time(), last_traj_plan_committed_to_stopping_, nh_, timesteps_since_last_req_, carma_cooperative_perception::to_string(), update_traj_for_cooperative_behavior(), and update_traj_for_object().

Here is the call graph for this function:

polynomial_calc()

double yield_plugin::YieldPlugin::polynomial_calc	(	std::vector< double >	coeff,
		double	x
	)		const

calculate quintic polynomial equation for a given x

Parameters

coeff	vector including polynomial coefficiencrs
x	input variable to the polynomial

Returns

value of polynomial for given input

Definition at line 1396 of file yield_plugin.cpp.

  {
    double result = 0;
    for (size_t i = 0; i < coeff.size(); i++)
    {
      double value = coeff.at(i) * pow(x, static_cast<int>(coeff.size() - 1 - i));
      result = result + value;
    }
    return result;
  }

References process_bag::i, and osm_transform::x.

Referenced by generate_JMT_trajectory().

Here is the caller graph for this function:

polynomial_calc_d()

double yield_plugin::YieldPlugin::polynomial_calc_d	(	std::vector< double >	coeff,
		double	x
	)		const

calculate derivative of quintic polynomial equation for a given x

Parameters

coeff	vector including polynomial coefficiencrs
x	input variable to the polynomial

Returns

value of derivative polynomial for given input

Definition at line 1407 of file yield_plugin.cpp.

  {
    double result = 0;
    for (size_t i = 0; i < coeff.size()-1; i++)
    {
      double value = static_cast<int>(coeff.size() - 1 - i) * coeff.at(i) * pow(x, static_cast<int>(coeff.size() - 2 - i));
      result = result + value;
    }
    return result;
  }

References process_bag::i, and osm_transform::x.

Referenced by generate_JMT_trajectory().

Here is the caller graph for this function:

set_external_objects()

void yield_plugin::YieldPlugin::set_external_objects

(

const std::vector< carma_perception_msgs::msg::ExternalObject > &

object_list

)

Setter for external objects with predictions in the environment.

Parameters

object_list

The object list.

Definition at line 1518 of file yield_plugin.cpp.

  {
    external_objects_ = object_list;
  }

References external_objects_.

set_georeference_string()

void yield_plugin::YieldPlugin::set_georeference_string

(

const std::string &

georeference

)

Setter for map projection string to define lat/lon -> map conversion.

Parameters

georeference

The proj string defining the projection.

Definition at line 1509 of file yield_plugin.cpp.

  {
    if (georeference_ != georeference)
    {
      georeference_ = georeference;
      map_projector_ = std::make_shared<lanelet::projection::LocalFrameProjector>(georeference.c_str());  // Build projector from proj string
    }
  }

References georeference_, and map_projector_.

set_incoming_request_info()

void yield_plugin::YieldPlugin::set_incoming_request_info	(	std::vector< lanelet::BasicPoint2d >	req_trajectory,
		double	req_speed,
		double	req_planning_time,
		double	req_timestamp
	)

set values for member variables related to cooperative behavior

Parameters

req_trajectory	requested trajectory
req_speed	speed of requested cooperative behavior
req_planning_time	planning time for the requested cooperative behavior
req_timestamp	the mobility request time stamp

Definition at line 241 of file yield_plugin.cpp.

  {
    req_trajectory_points_ = req_trajectory;
    req_target_speed_ = req_speed;
    req_target_plan_time_ = req_planning_time;
    RCLCPP_DEBUG_STREAM(nh_->get_logger(),"req_target_plan_time_" << req_target_plan_time_);
    req_timestamp_ = req_timestamp;
  }

References nh_, req_target_plan_time_, req_target_speed_, req_timestamp_, and req_trajectory_points_.

Referenced by mobilityrequest_cb().

Here is the caller graph for this function:

update_route_llt_cache()

void yield_plugin::YieldPlugin::update_route_llt_cache

(

)

Rebuild the cached route lanelet polygons and IDs from the current route. Should be called once whenever the route changes via setRouteCallback().

Definition at line 1523 of file yield_plugin.cpp.

  {
    if (!wm_->getRoute())
    {
      RCLCPP_WARN(nh_->get_logger(), "update_route_llt_cache called but route is not available");
      return;
    }
    route_llt_ids_.clear();
    route_llt_polygons_.clear();
    for (const auto& llt : wm_->getRoute()->shortestPath())
    {
      route_llt_ids_.insert(llt.id());
      route_llt_polygons_.push_back(llt);
    }
  }

References nh_, route_llt_ids_, route_llt_polygons_, and wm_.

Referenced by get_earliest_collision_object_and_time().

Here is the caller graph for this function:

update_traj_for_cooperative_behavior()

carma_planning_msgs::msg::TrajectoryPlan yield_plugin::YieldPlugin::update_traj_for_cooperative_behavior	(	const carma_planning_msgs::msg::TrajectoryPlan &	original_tp,
		double	current_speed
	)

update trajectory for yielding to an incoming cooperative behavior

Parameters

original_tp	original trajectory plan
current_speed	current speed of the vehicle

Returns

updated trajectory for cooperative behavior

Definition at line 384 of file yield_plugin.cpp.

  {
    carma_planning_msgs::msg::TrajectoryPlan cooperative_trajectory;
 
    double initial_pos = 0;
    double goal_pos;
    double initial_velocity = current_speed;
    double goal_velocity = req_target_speed_;
    double planning_time = req_target_plan_time_;
 
    std::vector<lanelet::BasicPoint2d> host_traj_points = {};
    for (size_t i=0; i<original_tp.trajectory_points.size(); i++)
    {
      lanelet::BasicPoint2d traj_point;
      traj_point.x() = original_tp.trajectory_points.at(i).x;
      traj_point.y() = original_tp.trajectory_points.at(i).y;
      host_traj_points.push_back(traj_point);
    }
 
    std::vector<std::pair<int, lanelet::BasicPoint2d>> intersection_points = detect_trajectories_intersection(host_traj_points, req_trajectory_points_);
    if (!intersection_points.empty())
    {
      lanelet::BasicPoint2d intersection_point = intersection_points[0].second;
      double dx = original_tp.trajectory_points[0].x - intersection_point.x();
      double dy = original_tp.trajectory_points[0].y - intersection_point.y();
      // check if a digital_gap is available
      double digital_gap = check_traj_for_digital_min_gap(original_tp);
      RCLCPP_DEBUG_STREAM(nh_->get_logger(),"digital_gap: " << digital_gap);
      goal_pos = sqrt(dx*dx + dy*dy) - std::max(config_.minimum_safety_gap_in_meters, digital_gap);
      RCLCPP_DEBUG_STREAM(nh_->get_logger(),"Goal position (goal_pos): " << goal_pos);
      double collision_time = req_timestamp_ + (intersection_points[0].first * ecef_traj_timestep_) - config_.safety_collision_time_gap_in_s;
      RCLCPP_DEBUG_STREAM(nh_->get_logger(),"req time stamp: " << req_timestamp_);
      RCLCPP_DEBUG_STREAM(nh_->get_logger(),"Collision time: " << collision_time);
      RCLCPP_DEBUG_STREAM(nh_->get_logger(),"intersection num: " << intersection_points[0].first);
      RCLCPP_DEBUG_STREAM(nh_->get_logger(),"Planning time: " << planning_time);
      // calculate distance traveled from beginning of trajectory to collision point
      double dx2 = intersection_point.x() - req_trajectory_points_[0].x();
      double dy2 = intersection_point.y() - req_trajectory_points_[0].y();
      // calculate incoming trajectory speed from time and distance between trajectory points
      double incoming_trajectory_speed = sqrt(dx2*dx2 + dy2*dy2)/(intersection_points[0].first * ecef_traj_timestep_);
      // calculate goal velocity from request trajectory
      goal_velocity = std::min(goal_velocity, incoming_trajectory_speed);
      double min_time = (initial_velocity - goal_velocity)/config_.yield_max_deceleration_in_ms2;
 
      RCLCPP_DEBUG_STREAM(nh_->get_logger(),"goal_velocity: " << goal_velocity);
      RCLCPP_DEBUG_STREAM(nh_->get_logger(),"incoming_trajectory_speed: " << incoming_trajectory_speed);
 
      if (planning_time > min_time)
      {
        cooperative_request_acceptable_ = true;
        double original_max_speed = max_trajectory_speed(original_tp.trajectory_points, get_trajectory_end_time(original_tp));
        cooperative_trajectory = generate_JMT_trajectory(original_tp, initial_pos, goal_pos, initial_velocity, goal_velocity, planning_time, original_max_speed);
      }
      else
      {
        cooperative_request_acceptable_ = false;
        RCLCPP_DEBUG(nh_->get_logger(),"The incoming requested trajectory is rejected, due to insufficient gap");
        cooperative_trajectory = original_tp;
      }
 
    }
    else
    {
      cooperative_request_acceptable_ = true;
      RCLCPP_DEBUG(nh_->get_logger(),"The incoming requested trajectory does not overlap with host vehicle's trajectory");
      cooperative_trajectory = original_tp;
    }
 
    return cooperative_trajectory;
  }

References check_traj_for_digital_min_gap(), config_, cooperative_request_acceptable_, detect_trajectories_intersection(), visualize_xodr::dx, visualize_xodr::dy, ecef_traj_timestep_, generate_JMT_trajectory(), yield_plugin::get_trajectory_end_time(), process_bag::i, max_trajectory_speed(), YieldPluginConfig::minimum_safety_gap_in_meters, nh_, req_target_plan_time_, req_target_speed_, req_timestamp_, req_trajectory_points_, YieldPluginConfig::safety_collision_time_gap_in_s, and YieldPluginConfig::yield_max_deceleration_in_ms2.

Referenced by plan_trajectory_callback().

Here is the call graph for this function:

Here is the caller graph for this function:

update_traj_for_object()

carma_planning_msgs::msg::TrajectoryPlan yield_plugin::YieldPlugin::update_traj_for_object	(	const carma_planning_msgs::msg::TrajectoryPlan &	original_tp,
		const std::vector< carma_perception_msgs::msg::ExternalObject > &	external_objects,
		double	initial_velocity
	)

trajectory is modified to safely avoid obstacles on the road

Parameters

original_tp	original trajectory plan without object avoidance
current_speed_	current speed of the vehicle

return modified trajectory plan

Definition at line 1264 of file yield_plugin.cpp.

  {
    if (original_tp.trajectory_points.size() < 2)
    {
      RCLCPP_WARN(nh_->get_logger(), "Yield plugin received less than 2 points in update_traj_for_object, returning unchanged...");
      return original_tp;
    }
 
    // Get earliest collision object
    auto _t0_ect = std::chrono::steady_clock::now();
    const auto earliest_collision_obj_pair = get_earliest_collision_object_and_time(original_tp, external_objects);
    const double _ect_ms = std::chrono::duration<double, std::milli>(std::chrono::steady_clock::now() - _t0_ect).count();
    RCLCPP_DEBUG_STREAM(rclcpp::get_logger("yield_plugin"),
      "[timing] get_earliest_collision_object_and_time: " << _ect_ms << " ms");
 
    if (!earliest_collision_obj_pair)
    {
      RCLCPP_DEBUG(nh_->get_logger(),"No collision detected, so trajectory not modified.");
      return original_tp;
    }
 
    carma_perception_msgs::msg::ExternalObject earliest_collision_obj = earliest_collision_obj_pair.value().first;
    double earliest_collision_time_in_seconds = earliest_collision_obj_pair.value().second;
 
    // Issue (https://github.com/usdot-fhwa-stol/carma-platform/issues/2155): If the yield_plugin can detect if the roadway object is moving along the route,
    // it is able to plan yielding much earlier and smoother using on_route_vehicle_collision_horizon_in_s.
 
    const lanelet::BasicPoint2d vehicle_point(original_tp.trajectory_points[0].x,original_tp.trajectory_points[0].y);
    auto _rtp_t0_upd = std::chrono::steady_clock::now();
    const double vehicle_downtrack = wm_->routeTrackPos(vehicle_point).downtrack;
 
    RCLCPP_DEBUG_STREAM(nh_->get_logger(),"vehicle_downtrack: " << vehicle_downtrack);
 
    RCLCPP_WARN_STREAM(nh_->get_logger(),"Collision Detected!");
 
    const lanelet::BasicPoint2d object_point(earliest_collision_obj.pose.pose.position.x, earliest_collision_obj.pose.pose.position.y);
    const double object_downtrack = wm_->routeTrackPos(object_point).downtrack;
 
    RCLCPP_DEBUG_STREAM(nh_->get_logger(),"object_downtrack: " << object_downtrack);
    const double _rtp_ms_upd = std::chrono::duration<double, std::milli>(
      std::chrono::steady_clock::now() - _rtp_t0_upd).count();
    RCLCPP_DEBUG_STREAM(rclcpp::get_logger("yield_plugin"),
      "[update_traj] routeTrackPos x2: " << _rtp_ms_upd << " ms");
 
    const double object_downtrack_lead = std::max(0.0, object_downtrack - vehicle_downtrack);
    RCLCPP_DEBUG_STREAM(nh_->get_logger(),"object_downtrack_lead: " << object_downtrack_lead);
 
    // The vehicle's goal velocity of the yielding behavior is to match the velocity of the object along the trajectory.
    double goal_velocity = get_predicted_velocity_at_time(earliest_collision_obj.velocity.twist, original_tp, earliest_collision_time_in_seconds);
    RCLCPP_DEBUG_STREAM(nh_->get_logger(),"object's speed along trajectory at collision: " << goal_velocity);
 
    // roadway object position
    const double gap_time_until_min_gap_distance = std::max(0.0, object_downtrack_lead - config_.minimum_safety_gap_in_meters)/initial_velocity;
 
    if (goal_velocity <= config_.obstacle_zero_speed_threshold_in_ms){
      RCLCPP_WARN_STREAM(nh_->get_logger(),"The obstacle is not moving, goal velocity is set to 0 from: " << goal_velocity);
      goal_velocity = 0.0;
    }
 
    // determine the safety inter-vehicle gap based on speed
    double safety_gap = std::max(goal_velocity * gap_time_until_min_gap_distance, config_.minimum_safety_gap_in_meters);
    if (!std::isnormal(safety_gap))
    {
      RCLCPP_WARN_STREAM(rclcpp::get_logger("yield_plugin"),"Detected non-normal (nan, inf, etc.) safety_gap."
        "Making it desired safety gap configured at config_.minimum_safety_gap_in_meters: " << config_.minimum_safety_gap_in_meters);
      safety_gap = config_.minimum_safety_gap_in_meters;
    }
    if (config_.enable_adjustable_gap)
    {
      // externally_commanded_safety_gap is desired distance gap commanded from external sources
      // such as different plugin, map, or infrastructure depending on the use case
      auto _t0_gap = std::chrono::steady_clock::now();
      double externally_commanded_safety_gap = check_traj_for_digital_min_gap(original_tp);
      const double _gap_ms = std::chrono::duration<double, std::milli>(std::chrono::steady_clock::now() - _t0_gap).count();
      RCLCPP_DEBUG_STREAM(rclcpp::get_logger("yield_plugin"),
        "[timing] check_traj_for_digital_min_gap: " << _gap_ms << " ms");
      RCLCPP_DEBUG_STREAM(nh_->get_logger(),"externally_commanded_safety_gap: " << externally_commanded_safety_gap);
      // if a digital gap is available, it is replaced as safety gap
      safety_gap = std::max(safety_gap, externally_commanded_safety_gap);
    }
 
    const double goal_pos = std::max(0.0, object_downtrack_lead - safety_gap - config_.vehicle_length);
    const double initial_pos = 0.0; //relative initial position (first trajectory point)
    const double original_max_speed = max_trajectory_speed(original_tp.trajectory_points, earliest_collision_time_in_seconds);
    const double delta_v_max = fabs(goal_velocity - original_max_speed);
    RCLCPP_DEBUG_STREAM(nh_->get_logger(),"delta_v_max: " << delta_v_max << ", safety_gap: " << safety_gap);
 
    const double time_required_for_comfortable_decel_in_s = config_.acceleration_adjustment_factor * 2 * goal_pos / delta_v_max;
    const double min_time_required_for_comfortable_decel_in_s = delta_v_max / config_.yield_max_deceleration_in_ms2;
 
    // planning time for object avoidance
    double planning_time_in_s = std::max({config_.min_obj_avoidance_plan_time_in_s, time_required_for_comfortable_decel_in_s, min_time_required_for_comfortable_decel_in_s});
    RCLCPP_DEBUG_STREAM(nh_->get_logger(),"time_required_for_comfortable_decel_in_s: " << time_required_for_comfortable_decel_in_s << ", min_time_required_for_comfortable_decel_in_s: " << min_time_required_for_comfortable_decel_in_s);
 
    RCLCPP_DEBUG_STREAM(nh_->get_logger(),"Object avoidance planning time: " << planning_time_in_s);
 
    auto _t0_jmt = std::chrono::steady_clock::now();
    auto jmt_trajectory = generate_JMT_trajectory(original_tp,
      initial_pos, goal_pos, initial_velocity, goal_velocity,
      planning_time_in_s, original_max_speed);
    const double _jmt_ms = std::chrono::duration<double, std::milli>(std::chrono::steady_clock::now() - _t0_jmt).count();
    RCLCPP_DEBUG_STREAM(rclcpp::get_logger("yield_plugin"),
      "[timing] generate_JMT_trajectory: " << _jmt_ms << " ms");
 
    // If expected to stop to prevent collision, we should save this trajectory to commit to it
    if (!last_traj_plan_committed_to_stopping_.has_value() &&
        goal_velocity < EPSILON &&
        earliest_collision_time_in_seconds - nh_->now().seconds()
        <= config_.time_horizon_until_collision_to_commit_to_stop_in_s)
    {
      last_traj_plan_committed_to_stopping_ = jmt_trajectory;
    }
    return jmt_trajectory;
  }

References YieldPluginConfig::acceleration_adjustment_factor, check_traj_for_digital_min_gap(), config_, YieldPluginConfig::enable_adjustable_gap, EPSILON, generate_JMT_trajectory(), get_earliest_collision_object_and_time(), get_predicted_velocity_at_time(), last_traj_plan_committed_to_stopping_, max_trajectory_speed(), YieldPluginConfig::min_obj_avoidance_plan_time_in_s, YieldPluginConfig::minimum_safety_gap_in_meters, nh_, YieldPluginConfig::obstacle_zero_speed_threshold_in_ms, YieldPluginConfig::time_horizon_until_collision_to_commit_to_stop_in_s, YieldPluginConfig::vehicle_length, wm_, and YieldPluginConfig::yield_max_deceleration_in_ms2.

Referenced by plan_trajectory_callback().

Here is the call graph for this function:

Here is the caller graph for this function:

Member Data Documentation

clc_urgency_

int yield_plugin::YieldPlugin::clc_urgency_ = 0

private

Definition at line 377 of file yield_plugin.hpp.

Referenced by mobilityrequest_cb(), and plan_trajectory_callback().

config_

YieldPluginConfig yield_plugin::YieldPlugin::config_

private

Definition at line 358 of file yield_plugin.hpp.

Referenced by compose_mobility_response(), detect_trajectories_intersection(), generate_JMT_trajectory(), get_collision(), get_collision_time(), get_collision_times_concurrently_cuda(), is_object_behind_vehicle(), mobilityrequest_cb(), plan_trajectory_callback(), update_traj_for_cooperative_behavior(), and update_traj_for_object().

consecutive_clearance_count_for_obstacles_

std::unordered_map<uint32_t, int> yield_plugin::YieldPlugin::consecutive_clearance_count_for_obstacles_

private

Definition at line 367 of file yield_plugin.hpp.

Referenced by get_collision_time(), get_collision_times_concurrently_cuda(), and is_object_behind_vehicle().

cooperative_request_acceptable_

bool yield_plugin::YieldPlugin::cooperative_request_acceptable_ = false

private

Definition at line 369 of file yield_plugin.hpp.

Referenced by mobilityrequest_cb(), and update_traj_for_cooperative_behavior().

current_speed_

double yield_plugin::YieldPlugin::current_speed_

private

Definition at line 387 of file yield_plugin.hpp.

ecef_traj_timestep_

double yield_plugin::YieldPlugin::ecef_traj_timestep_ = 0.1

private

Definition at line 384 of file yield_plugin.hpp.

Referenced by update_traj_for_cooperative_behavior().

external_objects_

std::vector<carma_perception_msgs::msg::ExternalObject> yield_plugin::YieldPlugin::external_objects_

private

Definition at line 366 of file yield_plugin.hpp.

Referenced by plan_trajectory_callback(), and set_external_objects().

first_time_stopped_to_prevent_collision_

std::optional<rclcpp::Time> yield_plugin::YieldPlugin::first_time_stopped_to_prevent_collision_ = std::nullopt

private

Definition at line 380 of file yield_plugin.hpp.

Referenced by plan_trajectory_callback().

georeference_

std::string yield_plugin::YieldPlugin::georeference_ {""}

private

Definition at line 391 of file yield_plugin.hpp.

Referenced by set_georeference_string().

host_bsm_id_

std::string yield_plugin::YieldPlugin::host_bsm_id_

private

Definition at line 389 of file yield_plugin.hpp.

Referenced by bsm_cb(), and compose_mobility_response().

host_vehicle_size

geometry_msgs::msg::Vector3 yield_plugin::YieldPlugin::host_vehicle_size

private

Definition at line 386 of file yield_plugin.hpp.

last_speed_

std::optional<double> yield_plugin::YieldPlugin::last_speed_ = std::nullopt

private

Definition at line 378 of file yield_plugin.hpp.

Referenced by generate_JMT_trajectory().

last_speed_time_

std::optional<rclcpp::Time> yield_plugin::YieldPlugin::last_speed_time_ = std::nullopt

private

Definition at line 379 of file yield_plugin.hpp.

Referenced by generate_JMT_trajectory().

last_traj_plan_committed_to_stopping_

std::optional< carma_planning_msgs::msg::TrajectoryPlan> yield_plugin::YieldPlugin::last_traj_plan_committed_to_stopping_ = std::nullopt

private

Definition at line 382 of file yield_plugin.hpp.

Referenced by plan_trajectory_callback(), and update_traj_for_object().

lc_status_publisher_

LaneChangeStatusCB yield_plugin::YieldPlugin::lc_status_publisher_

private

Definition at line 360 of file yield_plugin.hpp.

Referenced by mobilityrequest_cb().

map_projector_

std::shared_ptr<lanelet::projection::LocalFrameProjector> yield_plugin::YieldPlugin::map_projector_

private

Definition at line 392 of file yield_plugin.hpp.

Referenced by ecef_to_map_point(), mobilityrequest_cb(), and set_georeference_string().

mobility_response_publisher_

MobilityResponseCB yield_plugin::YieldPlugin::mobility_response_publisher_

private

Definition at line 359 of file yield_plugin.hpp.

Referenced by mobilityrequest_cb().

nh_

std::shared_ptr<carma_ros2_utils::CarmaLifecycleNode> yield_plugin::YieldPlugin::nh_

private

Definition at line 361 of file yield_plugin.hpp.

Referenced by check_traj_for_digital_min_gap(), compose_mobility_response(), generate_JMT_trajectory(), get_collision(), get_collision_time(), get_collision_times_concurrently_cuda(), get_earliest_collision_object_and_time(), get_predicted_velocity_at_time(), is_object_behind_vehicle(), mobilityrequest_cb(), plan_trajectory_callback(), set_incoming_request_info(), update_route_llt_cache(), update_traj_for_cooperative_behavior(), and update_traj_for_object().

previous_llt_id_

lanelet::Id yield_plugin::YieldPlugin::previous_llt_id_

private

Definition at line 365 of file yield_plugin.hpp.

req_target_plan_time_

double yield_plugin::YieldPlugin::req_target_plan_time_ = 0

private

Definition at line 375 of file yield_plugin.hpp.

Referenced by set_incoming_request_info(), and update_traj_for_cooperative_behavior().

req_target_speed_

double yield_plugin::YieldPlugin::req_target_speed_ = 0

private

Definition at line 373 of file yield_plugin.hpp.

Referenced by set_incoming_request_info(), and update_traj_for_cooperative_behavior().

req_timestamp_

double yield_plugin::YieldPlugin::req_timestamp_ = 0

private

Definition at line 374 of file yield_plugin.hpp.

Referenced by set_incoming_request_info(), and update_traj_for_cooperative_behavior().

req_trajectory_points_

std::vector<lanelet::BasicPoint2d> yield_plugin::YieldPlugin::req_trajectory_points_

private

Definition at line 372 of file yield_plugin.hpp.

Referenced by set_incoming_request_info(), and update_traj_for_cooperative_behavior().

route_llt_ids_

std::set<lanelet::Id> yield_plugin::YieldPlugin::route_llt_ids_

private

Definition at line 362 of file yield_plugin.hpp.

Referenced by update_route_llt_cache().

route_llt_polygons_

std::vector<lanelet::ConstLanelet> yield_plugin::YieldPlugin::route_llt_polygons_

private

Definition at line 364 of file yield_plugin.hpp.

Referenced by find_on_route_in_predictions(), get_collision(), get_earliest_collision_object_and_time(), and update_route_llt_cache().

timesteps_since_last_req_

int yield_plugin::YieldPlugin::timesteps_since_last_req_ = 0

private

Definition at line 376 of file yield_plugin.hpp.

Referenced by mobilityrequest_cb(), and plan_trajectory_callback().

wm_

carma_wm::WorldModelConstPtr yield_plugin::YieldPlugin::wm_

private

Definition at line 357 of file yield_plugin.hpp.

Referenced by check_traj_for_digital_min_gap(), get_collision_time(), get_collision_times_concurrently_cuda(), get_earliest_collision_object_and_time(), update_route_llt_cache(), and update_traj_for_object().

---

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

• yield_plugin/include/yield_plugin/yield_plugin.hpp
• yield_plugin/src/yield_plugin.cpp