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...
std::optional< GetCollisionResult >	get_collision (const carma_planning_msgs::msg::TrajectoryPlan &trajectory1, const std::vector< carma_perception_msgs::msg::PredictedState > &trajectory2, double collision_radius, double trajectory1_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::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_
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 46 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 247 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 346 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 1139 of file yield_plugin.cpp.

  {
    double desired_gap = 0;
 
    for (size_t i = 0; i < original_tp.trajectory_points.size(); i++)
    {
      lanelet::BasicPoint2d veh_pos(original_tp.trajectory_points.at(i).x, original_tp.trajectory_points.at(i).y);
      auto llts = wm_->getLaneletsFromPoint(veh_pos, 1);
      if (llts.empty())
      {
        // This should technically never happen
        // However, trajectory generation currently may fail due to osm map issue https://github.com/usdot-fhwa-stol/carma-platform/issues/2503
        RCLCPP_WARN_STREAM(nh_->get_logger(),"Trajectory point: x= " << original_tp.trajectory_points.at(i).x << "y="<< original_tp.trajectory_points.at(i).y);
        RCLCPP_WARN_STREAM(nh_->get_logger(),"Trajectory is not on the road, so was unable to get the digital minimum gap. Returning default minimum_safety_gap_in_meters: " << config_.minimum_safety_gap_in_meters);
        return desired_gap;
      }
      auto digital_min_gap = llts[0].regulatoryElementsAs<lanelet::DigitalMinimumGap>(); //Returns a list of these elements)
      if (!digital_min_gap.empty())
      {
        double digital_gap = digital_min_gap[0]->getMinimumGap(); // Provided gap is in meters
        RCLCPP_DEBUG_STREAM(nh_->get_logger(),"Digital Gap found with value: " << digital_gap);
        desired_gap = std::max(desired_gap, digital_gap);
      }
    }
    return desired_gap;
  }

References config_, process_bag::i, YieldPluginConfig::minimum_safety_gap_in_meters, 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 134 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 95 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

trajectory1	vector of 2d trajectory points
trajectory2	vector of 2d trajectory points

Returns

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

Definition at line 74 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 120 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:

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 462 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]);
    }
    const auto smallest_time_step = get_smallest_time_step_of_traj(original_tp);
    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);
 
      RCLCPP_DEBUG_STREAM(rclcpp::get_logger("yield_plugin"), "Calculated speed velocity_at_target_time: " << velocity_at_target_time
        << ", downtrack_at_target_time: "<< downtrack_at_target_time << ", target_time: " << 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)
      {
        RCLCPP_DEBUG_STREAM(rclcpp::get_logger("yield_plugin"), "Picked calculated speed velocity_at_target_time: " << velocity_at_target_time
          << ", downtrack_at_target_time: "<< downtrack_at_target_time << ", target_time: " << target_time);
        // 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);
    RCLCPP_DEBUG_STREAM(rclcpp::get_logger("yield_plugin"), "filtered_speeds size: " << filtered_speeds.size());
    for (const auto& speed : filtered_speeds)
    {
      RCLCPP_DEBUG_STREAM(rclcpp::get_logger("yield_plugin"), "filtered speed: " << speed);
    }
    // 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();
    RCLCPP_DEBUG_STREAM(rclcpp::get_logger("yield_plugin"), "start speed: " << prev_speed << ", target_time: " << std::to_string(rclcpp::Time(original_tp.trajectory_points[0].target_time).seconds()));
 
    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);
        RCLCPP_DEBUG_STREAM(rclcpp::get_logger("yield_plugin"), "Zero speed = x: " << jmt_tpp.x << ", y:" << jmt_tpp.y
          << ", t:" << std::to_string(rclcpp::Time(jmt_tpp.target_time).seconds())
          << ", prev_speed: " << prev_speed << ", current_speed: " << current_speed);
      }
      else
      {
        RCLCPP_DEBUG_STREAM(rclcpp::get_logger("yield_plugin"), "Non-zero speed = x: " << jmt_tpp.x << ", y:" << jmt_tpp.y
          << ", t:" << std::to_string(rclcpp::Time(jmt_tpp.target_time).seconds())
          << ", prev_speed: " << prev_speed << ", current_speed: " << current_speed);
      }
 
      jmt_trajectory_points.push_back(jmt_tpp);
      double insta_decel = (current_speed - prev_speed) / (rclcpp::Time(jmt_trajectory_points.at(i).target_time).seconds() - rclcpp::Time(jmt_trajectory_points.at(i - 1).target_time).seconds());
      RCLCPP_DEBUG_STREAM(rclcpp::get_logger("yield_plugin"), "insta_decel: " << insta_decel );
      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(), YieldPluginConfig::speed_moving_average_window_size, and carma_cooperative_perception::to_string().

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 &	trajectory1,
		const std::vector< carma_perception_msgs::msg::PredictedState > &	trajectory2,
		double	collision_radius,
		double	trajectory1_max_speed
	)

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

Parameters

trajectory1	trajectory of the ego vehicle
trajectory2	trajectory of predicted steps
collision_radius	a distance to check between two trajectory points at a same timestamp that is considered a collision
trajectory1_max_speed	max speed of the trajectory1 to efficiently traverse through possible collision combination of the two trajectories NOTE: Currently Traj2 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 636 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: "
      << trajectory1.trajectory_points.size() << ". prediction size: " << trajectory2.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 traj2_speed{std::hypot(trajectory2.front().predicted_velocity.linear.x,
                                  trajectory2.front().predicted_velocity.linear.y)};
    bool traj2_has_zero_speed = traj2_speed < config_.obstacle_zero_speed_threshold_in_ms;
 
    if (trajectory2.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 predict_step_duration = (rclcpp::Time(trajectory2.at(1).header.stamp) - rclcpp::Time(trajectory2.front().header.stamp)).seconds();
    const double predict_total_duration = get_trajectory_duration(trajectory2);
 
    if (predict_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 traj2.
    // Since skipping number of points from the traj2 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(traj2_speed, 2) + pow(trajectory1_max_speed, 2));
    int iteration_stride = std::max(1, static_cast<int>(iteration_stride_max_time_s / predict_step_duration));
 
    RCLCPP_DEBUG_STREAM(nh_->get_logger(), "Determined iteration_stride: " << iteration_stride
      << ", with traj2_speed: " << traj2_speed
      << ", with trajectory1_max_speed: " << trajectory1_max_speed
      << ", with predict_step_duration: " << predict_step_duration
      << ", iteration_stride_max_time_s: " << iteration_stride_max_time_s);
 
    for (size_t j = 0; j < trajectory2.size(); j += iteration_stride) // Saving computation time aiming for 1.5 meter interval
    {
      lanelet::BasicPoint2d curr_point;
      curr_point.x() = trajectory2.at(j).predicted_position.position.x;
      curr_point.y() = trajectory2.at(j).predicted_position.position.y;
 
      auto corresponding_lanelets = wm_->getLaneletsFromPoint(curr_point, 8); // some intersection can have 8 overlapping lanelets
 
      for (const auto& llt: corresponding_lanelets)
      {
        RCLCPP_DEBUG_STREAM(nh_->get_logger(), "Checking llt: " << llt.id());
 
        if (route_llt_ids_.find(llt.id()) != route_llt_ids_.end())
        {
          on_route = true;
          on_route_idx = j;
          break;
        }
      }
      if (on_route || traj2_has_zero_speed)
        break;
    }
 
    if (!on_route)
    {
      RCLCPP_DEBUG(nh_->get_logger(), "Predicted states are not on the route! ignoring");
      return std::nullopt;
    }
 
    double smallest_dist = std::numeric_limits<double>::infinity();
    for (size_t i = 0; i < trajectory1.trajectory_points.size() - 1; ++i)
    {
      auto p1a = trajectory1.trajectory_points.at(i);
      auto p1b = trajectory1.trajectory_points.at(i + 1);
      double previous_distance_between_predictions = std::numeric_limits<double>::infinity();
      for (size_t j = on_route_idx; j < trajectory2.size() - 1; j += iteration_stride)
      {
        auto p2a = trajectory2.at(j);
        auto p2b = trajectory2.at(j + 1);
        double p1a_t = rclcpp::Time(p1a.target_time).seconds();
        double p1b_t = rclcpp::Time(p1b.target_time).seconds();
        double p2a_t = rclcpp::Time(p2a.header.stamp).seconds();
        double p2b_t = rclcpp::Time(p2b.header.stamp).seconds();
 
        RCLCPP_DEBUG_STREAM(nh_->get_logger(), "p1a.target_time: " << std::to_string(p1a_t) << ", p1b.target_time: " << std::to_string(p1b_t));
        RCLCPP_DEBUG_STREAM(nh_->get_logger(), "p2a.target_time: " << std::to_string(p2a_t) << ", p2b.target_time: " << std::to_string(p2b_t));
        RCLCPP_DEBUG_STREAM(nh_->get_logger(), "p1a.x: " << p1a.x << ", p1a.y: " << p1a.y);
        RCLCPP_DEBUG_STREAM(nh_->get_logger(), "p1b.x: " << p1b.x << ", p1b.y: " << p1b.y);
 
        RCLCPP_DEBUG_STREAM(nh_->get_logger(), "p2a.x: " << p2a.predicted_position.position.x << ", p2a.y: " << p2a.predicted_position.position.y);
        RCLCPP_DEBUG_STREAM(nh_->get_logger(), "p2b.x: " << p2b.predicted_position.position.x << ", p2b.y: " << p2b.predicted_position.position.y);
 
        // Linearly interpolate positions at a common timestamp for both trajectories
        double dt = (p2a_t - p1a_t) / (p1b_t - p1a_t);
        // if negative extrapolation, skip because car wouldn't go backwards
        if (dt < 0)
        {
          continue;
        }
        double x1 = p1a.x + dt * (p1b.x - p1a.x);
        double y1 = p1a.y + dt * (p1b.y - p1a.y);
        double x2 = p2a.predicted_position.position.x;
        double y2 = p2a.predicted_position.position.y;
 
        // Calculate the distance between the two interpolated points
        const auto distance{std::hypot(x1 - x2, y1 - y2)};
 
        smallest_dist = std::min(distance, smallest_dist);
 
        // Following "if logic" assumes the traj2 is a simple cv model, aka, traj2 point is a straight line over time.
        // And current traj1 point is fixed in this iteration.
        // Then once the distance between the two start to increase over traj2 iteration,
        // the distance will always increase and it's unnecessary to continue the logic to find the smallest_dist
        if (previous_distance_between_predictions < distance)
        {
          RCLCPP_DEBUG_STREAM(nh_->get_logger(), "Stopping search here because the distance between predictions started to increase");
          break;
        }
        previous_distance_between_predictions = 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.point1 = lanelet::BasicPoint2d(x1,y1);
        collision_result.point2 = lanelet::BasicPoint2d(x2,y2);
        collision_result.collision_time = rclcpp::Time(p2a.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::get_trajectory_duration(), process_bag::i, YieldPluginConfig::intervehicle_collision_distance_in_m, nh_, YieldPluginConfig::obstacle_zero_speed_threshold_in_ms, yield_plugin::GetCollisionResult::point1, yield_plugin::GetCollisionResult::point2, route_llt_ids_, carma_cooperative_perception::to_string(), and wm_.

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

trajectory1	trajectory of the ego vehicle
trajectory2	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 814 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;
      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().point1).downtrack;
    const double object_downtrack = wm_->routeTrackPos(collision_result.value().point2).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().point1.x() - collision_result.value().point2.x(),
      collision_result.value().point1.y() - collision_result.value().point2.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().point1.x() << ", y: " << collision_result.value().point1.y() <<
      ", within actual downtrack distance: " << object_downtrack - vehicle_downtrack <<
      ", and collision distance: " << distance);
 
    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().

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 874 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;
 
    // Launch asynchronous tasks to check for collision times
    for (const auto& object : external_objects) {
      futures[object.id] = std::async(std::launch::async,[this, &original_tp, &object, &original_tp_max_speed]{
          return get_collision_time(original_tp, object, original_tp_max_speed);
        });
    }
 
    // Collect results from futures and update collision_times
    for (const auto& object : external_objects) {
      if (const auto collision_time{futures.at(object.id).get()}) {
        collision_times[object.id] = collision_time.value();
      }
    }
 
    return collision_times;
  }

References get_collision_time().

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_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 898 of file yield_plugin.cpp.

  {
    RCLCPP_DEBUG_STREAM(nh_->get_logger(), "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;
    }
 
    // save route Ids for faster access
    for (const auto& llt: wm_->getRoute()->shortestPath())
    {
      // TODO: Enhancement https://github.com/usdot-fhwa-stol/carma-platform/issues/2316
      route_llt_ids_.insert(llt.id());
    }
 
    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));
    std::unordered_map<uint32_t, rclcpp::Time> collision_times = get_collision_times_concurrently(original_tp,external_objects, original_max_speed);
 
    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_ids_, 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 936 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 1079 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 788 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().

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 1116 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 154 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 253 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");
        yield_trajectory = update_traj_for_object(original_trajectory, external_objects_, initial_velocity); // Compute the trajectory
      }
 
 
      // 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 1094 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 1105 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 1175 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 1166 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 238 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_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 377 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 979 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
    const auto earliest_collision_obj_pair = get_earliest_collision_object_and_time(original_tp, external_objects);
 
    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);
    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 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
      double externally_commanded_safety_gap = check_traj_for_digital_min_gap(original_tp);
      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 jmt_trajectory = generate_JMT_trajectory(original_tp,
      initial_pos, goal_pos, initial_velocity, goal_velocity,
      planning_time_in_s, original_max_speed);
 
    // 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 329 of file yield_plugin.hpp.

Referenced by mobilityrequest_cb(), and plan_trajectory_callback().

config_

YieldPluginConfig yield_plugin::YieldPlugin::config_

private

Definition at line 312 of file yield_plugin.hpp.

Referenced by check_traj_for_digital_min_gap(), compose_mobility_response(), detect_trajectories_intersection(), generate_JMT_trajectory(), get_collision(), get_collision_time(), 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 319 of file yield_plugin.hpp.

Referenced by get_collision_time(), and is_object_behind_vehicle().

cooperative_request_acceptable_

bool yield_plugin::YieldPlugin::cooperative_request_acceptable_ = false

private

Definition at line 321 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 339 of file yield_plugin.hpp.

ecef_traj_timestep_

double yield_plugin::YieldPlugin::ecef_traj_timestep_ = 0.1

private

Definition at line 336 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 318 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 332 of file yield_plugin.hpp.

Referenced by plan_trajectory_callback().

georeference_

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

private

Definition at line 343 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 341 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 338 of file yield_plugin.hpp.

last_speed_

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

private

Definition at line 330 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 331 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 334 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 314 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 344 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 313 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 315 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_earliest_collision_object_and_time(), get_predicted_velocity_at_time(), is_object_behind_vehicle(), mobilityrequest_cb(), plan_trajectory_callback(), set_incoming_request_info(), 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 317 of file yield_plugin.hpp.

req_target_plan_time_

double yield_plugin::YieldPlugin::req_target_plan_time_ = 0

private

Definition at line 327 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 325 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 326 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 324 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 316 of file yield_plugin.hpp.

Referenced by get_collision(), and get_earliest_collision_object_and_time().

timesteps_since_last_req_

int yield_plugin::YieldPlugin::timesteps_since_last_req_ = 0

private

Definition at line 328 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 311 of file yield_plugin.hpp.

Referenced by check_traj_for_digital_min_gap(), get_collision(), get_collision_time(), get_earliest_collision_object_and_time(), 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