yield_plugin Namespace Reference

Classes
struct	GetCollisionResult
	Convenience class for saving collision results. More...
struct	PointSpeedPair
	Convenience class for pairing 2d points with speeds. More...
class	YieldPlugin
	Class containing primary business logic for the In-Lane Cruising Plugin. More...
class	YieldPluginNode
	ROS node for the YieldPlugin. More...

Typedefs
using	MobilityResponseCB = std::function< void(const carma_v2x_msgs::msg::MobilityResponse &)>
using	LaneChangeStatusCB = std::function< void(const carma_planning_msgs::msg::LaneChangeStatus &)>

Functions
def	generate_launch_description ()
double	get_trajectory_end_time (const carma_planning_msgs::msg::TrajectoryPlan &trajectory)
double	get_trajectory_start_time (const carma_planning_msgs::msg::TrajectoryPlan &trajectory)
double	get_trajectory_duration (const carma_planning_msgs::msg::TrajectoryPlan &trajectory)
double	get_trajectory_duration (const std::vector< carma_perception_msgs::msg::PredictedState > &trajectory)
double	get_smallest_time_step_of_traj (const carma_planning_msgs::msg::TrajectoryPlan &original_tp)
static lanelet::BasicPoint2d	interp_trajectory_pt_at_time (double query_time, const std::vector< CudaPoint > &ego_points, int num_ego_points, const carma_planning_msgs::msg::TrajectoryPlan &trajectory_plan)
static lanelet::BasicPoint2d	interp_predicted_pt_at_time (double query_time, const std::vector< carma_perception_msgs::msg::PredictedState > &predictions, int start_index)

Typedef Documentation

LaneChangeStatusCB

using yield_plugin::LaneChangeStatusCB = typedef std::function<void(const carma_planning_msgs::msg::LaneChangeStatus&)>

Definition at line 52 of file yield_plugin.hpp.

MobilityResponseCB

using yield_plugin::MobilityResponseCB = typedef std::function<void(const carma_v2x_msgs::msg::MobilityResponse&)>

Definition at line 51 of file yield_plugin.hpp.

Function Documentation

generate_launch_description()

def yield_plugin.generate_launch_description

(

)

Definition at line 31 of file yield_plugin.launch.py.

def generate_launch_description():
 
    # Declare the log_level launch argument
    log_level = LaunchConfiguration('log_level')
    declare_log_level_arg = DeclareLaunchArgument(
        name ='log_level', default_value='DEBUG')
    
    # Get parameter file path
    param_file_path = os.path.join(
        get_package_share_directory('yield_plugin'), 'config/parameters.yaml')
 
    # Launch node(s) in a carma container to allow logging to be configured
    container = ComposableNodeContainer(
        package='carma_ros2_utils',
        name='yield_plugin_container',
        namespace=GetCurrentNamespace(),
        executable='carma_component_container_mt',
        composable_node_descriptions=[
            
            # Launch the core node(s)
            ComposableNode(
                    package='yield_plugin',
                    plugin='yield_plugin::YieldPluginNode',
                    name='yield_plugin',
                    extra_arguments=[
                        {'use_intra_process_comms': True},
                        {'--log-level' : log_level }
                    ],
                    parameters=[ param_file_path ]
            ),
        ]
    )
 
    return LaunchDescription([
        declare_log_level_arg,
        container
    ])

get_smallest_time_step_of_traj()

double yield_plugin::get_smallest_time_step_of_traj

(

const carma_planning_msgs::msg::TrajectoryPlan &

original_tp

)

Definition at line 455 of file yield_plugin.cpp.

  {
    double smallest_time_step = std::numeric_limits<double>::infinity();
    for (size_t i = 0; i < original_tp.trajectory_points.size() - 1; i ++)
    {
      smallest_time_step = std::min(smallest_time_step,
        (rclcpp::Time(original_tp.trajectory_points.at(i + 1).target_time)
        - rclcpp::Time(original_tp.trajectory_points.at(i).target_time)).seconds());
    }
    RCLCPP_DEBUG_STREAM(rclcpp::get_logger("yield_plugin"),"smallest_time_step: " << smallest_time_step);
 
    return smallest_time_step;
  }

References process_bag::i.

Referenced by yield_plugin::YieldPlugin::generate_JMT_trajectory().

Here is the caller graph for this function:

get_trajectory_duration() [1/2]

double yield_plugin::get_trajectory_duration

(

const carma_planning_msgs::msg::TrajectoryPlan &

trajectory

)

Definition at line 67 of file yield_plugin.cpp.

  {
    return fabs(get_trajectory_end_time(trajectory) - get_trajectory_start_time(trajectory));
  }

References get_trajectory_end_time(), and get_trajectory_start_time().

Referenced by yield_plugin::YieldPlugin::get_collision().

Here is the call graph for this function:

Here is the caller graph for this function:

get_trajectory_duration() [2/2]

double yield_plugin::get_trajectory_duration

(

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

trajectory

)

Definition at line 72 of file yield_plugin.cpp.

  {
    return (rclcpp::Time(trajectory.back().header.stamp) - rclcpp::Time(trajectory.front().header.stamp)).seconds();
  }

get_trajectory_end_time()

double yield_plugin::get_trajectory_end_time

(

const carma_planning_msgs::msg::TrajectoryPlan &

trajectory

)

Definition at line 57 of file yield_plugin.cpp.

  {
    return rclcpp::Time(trajectory.trajectory_points.back().target_time).seconds();
  }

Referenced by yield_plugin::YieldPlugin::get_earliest_collision_object_and_time(), yield_plugin::YieldPlugin::get_predicted_velocity_at_time(), get_trajectory_duration(), yield_plugin::YieldPlugin::plan_trajectory_callback(), and yield_plugin::YieldPlugin::update_traj_for_cooperative_behavior().

Here is the caller graph for this function:

get_trajectory_start_time()

double yield_plugin::get_trajectory_start_time

(

const carma_planning_msgs::msg::TrajectoryPlan &

trajectory

)

Definition at line 62 of file yield_plugin.cpp.

  {
    return rclcpp::Time(trajectory.trajectory_points.front().target_time).seconds();
  }

Referenced by yield_plugin::YieldPlugin::get_collision_time(), yield_plugin::YieldPlugin::get_collision_times_concurrently_cuda(), and get_trajectory_duration().

Here is the caller graph for this function:

interp_predicted_pt_at_time()

static lanelet::BasicPoint2d yield_plugin::interp_predicted_pt_at_time ( double  query_time, const std::vector< carma_perception_msgs::msg::PredictedState > &  predictions, int  start_index  )

static

Definition at line 887 of file yield_plugin.cpp.

  {
    lanelet::BasicPoint2d result(
      predictions.front().predicted_position.position.x,
      predictions.front().predicted_position.position.y);
    for (int j = start_index; j < static_cast<int>(predictions.size()) - 1; ++j) {
      const double seg_start_time = rclcpp::Time(predictions[j].header.stamp).seconds();
      const double seg_end_time = rclcpp::Time(predictions[j + 1].header.stamp).seconds();
      if (seg_start_time <= query_time && query_time <= seg_end_time) {
        const double interp_ratio = (seg_end_time > seg_start_time) ? (query_time - seg_start_time) / (seg_end_time - seg_start_time) : 0.0;
        result.x() = predictions[j].predicted_position.position.x +
                     interp_ratio * (predictions[j+1].predicted_position.position.x - predictions[j].predicted_position.position.x);
        result.y() = predictions[j].predicted_position.position.y +
                     interp_ratio * (predictions[j+1].predicted_position.position.y - predictions[j].predicted_position.position.y);
        break;
      }
    }
    return result;
  }

References process_traj_logs::start_index.

Referenced by yield_plugin::YieldPlugin::get_collision_times_concurrently_cuda().

Here is the caller graph for this function:

interp_trajectory_pt_at_time()

static lanelet::BasicPoint2d yield_plugin::interp_trajectory_pt_at_time ( double  query_time, const std::vector< CudaPoint > &  ego_points, int  num_ego_points, const carma_planning_msgs::msg::TrajectoryPlan &  trajectory_plan  )

static

Definition at line 867 of file yield_plugin.cpp.

  {
    lanelet::BasicPoint2d result(ego_points[0].x, ego_points[0].y);
    for (int i = 0; i < num_ego_points - 1; ++i) {
      const double seg_start_time = rclcpp::Time(trajectory_plan.trajectory_points[i].target_time).seconds();
      const double seg_end_time = rclcpp::Time(trajectory_plan.trajectory_points[i + 1].target_time).seconds();
      if (seg_start_time <= query_time && query_time <= seg_end_time) {
        const double interp_ratio = (seg_end_time > seg_start_time) ? (query_time - seg_start_time) / (seg_end_time - seg_start_time) : 0.0;
        result.x() = trajectory_plan.trajectory_points[i].x + interp_ratio * (trajectory_plan.trajectory_points[i+1].x - trajectory_plan.trajectory_points[i].x);
        result.y() = trajectory_plan.trajectory_points[i].y + interp_ratio * (trajectory_plan.trajectory_points[i+1].y - trajectory_plan.trajectory_points[i].y);
        break;
      }
    }
    return result;
  }

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

Referenced by yield_plugin::YieldPlugin::get_collision_times_concurrently_cuda().

Here is the caller graph for this function: