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...

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_
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 41 of file inlanecruising_plugin.cpp.

  : nh_(nh), wm_(wm), config_(config), debug_publisher_(debug_publisher), plugin_name_(plugin_name), version_id_ (version_id)
{
  basic_autonomy::set_logger(nh_->get_logger().get_child("basic_autonomy"));
}

References nh_, and basic_autonomy::set_logger().

Here is the call graph for this function:

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 52 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;
 
  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(
    rclcpp::get_logger("inlanecruising_plugin"),
    "ILC 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, ending_state_before_buffer_, basic_autonomy::get_logger(), process_bag::i, InLaneCruisingPluginConfig::lat_accel_multiplier, InLaneCruisingPluginConfig::lateral_accel_limit, InLaneCruisingPluginConfig::max_accel, InLaneCruisingPluginConfig::max_accel_multiplier, InLaneCruisingPluginConfig::minimum_speed, nh_, plugin_name_, InLaneCruisingPluginConfig::publish_debug, InLaneCruisingPluginConfig::speed_moving_average_window_size, carma_cooperative_perception::to_string(), InLaneCruisingPluginConfig::trajectory_time_length, InLaneCruisingPluginConfig::turn_downsample_ratio, and wm_.

Here is the call graph for this function:

Member Data Documentation

config_

InLaneCruisingPluginConfig inlanecruising_plugin::InLaneCruisingPlugin::config_

private

Definition at line 87 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 89 of file inlanecruising_plugin.hpp.

Referenced by plan_trajectory_callback().

debug_publisher_

DebugPublisher inlanecruising_plugin::InLaneCruisingPlugin::debug_publisher_

private

Definition at line 88 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 80 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 90 of file inlanecruising_plugin.hpp.

Referenced by InLaneCruisingPlugin(), and plan_trajectory_callback().

plugin_name_

std::string inlanecruising_plugin::InLaneCruisingPlugin::plugin_name_

private

Definition at line 84 of file inlanecruising_plugin.hpp.

Referenced by plan_trajectory_callback().

version_id_

std::string inlanecruising_plugin::InLaneCruisingPlugin::version_id_

private

Definition at line 85 of file inlanecruising_plugin.hpp.

wm_

carma_wm::WorldModelConstPtr inlanecruising_plugin::InLaneCruisingPlugin::wm_

private

Definition at line 86 of file inlanecruising_plugin.hpp.

Referenced by plan_trajectory_callback().

---

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