Class containing the logic for platoon manager. It is responsible for keeping track of the platoon members and role of the host vehicle in the platoon. More...

#include <platoon_manager_ihp.h>

Collaboration diagram for platoon_strategic_ihp::PlatoonManager:

Public Member Functions
	PlatoonManager (std::shared_ptr< carma_ros2_utils::timers::TimerFactory > timer_factory)
	Constructor. More...

void	updateHostPose (const double downtrack, const double crosstrack)
	Stores the latest info on location of the host vehicle. More...

void	updateHostSpeeds (const double cmdSpeed, const double actualSpeed)
	Stores the latest info on host vehicle's command & actual speeds. More...

void	hostMemberUpdates (const std::string &senderId, const std::string &platoonId, const std::string &params, const double &DtD, const double &CtD)
	Update information for members of the host's platoon based on a mobility operation STATUS message. More...

void	neighborMemberUpdates (const std::string &senderId, const std::string &platoonId, const std::string &params, const double &DtD, const double &CtD)
	Update information for members of a neighboring platoon based on a mobility operation STATUS message. More...

void	updatesOrAddMemberInfo (std::vector< PlatoonMember > &platoon, std::string senderId, double cmdSpeed, double dtDistance, double ctDistance, double curSpeed)
	Updates the list of vehicles in the specified platoon, based on info available from a mobility operation STATUS message from one of that platoon's vehicles. It ensures the list of vehicles is properly sorted in order of appearance from front to rear in the platoon. If the host is in the platoon, it will update host info as well. More...

int	getHostPlatoonSize ()
	Returns total size of the platoon , in number of vehicles. More...

void	clearActionPlan ()
	Resets necessary variables to indicate that the current ActionPlan is dead. More...

void	resetHostPlatoon ()
	Resets all variables that might indicate other members of the host's platoon; sets the host back to solo vehicle. More...

void	resetNeighborPlatoon ()
	Resets all variables that describe a neighbor platoon, so that no neighbor is known. More...

bool	removeMember (const size_t mem)
	Removes a single member from the internal record of platoon members. More...

bool	removeMemberById (const std::string id)
	Removes a single member from the internal record of platoon members. More...

PlatoonMember	getDynamicLeader ()
	Returns dynamic leader of the host vehicle. More...

int	allPredecessorFollowing ()
	This is the implementation of all predecessor following (APF) algorithm for leader selection in a platoon. This function will recognize who is acting as the current leader of the subject vehicle. The current leader of the subject vehicle will be any ONE of the vehicles in front of it. Having a vehicle further downstream function as the leader is more efficient and more stable; however, having a vehicle closer to the subject vehicle function as the leader is safer. For this reason, the subject vehicle will monitor all time headways between every single set of consecutive vehicles starting from itself to the leader. If the time headways are within some safe thresholds then vehicles further downstream may function as the leader. Otherwise, for the sake of safety, vehicles closer to the subject vehicle, potentially even the predecessor, will function as the leader. More...

void	changeFromFollowerToLeader ()
	Update status when state change from Follower to Leader. More...

void	changeFromLeaderToFollower (std::string newPlatoonId, std::string newLeaderId)
	Update status when state change from Leader to Follower. More...

int	getNumberOfVehicleInFront ()
	Get number of vehicles in front of host vehicle inside platoon. More...

double	getCurrentPlatoonLength ()
	Returns overall length of the platoon. in m. More...

double	getPlatoonRearDowntrackDistance ()
	Returns downtrack distance of the rear vehicle in platoon, in m. More...

double	getDistanceToPredVehicle ()
	Returns distance to the predessecor vehicle, in m. More...

double	getCurrentSpeed () const
	Returns current speed, in m/s. More...

double	getCommandSpeed () const
	Returns command speed. in m/s. More...

double	getCurrentDowntrackDistance () const
	Returns current downtrack distance, in m. More...

double	getCurrentCrosstrackDistance () const
	Returns current crosstrack distance, in m. More...

std::string	getHostStaticID () const
	Returns current host static ID as a string. More...

double	getPlatoonFrontDowntrackDistance ()
	UCLA: Return the platoon leader downtrack distance, in m. More...

double	getPredecessorTimeHeadwaySum ()
	UCLA: Return the time headway summation of all predecessors, in s. More...

double	getPredecessorSpeed ()
	UCLA: Return the speed of the preceding vehicle, in m/s. More...

double	getPredecessorPosition ()
	UCLA: Return the position of the preceding vehicle, in m. More...

double	getIHPDesPosFollower (double dt)
	UCLA: Return follower's desired position (i.e., downtrack, in m) that maintains the desired intra-platoon time gap, based on IHP platoon trajectory regulation algorithm. More...

int	getClosestIndex (double joinerDtD)
	UCLA: Return joiner's desired position in terms of target platoon index to cut into the platoon. CAUTION: ASSUMES that the neighbor platoon info is fully populated! More...

double	getCutInGap (const int gap_leading_index, const double joinerDtD)
	UCLA: Return the current actual gap size in the target platoon for cut-in join, in m. Note: The origin of the vehicle (for downtrack distance calculation) is located at the rear axle. CAUTION: ASSUMES that the neighbor platoon info is fully populated! More...

Public Attributes
const std::string	dummyID = "00000000-0000-0000-0000-000000000000"

std::vector< PlatoonMember >	host_platoon_

std::string	currentPlatoonID = dummyID

std::string	platoonLeaderID = dummyID

PlatoonState	current_platoon_state = PlatoonState::STANDBY

size_t	hostPosInPlatoon_ = 0

ActionPlan	current_plan = ActionPlan()

bool	isFollower = false

std::vector< PlatoonMember >	neighbor_platoon_

size_t	neighbor_platoon_info_size_ = 0

std::string	targetPlatoonID = dummyID

std::string	neighborPlatoonID = dummyID

std::string	neighbor_platoon_leader_id_ = dummyID

bool	is_neighbor_record_complete_ = false

geometry_msgs::msg::PoseStamped	pose_msg_

std::string	HostMobilityId = "default_host_id"

bool	isCreateGap = false

size_t	dynamic_leader_index_ = 0

std::shared_ptr< carma_ros2_utils::timers::TimerFactory >	timer_factory_

Private Member Functions
bool	insufficientGapWithPredecessor (double distanceToPredVehicle)
	Check the gap with the predecessor vehicle. More...

std::vector< double >	calculateTimeHeadway (std::vector< double > downtrackDistance, std::vector< double > speed) const
	Calculate the time headaway of each platoon member and save as a vector. More...

int	determineDynamicLeaderBasedOnViolation (std::vector< double > timeHeadways)
	Determine the proper vehicle to follow based the time headway of each member. Note that the host will always choose the closest violator (i.e. time headaway too small or too large) to follow. More...

int	findLowerBoundaryViolationClosestToTheHostVehicle (std::vector< double > timeHeadways) const
	Find the closest vehicle to the host vehicle that violates the (time headaway) lower boundary condition. More...

int	findMaximumSpacingViolationClosestToTheHostVehicle (std::vector< double > timeHeadways) const
	Find the closest vehicle to the host vehicle that violates the (time headaway) maximum spacing condition. More...

std::vector< double >	getTimeHeadwayFromIndex (std::vector< double > timeHeadways, int start) const
	Return a sub-vector of the platoon-wise time headaways vector that start with a given index. More...

Private Attributes
PlatoonPluginConfig	config_

std::string	OPERATION_INFO_TYPE = "INFO"

std::string	OPERATION_STATUS_TYPE = "STATUS"

std::string	JOIN_AT_REAR_PARAMS = "SIZE:%1%,SPEED:%2%,DTD:%3%"

std::string	JOIN_FROM_FRONT_PARAMS = "SIZE:%1%,SPEED:%2%,DTD:%3%"

std::string	MOBILITY_STRATEGY = "Carma/Platooning"

double	minCutinGap_ = 22.0

double	maxCutinGap_ = 32.0

std::string	previousFunctionalDynamicLeaderID_ = ""

int	previousFunctionalDynamicLeaderIndex_ = -1

double	vehicleLength_ = 5.0

double	gapWithPred_ = 0.0

double	downtrack_progress_ = 0

std::string	algorithmType_ = "APF_ALGORITHM"

Detailed Description

Class containing the logic for platoon manager. It is responsible for keeping track of the platoon members and role of the host vehicle in the platoon.

Definition at line 120 of file platoon_manager_ihp.h.

Constructor & Destructor Documentation

◆ PlatoonManager()

platoon_strategic_ihp::PlatoonManager::PlatoonManager ( std::shared_ptr< carma_ros2_utils::timers::TimerFactory > timer_factory )

Constructor.

Parameters

timer_factory An interface which can be used to get access to the current time

Implementation notes:

platoon vector indexing: A vector of platoon members (vehicles), sorted by downtrack distance in descending order (i.e., [dtd_1 > dtd_2 > ... > dtd_n] ).
speed vector indexing: A vector that only contains speed (m/s) of each platoon member. Same order with platoon list (i.e., [platoon_front, follwer_1, ..., follower_n]).
downtrackDistance vector indexing: A vector that only contains downtrack distance (m) of each platoon member. Same order with platoon list (i.e., [platoon_front, follwer_1, ..., follower_n])
timeHeadway vector indexing A vector that only contains time headaway (s) behind each platoon member (i.e., the time gap between host vehicle and its following vehicle). Same order with platoon list. For APF, when gap too small, the dynamic leader will be the front vehicle of the small gap. If gap too large, the dynamic leader wil be the rear vehicle of the large gap.

Definition at line 51 of file platoon_manager_ihp.cpp.

                                                                                                  : timer_factory_(std::move(timer_factory))
    {
        RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "Top of PlatoonManager ctor.");
    }

Member Function Documentation

◆ allPredecessorFollowing()

int platoon_strategic_ihp::PlatoonManager::allPredecessorFollowing ( )

This is the implementation of all predecessor following (APF) algorithm for leader selection in a platoon. This function will recognize who is acting as the current leader of the subject vehicle. The current leader of the subject vehicle will be any ONE of the vehicles in front of it. Having a vehicle further downstream function as the leader is more efficient and more stable; however, having a vehicle closer to the subject vehicle function as the leader is safer. For this reason, the subject vehicle will monitor all time headways between every single set of consecutive vehicles starting from itself to the leader. If the time headways are within some safe thresholds then vehicles further downstream may function as the leader. Otherwise, for the sake of safety, vehicles closer to the subject vehicle, potentially even the predecessor, will function as the leader.

Returns: the index of the leader in the platoon list.

‍***** Case Zero *****‍///

‍***** Case One *****‍///

‍***** Case Two *****‍///

‍***** Case Three *****‍///

‍***** Case Four *****‍///

‍***** Case Five *****‍///

‍***** Case Six *****‍///

‍***** Case Seven *****‍///

‍***** Case Eight *****‍///

‍***** Case Nine *****‍///

Definition at line 428 of file platoon_manager_ihp.cpp.

                                               {
 
        // If the host vehicle is the second follower of a platoon, it will always follow the platoon leader in the front 
        if(getNumberOfVehicleInFront() == 1)
        {
            // If the host is the second vehicle, then follow the leader.
            RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "As the second vehicle in the platoon, it will always follow the leader. Case Zero");
            return 0;
        }
        // If there weren't a leader in the previous time step, follow the first vehicle (i.e., the platoon leader) as default.
        if(previousFunctionalDynamicLeaderID_ == "") {
            RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "APF algorithm did not found a dynamic leader in previous time step. Case one");
            return 0;
        }
 
        //***** Formulate speed and downtrack vector *****//
        // Update host vehicle info when update member info, so platoon list include host vehicle, direct use platoon size for downtrack/speed vector.
        // Record downtrack distance (m) of each member
        std::vector<double> downtrackDistance(hostPosInPlatoon_);
        for(size_t i = 0; i < hostPosInPlatoon_; i++) {
            downtrackDistance[i] = host_platoon_[i].vehiclePosition; // m
        }
        // Record speed (m/s) of each member
        std::vector<double> speed(host_platoon_.size());
        for(size_t i = 0; i < host_platoon_.size(); i++) {
            speed[i] = host_platoon_[i].vehicleSpeed; // m/s
        }
        
 
        // If the distance headway between the subject vehicle and its predecessor is an issue
        // according to the "min_gap" and "max_gap" thresholds, then it should follow its predecessor
        // The following line will not throw exception because the length of downtrack array is larger than two in this case
        double distHeadwayWithPredecessor = downtrackDistance[hostPosInPlatoon_ - 1] - downtrackDistance[hostPosInPlatoon_];
        gapWithPred_ = distHeadwayWithPredecessor;
        if(insufficientGapWithPredecessor(distHeadwayWithPredecessor)) {
            RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "APF algorithm decides there is an issue with the gap with preceding vehicle: " << distHeadwayWithPredecessor << " m. Case Two");
            return hostPosInPlatoon_ - 1;
        }
        else{
            // implementation of the main part of APF algorithm
            // calculate the time headway between every consecutive pair of vehicles
            std::vector<double> timeHeadways = calculateTimeHeadway(downtrackDistance, speed);
            RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "APF calculate time headways: " );
            for (const auto& value : timeHeadways)
            {
                RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "APF time headways: " << value);
            }
            RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "APF found the previous dynamic leader is " << previousFunctionalDynamicLeaderID_);
            // if the previous dynamic leader is the first vehicle in the platoon
            
            if(previousFunctionalDynamicLeaderIndex_ == 0) {
 
                // If there is a violation, the return value is the desired dynamic leader index
                RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "APF use violations on lower boundary or maximum spacing to choose dynamic leader. Case Three.");
                return determineDynamicLeaderBasedOnViolation(timeHeadways);
            }
            else{
                // if the previous dynamic leader is not the first vehicle
                // get the time headway between every consecutive pair of vehicles from index Of Previous dynamic Leader
                std::vector<double> partialTimeHeadways = getTimeHeadwayFromIndex(timeHeadways, previousFunctionalDynamicLeaderIndex_);
                for (const auto& value : partialTimeHeadways)
                {
                    RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "APF partial time headways: " << value);
                }
                int closestLowerBoundaryViolation = findLowerBoundaryViolationClosestToTheHostVehicle(partialTimeHeadways);
                int closestMaximumSpacingViolation = findMaximumSpacingViolationClosestToTheHostVehicle(partialTimeHeadways);
                // if there are no violations anywhere between the subject vehicle and the current dynamic leader,
                // then depending on the time headways of the ENTIRE platoon, the subject vehicle may switch
                // dynamic leader further downstream. This is because the subject vehicle has determined that there are
                // no time headways between itself and the current dynamic leader which would cause the platoon to be unsafe.
                // if there are violations somewhere betweent the subject vehicle and the current dynamic leader,
                // then rather than assigning dynamic leadership further DOWNSTREAM, we must go further UPSTREAM in the following lines
                if(closestLowerBoundaryViolation == -1 && closestMaximumSpacingViolation == -1) {
                    // In order for the subject vehicle to assign dynamic leadership further downstream,
                    // two criteria must be satisfied: first the leading vehicle and its immediate follower must
                    // have a time headway greater than "upper_boundary." The purpose of this criteria is to
                    // introduce a hysteresis in order to eliminate the possibility of a vehicle continually switching back 
                    // and forth between two dynamic leaders because one of the time headways is hovering right around
                    // the "lower_boundary" threshold; second the leading vehicle and its predecessor must have
                    // a time headway less than "min_spacing" second. Just as with "upper_boundary", "min_spacing" exists to
                    // introduce a hysteresis where dynamic leaders are continually being switched.
                    bool condition1 = timeHeadways[previousFunctionalDynamicLeaderIndex_] > config_.headawayStableLowerBond; 
                    bool condition2 = timeHeadways[previousFunctionalDynamicLeaderIndex_ - 1] < config_.headawayStableUpperBond; 
                    
                    //we may switch dynamic leader further downstream
                    if(condition1 && condition2) {
                        RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "APF found two conditions for assigning local dynamic leadership further downstream are satisfied. Case Four");
                        return determineDynamicLeaderBasedOnViolation(timeHeadways);
                    } else {
                        
                        // We may not switch dynamic leadership to another vehicle further downstream because some criteria are not satisfied
                        RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "APF found two conditions for assigning local dynamic leadership further downstream are not satisfied. Case Five.");
                        RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "condition1: " << condition1 << " & condition2: " << condition2);
                        return previousFunctionalDynamicLeaderIndex_;
                    }
                } else if(closestLowerBoundaryViolation != -1 && closestMaximumSpacingViolation == -1) {
                    // The rest four cases have roughly the same logic: locate the closest violation and assign dynamic leadership accordingly
                    
                    RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "APF found closestLowerBoundaryViolation on partial time headways. Case Six.");
                    return previousFunctionalDynamicLeaderIndex_ - 1 + closestLowerBoundaryViolation;
 
                } else if(closestLowerBoundaryViolation == -1 && closestMaximumSpacingViolation != -1) {
                    
                    RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "APF found closestMaximumSpacingViolation on partial time headways. Case Seven.");
                    return previousFunctionalDynamicLeaderIndex_ + closestMaximumSpacingViolation;
                } else{
                    RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "APF found closestMaximumSpacingViolation and closestLowerBoundaryViolation on partial time headways.");
                    if(closestLowerBoundaryViolation > closestMaximumSpacingViolation) {
                        
                        RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "closest LowerBoundaryViolation is higher than closestMaximumSpacingViolation on partial time headways. Case Eight.");
                        return previousFunctionalDynamicLeaderIndex_ - 1 + closestLowerBoundaryViolation;
                    } else if(closestLowerBoundaryViolation < closestMaximumSpacingViolation) {
                        
                        RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "closestMaximumSpacingViolation is higher than closestLowerBoundaryViolation on partial time headways. Case Nine.");
                        return previousFunctionalDynamicLeaderIndex_ + closestMaximumSpacingViolation;
                    } else {
                        RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "APF dynamic Leader selection parameter is wrong!");
                        return 0;
                    }
                }
            }
 
        }
    }

References calculateTimeHeadway(), config_, determineDynamicLeaderBasedOnViolation(), findLowerBoundaryViolationClosestToTheHostVehicle(), findMaximumSpacingViolationClosestToTheHostVehicle(), gapWithPred_, getNumberOfVehicleInFront(), getTimeHeadwayFromIndex(), platoon_strategic_ihp::PlatoonPluginConfig::headawayStableLowerBond, platoon_strategic_ihp::PlatoonPluginConfig::headawayStableUpperBond, host_platoon_, hostPosInPlatoon_, process_bag::i, insufficientGapWithPredecessor(), previousFunctionalDynamicLeaderID_, and previousFunctionalDynamicLeaderIndex_.

Referenced by getDynamicLeader().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ calculateTimeHeadway()

std::vector< double > platoon_strategic_ihp::PlatoonManager::calculateTimeHeadway	(	std::vector< double >	downtrackDistance,
		std::vector< double >	speed
	)		const

private

Calculate the time headaway of each platoon member and save as a vector.

Parameters

downtrackDistance	a vector containing the downtrack distances of all members.
speed	a vector containing the speeds of all members.

Returns: A vector containing the time headaway of all platoon members, each headway is in s.

Definition at line 585 of file platoon_manager_ihp.cpp.

                                                                                                                            {
        std::vector<double> timeHeadways(downtrackDistance.size() - 1);
        // Due to downtrack descending order, the platoon member with smaller index has larger downtrack, hence closer to the front of the platoon.
        for (size_t i = 0; i < timeHeadways.size(); i++){
            // Calculate time headaway between the vehicle behind. 
            if (speed[i+1] >= config_.ss_theta) // speed is in m/s
            {
                timeHeadways[i] = (downtrackDistance[i] - downtrackDistance[i+1]) / speed[i+1]; // downtrack is in m, speed is in m/s.
            }
            // If no one behind or following car stoped, return infinity. 
            else
            {
                timeHeadways[i] = std::numeric_limits<double>::infinity();
            }
        }
        return timeHeadways; // time is in s.
    }

References config_, process_bag::i, and platoon_strategic_ihp::PlatoonPluginConfig::ss_theta.

Referenced by allPredecessorFollowing().

Here is the caller graph for this function:

◆ changeFromFollowerToLeader()

void platoon_strategic_ihp::PlatoonManager::changeFromFollowerToLeader ( )

Update status when state change from Follower to Leader.

Definition at line 672 of file platoon_manager_ihp.cpp.

                                                    {
        
        // Get current host info - assumes departing leader or front of platoon hasn't already been removed from the vector
        PlatoonMember hostInfo = host_platoon_[hostPosInPlatoon_];
        
        // Clear the front part of the platoon info, since we are splitting off from it; leaves host as element 0
        if (hostPosInPlatoon_ > 0)
        {
            host_platoon_.erase(host_platoon_.begin(), host_platoon_.begin() + hostPosInPlatoon_);
        }else
        {
            RCLCPP_WARN(rclcpp::get_logger("platoon_strategic_ihp"), "### Host becoming leader, but is already at index 0 in host_platoon_ vector!  Vector unchanged.");
        }
 
        hostPosInPlatoon_ = 0;
        isFollower = false;
        currentPlatoonID = boost::uuids::to_string(boost::uuids::random_generator()());
        previousFunctionalDynamicLeaderID_ = "";
        previousFunctionalDynamicLeaderIndex_ = -1;
        RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "The platoon manager is changed from follower state to leader state. New platoon ID = " << currentPlatoonID);
    }

References currentPlatoonID, host_platoon_, hostPosInPlatoon_, isFollower, previousFunctionalDynamicLeaderID_, previousFunctionalDynamicLeaderIndex_, and carma_cooperative_perception::to_string().

Referenced by platoon_strategic_ihp::PlatoonStrategicIHPPlugin::run_follower().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ changeFromLeaderToFollower()

void platoon_strategic_ihp::PlatoonManager::changeFromLeaderToFollower	(	std::string	newPlatoonId,
		std::string	newLeaderId
	)

Update status when state change from Leader to Follower.

Parameters

newPlatoonId	new ID of the platoon
newLeaderId	ID of the new lead vehicle

Definition at line 699 of file platoon_manager_ihp.cpp.

                                                                                                 {
        
        // Save the current host info
        PlatoonMember hostInfo = host_platoon_[hostPosInPlatoon_];
        
        // Clear contents of the platoon vector and rebuild it with the two known members at this time, leader & host.
        // Remaining leader info and info about any other members will get populated as messages come in.
        PlatoonMember newLeader = PlatoonMember();
        newLeader.staticId = newLeaderId;
        host_platoon_.clear();
        host_platoon_.push_back(newLeader); //can get location info updated later with a STATUS or INFO message
        host_platoon_.push_back(hostInfo);
 
        hostPosInPlatoon_ = 1; //since host was previously leader it is now guaranteed to be 2nd in the line (index 1)
        isFollower = true;
        currentPlatoonID = newPlatoonId;
 
        // Clear the record of neighbor platoon, since we likely just joined it
        resetNeighborPlatoon();
 
        RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "The platoon manager is changed from leader state to follower state. Platoon vector re-initialized. Plan ID = " << newPlatoonId);
    }

References currentPlatoonID, host_platoon_, hostPosInPlatoon_, isFollower, resetNeighborPlatoon(), and platoon_strategic_ihp::PlatoonMember::staticId.

Referenced by platoon_strategic_ihp::PlatoonStrategicIHPPlugin::mob_resp_cb_candidatefollower(), and platoon_strategic_ihp::PlatoonStrategicIHPPlugin::mob_resp_cb_leaderaborting().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ clearActionPlan()

void platoon_strategic_ihp::PlatoonManager::clearActionPlan ( )

Resets necessary variables to indicate that the current ActionPlan is dead.

Definition at line 285 of file platoon_manager_ihp.cpp.

    {
        current_plan.valid = false;
        current_plan.planId = dummyID;
        current_plan.peerId = dummyID;
        targetPlatoonID = dummyID;
        // Leave the platoon & leader IDs alone since we might continue to be in one
    }

References current_plan, dummyID, platoon_strategic_ihp::ActionPlan::peerId, platoon_strategic_ihp::ActionPlan::planId, targetPlatoonID, and platoon_strategic_ihp::ActionPlan::valid.

Here is the caller graph for this function:

◆ determineDynamicLeaderBasedOnViolation()

int platoon_strategic_ihp::PlatoonManager::determineDynamicLeaderBasedOnViolation ( std::vector< double > timeHeadways )

private

Determine the proper vehicle to follow based the time headway of each member. Note that the host will always choose the closest violator (i.e. time headaway too small or too large) to follow.

Parameters

timeHeadways A vector containing the time headaway of all platoon members.

Returns: An index indicating the proper vehicle to follow (i.e., leader). If choose to follow platoon leader, return 0.

Note: For both condition, the host will always choose to follow the vechile that has a relatively larger gap in front.

max-vilation (follow veh2): [veh3] -------— [veh2] ----------------------------— [veh1] -------— [veh0] ^ gap2 gap1(max violation) gap0

min-vilation (follow veh1): [veh3] --------—[veh2]—[veh1] -------— [veh0] ^ gap2 gap1 gap0 (min violation)

Definition at line 604 of file platoon_manager_ihp.cpp.

                                                                                            {
        
        // Find the closest violations. 
        int closestLowerBoundaryViolation = findLowerBoundaryViolationClosestToTheHostVehicle(timeHeadways);
        int closestMaximumSpacingViolation = findMaximumSpacingViolationClosestToTheHostVehicle(timeHeadways);
        
        // Compare the violation locations, always following the closer violation vehicle (larger index) first, then the furthur ones.
        if(closestLowerBoundaryViolation > closestMaximumSpacingViolation) {
            RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "APF found violation on closestLowerBoundaryViolation at " << closestLowerBoundaryViolation);
            return closestLowerBoundaryViolation; // Min violation, following the vehicle that is in font of the violating gap.
        } 
        else if(closestLowerBoundaryViolation < closestMaximumSpacingViolation){
            RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "APF found violation on closestMaximumSpacingViolation at " << closestMaximumSpacingViolation);
            return closestMaximumSpacingViolation + 1; // Max violation, follow the vehicle that is behinf the violating gap.
        }
        else{
            RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "APF found no violations on both closestLowerBoundaryViolation and closestMaximumSpacingViolation");
            return 0;
        }
    }

References findLowerBoundaryViolationClosestToTheHostVehicle(), and findMaximumSpacingViolationClosestToTheHostVehicle().

Referenced by allPredecessorFollowing().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ findLowerBoundaryViolationClosestToTheHostVehicle()

int platoon_strategic_ihp::PlatoonManager::findLowerBoundaryViolationClosestToTheHostVehicle ( std::vector< double > timeHeadways ) const

private

Find the closest vehicle to the host vehicle that violates the (time headaway) lower boundary condition.

Parameters

timeHeadways A vector containing the time headaway of all platoon members.

Returns: An index indicating the closest violating vehicle. If no violator, return -1.

Definition at line 639 of file platoon_manager_ihp.cpp.

                                                                                                             {
        // Due to descending downtrack order, the search starts from the platoon rear, which corresponds to last in list.
        for(int i = timeHeadways.size()-1; i >= 0; i--) {
            if(timeHeadways[i] < config_.minAllowableHeadaway)  // in s
            {
                return i;
            }
        }
        return -1;
    }

References config_, process_bag::i, and platoon_strategic_ihp::PlatoonPluginConfig::minAllowableHeadaway.

Referenced by allPredecessorFollowing(), and determineDynamicLeaderBasedOnViolation().

Here is the caller graph for this function:

◆ findMaximumSpacingViolationClosestToTheHostVehicle()

int platoon_strategic_ihp::PlatoonManager::findMaximumSpacingViolationClosestToTheHostVehicle ( std::vector< double > timeHeadways ) const

private

Find the closest vehicle to the host vehicle that violates the (time headaway) maximum spacing condition.

Parameters

timeHeadways A vector containing the time headaway of all platoon members.

Returns: An index indicating the closest violating vehicle. If no violator, return -1.

Definition at line 651 of file platoon_manager_ihp.cpp.

                                                                                                               {
        // UCLA: Add maxAllowableHeadaway adjuster to increase the threshold during gap creating period.
        double maxAllowableHeadaway_adjusted = config_.maxAllowableHeadaway;
        if (isCreateGap) {
            // adjust maximum allowed headway to allow for a bigger gap 
            maxAllowableHeadaway_adjusted = maxAllowableHeadaway_adjusted*(1 + config_.createGapAdjuster);
            } 
        
        //  Due to descending downtrack order, the search starts from the platoon rear, which corresponds to last in list.
        for(int i = timeHeadways.size()-1; i >= 0 ; i--) {
            if(timeHeadways[i] > maxAllowableHeadaway_adjusted) // in s
            {
                return i;
            }
        }
        return -1;
    }

References config_, platoon_strategic_ihp::PlatoonPluginConfig::createGapAdjuster, process_bag::i, isCreateGap, and platoon_strategic_ihp::PlatoonPluginConfig::maxAllowableHeadaway.

Referenced by allPredecessorFollowing(), and determineDynamicLeaderBasedOnViolation().

Here is the caller graph for this function:

◆ getClosestIndex()

int platoon_strategic_ihp::PlatoonManager::getClosestIndex ( double joinerDtD )

UCLA: Return joiner's desired position in terms of target platoon index to cut into the platoon. CAUTION: ASSUMES that the neighbor platoon info is fully populated!

Parameters

joinerDtD The current downtrack distance (with regards to host vehicle) of the joiner vehicle.

Returns: : cut-in index: The index of the gap-leading vehicle within the platoon. If front join, return -1.

Definition at line 911 of file platoon_manager_ihp.cpp.

    {   
        /*
            A naive way to find the closest member that is in front of the host that point to the gap to join the platoon.
            Note: The potential gap leading vehicle should be in front of the joiner (i.e., gap leading vehicle's dtd > joiner's dtd).
                  If the joiner is already in front of the platoon leader, this function will return -1 (i.e., cut-in front).
        */
 
        double min_diff = 99999.0;
        int cut_in_index = -1; //-2 is meaningless default
 
        RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "neighbor_platoon_.size(): " << neighbor_platoon_.size());
 
        // Loop through all target platoon members  
        for(size_t i = 0; i < neighbor_platoon_.size(); i++) 
        {
            double current_member_dtd = neighbor_platoon_[i].vehiclePosition; 
            RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "current_member_dtd: "<< current_member_dtd);
            double curent_dtd_diff = current_member_dtd - joinerDtD;
            RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "curent_dtd_diff: "<< curent_dtd_diff);
            // update min index
            if (curent_dtd_diff > 0 && curent_dtd_diff < min_diff)
            {
                min_diff = current_member_dtd;
                cut_in_index = i;
            }
        }
        
        return cut_in_index;
    }

References process_bag::i, and neighbor_platoon_.

Referenced by platoon_strategic_ihp::PlatoonStrategicIHPPlugin::run_prepare_to_join().

Here is the caller graph for this function:

◆ getCommandSpeed()

double platoon_strategic_ihp::PlatoonManager::getCommandSpeed ( ) const

Returns command speed. in m/s.

Definition at line 738 of file platoon_manager_ihp.cpp.

    {
        return host_platoon_[hostPosInPlatoon_].commandSpeed;
    }

References host_platoon_, and hostPosInPlatoon_.

Referenced by hostMemberUpdates().

Here is the caller graph for this function:

◆ getCurrentCrosstrackDistance()

double platoon_strategic_ihp::PlatoonManager::getCurrentCrosstrackDistance ( ) const

Returns current crosstrack distance, in m.

Definition at line 750 of file platoon_manager_ihp.cpp.

    {
        return host_platoon_[hostPosInPlatoon_].vehicleCrossTrack;
    }

References host_platoon_, and hostPosInPlatoon_.

Referenced by hostMemberUpdates().

Here is the caller graph for this function:

◆ getCurrentDowntrackDistance()

double platoon_strategic_ihp::PlatoonManager::getCurrentDowntrackDistance ( ) const

Returns current downtrack distance, in m.

Definition at line 744 of file platoon_manager_ihp.cpp.

    {
        return host_platoon_[hostPosInPlatoon_].vehiclePosition;
    }

References host_platoon_, and hostPosInPlatoon_.

Referenced by getIHPDesPosFollower(), getPredecessorTimeHeadwaySum(), and hostMemberUpdates().

Here is the caller graph for this function:

◆ getCurrentPlatoonLength()

double platoon_strategic_ihp::PlatoonManager::getCurrentPlatoonLength ( )

Returns overall length of the platoon. in m.

Definition at line 762 of file platoon_manager_ihp.cpp.

                                                   {
        //this works even if platoon size is 1 (can't be 0)
        return host_platoon_[0].vehiclePosition - host_platoon_[host_platoon_.size() - 1].vehiclePosition + config_.vehicleLength; 
    }

References config_, host_platoon_, and platoon_strategic_ihp::PlatoonPluginConfig::vehicleLength.

Referenced by platoon_strategic_ihp::PlatoonStrategicIHPPlugin::composeMobilityOperationINFO().

Here is the caller graph for this function:

◆ getCurrentSpeed()

double platoon_strategic_ihp::PlatoonManager::getCurrentSpeed ( ) const

Returns current speed, in m/s.

Definition at line 733 of file platoon_manager_ihp.cpp.

                                                 {
        return host_platoon_[hostPosInPlatoon_].vehicleSpeed;
    }

References host_platoon_, and hostPosInPlatoon_.

Referenced by getIHPDesPosFollower(), and hostMemberUpdates().

Here is the caller graph for this function:

◆ getCutInGap()

double platoon_strategic_ihp::PlatoonManager::getCutInGap	(	const int	gap_leading_index,
		const double	joinerDtD
	)

UCLA: Return the current actual gap size in the target platoon for cut-in join, in m. Note: The origin of the vehicle (for downtrack distance calculation) is located at the rear axle. CAUTION: ASSUMES that the neighbor platoon info is fully populated!

Parameters

gap_leading_index	The platoon index of the gap-leading vehicle.
joinerDtD	The current downtrack distance (with regards to host vehicle route) of the joiner vehicle.

Returns: : cut-in gap: The desired gap size for cut-in join, in m.

Definition at line 943 of file platoon_manager_ihp.cpp.

    {
        /*
            Locate the target cut-in join gap size based on the index.   
        */
 
        // Initiate variables 
        double gap_size = -0.999;
        size_t index = 0;
        if (gap_leading_index >= 0)
        {
            index = static_cast<size_t>(gap_leading_index);
        }
 
        // cut-in from front 
        if (gap_leading_index == -1)
        {
            double gap_rear_dtd = neighbor_platoon_[0].vehiclePosition;
            gap_size = joinerDtD - gap_rear_dtd - config_.vehicleLength;
        }
        // cut-in from behind 
        else if (index == neighbor_platoon_.size() - 1)
        {    
            double gap_leading_dtd = neighbor_platoon_[index].vehiclePosition;
            gap_size = gap_leading_dtd - joinerDtD - config_.vehicleLength;;
        }
        // cut-in in the middle
        else
        {
            double gap_leading_dtd = neighbor_platoon_[index].vehiclePosition;
            double gap_rear_dtd = neighbor_platoon_[index + 1].vehiclePosition;
            gap_size = gap_leading_dtd - gap_rear_dtd - config_.vehicleLength;
        }
 
        // return gap_size value
        return gap_size;
    }

References config_, neighbor_platoon_, and platoon_strategic_ihp::PlatoonPluginConfig::vehicleLength.

◆ getDistanceToPredVehicle()

double platoon_strategic_ihp::PlatoonManager::getDistanceToPredVehicle ( )

Returns distance to the predessecor vehicle, in m.

Definition at line 728 of file platoon_manager_ihp.cpp.

                                                    {
        return gapWithPred_;
    }

References gapWithPred_.

Referenced by platoon_strategic_ihp::PlatoonStrategicIHPPlugin::run_candidate_follower(), and platoon_strategic_ihp::PlatoonStrategicIHPPlugin::run_leader_aborting().

Here is the caller graph for this function:

◆ getDynamicLeader()

PlatoonMember platoon_strategic_ihp::PlatoonManager::getDynamicLeader ( )

Returns dynamic leader of the host vehicle.

Returns: The current dynamic leader as a vehcile object.

it might happened when the subject vehicle gets far away from the preceding vehicle, in which case the host vehicle will follow the one in front.

Definition at line 388 of file platoon_manager_ihp.cpp.

                                                  {
        PlatoonMember dynamicLeader;
        RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "host_platoon_ size: " << host_platoon_.size());
        if(isFollower) 
        {
            RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "Leader initially set as first vehicle in platoon");
            // return the first vehicle in the platoon as default if no valid algorithm applied
            // due to downtrack descending order, the platoon front veihcle is the first in list. 
            dynamicLeader = host_platoon_[0];
            if (algorithmType_ == "APF_ALGORITHM"){
                size_t newLeaderIndex = allPredecessorFollowing();
 
                dynamic_leader_index_ = (int)newLeaderIndex;
                RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "dynamic_leader_index_: " << dynamic_leader_index_);
                if(newLeaderIndex < host_platoon_.size()) { //this must always be true!
                    dynamicLeader = host_platoon_[newLeaderIndex];
                    RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "APF output: " << dynamicLeader.staticId);
                    previousFunctionalDynamicLeaderIndex_ = newLeaderIndex;
                    previousFunctionalDynamicLeaderID_ = dynamicLeader.staticId;
                }
                else //something is terribly wrong in the logic!
                {
                    RCLCPP_WARN_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "newLeaderIndex = " << newLeaderIndex << " is invalid coming from allPredecessorFollowing!");
                    dynamicLeader = host_platoon_[getNumberOfVehicleInFront() - 1];
                    // update index and ID 
                    previousFunctionalDynamicLeaderIndex_ = getNumberOfVehicleInFront()-1;
                    previousFunctionalDynamicLeaderID_ = dynamicLeader.staticId;
                    RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "Based on the output of APF algorithm we start to follow our predecessor.");
                }
            }
        }
        return dynamicLeader;
 
    }

References algorithmType_, allPredecessorFollowing(), dynamic_leader_index_, getNumberOfVehicleInFront(), host_platoon_, isFollower, previousFunctionalDynamicLeaderID_, previousFunctionalDynamicLeaderIndex_, and platoon_strategic_ihp::PlatoonMember::staticId.

Referenced by platoon_strategic_ihp::PlatoonStrategicIHPPlugin::composePlatoonInfoMsg().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ getHostPlatoonSize()

int platoon_strategic_ihp::PlatoonManager::getHostPlatoonSize ( )

Returns total size of the platoon , in number of vehicles.

Definition at line 279 of file platoon_manager_ihp.cpp.

                                           {
        RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "host platoon size: " << host_platoon_.size());
        return host_platoon_.size();
    }

References host_platoon_.

Here is the caller graph for this function:

◆ getHostStaticID()

std::string platoon_strategic_ihp::PlatoonManager::getHostStaticID ( ) const

Returns current host static ID as a string.

Definition at line 756 of file platoon_manager_ihp.cpp.

    {
        return HostMobilityId;
    }

References HostMobilityId.

Referenced by hostMemberUpdates(), platoon_strategic_ihp::PlatoonStrategicIHPPlugin::mob_req_cb_follower(), and updatesOrAddMemberInfo().

Here is the caller graph for this function:

◆ getIHPDesPosFollower()

double platoon_strategic_ihp::PlatoonManager::getIHPDesPosFollower ( double dt )

UCLA: Return follower's desired position (i.e., downtrack, in m) that maintains the desired intra-platoon time gap, based on IHP platoon trajectory regulation algorithm.

\params dt: The planning time step, in s.

Returns: : Host vehicle's desired position as downtrack distance, in m.

Calculate desired position based on previous vehicle's trajectory for followers.

TODO: The platoon trajectory regulation is derived with the assumption that all vehicle have identical length (i.e., 5m). Future development is needed to include variable vehicle length into consideration.

Definition at line 819 of file platoon_manager_ihp.cpp.

    {
        // 1. read dtd vector 
        // dtd vector 
        std::vector<double> downtrackDistance(host_platoon_.size());
        for (size_t i = 0; i < host_platoon_.size(); i++)
        {
            downtrackDistance[i] = host_platoon_[i].vehiclePosition;
        }
        // speed vector
        std::vector<double> speed(host_platoon_.size());
        for (size_t i = 0; i < host_platoon_.size(); i++)
        {
            speed[i] = host_platoon_[i].vehicleSpeed;
        }
 
        // 2. find the summation of "veh_len/veh_speed" for all predecessors
        double tmp_time_hdw = 0.0;
        double cur_dtd;
        for (size_t index = 0; index < downtrackDistance.size(); index++)
        {
            cur_dtd = downtrackDistance[index];
            if (cur_dtd > getCurrentDowntrackDistance())
            {
                // greater dtd ==> in front of host veh 
                tmp_time_hdw += config_.vehicleLength / (speed[index] + 0.00001);
            }
        }
 
        // 3. read host veh and front veh info 
        // Predecessor vehicle data.
        double pred_spd = speed[getNumberOfVehicleInFront()-1]; // m/s 
        double pred_pos = downtrackDistance[getNumberOfVehicleInFront()-1]; // m
        
        // host data. 
        double ego_spd = getCurrentSpeed(); // m/s
        double ego_pos = getCurrentDowntrackDistance(); // m
        
        // platoon position index
        int pos_idx = getNumberOfVehicleInFront();
 
        double desirePlatoonGap = config_.intra_tau; //s
        
        // IHP desired position calculation methods
        double pos_g; // desired downtrack position calculated with time-gap, in m.
        double pos_h; // desired downtrack position calculated with distance headaway, in m.
 
        // 4. IHP gap regualtion 
        
        // intermediate variables 
        double timeGapAndStepRatio = desirePlatoonGap/time_step;      // The ratio between desired platoon time gap and the current time_step.
        double totalTimeGap = desirePlatoonGap*pos_idx;               // The overall time gap from host vehicle to the platoon leader, in s.
 
        // calcilate pos_gap and pos_headway
        if ((pred_spd <= ego_spd) && (ego_spd <= config_.ss_theta))
        {
            // ---> 4.1 pos_g 
            pos_g = (pred_pos - config_.vehicleLength - config_.standstill + ego_pos*timeGapAndStepRatio) / (1 + timeGapAndStepRatio);
            // ---> 4.2 pos_h
            double pos_h_nom = (pred_pos - config_.standstill + ego_pos*(totalTimeGap + tmp_time_hdw)/time_step);
            double pos_h_denom = (1 + ((totalTimeGap + tmp_time_hdw)/time_step));
            pos_h = pos_h_nom/pos_h_denom;
 
        }
        else
        {   
            // ---> 4.1 pos_g 
            pos_g = (pred_pos - config_.vehicleLength + ego_pos*(timeGapAndStepRatio)) / (1 + timeGapAndStepRatio);
            // ---> 4.2 pos_h
            double pos_h_nom = (pred_pos + ego_pos*(totalTimeGap + tmp_time_hdw)/time_step);
            double pos_h_denom = (1 + ((totalTimeGap + tmp_time_hdw)/time_step));
            pos_h = pos_h_nom/pos_h_denom;
        }
 
        // ---> 4.3 desire speed and desire location 
        double pos_des = config_.gap_weight*pos_g + (1.0 - config_.gap_weight)*pos_h;
        // double des_spd = (pos_des - ego_pos) / time_step;
 
        // ---> 4.4 return IHP calculated desired location
        return pos_des;
    }

References config_, platoon_strategic_ihp::PlatoonPluginConfig::gap_weight, getCurrentDowntrackDistance(), getCurrentSpeed(), getNumberOfVehicleInFront(), host_platoon_, process_bag::i, platoon_strategic_ihp::PlatoonPluginConfig::intra_tau, platoon_strategic_ihp::PlatoonPluginConfig::ss_theta, platoon_strategic_ihp::PlatoonPluginConfig::standstill, and platoon_strategic_ihp::PlatoonPluginConfig::vehicleLength.

Here is the call graph for this function:

◆ getNumberOfVehicleInFront()

int platoon_strategic_ihp::PlatoonManager::getNumberOfVehicleInFront ( )

Get number of vehicles in front of host vehicle inside platoon.

Definition at line 723 of file platoon_manager_ihp.cpp.

                                                  {
        return hostPosInPlatoon_;
    }

References hostPosInPlatoon_.

Referenced by allPredecessorFollowing(), platoon_strategic_ihp::PlatoonStrategicIHPPlugin::composePlatoonInfoMsg(), getDynamicLeader(), getIHPDesPosFollower(), getPredecessorPosition(), and platoon_strategic_ihp::PlatoonStrategicIHPPlugin::run_follower().

Here is the caller graph for this function:

◆ getPlatoonFrontDowntrackDistance()

double platoon_strategic_ihp::PlatoonManager::getPlatoonFrontDowntrackDistance ( )

UCLA: Return the platoon leader downtrack distance, in m.

Definition at line 382 of file platoon_manager_ihp.cpp.

                                                           {
        // due to downtrack descending order, the firest vehicle in list is the platoon front vehicle. 
        return host_platoon_[0].vehiclePosition;
    }

References host_platoon_.

Referenced by platoon_strategic_ihp::PlatoonStrategicIHPPlugin::mob_req_cb_leader().

Here is the caller graph for this function:

◆ getPlatoonRearDowntrackDistance()

double platoon_strategic_ihp::PlatoonManager::getPlatoonRearDowntrackDistance ( )

Returns downtrack distance of the rear vehicle in platoon, in m.

Definition at line 375 of file platoon_manager_ihp.cpp.

                                                          {
        // due to downtrack descending order, the 1ast vehicle in list is the platoon rear vehicle.
        // Even if host is solo, platoon size is 1 so this works.
        return host_platoon_[host_platoon_.size()-1].vehiclePosition;
    }

References host_platoon_.

Referenced by platoon_strategic_ihp::PlatoonStrategicIHPPlugin::mob_req_cb_leader().

Here is the caller graph for this function:

◆ getPredecessorPosition()

double platoon_strategic_ihp::PlatoonManager::getPredecessorPosition ( )

UCLA: Return the position of the preceding vehicle, in m.

Definition at line 809 of file platoon_manager_ihp.cpp.

    {
        // Read host index. 
        int host_platoon_index = getNumberOfVehicleInFront();
 
        // Return speed
        return host_platoon_[host_platoon_index].vehiclePosition; // m
    }

References getNumberOfVehicleInFront(), and host_platoon_.

Referenced by platoon_strategic_ihp::PlatoonStrategicIHPPlugin::composePlatoonInfoMsg().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ getPredecessorSpeed()

double platoon_strategic_ihp::PlatoonManager::getPredecessorSpeed ( )

UCLA: Return the speed of the preceding vehicle, in m/s.

Definition at line 803 of file platoon_manager_ihp.cpp.

    {
        return host_platoon_[hostPosInPlatoon_].vehicleSpeed; // m/s 
    }

References host_platoon_, and hostPosInPlatoon_.

Referenced by platoon_strategic_ihp::PlatoonStrategicIHPPlugin::composePlatoonInfoMsg().

Here is the caller graph for this function:

◆ getPredecessorTimeHeadwaySum()

double platoon_strategic_ihp::PlatoonManager::getPredecessorTimeHeadwaySum ( )

UCLA: Return the time headway summation of all predecessors, in s.

Definition at line 769 of file platoon_manager_ihp.cpp.

    {
        // 1. read dtd vector 
        // dtd vector 
        std::vector<double> downtrackDistance(host_platoon_.size());
        for (size_t i = 0; i < host_platoon_.size(); i++)
        {
            downtrackDistance[i] = host_platoon_[i].vehiclePosition;
        }
        // speed vector
        std::vector<double> speed(host_platoon_.size());
        for (size_t i = 0; i < host_platoon_.size(); i++)
        {
            speed[i] = host_platoon_[i].vehicleSpeed;
        }
 
        // 2. find the summation of "veh_len/veh_speed" for all predecessors
        double tmp_time_hdw = 0.0;
        double cur_dtd;
        for (size_t index = 0; index < downtrackDistance.size(); index++)
        {
            cur_dtd = downtrackDistance[index];
            if (cur_dtd > getCurrentDowntrackDistance())
            {
                // greater dtd ==> in front of host veh 
                tmp_time_hdw += config_.vehicleLength / (speed[index] + 0.00001);
            }
        }
 
        // Return the calcualted value
        return tmp_time_hdw;
    }

References config_, getCurrentDowntrackDistance(), host_platoon_, process_bag::i, and platoon_strategic_ihp::PlatoonPluginConfig::vehicleLength.

Referenced by platoon_strategic_ihp::PlatoonStrategicIHPPlugin::composePlatoonInfoMsg().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ getTimeHeadwayFromIndex()

std::vector< double > platoon_strategic_ihp::PlatoonManager::getTimeHeadwayFromIndex	(	std::vector< double >	timeHeadways,
		int	start
	)		const

private

Return a sub-vector of the platoon-wise time headaways vector that start with a given index.

Parameters

timeHeadways A vector containing the time headaway of all platoon members. start: An integer indicates the starting position.

Returns: An sub-vector start with given index.

Definition at line 564 of file platoon_manager_ihp.cpp.

                                                                                                             {
        std::vector<double> result(timeHeadways.begin() + start-1, timeHeadways.end());
        return result;
    }

Referenced by allPredecessorFollowing().

Here is the caller graph for this function:

◆ hostMemberUpdates()

void platoon_strategic_ihp::PlatoonManager::hostMemberUpdates	(	const std::string &	senderId,
		const std::string &	platoonId,
		const std::string &	params,
		const double &	DtD,
		const double &	CtD
	)

Update information for members of the host's platoon based on a mobility operation STATUS message.

Parameters

senderId	static id of the broadcasting vehicle
platoonId	platoon id
params	message strategy parameters
DtD	downtrack distance along host's route, m
CtD	crosstrack distance in roadway at host's location, m

Definition at line 73 of file platoon_manager_ihp.cpp.

    {
 
        // parse params, read member data
        std::vector<std::string> inputsParams;
        boost::algorithm::split(inputsParams, params, boost::is_any_of(","));
        // read command speed, m/s
        std::vector<std::string> cmd_parsed;
        boost::algorithm::split(cmd_parsed, inputsParams[0], boost::is_any_of(":"));
        double cmdSpeed = std::stod(cmd_parsed[1]);
        // get DtD directly instead of parsing message, m
        double dtDistance = DtD;
        // get CtD directly 
        double ctDistance = CtD;
        // read current speed, m/s
        std::vector<std::string> cur_parsed;
        boost::algorithm::split(cur_parsed, inputsParams[1], boost::is_any_of(":"));
        double curSpeed = std::stod(cur_parsed[1]);
 
        // If we are currently in a follower state:
        // 1. We will update platoon ID based on leader's STATUS
        // 2. We will update platoon members info based on platoon ID for all members
        if (isFollower) 
        {
            // Sanity check
            if (platoonLeaderID.compare(host_platoon_[0].staticId) != 0)
            {
                RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "///// platoonLeaderID NOT PROPERLY ASSIGNED! Value = " << platoonLeaderID
                                << ", host_platoon_[0].staticId = " << host_platoon_[0].staticId);
            }
 
            // read message status        
            bool isFromLeader = platoonLeaderID == senderId;
            bool needPlatoonIdChange = isFromLeader && (currentPlatoonID != platoonId);
 
            if(needPlatoonIdChange)
            {
                // TODO: better to have leader explicitly inform us that it is merging into another platoon, rather than require us to deduce it here.
                //       This condition may also result from missing some message traffic, new leader in this platoon, or other confusion/incorrect code.
                //       Consider using a new STATUS msg param that tells members of new platoon ID and new leader ID when a change is made.
                RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "It seems that the current leader is joining another platoon.");
                RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "So the platoon ID is changed from " << currentPlatoonID << " to " << platoonId);
                currentPlatoonID = platoonId;
                updatesOrAddMemberInfo(host_platoon_, senderId, cmdSpeed, dtDistance, ctDistance, curSpeed); 
            } 
            else if (currentPlatoonID == platoonId)
            {
                RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "This STATUS messages is from our platoon. Updating the info...");
                updatesOrAddMemberInfo(host_platoon_, senderId, cmdSpeed, dtDistance, ctDistance, curSpeed);
                RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "The first vehicle in our list is now " << host_platoon_[0].staticId);
            } 
            else //sender is in a different platoon
            {
                RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "This STATUS message is not from a vehicle we care about. Ignore this message with id: " << senderId);
            }
        }
        else //host is leader
        {
            // If we are currently in any leader state, we only update platoon member based on platoon ID
            RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "Host is leader: currentPlatoonID = " << currentPlatoonID << ", incoming platoonId = " << platoonId);
            if (currentPlatoonID == platoonId)
            {
                RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "This STATUS messages is from our platoon. Updating the info...");
                updatesOrAddMemberInfo(host_platoon_, senderId, cmdSpeed, dtDistance, ctDistance, curSpeed); 
            }
            else
            {
                RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "Platoon IDs not matched");
                RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "currentPlatoonID: " << currentPlatoonID);
                RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "incoming platoonId: " << platoonId);
            }
        }
        
        // update host vehicle information each time new member is updated. Now platoon contains host vehicle.
        std::string hostStaticId = getHostStaticID();
        double hostcmdSpeed = getCommandSpeed();
        double hostDtD = getCurrentDowntrackDistance();
        double hostCtD = getCurrentCrosstrackDistance();
        double hostCurSpeed = getCurrentSpeed();
        updatesOrAddMemberInfo(host_platoon_, hostStaticId, hostcmdSpeed, hostDtD, hostCtD, hostCurSpeed);
    }

References currentPlatoonID, getCommandSpeed(), getCurrentCrosstrackDistance(), getCurrentDowntrackDistance(), getCurrentSpeed(), getHostStaticID(), host_platoon_, isFollower, platoonLeaderID, process_traj_logs::split, and updatesOrAddMemberInfo().

Referenced by platoon_strategic_ihp::PlatoonStrategicIHPPlugin::mob_op_cb_STATUS().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ insufficientGapWithPredecessor()

bool platoon_strategic_ihp::PlatoonManager::insufficientGapWithPredecessor ( double distanceToPredVehicle )

private

Check the gap with the predecessor vehicle.

Parameters

distanceToPredVehicle The distance between the host to the predecessor vehicle.

Returns: (bool) if the predecessor is to close.

Definition at line 570 of file platoon_manager_ihp.cpp.

                                                                                    {
        
        // For normal operation, gap > minGap is necessary. 
        bool frontGapIsTooSmall = distanceToPredVehicle < config_.minCutinGap; 
        
        // Host vehicle was following predecessor vehicle. --> The predecessor vehicle was violating gap threshold.
        bool previousLeaderIsPredecessor = previousFunctionalDynamicLeaderID_ == host_platoon_[host_platoon_.size() - 1].staticId; 
        
        // Gap greater than maxGap_ is necessary for host to stop choosing predecessor as dynamic leader. 
        bool frontGapIsNotLargeEnough = distanceToPredVehicle < config_.maxCutinGap && previousLeaderIsPredecessor;
 
        return (frontGapIsTooSmall || frontGapIsNotLargeEnough);
    }

References config_, host_platoon_, platoon_strategic_ihp::PlatoonPluginConfig::maxCutinGap, platoon_strategic_ihp::PlatoonPluginConfig::minCutinGap, and previousFunctionalDynamicLeaderID_.

Referenced by allPredecessorFollowing().

Here is the caller graph for this function:

◆ neighborMemberUpdates()

void platoon_strategic_ihp::PlatoonManager::neighborMemberUpdates	(	const std::string &	senderId,
		const std::string &	platoonId,
		const std::string &	params,
		const double &	DtD,
		const double &	CtD
	)

Update information for members of a neighboring platoon based on a mobility operation STATUS message.

Parameters

senderId	static id of the broadcasting vehicle
platoonId	platoon id
params	message strategy parameters
DtD	downtrack distance along host's route, m
CtD	crosstrack distance in roadway at host's location, m

Definition at line 157 of file platoon_manager_ihp.cpp.

    {
 
        // parse params, read member data
        std::vector<std::string> inputsParams;
        boost::algorithm::split(inputsParams, params, boost::is_any_of(","));
        // read command speed, m/s
        std::vector<std::string> cmd_parsed;
        boost::algorithm::split(cmd_parsed, inputsParams[0], boost::is_any_of(":"));
        double cmdSpeed = std::stod(cmd_parsed[1]);
        // get DtD directly instead of parsing message, m
        double dtDistance = DtD;
        // get CtD directly 
        double ctDistance = CtD;
        // read current speed, m/s
        std::vector<std::string> cur_parsed;
        boost::algorithm::split(cur_parsed, inputsParams[1], boost::is_any_of(":"));
        double curSpeed = std::stod(cur_parsed[1]);
 
        if (neighborPlatoonID == platoonId)
        {
            updatesOrAddMemberInfo(neighbor_platoon_, senderId, cmdSpeed, dtDistance, ctDistance, curSpeed);
            RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "This STATUS messages is from the target platoon. Updating the info...");
            RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "The first vehicle in that platoon is now " << neighbor_platoon_[0].staticId);
 
            // If we have data on all members of a neighboring platoon, set a complete record flag
            if (neighbor_platoon_info_size_ > 1  &&
                neighbor_platoon_.size() == neighbor_platoon_info_size_)
            {
                is_neighbor_record_complete_ = true;
                RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "Neighbor record is complete!");
            }
        } 
        else //sender is in a different platoon
        {
            RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "This STATUS message is not from a vehicle we care about. Ignore this message from sender: " 
                            << senderId << " about platoon: " << platoonId);
        }
    }

References is_neighbor_record_complete_, neighbor_platoon_, neighbor_platoon_info_size_, neighborPlatoonID, process_traj_logs::split, and updatesOrAddMemberInfo().

Referenced by platoon_strategic_ihp::PlatoonStrategicIHPPlugin::mob_op_cb_STATUS().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ removeMember()

bool platoon_strategic_ihp::PlatoonManager::removeMember ( const size_t mem )

Removes a single member from the internal record of platoon members.

Parameters

mem	index of the member to be removed (zero-based)

Returns: true if removal was successful, false otherwise

Definition at line 322 of file platoon_manager_ihp.cpp.

    {
        // Don't remove ourselves!
        if (hostPosInPlatoon_ == mem)
        {
            return false;
        }
 
        // Don't remove a member that isn't there
        else if (host_platoon_.size() <= 1  ||  mem >= host_platoon_.size())
        {
            return false;
        }
 
        if (mem < hostPosInPlatoon_)
        {
            --hostPosInPlatoon_;
        }
        host_platoon_.erase(host_platoon_.begin() + mem, host_platoon_.begin() + mem + 1);
 
        // If host is the only remaining member then clean up the other platoon data
        if (host_platoon_.size() == 1)
        {
            currentPlatoonID = dummyID;
            platoonLeaderID = dummyID;
            hostPosInPlatoon_ = 0;
        }
 
        return true;
    }

References currentPlatoonID, dummyID, host_platoon_, hostPosInPlatoon_, and platoonLeaderID.

Referenced by platoon_strategic_ihp::PlatoonStrategicIHPPlugin::mob_resp_cb_leaderaborting(), and removeMemberById().

Here is the caller graph for this function:

◆ removeMemberById()

bool platoon_strategic_ihp::PlatoonManager::removeMemberById ( const std::string id )

Removes a single member from the internal record of platoon members.

Parameters

id	the vehicle ID of the member to be removed

Returns: true if removal was successful, false otherwise

Definition at line 353 of file platoon_manager_ihp.cpp.

    {
        // Don't remove ourselves!
        if (id.compare(HostMobilityId) == 0)
        {
            return false;
        }
 
        // Search for the member with a matching ID and remove it
        for (size_t m = 0;  m < host_platoon_.size();  ++m)
        {
            if (id.compare(host_platoon_[m].staticId) == 0)
            {
                return removeMember(m);
            }
        }
 
        // Indicate the member was not found
        return false;
    }

References host_platoon_, HostMobilityId, and removeMember().

Here is the call graph for this function:

◆ resetHostPlatoon()

void platoon_strategic_ihp::PlatoonManager::resetHostPlatoon ( )

Resets all variables that might indicate other members of the host's platoon; sets the host back to solo vehicle.

Definition at line 295 of file platoon_manager_ihp.cpp.

    {
        // Remove any elements in the platoon vector other than the host vehicle
        if (host_platoon_.size() > hostPosInPlatoon_ + 1)
        {
            host_platoon_.erase(host_platoon_.begin() + hostPosInPlatoon_ + 1, host_platoon_.end());
        }
        host_platoon_.erase(host_platoon_.begin(), host_platoon_.begin() + hostPosInPlatoon_);
 
        // Clean up other variables
        currentPlatoonID = dummyID;
        platoonLeaderID = dummyID;
        hostPosInPlatoon_ = 0;
        isCreateGap = false;
        dynamic_leader_index_ = 0;
    }

References currentPlatoonID, dummyID, dynamic_leader_index_, host_platoon_, hostPosInPlatoon_, isCreateGap, and platoonLeaderID.

Referenced by platoon_strategic_ihp::PlatoonStrategicIHPPlugin::mob_req_cb_candidateleader(), platoon_strategic_ihp::PlatoonStrategicIHPPlugin::mob_resp_cb_candidatefollower(), platoon_strategic_ihp::PlatoonStrategicIHPPlugin::mob_resp_cb_leader(), platoon_strategic_ihp::PlatoonStrategicIHPPlugin::run_candidate_leader(), and platoon_strategic_ihp::PlatoonStrategicIHPPlugin::run_prepare_to_join().

Here is the caller graph for this function:

◆ resetNeighborPlatoon()

void platoon_strategic_ihp::PlatoonManager::resetNeighborPlatoon ( )

Resets all variables that describe a neighbor platoon, so that no neighbor is known.

Definition at line 313 of file platoon_manager_ihp.cpp.

    {
        neighbor_platoon_.clear();
        neighbor_platoon_info_size_ = 0;
        neighborPlatoonID = dummyID;
        neighbor_platoon_leader_id_ = dummyID;
        is_neighbor_record_complete_ = false;
    }

References dummyID, is_neighbor_record_complete_, neighbor_platoon_, neighbor_platoon_info_size_, neighbor_platoon_leader_id_, and neighborPlatoonID.

Referenced by changeFromLeaderToFollower(), platoon_strategic_ihp::PlatoonStrategicIHPPlugin::mob_op_cb(), platoon_strategic_ihp::PlatoonStrategicIHPPlugin::mob_req_cb_candidateleader(), and platoon_strategic_ihp::PlatoonStrategicIHPPlugin::mob_resp_cb_candidatefollower().

Here is the caller graph for this function:

◆ updateHostPose()

void platoon_strategic_ihp::PlatoonManager::updateHostPose	(	const double	downtrack,
		const double	crosstrack
	)

Stores the latest info on location of the host vehicle.

Parameters

downtrack	distance downtrack from beginning of route, m
crosstrack	distance crosstrack from roadway centerline, m

Definition at line 58 of file platoon_manager_ihp.cpp.

    {
        RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "Host (index " << hostPosInPlatoon_ << "): downtrack = " << downtrack << ", crosstrack = " << crosstrack);
        host_platoon_[hostPosInPlatoon_].vehiclePosition = downtrack;
        host_platoon_[hostPosInPlatoon_].vehicleCrossTrack = crosstrack;
    }

References host_platoon_, and hostPosInPlatoon_.

Referenced by platoon_strategic_ihp::PlatoonStrategicIHPPlugin::onSpin(), and platoon_strategic_ihp::PlatoonStrategicIHPPlugin::pose_cb().

Here is the caller graph for this function:

◆ updateHostSpeeds()

void platoon_strategic_ihp::PlatoonManager::updateHostSpeeds	(	const double	cmdSpeed,
		const double	actualSpeed
	)

Stores the latest info on host vehicle's command & actual speeds.

Parameters

cmdSpeed	current commanded speed, m/s
actualSpeed	current measured speed, m/s

Definition at line 66 of file platoon_manager_ihp.cpp.

    {
        host_platoon_[hostPosInPlatoon_].commandSpeed = cmdSpeed;
        host_platoon_[hostPosInPlatoon_].vehicleSpeed = actualSpeed;
    }

References host_platoon_, and hostPosInPlatoon_.

Referenced by platoon_strategic_ihp::PlatoonStrategicIHPPlugin::onSpin().

Here is the caller graph for this function:

◆ updatesOrAddMemberInfo()

void platoon_strategic_ihp::PlatoonManager::updatesOrAddMemberInfo	(	std::vector< PlatoonMember > &	platoon,
		std::string	senderId,
		double	cmdSpeed,
		double	dtDistance,
		double	ctDistance,
		double	curSpeed
	)

Updates the list of vehicles in the specified platoon, based on info available from a mobility operation STATUS message from one of that platoon's vehicles. It ensures the list of vehicles is properly sorted in order of appearance from front to rear in the platoon. If the host is in the platoon, it will update host info as well.

Parameters

platoon	the list of vehicles in the platoon in question
senderId	vehicle ID that sent the current info
cmdSpeed	the commanded speed of the sending vehicle
dtDistance	the downtrack location (from beginning of host's route) of the sending vehicle, m
ctDistance	the crosstrack location (from center of roadway at host's current route location) of sending vehicle, m
curSpeed	the current actual speed of the sending vehicle, m/s

Definition at line 199 of file platoon_manager_ihp.cpp.

    {
        bool isExisted = false;
        bool sortNeeded = false;
 
        // update/add this info into the list
        for (size_t i = 0;  i < platoon.size();  ++i){
            if(platoon[i].staticId == senderId) {
                if (abs(dtDistance - platoon[i].vehiclePosition)/(platoon[i].vehiclePosition + 0.01) > config_.significantDTDchange)
                {
                    RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"),  "DTD of member " << platoon[i].staticId << " is changed significantly, so a new sort is needed");
 
                    sortNeeded = true;
                }
                platoon[i].commandSpeed = cmdSpeed;         // m/s
                platoon[i].vehiclePosition = dtDistance;    // m 
                platoon[i].vehicleCrossTrack = ctDistance;  // m
                platoon[i].vehicleSpeed = curSpeed;         // m/s
                platoon[i].timestamp = timer_factory_->now().nanoseconds()/1000000;
                RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "Receive and update platooning info on member " << i << ", ID:" << platoon[i].staticId);
                RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "    CommandSpeed       = " << platoon[i].commandSpeed);
                RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "    Actual Speed       = " << platoon[i].vehicleSpeed);
                RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "    Downtrack Location = " << platoon[i].vehiclePosition);
                RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "    Crosstrack dist    = " << platoon[i].vehicleCrossTrack);
 
                if (senderId == HostMobilityId)
                {
                    hostPosInPlatoon_ = i;
                    RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "    This is the HOST vehicle");
                }
                isExisted = true;
            }
        }
 
        if (sortNeeded)
        {
            // sort the platoon member based on dowtrack distance (m) in an descending order.
            std::sort(std::begin(platoon), std::end(platoon), [](const PlatoonMember &a, const PlatoonMember &b){return a.vehiclePosition > b.vehiclePosition;});
            RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "Platoon is re-sorted due to large difference in dtd update.");
            RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "    Platoon order is now:");
            for (size_t i = 0;  i < platoon.size();  ++i)
            {
                std::string hostFlag = " ";
                if (platoon[i].staticId == getHostStaticID())
                {
                    hostPosInPlatoon_ = i;
                    hostFlag = "Host";
                }
                RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "    " << platoon[i].staticId << "its DTD: " << platoon[i].vehiclePosition << " " << hostFlag);
            }
        }
 
        // if not already exist, add to platoon list.
        if(!isExisted) {
            long cur_t = timer_factory_->now().nanoseconds()/1000000; // time in millisecond
 
            PlatoonMember newMember = PlatoonMember(senderId, cmdSpeed, curSpeed, dtDistance, ctDistance, cur_t);
            platoon.push_back(newMember);
            // sort the platoon member based on downtrack distance (m) in an descending order.
            std::sort(std::begin(platoon), std::end(platoon), [](const PlatoonMember &a, const PlatoonMember &b){return a.vehiclePosition > b.vehiclePosition;});
 
            RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "Add a new vehicle into our platoon list " << newMember.staticId << " platoon.size now = " << platoon.size());
            RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "    Platoon order is now:");
            for (size_t i = 0;  i < platoon.size();  ++i)
            {
                std::string hostFlag = " ";
                if (platoon[i].staticId == getHostStaticID())
                {
                    hostPosInPlatoon_ = i;
                    hostFlag = "Host";
                }
                RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "    " << platoon[i].staticId << "its DTD: " << platoon[i].vehiclePosition << " " << hostFlag);
            }
        }
    }

References config_, getHostStaticID(), HostMobilityId, hostPosInPlatoon_, process_bag::i, platoon_strategic_ihp::PlatoonPluginConfig::significantDTDchange, platoon_strategic_ihp::PlatoonMember::staticId, timer_factory_, and platoon_strategic_ihp::PlatoonMember::vehiclePosition.

Referenced by hostMemberUpdates(), and neighborMemberUpdates().

Here is the call graph for this function:

Here is the caller graph for this function:

Member Data Documentation

◆ algorithmType_

std::string platoon_strategic_ihp::PlatoonManager::algorithmType_ = "APF_ALGORITHM"

private

Definition at line 439 of file platoon_manager_ihp.h.

Referenced by getDynamicLeader().

◆ config_

PlatoonPluginConfig platoon_strategic_ihp::PlatoonManager::config_

private

Definition at line 417 of file platoon_manager_ihp.h.

Referenced by allPredecessorFollowing(), calculateTimeHeadway(), findLowerBoundaryViolationClosestToTheHostVehicle(), findMaximumSpacingViolationClosestToTheHostVehicle(), getCurrentPlatoonLength(), getCutInGap(), getIHPDesPosFollower(), getPredecessorTimeHeadwaySum(), insufficientGapWithPredecessor(), and updatesOrAddMemberInfo().

◆ current_plan

ActionPlan platoon_strategic_ihp::PlatoonManager::current_plan = ActionPlan()

Definition at line 376 of file platoon_manager_ihp.h.

◆ current_platoon_state

PlatoonState platoon_strategic_ihp::PlatoonManager::current_platoon_state = PlatoonState::STANDBY

Definition at line 370 of file platoon_manager_ihp.h.

◆ currentPlatoonID

std::string platoon_strategic_ihp::PlatoonManager::currentPlatoonID = dummyID

Definition at line 364 of file platoon_manager_ihp.h.

◆ downtrack_progress_

double platoon_strategic_ihp::PlatoonManager::downtrack_progress_ = 0

private

Definition at line 435 of file platoon_manager_ihp.h.

◆ dummyID

const std::string platoon_strategic_ihp::PlatoonManager::dummyID = "00000000-0000-0000-0000-000000000000"

Definition at line 358 of file platoon_manager_ihp.h.

Referenced by clearActionPlan(), platoon_strategic_ihp::PlatoonStrategicIHPPlugin::mob_op_cb_leader(), platoon_strategic_ihp::PlatoonStrategicIHPPlugin::mob_op_cb_preparetojoin(), platoon_strategic_ihp::PlatoonStrategicIHPPlugin::mob_op_cb_STATUS(), platoon_strategic_ihp::PlatoonStrategicIHPPlugin::mob_req_cb_leaderwaiting(), platoon_strategic_ihp::PlatoonStrategicIHPPlugin::mob_req_cb_leadwithoperation(), platoon_strategic_ihp::PlatoonStrategicIHPPlugin::mob_resp_cb_candidatefollower(), platoon_strategic_ihp::PlatoonStrategicIHPPlugin::mob_resp_cb_preparetojoin(), removeMember(), resetHostPlatoon(), and resetNeighborPlatoon().

◆ dynamic_leader_index_

size_t platoon_strategic_ihp::PlatoonManager::dynamic_leader_index_ = 0

Definition at line 409 of file platoon_manager_ihp.h.

Referenced by platoon_strategic_ihp::PlatoonStrategicIHPPlugin::composePlatoonInfoMsg(), getDynamicLeader(), and resetHostPlatoon().

◆ gapWithPred_

double platoon_strategic_ihp::PlatoonManager::gapWithPred_ = 0.0

private

Definition at line 434 of file platoon_manager_ihp.h.

Referenced by allPredecessorFollowing(), and getDistanceToPredVehicle().

◆ host_platoon_

std::vector<PlatoonMember> platoon_strategic_ihp::PlatoonManager::host_platoon_

Definition at line 361 of file platoon_manager_ihp.h.

Referenced by platoon_strategic_ihp::PlatoonStrategicIHPPlugin::PlatoonStrategicIHPPlugin(), allPredecessorFollowing(), changeFromFollowerToLeader(), changeFromLeaderToFollower(), getCommandSpeed(), getCurrentCrosstrackDistance(), getCurrentDowntrackDistance(), getCurrentPlatoonLength(), getCurrentSpeed(), getDynamicLeader(), getHostPlatoonSize(), getIHPDesPosFollower(), getPlatoonFrontDowntrackDistance(), getPlatoonRearDowntrackDistance(), getPredecessorPosition(), getPredecessorSpeed(), getPredecessorTimeHeadwaySum(), hostMemberUpdates(), insufficientGapWithPredecessor(), platoon_strategic_ihp::PlatoonStrategicIHPPlugin::isJoiningVehicleNearPlatoon(), platoon_strategic_ihp::PlatoonStrategicIHPPlugin::mob_req_cb_follower(), platoon_strategic_ihp::PlatoonStrategicIHPPlugin::mob_req_cb_leaderwaiting(), platoon_strategic_ihp::PlatoonStrategicIHPPlugin::mob_req_cb_leadwithoperation(), removeMember(), removeMemberById(), resetHostPlatoon(), updateHostPose(), updateHostSpeeds(), and platoon_strategic_ihp::PlatoonStrategicIHPPlugin::updatePlatoonList().

◆ HostMobilityId

std::string platoon_strategic_ihp::PlatoonManager::HostMobilityId = "default_host_id"

Definition at line 404 of file platoon_manager_ihp.h.

Referenced by platoon_strategic_ihp::PlatoonStrategicIHPPlugin::PlatoonStrategicIHPPlugin(), getHostStaticID(), platoon_strategic_ihp::PlatoonStrategicIHPPlugin::mob_req_cb_leader(), removeMemberById(), and updatesOrAddMemberInfo().

◆ hostPosInPlatoon_

size_t platoon_strategic_ihp::PlatoonManager::hostPosInPlatoon_ = 0

Definition at line 373 of file platoon_manager_ihp.h.

Referenced by allPredecessorFollowing(), changeFromFollowerToLeader(), changeFromLeaderToFollower(), getCommandSpeed(), getCurrentCrosstrackDistance(), getCurrentDowntrackDistance(), getCurrentSpeed(), getNumberOfVehicleInFront(), getPredecessorSpeed(), removeMember(), resetHostPlatoon(), updateHostPose(), updateHostSpeeds(), and updatesOrAddMemberInfo().

◆ is_neighbor_record_complete_

bool platoon_strategic_ihp::PlatoonManager::is_neighbor_record_complete_ = false

Definition at line 398 of file platoon_manager_ihp.h.

Referenced by platoon_strategic_ihp::PlatoonStrategicIHPPlugin::mob_op_cb_preparetojoin(), platoon_strategic_ihp::PlatoonStrategicIHPPlugin::mob_resp_cb_preparetojoin(), neighborMemberUpdates(), resetNeighborPlatoon(), and platoon_strategic_ihp::PlatoonStrategicIHPPlugin::run_prepare_to_join().

◆ isCreateGap

bool platoon_strategic_ihp::PlatoonManager::isCreateGap = false

Definition at line 407 of file platoon_manager_ihp.h.

Referenced by platoon_strategic_ihp::PlatoonStrategicIHPPlugin::composePlatoonInfoMsg(), findMaximumSpacingViolationClosestToTheHostVehicle(), platoon_strategic_ihp::PlatoonStrategicIHPPlugin::mob_req_cb_follower(), platoon_strategic_ihp::PlatoonStrategicIHPPlugin::mob_req_cb_leadwithoperation(), and resetHostPlatoon().

◆ isFollower

bool platoon_strategic_ihp::PlatoonManager::isFollower = false

Definition at line 379 of file platoon_manager_ihp.h.

Referenced by changeFromFollowerToLeader(), changeFromLeaderToFollower(), getDynamicLeader(), and hostMemberUpdates().

◆ JOIN_AT_REAR_PARAMS

std::string platoon_strategic_ihp::PlatoonManager::JOIN_AT_REAR_PARAMS = "SIZE:%1%,SPEED:%2%,DTD:%3%"

private

Definition at line 421 of file platoon_manager_ihp.h.

◆ JOIN_FROM_FRONT_PARAMS

std::string platoon_strategic_ihp::PlatoonManager::JOIN_FROM_FRONT_PARAMS = "SIZE:%1%,SPEED:%2%,DTD:%3%"

private

Definition at line 423 of file platoon_manager_ihp.h.

◆ maxCutinGap_

double platoon_strategic_ihp::PlatoonManager::maxCutinGap_ = 32.0

private

Definition at line 427 of file platoon_manager_ihp.h.

◆ minCutinGap_

double platoon_strategic_ihp::PlatoonManager::minCutinGap_ = 22.0

private

Definition at line 426 of file platoon_manager_ihp.h.

◆ MOBILITY_STRATEGY

std::string platoon_strategic_ihp::PlatoonManager::MOBILITY_STRATEGY = "Carma/Platooning"

private

Definition at line 424 of file platoon_manager_ihp.h.

◆ neighbor_platoon_

std::vector<PlatoonMember> platoon_strategic_ihp::PlatoonManager::neighbor_platoon_

Definition at line 384 of file platoon_manager_ihp.h.

Referenced by getClosestIndex(), getCutInGap(), platoon_strategic_ihp::PlatoonStrategicIHPPlugin::mob_op_cb_leader(), platoon_strategic_ihp::PlatoonStrategicIHPPlugin::mob_op_cb_preparetojoin(), neighborMemberUpdates(), resetNeighborPlatoon(), and platoon_strategic_ihp::PlatoonStrategicIHPPlugin::run_candidate_follower().

◆ neighbor_platoon_info_size_

size_t platoon_strategic_ihp::PlatoonManager::neighbor_platoon_info_size_ = 0

Definition at line 387 of file platoon_manager_ihp.h.

Referenced by platoon_strategic_ihp::PlatoonStrategicIHPPlugin::mob_op_cb(), neighborMemberUpdates(), and resetNeighborPlatoon().

◆ neighbor_platoon_leader_id_

std::string platoon_strategic_ihp::PlatoonManager::neighbor_platoon_leader_id_ = dummyID

Definition at line 395 of file platoon_manager_ihp.h.

Referenced by platoon_strategic_ihp::PlatoonStrategicIHPPlugin::mob_op_cb(), platoon_strategic_ihp::PlatoonStrategicIHPPlugin::mob_resp_cb_leader(), resetNeighborPlatoon(), and platoon_strategic_ihp::PlatoonStrategicIHPPlugin::run_prepare_to_join().

◆ neighborPlatoonID

std::string platoon_strategic_ihp::PlatoonManager::neighborPlatoonID = dummyID

Definition at line 392 of file platoon_manager_ihp.h.

Referenced by platoon_strategic_ihp::PlatoonStrategicIHPPlugin::mob_op_cb(), platoon_strategic_ihp::PlatoonStrategicIHPPlugin::mob_op_cb_STATUS(), neighborMemberUpdates(), and resetNeighborPlatoon().

◆ OPERATION_INFO_TYPE

std::string platoon_strategic_ihp::PlatoonManager::OPERATION_INFO_TYPE = "INFO"

private

Definition at line 419 of file platoon_manager_ihp.h.

◆ OPERATION_STATUS_TYPE

std::string platoon_strategic_ihp::PlatoonManager::OPERATION_STATUS_TYPE = "STATUS"

private

Definition at line 420 of file platoon_manager_ihp.h.

◆ platoonLeaderID

std::string platoon_strategic_ihp::PlatoonManager::platoonLeaderID = dummyID

Definition at line 367 of file platoon_manager_ihp.h.

Referenced by hostMemberUpdates(), platoon_strategic_ihp::PlatoonStrategicIHPPlugin::mob_req_cb_candidateleader(), platoon_strategic_ihp::PlatoonStrategicIHPPlugin::mob_req_cb_leader(), platoon_strategic_ihp::PlatoonStrategicIHPPlugin::mob_req_cb_leadwithoperation(), removeMember(), resetHostPlatoon(), platoon_strategic_ihp::PlatoonStrategicIHPPlugin::run_candidate_follower(), and platoon_strategic_ihp::PlatoonStrategicIHPPlugin::run_leader_aborting().

◆ pose_msg_

geometry_msgs::msg::PoseStamped platoon_strategic_ihp::PlatoonManager::pose_msg_

Definition at line 401 of file platoon_manager_ihp.h.

◆ previousFunctionalDynamicLeaderID_

std::string platoon_strategic_ihp::PlatoonManager::previousFunctionalDynamicLeaderID_ = ""

private

Definition at line 428 of file platoon_manager_ihp.h.

Referenced by allPredecessorFollowing(), changeFromFollowerToLeader(), getDynamicLeader(), and insufficientGapWithPredecessor().

◆ previousFunctionalDynamicLeaderIndex_

int platoon_strategic_ihp::PlatoonManager::previousFunctionalDynamicLeaderIndex_ = -1

private

Definition at line 429 of file platoon_manager_ihp.h.

Referenced by allPredecessorFollowing(), changeFromFollowerToLeader(), and getDynamicLeader().

◆ targetPlatoonID

std::string platoon_strategic_ihp::PlatoonManager::targetPlatoonID = dummyID

Definition at line 390 of file platoon_manager_ihp.h.

Referenced by clearActionPlan(), platoon_strategic_ihp::PlatoonStrategicIHPPlugin::mob_op_cb_leader(), platoon_strategic_ihp::PlatoonStrategicIHPPlugin::mob_op_cb_STATUS(), platoon_strategic_ihp::PlatoonStrategicIHPPlugin::mob_resp_cb_candidatefollower(), platoon_strategic_ihp::PlatoonStrategicIHPPlugin::mob_resp_cb_leader(), and platoon_strategic_ihp::PlatoonStrategicIHPPlugin::mob_resp_cb_preparetojoin().

◆ timer_factory_

std::shared_ptr<carma_ros2_utils::timers::TimerFactory> platoon_strategic_ihp::PlatoonManager::timer_factory_

Definition at line 412 of file platoon_manager_ihp.h.

Referenced by updatesOrAddMemberInfo().

◆ vehicleLength_

double platoon_strategic_ihp::PlatoonManager::vehicleLength_ = 5.0

private

Definition at line 433 of file platoon_manager_ihp.h.

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

platooning_strategic_IHP/include/platoon_strategic_ihp/platoon_manager_ihp.h
platooning_strategic_IHP/src/platoon_manager_ihp.cpp

Public Member Functions

Public Attributes

Private Member Functions

Private Attributes

Detailed Description

Constructor & Destructor Documentation

◆ PlatoonManager()

Member Function Documentation

◆ allPredecessorFollowing()

◆ calculateTimeHeadway()

◆ changeFromFollowerToLeader()

◆ changeFromLeaderToFollower()

◆ clearActionPlan()

◆ determineDynamicLeaderBasedOnViolation()

◆ findLowerBoundaryViolationClosestToTheHostVehicle()

◆ findMaximumSpacingViolationClosestToTheHostVehicle()

◆ getClosestIndex()

◆ getCommandSpeed()

◆ getCurrentCrosstrackDistance()

◆ getCurrentDowntrackDistance()

◆ getCurrentPlatoonLength()

◆ getCurrentSpeed()

◆ getCutInGap()

◆ getDistanceToPredVehicle()

◆ getDynamicLeader()

◆ getHostPlatoonSize()

◆ getHostStaticID()

◆ getIHPDesPosFollower()

◆ getNumberOfVehicleInFront()

◆ getPlatoonFrontDowntrackDistance()

◆ getPlatoonRearDowntrackDistance()

◆ getPredecessorPosition()

◆ getPredecessorSpeed()

◆ getPredecessorTimeHeadwaySum()

◆ getTimeHeadwayFromIndex()

◆ hostMemberUpdates()

◆ insufficientGapWithPredecessor()

◆ neighborMemberUpdates()

◆ removeMember()

◆ removeMemberById()

◆ resetHostPlatoon()

◆ resetNeighborPlatoon()

◆ updateHostPose()

◆ updateHostSpeeds()

◆ updatesOrAddMemberInfo()

Member Data Documentation

◆ algorithmType_

◆ config_

◆ current_plan

◆ current_platoon_state

◆ currentPlatoonID

◆ downtrack_progress_

◆ dummyID

◆ dynamic_leader_index_

◆ gapWithPred_

◆ host_platoon_

◆ HostMobilityId

◆ hostPosInPlatoon_

◆ is_neighbor_record_complete_

◆ isCreateGap

◆ isFollower

◆ JOIN_AT_REAR_PARAMS

◆ JOIN_FROM_FRONT_PARAMS

◆ maxCutinGap_

◆ minCutinGap_

◆ MOBILITY_STRATEGY

◆ neighbor_platoon_

◆ neighbor_platoon_info_size_

◆ neighbor_platoon_leader_id_

◆ neighborPlatoonID

◆ OPERATION_INFO_TYPE

◆ OPERATION_STATUS_TYPE

◆ platoonLeaderID

◆ pose_msg_

◆ previousFunctionalDynamicLeaderID_

◆ previousFunctionalDynamicLeaderIndex_

◆ targetPlatoonID

◆ timer_factory_

◆ vehicleLength_