inlanecruising_plugin::InLaneCruisingPlugin Class Reference

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

#include <inlanecruising_plugin.hpp>

Collaboration diagram for inlanecruising_plugin::InLaneCruisingPlugin:

Public Member Functions
	InLaneCruisingPlugin (std::shared_ptr< carma_ros2_utils::CarmaLifecycleNode > nh, carma_wm::WorldModelConstPtr wm, const InLaneCruisingPluginConfig &config, const DebugPublisher &debug_publisher=[](const auto &msg){}, const std::string &plugin_name="inlanecruising_plugin", const std::string &version_id="v1.0")
	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...
void	set_yield_client (carma_ros2_utils::ClientPtr< carma_planning_msgs::srv::PlanTrajectory > client)
	set the yield service More...

Public Attributes
carma_planning_msgs::msg::VehicleState	ending_state_before_buffer_

Private Member Functions
	FRIEND_TEST (InLaneCruisingPluginTest, rostest1)

Private Attributes
std::string	plugin_name_
std::string	version_id_
carma_wm::WorldModelConstPtr	wm_
InLaneCruisingPluginConfig	config_
carma_ros2_utils::ClientPtr< carma_planning_msgs::srv::PlanTrajectory >	yield_client_
DebugPublisher	debug_publisher_
carma_debug_ros2_msgs::msg::TrajectoryCurvatureSpeeds	debug_msg_
std::shared_ptr< carma_ros2_utils::CarmaLifecycleNode >	nh_

Detailed Description

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

Definition at line 50 of file inlanecruising_plugin.hpp.

Constructor & Destructor Documentation

InLaneCruisingPlugin()

inlanecruising_plugin::InLaneCruisingPlugin::InLaneCruisingPlugin	(	std::shared_ptr< carma_ros2_utils::CarmaLifecycleNode >	nh,
		carma_wm::WorldModelConstPtr	wm,
		const InLaneCruisingPluginConfig &	config,
		const DebugPublisher &	debug_publisher = [](const auto& msg){},
		const std::string &	plugin_name = "inlanecruising_plugin",
		const std::string &	version_id = "v1.0"
	)

Constructor.

Parameters

nh	Pointer to the lifecyle node
wm	Pointer to intialized instance of the carma world model for accessing semantic map data
config	The configuration to be used for this object
debug_publisher	Callback which will publish a debug message. The callback defaults to no-op.
plugin_name	Retrieved from the plugin node
version_id	Retrieved from the plugin node

Definition at line 40 of file inlanecruising_plugin.cpp.

  : nh_(nh), wm_(wm), config_(config), debug_publisher_(debug_publisher), plugin_name_(plugin_name), version_id_ (version_id)
{}

Member Function Documentation

FRIEND_TEST()

inlanecruising_plugin::InLaneCruisingPlugin::FRIEND_TEST ( InLaneCruisingPluginTest  , rostest1    )

private

plan_trajectory_callback()

void inlanecruising_plugin::InLaneCruisingPlugin::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 49 of file inlanecruising_plugin.cpp.

{
  std::chrono::system_clock::time_point start_time = std::chrono::system_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)
    {
      maneuver_plan.push_back(req->maneuver_plan.maneuvers[i]);
      resp->related_maneuvers.push_back((uint8_t)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("inlanecruising",
                                                                              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);
 
  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(nh_->get_logger(), "points_and_target_speeds: " << points_and_target_speeds.size());
 
  RCLCPP_DEBUG_STREAM(nh_->get_logger(), "PlanTrajectory");
 
  carma_planning_msgs::msg::TrajectoryPlan original_trajectory;
  original_trajectory.header.frame_id = "map";
  original_trajectory.header.stamp = nh_->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);
 
  // Set the planning plugin field name
  for (auto& p : original_trajectory.trajectory_points) {
    p.planner_plugin_name = plugin_name_;
  }
 
  resp->trajectory_plan = original_trajectory;
 
  // Aside from the flag, ILC should not call yield_plugin on invalid trajectories
  if (config_.enable_object_avoidance && original_trajectory.trajectory_points.size() >= 2)
  {
    basic_autonomy::waypoint_generation::modify_trajectory_to_yield_to_obstacles(nh_, req, resp, yield_client_, config_.tactical_plugin_service_call_timeout);
  }
  else
  {
    RCLCPP_DEBUG(nh_->get_logger(), "Ignored Object Avoidance");
  }
 
  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);
 
  std::chrono::system_clock::time_point end_time = std::chrono::system_clock::now();  // Planning complete
 
  auto duration = end_time - start_time;
  RCLCPP_DEBUG_STREAM(nh_->get_logger(), "ExecutionTime: " << std::chrono::duration<double>(duration).count());
}

References InLaneCruisingPluginConfig::back_distance, InLaneCruisingPluginConfig::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(), InLaneCruisingPluginConfig::curvature_moving_average_window_size, InLaneCruisingPluginConfig::curve_resample_step_size, debug_msg_, debug_publisher_, InLaneCruisingPluginConfig::default_downsample_ratio, InLaneCruisingPluginConfig::enable_object_avoidance, ending_state_before_buffer_, process_bag::i, InLaneCruisingPluginConfig::lat_accel_multiplier, InLaneCruisingPluginConfig::lateral_accel_limit, InLaneCruisingPluginConfig::max_accel, InLaneCruisingPluginConfig::max_accel_multiplier, InLaneCruisingPluginConfig::minimum_speed, basic_autonomy::waypoint_generation::modify_trajectory_to_yield_to_obstacles(), nh_, plugin_name_, InLaneCruisingPluginConfig::publish_debug, InLaneCruisingPluginConfig::speed_moving_average_window_size, InLaneCruisingPluginConfig::tactical_plugin_service_call_timeout, carma_cooperative_perception::to_string(), InLaneCruisingPluginConfig::trajectory_time_length, InLaneCruisingPluginConfig::turn_downsample_ratio, wm_, and yield_client_.

Here is the call graph for this function:

set_yield_client()

void inlanecruising_plugin::InLaneCruisingPlugin::set_yield_client

(

carma_ros2_utils::ClientPtr< carma_planning_msgs::srv::PlanTrajectory >

client

)

set the yield service

Parameters

yield_srv

input yield service

Definition at line 135 of file inlanecruising_plugin.cpp.

{
  yield_client_ = client;
}

References yield_client_.

Member Data Documentation

config_

InLaneCruisingPluginConfig inlanecruising_plugin::InLaneCruisingPlugin::config_

private

Definition at line 94 of file inlanecruising_plugin.hpp.

Referenced by plan_trajectory_callback().

debug_msg_

carma_debug_ros2_msgs::msg::TrajectoryCurvatureSpeeds inlanecruising_plugin::InLaneCruisingPlugin::debug_msg_

private

Definition at line 97 of file inlanecruising_plugin.hpp.

Referenced by plan_trajectory_callback().

debug_publisher_

DebugPublisher inlanecruising_plugin::InLaneCruisingPlugin::debug_publisher_

private

Definition at line 96 of file inlanecruising_plugin.hpp.

Referenced by plan_trajectory_callback().

ending_state_before_buffer_

carma_planning_msgs::msg::VehicleState inlanecruising_plugin::InLaneCruisingPlugin::ending_state_before_buffer_

Definition at line 87 of file inlanecruising_plugin.hpp.

Referenced by plan_trajectory_callback().

nh_

std::shared_ptr<carma_ros2_utils::CarmaLifecycleNode> inlanecruising_plugin::InLaneCruisingPlugin::nh_

private

Definition at line 98 of file inlanecruising_plugin.hpp.

Referenced by plan_trajectory_callback().

plugin_name_

std::string inlanecruising_plugin::InLaneCruisingPlugin::plugin_name_

private

Definition at line 91 of file inlanecruising_plugin.hpp.

Referenced by plan_trajectory_callback().

version_id_

std::string inlanecruising_plugin::InLaneCruisingPlugin::version_id_

private

Definition at line 92 of file inlanecruising_plugin.hpp.

wm_

carma_wm::WorldModelConstPtr inlanecruising_plugin::InLaneCruisingPlugin::wm_

private

Definition at line 93 of file inlanecruising_plugin.hpp.

Referenced by plan_trajectory_callback().

yield_client_

carma_ros2_utils::ClientPtr<carma_planning_msgs::srv::PlanTrajectory> inlanecruising_plugin::InLaneCruisingPlugin::yield_client_

private

Definition at line 95 of file inlanecruising_plugin.hpp.

Referenced by plan_trajectory_callback(), and set_yield_client().

---

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

• inlanecruising_plugin/include/inlanecruising_plugin/inlanecruising_plugin.hpp
• inlanecruising_plugin/src/inlanecruising_plugin.cpp