#include <stop_and_dwell_strategic_plugin.hpp>

Inheritance diagram for stop_and_dwell_strategic_plugin::StopAndDwellStrategicPlugin:

Collaboration diagram for stop_and_dwell_strategic_plugin::StopAndDwellStrategicPlugin:

Public Member Functions
	StopAndDwellStrategicPlugin (const rclcpp::NodeOptions &)
	Default constructor for RouteFollowingPlugin class. More...

void	plan_maneuvers_callback (std::shared_ptr< rmw_request_id_t > srv_header, carma_planning_msgs::srv::PlanManeuvers::Request::SharedPtr req, carma_planning_msgs::srv::PlanManeuvers::Response::SharedPtr resp)
	Service callback for arbitrator maneuver planning. More...

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

void	currentPoseCb (geometry_msgs::msg::PoseStamped::UniquePtr msg)
	callback function for current pose More...

carma_planning_msgs::msg::Maneuver	composeLaneFollowingManeuverMessage (double start_dist, double end_dist, double start_speed, double target_speed, rclcpp::Time start_time, double time_to_stop, std::vector< lanelet::Id > lane_ids)
	Compose a lane keeping maneuver message based on input params. More...

carma_planning_msgs::msg::Maneuver	composeStopAndWaitManeuverMessage (double current_dist, double end_dist, double start_speed, const lanelet::Id &starting_lane_id, const lanelet::Id &ending_lane_id, double stopping_accel, rclcpp::Time start_time, rclcpp::Time end_time) const

VehicleState	extractInitialState (const carma_planning_msgs::srv::PlanManeuvers::Request &req) const
	Helper method to extract the initial vehicle state from the planning request method based on if the prior_plan was set or not. More...

std::vector< lanelet::ConstLanelet >	getLaneletsBetweenWithException (double start_downtrack, double end_downtrack, bool shortest_path_only=false, bool bounds_inclusive=true) const
	Helper method which calls carma_wm::WorldModel::getLaneletsBetween(start_downtrack, end_downtrack, shortest_path_only, bounds_inclusive) and throws and exception if the returned list of lanelets is empty. See the referenced method for additional details on parameters. More...

double	findSpeedLimit (const lanelet::ConstLanelet &llt) const
	Given a Lanelet, find it's associated Speed Limit. More...

void	guidance_state_cb (const carma_planning_msgs::msg::GuidanceState::UniquePtr msg)
	Callback for the Guidance State. More...

VehicleState	extractInitialState (carma_planning_msgs::srv::PlanManeuvers::Request::SharedPtr req) const
	Helper method to extract the initial vehicle state from the planning request method based on if the prior_plan was set or not. More...

carma_ros2_utils::CallbackReturn	on_configure_plugin ()
	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...

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

bool	get_availability ()
	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 ()
	Returns the version id of this plugin. More...

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

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

virtual void	plan_maneuvers_callback (std::shared_ptr< rmw_request_id_t > srv_header, carma_planning_msgs::srv::PlanManeuvers::Request::SharedPtr req, carma_planning_msgs::srv::PlanManeuvers::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
bool	vehicle_engaged_ = false

double	speed_limit_ = 100.0

double	current_downtrack_ = 0.0

double	bus_stop_downtrack_ = 0.0

bool	first_stop_ = true

rclcpp::Time	time_to_move_

std::string	logger_name_ ="stop_and_dwell_strategic_plugin"

double	max_comfort_accel_ = 2.0

double	max_comfort_decel_ = -2.0

double	max_comfort_decel_norm_ = -1 * max_comfort_decel_

bool	approaching_stop_controlled_interction_ = false

Private Member Functions
carma_wm::WorldModelConstPtr	get_wm ()

void	set_wm (carma_wm::WorldModelConstPtr new_wm)

	FRIEND_TEST (StopAndDwellStrategicPluginTest, findSpeedLimit)

	FRIEND_TEST (StopAndDwellStrategicPluginTest, maneuvercbtest)

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

carma_ros2_utils::SubPtr< carma_planning_msgs::msg::GuidanceState >	guidance_state_sub_

bool	guidance_engaged_ = false

carma_wm::WorldModelConstPtr	wm_
	World Model pointer. More...

StopAndDwellStrategicPluginConfig	config_
	Config containing configurable algorithm parameters. More...

Detailed Description

Definition at line 51 of file stop_and_dwell_strategic_plugin.hpp.

Constructor & Destructor Documentation

◆ StopAndDwellStrategicPlugin()

stop_and_dwell_strategic_plugin::StopAndDwellStrategicPlugin::StopAndDwellStrategicPlugin ( const rclcpp::NodeOptions & options )

explicit

Default constructor for RouteFollowingPlugin class.

Definition at line 58 of file stop_and_dwell_strategic_plugin.cpp.

  : carma_guidance_plugins::StrategicPlugin(options), config_(StopAndDwellStrategicPluginConfig())
{
  // Declare parameters
  config_.vehicle_decel_limit_multiplier = declare_parameter<double>("vehicle_decel_limit_multiplier",   config_.vehicle_decel_limit_multiplier);
  config_.vehicle_accel_limit_multiplier = declare_parameter<double>("vehicle_accel_limit_multiplier",   config_.vehicle_accel_limit_multiplier);
  config_.stop_line_buffer = declare_parameter<double>("stop_line_buffer",   config_.stop_line_buffer);
  config_.bus_line_exit_zone_length = declare_parameter<double>("bus_line_exit_zone_length",   config_.bus_line_exit_zone_length);
  config_.strategic_plugin_name = declare_parameter<std::string>("strategic_plugin_name",            config_.strategic_plugin_name);
  config_.lane_following_plugin_name = declare_parameter<std::string>("lane_following_plugin_name",       config_.lane_following_plugin_name);
  config_.vehicle_id = declare_parameter<std::string>("vehicle_id", config_.vehicle_id);
  config_.veh_length = declare_parameter<double>("vehicle_length", config_.veh_length);
  config_.vehicle_decel_limit = declare_parameter<double>("vehicle_deceleration_limit", config_.vehicle_decel_limit);
  config_.vehicle_accel_limit = declare_parameter<double>("vehicle_acceleration_limit", config_.vehicle_accel_limit);
  config_.activation_distance = declare_parameter<double>("activation_distance", config_.activation_distance);
  config_.dwell_time = declare_parameter<double>("dwell_time", config_.dwell_time);
  config_.deceleration_fraction = declare_parameter<double>("deceleration_fraction", config_.deceleration_fraction);
  config_.desired_distance_to_stop_buffer = declare_parameter<double>("desired_distance_to_stop_buffer", config_.desired_distance_to_stop_buffer);
 
  max_comfort_accel_ = config_.vehicle_accel_limit * config_.vehicle_accel_limit_multiplier;
  max_comfort_decel_ = -1 * config_.vehicle_decel_limit * config_.vehicle_decel_limit_multiplier;
  max_comfort_decel_norm_ = config_.vehicle_decel_limit * config_.vehicle_decel_limit_multiplier;
};

Member Function Documentation

◆ composeLaneFollowingManeuverMessage()

carma_planning_msgs::msg::Maneuver stop_and_dwell_strategic_plugin::StopAndDwellStrategicPlugin::composeLaneFollowingManeuverMessage	(	double	start_dist,
		double	end_dist,
		double	start_speed,
		double	target_speed,
		rclcpp::Time	start_time,
		double	time_to_stop,
		std::vector< lanelet::Id >	lane_ids
	)

Compose a lane keeping maneuver message based on input params.

Parameters

start_dist	Start downtrack distance of the current maneuver
end_dist	End downtrack distance of the current maneuver
start_speed	Start speed of the current maneuver
target_speed	Target speed pf the current maneuver, usually it is the lanelet speed limit
start_time	The starting time of the maneuver
end_time	The ending time of the maneuver
lane_ids	List of lanelet IDs that the current maneuver traverses. Message expects these to be contiguous and end to end

Returns: A lane keeping maneuver message which is ready to be published

Definition at line 346 of file stop_and_dwell_strategic_plugin.cpp.

{
  carma_planning_msgs::msg::Maneuver maneuver_msg;
  carma_planning_msgs::msg::Maneuver empty_msg;
  maneuver_msg.type = carma_planning_msgs::msg::Maneuver::LANE_FOLLOWING;
  maneuver_msg.lane_following_maneuver.parameters.planning_strategic_plugin = config_.strategic_plugin_name;
  maneuver_msg.lane_following_maneuver.parameters.presence_vector = 
  carma_planning_msgs::msg::ManeuverParameters::HAS_TACTICAL_PLUGIN;
  maneuver_msg.lane_following_maneuver.parameters.negotiation_type = carma_planning_msgs::msg::ManeuverParameters::NO_NEGOTIATION;
  maneuver_msg.lane_following_maneuver.parameters.planning_tactical_plugin = config_.lane_following_plugin_name;
  maneuver_msg.lane_following_maneuver.start_dist = start_dist;
  maneuver_msg.lane_following_maneuver.end_dist = end_dist;
  maneuver_msg.lane_following_maneuver.start_speed = start_speed;
  maneuver_msg.lane_following_maneuver.end_speed = target_speed;
  maneuver_msg.lane_following_maneuver.start_time = start_time;
  maneuver_msg.lane_following_maneuver.end_time =  start_time + rclcpp::Duration(time_to_stop*1e9);
  maneuver_msg.lane_following_maneuver.lane_ids =
      lanelet::utils::transform(lane_ids, [](auto id) { return std::to_string(id); });
 
  RCLCPP_INFO_STREAM(rclcpp::get_logger("stop_and_dwell_strategic_plugin"), "Creating lane follow start dist: " << start_dist << " end dist: " << end_dist);
return maneuver_msg;
}

References config_, stop_and_dwell_strategic_plugin::StopAndDwellStrategicPluginConfig::lane_following_plugin_name, stop_and_dwell_strategic_plugin::StopAndDwellStrategicPluginConfig::strategic_plugin_name, and carma_cooperative_perception::to_string().

Referenced by plan_maneuvers_callback().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ composeStopAndWaitManeuverMessage()

carma_planning_msgs::msg::Maneuver stop_and_dwell_strategic_plugin::StopAndDwellStrategicPlugin::composeStopAndWaitManeuverMessage	(	double	current_dist,
		double	end_dist,
		double	start_speed,
		const lanelet::Id &	starting_lane_id,
		const lanelet::Id &	ending_lane_id,
		double	stopping_accel,
		rclcpp::Time	start_time,
		rclcpp::Time	end_time
	)		const

Definition at line 386 of file stop_and_dwell_strategic_plugin.cpp.

{
  carma_planning_msgs::msg::Maneuver maneuver_msg;
  maneuver_msg.type = carma_planning_msgs::msg::Maneuver::STOP_AND_WAIT;
  maneuver_msg.stop_and_wait_maneuver.parameters.planning_strategic_plugin = config_.strategic_plugin_name;
  maneuver_msg.stop_and_wait_maneuver.parameters.presence_vector =
      carma_planning_msgs::msg::ManeuverParameters::HAS_TACTICAL_PLUGIN | carma_planning_msgs::msg::ManeuverParameters::HAS_FLOAT_META_DATA;
  maneuver_msg.stop_and_wait_maneuver.parameters.negotiation_type = carma_planning_msgs::msg::ManeuverParameters::NO_NEGOTIATION;
  maneuver_msg.stop_and_wait_maneuver.parameters.planning_tactical_plugin = config_.stop_and_wait_plugin_name;
  maneuver_msg.stop_and_wait_maneuver.start_speed = start_speed;
  maneuver_msg.stop_and_wait_maneuver.start_dist = current_dist;
  maneuver_msg.stop_and_wait_maneuver.end_dist = end_dist;
  maneuver_msg.stop_and_wait_maneuver.start_time = start_time;
  maneuver_msg.stop_and_wait_maneuver.end_time = end_time;
  maneuver_msg.stop_and_wait_maneuver.starting_lane_id = std::to_string(starting_lane_id);
  maneuver_msg.stop_and_wait_maneuver.ending_lane_id = std::to_string(ending_lane_id);
  // Set the meta data for the stop location buffer
  maneuver_msg.stop_and_wait_maneuver.parameters.float_valued_meta_data.push_back(config_.stop_line_buffer);
  maneuver_msg.stop_and_wait_maneuver.parameters.float_valued_meta_data.push_back(stopping_accel);
 
  RCLCPP_INFO_STREAM(rclcpp::get_logger("stop_and_dwell_strategic_plugin"), "Creating stop and wait start dist: " << current_dist << " end dist: " << end_dist);
  
  return maneuver_msg;
}

References config_, stop_and_dwell_strategic_plugin::StopAndDwellStrategicPluginConfig::stop_and_wait_plugin_name, stop_and_dwell_strategic_plugin::StopAndDwellStrategicPluginConfig::stop_line_buffer, stop_and_dwell_strategic_plugin::StopAndDwellStrategicPluginConfig::strategic_plugin_name, and carma_cooperative_perception::to_string().

Referenced by plan_maneuvers_callback().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ currentPoseCb()

void stop_and_dwell_strategic_plugin::StopAndDwellStrategicPlugin::currentPoseCb ( geometry_msgs::msg::PoseStamped::UniquePtr msg )

callback function for current pose

Parameters

msg	input pose stamed msg

Definition at line 181 of file stop_and_dwell_strategic_plugin.cpp.

{
  geometry_msgs::msg::PoseStamped pose_msg = geometry_msgs::msg::PoseStamped(*msg.get());
  if (vehicle_engaged_)
  {
    lanelet::BasicPoint2d current_loc(pose_msg.pose.position.x, pose_msg.pose.position.y);
    current_downtrack_ = wm_->routeTrackPos(current_loc).downtrack;
    RCLCPP_DEBUG_STREAM(rclcpp::get_logger("stop_and_dwell_strategic_plugin"), "Downtrack from current pose: " << current_downtrack_);
  }
  
}

References current_downtrack_, vehicle_engaged_, and wm_.

Referenced by on_configure_plugin().

Here is the caller graph for this function:

◆ extractInitialState() [1/2]

VehicleState stop_and_dwell_strategic_plugin::StopAndDwellStrategicPlugin::extractInitialState ( carma_planning_msgs::srv::PlanManeuvers::Request::SharedPtr req ) const

Helper method to extract the initial vehicle state from the planning request method based on if the prior_plan was set or not.

Parameters

req	The maneuver planning request to extract the vehicle state from

Returns: The extracted VehicleState

Definition at line 211 of file stop_and_dwell_strategic_plugin.cpp.

{
  VehicleState state;
  if (!req->prior_plan.maneuvers.empty())
  {
    RCLCPP_DEBUG_STREAM(rclcpp::get_logger(logger_name_), "Provided with initial plan...");
    state.stamp = GET_MANEUVER_PROPERTY(req->prior_plan.maneuvers.back(), end_time);
    state.downtrack = GET_MANEUVER_PROPERTY(req->prior_plan.maneuvers.back(), end_dist);
    state.speed = getManeuverEndSpeed(req->prior_plan.maneuvers.back());
    state.lane_id = getLaneletsBetweenWithException(state.downtrack, state.downtrack, true).front().id();
  }
  else
  {
    RCLCPP_DEBUG_STREAM(rclcpp::get_logger(logger_name_), "No initial plan provided...");
    
    state.stamp = rclcpp::Time(req->header.stamp, RCL_SYSTEM_TIME);
    state.downtrack = req->veh_downtrack;
    state.speed = req->veh_logitudinal_velocity;
    state.lane_id = stoi(req->veh_lane_id);
  }
  RCLCPP_DEBUG_STREAM(rclcpp::get_logger(logger_name_), "extractInitialState >>>> state.stamp: " << std::to_string(state.stamp.seconds()));
  RCLCPP_DEBUG_STREAM(rclcpp::get_logger(logger_name_), "extractInitialState >>>> state.downtrack : " << state.downtrack );
  RCLCPP_DEBUG_STREAM(rclcpp::get_logger(logger_name_), "extractInitialState >>>> state.speed: " << state.speed);
  RCLCPP_DEBUG_STREAM(rclcpp::get_logger(logger_name_), "extractInitialState >>>> state.lane_id: " << state.lane_id);
 
  return state;
}

References stop_and_dwell_strategic_plugin::VehicleState::downtrack, GET_MANEUVER_PROPERTY, getLaneletsBetweenWithException(), stop_and_dwell_strategic_plugin::anonymous_namespace{stop_and_dwell_strategic_plugin.cpp}::getManeuverEndSpeed(), stop_and_dwell_strategic_plugin::VehicleState::lane_id, logger_name_, stop_and_dwell_strategic_plugin::VehicleState::speed, stop_and_dwell_strategic_plugin::VehicleState::stamp, and carma_cooperative_perception::to_string().

Here is the call graph for this function:

◆ extractInitialState() [2/2]

VehicleState stop_and_dwell_strategic_plugin::StopAndDwellStrategicPlugin::extractInitialState ( const carma_planning_msgs::srv::PlanManeuvers::Request & req ) const

Helper method to extract the initial vehicle state from the planning request method based on if the prior_plan was set or not.

Parameters

req	The maneuver planning request to extract the vehicle state from

Returns: The extracted VehicleState

Definition at line 147 of file stop_and_dwell_strategic_plugin.cpp.

{
  VehicleState state;
  if (!req.prior_plan.maneuvers.empty())
  {
    RCLCPP_DEBUG_STREAM(rclcpp::get_logger("stop_and_dwell_strategic_plugin"), "Provided with initial plan...");
    state.stamp = GET_MANEUVER_PROPERTY(req.prior_plan.maneuvers.back(), end_time);
    state.speed = getManeuverEndSpeed(req.prior_plan.maneuvers.back());
    state.downtrack = GET_MANEUVER_PROPERTY(req.prior_plan.maneuvers.back(), end_dist);
    state.lane_id = getLaneletsBetweenWithException(state.downtrack, state.downtrack, true).front().id();
  }
  else
  {
    RCLCPP_DEBUG_STREAM(rclcpp::get_logger("stop_and_dwell_strategic_plugin"), "No initial plan provided...");
    
    state.stamp = req.header.stamp;
    state.downtrack = req.veh_downtrack;
    state.speed = req.veh_logitudinal_velocity;
    state.lane_id = stoi(req.veh_lane_id);
  }
  
  RCLCPP_DEBUG_STREAM(rclcpp::get_logger("stop_and_dwell_strategic_plugin"), "state.stamp: " << std::to_string(state.stamp.seconds()));
  RCLCPP_DEBUG_STREAM(rclcpp::get_logger("stop_and_dwell_strategic_plugin"), "state.downtrack : " << state.downtrack );
  RCLCPP_DEBUG_STREAM(rclcpp::get_logger("stop_and_dwell_strategic_plugin"), "state.speed: " << state.speed);
  RCLCPP_DEBUG_STREAM(rclcpp::get_logger("stop_and_dwell_strategic_plugin"), "state.lane_id: " << state.lane_id);
 
  return state;
}

References stop_and_dwell_strategic_plugin::VehicleState::downtrack, GET_MANEUVER_PROPERTY, getLaneletsBetweenWithException(), stop_and_dwell_strategic_plugin::anonymous_namespace{stop_and_dwell_strategic_plugin.cpp}::getManeuverEndSpeed(), stop_and_dwell_strategic_plugin::VehicleState::lane_id, stop_and_dwell_strategic_plugin::VehicleState::speed, stop_and_dwell_strategic_plugin::VehicleState::stamp, and carma_cooperative_perception::to_string().

Referenced by plan_maneuvers_callback().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ findSpeedLimit()

double stop_and_dwell_strategic_plugin::StopAndDwellStrategicPlugin::findSpeedLimit ( const lanelet::ConstLanelet & llt ) const

Given a Lanelet, find it's associated Speed Limit.

Parameters

llt	Constant Lanelet object

Exceptions

std::invalid_argument if the speed limit could not be retrieved

Returns: value of speed limit in mps

Definition at line 373 of file stop_and_dwell_strategic_plugin.cpp.

{
  lanelet::Optional<carma_wm::TrafficRulesConstPtr> traffic_rules = wm_->getTrafficRules();
  if (traffic_rules)
  {
    return (*traffic_rules)->speedLimit(llt).speedLimit.value();
  }
  else
  {
    throw std::invalid_argument("Valid traffic rules object could not be built");
  }
}

References wm_.

Referenced by plan_maneuvers_callback().

Here is the caller graph for this function:

◆ FRIEND_TEST() [1/2]

stop_and_dwell_strategic_plugin::StopAndDwellStrategicPlugin::FRIEND_TEST	(	StopAndDwellStrategicPluginTest	,
		findSpeedLimit
	)

private

◆ FRIEND_TEST() [2/2]

stop_and_dwell_strategic_plugin::StopAndDwellStrategicPlugin::FRIEND_TEST	(	StopAndDwellStrategicPluginTest	,
		maneuvercbtest
	)

private

◆ get_availability()

bool stop_and_dwell_strategic_plugin::StopAndDwellStrategicPlugin::get_availability ( )

virtual

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 415 of file stop_and_dwell_strategic_plugin.cpp.

{
  return true;
}

◆ get_version_id()

std::string stop_and_dwell_strategic_plugin::StopAndDwellStrategicPlugin::get_version_id ( )

virtual

Returns the version id of this plugin.

Returns: The version id represented as a string

Implements carma_guidance_plugins::PluginBaseNode.

Definition at line 420 of file stop_and_dwell_strategic_plugin.cpp.

{
  return "v1.0";
}

◆ get_wm()

carma_wm::WorldModelConstPtr stop_and_dwell_strategic_plugin::StopAndDwellStrategicPlugin::get_wm ( )

inlineprivate

Definition at line 196 of file stop_and_dwell_strategic_plugin.hpp.

196{ return wm_; }

References wm_.

◆ getLaneletsBetweenWithException()

std::vector< lanelet::ConstLanelet > stop_and_dwell_strategic_plugin::StopAndDwellStrategicPlugin::getLaneletsBetweenWithException	(	double	start_downtrack,
		double	end_downtrack,
		bool	shortest_path_only = `false`,
		bool	bounds_inclusive = `true`
	)		const

Helper method which calls carma_wm::WorldModel::getLaneletsBetween(start_downtrack, end_downtrack, shortest_path_only, bounds_inclusive) and throws and exception if the returned list of lanelets is empty. See the referenced method for additional details on parameters.

Definition at line 193 of file stop_and_dwell_strategic_plugin.cpp.

{
  std::vector<lanelet::ConstLanelet> crossed_lanelets =
      wm_->getLaneletsBetween(start_downtrack, end_downtrack, shortest_path_only, bounds_inclusive);
 
  if (crossed_lanelets.empty())
  {
    throw std::invalid_argument("getLaneletsBetweenWithException called but inputs do not cross any lanelets going "
                                "from: " +
                                std::to_string(start_downtrack) + " to: " + std::to_string(end_downtrack));
  }
 
  return crossed_lanelets;
}

References carma_cooperative_perception::to_string(), and wm_.

Referenced by extractInitialState(), and plan_maneuvers_callback().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ guidance_state_cb()

void stop_and_dwell_strategic_plugin::StopAndDwellStrategicPlugin::guidance_state_cb ( const carma_planning_msgs::msg::GuidanceState::UniquePtr msg )

Callback for the Guidance State.

Parameters

msg	Latest GuidanceState message

Definition at line 176 of file stop_and_dwell_strategic_plugin.cpp.

{
  guidance_engaged_ = (msg->state == carma_planning_msgs::msg::GuidanceState::ENGAGED);
}

References lightbar_manager::ENGAGED, and guidance_engaged_.

Referenced by on_configure_plugin().

Here is the caller graph for this function:

◆ on_activate_plugin()

carma_ros2_utils::CallbackReturn stop_and_dwell_strategic_plugin::StopAndDwellStrategicPlugin::on_activate_plugin ( )

virtual

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.

Returns: SUCCESS, FAILURE, or ERROR. Transition to ACTIVE will only complete on SUCCESS.

Reimplemented from carma_guidance_plugins::PluginBaseNode.

Definition at line 142 of file stop_and_dwell_strategic_plugin.cpp.

{
  return CallbackReturn::SUCCESS;
}

◆ on_configure_plugin()

carma_ros2_utils::CallbackReturn stop_and_dwell_strategic_plugin::StopAndDwellStrategicPlugin::on_configure_plugin ( )

virtual

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.

Returns: SUCCESS, FAILURE, or ERROR. Transition to INACTIVE will only complete on SUCCESS.

Implements carma_guidance_plugins::PluginBaseNode.

Definition at line 82 of file stop_and_dwell_strategic_plugin.cpp.

{
  // reset config
  config_ = StopAndDwellStrategicPluginConfig();
  // Declare parameters
  get_parameter<double>("vehicle_decel_limit_multiplier",   config_.vehicle_decel_limit_multiplier);
  get_parameter<double>("vehicle_accel_limit_multiplier",   config_.vehicle_accel_limit_multiplier);
  get_parameter<double>("stop_line_buffer",   config_.stop_line_buffer);
  get_parameter<double>("bus_line_exit_zone_length",   config_.bus_line_exit_zone_length);
  get_parameter<std::string>("strategic_plugin_name",            config_.strategic_plugin_name);
  get_parameter<std::string>("lane_following_plugin_name",       config_.lane_following_plugin_name);
  get_parameter<std::string>("vehicle_id", config_.vehicle_id);
  get_parameter<double>("vehicle_length", config_.veh_length);
  get_parameter<double>("vehicle_deceleration_limit", config_.vehicle_decel_limit);
  get_parameter<double>("vehicle_acceleration_limit", config_.vehicle_accel_limit);
  get_parameter<double>("activation_distance", config_.activation_distance);
  get_parameter<double>("dwell_time", config_.dwell_time);
  get_parameter<double>("deceleration_fraction", config_.deceleration_fraction);
  get_parameter<double>("desired_distance_to_stop_buffer", config_.desired_distance_to_stop_buffer);
 
  max_comfort_accel_ = config_.vehicle_accel_limit * config_.vehicle_accel_limit_multiplier;
  max_comfort_decel_ = -1 * config_.vehicle_decel_limit * config_.vehicle_decel_limit_multiplier;
  max_comfort_decel_norm_ = config_.vehicle_decel_limit * config_.vehicle_decel_limit_multiplier;
 
   // Register runtime parameter update callback
  add_on_set_parameters_callback(std::bind(&StopAndDwellStrategicPlugin::parameter_update_callback, this, std_ph::_1));
 
  RCLCPP_INFO_STREAM(rclcpp::get_logger("stop_and_dwell_strategic_plugin"),"Done loading parameters: " << config_);
 
  // Current Pose Subscriber
  current_pose_sub_ = create_subscription<geometry_msgs::msg::PoseStamped>("current_pose", 1, 
    std::bind(&StopAndDwellStrategicPlugin::currentPoseCb,this,std_ph::_1));
 
  // Guidance State subscriber
  guidance_state_sub_ = create_subscription<carma_planning_msgs::msg::GuidanceState>("guidance_state", 5, 
    std::bind(&StopAndDwellStrategicPlugin::guidance_state_cb, this, std::placeholders::_1));
 
  // set world model point form wm listener
  wm_ = get_world_model();
 
  // Return success if everthing initialized successfully
  return CallbackReturn::SUCCESS;
}

Here is the call graph for this function:

◆ parameter_update_callback()

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

Callback for dynamic parameter updates.

Definition at line 126 of file stop_and_dwell_strategic_plugin.cpp.

{
  auto error_double = update_params<double>({
    {"vehicle_decel_limit_multiplier", config_.vehicle_decel_limit_multiplier},
    {"vehicle_accel_limit_multiplier", config_.vehicle_accel_limit_multiplier},
    {"stop_line_buffer", config_.stop_line_buffer},
    {"bus_line_exit_zone_length", config_.bus_line_exit_zone_length}
  }, parameters); // vehicle_acceleration_limit not updated as it's global param
 
  rcl_interfaces::msg::SetParametersResult result;
 
  result.successful = !error_double;
 
  return result;
}

References stop_and_dwell_strategic_plugin::StopAndDwellStrategicPluginConfig::bus_line_exit_zone_length, config_, stop_and_dwell_strategic_plugin::StopAndDwellStrategicPluginConfig::stop_line_buffer, stop_and_dwell_strategic_plugin::StopAndDwellStrategicPluginConfig::vehicle_accel_limit_multiplier, and stop_and_dwell_strategic_plugin::StopAndDwellStrategicPluginConfig::vehicle_decel_limit_multiplier.

Referenced by on_configure_plugin().

Here is the caller graph for this function:

◆ plan_maneuvers_callback()

void stop_and_dwell_strategic_plugin::StopAndDwellStrategicPlugin::plan_maneuvers_callback	(	std::shared_ptr< rmw_request_id_t >	srv_header,
		carma_planning_msgs::srv::PlanManeuvers::Request::SharedPtr	req,
		carma_planning_msgs::srv::PlanManeuvers::Response::SharedPtr	resp
	)

virtual

Service callback for arbitrator maneuver planning.

Parameters

req	Plan maneuver request
resp	Plan maneuver response with a list of maneuver plan

Returns: If service call successed

Implements carma_guidance_plugins::StrategicPlugin.

Definition at line 239 of file stop_and_dwell_strategic_plugin.cpp.

{
  std::chrono::system_clock::time_point execution_start_time = std::chrono::system_clock::now();  // Start timing the execution time for planning so it can be logged
  
  RCLCPP_DEBUG_STREAM(rclcpp::get_logger(logger_name_), "<<<<<<<<<<<<<<<<< STARTING STOP_AND_DWELL_STRATEGIC_PLUIGN!!!!!!!!! >>>>>>>>>>>>>>>>");
 
  if (!wm_->getRoute())
  {
    RCLCPP_ERROR_STREAM(rclcpp::get_logger(logger_name_), "Could not plan maneuvers as route was not available");
    return;
  }
 
  RCLCPP_DEBUG_STREAM(rclcpp::get_logger(logger_name_), "Finding car information");
 
  // Extract vehicle data from request
  VehicleState current_state = extractInitialState(req);
 
  auto bus_stop_list = wm_->getBusStopsAlongRoute({ req->veh_x, req->veh_y });
 
  RCLCPP_DEBUG_STREAM(rclcpp::get_logger(logger_name_), "Found bus stops of size: " << bus_stop_list .size());
 
  if(bus_stop_list.empty())
  {
    RCLCPP_DEBUG_STREAM(rclcpp::get_logger(logger_name_), "Bus stops list is empty");
    return;
  }
 
  lanelet::BusStopRulePtr nearest_bus_stop = bus_stop_list.front();
 
  double bus_stop_downtrack_  = wm_->routeTrackPos(nearest_bus_stop->stopAndWaitLine().front().front().basicPoint2d()).downtrack;
  RCLCPP_DEBUG_STREAM(rclcpp::get_logger(logger_name_), "bus_stop_downtrack_ : " << bus_stop_downtrack_ );
  double distance_remaining_to_bus_stop = bus_stop_downtrack_  - current_state.downtrack;
  RCLCPP_DEBUG_STREAM(rclcpp::get_logger(logger_name_), "distance_remaining_to_bus_stop: " << distance_remaining_to_bus_stop <<
                    ", current_state.downtrack: " << current_state.downtrack);
 
  if (distance_remaining_to_bus_stop < -config_.bus_line_exit_zone_length)
  {
    resp->new_plan.maneuvers = {};
    RCLCPP_WARN_STREAM(rclcpp::get_logger(logger_name_), "Already passed bus stop, sending empty maneuvers");
    return;
  }                  
 
  constexpr double HALF_MPH_IN_MPS = 0.22352;  
 
  if (current_state.speed < HALF_MPH_IN_MPS && fabs(bus_stop_downtrack_ - current_state.downtrack ) < config_.stop_line_buffer) 
  {
    if(first_stop_)
    {
      time_to_move_ = now() + rclcpp::Duration(config_.dwell_time  * 1e9); 
      first_stop_ = false;
    }
 
    if(time_to_move_ <= now())
    {
      std::vector<lanelet::ConstLanelet> crossed_lanelets = getLaneletsBetweenWithException(current_state.downtrack, bus_stop_downtrack_, true, true);
      auto starting_lane_id = crossed_lanelets.front().id();
      auto ending_lane_id = crossed_lanelets.back().id();
      resp->new_plan.maneuvers.push_back(composeStopAndWaitManeuverMessage(current_state.downtrack ,bus_stop_downtrack_,current_state.speed,starting_lane_id,ending_lane_id,max_comfort_decel_norm_ ,now(),now() + rclcpp::Duration(config_.min_maneuver_planning_period * 1e9) )); 
    }
    else
    {
      double maneuver_end_distance = bus_stop_downtrack_ + config_.bus_line_exit_zone_length;
      std::vector<lanelet::ConstLanelet> crossed_lanelets = getLaneletsBetweenWithException(current_state.downtrack, maneuver_end_distance, true, true);
      std::vector<lanelet::Id> lane_ids = lanelet::utils::transform(crossed_lanelets, [](const auto& ll) { return ll.id(); });
      speed_limit_ = findSpeedLimit(crossed_lanelets.front());
      resp->new_plan.maneuvers.push_back(composeLaneFollowingManeuverMessage(current_state.downtrack ,maneuver_end_distance,current_state.speed,speed_limit_,now(),config_.min_maneuver_planning_period,lane_ids));
    }
  }
 else if ( current_state.downtrack>= ( bus_stop_downtrack_ - config_.activation_distance ))
  {
    double desired_distance_to_stop = pow(current_state.speed, 2)/(2 * max_comfort_decel_norm_ * config_.deceleration_fraction) + config_.desired_distance_to_stop_buffer;
    
    if(current_state.downtrack >= ( bus_stop_downtrack_ - desired_distance_to_stop))
    {
      std::vector<lanelet::ConstLanelet> crossed_lanelets = getLaneletsBetweenWithException(current_state.downtrack, bus_stop_downtrack_, true, true);
      auto starting_lane_id = crossed_lanelets.front().id();
      auto ending_lane_id = crossed_lanelets.back().id();
      rclcpp::Time start_time = now();
      rclcpp::Time end_time = now() + rclcpp::Duration(config_.min_maneuver_planning_period * 1e9) ;
      //Stop at desired distance before bus stop
      resp->new_plan.maneuvers.push_back(composeStopAndWaitManeuverMessage(current_state.downtrack ,bus_stop_downtrack_,current_state.speed,starting_lane_id,ending_lane_id,max_comfort_decel_norm_ ,start_time,end_time));
    }
    else
    {    
      double time_to_stop = (distance_remaining_to_bus_stop - desired_distance_to_stop)/speed_limit_;
      rclcpp::Time timestamp_to_stop = now() + rclcpp::Duration(time_to_stop * 1e9);
      std::vector<lanelet::ConstLanelet> crossed_lanelets = getLaneletsBetweenWithException(current_state.downtrack, (bus_stop_downtrack_ - desired_distance_to_stop) , true, true);
      std::vector<lanelet::ConstLanelet> crossed_lanelets_stop = getLaneletsBetweenWithException((bus_stop_downtrack_ - desired_distance_to_stop), bus_stop_downtrack_, true, true);
      std::vector<lanelet::Id> lane_ids = lanelet::utils::transform(crossed_lanelets, [](const auto& ll) { return ll.id(); });
      
      auto starting_lane_id = crossed_lanelets_stop.front().id();
      auto ending_lane_id = crossed_lanelets_stop.back().id();
 
      resp->new_plan.maneuvers.push_back(composeLaneFollowingManeuverMessage(current_state.downtrack ,(bus_stop_downtrack_ - desired_distance_to_stop),current_state.speed,speed_limit_,now(), time_to_stop,lane_ids));
      resp->new_plan.maneuvers.push_back(composeStopAndWaitManeuverMessage((bus_stop_downtrack_ - desired_distance_to_stop),bus_stop_downtrack_,speed_limit_,starting_lane_id,ending_lane_id,max_comfort_decel_norm_ ,timestamp_to_stop ,(timestamp_to_stop + rclcpp::Duration(config_.min_maneuver_planning_period * 1e9))));
    }
  }
  std::chrono::system_clock::time_point execution_end_time = std::chrono::system_clock::now();  // Planning complete
 
  auto execution_duration = execution_end_time - execution_start_time;
  RCLCPP_DEBUG_STREAM(rclcpp::get_logger(logger_name_), "ExecutionTime stop_and_dwell_strategic_plugin: " << std::chrono::duration<double>(execution_duration).count());
  return;
}