#include <route_following_plugin.hpp>

Inheritance diagram for route_following_plugin::RouteFollowingPlugin:

Collaboration diagram for route_following_plugin::RouteFollowingPlugin:

Public Member Functions
	RouteFollowingPlugin (const rclcpp::NodeOptions &)
	Default constructor for RouteFollowingPlugin class. 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
std::shared_ptr< carma_wm::WMListener >	wml_

carma_wm::WorldModelConstPtr	wm_

Private Member Functions
carma_planning_msgs::msg::Maneuver	composeLaneFollowingManeuverMessage (double start_dist, double end_dist, double start_speed, double target_speed, const std::vector< lanelet::Id > &lane_ids) const
	Compose a lane keeping maneuver message based on input params. More...

carma_planning_msgs::msg::Maneuver	composeLaneChangeManeuverMessage (double start_dist, double end_dist, double start_speed, double target_speed, lanelet::Id starting_lane_id, lanelet::Id ending_lane_id) const
	Compose a lane change maneuver message based on input params NOTE: The start and stop time are not set. This is because this is recomputed based on requests from the arbitrator. More...

carma_planning_msgs::msg::Maneuver	composeStopAndWaitManeuverMessage (double start_dist, double end_dist, double start_speed, lanelet::Id starting_lane_id, lanelet::Id ending_lane_id, double stopping_accel) const
	Compose a stop and wait maneuver message based on input params. NOTE: The start and stop time are not set. This is because this is recomputed based on requests from the arbitrator. More...

bool	isLaneChangeNeeded (lanelet::routing::LaneletRelations relations, lanelet::Id target_id) const
	Given a LaneletRelations and ID of the next lanelet in the shortest path. More...

void	setManeuverStartDist (carma_planning_msgs::msg::Maneuver &maneuver, double start_dist) const
	Set the start distance of a maneuver based on the progress along the route. More...

void	updateTimeProgress (std::vector< carma_planning_msgs::msg::Maneuver > &maneuvers, rclcpp::Time start_time) const
	Given an array of maneuvers update the starting time for each. More...

void	updateStartingSpeed (carma_planning_msgs::msg::Maneuver &maneuver, double start_speed) const
	Given an maneuver update the starting speed. 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...

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

std::vector< carma_planning_msgs::msg::Maneuver >	routeCb (const lanelet::routing::LaneletPath &route_shortest_path)
	Calculate maneuver plan for remaining route. This callback is triggered when a new route has been received and processed by the world model. More...

std::vector< carma_planning_msgs::msg::Maneuver >	addStopAndWaitAtRouteEnd (const std::vector< carma_planning_msgs::msg::Maneuver > &input_maneuvers, double route_end_downtrack, double stopping_entry_speed, double stopping_logitudinal_accel, double lateral_accel_limit, double min_maneuver_length) const
	Adds a StopAndWait maneuver to the end of a maneuver set stopping at the provided downtrack value NOTE: The priority of this method is to plan the stopping maneuver therefore earlier maneuvers will be modified or removed if required to allow the stopping behavior to be executed. More...

bool	maneuverWithBufferStartsAfterDowntrack (const carma_planning_msgs::msg::Maneuver &maneuver, double downtrack, double lateral_accel, double min_maneuver_length) const
	Identifies if a maneuver starts after the provided downtrack with compensation for a dynamic buffer size based on the maneuver type. More...

std::string	getNewManeuverId () const
	This method returns a new UUID as a string for assignment to a Maneuver message. More...

void	returnToShortestPath (const lanelet::ConstLanelet &current_lanelet)
	This method re-routes the vehicle back to the shortest path, if the vehicle has left the shortest path, but is still on the route Re-routing is performed by generating a new shortest path via the closest lanelet on the original shortest path. More...

void	bumper_pose_cb ()
	Callback for the front bumper pose transform. More...

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

void	current_maneuver_plan_cb (carma_planning_msgs::msg::ManeuverPlan::UniquePtr msg)
	Callback for the ManeuverPlan subscriber, will store the current maneuver plan received locally. Used as part of the detection system for differentiating leaving the shortest path via another plugin vs. control drift taking the vehicle's reference point outside of the intended lane. More...

rclcpp::Duration	getManeuverDuration (carma_planning_msgs::msg::Maneuver &maneuver, double epsilon) const
	returns duration as ros::Duration required to complete maneuver given its start dist, end dist, start speed and end speed More...

void	initializeBumperTransformLookup ()
	Initialize transform lookup from front bumper to map. More...

	FRIEND_TEST (RouteFollowingPluginTest, testComposeManeuverMessage)

	FRIEND_TEST (RouteFollowingPluginTest, testIdentifyLaneChange)

	FRIEND_TEST (RouteFollowingPlugin, TestAssociateSpeedLimit)

	FRIEND_TEST (RouteFollowingPlugin, TestAssociateSpeedLimitusingosm)

	FRIEND_TEST (RouteFollowingPlugin, TestHelperfunctions)

	FRIEND_TEST (RouteFollowingPlugin, TestReturnToShortestPath)

	FRIEND_TEST (StopAndWaitTestFixture, CaseOne)

	FRIEND_TEST (StopAndWaitTestFixture, CaseTwo)

	FRIEND_TEST (StopAndWaitTestFixture, CaseThree)

	FRIEND_TEST (StopAndWaitTestFixture, CaseFour)

	FRIEND_TEST (StopAndWaitTestFixture, CaseFive)

	FRIEND_TEST (StopAndWaitTestFixture, CaseSix)

	FRIEND_TEST (StopAndWaitTestFixture, CaseSeven)

	FRIEND_TEST (StopAndWaitTestFixture, CaseEight)

	FRIEND_TEST (StopAndWaitTestFixture, CaseNine)

	FRIEND_TEST (StopAndWaitTestFixture, CaseTen)

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

carma_ros2_utils::SubPtr< carma_planning_msgs::msg::ManeuverPlan >	current_maneuver_plan_sub_

std::unordered_set< lanelet::Id >	shortest_path_set_

Config	config_

double	current_speed_ = 0.0

double	epsilon_ = 0.0001

geometry_msgs::msg::PoseStamped	pose_msg_

lanelet::BasicPoint2d	current_loc_

carma_planning_msgs::msg::ManeuverPlan::UniquePtr	current_maneuver_plan_

std::vector< carma_planning_msgs::msg::Maneuver >	latest_maneuver_plan_

std::string	lane_change_plugin_ = "cooperative_lanechange"

std::string	stop_and_wait_plugin_ = "stop_and_wait_plugin"

std::string	planning_strategic_plugin_ = "route_following_plugin"

std::string	lanefollow_planning_tactical_plugin_ = "inlanecruising_plugin"

rclcpp::TimerBase::SharedPtr	bumper_pose_timer_

geometry_msgs::msg::TransformStamped	tf_

tf2::Stamped< tf2::Transform >	frontbumper_transform_

tf2_ros::Buffer	tf2_buffer_

std::unique_ptr< tf2_ros::TransformListener >	tf2_listener_

Static Private Attributes
static constexpr double	MAX_LANE_WIDTH = 3.70

Detailed Description

Definition at line 62 of file route_following_plugin.hpp.

Constructor & Destructor Documentation

◆ RouteFollowingPlugin()

route_following_plugin::RouteFollowingPlugin::RouteFollowingPlugin ( const rclcpp::NodeOptions & options )

explicit

Default constructor for RouteFollowingPlugin class.

Definition at line 88 of file route_following_plugin.cpp.

      : carma_guidance_plugins::StrategicPlugin(options), tf2_buffer_(get_clock()), config_(Config())
  {
    // Declare parameters
    config_.min_plan_duration_ = declare_parameter<double>("minimal_plan_duration", config_.min_plan_duration_);
    config_.lane_change_plugin_= declare_parameter<std::string>("lane_change_plugin", config_.lane_change_plugin_);
    config_.stop_and_wait_plugin_ = declare_parameter<std::string>("stop_and_wait_plugin", config_.stop_and_wait_plugin_);
    config_.lanefollow_planning_tactical_plugin_ = declare_parameter<std::string>("lane_following_plugin", config_.lanefollow_planning_tactical_plugin_);
    config_.route_end_point_buffer_ = declare_parameter<double>("guidance/route/destination_downtrack_range", config_.route_end_point_buffer_);
    config_.accel_limit_ = declare_parameter<double>("vehicle_acceleration_limit", config_.accel_limit_);
    config_.lateral_accel_limit_ = declare_parameter<double>("vehicle_lateral_accel_limit", config_.lateral_accel_limit_);
    config_.stopping_accel_limit_multiplier_ = declare_parameter<double>("stopping_accel_limit_multiplier", config_.stopping_accel_limit_multiplier_);
    config_.vehicle_id = declare_parameter<std::string>("vehicle_id", config_.vehicle_id);
    config_.min_maneuver_length_ = declare_parameter<double>("min_maneuver_length", config_.min_maneuver_length_);
  }

Member Function Documentation

◆ addStopAndWaitAtRouteEnd()

std::vector< carma_planning_msgs::msg::Maneuver > route_following_plugin::RouteFollowingPlugin::addStopAndWaitAtRouteEnd	(	const std::vector< carma_planning_msgs::msg::Maneuver > &	input_maneuvers,
		double	route_end_downtrack,
		double	stopping_entry_speed,
		double	stopping_logitudinal_accel,
		double	lateral_accel_limit,
		double	min_maneuver_length
	)		const

private

Adds a StopAndWait maneuver to the end of a maneuver set stopping at the provided downtrack value NOTE: The priority of this method is to plan the stopping maneuver therefore earlier maneuvers will be modified or removed if required to allow the stopping behavior to be executed.

Parameters

input_maneuvers	The set of maneuvers to modify to support the StopAndWait maneuver.
route_end_downtrack	The target stopping point (normally the end of the route) which the vehicle should stop before. Units meters
stopping_entry_speed	The expected entry speed for stopping. This is used to compute the stopping distance. Units m/s
stopping_logitudinal_accel	The target deceleration (unsigned) for the stopping operation. Units m/s/s
lateral_accel_limit	The lateral acceleration limit allowed for lane changes. Units m/s/s
min_maneuver_length	The absolute minimum allowable maneuver length for any existing maneuvers in meters

NOTE: Only min_maneuver_length can be a zero-valued input. All other parameters must be positive values greater than zero.

Exceptions

std::invalid_argument If existing maneuvers cannot be modified to allow stopping maneuver creation, or if the generated maneuvers do not overlap any lanelets in the map.

Returns: A list of maneuvers which mirrors the input list but with the modifications required to include a stopping maneuver at the end

ASSUMPTION: At the moment the stopping entry speed is not updated because the assumption is that any previous maneuvers which were slower need not be accounted for as planning for a higher speed will always be capable of handling that case and any which were faster would already have their speed reduced by the maneuver which this speed was derived from.

Alogirthm in this block is as follows

Compute distance to slowdown to stop at acceleration limit
Drop maneuvers which are wholly encompassed in stopping distance
If end downtrack - stop distance is > last maneuver end distance then fill delta with lane follow followed by stop and wait
If end downtrack - stop distance is < last maneuver end distance then reduce the existing maneuver to allow for stopping maneuver to be planned
Add maneuver to list

Definition at line 265 of file route_following_plugin.cpp.

    {
        RCLCPP_INFO_STREAM(get_logger(),"Attempting to plan Stop and Wait Maneuver");
        std::vector<carma_planning_msgs::msg::Maneuver> maneuvers = input_maneuvers; // Output maneuvers which will be modified
 
 
        // Compute stopping distance where v_f = 0
        // (v_f^2 - v_i^2) / (2*a) = d 
        double stopping_distance = 0.5 * (stopping_entry_speed * stopping_entry_speed) / stopping_logitudinal_accel;
 
        // Compute required starting downtrack for maneuver
        double required_start_downtrack = std::max(0.0, route_end_downtrack - stopping_distance);
 
        // Loop to drop any maneuvers which fully overlap our stopping maneuver while accounting for minimum length maneuver buffers
        while ( !maneuvers.empty() && maneuverWithBufferStartsAfterDowntrack(maneuvers.back(), required_start_downtrack, lateral_accel_limit, config_.min_maneuver_length_) ) { 
            
            RCLCPP_WARN_STREAM(get_logger(),"Dropping maneuver with id: " <<  GET_MANEUVER_PROPERTY(maneuvers.back(), parameters.maneuver_id) );
 
            if (maneuvers.back().type == carma_planning_msgs::msg::Maneuver::LANE_CHANGE) {
 
                // TODO develop more robust approach for this case per: https://github.com/usdot-fhwa-stol/carma-platform/issues/1350
                throw std::invalid_argument("Stopping at the end of the route requires replanning a lane change. RouteFollowing cannot yet handle this case");
            }
 
            maneuvers.pop_back(); // Drop maneuver
 
        }
 
        double last_maneuver_end_downtrack =  required_start_downtrack; // Set default starting location for stop and wait maneuver 
 
        if ( !maneuvers.empty() ) { // If there are existing maneuvers we need to make sure stop and wait does not overwrite them
            
            last_maneuver_end_downtrack = GET_MANEUVER_PROPERTY(maneuvers.back(), end_dist);
            
            // If our stopping maneuver does not intersect with existing maneuvers
            if ( required_start_downtrack >=  last_maneuver_end_downtrack ) {  
                
                // If the delta is under minimum_maneuver_length we can just extend the stopping maneuver
                // Otherwise add a new lane follow maneuver
                if (required_start_downtrack - last_maneuver_end_downtrack > min_maneuver_length) {
                 
                    // Identify the lanelets which will be crossed by this lane follow maneuver
                    std::vector<lanelet::ConstLanelet> crossed_lanelets = wm_->getLaneletsBetween(last_maneuver_end_downtrack, required_start_downtrack, true, false); 
 
                    if (crossed_lanelets.empty()) {
                        throw std::invalid_argument("The new lane follow maneuver does not cross any lanelets going from: " + std::to_string(last_maneuver_end_downtrack) + " to: " + std::to_string(required_start_downtrack));
                    }
 
                    // Create the lane follow maneuver
                    maneuvers.push_back(composeLaneFollowingManeuverMessage(last_maneuver_end_downtrack, required_start_downtrack, stopping_entry_speed, stopping_entry_speed, lanelet::utils::transform(crossed_lanelets, [](auto ll) { return ll.id(); })));
                    
                    // Update last maneuver end downtrack so the stop and wait maneuver can be properly formulated
                    last_maneuver_end_downtrack = required_start_downtrack; 
                } else {
                    RCLCPP_DEBUG_STREAM(get_logger(),"Stop and wait maneuver being extended to nearest maneuver which is closer than the minimum maneuver length");
                }
 
            } else { // If our stopping maneuver intersects with existing maneuvers
 
                                    
                SET_MANEUVER_PROPERTY(maneuvers.back(), end_dist, required_start_downtrack);
 
                // Identify the lanelets which will be crossed by this updated maneuver
                std::vector<lanelet::ConstLanelet> crossed_lanelets = wm_->getLaneletsBetween(GET_MANEUVER_PROPERTY(maneuvers.back(), start_dist), required_start_downtrack, true, false);
 
                if (crossed_lanelets.empty()) {
                    throw std::invalid_argument("Updated maneuver does not cross any lanelets going from: " + std::to_string(GET_MANEUVER_PROPERTY(maneuvers.back(), start_dist)) + " to: " + std::to_string(required_start_downtrack));
                }
 
                // Set the impact lane ids for maneuvers
                if (maneuvers.back().type == carma_planning_msgs::msg::Maneuver::LANE_FOLLOWING) {
 
                    maneuvers.back().lane_following_maneuver.lane_ids = lanelet::utils::transform(crossed_lanelets, [](auto ll) { return std::to_string(ll.id()); });
 
                } else {
 
                    setManeuverLaneletIds(maneuvers.back(), crossed_lanelets.front().id(), crossed_lanelets.back().id());
 
                }
 
                last_maneuver_end_downtrack = required_start_downtrack;
 
            }   
        }
        
        // Identify the lanelets which will be crossed by this stop and wait maneuver
        std::vector<lanelet::ConstLanelet> crossed_lanelets = wm_->getLaneletsBetween(last_maneuver_end_downtrack, route_end_downtrack, true, false);
 
        if (crossed_lanelets.empty()) {
 
            throw std::invalid_argument("Stopping maneuver does not cross any lanelets going from: " + std::to_string(last_maneuver_end_downtrack) + " to: " + std::to_string(route_end_downtrack));
 
        }
 
        lanelet::Id start_lane = crossed_lanelets.front().id();
        lanelet::Id end_lane = crossed_lanelets.back().id();
 
        // Build stop and wait maneuver
        maneuvers.push_back(composeStopAndWaitManeuverMessage(last_maneuver_end_downtrack, route_end_downtrack, stopping_entry_speed, start_lane, end_lane,stopping_logitudinal_accel));
 
        return maneuvers;
 
    }

References composeLaneFollowingManeuverMessage(), composeStopAndWaitManeuverMessage(), config_, GET_MANEUVER_PROPERTY, maneuverWithBufferStartsAfterDowntrack(), route_following_plugin::Config::min_maneuver_length_, SET_MANEUVER_PROPERTY, route_following_plugin::anonymous_namespace{route_following_plugin.cpp}::setManeuverLaneletIds(), carma_cooperative_perception::to_string(), and wm_.

Referenced by routeCb().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ bumper_pose_cb()

void route_following_plugin::RouteFollowingPlugin::bumper_pose_cb ( )

private

Callback for the front bumper pose transform.

Definition at line 531 of file route_following_plugin.cpp.

    {
        RCLCPP_DEBUG_STREAM(get_logger(),"Entering pose_cb");
 
        RCLCPP_DEBUG_STREAM(get_logger(),"Looking up front bumper pose...");
        
        try
        {
            tf_ = tf2_buffer_.lookupTransform("map", "vehicle_front", rclcpp::Time(0), rclcpp::Duration(1, 0)); //save to local copy of transform 1 sec timeout
            tf2::fromMsg(tf_, frontbumper_transform_);
        }
        catch (const tf2::TransformException &ex)
        {
            RCLCPP_WARN_STREAM(get_logger(),std::string(ex.what()));
        }
 
        geometry_msgs::msg::Pose front_bumper_pose;
        front_bumper_pose.position.x = frontbumper_transform_.getOrigin().getX();
        front_bumper_pose.position.y = frontbumper_transform_.getOrigin().getY();
        
        if (!wm_->getRoute())
            return;
 
        lanelet::BasicPoint2d current_loc(front_bumper_pose.position.x, front_bumper_pose.position.y);
        current_loc_ = current_loc;
        double current_progress = wm_->routeTrackPos(current_loc).downtrack;
        
        RCLCPP_DEBUG_STREAM(get_logger(),"pose_cb : current_progress" << current_progress);
        
        // Check if we need to return to route shortest path.
        // Step 1. Check if another plugin aside from RFP has been in control
        if (current_maneuver_plan_  != nullptr &&
            GET_MANEUVER_PROPERTY(current_maneuver_plan_->maneuvers[0], parameters.planning_strategic_plugin) \
            != planning_strategic_plugin_) {
            // If another plugin may have brought us off shortest path, check our current lanelet
            auto llts = wm_->getLaneletsFromPoint(current_loc, 10);                                          
            // Remove any candidate lanelets not on the route
            llts.erase(std::remove_if(llts.begin(), llts.end(),
                [&](auto lanelet) -> bool { return !wm_->getRoute()->contains(lanelet); }),
                llts.end());
 
            // !!! ASSUMPTION !!!:
            // Once non-route lanelets have been removed, it is assumed that our actual current lanelet is the only one that can remain.
            // TODO: Verify that this assumption is true in all cases OR implement more robust logic to track current lanelet when there are overlaps
 
            if (llts.size() > 1) {
                // Assumed that:
                // 1. Vehicle is in a lanelet on the route.
                // 2. The route does not contain overlapping lanelets.
               RCLCPP_WARN_STREAM(get_logger(),"ANOMALOUS SIZE DETECTED FOR CURRENT LANELET CANDIDATES! SIZE: " << llts.size());
            } else if (llts.size() < 1) {
                //  We've left the route entirely.
                RCLCPP_ERROR_STREAM(get_logger(),"Vehicle has left the route entirely. Unable to compute new shortest path.");
                throw std::domain_error("Vehicle not on route, unable to compute shortest path.");
            }
 
            const auto& current_lanelet = llts[0];
            RCLCPP_DEBUG_STREAM(get_logger(), "Vehicle is currently in lanelet " << current_lanelet.id());
 
            // if the current lanelet is not on the shortest path
            if (shortest_path_set_.find(current_lanelet.id()) == shortest_path_set_.end())
            {
                RCLCPP_DEBUG_STREAM(get_logger(), "Generating a new shortest path since the vehicle is not on the shortest path");
                returnToShortestPath(current_lanelet);
            }
 
            // Replan if vehicle currently located in a lane change planned by route_following_plugin, since a lane change maneuver can't begin part way through
            for(size_t i = 0; i < latest_maneuver_plan_.size(); ++i){
                if((GET_MANEUVER_PROPERTY(latest_maneuver_plan_[i], start_dist) <= current_progress) && (current_progress <= GET_MANEUVER_PROPERTY(latest_maneuver_plan_[i], end_dist))){
                    if(latest_maneuver_plan_[i].type == carma_planning_msgs::msg::Maneuver::LANE_CHANGE){
                        RCLCPP_DEBUG_STREAM(get_logger(), "Generating a new shortest path since vehicle is positioned in a lane change but the previous maneuver plan was not generated by route_following_plugin");
                        returnToShortestPath(current_lanelet);
                    }
                }
            }
        }
    }

References current_loc_, current_maneuver_plan_, frontbumper_transform_, GET_MANEUVER_PROPERTY, process_bag::i, latest_maneuver_plan_, planning_strategic_plugin_, returnToShortestPath(), shortest_path_set_, tf2_buffer_, tf_, and wm_.

Referenced by on_activate_plugin().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ composeLaneChangeManeuverMessage()

carma_planning_msgs::msg::Maneuver route_following_plugin::RouteFollowingPlugin::composeLaneChangeManeuverMessage	(	double	start_dist,
		double	end_dist,
		double	start_speed,
		double	target_speed,
		lanelet::Id	starting_lane_id,
		lanelet::Id	ending_lane_id
	)		const

private

Compose a lane change maneuver message based on input params NOTE: The start and stop time are not set. This is because this is recomputed based on requests from the arbitrator.

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
start_lane_id	Starting Lanelet ID of the current maneuver
ending_lane_id	Ending Lanelet ID of the current maneuver
target_speed	Target speed of the current maneuver

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

Definition at line 753 of file route_following_plugin.cpp.

    {
        carma_planning_msgs::msg::Maneuver maneuver_msg;
        maneuver_msg.type = carma_planning_msgs::msg::Maneuver::LANE_CHANGE;
        maneuver_msg.lane_change_maneuver.parameters.negotiation_type = carma_planning_msgs::msg::ManeuverParameters::NO_NEGOTIATION;
        maneuver_msg.lane_change_maneuver.parameters.presence_vector = carma_planning_msgs::msg::ManeuverParameters::HAS_TACTICAL_PLUGIN;
        maneuver_msg.lane_change_maneuver.parameters.planning_tactical_plugin = lane_change_plugin_;
        maneuver_msg.lane_change_maneuver.parameters.planning_strategic_plugin = planning_strategic_plugin_;
        maneuver_msg.lane_change_maneuver.start_dist = start_dist;
        maneuver_msg.lane_change_maneuver.start_speed = start_speed;
        maneuver_msg.lane_change_maneuver.end_dist = end_dist;
        maneuver_msg.lane_change_maneuver.end_speed = target_speed;
        maneuver_msg.lane_change_maneuver.starting_lane_id = std::to_string(starting_lane_id);
        maneuver_msg.lane_change_maneuver.ending_lane_id = std::to_string(ending_lane_id);
        //Start time and end time for maneuver are assigned in updateTimeProgress
 
        // NOTE: The maneuver id is set here because maneuvers are regenerated once per route, so it is acceptable to regenerate them on route updates.
        //       If maneuvers were not generated only on route updates we would want to preserve the ids across plans
        maneuver_msg.lane_change_maneuver.parameters.maneuver_id = getNewManeuverId();
 
        RCLCPP_DEBUG_STREAM(get_logger(),"Creating lane change id: "  << maneuver_msg.lane_change_maneuver.parameters.maneuver_id << "start dist: " << start_dist << " end dist: " << end_dist << " Starting llt: " << starting_lane_id << " Ending llt: " << ending_lane_id);
 
        return maneuver_msg;
    }

References getNewManeuverId(), lane_change_plugin_, planning_strategic_plugin_, and carma_cooperative_perception::to_string().

Referenced by routeCb().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ composeLaneFollowingManeuverMessage()

carma_planning_msgs::msg::Maneuver route_following_plugin::RouteFollowingPlugin::composeLaneFollowingManeuverMessage	(	double	start_dist,
		double	end_dist,
		double	start_speed,
		double	target_speed,
		const std::vector< lanelet::Id > &	lane_ids
	)		const

private

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
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 724 of file route_following_plugin.cpp.

    {
        carma_planning_msgs::msg::Maneuver maneuver_msg;
        maneuver_msg.type = carma_planning_msgs::msg::Maneuver::LANE_FOLLOWING;
        maneuver_msg.lane_following_maneuver.parameters.negotiation_type = carma_planning_msgs::msg::ManeuverParameters::NO_NEGOTIATION;
        maneuver_msg.lane_following_maneuver.parameters.presence_vector = carma_planning_msgs::msg::ManeuverParameters::HAS_TACTICAL_PLUGIN;
        maneuver_msg.lane_following_maneuver.parameters.planning_tactical_plugin = lanefollow_planning_tactical_plugin_;
        maneuver_msg.lane_following_maneuver.parameters.planning_strategic_plugin = planning_strategic_plugin_;
        maneuver_msg.lane_following_maneuver.start_dist = start_dist;
        maneuver_msg.lane_following_maneuver.start_speed = start_speed;
        maneuver_msg.lane_following_maneuver.end_dist = end_dist;
        maneuver_msg.lane_following_maneuver.end_speed = target_speed;
        maneuver_msg.lane_following_maneuver.lane_ids = lanelet::utils::transform(lane_ids, [](auto id) { return std::to_string(id); });
        //Start time and end time for maneuver are assigned in updateTimeProgress
 
        // NOTE: The maneuver id is set here because maneuvers are regenerated once per route, so it is acceptable to regenerate them on route updates.
        //       If maneuvers were not generated only on route updates we would want to preserve the ids across plans
        maneuver_msg.lane_following_maneuver.parameters.maneuver_id = getNewManeuverId();
 
        std::stringstream ss;
        for (const auto& id : maneuver_msg.lane_following_maneuver.lane_ids)
            ss << " " << id;
 
        RCLCPP_DEBUG_STREAM(get_logger(),"Creating lane follow id: " << maneuver_msg.lane_following_maneuver.parameters.maneuver_id 
                        << " start dist: " << start_dist << " end dist: " << end_dist << "lane_ids: " << ss.str());
        
        return maneuver_msg;
    }

References getNewManeuverId(), lanefollow_planning_tactical_plugin_, planning_strategic_plugin_, and carma_cooperative_perception::to_string().

Referenced by addStopAndWaitAtRouteEnd(), and routeCb().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ composeStopAndWaitManeuverMessage()

carma_planning_msgs::msg::Maneuver route_following_plugin::RouteFollowingPlugin::composeStopAndWaitManeuverMessage	(	double	start_dist,
		double	end_dist,
		double	start_speed,
		lanelet::Id	starting_lane_id,
		lanelet::Id	ending_lane_id,
		double	stopping_accel
	)		const

private

Compose a stop and wait maneuver message based on input params. NOTE: The start and stop time are not set. This is because this is recomputed based on requests from the arbitrator.

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
start_lane_id	Starting Lanelet ID of the current maneuver
ending_lane_id	Ending Lanelet ID of the current maneuver
stopping_accel	Acceleration used for calculating the stopping distance

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

Definition at line 784 of file route_following_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.negotiation_type = carma_planning_msgs::msg::ManeuverParameters::NO_NEGOTIATION;
        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.planning_tactical_plugin = stop_and_wait_plugin_;
        maneuver_msg.stop_and_wait_maneuver.parameters.planning_strategic_plugin = planning_strategic_plugin_;
        maneuver_msg.stop_and_wait_maneuver.start_dist = start_dist;
        maneuver_msg.stop_and_wait_maneuver.start_speed = start_speed;
        maneuver_msg.stop_and_wait_maneuver.end_dist = end_dist;
        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);
        //Start time and end time for maneuver are assigned in updateTimeProgress
 
        // Set the meta-data for the StopAndWait Maneuver to define the buffer in the route end point stopping location
        maneuver_msg.stop_and_wait_maneuver.parameters.float_valued_meta_data.push_back(config_.route_end_point_buffer_);
        maneuver_msg.stop_and_wait_maneuver.parameters.float_valued_meta_data.push_back(stopping_accel);
        
        // NOTE: The maneuver id is set here because maneuvers are regenerated once per route, so it is acceptable to regenerate them on route updates.
        //       If maneuvers were not generated only on route updates we would want to preserve the ids across plans
        maneuver_msg.stop_and_wait_maneuver.parameters.maneuver_id = getNewManeuverId();
 
        RCLCPP_DEBUG_STREAM(get_logger(),"Creating stop and wait maneuver id:" << maneuver_msg.stop_and_wait_maneuver.parameters.maneuver_id << "start dist: " << start_dist << " end dist: " << end_dist << " start_speed: " << start_speed << " Starting llt: " << starting_lane_id << " Ending llt: " << ending_lane_id);
 
        return maneuver_msg;
    }

References config_, getNewManeuverId(), planning_strategic_plugin_, route_following_plugin::Config::route_end_point_buffer_, stop_and_wait_plugin_, and carma_cooperative_perception::to_string().

Referenced by addStopAndWaitAtRouteEnd().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ current_maneuver_plan_cb()

void route_following_plugin::RouteFollowingPlugin::current_maneuver_plan_cb ( carma_planning_msgs::msg::ManeuverPlan::UniquePtr msg )

private

Callback for the ManeuverPlan subscriber, will store the current maneuver plan received locally. Used as part of the detection system for differentiating leaving the shortest path via another plugin vs. control drift taking the vehicle's reference point outside of the intended lane.

Parameters

msg	Latest ManeuverPlan message

Definition at line 165 of file route_following_plugin.cpp.

                                                                                                       {
        current_maneuver_plan_ = std::move(msg);
    }

References current_maneuver_plan_.

Referenced by on_configure_plugin().

Here is the caller graph for this function:

◆ findSpeedLimit()

double route_following_plugin::RouteFollowingPlugin::findSpeedLimit ( const lanelet::ConstLanelet & llt )

private

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

Parameters

llt	Constant Lanelet object

Returns: value of speed limit in mps

Definition at line 826 of file route_following_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 routeCb().

Here is the caller graph for this function:

◆ FRIEND_TEST() [1/16]

route_following_plugin::RouteFollowingPlugin::FRIEND_TEST	(	RouteFollowingPlugin	,
		TestAssociateSpeedLimit
	)

private

◆ FRIEND_TEST() [2/16]

route_following_plugin::RouteFollowingPlugin::FRIEND_TEST	(	RouteFollowingPlugin	,
		TestAssociateSpeedLimitusingosm
	)

private

◆ FRIEND_TEST() [3/16]

route_following_plugin::RouteFollowingPlugin::FRIEND_TEST	(	RouteFollowingPlugin	,
		TestHelperfunctions
	)

private

◆ FRIEND_TEST() [4/16]

route_following_plugin::RouteFollowingPlugin::FRIEND_TEST	(	RouteFollowingPlugin	,
		TestReturnToShortestPath
	)

private

◆ FRIEND_TEST() [5/16]

route_following_plugin::RouteFollowingPlugin::FRIEND_TEST	(	RouteFollowingPluginTest	,
		testComposeManeuverMessage
	)

private

◆ FRIEND_TEST() [6/16]

route_following_plugin::RouteFollowingPlugin::FRIEND_TEST	(	RouteFollowingPluginTest	,
		testIdentifyLaneChange
	)

private

◆ FRIEND_TEST() [7/16]

route_following_plugin::RouteFollowingPlugin::FRIEND_TEST	(	StopAndWaitTestFixture	,
		CaseEight
	)

private

◆ FRIEND_TEST() [8/16]

route_following_plugin::RouteFollowingPlugin::FRIEND_TEST	(	StopAndWaitTestFixture	,
		CaseFive
	)

private

◆ FRIEND_TEST() [9/16]

route_following_plugin::RouteFollowingPlugin::FRIEND_TEST	(	StopAndWaitTestFixture	,
		CaseFour
	)

private

◆ FRIEND_TEST() [10/16]

route_following_plugin::RouteFollowingPlugin::FRIEND_TEST	(	StopAndWaitTestFixture	,
		CaseNine
	)

private

◆ FRIEND_TEST() [11/16]

route_following_plugin::RouteFollowingPlugin::FRIEND_TEST	(	StopAndWaitTestFixture	,
		CaseOne
	)

private

◆ FRIEND_TEST() [12/16]

route_following_plugin::RouteFollowingPlugin::FRIEND_TEST	(	StopAndWaitTestFixture	,
		CaseSeven
	)

private

◆ FRIEND_TEST() [13/16]

route_following_plugin::RouteFollowingPlugin::FRIEND_TEST	(	StopAndWaitTestFixture	,
		CaseSix
	)

private

◆ FRIEND_TEST() [14/16]

route_following_plugin::RouteFollowingPlugin::FRIEND_TEST	(	StopAndWaitTestFixture	,
		CaseTen
	)

private

◆ FRIEND_TEST() [15/16]

route_following_plugin::RouteFollowingPlugin::FRIEND_TEST	(	StopAndWaitTestFixture	,
		CaseThree
	)

private

◆ FRIEND_TEST() [16/16]

route_following_plugin::RouteFollowingPlugin::FRIEND_TEST	(	StopAndWaitTestFixture	,
		CaseTwo
	)

private

◆ get_availability()

bool route_following_plugin::RouteFollowingPlugin::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 403 of file route_following_plugin.cpp.

        {
         return true;
        }

◆ get_version_id()

std::string route_following_plugin::RouteFollowingPlugin::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 408 of file route_following_plugin.cpp.

        {
                return "v1.0";
        }

◆ getManeuverDuration()

rclcpp::Duration route_following_plugin::RouteFollowingPlugin::getManeuverDuration	(	carma_planning_msgs::msg::Maneuver &	maneuver,
		double	epsilon
	)		const

private

returns duration as ros::Duration required to complete maneuver given its start dist, end dist, start speed and end speed

Parameters

maneuver	The maneuver message to calculate duration for
epsilon	The acceptable min start_speed + target_speed in the maneuver message, under which the maneuver is treated as faulty. Throws exception if sum of start and target speed of maneuver is below limit defined by parameter epsilon

Definition at line 609 of file route_following_plugin.cpp.

    {
        double maneuver_start_speed = GET_MANEUVER_PROPERTY(maneuver, start_speed);
        double manever_end_speed = getManeuverEndSpeed(maneuver);
        double cur_plus_target = maneuver_start_speed + manever_end_speed;
        if(cur_plus_target < epsilon){
            throw std::invalid_argument("Maneuver start and ending speed is zero");
        }
        rclcpp::Duration duration{0,0};
        double maneuver_start_dist = GET_MANEUVER_PROPERTY(maneuver, start_dist);
        double maneuver_end_dist = GET_MANEUVER_PROPERTY(maneuver, end_dist);
 
        RCLCPP_DEBUG_STREAM(get_logger(),"maneuver_end_dist: " << maneuver_end_dist << ", maneuver_start_dist: " << maneuver_start_dist << ", cur_plus_target: " << cur_plus_target);
 
        duration = rclcpp::Duration((maneuver_end_dist - maneuver_start_dist) / (0.5 * cur_plus_target) * 1e9);
 
        return duration;
    }

References GET_MANEUVER_PROPERTY, and route_following_plugin::anonymous_namespace{route_following_plugin.cpp}::getManeuverEndSpeed().

Referenced by plan_maneuvers_callback(), and updateTimeProgress().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ getNewManeuverId()

std::string route_following_plugin::RouteFollowingPlugin::getNewManeuverId ( ) const

private

This method returns a new UUID as a string for assignment to a Maneuver message.

Returns: A new UUID as a string

Definition at line 778 of file route_following_plugin.cpp.

                                                           {
        static auto gen = boost::uuids::random_generator(); // Initialize uuid generator
        
        return boost::lexical_cast<std::string>(gen()); // generate uuid and convert to string
    }

Referenced by composeLaneChangeManeuverMessage(), composeLaneFollowingManeuverMessage(), and composeStopAndWaitManeuverMessage().

Here is the caller graph for this function:

◆ initializeBumperTransformLookup()

void route_following_plugin::RouteFollowingPlugin::initializeBumperTransformLookup ( )

private

Initialize transform lookup from front bumper to map.

Definition at line 839 of file route_following_plugin.cpp.

    {
        tf2_listener_.reset(new tf2_ros::TransformListener(tf2_buffer_));
        tf2_buffer_.setUsingDedicatedThread(true);
    }

References tf2_buffer_, and tf2_listener_.

Referenced by on_configure_plugin().

Here is the caller graph for this function:

◆ isLaneChangeNeeded()

bool route_following_plugin::RouteFollowingPlugin::isLaneChangeNeeded	(	lanelet::routing::LaneletRelations	relations,
		lanelet::Id	target_id
	)		const

private

Given a LaneletRelations and ID of the next lanelet in the shortest path.

Parameters

relations	LaneletRelations relative to the previous lanelet
target_id	ID of the next lanelet in the shortest path

Returns: Whether we need a lanechange to reach to the next lanelet in the shortest path.

Definition at line 813 of file route_following_plugin.cpp.

    {
        //This method is constrained to the lanelet being checked against being accessible. A non-accessible target lanelet would result in unspecified behavior
        for (auto &relation : relations)
        {
            if (relation.lanelet.id() == target_id && relation.relationType == lanelet::routing::RelationType::Successor)
            {
                return false;
            }
        }
        return true;
    }

Referenced by routeCb().

Here is the caller graph for this function:

◆ maneuverWithBufferStartsAfterDowntrack()

bool route_following_plugin::RouteFollowingPlugin::maneuverWithBufferStartsAfterDowntrack	(	const carma_planning_msgs::msg::Maneuver &	maneuver,
		double	downtrack,
		double	lateral_accel,
		double	min_maneuver_length
	)		const

private

Identifies if a maneuver starts after the provided downtrack with compensation for a dynamic buffer size based on the maneuver type.

Parameters

maneuver	The maneuver to compare
downtrack	The downtrack value to evaluate in meters
lateral_accel	The max lateral acceleration allowed for lane changes in m/s/s
min_maneuver_length	The absolute minimum allowable for any maneuver in meters

Returns: true if the provided maneuver plus the computed dynamic buffer starts after the provided downtrack value

Definition at line 381 of file route_following_plugin.cpp.

                                                                                                                                                                                              {
       
        if (maneuver.type == carma_planning_msgs::msg::Maneuver::LANE_CHANGE) {
                
            // Compute the time it takes to move laterally to the next lane
            double lane_change_time = sqrt(0.5 * MAX_LANE_WIDTH / lateral_accel);
            
            // Compute logitudinal distance covered in lane change time
            double min_lane_change_distance = std::max(
                min_maneuver_length, 
                lane_change_time * (GET_MANEUVER_PROPERTY(maneuver, start_speed) + getManeuverEndSpeed(maneuver)) / 2.0 // dist = v_avg * t
            );
 
            return GET_MANEUVER_PROPERTY(maneuver, start_dist) + min_lane_change_distance > downtrack;
 
        } else { 
            
            return GET_MANEUVER_PROPERTY(maneuver, start_dist) + min_maneuver_length > downtrack; 
        
        }
    }

References GET_MANEUVER_PROPERTY, route_following_plugin::anonymous_namespace{route_following_plugin.cpp}::getManeuverEndSpeed(), and MAX_LANE_WIDTH.

Referenced by addStopAndWaitAtRouteEnd().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ on_activate_plugin()

carma_ros2_utils::CallbackReturn route_following_plugin::RouteFollowingPlugin::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 150 of file route_following_plugin.cpp.

    {
        bumper_pose_timer_ = create_timer(get_clock(),
            std::chrono::milliseconds(100),
            std::bind(&RouteFollowingPlugin::bumper_pose_cb, this));
 
        // Return success if everthing initialized successfully
        return CallbackReturn::SUCCESS;
    }

References bumper_pose_cb(), and bumper_pose_timer_.

Here is the call graph for this function:

◆ on_configure_plugin()

carma_ros2_utils::CallbackReturn route_following_plugin::RouteFollowingPlugin::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 104 of file route_following_plugin.cpp.

  {
    config_ = Config();
 
    get_parameter<double>("minimal_plan_duration", config_.min_plan_duration_);
    get_parameter<std::string>("lane_change_plugin", config_.lane_change_plugin_);
    get_parameter<std::string>("stop_and_wait_plugin", config_.stop_and_wait_plugin_);
    get_parameter<std::string>("lane_following_plugin", config_.lanefollow_planning_tactical_plugin_);
    get_parameter<double>("guidance/route/destination_downtrack_range", config_.route_end_point_buffer_);
    get_parameter<double>("vehicle_acceleration_limit", config_.accel_limit_);
    get_parameter<double>("vehicle_lateral_accel_limit", config_.lateral_accel_limit_);
    get_parameter<double>("stopping_accel_limit_multiplier", config_.stopping_accel_limit_multiplier_);
    get_parameter<double>("min_maneuver_length", config_.min_maneuver_length_);
    
    RCLCPP_INFO_STREAM(rclcpp::get_logger("route_following_plugin"), "RouteFollowingPlugin Config: " << config_);
 
    // Setup subscribers
    twist_sub_ = create_subscription<geometry_msgs::msg::TwistStamped>("current_velocity", 50,
                                                              std::bind(&RouteFollowingPlugin::twist_cb,this,std_ph::_1));
    current_maneuver_plan_sub_ = create_subscription<carma_planning_msgs::msg::ManeuverPlan>("maneuver_plan", 50,
                                                              std::bind(&RouteFollowingPlugin::current_maneuver_plan_cb,this,std_ph::_1));
    
    // set world model point form wm listener
    wml_ = get_world_model_listener();
 
    wm_ = get_world_model();
 
    //set a route callback to update route and calculate maneuver
    wml_->setRouteCallback([this]() {
        RCLCPP_INFO_STREAM(get_logger(),"Recomputing maneuvers due to a route update");
        this->latest_maneuver_plan_ = routeCb(wm_->getRoute()->shortestPath());
    });
 
    wml_->setMapCallback([this]() {
        if (wm_->getRoute()) { // If this map update occured after a route was provided we need to regenerate maneuvers
            RCLCPP_INFO_STREAM(get_logger(),"Recomputing maneuvers due to map update");
            this->latest_maneuver_plan_ = routeCb(wm_->getRoute()->shortestPath());
        }
    });
 
    initializeBumperTransformLookup();
    
    // Return success if everthing initialized successfully
    return CallbackReturn::SUCCESS;
  }

Here is the call graph for this function:

◆ plan_maneuvers_callback()

void route_following_plugin::RouteFollowingPlugin::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
	)

privatevirtual

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 413 of file route_following_plugin.cpp.

      {
 
        if (latest_maneuver_plan_.empty())
        {
           RCLCPP_ERROR_STREAM(get_logger(),"A maneuver plan has not been generated");
            return;
        }
 
        double current_downtrack;
        
        if (!req->prior_plan.maneuvers.empty())
        {
            current_downtrack = GET_MANEUVER_PROPERTY(req->prior_plan.maneuvers.back(), end_dist);
            RCLCPP_DEBUG_STREAM(get_logger(),"Detected a prior plan! Using back maneuver's end_dist:"<< current_downtrack);            
        }
        else
        {
            current_downtrack = req->veh_downtrack;
            RCLCPP_DEBUG_STREAM(get_logger(),"Detected NO prior plan! Using req.veh_downtrack: "<< current_downtrack);
        }
        
        //Return the set of maneuvers which intersect with min_plan_duration
        size_t i = 0;
        double planned_time = 0.0;
 
        std::vector<carma_planning_msgs::msg::Maneuver> new_maneuvers;
 
        while (planned_time < config_.min_plan_duration_ && i < latest_maneuver_plan_.size())
        {
            RCLCPP_DEBUG_STREAM(get_logger(),"Checking maneuver id " << i);
            //Ignore plans for distance already covered
            if (GET_MANEUVER_PROPERTY(latest_maneuver_plan_[i], end_dist) <= current_downtrack)
            {
                RCLCPP_DEBUG_STREAM(get_logger(),"Skipping maneuver id " << i);
 
                ++i;
                continue;
            }
            if(planned_time == 0.0){
                //update start distance of first maneuver
                setManeuverStartDist(latest_maneuver_plan_[i], current_downtrack);
            }
            planned_time += getManeuverDuration(latest_maneuver_plan_[i], epsilon_).seconds();
 
            new_maneuvers.push_back(latest_maneuver_plan_[i]);
            ++i;
        }
 
        if (new_maneuvers.empty())
        {
            RCLCPP_WARN_STREAM(get_logger(),"Cannot plan maneuver because no route is found");
            return;
        }
 
        //Update time progress for maneuvers
        if (!req->prior_plan.maneuvers.empty())
        {
            updateTimeProgress(new_maneuvers, GET_MANEUVER_PROPERTY(req->prior_plan.maneuvers.back(), end_time));
           RCLCPP_DEBUG_STREAM(get_logger(),"Detected a prior plan! Using back maneuver's end time:"<< std::to_string(rclcpp::Time(GET_MANEUVER_PROPERTY(req->prior_plan.maneuvers.back(), end_time)).seconds()));    
           RCLCPP_DEBUG_STREAM(get_logger(),"Where plan_completion_time was:"<< std::to_string(rclcpp::Time(req->prior_plan.planning_completion_time).seconds()));            
        }
        else
        {
            updateTimeProgress(new_maneuvers, rclcpp::Time(req->header.stamp));
           RCLCPP_DEBUG_STREAM(get_logger(),"Detected NO prior plan! Using this->now():"<< std::to_string(this->now().seconds()));   
        }
 
        //update starting speed of first maneuver
        if (!req->prior_plan.maneuvers.empty())
        {
            double start_speed;
            switch (req->prior_plan.maneuvers.back().type)
            {
                case carma_planning_msgs::msg::Maneuver::LANE_FOLLOWING:
                    start_speed = req->prior_plan.maneuvers.back().lane_following_maneuver.end_speed;
                    break;
                case carma_planning_msgs::msg::Maneuver::LANE_CHANGE:
                    start_speed = req->prior_plan.maneuvers.back().lane_change_maneuver.end_speed;
                    break;
                case carma_planning_msgs::msg::Maneuver::INTERSECTION_TRANSIT_STRAIGHT:
                    start_speed = req->prior_plan.maneuvers.back().intersection_transit_straight_maneuver.end_speed;
                    break;
                case carma_planning_msgs::msg::Maneuver::INTERSECTION_TRANSIT_LEFT_TURN:
                    start_speed = req->prior_plan.maneuvers.back().intersection_transit_left_turn_maneuver.end_speed;
                    break;
                case carma_planning_msgs::msg::Maneuver::INTERSECTION_TRANSIT_RIGHT_TURN:
                    start_speed = req->prior_plan.maneuvers.back().intersection_transit_right_turn_maneuver.end_speed;
                    break;
                default:
                    throw std::invalid_argument("Invalid maneuver type, cannot update starting speed for maneuver");
            }
            
            updateStartingSpeed(new_maneuvers.front(), start_speed);
            RCLCPP_DEBUG_STREAM(get_logger(),"Detected a prior plan! Using back maneuver's end speed:"<< start_speed);    
        }
        else 
        {
            updateStartingSpeed(new_maneuvers.front(), req->veh_logitudinal_velocity);
            RCLCPP_DEBUG_STREAM(get_logger(),"Detected NO prior plan! Using req->veh_logitudinal_velocity:"<< req->veh_logitudinal_velocity);    
        }
        //update plan
        resp->new_plan = req->prior_plan;
        RCLCPP_DEBUG_STREAM(get_logger(),"Updating maneuvers before returning... Prior plan size:" << req->prior_plan.maneuvers.size());
        for (const auto& mvr : new_maneuvers)
        {
            resp->new_plan.maneuvers.push_back(mvr);
        }
 
        RCLCPP_DEBUG_STREAM(get_logger(),"Returning total of maneuver size: " << resp->new_plan.maneuvers.size());
        resp->new_plan.planning_completion_time = this->now();
 
        return;
    }

References config_, epsilon_, GET_MANEUVER_PROPERTY, getManeuverDuration(), process_bag::i, latest_maneuver_plan_, route_following_plugin::Config::min_plan_duration_, setManeuverStartDist(), carma_cooperative_perception::to_string(), updateStartingSpeed(), and updateTimeProgress().

Here is the call graph for this function:

◆ returnToShortestPath()

void route_following_plugin::RouteFollowingPlugin::returnToShortestPath ( const lanelet::ConstLanelet & current_lanelet )

private

This method re-routes the vehicle back to the shortest path, if the vehicle has left the shortest path, but is still on the route Re-routing is performed by generating a new shortest path via the closest lanelet on the original shortest path.

Parameters

current_lanelet curretn lanelet where the vehicle is at

Definition at line 845 of file route_following_plugin.cpp.

    {
        auto original_shortestpath = wm_->getRoute()->shortestPath();
        RCLCPP_DEBUG_STREAM(get_logger(),"The vehicle has left the shortest path");
        auto routing_graph = wm_->getMapRoutingGraph();
        
        // Obtain the lanelet following the current lanelet in this same lane, this lanelet must exist since the new shortest path cannot begin with a lane change
        auto current_lane_following_lanelets = routing_graph->following(current_lanelet);
        lanelet::ConstLanelet current_lane_following_lanelet;
        if(!current_lane_following_lanelets.empty()){
            current_lane_following_lanelet = current_lane_following_lanelets[0];
        }
        else{
            throw std::invalid_argument("The current lanelet does not have a following lanelet. Vehicle cannot return to shortest path!");
        }
 
        // In order to return to the shortest path, the closest future lanelet on the shortest path needs to be found.
        // That is the adjacent lanelet of the following lanelet of the current lanelet.
        auto adjacent_lanelets = routing_graph->besides(current_lane_following_lanelet);
        if (!adjacent_lanelets.empty())
        {
            for (const auto& adjacent:adjacent_lanelets)
            {
                if (shortest_path_set_.find(adjacent.id())!=shortest_path_set_.end())
                {
                    auto following_lanelets = routing_graph->following(adjacent);
                    const auto& target_following_lanelet = following_lanelets[0];
                    RCLCPP_DEBUG_STREAM(get_logger(),"The target_following_lanelet id is: " << target_following_lanelet.id());
                    lanelet::ConstLanelets interm;
                    interm.push_back(static_cast<lanelet::ConstLanelet>(current_lane_following_lanelet));
                    interm.push_back(static_cast<lanelet::ConstLanelet>(target_following_lanelet));
                    // a new shortest path, via the the lanelets in 'interm' is calculated and used an alternative shortest path
                    auto new_shortestpath = routing_graph->shortestPathVia(current_lanelet, interm, original_shortestpath.back());
                    RCLCPP_DEBUG_STREAM(get_logger(),"a new shortestpath is generated to return to original shortestpath");
                    // routeCb is called to update latest_maneuver_plan_
                    if (new_shortestpath) this->latest_maneuver_plan_ = routeCb(new_shortestpath.get());
                    break;
                }
                else
                {
                    // a new shortest path, via the current_lanelet is calculated and used an alternative shortest path
                    lanelet::ConstLanelets new_interm;
                    new_interm.push_back(static_cast<lanelet::ConstLanelet>(current_lane_following_lanelet));
                    auto new_shortestpath = routing_graph->shortestPathVia(current_lanelet, new_interm, original_shortestpath.back());
                    RCLCPP_DEBUG_STREAM(get_logger(),"Cannot return to the original shortestpath from adjacent lanes, so a new shortestpath is generated");
                    // routeCb is called to update latest_maneuver_plan_
                    if (new_shortestpath) this->latest_maneuver_plan_ = routeCb(new_shortestpath.get());
                }
            }
            
        }
        else
        {
           RCLCPP_WARN_STREAM(get_logger(),"Alternative shortest path cannot be generated");
        }
 
    }

References latest_maneuver_plan_, routeCb(), shortest_path_set_, and wm_.

Referenced by bumper_pose_cb().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ routeCb()

std::vector< carma_planning_msgs::msg::Maneuver > route_following_plugin::RouteFollowingPlugin::routeCb ( const lanelet::routing::LaneletPath & route_shortest_path )

private

Calculate maneuver plan for remaining route. This callback is triggered when a new route has been received and processed by the world model.

Parameters

route_shortest_path A list of lanelets along the shortest path of the route using which the maneuver plan is calculated.

Definition at line 169 of file route_following_plugin.cpp.

    {
        
        
        for (const auto& ll:route_shortest_path)
        {
            shortest_path_set_.insert(ll.id());
        }
        std::vector<carma_planning_msgs::msg::Maneuver> maneuvers;
        //This function calculates the maneuver plan every time the route is set
        RCLCPP_DEBUG_STREAM(get_logger(),"New route created");
 
        //Go through entire route - identify lane changes and fill in the spaces with lane following
        auto nearest_lanelets = lanelet::geometry::findNearest(wm_->getMap()->laneletLayer, current_loc_, 10); //Return 10 nearest lanelets
        if (nearest_lanelets.empty())
        {
            RCLCPP_WARN_STREAM(get_logger(),"Cannot find any lanelet in map!");
 
            return maneuvers;
        }
 
        maneuvers.reserve(route_shortest_path.size());
 
        double route_length = wm_->getRouteEndTrackPos().downtrack;
        double start_dist = 0.0;
        double end_dist = 0.0;
        double start_speed = 0.0;
 
        size_t shortest_path_index;
        //Find lane changes in path - up to the second to last lanelet in path (till lane change is possible)
        for (shortest_path_index = 0; shortest_path_index < route_shortest_path.size() - 1; ++shortest_path_index)
        {
            RCLCPP_DEBUG_STREAM(get_logger(),"current shortest_path_index:" << shortest_path_index);
 
            auto following_lanelets = wm_->getRoute()->followingRelations(route_shortest_path[shortest_path_index]);
            RCLCPP_DEBUG_STREAM(get_logger(),"following_lanelets.size():" << following_lanelets.size());
 
            double target_speed_in_lanelet = findSpeedLimit(route_shortest_path[shortest_path_index]);
 
            //update start distance and start speed from previous maneuver if it exists
            start_dist = (maneuvers.empty()) ? wm_->routeTrackPos(route_shortest_path[shortest_path_index].centerline2d().front()).downtrack : GET_MANEUVER_PROPERTY(maneuvers.back(), end_dist); // TODO_REFAC if there is no initial maneuver start distance and start speed should be derived from current state. Current state ought to be provided in planning request
            start_speed = (maneuvers.empty()) ? 0.0 : getManeuverEndSpeed(maneuvers.back());
            RCLCPP_DEBUG_STREAM(get_logger(),"start_dist:" << start_dist << ", start_speed:" << start_speed);
 
            end_dist = wm_->routeTrackPos(route_shortest_path[shortest_path_index].centerline2d().back()).downtrack;
            RCLCPP_DEBUG_STREAM(get_logger(),"end_dist:" << end_dist);
            end_dist = std::min(end_dist, route_length);
            RCLCPP_DEBUG_STREAM(get_logger(),"min end_dist:" << end_dist);
 
            if (std::fabs(start_dist - end_dist) < 0.1) //TODO: edge case that was not recreatable. Sometimes start and end dist was same which crashes inlanecruising
            {
                RCLCPP_WARN_STREAM(get_logger(),"start and end dist are equal! shortest path id" << shortest_path_index << ", lanelet id:" << route_shortest_path[shortest_path_index].id() <<
                    ", start and end dist:" << start_dist);
                continue;
            }
 
 
 
            if (isLaneChangeNeeded(following_lanelets, route_shortest_path[shortest_path_index + 1].id()))
            {
                RCLCPP_DEBUG_STREAM(get_logger(),"LaneChangeNeeded");
    
                // Determine the Lane Change Status
                RCLCPP_DEBUG_STREAM(get_logger(),"Recording lanechange start_dist <<" << start_dist  << ", from llt id:" << route_shortest_path[shortest_path_index].id() << " to llt id: " << 
                    route_shortest_path[shortest_path_index+ 1].id());
 
                maneuvers.push_back(composeLaneChangeManeuverMessage(start_dist, end_dist, start_speed, target_speed_in_lanelet, route_shortest_path[shortest_path_index].id(), route_shortest_path[shortest_path_index + 1].id()));
                ++shortest_path_index; //Since lane change covers 2 lanelets - skip planning for the next lanelet
 
            }
            else
            {
                RCLCPP_DEBUG_STREAM(get_logger(),"Lanechange NOT Needed ");
                maneuvers.push_back(composeLaneFollowingManeuverMessage(start_dist, end_dist, start_speed, target_speed_in_lanelet, { route_shortest_path[shortest_path_index].id() } ));
            }
        }
 
        // Add stop and wait maneuver as last maneuver if there is a lanelet unplanned for in path
        if (shortest_path_index < route_shortest_path.size())
        {
 
            // Compute target deceleration for stopping
            double stopping_accel_limit = config_.accel_limit_ * config_.stopping_accel_limit_multiplier_;
 
            // Estimate the entry speed for the stopping maneuver
            double stopping_entry_speed = maneuvers.empty() ? current_speed_ : getManeuverEndSpeed(maneuvers.back());
 
            // Add stop and wait maneuver based on deceleration target and entry speed
            maneuvers = addStopAndWaitAtRouteEnd( maneuvers, route_length, stopping_entry_speed, stopping_accel_limit, config_.lateral_accel_limit_, config_.min_maneuver_length_ );
        
        }
        RCLCPP_DEBUG_STREAM(get_logger(),"Maneuver plan along route successfully generated");
        return maneuvers;
    }

References route_following_plugin::Config::accel_limit_, addStopAndWaitAtRouteEnd(), composeLaneChangeManeuverMessage(), composeLaneFollowingManeuverMessage(), config_, current_loc_, current_speed_, findSpeedLimit(), GET_MANEUVER_PROPERTY, route_following_plugin::anonymous_namespace{route_following_plugin.cpp}::getManeuverEndSpeed(), isLaneChangeNeeded(), route_following_plugin::Config::lateral_accel_limit_, route_following_plugin::Config::min_maneuver_length_, shortest_path_set_, route_following_plugin::Config::stopping_accel_limit_multiplier_, and wm_.

Referenced by on_configure_plugin(), and returnToShortestPath().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ setManeuverStartDist()

void route_following_plugin::RouteFollowingPlugin::setManeuverStartDist	(	carma_planning_msgs::msg::Maneuver &	maneuver,
		double	start_dist
	)		const

private

Set the start distance of a maneuver based on the progress along the route.

Parameters

maneuver	A maneuver (non-specific to type) to be performed
start_dist	the starting distance that the maneuver need to be updated to

Definition at line 697 of file route_following_plugin.cpp.

    {
        switch (maneuver.type)
        {
        case carma_planning_msgs::msg::Maneuver::LANE_FOLLOWING:
            maneuver.lane_following_maneuver.start_dist = start_dist;
            break;
        case carma_planning_msgs::msg::Maneuver::LANE_CHANGE:
            maneuver.lane_change_maneuver.start_dist = start_dist;
            break;
        case carma_planning_msgs::msg::Maneuver::INTERSECTION_TRANSIT_STRAIGHT:
            maneuver.intersection_transit_straight_maneuver.start_dist = start_dist;
            break;
        case carma_planning_msgs::msg::Maneuver::INTERSECTION_TRANSIT_LEFT_TURN:
            maneuver.intersection_transit_left_turn_maneuver.start_dist = start_dist;
            break;
        case carma_planning_msgs::msg::Maneuver::INTERSECTION_TRANSIT_RIGHT_TURN:
            maneuver.intersection_transit_right_turn_maneuver.start_dist = start_dist;
            break;
        case carma_planning_msgs::msg::Maneuver::STOP_AND_WAIT:
            maneuver.stop_and_wait_maneuver.start_dist = start_dist;
            break;
        default:
            throw std::invalid_argument("Invalid maneuver type");
        }
    }

Referenced by plan_maneuvers_callback().

Here is the caller graph for this function:

◆ twist_cb()

void route_following_plugin::RouteFollowingPlugin::twist_cb ( 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 160 of file route_following_plugin.cpp.

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

References current_speed_.

Referenced by on_configure_plugin().

Here is the caller graph for this function:

◆ updateStartingSpeed()

void route_following_plugin::RouteFollowingPlugin::updateStartingSpeed	(	carma_planning_msgs::msg::Maneuver &	maneuver,
		double	start_speed
	)		const

private

Given an maneuver update the starting speed.

Parameters

maneuver	maneuver to update the starting speed for
start_time	The starting speed for the maneuver passed as argument

Definition at line 670 of file route_following_plugin.cpp.

    {
        switch (maneuver.type)
        {
        case carma_planning_msgs::msg::Maneuver::LANE_FOLLOWING:
            maneuver.lane_following_maneuver.start_speed = start_speed;
            break;
        case carma_planning_msgs::msg::Maneuver::LANE_CHANGE:
            maneuver.lane_change_maneuver.start_speed = start_speed;
            break;
        case carma_planning_msgs::msg::Maneuver::INTERSECTION_TRANSIT_STRAIGHT:
            maneuver.intersection_transit_straight_maneuver.start_speed = start_speed;
            break;
        case carma_planning_msgs::msg::Maneuver::INTERSECTION_TRANSIT_LEFT_TURN:
            maneuver.intersection_transit_left_turn_maneuver.start_speed = start_speed;
            break;
        case carma_planning_msgs::msg::Maneuver::INTERSECTION_TRANSIT_RIGHT_TURN:
            maneuver.intersection_transit_right_turn_maneuver.start_speed = start_speed;
            break;
        case carma_planning_msgs::msg::Maneuver::STOP_AND_WAIT:
            maneuver.stop_and_wait_maneuver.start_speed = start_speed;
            break;
        default:
            throw std::invalid_argument("Invalid maneuver type, cannot update starting speed for maneuver");
        }
    }

Referenced by plan_maneuvers_callback().

Here is the caller graph for this function:

◆ updateTimeProgress()

void route_following_plugin::RouteFollowingPlugin::updateTimeProgress	(	std::vector< carma_planning_msgs::msg::Maneuver > &	maneuvers,
		rclcpp::Time	start_time
	)		const

private

Given an array of maneuvers update the starting time for each.

Parameters

maneuvers	An array of maneuvers (non-specific to type)
start_time	The starting time for the first maneuver in the sequence, each consequent maneuver is pushed ahead by same amount

Definition at line 628 of file route_following_plugin.cpp.

    {
        rclcpp::Time time_progress = start_time;
        rclcpp::Time prev_time = time_progress;
 
        for (auto &maneuver : maneuvers)
        {
            time_progress += getManeuverDuration(maneuver, epsilon_);
            switch (maneuver.type)
            {
            case carma_planning_msgs::msg::Maneuver::LANE_FOLLOWING:
                maneuver.lane_following_maneuver.start_time = prev_time;
                maneuver.lane_following_maneuver.end_time = time_progress;
                break;
            case carma_planning_msgs::msg::Maneuver::LANE_CHANGE:
                maneuver.lane_change_maneuver.start_time = prev_time;
                maneuver.lane_change_maneuver.end_time = time_progress;
                break;
            case carma_planning_msgs::msg::Maneuver::INTERSECTION_TRANSIT_STRAIGHT:
                maneuver.intersection_transit_straight_maneuver.start_time = prev_time;
                maneuver.intersection_transit_straight_maneuver.end_time = time_progress;
                break;
            case carma_planning_msgs::msg::Maneuver::INTERSECTION_TRANSIT_LEFT_TURN:
                maneuver.intersection_transit_left_turn_maneuver.start_time = prev_time;
                maneuver.intersection_transit_left_turn_maneuver.end_time = time_progress;
                break;
            case carma_planning_msgs::msg::Maneuver::INTERSECTION_TRANSIT_RIGHT_TURN:
                maneuver.intersection_transit_right_turn_maneuver.start_time = prev_time;
                maneuver.intersection_transit_right_turn_maneuver.end_time = time_progress;
                break;
            case carma_planning_msgs::msg::Maneuver::STOP_AND_WAIT:
                maneuver.stop_and_wait_maneuver.start_time = prev_time;
                maneuver.stop_and_wait_maneuver.start_time = time_progress;
                maneuver.stop_and_wait_maneuver.end_time = start_time + rclcpp::Duration(86400, 0); // Set maneuver time period as 24hrs since this is the end of the route
                break;
            default:
                throw std::invalid_argument("Invalid maneuver type, cannot update time progress for maneuver");
            }
            prev_time = time_progress;
        }
    }

References epsilon_, and getManeuverDuration().

Referenced by plan_maneuvers_callback().

Here is the call graph for this function:

Here is the caller graph for this function:

Member Data Documentation

◆ bumper_pose_timer_

rclcpp::TimerBase::SharedPtr route_following_plugin::RouteFollowingPlugin::bumper_pose_timer_

private

Definition at line 264 of file route_following_plugin.hpp.

Referenced by on_activate_plugin().

◆ config_

Config route_following_plugin::RouteFollowingPlugin::config_

private

Definition at line 238 of file route_following_plugin.hpp.

Referenced by RouteFollowingPlugin(), addStopAndWaitAtRouteEnd(), composeStopAndWaitManeuverMessage(), on_configure_plugin(), plan_maneuvers_callback(), and routeCb().

◆ current_loc_

lanelet::BasicPoint2d route_following_plugin::RouteFollowingPlugin::current_loc_

private

Definition at line 248 of file route_following_plugin.hpp.

Referenced by bumper_pose_cb(), and routeCb().

◆ current_maneuver_plan_

carma_planning_msgs::msg::ManeuverPlan::UniquePtr route_following_plugin::RouteFollowingPlugin::current_maneuver_plan_

private

Definition at line 251 of file route_following_plugin.hpp.

Referenced by bumper_pose_cb(), and current_maneuver_plan_cb().

◆ current_maneuver_plan_sub_

carma_ros2_utils::SubPtr<carma_planning_msgs::msg::ManeuverPlan> route_following_plugin::RouteFollowingPlugin::current_maneuver_plan_sub_

private

Definition at line 230 of file route_following_plugin.hpp.

Referenced by on_configure_plugin().

◆ current_speed_

double route_following_plugin::RouteFollowingPlugin::current_speed_ = 0.0

private

Definition at line 241 of file route_following_plugin.hpp.

Referenced by routeCb(), and twist_cb().

◆ epsilon_

double route_following_plugin::RouteFollowingPlugin::epsilon_ = 0.0001

private

Definition at line 244 of file route_following_plugin.hpp.

Referenced by plan_maneuvers_callback(), and updateTimeProgress().

◆ frontbumper_transform_

tf2::Stamped<tf2::Transform> route_following_plugin::RouteFollowingPlugin::frontbumper_transform_

private

Definition at line 301 of file route_following_plugin.hpp.

Referenced by bumper_pose_cb().

◆ lane_change_plugin_

std::string route_following_plugin::RouteFollowingPlugin::lane_change_plugin_ = "cooperative_lanechange"

private

Definition at line 257 of file route_following_plugin.hpp.

Referenced by composeLaneChangeManeuverMessage().

◆ lanefollow_planning_tactical_plugin_

std::string route_following_plugin::RouteFollowingPlugin::lanefollow_planning_tactical_plugin_ = "inlanecruising_plugin"

private

Definition at line 261 of file route_following_plugin.hpp.

Referenced by composeLaneFollowingManeuverMessage().

◆ latest_maneuver_plan_

std::vector<carma_planning_msgs::msg::Maneuver> route_following_plugin::RouteFollowingPlugin::latest_maneuver_plan_

private

Definition at line 254 of file route_following_plugin.hpp.

Referenced by bumper_pose_cb(), on_configure_plugin(), plan_maneuvers_callback(), and returnToShortestPath().

◆ MAX_LANE_WIDTH

constexpr double route_following_plugin::RouteFollowingPlugin::MAX_LANE_WIDTH = 3.70

staticconstexprprivate

Definition at line 235 of file route_following_plugin.hpp.

Referenced by maneuverWithBufferStartsAfterDowntrack().

◆ planning_strategic_plugin_

std::string route_following_plugin::RouteFollowingPlugin::planning_strategic_plugin_ = "route_following_plugin"

private

Definition at line 260 of file route_following_plugin.hpp.

Referenced by bumper_pose_cb(), composeLaneChangeManeuverMessage(), composeLaneFollowingManeuverMessage(), and composeStopAndWaitManeuverMessage().

◆ pose_msg_

geometry_msgs::msg::PoseStamped route_following_plugin::RouteFollowingPlugin::pose_msg_

private

Definition at line 247 of file route_following_plugin.hpp.

◆ shortest_path_set_

std::unordered_set<lanelet::Id> route_following_plugin::RouteFollowingPlugin::shortest_path_set_

private

Definition at line 233 of file route_following_plugin.hpp.

Referenced by bumper_pose_cb(), returnToShortestPath(), and routeCb().

◆ stop_and_wait_plugin_

std::string route_following_plugin::RouteFollowingPlugin::stop_and_wait_plugin_ = "stop_and_wait_plugin"

private

Definition at line 258 of file route_following_plugin.hpp.

Referenced by composeStopAndWaitManeuverMessage().

◆ tf2_buffer_

tf2_ros::Buffer route_following_plugin::RouteFollowingPlugin::tf2_buffer_

private

Definition at line 304 of file route_following_plugin.hpp.

Referenced by bumper_pose_cb(), and initializeBumperTransformLookup().

◆ tf2_listener_

std::unique_ptr<tf2_ros::TransformListener> route_following_plugin::RouteFollowingPlugin::tf2_listener_

private

Definition at line 305 of file route_following_plugin.hpp.

Referenced by initializeBumperTransformLookup().

◆ tf_

geometry_msgs::msg::TransformStamped route_following_plugin::RouteFollowingPlugin::tf_

private

Definition at line 298 of file route_following_plugin.hpp.

Referenced by bumper_pose_cb().

◆ twist_sub_

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

private

Definition at line 229 of file route_following_plugin.hpp.

Referenced by on_configure_plugin().

◆ wm_

carma_wm::WorldModelConstPtr route_following_plugin::RouteFollowingPlugin::wm_

Definition at line 77 of file route_following_plugin.hpp.

Referenced by addStopAndWaitAtRouteEnd(), bumper_pose_cb(), findSpeedLimit(), on_configure_plugin(), returnToShortestPath(), and routeCb().

◆ wml_

std::shared_ptr<carma_wm::WMListener> route_following_plugin::RouteFollowingPlugin::wml_

Definition at line 76 of file route_following_plugin.hpp.

Referenced by on_configure_plugin().

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

route_following_plugin/include/route_following_plugin.hpp
route_following_plugin/src/route_following_plugin.cpp

Public Member Functions

Public Attributes

Private Member Functions

Private Attributes

Static Private Attributes

Detailed Description

Constructor & Destructor Documentation

◆ RouteFollowingPlugin()

Member Function Documentation

◆ addStopAndWaitAtRouteEnd()

◆ bumper_pose_cb()

◆ composeLaneChangeManeuverMessage()

◆ composeLaneFollowingManeuverMessage()

◆ composeStopAndWaitManeuverMessage()

◆ current_maneuver_plan_cb()

◆ findSpeedLimit()

◆ FRIEND_TEST() [1/16]

◆ FRIEND_TEST() [2/16]

◆ FRIEND_TEST() [3/16]

◆ FRIEND_TEST() [4/16]

◆ FRIEND_TEST() [5/16]

◆ FRIEND_TEST() [6/16]

◆ FRIEND_TEST() [7/16]

◆ FRIEND_TEST() [8/16]

◆ FRIEND_TEST() [9/16]

◆ FRIEND_TEST() [10/16]

◆ FRIEND_TEST() [11/16]

◆ FRIEND_TEST() [12/16]

◆ FRIEND_TEST() [13/16]

◆ FRIEND_TEST() [14/16]

◆ FRIEND_TEST() [15/16]

◆ FRIEND_TEST() [16/16]

◆ get_availability()

◆ get_version_id()

◆ getManeuverDuration()

◆ getNewManeuverId()

◆ initializeBumperTransformLookup()

◆ isLaneChangeNeeded()

◆ maneuverWithBufferStartsAfterDowntrack()

◆ on_activate_plugin()

◆ on_configure_plugin()

◆ plan_maneuvers_callback()

◆ returnToShortestPath()

◆ routeCb()

◆ setManeuverStartDist()

◆ twist_cb()

◆ updateStartingSpeed()

◆ updateTimeProgress()

Member Data Documentation

◆ bumper_pose_timer_

◆ config_

◆ current_loc_

◆ current_maneuver_plan_

◆ current_maneuver_plan_sub_

◆ current_speed_

◆ epsilon_

◆ frontbumper_transform_

◆ lane_change_plugin_

◆ lanefollow_planning_tactical_plugin_

◆ latest_maneuver_plan_

◆ MAX_LANE_WIDTH

◆ planning_strategic_plugin_

◆ pose_msg_

◆ shortest_path_set_

◆ stop_and_wait_plugin_

◆ tf2_buffer_

◆ tf2_listener_

◆ tf_

◆ twist_sub_

◆ wm_

◆ wml_