The class responsible for generating cooperative lanechange trajectories from received lane change maneuvers. More...

#include <cooperative_lanechange_node.hpp>

Inheritance diagram for cooperative_lanechange::CooperativeLaneChangePlugin:

Collaboration diagram for cooperative_lanechange::CooperativeLaneChangePlugin:

Public Member Functions
	CooperativeLaneChangePlugin (const rclcpp::NodeOptions &)
	CooperativeLaneChangePlugin constructor. More...

rcl_interfaces::msg::SetParametersResult	parameter_update_callback (const std::vector< rclcpp::Parameter > &parameters)
	Callback for dynamic parameter updates. More...

std::vector< carma_planning_msgs::msg::TrajectoryPlanPoint >	plan_lanechange (carma_planning_msgs::srv::PlanTrajectory::Request::SharedPtr req)
	Creates a vector of Trajectory Points from maneuver information in trajectory request. More...

double	find_current_gap (long veh2_lanelet_id, double veh2_downtrack, carma_planning_msgs::msg::VehicleState &ego_state) const
	Calculates distance between subject vehicle and vehicle 2. More...

void	mobilityresponse_cb (const carma_v2x_msgs::msg::MobilityResponse::UniquePtr msg)
	Callback to subscribed mobility response topic. More...

carma_v2x_msgs::msg::MobilityRequest	create_mobility_request (std::vector< carma_planning_msgs::msg::TrajectoryPlanPoint > &trajectory_plan, carma_planning_msgs::msg::Maneuver &maneuver)
	Creates a mobility request message from planned trajectory and requested maneuver info. More...

carma_v2x_msgs::msg::Trajectory	trajectory_plan_to_trajectory (const std::vector< carma_planning_msgs::msg::TrajectoryPlanPoint > &traj_points) const
	Converts Trajectory Plan to (Mobility) Trajectory. More...

carma_v2x_msgs::msg::LocationECEF	trajectory_point_to_ecef (const carma_planning_msgs::msg::TrajectoryPlanPoint &traj_point) const
	Converts Trajectory Point to ECEF frame using map projection. More...

void	add_trajectory_to_response (carma_planning_msgs::srv::PlanTrajectory::Request::SharedPtr req, carma_planning_msgs::srv::PlanTrajectory::Response::SharedPtr resp, const std::vector< carma_planning_msgs::msg::TrajectoryPlanPoint > &planned_trajectory_points)
	Adds the generated trajectory plan to the service response. More...

void	georeference_cb (const std_msgs::msg::String::UniquePtr msg)
	Callback for map projection string to define lat/lon -> map conversion. More...

void	plan_trajectory_callback (std::shared_ptr< rmw_request_id_t >, carma_planning_msgs::srv::PlanTrajectory::Request::SharedPtr req, carma_planning_msgs::srv::PlanTrajectory::Response::SharedPtr resp) override
	Extending class provided callback which should return a planned trajectory based on the provided trajectory planning request. More...

bool	get_availability () override
	Get the availability status of this plugin based on the current operating environment. Method must be overriden by extending classes. More...

std::string	get_version_id () override
	Returns the version id of this plugin. More...

carma_ros2_utils::CallbackReturn	on_configure_plugin ()
	This method should be used to load parameters and will be called on the configure state transition. More...

	FRIEND_TEST (CooperativeLaneChangePlugin, TestLaneChangefunctions)

	FRIEND_TEST (CooperativeLaneChangePlugin, Testcurrentgapcb)

	FRIEND_TEST (CooperativeLaneChangePlugin, TestNoPath_roadwayobject)

Public Member Functions inherited from carma_guidance_plugins::TacticalPlugin
	TacticalPlugin (const rclcpp::NodeOptions &)
	TacticalPlugin constructor. More...

virtual	~TacticalPlugin ()=default
	Virtual destructor for safe deletion. More...

virtual void	plan_trajectory_callback (std::shared_ptr< rmw_request_id_t > srv_header, carma_planning_msgs::srv::PlanTrajectory::Request::SharedPtr req, carma_planning_msgs::srv::PlanTrajectory::Response::SharedPtr resp)=0
	Extending class provided callback which should return a planned trajectory based on the provided trajectory planning request. More...

std::string	get_capability () override
	Get the capability string representing this plugins capabilities Method must be overriden by extending classes. Expectation is that abstract plugin type parent classes will provide a default implementation. More...

uint8_t	get_type () override final
	Returns the type of this plugin according to the carma_planning_msgs::Plugin type enum. Extending classes for the specific type should override this method. More...

carma_ros2_utils::CallbackReturn	handle_on_configure (const rclcpp_lifecycle::State &) override final

carma_ros2_utils::CallbackReturn	handle_on_activate (const rclcpp_lifecycle::State &) override final

carma_ros2_utils::CallbackReturn	handle_on_deactivate (const rclcpp_lifecycle::State &) override final

carma_ros2_utils::CallbackReturn	handle_on_cleanup (const rclcpp_lifecycle::State &) override final

carma_ros2_utils::CallbackReturn	handle_on_shutdown (const rclcpp_lifecycle::State &) override final

carma_ros2_utils::CallbackReturn	handle_on_error (const rclcpp_lifecycle::State &, const std::string &exception_string) override final

Public Member Functions inherited from carma_guidance_plugins::PluginBaseNode
	PluginBaseNode (const rclcpp::NodeOptions &)
	PluginBaseNode constructor. More...

virtual	~PluginBaseNode ()=default
	Virtual destructor for safe deletion. More...

virtual std::shared_ptr< carma_wm::WMListener >	get_world_model_listener () final
	Method to return the default world model listener provided as a convience by this base class If this method or get_world_model() are not called then the world model remains uninitialized and will not create unnecessary subscriptions. More...

virtual carma_wm::WorldModelConstPtr	get_world_model () final
	Method to return the default world model provided as a convience by this base class If this method or get_world_model_listener() are not called then the world model remains uninitialized and will not create unnecessary subscriptions. More...

virtual bool	get_activation_status () final
	Returns the activation status of this plugin. The plugins API callbacks will only be triggered when this method returns true. More...

virtual uint8_t	get_type ()
	Returns the type of this plugin according to the carma_planning_msgs::Plugin type enum. Extending classes for the specific type should override this method. More...

std::string	get_plugin_name_and_ns () const
	Return the name of this plugin with namespace. NOTE: If only the name of the plugin is required, use get_plugin_name() More...

std::string	get_plugin_name () const
	Return the name of this plugin. More...

virtual bool	get_availability ()=0
	Get the availability status of this plugin based on the current operating environment. Method must be overriden by extending classes. More...

virtual std::string	get_capability ()=0
	Get the capability string representing this plugins capabilities Method must be overriden by extending classes. Expectation is that abstract plugin type parent classes will provide a default implementation. More...

virtual std::string	get_version_id ()=0
	Returns the version id of this plugin. More...

virtual carma_ros2_utils::CallbackReturn	on_configure_plugin ()=0
	Method which is triggered when this plugin is moved from the UNCONFIGURED to INACTIVE states. This method should be used to load parameters and is required to be implemented. More...

virtual carma_ros2_utils::CallbackReturn	on_activate_plugin ()
	Method which is triggered when this plugin is moved from the INACTIVE to ACTIVE states. This method should be used to prepare for future callbacks for plugin's capabilites. More...

virtual carma_ros2_utils::CallbackReturn	on_deactivate_plugin ()
	Method which is triggered when this plugin is moved from the ACTIVE to INACTIVE states. This method should be used to disable any functionality which should cease execution when plugin is inactive. More...

virtual carma_ros2_utils::CallbackReturn	on_cleanup_plugin ()
	Method which is triggered when this plugin is moved from the INACTIVE to UNCONFIGURED states. This method should be used to fully reset the plugin such that a future call to on_configure_plugin would leave the plugin in a fresh state as though just launched. More...

virtual carma_ros2_utils::CallbackReturn	on_shutdown_plugin ()
	Method which is triggered when this plugin is moved from any state to FINALIZED This method should be used to generate any shutdown logs or final cleanup. More...

virtual carma_ros2_utils::CallbackReturn	on_error_plugin (const std::string &exception_string)
	Method which is triggered when an unhandled exception occurs in this plugin This method should be used to cleanup such that the plugin could be moved to UNCONFIGURED state if possible. More...

carma_ros2_utils::CallbackReturn	handle_on_configure (const rclcpp_lifecycle::State &) override

carma_ros2_utils::CallbackReturn	handle_on_activate (const rclcpp_lifecycle::State &) override

carma_ros2_utils::CallbackReturn	handle_on_deactivate (const rclcpp_lifecycle::State &) override

carma_ros2_utils::CallbackReturn	handle_on_cleanup (const rclcpp_lifecycle::State &) override

carma_ros2_utils::CallbackReturn	handle_on_shutdown (const rclcpp_lifecycle::State &) override

carma_ros2_utils::CallbackReturn	handle_on_error (const rclcpp_lifecycle::State &, const std::string &exception_string) override

	FRIEND_TEST (carma_guidance_plugins_test, connections_test)

Public Attributes
double	current_speed_

geometry_msgs::msg::PoseStamped	pose_msg_

bool	is_lanechange_accepted_ = false

carma_planning_msgs::msg::VehicleState	ending_state_before_buffer_

Private Member Functions
void	pose_cb (const geometry_msgs::msg::PoseStamped::UniquePtr msg)
	Callback for the pose subscriber, which will store latest pose locally. More...

void	twist_cb (const geometry_msgs::msg::TwistStamped::UniquePtr msg)
	Callback for the twist subscriber, which will store latest twist locally. More...

void	bsm_cb (const carma_v2x_msgs::msg::BSM::UniquePtr msg)
	Callback for the BSM subscriber, which will store the latest BSM Core Data broadcasted by the host vehicle. More...

std::string	bsmIDtoString (carma_v2x_msgs::msg::BSMCoreData bsm_core)
	Method for extracting the BSM ID from a BSM Core Data object and converting it to a string. More...

Private Attributes
carma_ros2_utils::SubPtr< geometry_msgs::msg::PoseStamped >	pose_sub_

carma_ros2_utils::SubPtr< geometry_msgs::msg::TwistStamped >	twist_sub_

carma_ros2_utils::SubPtr< carma_v2x_msgs::msg::MobilityResponse >	incoming_mobility_response_sub_

carma_ros2_utils::SubPtr< std_msgs::msg::String >	georeference_sub_

carma_ros2_utils::SubPtr< carma_v2x_msgs::msg::BSM >	bsm_sub_

carma_ros2_utils::PubPtr< carma_v2x_msgs::msg::MobilityRequest >	outgoing_mobility_request_pub_

carma_ros2_utils::PubPtr< carma_planning_msgs::msg::LaneChangeStatus >	lanechange_status_pub_

Config	config_

carma_wm::WorldModelConstPtr	wm_

std::string	map_georeference_ {""}

std::shared_ptr< lanelet::projection::LocalFrameProjector >	map_projector_

double	traj_freq_ = 10

std::string	DEFAULT_STRING_ = ""

rclcpp::Time	request_sent_time_

bool	request_sent_ = false

double	maneuver_fraction_completed_ = 0

bool	clc_called_ = false

std::string	clc_request_id_ = "default_request_id"

carma_v2x_msgs::msg::BSMCoreData	bsm_core_

std::unordered_map< std::string, LaneChangeManeuverOriginalValues >	original_lc_maneuver_values_

Detailed Description

The class responsible for generating cooperative lanechange trajectories from received lane change maneuvers.

Definition at line 67 of file cooperative_lanechange_node.hpp.

Constructor & Destructor Documentation

◆ CooperativeLaneChangePlugin()

cooperative_lanechange::CooperativeLaneChangePlugin::CooperativeLaneChangePlugin ( const rclcpp::NodeOptions & options )

explicit

CooperativeLaneChangePlugin constructor.

Definition at line 23 of file cooperative_lanechange_node.cpp.

      : carma_guidance_plugins::TacticalPlugin(options)
  {
    basic_autonomy::set_logger(get_logger().get_child("basic_autonomy"));
    // Create initial config
    config_ = Config();
 
    // Declare parameters
    config_.trajectory_time_length = declare_parameter<double>("trajectory_time_length", config_.trajectory_time_length);
    config_.control_plugin_name = declare_parameter<std::string>("control_plugin_name", config_.control_plugin_name);
    config_.minimum_speed = declare_parameter<double>("minimum_speed", config_.minimum_speed);
    config_.max_accel = declare_parameter<double>("max_accel", config_.max_accel);
    config_.minimum_lookahead_distance = declare_parameter<double>("minimum_lookahead_distance", config_.minimum_lookahead_distance);
    config_.maximum_lookahead_distance = declare_parameter<double>("maximum_lookahead_distance", config_.maximum_lookahead_distance);
    config_.minimum_lookahead_speed = declare_parameter<double>("minimum_lookahead_speed", config_.minimum_lookahead_speed);
    config_.maximum_lookahead_speed = declare_parameter<double>("maximum_lookahead_speed", config_.maximum_lookahead_speed);
    config_.lateral_accel_limit = declare_parameter<double>("lateral_accel_limit", config_.lateral_accel_limit);
    config_.speed_moving_average_window_size = declare_parameter<int>("speed_moving_average_window_size", config_.speed_moving_average_window_size);
    config_.curvature_moving_average_window_size = declare_parameter<int>("curvature_moving_average_window_size", config_.curvature_moving_average_window_size);
    config_.curvature_calc_lookahead_count = declare_parameter<int>("curvature_calc_lookahead_count", config_.curvature_calc_lookahead_count);
    config_.downsample_ratio = declare_parameter<int>("downsample_ratio", config_.downsample_ratio);
    config_.destination_range = declare_parameter<double>("destination_range", config_.destination_range);
    config_.lanechange_time_out = declare_parameter<double>("lanechange_time_out", config_.lanechange_time_out);
    config_.min_timestep = declare_parameter<double>("min_timestep", config_.min_timestep);
    config_.starting_downtrack_range = declare_parameter<double>("starting_downtrack_range", config_.starting_downtrack_range);
    config_.starting_fraction = declare_parameter<double>("starting_fraction", config_.starting_fraction);
    config_.mid_fraction = declare_parameter<double>("mid_fraction", config_.mid_fraction);
    config_.min_desired_gap = declare_parameter<double>("min_desired_gap", config_.min_desired_gap);
    config_.desired_time_gap = declare_parameter<double>("desired_time_gap", config_.desired_time_gap);
    config_.turn_downsample_ratio = declare_parameter<int>("turn_downsample_ratio", config_.turn_downsample_ratio);
    config_.curve_resample_step_size = declare_parameter<double>("curve_resample_step_size", config_.curve_resample_step_size);
    config_.back_distance = declare_parameter<double>("back_distance", config_.back_distance);
    config_.buffer_ending_downtrack = declare_parameter<double>("buffer_ending_downtrack", config_.buffer_ending_downtrack);
    config_.vehicle_id = declare_parameter<std::string>("vehicle_id", config_.vehicle_id);
  }

Here is the call graph for this function:

Member Function Documentation

◆ add_trajectory_to_response()

void cooperative_lanechange::CooperativeLaneChangePlugin::add_trajectory_to_response	(	carma_planning_msgs::srv::PlanTrajectory::Request::SharedPtr	req,
		carma_planning_msgs::srv::PlanTrajectory::Response::SharedPtr	resp,
		const std::vector< carma_planning_msgs::msg::TrajectoryPlanPoint > &	planned_trajectory_points
	)

Adds the generated trajectory plan to the service response.

Parameters

req	The plan trajectory service request
resp	The plan trajectory service response
planned_trajectory_points	The generated trajectory plan points, which are added to the service response

Definition at line 448 of file cooperative_lanechange_node.cpp.

  {
    carma_planning_msgs::msg::TrajectoryPlan trajectory_plan;
    trajectory_plan.header.frame_id = "map";
    trajectory_plan.header.stamp = this->now();
    trajectory_plan.trajectory_id = boost::uuids::to_string(boost::uuids::random_generator()());
 
    trajectory_plan.trajectory_points = planned_trajectory_points;
    trajectory_plan.initial_longitudinal_velocity = std::max(req->vehicle_state.longitudinal_vel, config_.minimum_speed);
    resp->trajectory_plan = trajectory_plan;
 
    resp->related_maneuvers.push_back(req->maneuver_index_to_plan);
 
    resp->maneuver_status.push_back(carma_planning_msgs::srv::PlanTrajectory::Response::MANEUVER_IN_PROGRESS);
  }

References config_, cooperative_lanechange::Config::minimum_speed, and carma_cooperative_perception::to_string().

Referenced by plan_trajectory_callback().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ bsm_cb()

void cooperative_lanechange::CooperativeLaneChangePlugin::bsm_cb ( const carma_v2x_msgs::msg::BSM::UniquePtr msg )

private

Callback for the BSM subscriber, which will store the latest BSM Core Data broadcasted by the host vehicle.

Parameters

msg	Latest BSM message

Definition at line 259 of file cooperative_lanechange_node.cpp.

  {
    bsm_core_ = msg->core_data;
  }

References bsm_core_.

Referenced by on_configure_plugin().

Here is the caller graph for this function:

◆ bsmIDtoString()

std::string cooperative_lanechange::CooperativeLaneChangePlugin::bsmIDtoString ( carma_v2x_msgs::msg::BSMCoreData bsm_core )

private

Method for extracting the BSM ID from a BSM Core Data object and converting it to a string.

Parameters

bsm_core The BSM Core Data object from which the BSM ID is converted to a string

Returns: The BSM ID (as a string) from the BSM Core Data object

Definition at line 666 of file cooperative_lanechange_node.cpp.

  {
    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 create_mobility_request().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ create_mobility_request()

carma_v2x_msgs::msg::MobilityRequest cooperative_lanechange::CooperativeLaneChangePlugin::create_mobility_request	(	std::vector< carma_planning_msgs::msg::TrajectoryPlanPoint > &	trajectory_plan,
		carma_planning_msgs::msg::Maneuver &	maneuver
	)

Creates a mobility request message from planned trajectory and requested maneuver info.

Parameters

trajectory_plan	A vector of lane change trajectory points
The	mobility request message created from trajectory points, for publishing

Definition at line 466 of file cooperative_lanechange_node.cpp.

  {
    carma_v2x_msgs::msg::MobilityRequest request_msg;
    carma_v2x_msgs::msg::MobilityHeader header;
    header.sender_id = config_.vehicle_id;
    header.recipient_id = DEFAULT_STRING_;
    header.sender_bsm_id = bsmIDtoString(bsm_core_);
    header.plan_id = boost::uuids::to_string(boost::uuids::random_generator()());
    clc_request_id_ = header.plan_id;
    header.timestamp = rclcpp::Time(trajectory_plan.front().target_time).nanoseconds() * 1000000;
    request_msg.m_header = header;
 
    request_msg.strategy = "carma/cooperative-lane-change";
    request_msg.plan_type.type = carma_v2x_msgs::msg::PlanType::CHANGE_LANE_LEFT;
 
    //Urgency- Currently unassigned
    int urgency;
    if(maneuver_fraction_completed_ <= config_.starting_fraction){
      urgency = 10;
    }
    else if(maneuver_fraction_completed_ <= config_.mid_fraction){
      urgency = 5;
    }
    else{
      urgency = 1;
    }
    RCLCPP_DEBUG_STREAM(get_logger(), "Maneuver fraction completed:"<<maneuver_fraction_completed_);
    request_msg.urgency = urgency;
 
    //Strategy params
    //Encode JSON with Boost Property Tree
    using boost::property_tree::ptree;
    ptree pt;
    double end_speed_floor = std::floor(maneuver.lane_change_maneuver.end_speed);
    int end_speed_fractional = (maneuver.lane_change_maneuver.end_speed - end_speed_floor) * 10;
 
    RCLCPP_DEBUG_STREAM(get_logger(), "end_speed_floor: " << end_speed_floor);
    RCLCPP_DEBUG_STREAM(get_logger(), "end_speed_fractional: " << end_speed_fractional);
    RCLCPP_DEBUG_STREAM(get_logger(), "start_lanelet_id: " << maneuver.lane_change_maneuver.starting_lane_id);
    RCLCPP_DEBUG_STREAM(get_logger(), "end_lanelet_id: " << maneuver.lane_change_maneuver.ending_lane_id);
 
    pt.put("s",(int)end_speed_floor);
    pt.put("f",end_speed_fractional);
    pt.put("sl",maneuver.lane_change_maneuver.starting_lane_id);
    pt.put("el", maneuver.lane_change_maneuver.ending_lane_id);
 
    std::stringstream body_stream;
    boost::property_tree::json_parser::write_json(body_stream,pt);
    request_msg.strategy_params = body_stream.str();
    RCLCPP_DEBUG_STREAM(get_logger(), "request_msg.strategy_params: " << request_msg.strategy_params);
 
    //Trajectory
    carma_v2x_msgs::msg::Trajectory trajectory;
    if (map_projector_) {
      trajectory = trajectory_plan_to_trajectory(trajectory_plan);
      //Location
      carma_planning_msgs::msg::TrajectoryPlanPoint temp_loc_to_convert;
      temp_loc_to_convert.x = pose_msg_.pose.position.x;
      temp_loc_to_convert.y = pose_msg_.pose.position.y;
      carma_v2x_msgs::msg::LocationECEF location = trajectory_point_to_ecef(temp_loc_to_convert);
 
      //Using trajectory first point time as location timestamp
      location.timestamp = rclcpp::Time(trajectory_plan.front().target_time).nanoseconds() * 1000000;
 
      request_msg.location = location;
    }
    else
    {
      RCLCPP_ERROR_STREAM(get_logger(), "Map projection not available to be used with request message");
    }
 
    request_msg.trajectory = trajectory;
    request_msg.expiration = rclcpp::Time(trajectory_plan.back().target_time).seconds();
    RCLCPP_DEBUG_STREAM(get_logger(), "request_msg.expiration: " << request_msg.expiration << " of which string size: " << std::to_string(request_msg.expiration).size());
 
    return request_msg;
  }

References bsm_core_, bsmIDtoString(), clc_request_id_, config_, DEFAULT_STRING_, basic_autonomy::get_logger(), maneuver_fraction_completed_, map_projector_, cooperative_lanechange::Config::mid_fraction, pose_msg_, cooperative_lanechange::Config::starting_fraction, carma_cooperative_perception::to_string(), trajectory_plan_to_trajectory(), trajectory_point_to_ecef(), and cooperative_lanechange::Config::vehicle_id.

Referenced by plan_trajectory_callback().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ find_current_gap()

double cooperative_lanechange::CooperativeLaneChangePlugin::find_current_gap	(	long	veh2_lanelet_id,
		double	veh2_downtrack,
		carma_planning_msgs::msg::VehicleState &	ego_state
	)		const

Calculates distance between subject vehicle and vehicle 2.

Parameters

veh2_lanelet_id	Current lanelet id of vehicle 2
veh2_downtrack	Downtrack of vehicle 2 in its current lanelet
ego_state	vehicle state of the ego vehicle

Returns: the distance between subject vehicle and vehicle 2

Definition at line 190 of file cooperative_lanechange_node.cpp.

  {
    //find downtrack distance between ego and lag vehicle
    RCLCPP_DEBUG_STREAM(get_logger(), "entered find_current_gap");
    double current_gap = 0.0;
    lanelet::BasicPoint2d ego_pos(ego_state.x_pos_global, ego_state.y_pos_global);
    //double ego_current_downtrack = wm_->routeTrackPos(ego_pos).downtrack;
 
    lanelet::LaneletMapConstPtr const_map(wm_->getMap());
    lanelet::ConstLanelet veh2_lanelet = const_map->laneletLayer.get(veh2_lanelet_id);
    RCLCPP_DEBUG_STREAM(get_logger(), "veh2_lanelet id " << veh2_lanelet.id());
 
    auto current_lanelets = lanelet::geometry::findNearest(const_map->laneletLayer, ego_pos, 10);
    if(current_lanelets.size() == 0)
    {
      RCLCPP_WARN_STREAM(get_logger(), "Cannot find any lanelet in map!");
      return true;
    }
    lanelet::ConstLanelet current_lanelet = current_lanelets[0].second;
    RCLCPP_DEBUG_STREAM(get_logger(), "current llt id " << current_lanelet.id());
 
    //Create temporary route between the two vehicles
    lanelet::ConstLanelet start_lanelet = veh2_lanelet;
    lanelet::ConstLanelet end_lanelet = current_lanelet;
 
    auto map_graph = wm_->getMapRoutingGraph();
    RCLCPP_DEBUG_STREAM(get_logger(), "Graph created");
 
    auto temp_route = map_graph->getRoute(start_lanelet, end_lanelet);
    RCLCPP_DEBUG_STREAM(get_logger(), "Route created");
 
    //Throw exception if there is no shortest path from veh2 to subject vehicle
    lanelet::routing::LaneletPath shortest_path2;
    if(temp_route)
    {
      shortest_path2 = temp_route.get().shortestPath();
    }
    else{
      RCLCPP_ERROR_STREAM(get_logger(), "No path exists from roadway object to subject");
      throw std::invalid_argument("No path exists from roadway object to subject");
    }
 
    RCLCPP_DEBUG_STREAM(get_logger(), "Shorted path created size: " << shortest_path2.size());
    for (auto llt : shortest_path2)
    {
      RCLCPP_DEBUG_STREAM(get_logger(), "llt id  route: " << llt.id());
    }
 
    //To find downtrack- creating temporary route from veh2 to veh1(ego vehicle)
    double veh1_current_downtrack = wm_->routeTrackPos(ego_pos).downtrack;
    RCLCPP_DEBUG_STREAM(get_logger(), "ego_current_downtrack:" << veh1_current_downtrack);
 
    current_gap = veh1_current_downtrack - veh2_downtrack;
    RCLCPP_DEBUG_STREAM(get_logger(), "Finding current gap");
    RCLCPP_DEBUG_STREAM(get_logger(), "Veh1 current downtrack: " << veh1_current_downtrack << " veh2 downtrack: " << veh2_downtrack);
 
    return current_gap;
  }

References basic_autonomy::get_logger(), and wm_.

Referenced by plan_trajectory_callback().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ FRIEND_TEST() [1/3]

cooperative_lanechange::CooperativeLaneChangePlugin::FRIEND_TEST	(	CooperativeLaneChangePlugin	,
		Testcurrentgapcb
	)

◆ FRIEND_TEST() [2/3]

cooperative_lanechange::CooperativeLaneChangePlugin::FRIEND_TEST	(	CooperativeLaneChangePlugin	,
		TestLaneChangefunctions
	)

◆ FRIEND_TEST() [3/3]

cooperative_lanechange::CooperativeLaneChangePlugin::FRIEND_TEST	(	CooperativeLaneChangePlugin	,
		TestNoPath_roadwayobject
	)

◆ georeference_cb()

void cooperative_lanechange::CooperativeLaneChangePlugin::georeference_cb ( const std_msgs::msg::String::UniquePtr msg )

Callback for map projection string to define lat/lon -> map conversion.

msg The proj string defining the projection.

Definition at line 657 of file cooperative_lanechange_node.cpp.

  {
    if (map_georeference_ != msg->data)
    {
      map_georeference_ = msg->data;
      map_projector_ = std::make_shared<lanelet::projection::LocalFrameProjector>(msg->data.c_str()); // Build projector from proj string
    }
  }

References map_georeference_, and map_projector_.

Referenced by on_configure_plugin().

Here is the caller graph for this function:

◆ get_availability()

bool cooperative_lanechange::CooperativeLaneChangePlugin::get_availability ( )

overridevirtual

Get the availability status of this plugin based on the current operating environment. Method must be overriden by extending classes.

Returns: This method should return true if the plugin's current understanding of the world means it would be capable of planning or executing its capability.

Implements carma_guidance_plugins::PluginBaseNode.

Definition at line 675 of file cooperative_lanechange_node.cpp.

                                                     {
    return true;
  }

◆ get_version_id()

std::string cooperative_lanechange::CooperativeLaneChangePlugin::get_version_id ( )

overridevirtual

Returns the version id of this plugin.

Returns: The version id represented as a string

Implements carma_guidance_plugins::PluginBaseNode.

Definition at line 679 of file cooperative_lanechange_node.cpp.

                                                        {
    return "v4.0"; // Version ID matches the value set in this package's package.xml
  }

◆ mobilityresponse_cb()

void cooperative_lanechange::CooperativeLaneChangePlugin::mobilityresponse_cb ( const carma_v2x_msgs::msg::MobilityResponse::UniquePtr msg )

Callback to subscribed mobility response topic.

Parameters

msg	Latest mobility response message

Definition at line 163 of file cooperative_lanechange_node.cpp.

                                                                                                           {
    //@SONAR_STOP@
    if (clc_called_ && clc_request_id_ == msg->m_header.plan_id)
    {
      carma_planning_msgs::msg::LaneChangeStatus lc_status_msg;
      if(msg->is_accepted)
      {
        is_lanechange_accepted_ = true;
        lc_status_msg.status = carma_planning_msgs::msg::LaneChangeStatus::ACCEPTANCE_RECEIVED;
        lc_status_msg.description = "Received lane merge acceptance";
      }
      else
      {
        is_lanechange_accepted_ = false;
        lc_status_msg.status = carma_planning_msgs::msg::LaneChangeStatus::REJECTION_RECEIVED;
        lc_status_msg.description = "Received lane merge rejection";
      }
      lanechange_status_pub_->publish(lc_status_msg);
      //@SONAR_START@
    }
    else
    {
      RCLCPP_DEBUG_STREAM(get_logger(), "received mobility response is not related to CLC");
    }
 
  }

References clc_called_, clc_request_id_, basic_autonomy::get_logger(), is_lanechange_accepted_, and lanechange_status_pub_.

Referenced by on_configure_plugin().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ on_configure_plugin()

carma_ros2_utils::CallbackReturn cooperative_lanechange::CooperativeLaneChangePlugin::on_configure_plugin ( )

virtual

This method should be used to load parameters and will be called on the configure state transition.

Implements carma_guidance_plugins::PluginBaseNode.

Definition at line 100 of file cooperative_lanechange_node.cpp.

  {
    RCLCPP_INFO_STREAM(get_logger(), "CooperativeLaneChangePlugin trying to configure");
 
    // Reset config
    config_ = Config();
 
    // Load parameters
    get_parameter<double>("trajectory_time_length", config_.trajectory_time_length);
    get_parameter<std::string>("control_plugin_name", config_.control_plugin_name);
    get_parameter<double>("minimum_speed", config_.minimum_speed);
    get_parameter<double>("max_accel", config_.max_accel);
    get_parameter<double>("minimum_lookahead_distance", config_.minimum_lookahead_distance);
    get_parameter<double>("maximum_lookahead_distance", config_.maximum_lookahead_distance);
    get_parameter<double>("minimum_lookahead_speed", config_.minimum_lookahead_speed);
    get_parameter<double>("maximum_lookahead_speed", config_.maximum_lookahead_speed);
    get_parameter<double>("lateral_accel_limit", config_.lateral_accel_limit);
    get_parameter<int>("speed_moving_average_window_size", config_.speed_moving_average_window_size);
    get_parameter<int>("curvature_moving_average_window_size", config_.curvature_moving_average_window_size);
    get_parameter<int>("curvature_calc_lookahead_count", config_.curvature_calc_lookahead_count);
    get_parameter<int>("downsample_ratio", config_.downsample_ratio);
    get_parameter<double>("destination_range", config_.destination_range);
    get_parameter<double>("lanechange_time_out", config_.lanechange_time_out);
    get_parameter<double>("min_timestep", config_.min_timestep);
    get_parameter<double>("starting_downtrack_range", config_.starting_downtrack_range);
    get_parameter<double>("starting_fraction", config_.starting_fraction);
    get_parameter<double>("mid_fraction", config_.mid_fraction);
    get_parameter<double>("min_desired_gap", config_.min_desired_gap);
    get_parameter<double>("desired_time_gap", config_.desired_time_gap);
    get_parameter<int>("turn_downsample_ratio", config_.turn_downsample_ratio);
    get_parameter<double>("curve_resample_step_size", config_.curve_resample_step_size);
    get_parameter<double>("back_distance", config_.back_distance);
    get_parameter<double>("buffer_ending_downtrack", config_.buffer_ending_downtrack);
    get_parameter<std::string>("vehicle_id", config_.vehicle_id);
 
    // Register runtime parameter update callback
    add_on_set_parameters_callback(std::bind(&CooperativeLaneChangePlugin::parameter_update_callback, this, std_ph::_1));
 
    RCLCPP_INFO_STREAM(get_logger(), "Loaded params: " << config_);
 
    // Setup subscribers
    pose_sub_ = create_subscription<geometry_msgs::msg::PoseStamped>("current_pose", 1,
                                                              std::bind(&CooperativeLaneChangePlugin::pose_cb, this, std_ph::_1));
    twist_sub_ = create_subscription<geometry_msgs::msg::TwistStamped>("current_velocity", 1,
                                                              std::bind(&CooperativeLaneChangePlugin::twist_cb, this, std_ph::_1));
    incoming_mobility_response_sub_  = create_subscription<carma_v2x_msgs::msg::MobilityResponse>("incoming_mobility_response", 1,
                                                              std::bind(&CooperativeLaneChangePlugin::mobilityresponse_cb, this, std_ph::_1));
    georeference_sub_  = create_subscription<std_msgs::msg::String>("georeference", 1,
                                                              std::bind(&CooperativeLaneChangePlugin::georeference_cb, this, std_ph::_1));
    bsm_sub_  = create_subscription<carma_v2x_msgs::msg::BSM>("bsm_outbound", 1,
                                                              std::bind(&CooperativeLaneChangePlugin::bsm_cb, this, std_ph::_1));
 
    // Setup publishers
    outgoing_mobility_request_pub_ = create_publisher<carma_v2x_msgs::msg::MobilityRequest>("outgoing_mobility_request", 5); // Rate from yield plugin
    lanechange_status_pub_ = create_publisher<carma_planning_msgs::msg::LaneChangeStatus>("cooperative_lane_change_status", 10);
 
    // Initialize World Model
    wm_ = get_world_model();
 
    // Return success if everything initialized successfully
    return CallbackReturn::SUCCESS;
  }

Here is the call graph for this function:

◆ parameter_update_callback()

rcl_interfaces::msg::SetParametersResult cooperative_lanechange::CooperativeLaneChangePlugin::parameter_update_callback ( const std::vector< rclcpp::Parameter > & parameters )

Callback for dynamic parameter updates.

Definition at line 59 of file cooperative_lanechange_node.cpp.

  {
    auto error = update_params<std::string>(
      {{"control_plugin_name", config_.control_plugin_name},
      {"vehicle_id", config_.vehicle_id}}, parameters);
 
    auto error_2 = update_params<double>(
      {{"trajectory_time_length", config_.trajectory_time_length},
      {"minimum_speed", config_.minimum_speed},
      {"max_accel", config_.max_accel},
      {"minimum_lookahead_distance", config_.minimum_lookahead_distance},
      {"maximum_lookahead_distance", config_.maximum_lookahead_distance},
      {"minimum_lookahead_speed", config_.minimum_lookahead_speed},
      {"maximum_lookahead_speed", config_.maximum_lookahead_speed},
      {"lateral_accel_limit", config_.lateral_accel_limit},
      {"destination_range", config_.destination_range},
      {"lanechange_time_out", config_.lanechange_time_out},
      {"min_timestep", config_.min_timestep},
      {"starting_downtrack_range", config_.starting_downtrack_range},
      {"starting_fraction", config_.starting_fraction},
      {"mid_fraction", config_.mid_fraction},
      {"min_desired_gap", config_.min_desired_gap},
      {"curve_resample_step_size", config_.curve_resample_step_size},
      {"back_distance", config_.back_distance},
      {"buffer_ending_downtrack", config_.buffer_ending_downtrack},
      {"desired_time_gap", config_.desired_time_gap}}, parameters);
 
    auto error_3 = update_params<int>(
      {{"speed_moving_average_window_size", config_.speed_moving_average_window_size},
      {"curvature_moving_average_window_size", config_.curvature_moving_average_window_size},
      {"curvature_calc_lookahead_count", config_.curvature_calc_lookahead_count},
      {"downsample_ratio", config_.downsample_ratio},
      {"turn_downsample_ratio", config_.turn_downsample_ratio}}, parameters);
 
    rcl_interfaces::msg::SetParametersResult result;
 
    result.successful = !error && !error_2 && !error_3;
 
    return result;
  }

Referenced by on_configure_plugin().

Here is the caller graph for this function:

◆ plan_lanechange()

std::vector< carma_planning_msgs::msg::TrajectoryPlanPoint > cooperative_lanechange::CooperativeLaneChangePlugin::plan_lanechange ( carma_planning_msgs::srv::PlanTrajectory::Request::SharedPtr req )

Creates a vector of Trajectory Points from maneuver information in trajectory request.

Parameters

req	The service request

Returns: vector of unobstructed lane change trajectory points

Definition at line 587 of file cooperative_lanechange_node.cpp.

  {
    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 requested LANE_CHANGE maneuver
    std::vector<carma_planning_msgs::msg::Maneuver> maneuver_plan;
    if(req->maneuver_plan.maneuvers[req->maneuver_index_to_plan].type != carma_planning_msgs::msg::Maneuver::LANE_CHANGE) {
      throw std::invalid_argument ("Cooperative Lane Change Plugin doesn't support this maneuver type");
    }
    maneuver_plan.push_back(req->maneuver_plan.maneuvers[req->maneuver_index_to_plan]);
 
    if(current_downtrack >= maneuver_plan.front().lane_change_maneuver.end_dist){
      request_sent_ = false;
    }
 
    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("cooperative_lanechange", config_.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_.lateral_accel_limit,
                                                                        config_.speed_moving_average_window_size,
                                                                        config_.curvature_moving_average_window_size, config_.back_distance,
                                                                        config_.buffer_ending_downtrack);
 
    RCLCPP_DEBUG_STREAM(get_logger(), "Current downtrack: " << current_downtrack);
 
    std::string maneuver_id = maneuver_plan.front().lane_change_maneuver.parameters.maneuver_id;
    double original_start_dist = current_downtrack; // Initialize so original_start_dist cannot be less than the current downtrack
 
    if (original_lc_maneuver_values_.find(maneuver_id) != original_lc_maneuver_values_.end()) {
      // Obtain the original start_dist associated with this lane change maneuver
      original_start_dist = original_lc_maneuver_values_[maneuver_id].original_start_dist;
      RCLCPP_DEBUG_STREAM(get_logger(), "Maneuver id " << maneuver_id << " original start_dist is " << original_start_dist);
 
      // Set this maneuver's starting_lane_id to the original starting_lane_id associated with this lane change maneuver
      maneuver_plan.front().lane_change_maneuver.starting_lane_id = original_lc_maneuver_values_[maneuver_id].original_starting_lane_id;
      RCLCPP_DEBUG_STREAM(get_logger(), "Updated maneuver id " << maneuver_id << " starting_lane_id to its original value of " << original_lc_maneuver_values_[maneuver_id].original_starting_lane_id);
 
      // If the vehicle has started this lane change, set the request's vehicle_state.longitudinal_vel to the velocity that the vehicle began this lane change at
      if(original_lc_maneuver_values_[maneuver_id].has_started) {
        req->vehicle_state.longitudinal_vel = original_lc_maneuver_values_[maneuver_id].original_longitudinal_vel_ms;
        RCLCPP_DEBUG_STREAM(get_logger(), "Updating vehicle_state.longitudinal_vel to the initial lane change value of " << original_lc_maneuver_values_[maneuver_id].original_longitudinal_vel_ms);
      }
    }
    else {
      RCLCPP_WARN_STREAM(get_logger(), "No original values for lane change maneuver were found!");
    }
 
    double starting_downtrack = std::min(current_downtrack, original_start_dist);
 
    auto points_and_target_speeds = basic_autonomy::waypoint_generation::create_geometry_profile(maneuver_plan, starting_downtrack, wm_, ending_state_before_buffer_, req->vehicle_state, wpg_general_config, wpg_detail_config);
 
    // Calculate maneuver fraction completed (current_downtrack/(ending_downtrack-starting_downtrack)
    auto maneuver_end_dist = maneuver_plan.back().lane_change_maneuver.end_dist;
    auto maneuver_start_dist = maneuver_plan.front().lane_change_maneuver.start_dist;
    maneuver_fraction_completed_ = (maneuver_start_dist - current_downtrack)/(maneuver_end_dist - maneuver_start_dist);
 
    RCLCPP_DEBUG_STREAM(get_logger(), "Maneuvers to points size: " << points_and_target_speeds.size());
    auto downsampled_points = carma_ros2_utils::containers::downsample_vector(points_and_target_speeds, config_.downsample_ratio);
 
    std::vector<carma_planning_msgs::msg::TrajectoryPlanPoint> trajectory_points = basic_autonomy::waypoint_generation::compose_lanechange_trajectory_from_path(downsampled_points, req->vehicle_state, req->header.stamp,
                                                                                      wm_, ending_state_before_buffer_, wpg_detail_config);
    RCLCPP_DEBUG_STREAM(get_logger(), "Compose Trajectory size: " << trajectory_points.size());
    return trajectory_points;
  }

Referenced by plan_trajectory_callback().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ plan_trajectory_callback()

void cooperative_lanechange::CooperativeLaneChangePlugin::plan_trajectory_callback	(	std::shared_ptr< rmw_request_id_t >	srv_header,
		carma_planning_msgs::srv::PlanTrajectory::Request::SharedPtr	req,
		carma_planning_msgs::srv::PlanTrajectory::Response::SharedPtr	resp
	)

overridevirtual

Extending class provided callback which should return a planned trajectory based on the provided trajectory planning request.

Parameters

srv_header	RCL header for services calls. Can usually be ignored by implementers.
req	The service request containing the maneuvers to plan trajectories for and current vehicle state
resp	The response containing the planned trajectory

Implements carma_guidance_plugins::TacticalPlugin.

Definition at line 264 of file cooperative_lanechange_node.cpp.

  {
    std::chrono::system_clock::time_point start_time = std::chrono::system_clock::now();
 
    // Set boolean flag if this is the first time this service has been called
    if (!clc_called_)
    {
      clc_called_ = true;
    }
 
    //  Only plan the trajectory for the requested LANE_CHANGE maneuver
    std::vector<carma_planning_msgs::msg::Maneuver> maneuver_plan;
    if(req->maneuver_plan.maneuvers[req->maneuver_index_to_plan].type != carma_planning_msgs::msg::Maneuver::LANE_CHANGE)
    {
      throw std::invalid_argument ("Cooperative Lane Change Plugin doesn't support this maneuver type");
    }
    maneuver_plan.push_back(req->maneuver_plan.maneuvers[req->maneuver_index_to_plan]);
 
    // Currently only checking for first lane change maneuver message
    long target_lanelet_id = stol(maneuver_plan[0].lane_change_maneuver.ending_lane_id);
    double target_downtrack = maneuver_plan[0].lane_change_maneuver.end_dist;
 
    // Get subject vehicle info
    lanelet::BasicPoint2d veh_pos(req->vehicle_state.x_pos_global, req->vehicle_state.y_pos_global);
    double current_downtrack = wm_->routeTrackPos(veh_pos).downtrack;
 
    RCLCPP_DEBUG_STREAM(get_logger(), "target_lanelet_id: " << target_lanelet_id);
    RCLCPP_DEBUG_STREAM(get_logger(), "target_downtrack: " << target_downtrack);
    RCLCPP_DEBUG_STREAM(get_logger(), "current_downtrack: " << current_downtrack);
    RCLCPP_DEBUG_STREAM(get_logger(), "Starting CLC downtrack: " << maneuver_plan[0].lane_change_maneuver.start_dist);
 
    if(current_downtrack < maneuver_plan[0].lane_change_maneuver.start_dist - config_.starting_downtrack_range){
      RCLCPP_WARN_STREAM(get_logger(),
      "Lane change trajectory will not be planned. current_downtrack is more than "
      << config_.starting_downtrack_range << " meters before starting CLC downtrack");
 
      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("cooperative_lanechange"),
        "CLC ExecutionTime: " << std::chrono::duration<double>(duration).count());
      return;
    }
    auto current_lanelets = lanelet::geometry::findNearest(wm_->getMap()->laneletLayer, veh_pos, 10);
    long current_lanelet_id = current_lanelets[0].second.id();
    if(current_lanelet_id == target_lanelet_id && current_downtrack >= target_downtrack - config_.destination_range){
      carma_planning_msgs::msg::LaneChangeStatus lc_status_msg;
      lc_status_msg.status = carma_planning_msgs::msg::LaneChangeStatus::PLANNING_SUCCESS;
      //No description as per UI documentation
      lanechange_status_pub_->publish(lc_status_msg);
    }
 
    long veh2_lanelet_id = 0;
    double veh2_downtrack = 0.0, veh2_speed = 0.0;
    bool foundRoadwayObject = false;
    bool negotiate = true;
    std::vector<carma_perception_msgs::msg::RoadwayObstacle> rwol = wm_->getRoadwayObjects();
    //Assuming only one connected vehicle in list
    for(int i = 0; i < rwol.size(); i++){
      if(rwol[i].connected_vehicle_type.type == carma_perception_msgs::msg::ConnectedVehicleType::NOT_CONNECTED){
        veh2_lanelet_id = rwol[0].lanelet_id;
        veh2_downtrack = rwol[0].down_track; //Returns downtrack
        veh2_speed = rwol[0].object.velocity.twist.linear.x;
        foundRoadwayObject = true;
        break;
      }
    }
    if(foundRoadwayObject){
      RCLCPP_DEBUG_STREAM(get_logger(), "Found Roadway object");
      //get current_gap
      RCLCPP_DEBUG_STREAM(get_logger(), "veh2_lanelet_id: " << veh2_lanelet_id << ", veh2_downtrack: " << veh2_downtrack);
 
      double current_gap = find_current_gap(veh2_lanelet_id, veh2_downtrack, req->vehicle_state);
      RCLCPP_DEBUG_STREAM(get_logger(), "Current gap: " << current_gap);
 
      //get desired gap - desired time gap (default 3s)* relative velocity
      double relative_velocity = current_speed_ - veh2_speed;
      RCLCPP_DEBUG_STREAM(get_logger(), "Relative velocity: " << relative_velocity);
      double desired_gap = config_.desired_time_gap * relative_velocity;
      RCLCPP_DEBUG_STREAM(get_logger(), "Desired gap: " << desired_gap);
 
      if(desired_gap < config_.min_desired_gap){
        desired_gap = config_.min_desired_gap;
      }
      // TODO - this condition needs to be re-enabled after testing
      // if(current_gap > desired_gap){
      //     negotiate = false;  //No need for negotiation
      // }
 
    }
    else{
      RCLCPP_DEBUG_STREAM(get_logger(), "No roadway object");
      negotiate = false;
    }
 
    //plan lanechange without filling in response
    RCLCPP_DEBUG_STREAM(get_logger(), "Planning lane change trajectory");
 
    std::string maneuver_id = maneuver_plan[0].lane_change_maneuver.parameters.maneuver_id;
    if (original_lc_maneuver_values_.find(maneuver_id) == original_lc_maneuver_values_.end()) {
      // If this lane change maneuver ID is being received for this first time, store its original start_dist and starting_lane_id locally
      RCLCPP_DEBUG_STREAM(get_logger(), "Received maneuver id " << maneuver_id << " for the first time");
      RCLCPP_DEBUG_STREAM(get_logger(), "Original start dist is " << maneuver_plan[0].lane_change_maneuver.start_dist);
      RCLCPP_DEBUG_STREAM(get_logger(), "Original starting_lane_id is " << maneuver_plan[0].lane_change_maneuver.starting_lane_id);
 
      // Create LaneChangeManeuverOriginalValues object for this lane change maneuver and add it to original_lc_maneuver_values_
      LaneChangeManeuverOriginalValues original_lc_values;
      original_lc_values.maneuver_id = maneuver_id;
      original_lc_values.original_starting_lane_id = maneuver_plan[0].lane_change_maneuver.starting_lane_id;
      original_lc_values.original_start_dist = maneuver_plan[0].lane_change_maneuver.start_dist;
 
      original_lc_maneuver_values_[maneuver_id] = original_lc_values;
    }
    else {
      // If the vehicle has just started this lane change, store its initial velocity locally; this velocity will be maintained throughout the lane change
      if (current_downtrack >= (original_lc_maneuver_values_[maneuver_id]).original_start_dist  && !(original_lc_maneuver_values_[maneuver_id]).has_started) {
          original_lc_maneuver_values_[maneuver_id].has_started = true;
          original_lc_maneuver_values_[maneuver_id].original_longitudinal_vel_ms = std::max(req->vehicle_state.longitudinal_vel, config_.minimum_speed);
 
          RCLCPP_DEBUG_STREAM(get_logger(), "Lane change maneuver " << maneuver_id << " has started, maintaining speed (in m/s): " <<
                          original_lc_maneuver_values_[maneuver_id].original_longitudinal_vel_ms << " throughout lane change");
      }
    }
 
    std::vector<carma_planning_msgs::msg::TrajectoryPlanPoint> planned_trajectory_points = plan_lanechange(req);
 
    if(negotiate){
      RCLCPP_DEBUG_STREAM(get_logger(), "Negotiating");
      //send mobility request
      //Planning for first lane change maneuver
      carma_v2x_msgs::msg::MobilityRequest request = create_mobility_request(planned_trajectory_points, maneuver_plan[0]);
      outgoing_mobility_request_pub_->publish(request);
      if(!request_sent_){
        request_sent_time_ = this->now();
        request_sent_ = true;
      }
      carma_planning_msgs::msg::LaneChangeStatus lc_status_msg;
      lc_status_msg.status = carma_planning_msgs::msg::LaneChangeStatus::PLAN_SENT;
      lc_status_msg.description = "Requested lane merge";
      lanechange_status_pub_->publish(lc_status_msg);
    }
 
    //if ack mobility response, send lanechange response
    if(!negotiate || is_lanechange_accepted_){
      RCLCPP_DEBUG_STREAM(get_logger(), "negotiate:" << negotiate);
      RCLCPP_DEBUG_STREAM(get_logger(), "is_lanechange_accepted:" << is_lanechange_accepted_);
 
      RCLCPP_DEBUG_STREAM(get_logger(), "Adding to response");
      add_trajectory_to_response(req,resp,planned_trajectory_points);
 
    }
    else{
      if(!negotiate && !request_sent_){
        request_sent_time_ = this->now();
        request_sent_ = true;
      }
      rclcpp::Time planning_end_time = this->now();
      rclcpp::Duration passed_time = planning_end_time - request_sent_time_;
      if(passed_time.seconds() >= config_.lanechange_time_out){
        carma_planning_msgs::msg::LaneChangeStatus lc_status_msg;
        lc_status_msg.status = carma_planning_msgs::msg::LaneChangeStatus::TIMED_OUT;
        lc_status_msg.description = "Request timed out for lane merge";
        lanechange_status_pub_->publish(lc_status_msg);
        request_sent_ = false;  //Reset variable
      }
    }
 
    // Set the planning plugin field name
    for (auto& p : resp->trajectory_plan.trajectory_points) {
      p.planner_plugin_name = get_plugin_name();
    }
 
    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("cooperative_lanechange"),
      "CLC ExecutionTime: " << std::chrono::duration<double>(duration).count());
  }

Here is the call graph for this function:

◆ pose_cb()

void cooperative_lanechange::CooperativeLaneChangePlugin::pose_cb ( const geometry_msgs::msg::PoseStamped::UniquePtr msg )

private

Callback for the pose subscriber, which will store latest pose locally.

Parameters

msg	Latest pose message

Definition at line 249 of file cooperative_lanechange_node.cpp.

  {
    pose_msg_ = *msg;
  }

References pose_msg_.

Referenced by on_configure_plugin().

Here is the caller graph for this function:

◆ trajectory_plan_to_trajectory()

carma_v2x_msgs::msg::Trajectory cooperative_lanechange::CooperativeLaneChangePlugin::trajectory_plan_to_trajectory ( const std::vector< carma_planning_msgs::msg::TrajectoryPlanPoint > & traj_points ) const

Converts Trajectory Plan to (Mobility) Trajectory.

Parameters

traj_points vector of Trajectory Plan points to be converted to Trajectory type message

Returns: The Trajectory type message in world frame

Definition at line 544 of file cooperative_lanechange_node.cpp.

  {
    carma_v2x_msgs::msg::Trajectory traj;
    carma_v2x_msgs::msg::LocationECEF ecef_location = trajectory_point_to_ecef(traj_points[0]);
 
    if (traj_points.size() < 2){
      RCLCPP_WARN_STREAM(get_logger(), "Received Trajectory Plan is too small");
      traj.offsets = {};
    }
    else{
      carma_v2x_msgs::msg::LocationECEF prev_point = ecef_location;
      for (size_t i = 1; i < traj_points.size(); i++){
 
        carma_v2x_msgs::msg::LocationOffsetECEF offset;
        carma_v2x_msgs::msg::LocationECEF new_point = trajectory_point_to_ecef(traj_points[i]); // m to cm to fit the msg standard
        offset.offset_x = (int16_t)(new_point.ecef_x - prev_point.ecef_x);
        offset.offset_y = (int16_t)(new_point.ecef_y - prev_point.ecef_y);
        offset.offset_z = (int16_t)(new_point.ecef_z - prev_point.ecef_z);
        prev_point = new_point;
        traj.offsets.push_back(offset);
      }
    }
 
    traj.location = ecef_location;
 
    return traj;
  }

References basic_autonomy::get_logger(), process_bag::i, and trajectory_point_to_ecef().

Referenced by create_mobility_request().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ trajectory_point_to_ecef()

carma_v2x_msgs::msg::LocationECEF cooperative_lanechange::CooperativeLaneChangePlugin::trajectory_point_to_ecef ( const carma_planning_msgs::msg::TrajectoryPlanPoint & traj_point ) const

Converts Trajectory Point to ECEF frame using map projection.

Parameters

traj_points A Trajectory Plan point to be converted to Trajectory type message

Exceptions

std::invalid_argument If the map_projector_ member variable has not been set

Returns: The trajectory point message transformed to ecef frame

Definition at line 572 of file cooperative_lanechange_node.cpp.

  {
    if (!map_projector_) {
      throw std::invalid_argument("No map projector available for ecef conversion");
    }
    carma_v2x_msgs::msg::LocationECEF location;
 
    lanelet::BasicPoint3d ecef_point = map_projector_->projectECEF({traj_point.x, traj_point.y, 0.0}, 1);
    location.ecef_x = ecef_point.x() * 100.0; // Convert cm to m
    location.ecef_y = ecef_point.y() * 100.0;
    location.ecef_z = ecef_point.z() * 100.0;
 
    return location;
  }

References map_projector_.

Referenced by create_mobility_request(), and trajectory_plan_to_trajectory().

Here is the caller graph for this function:

◆ twist_cb()

void cooperative_lanechange::CooperativeLaneChangePlugin::twist_cb ( const geometry_msgs::msg::TwistStamped::UniquePtr msg )

private

Callback for the twist subscriber, which will store latest twist locally.

Parameters

msg	Latest twist message

Definition at line 254 of file cooperative_lanechange_node.cpp.

  {
    current_speed_ = msg->twist.linear.x;
  }

References current_speed_.

Referenced by on_configure_plugin().

Here is the caller graph for this function:

Member Data Documentation

◆ bsm_core_

carma_v2x_msgs::msg::BSMCoreData cooperative_lanechange::CooperativeLaneChangePlugin::bsm_core_

private

Definition at line 114 of file cooperative_lanechange_node.hpp.

Referenced by bsm_cb(), and create_mobility_request().

◆ bsm_sub_

carma_ros2_utils::SubPtr<carma_v2x_msgs::msg::BSM> cooperative_lanechange::CooperativeLaneChangePlugin::bsm_sub_

private

Definition at line 76 of file cooperative_lanechange_node.hpp.

Referenced by on_configure_plugin().

◆ clc_called_

bool cooperative_lanechange::CooperativeLaneChangePlugin::clc_called_ = false

private

Definition at line 108 of file cooperative_lanechange_node.hpp.

Referenced by mobilityresponse_cb(), and plan_trajectory_callback().

◆ clc_request_id_

std::string cooperative_lanechange::CooperativeLaneChangePlugin::clc_request_id_ = "default_request_id"

private

Definition at line 111 of file cooperative_lanechange_node.hpp.

Referenced by create_mobility_request(), and mobilityresponse_cb().

◆ config_

Config cooperative_lanechange::CooperativeLaneChangePlugin::config_

private

Definition at line 83 of file cooperative_lanechange_node.hpp.

Referenced by CooperativeLaneChangePlugin(), add_trajectory_to_response(), create_mobility_request(), on_configure_plugin(), parameter_update_callback(), plan_lanechange(), and plan_trajectory_callback().

◆ current_speed_

double cooperative_lanechange::CooperativeLaneChangePlugin::current_speed_

Definition at line 147 of file cooperative_lanechange_node.hpp.

Referenced by plan_trajectory_callback(), and twist_cb().

◆ DEFAULT_STRING_

std::string cooperative_lanechange::CooperativeLaneChangePlugin::DEFAULT_STRING_ = ""

private

Definition at line 96 of file cooperative_lanechange_node.hpp.

Referenced by create_mobility_request().

◆ ending_state_before_buffer_

carma_planning_msgs::msg::VehicleState cooperative_lanechange::CooperativeLaneChangePlugin::ending_state_before_buffer_

Definition at line 155 of file cooperative_lanechange_node.hpp.

Referenced by plan_lanechange().

◆ georeference_sub_

carma_ros2_utils::SubPtr<std_msgs::msg::String> cooperative_lanechange::CooperativeLaneChangePlugin::georeference_sub_

private

Definition at line 75 of file cooperative_lanechange_node.hpp.

Referenced by on_configure_plugin().

◆ incoming_mobility_response_sub_

carma_ros2_utils::SubPtr<carma_v2x_msgs::msg::MobilityResponse> cooperative_lanechange::CooperativeLaneChangePlugin::incoming_mobility_response_sub_

private

Definition at line 74 of file cooperative_lanechange_node.hpp.

Referenced by on_configure_plugin().

◆ is_lanechange_accepted_

bool cooperative_lanechange::CooperativeLaneChangePlugin::is_lanechange_accepted_ = false

Definition at line 153 of file cooperative_lanechange_node.hpp.

Referenced by mobilityresponse_cb(), and plan_trajectory_callback().

◆ lanechange_status_pub_

carma_ros2_utils::PubPtr<carma_planning_msgs::msg::LaneChangeStatus> cooperative_lanechange::CooperativeLaneChangePlugin::lanechange_status_pub_

private

Definition at line 80 of file cooperative_lanechange_node.hpp.

Referenced by mobilityresponse_cb(), on_configure_plugin(), and plan_trajectory_callback().

◆ maneuver_fraction_completed_

double cooperative_lanechange::CooperativeLaneChangePlugin::maneuver_fraction_completed_ = 0

private

Definition at line 105 of file cooperative_lanechange_node.hpp.

Referenced by create_mobility_request(), and plan_lanechange().

◆ map_georeference_

std::string cooperative_lanechange::CooperativeLaneChangePlugin::map_georeference_ {""}

private

Definition at line 89 of file cooperative_lanechange_node.hpp.

Referenced by georeference_cb().

◆ map_projector_

std::shared_ptr<lanelet::projection::LocalFrameProjector> cooperative_lanechange::CooperativeLaneChangePlugin::map_projector_

private

Definition at line 90 of file cooperative_lanechange_node.hpp.

Referenced by create_mobility_request(), georeference_cb(), and trajectory_point_to_ecef().

◆ original_lc_maneuver_values_

std::unordered_map<std::string, LaneChangeManeuverOriginalValues> cooperative_lanechange::CooperativeLaneChangePlugin::original_lc_maneuver_values_

private

Definition at line 117 of file cooperative_lanechange_node.hpp.

Referenced by plan_lanechange(), and plan_trajectory_callback().

◆ outgoing_mobility_request_pub_

carma_ros2_utils::PubPtr<carma_v2x_msgs::msg::MobilityRequest> cooperative_lanechange::CooperativeLaneChangePlugin::outgoing_mobility_request_pub_

private

Definition at line 79 of file cooperative_lanechange_node.hpp.

Referenced by on_configure_plugin(), and plan_trajectory_callback().

◆ pose_msg_

geometry_msgs::msg::PoseStamped cooperative_lanechange::CooperativeLaneChangePlugin::pose_msg_

Definition at line 150 of file cooperative_lanechange_node.hpp.

Referenced by create_mobility_request(), and pose_cb().

◆ pose_sub_

carma_ros2_utils::SubPtr<geometry_msgs::msg::PoseStamped> cooperative_lanechange::CooperativeLaneChangePlugin::pose_sub_

private

Definition at line 72 of file cooperative_lanechange_node.hpp.

Referenced by on_configure_plugin().

◆ request_sent_

bool cooperative_lanechange::CooperativeLaneChangePlugin::request_sent_ = false

private

Definition at line 102 of file cooperative_lanechange_node.hpp.

Referenced by plan_lanechange(), and plan_trajectory_callback().

◆ request_sent_time_

rclcpp::Time cooperative_lanechange::CooperativeLaneChangePlugin::request_sent_time_

private

Definition at line 99 of file cooperative_lanechange_node.hpp.

Referenced by plan_trajectory_callback().

◆ traj_freq_

double cooperative_lanechange::CooperativeLaneChangePlugin::traj_freq_ = 10

private

Definition at line 93 of file cooperative_lanechange_node.hpp.

◆ twist_sub_

carma_ros2_utils::SubPtr<geometry_msgs::msg::TwistStamped> cooperative_lanechange::CooperativeLaneChangePlugin::twist_sub_

private

Definition at line 73 of file cooperative_lanechange_node.hpp.

Referenced by on_configure_plugin().

◆ wm_

carma_wm::WorldModelConstPtr cooperative_lanechange::CooperativeLaneChangePlugin::wm_

private

Definition at line 86 of file cooperative_lanechange_node.hpp.

Referenced by find_current_gap(), on_configure_plugin(), plan_lanechange(), and plan_trajectory_callback().

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

cooperative_lanechange/include/cooperative_lanechange/cooperative_lanechange_node.hpp
cooperative_lanechange/src/cooperative_lanechange_node.cpp

Public Member Functions

Public Attributes

Private Member Functions

Private Attributes

Detailed Description

Constructor & Destructor Documentation

◆ CooperativeLaneChangePlugin()

Member Function Documentation

◆ add_trajectory_to_response()

◆ bsm_cb()

◆ bsmIDtoString()

◆ create_mobility_request()

◆ find_current_gap()

◆ FRIEND_TEST() [1/3]

◆ FRIEND_TEST() [2/3]

◆ FRIEND_TEST() [3/3]

◆ georeference_cb()

◆ get_availability()

◆ get_version_id()

◆ mobilityresponse_cb()

◆ on_configure_plugin()

◆ parameter_update_callback()

◆ plan_lanechange()

◆ plan_trajectory_callback()

◆ pose_cb()

◆ trajectory_plan_to_trajectory()

◆ trajectory_point_to_ecef()

◆ twist_cb()

Member Data Documentation

◆ bsm_core_

◆ bsm_sub_

◆ clc_called_

◆ clc_request_id_

◆ config_

◆ current_speed_

◆ DEFAULT_STRING_

◆ ending_state_before_buffer_

◆ georeference_sub_

◆ incoming_mobility_response_sub_

◆ is_lanechange_accepted_

◆ lanechange_status_pub_

◆ maneuver_fraction_completed_

◆ map_georeference_

◆ map_projector_

◆ original_lc_maneuver_values_

◆ outgoing_mobility_request_pub_

◆ pose_msg_

◆ pose_sub_

◆ request_sent_

◆ request_sent_time_

◆ traj_freq_

◆ twist_sub_

◆ wm_