platooning_tactical_plugin::PlatooningTacticalPlugin Class Reference

Class containing primary business logic for the Platooning Tactical Plugin for trajectory generation. More...

#include <platooning_tactical_plugin.h>

Collaboration diagram for platooning_tactical_plugin::PlatooningTacticalPlugin:

Public Member Functions
	PlatooningTacticalPlugin (carma_wm::WorldModelConstPtr wm, PlatooningTacticalPluginConfig config, std::shared_ptr< carma_ros2_utils::timers::TimerFactory > timer_factory)
	Constructor. More...
bool	plan_trajectory_cb (carma_planning_msgs::srv::PlanTrajectory::Request &req, carma_planning_msgs::srv::PlanTrajectory::Response &resp)
	Service callback for trajectory planning. More...
void	set_config (PlatooningTacticalPluginConfig config)
	Set the current config. More...

Public Attributes
carma_planning_msgs::msg::VehicleState	ending_state_before_buffer_

Private Attributes
carma_wm::WorldModelConstPtr	wm_
PlatooningTacticalPluginConfig	config_
carma_debug_ros2_msgs::msg::TrajectoryCurvatureSpeeds	debug_msg_
DebugPublisher	debug_publisher_
carma_planning_msgs::msg::VehicleState	ending_state_before_buffer
std::shared_ptr< carma_ros2_utils::timers::TimerFactory >	timer_factory_

Detailed Description

Class containing primary business logic for the Platooning Tactical Plugin for trajectory generation.

Definition at line 50 of file platooning_tactical_plugin.h.

Constructor & Destructor Documentation

PlatooningTacticalPlugin()

platooning_tactical_plugin::PlatooningTacticalPlugin::PlatooningTacticalPlugin	(	carma_wm::WorldModelConstPtr	wm,
		PlatooningTacticalPluginConfig	config,
		std::shared_ptr< carma_ros2_utils::timers::TimerFactory >	timer_factory
	)

Constructor.

Parameters

wm	Pointer to initalized instance of the carma world model for accessing semantic map data
config	The configuration to be used for this object

Definition at line 39 of file platooning_tactical_plugin.cpp.

  : wm_(wm), config_(config), timer_factory_(timer_factory)
{}

Member Function Documentation

plan_trajectory_cb()

bool platooning_tactical_plugin::PlatooningTacticalPlugin::plan_trajectory_cb	(	carma_planning_msgs::srv::PlanTrajectory::Request &	req,
		carma_planning_msgs::srv::PlanTrajectory::Response &	resp
	)

Service callback for trajectory planning.

Parameters

req	The service request
resp	The service response

Returns

True if success. False otherwise

Definition at line 44 of file platooning_tactical_plugin.cpp.

{
  auto start_time = std::chrono::high_resolution_clock::now(); // Start timing the execution time for planning so it can be logged
 
  lanelet::BasicPoint2d veh_pos(req.vehicle_state.x_pos_global, req.vehicle_state.y_pos_global);
  double current_downtrack = wm_->routeTrackPos(veh_pos).downtrack;
 
  // Only plan the trajectory for the initial LANE_FOLLOWING maneuver and any immediately sequential maneuvers of the same type
  std::vector<carma_planning_msgs::msg::Maneuver> maneuver_plan;
  for(size_t i = req.maneuver_index_to_plan; i < req.maneuver_plan.maneuvers.size(); i++)
  {
    if(req.maneuver_plan.maneuvers[i].type == carma_planning_msgs::msg::Maneuver::LANE_FOLLOWING)
    {
      if (req.maneuver_plan.maneuvers[i].lane_following_maneuver.parameters.negotiation_type != carma_planning_msgs::msg::ManeuverParameters::NO_NEGOTIATION)
      {
        maneuver_plan.push_back(req.maneuver_plan.maneuvers[i]);
        resp.related_maneuvers.push_back(i);
      }
    }
    else
    {
      break;
    }
  }
 
  basic_autonomy:: waypoint_generation::DetailedTrajConfig wpg_detail_config;
  basic_autonomy:: waypoint_generation::GeneralTrajConfig wpg_general_config;
 
  wpg_general_config = basic_autonomy:: waypoint_generation::compose_general_trajectory_config("platooning",
                                                                              config_.default_downsample_ratio,
                                                                              config_.turn_downsample_ratio);
  
  wpg_detail_config = basic_autonomy:: waypoint_generation::compose_detailed_trajectory_config(config_.trajectory_time_length, 
                                                                            config_.curve_resample_step_size, config_.minimum_speed, 
                                                                            config_.max_accel * config_.max_accel_multiplier,
                                                                            config_.lateral_accel_limit * config_.lat_accel_multiplier, 
                                                                            config_.speed_moving_average_window_size, 
                                                                            config_.curvature_moving_average_window_size, config_.back_distance,
                                                                            config_.buffer_ending_downtrack,
                                                                            config_.desired_controller_plugin);
  
  auto points_and_target_speeds = basic_autonomy::waypoint_generation::create_geometry_profile(maneuver_plan, std::max((double)0, current_downtrack - config_.back_distance),
                                                                         wm_, ending_state_before_buffer_, req.vehicle_state, wpg_general_config, wpg_detail_config);
 
  RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platooning_tactical_plugin"), "points_and_target_speeds: " << points_and_target_speeds.size());
 
  RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platooning_tactical_plugin"), "PlanTrajectory");
 
  carma_planning_msgs::msg::TrajectoryPlan original_trajectory;
  original_trajectory.header.frame_id = "map";
  original_trajectory.header.stamp = timer_factory_->now();
  original_trajectory.trajectory_id = boost::uuids::to_string(boost::uuids::random_generator()());
  original_trajectory.trajectory_points = basic_autonomy:: waypoint_generation::compose_lanefollow_trajectory_from_path(points_and_target_speeds, 
                                                                                req.vehicle_state, req.header.stamp, wm_, ending_state_before_buffer_, debug_msg_, 
                                                                                wpg_detail_config); // Compute the trajectory
  original_trajectory.initial_longitudinal_velocity = std::max(req.vehicle_state.longitudinal_vel, config_.minimum_speed);
 
  resp.trajectory_plan = original_trajectory;
 
  if (config_.publish_debug) { // Publish the debug message if in debug logging mode
    debug_msg_.trajectory_plan = resp.trajectory_plan;
    debug_publisher_(debug_msg_); 
  }
  
  resp.maneuver_status.push_back(carma_planning_msgs::srv::PlanTrajectory::Response::MANEUVER_IN_PROGRESS);
 
  auto end_time = std::chrono::high_resolution_clock::now(); // Planning complete
 
  auto nano_s = std::chrono::duration_cast<std::chrono::nanoseconds>(end_time - start_time);
  RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platooning_tactical_plugin"), "ExecutionTime: " << ((double)nano_s.count() * 1e9));
 
  return true;
 
}

References PlatooningTacticalPluginConfig::back_distance, PlatooningTacticalPluginConfig::buffer_ending_downtrack, basic_autonomy::waypoint_generation::compose_detailed_trajectory_config(), basic_autonomy::waypoint_generation::compose_general_trajectory_config(), basic_autonomy::waypoint_generation::compose_lanefollow_trajectory_from_path(), config_, basic_autonomy::waypoint_generation::create_geometry_profile(), PlatooningTacticalPluginConfig::curvature_moving_average_window_size, PlatooningTacticalPluginConfig::curve_resample_step_size, debug_msg_, debug_publisher_, PlatooningTacticalPluginConfig::default_downsample_ratio, PlatooningTacticalPluginConfig::desired_controller_plugin, ending_state_before_buffer_, process_bag::i, PlatooningTacticalPluginConfig::lat_accel_multiplier, PlatooningTacticalPluginConfig::lateral_accel_limit, PlatooningTacticalPluginConfig::max_accel, PlatooningTacticalPluginConfig::max_accel_multiplier, PlatooningTacticalPluginConfig::minimum_speed, PlatooningTacticalPluginConfig::publish_debug, PlatooningTacticalPluginConfig::speed_moving_average_window_size, timer_factory_, carma_cooperative_perception::to_string(), PlatooningTacticalPluginConfig::trajectory_time_length, PlatooningTacticalPluginConfig::turn_downsample_ratio, and wm_.

Here is the call graph for this function:

set_config()

void platooning_tactical_plugin::PlatooningTacticalPlugin::set_config

(

PlatooningTacticalPluginConfig

config

)

Set the current config.

Definition at line 120 of file platooning_tactical_plugin.cpp.

{
  config_ = config;
}

References config_.

Member Data Documentation

config_

PlatooningTacticalPluginConfig platooning_tactical_plugin::PlatooningTacticalPlugin::config_

private

Definition at line 82 of file platooning_tactical_plugin.h.

Referenced by plan_trajectory_cb(), and set_config().

debug_msg_

carma_debug_ros2_msgs::msg::TrajectoryCurvatureSpeeds platooning_tactical_plugin::PlatooningTacticalPlugin::debug_msg_

private

Definition at line 84 of file platooning_tactical_plugin.h.

Referenced by plan_trajectory_cb().

debug_publisher_

DebugPublisher platooning_tactical_plugin::PlatooningTacticalPlugin::debug_publisher_

private

Definition at line 85 of file platooning_tactical_plugin.h.

Referenced by plan_trajectory_cb().

ending_state_before_buffer

carma_planning_msgs::msg::VehicleState platooning_tactical_plugin::PlatooningTacticalPlugin::ending_state_before_buffer

private

Definition at line 87 of file platooning_tactical_plugin.h.

ending_state_before_buffer_

carma_planning_msgs::msg::VehicleState platooning_tactical_plugin::PlatooningTacticalPlugin::ending_state_before_buffer_

Definition at line 78 of file platooning_tactical_plugin.h.

Referenced by plan_trajectory_cb().

timer_factory_

std::shared_ptr<carma_ros2_utils::timers::TimerFactory> platooning_tactical_plugin::PlatooningTacticalPlugin::timer_factory_

private

Definition at line 89 of file platooning_tactical_plugin.h.

Referenced by plan_trajectory_cb().

wm_

carma_wm::WorldModelConstPtr platooning_tactical_plugin::PlatooningTacticalPlugin::wm_

private

Definition at line 81 of file platooning_tactical_plugin.h.

Referenced by plan_trajectory_cb().

---

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

• platooning_tactical_plugin/include/platooning_tactical_plugin/platooning_tactical_plugin.h
• platooning_tactical_plugin/src/platooning_tactical_plugin.cpp