Class containing the logic for Strategic IHP Plugin. This class is responsible for all the negotiations as well as generating a maneuver msg. More...

#include <platoon_strategic_ihp.h>

Collaboration diagram for platoon_strategic_ihp::PlatoonStrategicIHPPlugin:

Public Member Functions
	PlatoonStrategicIHPPlugin (carma_wm::WorldModelConstPtr wm, PlatoonPluginConfig config, MobilityResponseCB mobility_response_publisher, MobilityRequestCB mobility_request_publisher, MobilityOperationCB mobility_operation_publisher, PlatooningInfoCB platooning_info_publisher, std::shared_ptr< carma_ros2_utils::timers::TimerFactory > timer_factory)
	Constructor. More...

void	mob_op_cb (const carma_v2x_msgs::msg::MobilityOperation::UniquePtr msg)
	Callback function for Mobility Operation Message. More...

void	mob_req_cb (const carma_v2x_msgs::msg::MobilityRequest::UniquePtr msg)
	Callback function for Mobility Request Message. More...

void	mob_resp_cb (const carma_v2x_msgs::msg::MobilityResponse::UniquePtr msg)
	Callback function for Mobility Response Message. More...

MobilityRequestResponse	handle_mob_req (const carma_v2x_msgs::msg::MobilityRequest &msg)
	Function to the process and respond to the mobility request. More...

bool	plan_maneuver_cb (carma_planning_msgs::srv::PlanManeuvers::Request &req, carma_planning_msgs::srv::PlanManeuvers::Response &resp)
	Callback function to the maneuver request. More...

int	findLaneletIndexFromPath (int target_id, lanelet::routing::LaneletPath &path)
	Find lanelet index from path. More...

double	findLaneWidth ()
	Find lanelet width from local position. More...

carma_planning_msgs::msg::Maneuver	composeManeuverMessage (double current_dist, double end_dist, double current_speed, double target_speed, int lane_id, rclcpp::Time &current_time)
	Find lanelet index from path. More...

carma_planning_msgs::msg::Maneuver	composeLaneChangeManeuverMessage (double current_dist, double end_dist, double current_speed, double target_speed, int starting_lane_id, int ending_lane_id, rclcpp::Time &current_time)
	Find start(current) and target(end) lanelet index from path to generate lane change maneuver message. More...

void	updateCurrentStatus (carma_planning_msgs::msg::Maneuver maneuver, double &speed, double &current_progress, int &lane_id)
	Update maneuver status based on prior plan. More...

void	pose_cb (const geometry_msgs::msg::PoseStamped::UniquePtr msg)
	Callback function for current pose. More...

carma_planning_msgs::msg::PlatooningInfo	composePlatoonInfoMsg ()
	Compose Platoon information message. More...

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

void	cmd_cb (const geometry_msgs::msg::TwistStamped::UniquePtr msg)
	Callback for the control command. More...

void	georeference_cb (const std_msgs::msg::String::UniquePtr msg)
	Callback for the georeference. More...

bool	onSpin ()
	Spin callback function. More...

void	run_leader ()
	Run Leader State. More...

void	run_leader_waiting ()
	Run Leader Waiting State. More...

void	run_candidate_follower ()
	Run Candidate Follower State. More...

void	run_follower ()
	Run Follower State. More...

void	run_leader_aborting ()
	Run Leader Aborting State. More...

void	run_candidate_leader ()
	Run Candidate Leader State. More...

void	run_lead_with_operation ()
	UCLA Run lead with operation State. More...

void	run_prepare_to_join ()
	UCLA Run prepare to join State. More...

void	setHostECEF (carma_v2x_msgs::msg::LocationECEF pose_ecef_point)
	UCLA Update the private variable pose_ecef_point_. More...

PlatoonManager	getHostPM ()
	UCLA Getter: for PlatoonManager class. More...

void	setPMState (PlatoonState desiredState)
	UCLA Setter: function to set pm_.platoon_state. More...

void	updatePlatoonList (std::vector< PlatoonMember > platoon_list)
	UCLA Setter: Update platoon list (Unit Test). More...

void	setConfig (const PlatoonPluginConfig &config)
	Set the current config. More...

Public Attributes
carma_v2x_msgs::msg::LocationECEF	pose_ecef_point_

Private Member Functions
bool	isVehicleRightInFront (double downtrack, double crosstrack)
	Function to determine if a target vehicle is in the front of host vehicle note: This is only applicable for same lane application, so it will check cross track distance. More...

double	findSpeedLimit (const lanelet::ConstLanelet &llt)
	Function to find speed limit of a lanelet. More...

MobilityRequestResponse	mob_req_cb_leader (const carma_v2x_msgs::msg::MobilityRequest &msg)
	Function to process mobility request in leader state. More...

MobilityRequestResponse	mob_req_cb_leaderwaiting (const carma_v2x_msgs::msg::MobilityRequest &msg)
	Function to process mobility request in leader waiting state. More...

MobilityRequestResponse	mob_req_cb_follower (const carma_v2x_msgs::msg::MobilityRequest &msg)
	Function to process mobility request in follower state. More...

MobilityRequestResponse	mob_req_cb_candidatefollower (const carma_v2x_msgs::msg::MobilityRequest &msg)
	Function to process mobility request in candidate follower state. More...

MobilityRequestResponse	mob_req_cb_standby (const carma_v2x_msgs::msg::MobilityRequest &msg)
	Function to process mobility request in standby state. More...

void	mob_resp_cb_leader (const carma_v2x_msgs::msg::MobilityResponse &msg)
	Function to process mobility response in leader state. More...

void	mob_resp_cb_leaderwaiting (const carma_v2x_msgs::msg::MobilityResponse &msg)
	Function to process mobility response in leader waiting state. More...

void	mob_resp_cb_follower (const carma_v2x_msgs::msg::MobilityResponse &msg)
	Function to process mobility response in follower state. More...

void	mob_resp_cb_candidatefollower (const carma_v2x_msgs::msg::MobilityResponse &msg)
	Function to process mobility response in candidate follower state. More...

void	mob_resp_cb_standby (const carma_v2x_msgs::msg::MobilityResponse &msg)
	Function to process mobility response in standby state. More...

void	mob_op_cb_leader (const carma_v2x_msgs::msg::MobilityOperation &msg)
	Function to process mobility operation in leader state. More...

void	mob_op_cb_leaderwaiting (const carma_v2x_msgs::msg::MobilityOperation &msg)
	Function to process mobility operation in leader waiting state. More...

void	mob_op_cb_follower (const carma_v2x_msgs::msg::MobilityOperation &msg)
	Function to process mobility operation in follower state. More...

void	mob_op_cb_candidatefollower (const carma_v2x_msgs::msg::MobilityOperation &msg)
	Function to process mobility operation in candidate follower state. More...

void	mob_op_cb_standby (const carma_v2x_msgs::msg::MobilityOperation &msg)
	Function to process mobility operation in standby state. More...

carma_v2x_msgs::msg::MobilityOperation	composeMobilityOperationLeader (const std::string &type)
	Function to compose mobility operation in leader state. More...

carma_v2x_msgs::msg::MobilityOperation	composeMobilityOperationFollower ()
	Function to compose mobility operation in follower state. More...

carma_v2x_msgs::msg::MobilityOperation	composeMobilityOperationLeaderWaiting ()
	Function to compose mobility operation in leader waiting state. More...

carma_v2x_msgs::msg::MobilityOperation	composeMobilityOperationCandidateFollower ()
	Function to compose mobility operation in candidate follower state. More...

carma_v2x_msgs::msg::LocationECEF	pose_to_ecef (geometry_msgs::msg::PoseStamped pose_msg)
	Function to convert pose from map frame to ecef location. More...

lanelet::BasicPoint2d	ecef_to_map_point (carma_v2x_msgs::msg::LocationECEF ecef_point)
	Function to convert ecef location to a 2d point in map frame. More...

bool	isVehicleRightBehind (double downtrack, double crosstrack)
	Function to determine if the given downtrack distance (m) is behind the host vehicle. More...

bool	isJoiningVehicleNearPlatoon (double joining_downtrack, double joining_crosstrack)
	The method for platoon leader to determine if the joining vehicle is closeby. (Note: Used when host vehicle is the platoon leader) More...

bool	isVehicleNearTargetPlatoon (double rearVehicleDtd, double frontVehicleDtd, double frontVehicleCtd)
	Function to determine if the host vehicle is close to the target platoon (used for cut-in join scenarios). More...

int	find_target_lanelet_id (double start_downtrack, double end_downtrack)
	Function to find the starting and ending lanelet ID for lane change in a two-lane scenario (used for cut-in join scenarios). More...

carma_v2x_msgs::msg::MobilityOperation	composeMobilityOperationSTATUS ()
	Function to compose mobility operation message with STATUS params. More...

carma_v2x_msgs::msg::MobilityOperation	composeMobilityOperationINFO ()
	Function to compose mobility operation message with INFO params. More...

carma_v2x_msgs::msg::MobilityOperation	composeMobilityOperationLeaderAborting ()
	Function to compose mobility operation in LeaderAborting state. More...

carma_v2x_msgs::msg::MobilityOperation	composeMobilityOperationCandidateLeader ()
	Function to compose mobility operation in CandidateLeader. More...

carma_v2x_msgs::msg::MobilityOperation	composeMobilityOperationLeadWithOperation (const std::string &type)
	Function to compose mobility operation in LeadWithOperation (cut-in join) More...

carma_v2x_msgs::msg::MobilityOperation	composeMobilityOperationPrepareToJoin ()
	Function to compose mobility operation in PrepareToJoin (cut-in join) More...

void	mob_op_cb_STATUS (const carma_v2x_msgs::msg::MobilityOperation &msg)
	Function to process mobility operation message with STATUS params, read ecef location and update platoon member info. More...

double	mob_op_find_platoon_length_from_INFO_params (std::string strategyParams)
	Function to process mobility operation INFO params to find platoon length in m. More...

carma_v2x_msgs::msg::LocationECEF	mob_op_find_ecef_from_INFO_params (std::string strategyParams)
	Function to process mobility operation INFO params to find platoon leader's ecef location. More...

carma_v2x_msgs::msg::LocationECEF	mob_op_find_ecef_from_STATUS_params (std::string strategyParams)
	Function to process mobility operation for STATUS params. More...

void	mob_op_cb_leaderaborting (const carma_v2x_msgs::msg::MobilityOperation &msg)
	Function to process mobility operation in leaderaborting state. More...

void	mob_op_cb_candidateleader (const carma_v2x_msgs::msg::MobilityOperation &msg)
	Function to process mobility operation in candidateleader state. More...

void	mob_op_cb_leadwithoperation (const carma_v2x_msgs::msg::MobilityOperation &msg)
	Function to process mobility operation in LeadWithOperation state (cut-in join) More...

void	mob_op_cb_preparetojoin (const carma_v2x_msgs::msg::MobilityOperation &msg)
	Function to process mobility operation in PrepareToJoin state (cut-in join) More...

MobilityRequestResponse	mob_req_cb_leaderaborting (const carma_v2x_msgs::msg::MobilityRequest &msg)
	Function to process mobility request in leaderaborting state. More...

MobilityRequestResponse	mob_req_cb_candidateleader (const carma_v2x_msgs::msg::MobilityRequest &msg)
	Function to process mobility request in candidateleader state. More...

MobilityRequestResponse	mob_req_cb_leadwithoperation (const carma_v2x_msgs::msg::MobilityRequest &msg)
	Function to process mobility request in leadwithoperation state (cut-in join) More...

MobilityRequestResponse	mob_req_cb_preparetojoin (const carma_v2x_msgs::msg::MobilityRequest &msg)
	Function to process mobility request in preparetojoin state (cut-in join) More...

void	mob_resp_cb_leaderaborting (const carma_v2x_msgs::msg::MobilityResponse &msg)
	Function to process mobility response in leaderaborting state. More...

void	mob_resp_cb_candidateleader (const carma_v2x_msgs::msg::MobilityResponse &msg)
	Function to process mobility response in candidateleader state. More...

void	mob_resp_cb_leadwithoperation (const carma_v2x_msgs::msg::MobilityResponse &msg)
	Function to process mobility response in leadwithoperation state (cut-in join) More...

void	mob_resp_cb_preparetojoin (const carma_v2x_msgs::msg::MobilityResponse &msg)
	Function to process mobility response in preparetojoin state. More...

bool	is_lanechange_possible (lanelet::Id start_lanelet_id, lanelet::Id target_lanelet_id)
	Function to check if lanechange is possible. More...

	FRIEND_TEST (PlatoonStrategicIHPPlugin, platoon_info_pub_front)

	FRIEND_TEST (PlatoonStrategicIHPPlugin, is_lanechange_possible)

Private Attributes
MobilityRequestCB	mobility_request_publisher_

MobilityResponseCB	mobility_response_publisher_

MobilityOperationCB	mobility_operation_publisher_

PlatooningInfoCB	platooning_info_publisher_

carma_wm::WorldModelConstPtr	wm_

PlatoonPluginConfig	config_

geometry_msgs::msg::PoseStamped	pose_msg_

double	cmd_speed_ = 0

double	current_speed_ = 0

double	current_downtrack_ = 0

double	current_crosstrack_ = 0

long	waitingStartTime = 0

long	candidatestateStartTime = 0

std::string	HostMobilityId = "hostid"

std::string	georeference_ {""}

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

bool	map_loaded_ = false

carma_planning_msgs::msg::Plugin	plugin_discovery_msg_

int	numLeaderAbortingCalls_ = 0

int	candidate_follower_delay_count_ = 0

double	maxAllowedJoinTimeGap_ = 15.0

double	maxAllowedJoinGap_ = 90

int	maxPlatoonSize_ = 10

double	vehicleLength_ = 5.0

unsigned long	infoMessageInterval_ = 200

unsigned long	prevHeartBeatTime_ = 0.0

int	statusMessageInterval_ = 100

unsigned long	NEGOTIATION_TIMEOUT = 15000

unsigned long	LANE_CHANGE_TIMEOUT = 300000

int	noLeaderUpdatesCounter = 0

int	LEADER_TIMEOUT_COUNTER_LIMIT = 5

double	waitingStateTimeout = 25.0

double	desiredJoinGap = 30.0

double	desiredJoinTimeGap = 4.0

lanelet::BasicPoint2d	target_cutin_pose_

const double	STOPPED_SPEED = 0.5

int	target_join_index_ = -2

bool	safeToLaneChange_ = false

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

PlatoonManager	pm_

const std::string	PLATOONING_STRATEGY = "Carma/Platooning"

const std::string	OPERATION_INFO_TYPE = "INFO"

const std::string	OPERATION_STATUS_TYPE = "STATUS"

const std::string	OPERATION_INFO_PARAMS = "INFO\|LENGTH:%.2f,SPEED:%.2f,SIZE:%d,ECEFX:%.2f,ECEFY:%.2f,ECEFZ:%.2f"

const std::string	OPERATION_STATUS_PARAMS = "STATUS\|CMDSPEED:%1%,SPEED:%2%,ECEFX:%3%,ECEFY:%4%,ECEFZ:%5%"

const std::string	JOIN_PARAMS = "SIZE:%1%,SPEED:%2%,ECEFX:%3%,ECEFY:%4%,ECEFZ:%5%,JOINIDX:%6%"

Detailed Description

Class containing the logic for Strategic IHP Plugin. This class is responsible for all the negotiations as well as generating a maneuver msg.

Definition at line 69 of file platoon_strategic_ihp.h.

Constructor & Destructor Documentation

◆ PlatoonStrategicIHPPlugin()

platoon_strategic_ihp::PlatoonStrategicIHPPlugin::PlatoonStrategicIHPPlugin	(	carma_wm::WorldModelConstPtr	wm,
		PlatoonPluginConfig	config,
		MobilityResponseCB	mobility_response_publisher,
		MobilityRequestCB	mobility_request_publisher,
		MobilityOperationCB	mobility_operation_publisher,
		PlatooningInfoCB	platooning_info_publisher,
		std::shared_ptr< carma_ros2_utils::timers::TimerFactory >	timer_factory
	)

Constructor.

Parameters

wm	Pointer to initalized instance of the carma world model for accessing semantic map data
config	The configuration to be used for this object

Definition at line 36 of file platoon_strategic_ihp.cpp.

   : mobility_request_publisher_(mobility_request_publisher),
      mobility_response_publisher_(mobility_response_publisher), mobility_operation_publisher_(mobility_operation_publisher),
      platooning_info_publisher_(platooning_info_publisher), wm_(wm), config_(config), timer_factory_(timer_factory), pm_(timer_factory_)
    {
        RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "Top of PlatoonStrategicIHP ctor.");
        std::string hostStaticId = config_.vehicleID; //static ID for this vehicle
        pm_.HostMobilityId = hostStaticId;
        // construct platoon member for host vehicle as the first element in the vector, since it starts life as a solo vehicle
        long cur_t = timer_factory_->now().nanoseconds()/1000000; // time in millisecond
        PlatoonMember hostVehicleMember = PlatoonMember(hostStaticId, 0.0, 0.0, 0.0, 0.0, cur_t);
        pm_.host_platoon_.push_back(hostVehicleMember);
        plugin_discovery_msg_.name = "platoon_strategic_ihp";
        plugin_discovery_msg_.version_id = "v1.0";
        plugin_discovery_msg_.available = true;
        plugin_discovery_msg_.activated = true;
        plugin_discovery_msg_.type = carma_planning_msgs::msg::Plugin::STRATEGIC;
        plugin_discovery_msg_.capability = "strategic_plan/plan_maneuvers";
        RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "ctor complete. hostStaticId = " << hostStaticId);
    }

References config_, platoon_strategic_ihp::PlatoonManager::host_platoon_, platoon_strategic_ihp::PlatoonManager::HostMobilityId, plugin_discovery_msg_, pm_, timer_factory_, and platoon_strategic_ihp::PlatoonPluginConfig::vehicleID.

Member Function Documentation

◆ cmd_cb()

void platoon_strategic_ihp::PlatoonStrategicIHPPlugin::cmd_cb ( const geometry_msgs::msg::TwistStamped::UniquePtr msg )

Callback for the control command.

Parameters

msg	Latest twist cmd message

Definition at line 137 of file platoon_strategic_ihp.cpp.

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

References cmd_speed_.

Referenced by platoon_strategic_ihp::Node::on_configure_plugin().

Here is the caller graph for this function:

◆ composeLaneChangeManeuverMessage()

carma_planning_msgs::msg::Maneuver platoon_strategic_ihp::PlatoonStrategicIHPPlugin::composeLaneChangeManeuverMessage	(	double	current_dist,
		double	end_dist,
		double	current_speed,
		double	target_speed,
		int	starting_lane_id,
		int	ending_lane_id,
		rclcpp::Time &	current_time
	)

Find start(current) and target(end) lanelet index from path to generate lane change maneuver message.

Parameters

current_dist	current downtrack distance
end_dist	ending downtrack distance
current_speed	current speed
target_speed	target speed
starting_lane_id	current lanelet id which serves as the starting lanlet id
ending_lane_id	target lanelet id which is also the ending lanelet id that is in another lane
current_time	current time in seconds

Returns: Maneuver message

Definition at line 3165 of file platoon_strategic_ihp.cpp.

    {
        carma_planning_msgs::msg::Maneuver maneuver_msg;
        // UCLA: change to lane change maneuvers
        maneuver_msg.type = carma_planning_msgs::msg::Maneuver::LANE_CHANGE;
        maneuver_msg.lane_change_maneuver.parameters.negotiation_type = carma_planning_msgs::msg::ManeuverParameters::PLATOONING;
        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 = "cooperative_lanechange";
        maneuver_msg.lane_change_maneuver.parameters.planning_strategic_plugin = "platoon_strategic_ihp";
        maneuver_msg.lane_change_maneuver.start_dist = current_dist;
        maneuver_msg.lane_change_maneuver.start_speed = current_speed;
        maneuver_msg.lane_change_maneuver.start_time = current_time;
        maneuver_msg.lane_change_maneuver.end_dist = end_dist;
        maneuver_msg.lane_change_maneuver.end_speed = target_speed;
        // Generate a new maneuver ID for the lane change maneuver (not needed for lane following maneuver).
        maneuver_msg.lane_following_maneuver.parameters.maneuver_id = boost::uuids::to_string(boost::uuids::random_generator()());
 
        // because it is a rough plan, assume vehicle can always reach to the target speed in a lanelet
        double cur_plus_target = current_speed + target_speed;
        if (cur_plus_target < 0.00001)
        {
            maneuver_msg.lane_change_maneuver.end_time = current_time + rclcpp::Duration(config_.time_step*1e9);
        }
        else
        {
            // maneuver_msg.lane_change_maneuver.end_time = current_time + rclcpp::Duration((end_dist - current_dist) / (0.5 * cur_plus_target));
            maneuver_msg.lane_change_maneuver.end_time = current_time + rclcpp::Duration(20.0*1e9);
 
        }
 
        // UCLA: need both start laneID and end laneID  for lane change
        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) };
 
        current_time = maneuver_msg.lane_change_maneuver.end_time;
        RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "Creating lane change start dist:"<<current_dist<<" end dist:"<<end_dist);
        RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "Duration:"<< rclcpp::Time(maneuver_msg.lane_change_maneuver.end_time).seconds() - rclcpp::Time(maneuver_msg.lane_change_maneuver.start_time).seconds());
        return maneuver_msg;
    }

References config_, platoon_strategic_ihp::PlatoonPluginConfig::time_step, and carma_cooperative_perception::to_string().

Referenced by plan_maneuver_cb().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ composeManeuverMessage()

carma_planning_msgs::msg::Maneuver platoon_strategic_ihp::PlatoonStrategicIHPPlugin::composeManeuverMessage	(	double	current_dist,
		double	end_dist,
		double	current_speed,
		double	target_speed,
		int	lane_id,
		rclcpp::Time &	current_time
	)

Find lanelet index from path.

Parameters

current_dist	current downtrack distance (m)
end_dist	ending downtrack distance (m)
current_speed	current speed (m/s)
target_speed	target speed (m/s)
lane_id	lanelet id
current_time	current time (s)

Returns: Maneuver message

Definition at line 3125 of file platoon_strategic_ihp.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::PLATOONING;
        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 = "platooning_tactical_plugin";
        maneuver_msg.lane_following_maneuver.parameters.planning_strategic_plugin = "platoon_strategic_ihp";
        maneuver_msg.lane_following_maneuver.start_dist = current_dist;
        maneuver_msg.lane_following_maneuver.start_speed = current_speed;
        maneuver_msg.lane_following_maneuver.start_time = current_time;
        maneuver_msg.lane_following_maneuver.end_dist = end_dist;
        maneuver_msg.lane_following_maneuver.end_speed = target_speed;
 
        // because it is a rough plan, assume vehicle can always reach to the target speed in a lanelet
        maneuver_msg.lane_following_maneuver.end_time = current_time + rclcpp::Duration(config_.time_step*1e9);
        maneuver_msg.lane_following_maneuver.lane_ids = { std::to_string(lane_id) };
 
        RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "in compose maneuver lane id:"<< lane_id);
 
        lanelet::ConstLanelet current_lanelet = wm_->getMap()->laneletLayer.get(lane_id);
        if(!wm_->getMapRoutingGraph()->following(current_lanelet, false).empty())
        {
 
            auto next_lanelet_id = wm_->getMapRoutingGraph()->following(current_lanelet, false).front().id();
            RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "next_lanelet_id:"<< next_lanelet_id);
            maneuver_msg.lane_following_maneuver.lane_ids.push_back(std::to_string(next_lanelet_id));
        }
        else
        {
            RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "No following lanelets");
        }
 
        current_time = maneuver_msg.lane_following_maneuver.end_time;
        RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "Creating lane follow start dist:"<<current_dist<<" end dist:"<<end_dist);
        RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "Duration:"<< rclcpp::Time(maneuver_msg.lane_following_maneuver.end_time).seconds() - rclcpp::Time(maneuver_msg.lane_following_maneuver.start_time).seconds());
        return maneuver_msg;
    }

References config_, platoon_strategic_ihp::PlatoonPluginConfig::time_step, carma_cooperative_perception::to_string(), and wm_.

Referenced by plan_maneuver_cb().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ composeMobilityOperationCandidateFollower()

carma_v2x_msgs::msg::MobilityOperation platoon_strategic_ihp::PlatoonStrategicIHPPlugin::composeMobilityOperationCandidateFollower ( )

private

Function to compose mobility operation in candidate follower state.

Returns: mobility operation msg

Definition at line 604 of file platoon_strategic_ihp.cpp.

    {
        carma_v2x_msgs::msg::MobilityOperation msg;
        msg = composeMobilityOperationSTATUS();
        return msg;
    }

References composeMobilityOperationSTATUS().

Referenced by run_candidate_follower().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ composeMobilityOperationCandidateLeader()

carma_v2x_msgs::msg::MobilityOperation platoon_strategic_ihp::PlatoonStrategicIHPPlugin::composeMobilityOperationCandidateLeader ( )

private

Function to compose mobility operation in CandidateLeader.

Returns: mobility operation msg.

Definition at line 624 of file platoon_strategic_ihp.cpp.

    {
        /*
            UCLA Implementation note:
            This is the joiner which will later become the new leader,
            host vehicle publish status msgs and waiting to lead the rear platoon
        */
        carma_v2x_msgs::msg::MobilityOperation msg;
        msg = composeMobilityOperationSTATUS();
        return msg;
    }

References composeMobilityOperationSTATUS().

Referenced by run_candidate_leader().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ composeMobilityOperationFollower()

carma_v2x_msgs::msg::MobilityOperation platoon_strategic_ihp::PlatoonStrategicIHPPlugin::composeMobilityOperationFollower ( )

private

Function to compose mobility operation in follower state.

Returns: mobility operation msg

Definition at line 586 of file platoon_strategic_ihp.cpp.

    {
        carma_v2x_msgs::msg::MobilityOperation msg;
        msg = composeMobilityOperationSTATUS();
        return msg;
    }

References composeMobilityOperationSTATUS().

Referenced by run_follower().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ composeMobilityOperationINFO()

carma_v2x_msgs::msg::MobilityOperation platoon_strategic_ihp::PlatoonStrategicIHPPlugin::composeMobilityOperationINFO ( )

private

Function to compose mobility operation message with INFO params.

Returns: mobility operation msg.

Note: INFO param format: "INFO| --> LENGTH:%.2f,SPEED:%.2f,SIZE:%d,ECEFX:%.2f,ECEFY:%.2f,ECEFZ:%.2f" |----—0--------—1------—2-----—3-------—4-------—5----—|

Definition at line 317 of file platoon_strategic_ihp.cpp.

    {
        carma_v2x_msgs::msg::MobilityOperation msg;
        msg.m_header.plan_id = pm_.currentPlatoonID; // msg.m_header.plan_id is the platoon ID of the request sender (rear join and frontal join).
        msg.m_header.recipient_id = "";
        std::string hostStaticId = config_.vehicleID;
        msg.m_header.sender_id = hostStaticId;
        msg.m_header.timestamp = timer_factory_->now().nanoseconds() / 1000000;;
        msg.strategy = PLATOONING_STRATEGY;
 
        double CurrentPlatoonLength = pm_.getCurrentPlatoonLength();
        int PlatoonSize = pm_.getHostPlatoonSize();
 
        boost::format fmter(OPERATION_INFO_PARAMS);
        //  Note: need to update "OPERATION_INFO_PARAMS" in m_header file --> strategic_platoon_ihp.h
        fmter% CurrentPlatoonLength;            // index = 0, physical length of the platoon, in m.
        fmter% current_speed_;                  // index = 1, in m/s.
        fmter% PlatoonSize;                     // index = 2, number of members
        fmter% pose_ecef_point_.ecef_x;         // index = 3, in cm.
        fmter% pose_ecef_point_.ecef_y;         // index = 4, in cm.
        fmter% pose_ecef_point_.ecef_z;         // index = 5, in cm.
 
        std::string infoParams = fmter.str();
        msg.strategy_params = infoParams;
        RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "Composed a mobility operation message with params " << msg.strategy_params);
        return msg;
    }

References config_, current_speed_, platoon_strategic_ihp::PlatoonManager::currentPlatoonID, platoon_strategic_ihp::PlatoonManager::getCurrentPlatoonLength(), platoon_strategic_ihp::PlatoonManager::getHostPlatoonSize(), OPERATION_INFO_PARAMS, PLATOONING_STRATEGY, pm_, pose_ecef_point_, timer_factory_, and platoon_strategic_ihp::PlatoonPluginConfig::vehicleID.

Referenced by composeMobilityOperationLeader(), and composeMobilityOperationLeadWithOperation().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ composeMobilityOperationLeader()

carma_v2x_msgs::msg::MobilityOperation platoon_strategic_ihp::PlatoonStrategicIHPPlugin::composeMobilityOperationLeader ( const std::string & type )

private

Function to compose mobility operation in leader state.

Parameters

type	type of mobility operation (info or status)

Returns: mobility operation msg

Definition at line 561 of file platoon_strategic_ihp.cpp.

    {
        carma_v2x_msgs::msg::MobilityOperation msg;
 
        // info params
        if (type == OPERATION_INFO_TYPE)
        {
            msg = composeMobilityOperationINFO();
        }
 
        // status params
        else if (type == OPERATION_STATUS_TYPE)
        {
            msg = composeMobilityOperationSTATUS();
        }
        // Unknown strategy param.
        else
        {
            RCLCPP_ERROR(rclcpp::get_logger("platoon_strategic_ihp"),"UNKNOWN strategy param string!!!");
            msg.strategy_params = "";
        }
        return msg;
    }

References composeMobilityOperationINFO(), composeMobilityOperationSTATUS(), OPERATION_INFO_TYPE, and OPERATION_STATUS_TYPE.

Referenced by run_leader().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ composeMobilityOperationLeaderAborting()

carma_v2x_msgs::msg::MobilityOperation platoon_strategic_ihp::PlatoonStrategicIHPPlugin::composeMobilityOperationLeaderAborting ( )

private

Function to compose mobility operation in LeaderAborting state.

Returns: mobility operation msg.

Definition at line 612 of file platoon_strategic_ihp.cpp.

    {
        /*
            UCLA Implementation note:
            Sending STATUS info for member updates and platoon trajectory regulation.
        */
        carma_v2x_msgs::msg::MobilityOperation msg;
        msg = composeMobilityOperationSTATUS();
        return msg;
    }

References composeMobilityOperationSTATUS().

Referenced by run_leader_aborting().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ composeMobilityOperationLeaderWaiting()

carma_v2x_msgs::msg::MobilityOperation platoon_strategic_ihp::PlatoonStrategicIHPPlugin::composeMobilityOperationLeaderWaiting ( )

private

Function to compose mobility operation in leader waiting state.

Returns: mobility operation msg

Definition at line 594 of file platoon_strategic_ihp.cpp.

    {
        //TODO: shouldn't a leaderwaiting also be sending INFO messages since it is still leading?
 
        carma_v2x_msgs::msg::MobilityOperation msg;
        msg = composeMobilityOperationSTATUS();
        return msg;
    }

References composeMobilityOperationSTATUS().

Referenced by run_leader_waiting().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ composeMobilityOperationLeadWithOperation()

carma_v2x_msgs::msg::MobilityOperation platoon_strategic_ihp::PlatoonStrategicIHPPlugin::composeMobilityOperationLeadWithOperation ( const std::string & type )

private

Function to compose mobility operation in LeadWithOperation (cut-in join)

Parameters

type	type of mobility operation (info or status)

Returns: msg: mobility operation msg Return null ("") if the incoming message is trashed/unrecognized.

Definition at line 649 of file platoon_strategic_ihp.cpp.

    {
        carma_v2x_msgs::msg::MobilityOperation msg;
 
        // info params
        if (type == OPERATION_INFO_TYPE)
        {
            msg = composeMobilityOperationINFO();
        }
 
        // status params
        else if (type == OPERATION_STATUS_TYPE)
        {
            msg = composeMobilityOperationSTATUS();
        }
        // Unknown strategy param.
        else
        {
            RCLCPP_ERROR(rclcpp::get_logger("platoon_strategic_ihp"),"UNKNOWN strategy param string!!!");
            msg.strategy_params = "";
        }
        return msg;
    }

References composeMobilityOperationINFO(), composeMobilityOperationSTATUS(), OPERATION_INFO_TYPE, and OPERATION_STATUS_TYPE.

Referenced by run_lead_with_operation().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ composeMobilityOperationPrepareToJoin()

carma_v2x_msgs::msg::MobilityOperation platoon_strategic_ihp::PlatoonStrategicIHPPlugin::composeMobilityOperationPrepareToJoin ( )

private

Function to compose mobility operation in PrepareToJoin (cut-in join)

Parameters

type	type of mobility operation (info or status)

Returns: msg: mobility operation msg Return null ("") if the incoming message is trashed/unrecognized.

Definition at line 637 of file platoon_strategic_ihp.cpp.

    {
        /*
            UCLA Implementation note:
            This is the joiner that is preapring for cut-in join.
            host vehicle publish status msgs and waiting to lead the rear platoon.
        */
        carma_v2x_msgs::msg::MobilityOperation msg;
        msg = composeMobilityOperationSTATUS();
        return msg;
    }

References composeMobilityOperationSTATUS().

Referenced by run_prepare_to_join().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ composeMobilityOperationSTATUS()

carma_v2x_msgs::msg::MobilityOperation platoon_strategic_ihp::PlatoonStrategicIHPPlugin::composeMobilityOperationSTATUS ( )

private

Function to compose mobility operation message with STATUS params.

Returns: mobility operation msg.

Note: STATUS params format: STATUS | --> "CMDSPEED:%1%,SPEED:%2%,ECEFX:%3%,ECEFY:%4%,ECEFZ:%5%" |-------—0-------—1------—2------—3------—4---—|

Definition at line 280 of file platoon_strategic_ihp.cpp.

    {
        //TODO future: consider setting recipient_id to the platoon ID to make it obvious that anyone in that group is intended reader.
        //             This requires an architectural agreement on use of group messaging protocol.
 
        // Extract data
        carma_v2x_msgs::msg::MobilityOperation msg;
        msg.m_header.plan_id = pm_.currentPlatoonID;
        msg.m_header.recipient_id = "";
        std::string hostStaticId = config_.vehicleID;
        msg.m_header.sender_id = hostStaticId;
        msg.m_header.timestamp = timer_factory_->now().nanoseconds() / 1000000;
        msg.strategy = PLATOONING_STRATEGY;
 
        // form message
        double cmdSpeed = cmd_speed_;
        boost::format fmter(OPERATION_STATUS_PARAMS);
        fmter% cmdSpeed;                // index = 0, in m/s.
        fmter% current_speed_;          // index = 1, in m/s.
        fmter% pose_ecef_point_.ecef_x; // index = 2, in cm.
        fmter% pose_ecef_point_.ecef_y; // index = 3, in cm.
        fmter% pose_ecef_point_.ecef_z; // index = 4, in cm.
 
        // compose message
        std::string statusParams = fmter.str();
        msg.strategy_params = statusParams;
        RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "Composed a mobility operation message with params " << msg.strategy_params);
        return msg;
    }

References cmd_speed_, config_, current_speed_, platoon_strategic_ihp::PlatoonManager::currentPlatoonID, OPERATION_STATUS_PARAMS, PLATOONING_STRATEGY, pm_, pose_ecef_point_, timer_factory_, and platoon_strategic_ihp::PlatoonPluginConfig::vehicleID.

Referenced by composeMobilityOperationCandidateFollower(), composeMobilityOperationCandidateLeader(), composeMobilityOperationFollower(), composeMobilityOperationLeader(), composeMobilityOperationLeaderAborting(), composeMobilityOperationLeaderWaiting(), composeMobilityOperationLeadWithOperation(), and composeMobilityOperationPrepareToJoin().

Here is the caller graph for this function:

◆ composePlatoonInfoMsg()

carma_planning_msgs::msg::PlatooningInfo platoon_strategic_ihp::PlatoonStrategicIHPPlugin::composePlatoonInfoMsg ( )

Compose Platoon information message.

Returns: PlatooningInfo msg

Note: There is a difference between the "platoon info status" versus the the "platoon strategic plugin states".

  [platooning info status]
  The "platooning info status" reflect the overall operating status. The status can
  include vehicles in different "platoon strategic plugin states" as long as the current state is relavent
  to the genral status.

  [platoon strategic plugin states]
  The "platoon strategic plugin states" manage the negotiation strategies and
  vehicle communication in a more refined manner that can achieve the "platooning info status'" objective. Hence,
  vehicles in different states will behave differently based on the corresponding roles and predefined rules.
  However, they can belong to the same "platooning info statu" for the same operation purpose.

  [Example]
  Multiple strategic states, such as "leader aborting" and "candidate follower", can both be mapped to
  "connecting to new leader" platooning info status, as both states are serving the same purpose of connecting
  to the leader.

Note: There is a difference between the "platoon info status" versus the the "platoon strategic plugin states". The "platooning info status" reflect the overall operating status. The "platoon strategic plugin states" manage the negotiation strategies and vehicle communication in a more refined manner. A more detailed note can be found in the corresponding function declaration in "platoon_strategic_ihp.h" file.

Definition at line 422 of file platoon_strategic_ihp.cpp.

    {
        carma_planning_msgs::msg::PlatooningInfo status_msg;
 
        if (pm_.current_platoon_state == PlatoonState::STANDBY)
        {
            status_msg.state = carma_planning_msgs::msg::PlatooningInfo::DISABLED;
        }
        else if (pm_.current_platoon_state == PlatoonState::LEADER)
        {
            status_msg.state = pm_.getHostPlatoonSize() == 1 ? carma_planning_msgs::msg::PlatooningInfo::SEARCHING : carma_planning_msgs::msg::PlatooningInfo::LEADING;
        }
        else if (pm_.current_platoon_state == PlatoonState::LEADERWAITING)
        {
            status_msg.state = carma_planning_msgs::msg::PlatooningInfo::CONNECTING_TO_NEW_FOLLOWER;
        }
        else if (pm_.current_platoon_state == PlatoonState::CANDIDATEFOLLOWER)
        {
            status_msg.state = carma_planning_msgs::msg::PlatooningInfo::CONNECTING_TO_NEW_LEADER;
        }
        else if (pm_.current_platoon_state == PlatoonState::FOLLOWER)
        {
            status_msg.state = carma_planning_msgs::msg::PlatooningInfo::FOLLOWING;
        }
        // UCLA: add leader aborting for frontal join (inherited from candidate follower).
        else if (pm_.current_platoon_state == PlatoonState::LEADERABORTING)
        {
            status_msg.state = carma_planning_msgs::msg::PlatooningInfo::CONNECTING_TO_NEW_LEADER;
        }
        // UCLA: add candidate leader for frontal join (inherited from leader waiting).
        else if (pm_.current_platoon_state == PlatoonState::CANDIDATELEADER)
        {
            status_msg.state = carma_planning_msgs::msg::PlatooningInfo::CONNECTING_TO_NEW_FOLLOWER;
        }
        // UCLA: add "lead with operation" for frontal join (inherited from leader waiting).
        else if (pm_.current_platoon_state == PlatoonState::LEADWITHOPERATION)
        {
            status_msg.state = carma_planning_msgs::msg::PlatooningInfo::CONNECTING_TO_NEW_FOLLOWER;
        }
        // UCLA: add "prepare to join" for frontal join (inherited from leader waiting).
        else if (pm_.current_platoon_state == PlatoonState::PREPARETOJOIN)
        {
            status_msg.state = carma_planning_msgs::msg::PlatooningInfo::CONNECTING_TO_NEW_LEADER;
        }
        //TODO: Place holder for departure (PREPARE TO DEPART)
 
        // compose msgs for anything other than standby
        if (pm_.current_platoon_state != PlatoonState::STANDBY)
        {
            status_msg.platoon_id = pm_.currentPlatoonID;
            status_msg.size = pm_.getHostPlatoonSize();
            status_msg.size_limit = config_.maxPlatoonSize;
 
            if (pm_.current_platoon_state == PlatoonState::FOLLOWER)
            {
                PlatoonMember platoon_leader = pm_.getDynamicLeader();
                status_msg.leader_id = platoon_leader.staticId;
                status_msg.leader_downtrack_distance = platoon_leader.vehiclePosition;
                status_msg.leader_cmd_speed = platoon_leader.commandSpeed;
                status_msg.host_platoon_position = pm_.getNumberOfVehicleInFront();
                RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "pm platoonsize: " << pm_.getHostPlatoonSize() << ", platoon_leader " << platoon_leader.staticId);
 
                int numOfVehiclesGaps = pm_.getNumberOfVehicleInFront() - pm_.dynamic_leader_index_;
                RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "The host vehicle have " << numOfVehiclesGaps << " vehicles between itself and its leader (includes the leader)");
 
                // use current position to find lanelet ID
                lanelet::BasicPoint2d current_loc(pose_msg_.pose.position.x, pose_msg_.pose.position.y);
 
                auto llts = wm_->getLaneletsFromPoint(current_loc, 1);
 
                double lanelet_digitalgap = config_.standStillHeadway;
 
                if (!llts.empty())
                {
                    auto geofence_gaps = llts[0].regulatoryElementsAs<lanelet::DigitalMinimumGap>();
 
                    if (!geofence_gaps.empty())
                    {
                        lanelet_digitalgap = geofence_gaps[0]->getMinimumGap();
                    }
                }
 
                else
                {
                    RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "No lanelets in this location!!!: ");
                }
 
                RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "lanelet_digitalgap: " << lanelet_digitalgap);
                double desired_headway = std::max(current_speed_ * config_.timeHeadway, lanelet_digitalgap);
                RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "speed based gap: " << current_speed_ * config_.timeHeadway);
                RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "max desired_headway " << desired_headway);
                // TODO: currently the average length of the vehicle is obtained from a config parameter. In future, plugin needs to be updated to receive each vehicle's actual length through status or BSM messages for more accuracy.
                status_msg.desired_gap = std::max(config_.standStillHeadway * numOfVehiclesGaps, desired_headway * numOfVehiclesGaps) + (numOfVehiclesGaps - 1) * 5.0;//config_.averageVehicleLength;
                RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "The desired gap with the leader is " << status_msg.desired_gap);
 
 
                // TODO: To uncomment the following lines, platooninfo msg must be updated
                // UCLA: Add the value of the summation of "veh_len/veh_speed" for all predecessors
                status_msg.current_predecessor_time_headway_sum = pm_.getPredecessorTimeHeadwaySum();
                // UCLA: preceding vehicle info
                status_msg.predecessor_speed = pm_.getPredecessorSpeed();
                status_msg.predecessor_position = pm_.getPredecessorPosition();
 
                // Note: use isCreateGap to adjust the desired gap send to control plugin
                double regular_gap = status_msg.desired_gap;
                RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "regular_gap: " << regular_gap);
                RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "current_speed_: " << current_speed_);
                RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "speed based gap: " << desired_headway);
                if (pm_.isCreateGap){
                    // enlarged desired gap for gap creation
                    status_msg.desired_gap = regular_gap*(1 + config_.createGapAdjuster);
                }
                status_msg.actual_gap = platoon_leader.vehiclePosition - current_downtrack_;
                RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "status_msg.actual_gap: " << status_msg.actual_gap);
            }
            else
            {
                status_msg.leader_id = config_.vehicleID;
                status_msg.leader_downtrack_distance = current_downtrack_;
                status_msg.leader_cmd_speed = cmd_speed_;
                status_msg.host_platoon_position = 0;
 
            }
 
            // This info is updated at platoon control plugin
            status_msg.host_cmd_speed = cmd_speed_;
 
        }
        return status_msg;
    }

Referenced by onSpin().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ ecef_to_map_point()

lanelet::BasicPoint2d platoon_strategic_ihp::PlatoonStrategicIHPPlugin::ecef_to_map_point ( carma_v2x_msgs::msg::LocationECEF ecef_point )

private

Function to convert ecef location to a 2d point in map frame.

Parameters

ecef_point ecef location point

Returns: 2d point in map frame

Definition at line 252 of file platoon_strategic_ihp.cpp.

    {
        if (!map_projector_)
        {
            throw std::invalid_argument("No map projector available for ecef conversion");
        }
 
        lanelet::BasicPoint3d map_point = map_projector_->projectECEF( { (double)ecef_point.ecef_x/100.0, (double)ecef_point.ecef_y/100.0, (double)ecef_point.ecef_z/100.0 } , -1);
 
        lanelet::BasicPoint2d output {map_point.x(), map_point.y()};
        return output;
    }

References map_projector_.

Referenced by mob_op_cb_leader(), mob_op_cb_preparetojoin(), mob_op_cb_STATUS(), mob_req_cb_candidateleader(), mob_req_cb_leader(), mob_req_cb_leaderwaiting(), and mob_req_cb_leadwithoperation().

Here is the caller graph for this function:

◆ find_target_lanelet_id()

int platoon_strategic_ihp::PlatoonStrategicIHPPlugin::find_target_lanelet_id	(	double	start_downtrack,
		double	end_downtrack
	)

private

Function to find the starting and ending lanelet ID for lane change in a two-lane scenario (used for cut-in join scenarios).

Note: This is a temporary function for internal test only. The scenario is not generalized. Can only find adjacent lanletID based on predefined direction (left, right).

\TODO: This function should be replaced by the complete arbitrary lane change module.

Parameters

start_downtrack The downtrack distance (m) of the starting point. end_downtrack: The downtrack distance (m) of the target (end) point.

Returns: (int): The adjacent laneletID based on the provided downtrack distance range.

◆ findLaneletIndexFromPath()

int platoon_strategic_ihp::PlatoonStrategicIHPPlugin::findLaneletIndexFromPath	(	int	target_id,
		lanelet::routing::LaneletPath &	path
	)

Find lanelet index from path.

Parameters

path	path
target_id	target lanelet id

Returns: lanelet index

Definition at line 153 of file platoon_strategic_ihp.cpp.

    {
        for(size_t i = 0; i < path.size(); ++i)
        {
            if(path[i].id() == target_id)
            {
                return i;
            }
        }
        return -1;
    }

References process_bag::i.

◆ findLaneWidth()

double platoon_strategic_ihp::PlatoonStrategicIHPPlugin::findLaneWidth ( )

Find lanelet width from local position.

Returns: lanelet index

Definition at line 186 of file platoon_strategic_ihp.cpp.

    {
        lanelet::BasicPoint2d current_loc(pose_msg_.pose.position.x, pose_msg_.pose.position.y);
 
        auto current_lanelets = lanelet::geometry::findNearest(wm_->getMap()->laneletLayer, current_loc, 10);
        lanelet::ConstLanelet current_lanelet = current_lanelets[0].second;
 
        // find left and right bound
        lanelet::ConstLineString3d left_bound = current_lanelet.leftBound();
        lanelet::ConstLineString3d right_bound = current_lanelet.leftBound();
 
        // find lane width
        double dx = abs (left_bound[0].x() - right_bound[0].x());
        double dy = abs (left_bound[0].y() - right_bound[0].y());
        double laneWidth = sqrt(dx*dx + dy*dy);
        RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "calculated lane width: " << laneWidth);
 
        // TODO temporary disable this function and return constant value
        laneWidth = 3.5;
 
 
        return laneWidth;
    }

References pose_msg_, wm_, process_traj_logs::x, and process_traj_logs::y.

Referenced by isJoiningVehicleNearPlatoon(), and isVehicleNearTargetPlatoon().

Here is the caller graph for this function:

◆ findSpeedLimit()

double platoon_strategic_ihp::PlatoonStrategicIHPPlugin::findSpeedLimit ( const lanelet::ConstLanelet & llt )

private

Function to find speed limit of a lanelet.

Parameters

llt	inout lanelet

Returns: speed limit value (m/s)

Definition at line 166 of file platoon_strategic_ihp.cpp.

    {
        double target_speed = 0.0;
 
        lanelet::Optional<carma_wm::TrafficRulesConstPtr> traffic_rules = wm_->getTrafficRules();
        if (traffic_rules)
        {
            target_speed =(*traffic_rules)->speedLimit(llt).speedLimit.value();
        }
        else
        {
            throw std::invalid_argument("Valid traffic rules object could not be built");
        }
 
        RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "target speed (limit) " << target_speed);
 
        return target_speed;
    }

References wm_.

Referenced by plan_maneuver_cb().

Here is the caller graph for this function:

◆ FRIEND_TEST() [1/2]

platoon_strategic_ihp::PlatoonStrategicIHPPlugin::FRIEND_TEST	(	PlatoonStrategicIHPPlugin	,
		is_lanechange_possible
	)

private

◆ FRIEND_TEST() [2/2]

platoon_strategic_ihp::PlatoonStrategicIHPPlugin::FRIEND_TEST	(	PlatoonStrategicIHPPlugin	,
		platoon_info_pub_front
	)

private

◆ georeference_cb()

void platoon_strategic_ihp::PlatoonStrategicIHPPlugin::georeference_cb ( const std_msgs::msg::String::UniquePtr msg )

Callback for the georeference.

Parameters

msg	Latest georeference

Definition at line 266 of file platoon_strategic_ihp.cpp.

    {
        if (georeference_ != msg->data)
        {
            georeference_ = msg->data;
            map_projector_ = std::make_shared<lanelet::projection::LocalFrameProjector>(msg->data.c_str());
        }
    }

References georeference_, and map_projector_.

Referenced by platoon_strategic_ihp::Node::on_configure_plugin().

Here is the caller graph for this function:

◆ getHostPM()

PlatoonManager platoon_strategic_ihp::PlatoonStrategicIHPPlugin::getHostPM ( )

UCLA Getter: for PlatoonManager class.

Definition at line 97 of file platoon_strategic_ihp.cpp.

    {
        return pm_;
    }

References pm_.

◆ handle_mob_req()

MobilityRequestResponse platoon_strategic_ihp::PlatoonStrategicIHPPlugin::handle_mob_req ( const carma_v2x_msgs::msg::MobilityRequest & msg )

Function to the process and respond to the mobility request.

Parameters

msg	Mobility Request Message

Returns: Mobility response message

Definition at line 1357 of file platoon_strategic_ihp.cpp.

    {
        MobilityRequestResponse mobility_response = MobilityRequestResponse::NO_RESPONSE;
 
        // Check that this is a message about platooning (could be from some other Carma activity nearby)
        std::string strategy = msg.strategy;
        if (strategy.rfind(PLATOONING_STRATEGY, 0) != 0)
        {
            RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "Ignoring mobility operation message for " << strategy << " strategy.");
            return MobilityRequestResponse::NO_RESPONSE;
        }
 
        // Handle the message based on host's current state
        if (pm_.current_platoon_state == PlatoonState::LEADER)
        {
            mobility_response = mob_req_cb_leader(msg);
        }
        else if (pm_.current_platoon_state == PlatoonState::FOLLOWER)
        {
            mobility_response = mob_req_cb_follower(msg);
        }
        else if (pm_.current_platoon_state == PlatoonState::CANDIDATEFOLLOWER)
        {
            mobility_response = mob_req_cb_candidatefollower(msg);
        }
        else if (pm_.current_platoon_state == PlatoonState::LEADERWAITING)
        {
            mobility_response = mob_req_cb_leaderwaiting(msg);
        }
        else if (pm_.current_platoon_state == PlatoonState::STANDBY)
        {
            mobility_response = mob_req_cb_standby(msg);
        }
        // UCLA: leader aborting
        else if (pm_.current_platoon_state == PlatoonState::LEADERABORTING)
        {
            mobility_response = mob_req_cb_leaderaborting(msg);
        }
        // UCLA: candidate leader
        else if (pm_.current_platoon_state == PlatoonState::CANDIDATELEADER)
        {
            mobility_response = mob_req_cb_candidateleader(msg);
        }
 
        // UCLA: lead with operation (for cut-in join)
        else if (pm_.current_platoon_state == PlatoonState::LEADWITHOPERATION)
        {
            mobility_response = mob_req_cb_leadwithoperation(msg);
        }
        // UCLA: prepare to join (for cut-in join)
        else if (pm_.current_platoon_state == PlatoonState::PREPARETOJOIN)
        {
            mobility_response = mob_req_cb_preparetojoin(msg);
        }
        // TODO: Place holder for prepare to depart
 
        return mobility_response;
    }

Referenced by mob_req_cb().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ is_lanechange_possible()

bool platoon_strategic_ihp::PlatoonStrategicIHPPlugin::is_lanechange_possible	(	lanelet::Id	start_lanelet_id,
		lanelet::Id	target_lanelet_id
	)

private

Function to check if lanechange is possible.

Parameters

start_lanelet_id	start lanelet id
target_lanelet_id	start lanelet id

Returns: true or false

Definition at line 3475 of file platoon_strategic_ihp.cpp.

    {
        lanelet::ConstLanelet starting_lanelet = wm_->getMap()->laneletLayer.get(start_lanelet_id);
        lanelet::ConstLanelet ending_lanelet = wm_->getMap()->laneletLayer.get(target_lanelet_id);
        lanelet::ConstLanelet current_lanelet = starting_lanelet;
        bool shared_boundary_found = false;
 
        while(!shared_boundary_found)
        {
            //Assumption- Adjacent lanelets share lane boundary
            if(current_lanelet.leftBound() == ending_lanelet.rightBound())
            {
                RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "Lanelet " << std::to_string(current_lanelet.id()) << " shares left boundary with " << std::to_string(ending_lanelet.id()));
                shared_boundary_found = true;
            }
            else if(current_lanelet.rightBound() == ending_lanelet.leftBound())
            {
                RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "Lanelet " << std::to_string(current_lanelet.id()) << " shares right boundary with " << std::to_string(ending_lanelet.id()));
                shared_boundary_found = true;
            }
 
            else
            {
                //If there are no following lanelets on route, lanechange should be completing before reaching it
                if(wm_->getMapRoutingGraph()->following(current_lanelet, false).empty())
                {
                    //In this case we have reached a lanelet which does not have a routable lanelet ahead + isn't adjacent to the lanelet where lane change ends
                    return false;
                }
 
                current_lanelet = wm_->getMapRoutingGraph()->following(current_lanelet, false).front();
                if(current_lanelet.id() == starting_lanelet.id())
                {
                    //Looped back to starting lanelet
                    return false;
                }
            }
        }
 
 
        return true;
    }

References carma_cooperative_perception::to_string(), and wm_.

Referenced by plan_maneuver_cb().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ isJoiningVehicleNearPlatoon()

bool platoon_strategic_ihp::PlatoonStrategicIHPPlugin::isJoiningVehicleNearPlatoon	(	double	joining_downtrack,
		double	joining_crosstrack
	)

private

The method for platoon leader to determine if the joining vehicle is closeby. (Note: Used when host vehicle is the platoon leader)

Parameters

joining_downtrack The downtrack distance of the joining vehicle. joining_crosstrack: The crosstrack distance of the joining vehicle.

Returns: (boolean): if the host vehicle is close to the target platoon.

Definition at line 356 of file platoon_strategic_ihp.cpp.

    {
        // Position info of the host vehicle
        double frontVehicleDtd = current_downtrack_;
        double frontVehicleCtd = current_crosstrack_;
        // platoon rear positions
        int lastVehicleIndex = pm_.host_platoon_.size()-1;
        double rearVehicleDtd = pm_.host_platoon_[lastVehicleIndex].vehiclePosition;
 
        // lateral error for two lanes (lane width was calculated based on current lanelet)
        double two_lane_cross_error = 2*config_.maxCrosstrackError + findLaneWidth();
        // add longitudinal check threshold in config file
        bool longitudinalCheck = joining_downtrack >= rearVehicleDtd - config_.longitudinalCheckThresold  ||
                                 joining_downtrack <= frontVehicleDtd + config_.maxCutinGap;
        bool lateralCheck = joining_crosstrack >= frontVehicleCtd - two_lane_cross_error  ||
                            joining_crosstrack <= frontVehicleCtd + two_lane_cross_error;
        // logs for longitudinal and lateral check
        RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "The longitudinalCheck result is: " << longitudinalCheck );
        RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "The lateralCheck result is: " << lateralCheck );
 
        if (longitudinalCheck && lateralCheck)
        {
            RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "Joining vehicle is nearby. It is able to join.");
            return true;
        }
        else
        {
            RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "The joining vehicle is not close by, the join request will not be approved.");
            RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "The joining vehicle downtrack is " << joining_downtrack << " and the host (platoon leader) downtrack is " << frontVehicleDtd);
            RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "The joining vehicle crosstrack is " << joining_crosstrack << " and the host (platoon leader) crosstrack is " << frontVehicleCtd);
            return false;
        }
    }

References config_, current_crosstrack_, current_downtrack_, findLaneWidth(), platoon_strategic_ihp::PlatoonManager::host_platoon_, platoon_strategic_ihp::PlatoonPluginConfig::longitudinalCheckThresold, platoon_strategic_ihp::PlatoonPluginConfig::maxCrosstrackError, platoon_strategic_ihp::PlatoonPluginConfig::maxCutinGap, and pm_.

Referenced by mob_req_cb_leader().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ isVehicleNearTargetPlatoon()

bool platoon_strategic_ihp::PlatoonStrategicIHPPlugin::isVehicleNearTargetPlatoon	(	double	rearVehicleDtd,
		double	frontVehicleDtd,
		double	frontVehicleCtd
	)

private

Function to determine if the host vehicle is close to the target platoon (used for cut-in join scenarios).

Parameters

rearVehicleDtd	The downtrack of the neighbor platoon rear vehicle.
frontVehicleDtd	The downtrack of the neighbor platoon leader.
frontVehicleCtd	The crosstrack of the neighbor platoon leader.

Returns: (boolean): if the host vehicle is close to the target platoon.

Definition at line 391 of file platoon_strategic_ihp.cpp.

    {
 
        // lateral error for two lanes (lane width was calculated based on current lanelet)
        double two_lane_cross_error = 2*config_.maxCrosstrackError + findLaneWidth();
        // add longitudinal check threshold in config file
        bool longitudinalCheck = current_downtrack_ >= rearVehicleDtd - config_.longitudinalCheckThresold ||
                                 current_downtrack_ <= frontVehicleDtd + config_.longitudinalCheckThresold;
        bool lateralCheck = abs(current_crosstrack_ - frontVehicleCtd) <= two_lane_cross_error;
        // current_crosstrack_ >= frontVehicleCtd - two_lane_cross_error ||
        //                     current_crosstrack_ <= frontVehicleCtd + two_lane_cross_error;
        // logs for longitudinal and lateral check
        RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "The longitudinalCheck result is: " << longitudinalCheck );
        RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "The lateralCheck result is: " << lateralCheck );
 
        if (longitudinalCheck && lateralCheck)
        {
            // host vehicle is close to target platoon longitudinally (within 10m) and laterally (within 5m)
            RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "Found a platoon nearby. We are able to join.");
            return true;
        }
        else
        {
            RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "Ignoring platoon.");
            RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "The platoon leader dtd is " << frontVehicleDtd << " and we are current at " << current_downtrack_);
            RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "The platoon leader ctd is " << frontVehicleCtd << " and we are current at " << current_crosstrack_);
            return false;
        }
    }

References config_, current_crosstrack_, current_downtrack_, findLaneWidth(), platoon_strategic_ihp::PlatoonPluginConfig::longitudinalCheckThresold, and platoon_strategic_ihp::PlatoonPluginConfig::maxCrosstrackError.

Referenced by mob_op_cb_leader().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ isVehicleRightBehind()

bool platoon_strategic_ihp::PlatoonStrategicIHPPlugin::isVehicleRightBehind	(	double	downtrack,
		double	crosstrack
	)

private

Function to determine if the given downtrack distance (m) is behind the host vehicle.

note: This is only applicable for same lane application, so it will check cross track distance.

Parameters

downtrack The downtrack distance (m) of the target vehicle to compare with the host. crosstrack: The crosstrack distance (m) of the target vehicle to compart with the host.

Returns: (boolean): if target vehicle is behind the host vehicle.

Definition at line 232 of file platoon_strategic_ihp.cpp.

    {
        double currentDtd = current_downtrack_;
        double currentCtd = current_crosstrack_;
        bool samelane = abs(currentCtd-crosstrack) <= config_.maxCrosstrackError;
 
        if (downtrack < currentDtd && samelane)
        {
            RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "Found a platoon at behind. We are able to join");
            return true;
        }
        else
        {
            RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "Ignoring platoon that is either ahead of us or in another lane.");
            RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "The front platoon dtd is " << downtrack << " and we are current at " << currentDtd);
            return false;
        }
    }

References config_, current_crosstrack_, current_downtrack_, and platoon_strategic_ihp::PlatoonPluginConfig::maxCrosstrackError.

Referenced by mob_op_cb_leader().

Here is the caller graph for this function:

◆ isVehicleRightInFront()

bool platoon_strategic_ihp::PlatoonStrategicIHPPlugin::isVehicleRightInFront	(	double	downtrack,
		double	crosstrack
	)

private

Function to determine if a target vehicle is in the front of host vehicle note: This is only applicable for same lane application, so it will check cross track distance.

Parameters

downtrack target vehicle downtrack distance relative to host's route (m) crosstrack: target vehicle crosstrack (m)

Returns: true or false

Definition at line 211 of file platoon_strategic_ihp.cpp.

    {
        // Position info of the host vehcile
        double currentDtd = current_downtrack_;
        double currentCtd = current_crosstrack_;
        bool samelane = abs(currentCtd-crosstrack) <= config_.maxCrosstrackError;
 
        if (downtrack > currentDtd && samelane)
        {
            RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "Found a platoon in front. We are able to join");
            return true;
        }
        else
        {
            RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "Ignoring platoon that is either behind host or in another lane.");
            RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "The front platoon dtd is " << downtrack << " and we are current at " << currentDtd);
            return false;
        }
    }

References config_, current_crosstrack_, current_downtrack_, and platoon_strategic_ihp::PlatoonPluginConfig::maxCrosstrackError.

Referenced by mob_op_cb_leader().

Here is the caller graph for this function:

◆ mob_op_cb()

void platoon_strategic_ihp::PlatoonStrategicIHPPlugin::mob_op_cb ( const carma_v2x_msgs::msg::MobilityOperation::UniquePtr msg )

Callback function for Mobility Operation Message.

Parameters

msg	Mobility Operation Message

Definition at line 812 of file platoon_strategic_ihp.cpp.

    {
        if (pm_.current_platoon_state == PlatoonState::STANDBY)
        {
            return;
        }
 
        RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "mob_op_cb received msg with sender ID " << msg->m_header.sender_id
                        << ", plan ID " << msg->m_header.plan_id);
        RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "...strategy " << msg->strategy << ", strategy params " << msg->strategy_params);
 
        // Check that this is a message about platooning (could be from some other Carma activity nearby)
        std::string strategy = msg->strategy;
        if (strategy.rfind(PLATOONING_STRATEGY, 0) != 0)
        {
            RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "Ignoring mobility operation message for " << strategy << " strategy.");
            return;
        }
 
        // Ignore messages as long as host vehicle is stopped
        if (current_speed_ < STOPPED_SPEED)
        {
            RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "Ignoring message since host is stopped.");
            return;
        }
 
        // Perform common operations that apply to all states
        std::string strategyParams = msg->strategy_params;
        bool isPlatoonStatusMsg = strategyParams.rfind(OPERATION_STATUS_TYPE, 0) == 0;
        bool isPlatoonInfoMsg = strategyParams.rfind(OPERATION_INFO_TYPE, 0) == 0;
        RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "strategyParams: " << strategyParams << "isPlatoonStatusMsg: " << isPlatoonStatusMsg << "isPlatoonInfoMsg: " << isPlatoonInfoMsg);
        if (isPlatoonStatusMsg)
        {
            mob_op_cb_STATUS(*msg);
        }
        else if (isPlatoonInfoMsg)
        {
            // Note: this is where confusion will reign if multiple neighbor platoons are discovered; need more sophisticated
            //       logic to handle that situation
 
            // If it is a legitimate platoon (2 or more members) other than our own then
            std::vector<std::string> inputsParams;
            boost::algorithm::split(inputsParams, strategyParams, boost::is_any_of(","));
            std::vector<std::string> p_size;
            boost::algorithm::split(p_size, inputsParams[2], boost::is_any_of(":"));
            int platoon_size = std::stoi(p_size[1]);
            RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "neighbor platoon_size from INFO: " << platoon_size);
            if (platoon_size > 1  &&  msg->m_header.plan_id.compare(pm_.currentPlatoonID) != 0)
            {
                // If platoon ID doesn't match our known target platoon then clear any old neighbor platoon info and record
                // the platoon ID and the sender as the leader (only leaders send INFO)
                if (msg->m_header.plan_id.compare(pm_.neighborPlatoonID) != 0)
                {
                    pm_.resetNeighborPlatoon();
                    pm_.neighborPlatoonID = msg->m_header.plan_id;
                }
                pm_.neighbor_platoon_leader_id_ = msg->m_header.sender_id;
                RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "pm_.neighbor_platoon_leader_id_: " << pm_.neighbor_platoon_leader_id_);
                pm_.neighbor_platoon_info_size_ = platoon_size;
                RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "pm_.neighbor_platoon_info_size_: " << pm_.neighbor_platoon_info_size_);
            }
        }
 
        else
        {
            RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "Invalid Mob Op received");
        }
 
        // Perform state-specific additional actions
        if (pm_.current_platoon_state == PlatoonState::LEADER)
        {
            mob_op_cb_leader(*msg);
        }
        else if (pm_.current_platoon_state == PlatoonState::FOLLOWER)
        {
            mob_op_cb_follower(*msg);
        }
        else if (pm_.current_platoon_state == PlatoonState::CANDIDATEFOLLOWER)
        {
            mob_op_cb_candidatefollower(*msg);
        }
        else if (pm_.current_platoon_state == PlatoonState::LEADERWAITING)
        {
            mob_op_cb_leaderwaiting(*msg);
        }
        else if (pm_.current_platoon_state == PlatoonState::STANDBY)
        {
            mob_op_cb_standby(*msg);
        }
        // UCLA: add leader aborting
        else if (pm_.current_platoon_state == PlatoonState::LEADERABORTING)
        {
            mob_op_cb_leaderaborting(*msg);
        }
        // UCLA: add candidate leader
        else if (pm_.current_platoon_state == PlatoonState::CANDIDATELEADER)
        {
            mob_op_cb_candidateleader(*msg);
        }
        // UCLA: add lead with operation for cut-in join
        else if (pm_.current_platoon_state == PlatoonState::LEADWITHOPERATION)
        {
            mob_op_cb_leadwithoperation(*msg);
        }
        // UCLA: add prepare to join for cut-in join
        else if (pm_.current_platoon_state == PlatoonState::PREPARETOJOIN)
        {
            mob_op_cb_preparetojoin(*msg);
        }
        // TODO: Place holder for prepare to depart
 
        // TODO: If needed (with large size platoons), add a queue for status messages
        //       INFO messages always processed, STATUS messages if saved in que
    }

Referenced by platoon_strategic_ihp::Node::on_configure_plugin().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ mob_op_cb_candidatefollower()

void platoon_strategic_ihp::PlatoonStrategicIHPPlugin::mob_op_cb_candidatefollower ( const carma_v2x_msgs::msg::MobilityOperation & msg )

private

Function to process mobility operation in candidate follower state.

Parameters

msg	incoming mobility operation

Definition at line 934 of file platoon_strategic_ihp.cpp.

    {
        RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "CANDIDATEFOLLOWER state no further action on message from " << msg.m_header.sender_id);
    }

Referenced by mob_op_cb().

Here is the caller graph for this function:

◆ mob_op_cb_candidateleader()

void platoon_strategic_ihp::PlatoonStrategicIHPPlugin::mob_op_cb_candidateleader ( const carma_v2x_msgs::msg::MobilityOperation & msg )

private

Function to process mobility operation in candidateleader state.

Parameters

msg	incoming mobility operation message.

Definition at line 1235 of file platoon_strategic_ihp.cpp.

    {
        RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "CANDIDATELEADER state no further action on message from " << msg.m_header.sender_id);
    }

Referenced by mob_op_cb().

Here is the caller graph for this function:

◆ mob_op_cb_follower()

void platoon_strategic_ihp::PlatoonStrategicIHPPlugin::mob_op_cb_follower ( const carma_v2x_msgs::msg::MobilityOperation & msg )

private

Function to process mobility operation in follower state.

Parameters

msg	incoming mobility operation

Definition at line 940 of file platoon_strategic_ihp.cpp.

    {
        //std::string strategyParams = msg.strategy_params;
        //bool isPlatoonStatusMsg = (strategyParams.rfind(OPERATION_STATUS_TYPE, 0) == 0);
 
        // TODO: Place holder for prepare to depart
 
        RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "FOLLOWER state no further action on message from " << msg.m_header.sender_id);
    }

Referenced by mob_op_cb().

Here is the caller graph for this function:

◆ mob_op_cb_leader()

void platoon_strategic_ihp::PlatoonStrategicIHPPlugin::mob_op_cb_leader ( const carma_v2x_msgs::msg::MobilityOperation & msg )

private

Function to process mobility operation in leader state.

Parameters

msg	incoming mobility operation

Note: This is the function to handle the mobility operation message. Vehicle in leader state is either a single ADS vehicle or a platoon leader.

Single ADS will send out INFO messages. Platoon leaders will send out both INFO and STATUS messages.

If the host is single vehicle, it should have "isPlatoonInfoMsg = true" and "isInNegotiation = false". In such condition, single leader will start joining and send request to the platoon leader. mob_op_cb_leader(

If the host vehicle is platoon leader, then it should have "isPlatoonInfoMsg = true" and "isInNegotiation = true". But existing platoon leader do not need to send out joining request.

Both the platoon leader and single vehicle need to subscribe to the STATUS message to populate the platoon manager with existing platoon members, so the PM can calculate dtd and ctd corresponds to host vehicle's origin position, hence to be used for later calculation.

note: [veh3*] -------— [veh2] --------------— [veh1] -------— [*veh0] |<---------------— front-rear DTD difference -----------------—>| |<--------------------------— platoon length -------------------------—>|

front-rear DTD difference = platoon_length + one_vehicle_length Vehicle length is already accounted for in the message's LENGTH value

Note: "isVehicleRightInFront" tests for same lane

Note: "isVehicleRightBehind" tests for same lane

JOIN_PARAMS format: JOIN_PARAMS| --> "SIZE:%1%,SPEED:%2%,ECEFX:%3%,ECEFY:%4%,ECEFZ:%5%,JOINIDX:%6%" |----—0---— –1------—2------—3------—4-------—5----—|

UCLA Implementation note:

isVehicleNearTargetPlatoon --> determine if the platoon is next to host vehicle
sender_id == requesting veh ID --> joiner ID

Definition at line 957 of file platoon_strategic_ihp.cpp.

    {
        // Read incoming message info
        std::string strategyParams = msg.strategy_params;
        std::string senderId = msg.m_header.sender_id;
        std::string platoonId = msg.m_header.plan_id;
 
        // In the current state, host vehicle care about the INFO heart-beat operation message if we are not currently in
        // a negotiation, and host also need to care about operation from members in our current platoon.
        bool isPlatoonInfoMsg = strategyParams.rfind(OPERATION_INFO_TYPE, 0) == 0;            // INFO message only broadcast by leader and single CAV.
        bool isInNegotiation = pm_.current_plan.valid  ||  pm_.currentPlatoonID.compare(pm_.dummyID) != 0; // In negotiation indicates host is not available to become a joiner
                                                                                              // (i.e., not currently in a platoon or trying to join a platoon).
        RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "Top of mob_op_cb_leader, isInNegotiation = " << isInNegotiation);
 
        // Condition 1. Host vehicle is the single CAV joining the platoon.
        if (isPlatoonInfoMsg && !isInNegotiation)
        {
            //TODO future enhancement: add logic here for if/how to join two existing platoons
            //     (e.g. the shorter platoon should join the longer one, or rear joins front if same length)
 
            // step 1. read INFO message from the target platoon leader
 
            // read ecef location from strategy params.
            carma_v2x_msgs::msg::LocationECEF ecef_loc;
            ecef_loc = mob_op_find_ecef_from_INFO_params(strategyParams);
 
            // use ecef_loc to calculate front Dtd in m.
            lanelet::BasicPoint2d incoming_pose = ecef_to_map_point(ecef_loc);
            double frontVehicleDtd = wm_->routeTrackPos(incoming_pose).downtrack;
 
            // use ecef_loc to calculate front Ctd in m.
            double frontVehicleCtd = wm_->routeTrackPos(incoming_pose).crosstrack;
            // downtrack and crosstrack of the platoon leader --> used for frontal join
            RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "Neighbor platoon frontVehicleDtd from ecef: " << frontVehicleDtd << ", frontVehicleCtd from ecef: " << frontVehicleCtd);
 
            // use INFO param to find platoon rear vehicle DTD and CTD.
            double platoon_length = mob_op_find_platoon_length_from_INFO_params(strategyParams); // length of the entire platoon in meters.
            double rearVehicleDtd = frontVehicleDtd - platoon_length;
            RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "rear veh dtd from platoon length: " << rearVehicleDtd);
            if (!pm_.neighbor_platoon_.empty())
            {
                rearVehicleDtd = pm_.neighbor_platoon_.back().vehiclePosition;
                RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "rear veh dtd from neighbor platoon: " << rearVehicleDtd);
            }
 
            // Note: For one platoon, we assume all members are in the same lane.
            double rearVehicleCtd = frontVehicleCtd;
            RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "Neighbor platoon rearVehicleDtd: " << rearVehicleDtd << ", rearVehicleCtd: " << rearVehicleCtd);
 
            // Parse the strategy params
            std::vector<std::string> inputsParams;
            boost::algorithm::split(inputsParams, strategyParams, boost::is_any_of(","));
 
            // Get the target platoon's size (number of members) from strategy params
            std::vector<std::string> targetPlatoonSize_parsed;
            boost::algorithm::split(targetPlatoonSize_parsed, inputsParams[2], boost::is_any_of(":"));
            int targetPlatoonSize = std::stoi(targetPlatoonSize_parsed[1]);
            RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "target Platoon Size: " << targetPlatoonSize);
            RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "Found a vehicle/platoon with id = " << platoonId << " within range.");
 
            //TODO future: add logic here to assess closeness of the neighbor platoon, as well as its speed, destination
            //             & other attributes to decide if we want to join before assembling a join request
 
            // step 2. Generate default info for join request
            carma_v2x_msgs::msg::MobilityRequest request;
            request.m_header.plan_id = boost::uuids::to_string(boost::uuids::random_generator()());
            request.m_header.recipient_id = senderId;
            request.m_header.sender_id = config_.vehicleID;
            request.m_header.timestamp = timer_factory_->now().nanoseconds()/1000000;
            request.location = pose_to_ecef(pose_msg_);
            request.strategy = PLATOONING_STRATEGY;
            request.urgency = 50;
 
            int platoon_size = pm_.getHostPlatoonSize();
 
            // step 3. Request Rear Join
            if(isVehicleRightInFront(rearVehicleDtd, rearVehicleCtd)  &&  !config_.test_front_join)
            {
                RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "Neighbor platoon is right in front of us");
 
                request.plan_type.type = carma_v2x_msgs::msg::PlanType::JOIN_PLATOON_AT_REAR;
 
                /*
                 * JOIN_PARAMS format:
                 *        JOIN_PARAMS| --> "SIZE:%1%,SPEED:%2%,ECEFX:%3%,ECEFY:%4%,ECEFZ:%5%,JOINIDX:%6%"
                 *                   |-------0------ --1---------2---------3---------4----------5-------|
                 */
 
                boost::format fmter(JOIN_PARAMS);
                int dummy_join_index = -2; //not used for this message, but message spec requires it
                fmter %platoon_size;                //  index = 0
                fmter %current_speed_;              //  index = 1, in m/s
                fmter %pose_ecef_point_.ecef_x;     //  index = 2, in cm.
                fmter %pose_ecef_point_.ecef_y;     //  index = 3, in cm.
                fmter %pose_ecef_point_.ecef_z;     //  index = 4, in cm.
                fmter %dummy_join_index;            //  index = 5
                request.strategy_params = fmter.str();
                mobility_request_publisher_(request);
                RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "Publishing request to leader " << senderId << " with params " << request.strategy_params << " and plan id = " << request.m_header.plan_id);
 
                // Create a new join plan
                pm_.current_plan = ActionPlan(true, request.m_header.timestamp, request.m_header.plan_id, senderId);
 
                // If we are asking to join an actual platoon (not a solo vehicle), then save its ID for later use
                if (platoonId.compare(pm_.dummyID) != 0)
                {
                    pm_.targetPlatoonID = platoonId;
                    RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "Detected neighbor as a real platoon & storing its ID: " << platoonId);
                }
            }
 
            // step 4. Request frontal join, if the neighbor is a real platoon
            else if ((targetPlatoonSize > 1  ||  config_.test_front_join)  &&  isVehicleRightBehind(frontVehicleDtd, frontVehicleCtd))
            {
                RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "Neighbor platoon leader is right behind us");
 
                // UCLA: assign a new plan type
                request.plan_type.type = carma_v2x_msgs::msg::PlanType::JOIN_PLATOON_FROM_FRONT;
 
                boost::format fmter(JOIN_PARAMS); // Note: Front and rear join uses same params, hence merge to one param for both condition.
                int dummy_join_index = -2; //not used for this message, but message spec requires it
                fmter %platoon_size;                //  index = 0
                fmter %current_speed_;              //  index = 1, in m/s
                fmter %pose_ecef_point_.ecef_x;     //  index = 2, in cm.
                fmter %pose_ecef_point_.ecef_y;     //  index = 3, in cm.
                fmter %pose_ecef_point_.ecef_z;     //  index = 4, in cm.
                fmter %dummy_join_index;            //  index = 5
                request.strategy_params = fmter.str();
                mobility_request_publisher_(request);
                RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "Publishing front join request to the leader " << senderId << " with params " << request.strategy_params << " and plan id = " << request.m_header.plan_id);
 
                // Create a new join plan
                pm_.current_plan = ActionPlan(true, request.m_header.timestamp, request.m_header.plan_id, senderId);
 
                // If testing with a solo vehicle to represent the target platoon, then use the current join plan ID for that platoon;
                // otherwise, it will already have and ID so store it for future use
                if (config_.test_front_join)
                {
                    pm_.targetPlatoonID = request.m_header.plan_id;
                    RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "Since neighbor is a fake platoon, storing " << pm_.targetPlatoonID << " as its platoon ID");
                }
                else
                {
                    pm_.targetPlatoonID = platoonId;
                    RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "Storing real neighbor platoon's ID as target: " << pm_.targetPlatoonID);
                }
            }
 
            // step 5. Request cut-in join (front, middle or rear, from adjacent lane)
            else if ((targetPlatoonSize > 1  ||  config_.test_cutin_join)  &&  !config_.test_front_join
                        &&  isVehicleNearTargetPlatoon(rearVehicleDtd, frontVehicleDtd, frontVehicleCtd))
            {
 
                RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "starting cut-in join process");
                RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "rearVehicleDtd " << rearVehicleDtd);
                RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "rearVehicleCtd " << rearVehicleCtd);
 
                // If we are asking to join an actual platoon (not a solo vehicle), then save its ID for later use
                if (platoonId.compare(pm_.dummyID) != 0)
                {
                    pm_.targetPlatoonID = platoonId;
                    RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "Detected neighbor as a real platoon & storing its ID: " << platoonId);
                }
 
 
                carma_wm::TrackPos target_trackpose(rearVehicleDtd, rearVehicleCtd);
                auto target_rear_pose = wm_->pointFromRouteTrackPos(target_trackpose);
                if (target_rear_pose)
                {
                    target_cutin_pose_ = incoming_pose;
 
                    auto target_lanelets = lanelet::geometry::findNearest(wm_->getMap()->laneletLayer, target_rear_pose.get(), 1);
                    if (!target_lanelets.empty())
                    {
                        long target_rear_pose_lanelet_id = target_lanelets[0].second.id();
                        RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "target_rear_pose_lanelet_id: " << target_rear_pose_lanelet_id);
                    }
                    else
                    {
                        RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "target_rear_pose_lanelet not found!!");
                    }
                }
 
                else
                {
                    RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "No target pose is found, so we cannot prodeed with a cutin join request.");
                    return;
                }
 
                //TODO: verify purpose of this logic then its correctness accordingly:
                //  -is step 5 (this block) really for all 3 types of joining?
                //  -ensure join_index gets an appropriate value for the type of join
                //  -ensure plan type corresponds to join_index
 
                // complete the request
                // UCLA: this is a newly added plan type
                // Note: Request conposed outside of if conditions
                // UCLA: Desired joining index for cut-in join, indicate the index of gap-leading vehicle. -1 indicate cut-in from front.
                // Note: remove join_index to info param.
                request.plan_type.type = carma_v2x_msgs::msg::PlanType::PLATOON_CUT_IN_JOIN;
 
                // At this step all cut-in types start with this request, so the join_index at this point is set to default, -2.
                int join_index = -2;
                boost::format fmter(JOIN_PARAMS); // Note: Front and rear join uses same params, hence merge to one param for both condition.
                fmter %platoon_size;                //  index = 0
                fmter %current_speed_;              //  index = 1, in m/s
                fmter %pose_ecef_point_.ecef_x;     //  index = 2
                fmter %pose_ecef_point_.ecef_y;     //  index = 3
                fmter %pose_ecef_point_.ecef_z;     //  index = 4
                fmter %join_index;                  //  index = 5
                request.strategy_params = fmter.str();
                mobility_request_publisher_(request);
                RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "Publishing request to the leader " << senderId << " with params " << request.strategy_params << " and plan id = " << request.m_header.plan_id);
 
                // Create a new join plan
                pm_.current_plan = ActionPlan(true, request.m_header.timestamp, request.m_header.plan_id, senderId);
            }
 
            // step 6. Return none if no platoon nearby
            else
            {
                RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "Ignore platoon with platoon id: " << platoonId << " because it is too far away to join.");
            }
        }
 
        // TODO: Place holder for prepare to depart (else if isdepart)
 
    }

Referenced by mob_op_cb().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ mob_op_cb_leaderaborting()

void platoon_strategic_ihp::PlatoonStrategicIHPPlugin::mob_op_cb_leaderaborting ( const carma_v2x_msgs::msg::MobilityOperation & msg )

private

Function to process mobility operation in leaderaborting state.

Parameters

msg	incoming mobility operation message.

Definition at line 1229 of file platoon_strategic_ihp.cpp.

    {
        RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "LEADERABORTING state no further action on message from " << msg.m_header.sender_id);
    }

Referenced by mob_op_cb().

Here is the caller graph for this function:

◆ mob_op_cb_leaderwaiting()

void platoon_strategic_ihp::PlatoonStrategicIHPPlugin::mob_op_cb_leaderwaiting ( const carma_v2x_msgs::msg::MobilityOperation & msg )

private

Function to process mobility operation in leader waiting state.

Parameters

msg	incoming mobility operation

Definition at line 951 of file platoon_strategic_ihp.cpp.

    {
        RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "LEADERWAITING state no further action on message from " << msg.m_header.sender_id);
    }

Referenced by mob_op_cb().

Here is the caller graph for this function:

◆ mob_op_cb_leadwithoperation()

void platoon_strategic_ihp::PlatoonStrategicIHPPlugin::mob_op_cb_leadwithoperation ( const carma_v2x_msgs::msg::MobilityOperation & msg )

private

Function to process mobility operation in LeadWithOperation state (cut-in join)

Parameters

msg	incoming mobility operation

Definition at line 1241 of file platoon_strategic_ihp.cpp.

    {
        RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "LEADWITHOPERATION state no further action on message from " << msg.m_header.sender_id);
    }

Referenced by mob_op_cb().

Here is the caller graph for this function:

◆ mob_op_cb_preparetojoin()

void platoon_strategic_ihp::PlatoonStrategicIHPPlugin::mob_op_cb_preparetojoin ( const carma_v2x_msgs::msg::MobilityOperation & msg )

private

Function to process mobility operation in PrepareToJoin state (cut-in join)

Parameters

msg	incoming mobility operation

Definition at line 1247 of file platoon_strategic_ihp.cpp.

    {
        /*
         * If same lane with leader, then send request to do same lane join.
         * Otherwise, just send status params.
         * Note: leader in state 'leading with operation' also send out INFO msg
         */
 
        // read parameters
        std::string strategyParams = msg.strategy_params;
        std::string senderId = msg.m_header.sender_id;
 
        // locate INFO type
        bool isPlatoonInfoMsg = strategyParams.rfind(OPERATION_INFO_TYPE, 0) == 0;
 
        // If this is an INFO message and our record of the neighbor platoon is complete then
        // pm_.is_neighbor_record_complete_ = true; //TODO temporary
        RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "pm_.is_neighbor_record_complete_" << pm_.is_neighbor_record_complete_);
 
        if (isPlatoonInfoMsg  &&  pm_.is_neighbor_record_complete_)
        {
 
            //TODO: would be good to have a timeout here; if a neighbor platoon has been identified, and no INFO messages
            //      from it are received in a while, then its record should be erased, and any in-work joining should be
            //      aborted.
 
            // read ecef location from strategy params.
            carma_v2x_msgs::msg::LocationECEF ecef_loc;
            ecef_loc = mob_op_find_ecef_from_INFO_params(strategyParams);
            // use ecef_loc to calculate front Dtd in m.
            lanelet::BasicPoint2d incoming_pose = ecef_to_map_point(ecef_loc);
 
            double frontVehicleDtd = wm_->routeTrackPos(incoming_pose).downtrack;
 
            // use ecef_loc to calculate front Ctd in m.
            double frontVehicleCtd = wm_->routeTrackPos(incoming_pose).crosstrack;
 
            // // Find neighbor platoon end vehicle and its downtrack in m
            int rearVehicleIndex = pm_.neighbor_platoon_.size() - 1;
            RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "rearVehicleIndex: " << rearVehicleIndex);
            double rearVehicleDtd = pm_.neighbor_platoon_[rearVehicleIndex].vehiclePosition;
            RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "Neighbor rearVehicleDtd from ecef: " << rearVehicleDtd);
 
            // If lane change has not yet been authorized, stop here (this method will be running before the negotiations
            // with the platoon leader are complete)
            if (!safeToLaneChange_)
            {
                return;
            }
 
            // determine if the lane change is finished
            bool isSameLaneWithPlatoon = abs(frontVehicleCtd - current_crosstrack_) <= config_.maxCrosstrackError;
            RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "Lane change has been authorized. isSameLaneWithPlatoon = " << isSameLaneWithPlatoon);
            RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "crosstrack diff" << abs(frontVehicleCtd - current_crosstrack_));
            if (isSameLaneWithPlatoon)
            {
                // request 1. reset the safeToChangLane indicators if lane change is finished
                safeToLaneChange_ = false;
 
                // request 2. change to same lane operation states (determine based on DTD differences)
                carma_v2x_msgs::msg::MobilityRequest request;
                request.m_header.plan_id = boost::uuids::to_string(boost::uuids::random_generator()());
                request.m_header.recipient_id = senderId;
                request.m_header.sender_id = config_.vehicleID;
                request.m_header.timestamp = timer_factory_->now().nanoseconds()/1000000;
                request.location = pose_to_ecef(pose_msg_);
 
                // UCLA: send request based on cut-in type
                if (frontVehicleDtd < current_downtrack_)
                {
                    request.plan_type.type = carma_v2x_msgs::msg::PlanType::CUT_IN_FRONT_DONE;
                }
                else
                {
                    request.plan_type.type = carma_v2x_msgs::msg::PlanType::CUT_IN_MID_OR_REAR_DONE;
                }
                request.strategy = PLATOONING_STRATEGY;
                double host_platoon_size = pm_.getHostPlatoonSize();
 
                boost::format fmter(JOIN_PARAMS);
                fmter %host_platoon_size;           //  index = 0
                fmter %current_speed_;              //  index = 1, in m/s
                fmter %pose_ecef_point_.ecef_x;     //  index = 2
                fmter %pose_ecef_point_.ecef_y;     //  index = 3
                fmter %pose_ecef_point_.ecef_z;     //  index = 4
                fmter %target_join_index_;          //  index = 5
                request.strategy_params = fmter.str();
                request.urgency = 50;
 
                mobility_request_publisher_(request);
                if (pm_.currentPlatoonID.compare(pm_.dummyID) == 0)
                {
                    pm_.currentPlatoonID = request.m_header.plan_id;
                }
 
                RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "new platoon id: " << pm_.currentPlatoonID);
                pm_.current_plan = ActionPlan(true, request.m_header.timestamp, request.m_header.plan_id, senderId);
 
                RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "Published Mobility request to revert to same-lane operation");
            }
            else
            {
                RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "Lane Change not completed");
            }
        }
    }

Referenced by mob_op_cb().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ mob_op_cb_standby()

void platoon_strategic_ihp::PlatoonStrategicIHPPlugin::mob_op_cb_standby ( const carma_v2x_msgs::msg::MobilityOperation & msg )

private

Function to process mobility operation in standby state.

Parameters

msg	incoming mobility operation

Definition at line 927 of file platoon_strategic_ihp.cpp.

    {
        // In standby state, it will ignore operation message since it is not actively operating
        RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "STANDBY state no further action on message from " << msg.m_header.sender_id);
    }

Referenced by mob_op_cb().

Here is the caller graph for this function:

◆ mob_op_cb_STATUS()

void platoon_strategic_ihp::PlatoonStrategicIHPPlugin::mob_op_cb_STATUS ( const carma_v2x_msgs::msg::MobilityOperation & msg )

private

Function to process mobility operation message with STATUS params, read ecef location and update platoon member info.

Parameters

msg	incoming mobility operation message.

Note: STATUS params format: STATUS | --> "CMDSPEED:%1%,SPEED:%2%,ECEFX:%3%,ECEFY:%4%,ECEFZ:%5%" |-------—0-------—1------—2------—3------—4---—|

Definition at line 711 of file platoon_strategic_ihp.cpp.

    {
        RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "Entered mob_op_cb_STATUS");
        std::string strategyParams = msg.strategy_params;
        std::string vehicleID = msg.m_header.sender_id;
        std::string platoonId = msg.m_header.plan_id;
        RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "strategyParams = " << strategyParams);
        RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "platoonId = " << platoonId << ", sender ID = " << vehicleID);
        std::string statusParams = strategyParams.substr(OPERATION_STATUS_TYPE.size() + 1);
        RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "pm_.currentPlatoonID = " << pm_.currentPlatoonID << ", targetPlatoonID = " << pm_.targetPlatoonID);
 
        // read Downtrack
        carma_v2x_msgs::msg::LocationECEF ecef_loc = mob_op_find_ecef_from_STATUS_params(strategyParams);
        lanelet::BasicPoint2d incoming_pose = ecef_to_map_point(ecef_loc);
        double dtd = wm_->routeTrackPos(incoming_pose).downtrack;
        RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "DTD calculated from ecef is: " << dtd);
        // read Crosstrack
        double ctd = wm_->routeTrackPos(incoming_pose).crosstrack;
        RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "CTD calculated from ecef is: " << ctd);
 
        // If it comes from a member of an identified neighbor platoon, then
        if (platoonId.compare(pm_.neighborPlatoonID) == 0 && platoonId.compare(pm_.dummyID) != 0)
        {
            RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "Incoming platoonID matches target platoon id");
            // // Update this member's status (or add if it's unknown to us)
            pm_.neighborMemberUpdates(vehicleID, platoonId, statusParams, dtd, ctd);
        }
 
        // else if this message is for our platoon then store its info
        else if (platoonId.compare(pm_.currentPlatoonID) == 0 && platoonId.compare(pm_.dummyID) != 0)
        {
            pm_.hostMemberUpdates(vehicleID, platoonId, statusParams, dtd, ctd);
        }
 
        // else it represents an uninteresting platoon
        else
        {
            RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "Received mob op for platoon " << platoonId << " that doesn't match our platoon: " << pm_.currentPlatoonID
                             << " or known neighbor platoon: " << pm_.targetPlatoonID);
        }
    }

References platoon_strategic_ihp::PlatoonManager::currentPlatoonID, platoon_strategic_ihp::PlatoonManager::dummyID, ecef_to_map_point(), platoon_strategic_ihp::PlatoonManager::hostMemberUpdates(), mob_op_find_ecef_from_STATUS_params(), platoon_strategic_ihp::PlatoonManager::neighborMemberUpdates(), platoon_strategic_ihp::PlatoonManager::neighborPlatoonID, OPERATION_STATUS_TYPE, pm_, platoon_strategic_ihp::PlatoonManager::targetPlatoonID, and wm_.

Referenced by mob_op_cb().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ mob_op_find_ecef_from_INFO_params()

carma_v2x_msgs::msg::LocationECEF platoon_strategic_ihp::PlatoonStrategicIHPPlugin::mob_op_find_ecef_from_INFO_params ( std::string strategyParams )

private

Function to process mobility operation INFO params to find platoon leader's ecef location.

Parameters

strategyParams The parsed strategy params, used to find ecef locaton.

Returns: ecef location of the sender.

Note: INFO param format: "INFO| --> LENGTH:%.2f,SPEED:%.2f,SIZE:%d,ECEFX:%.2f,ECEFY:%.2f,ECEFZ:%.2f" |----—0--------—1------—2-----—3-------—4-------—5----—|

Definition at line 780 of file platoon_strategic_ihp.cpp.

    {
        // For INFO params, the string format is INFO|REAR:%s,LENGTH:%.2f,SPEED:%.2f,SIZE:%d,DTD:%.2f
        std::vector<std::string> inputsParams;
        boost::algorithm::split(inputsParams, strategyParams, boost::is_any_of(","));
 
        std::vector<std::string> ecef_x_parsed;
        boost::algorithm::split(ecef_x_parsed, inputsParams[3], boost::is_any_of(":"));
        double ecef_x = std::stod(ecef_x_parsed[1]);
 
        std::vector<std::string> ecef_y_parsed;
        boost::algorithm::split(ecef_y_parsed, inputsParams[4], boost::is_any_of(":"));
        double ecef_y = std::stod(ecef_y_parsed[1]);
 
        std::vector<std::string> ecef_z_parsed;
        boost::algorithm::split(ecef_z_parsed, inputsParams[5], boost::is_any_of(":"));
        double ecef_z = std::stod(ecef_z_parsed[1]);
 
        carma_v2x_msgs::msg::LocationECEF ecef_loc;
        ecef_loc.ecef_x = ecef_x;
        ecef_loc.ecef_y = ecef_y;
        ecef_loc.ecef_z = ecef_z;
 
        return ecef_loc;
    }

References process_traj_logs::split.

Referenced by mob_op_cb_leader(), and mob_op_cb_preparetojoin().

Here is the caller graph for this function:

◆ mob_op_find_ecef_from_STATUS_params()

carma_v2x_msgs::msg::LocationECEF platoon_strategic_ihp::PlatoonStrategicIHPPlugin::mob_op_find_ecef_from_STATUS_params ( std::string strategyParams )

private

Function to process mobility operation for STATUS params.

Parameters

strategyParams The parsed strategy params, used to find ecef locaton.

Returns: ecef location of the sender.

Definition at line 678 of file platoon_strategic_ihp.cpp.

    {
        /*
         * Helper function that extract ecef location from STATUS msg.
         * Note: STATUS params format:
         *       STATUS | --> "CMDSPEED:%1%,SPEED:%2%,ECEFX:%3%,ECEFY:%4%,ECEFZ:%5%"
         *              |----------0----------1---------2---------3---------4------|
         */
 
        std::vector<std::string> inputsParams;
        boost::algorithm::split(inputsParams, strategyParams, boost::is_any_of(","));
 
        std::vector<std::string> ecef_x_parsed;
        boost::algorithm::split(ecef_x_parsed, inputsParams[2], boost::is_any_of(":"));
        double ecef_x = std::stod(ecef_x_parsed[1]);
 
        std::vector<std::string> ecef_y_parsed;
        boost::algorithm::split(ecef_y_parsed, inputsParams[3], boost::is_any_of(":"));
        double ecef_y = std::stod(ecef_y_parsed[1]);
 
        std::vector<std::string> ecef_z_parsed;
        boost::algorithm::split(ecef_z_parsed, inputsParams[4], boost::is_any_of(":"));
        double ecef_z = std::stod(ecef_z_parsed[1]);
 
        carma_v2x_msgs::msg::LocationECEF ecef_loc;
        ecef_loc.ecef_x = ecef_x;
        ecef_loc.ecef_y = ecef_y;
        ecef_loc.ecef_z = ecef_z;
 
        return ecef_loc;
    }

References process_traj_logs::split.

Referenced by mob_op_cb_STATUS().

Here is the caller graph for this function:

◆ mob_op_find_platoon_length_from_INFO_params()

double platoon_strategic_ihp::PlatoonStrategicIHPPlugin::mob_op_find_platoon_length_from_INFO_params ( std::string strategyParams )

private

Function to process mobility operation INFO params to find platoon length in m.

Parameters

strategyParams The parsed strategy params, used to find ecef locaton.

Returns: The length of the platoon in m.

Note: INFO param format: "INFO| --> LENGTH:%.2f,SPEED:%.2f,SIZE:%d,ECEFX:%.2f,ECEFY:%.2f,ECEFZ:%.2f" |----—0--------—1------—2-----—3-------—4-------—5----—|

Definition at line 760 of file platoon_strategic_ihp.cpp.

    {
        // For INFO params, the string format is INFO|REAR:%s,LENGTH:%.2f,SPEED:%.2f,SIZE:%d,DTD:%.2f
        std::vector<std::string> inputsParams;
        boost::algorithm::split(inputsParams, strategyParams, boost::is_any_of(","));
 
        // Use the strategy params' length value and leader location to determine DTD of its rear
        std::vector<std::string> target_platoon_length;
        boost::algorithm::split(target_platoon_length, inputsParams[0], boost::is_any_of(":"));
        double platoon_length = std::stod(target_platoon_length[1]);
 
        return platoon_length;
    }

References process_traj_logs::split.

Referenced by mob_op_cb_leader().

Here is the caller graph for this function:

◆ mob_req_cb()

void platoon_strategic_ihp::PlatoonStrategicIHPPlugin::mob_req_cb ( const carma_v2x_msgs::msg::MobilityRequest::UniquePtr msg )

Callback function for Mobility Request Message.

Parameters

msg	Mobility Request Message

Definition at line 2554 of file platoon_strategic_ihp.cpp.

    {
        // Ignore messages as long as host vehicle is stopped
        if (current_speed_ < config_.minPlatooningSpeed)
        {
            RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "Ignoring message since host speed is below platooning speed.");
            return;
        }
 
        // Check that this is a message about platooning (could be from some other Carma activity nearby)
        std::string strategy = msg->strategy;
        if (strategy.rfind(PLATOONING_STRATEGY, 0) != 0)
        {
            RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "Ignoring mobility operation message for " << strategy << " strategy.");
            return;
        }
 
        carma_v2x_msgs::msg::MobilityResponse response;
        response.m_header.sender_id = config_.vehicleID;
        response.m_header.recipient_id = msg->m_header.sender_id;
        response.m_header.plan_id = msg->m_header.plan_id;
        response.m_header.timestamp = timer_factory_->now().nanoseconds() / 1000000;
 
       // UCLA: add plantype in response
        response.plan_type.type = msg->plan_type.type;
 
        MobilityRequestResponse req_response = handle_mob_req(*msg);
        if (req_response == MobilityRequestResponse::ACK)
        {
            response.is_accepted = true;
            mobility_response_publisher_(response);
        }
        else if (req_response == MobilityRequestResponse::NACK)
        {
            response.is_accepted = false;
            mobility_response_publisher_(response);
        }
        else
        {
            RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), " NO response to mobility request. ");
        }
    }

References platoon_strategic_ihp::ACK, config_, current_speed_, handle_mob_req(), platoon_strategic_ihp::PlatoonPluginConfig::minPlatooningSpeed, mobility_response_publisher_, platoon_strategic_ihp::NACK, PLATOONING_STRATEGY, timer_factory_, and platoon_strategic_ihp::PlatoonPluginConfig::vehicleID.

Referenced by platoon_strategic_ihp::Node::on_configure_plugin().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ mob_req_cb_candidatefollower()

MobilityRequestResponse platoon_strategic_ihp::PlatoonStrategicIHPPlugin::mob_req_cb_candidatefollower ( const carma_v2x_msgs::msg::MobilityRequest & msg )

private

Function to process mobility request in candidate follower state.

Parameters

msg	incoming mobility request

Returns: ACK, NACK, or No response

Definition at line 1423 of file platoon_strategic_ihp.cpp.

    {
        // This state does not handle any mobility request for now
        // TODO Maybe it should handle some ABORT request from a waiting leader
        RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "Received mobility request with type " << msg.plan_type.type << " but ignored.");
        return MobilityRequestResponse::NO_RESPONSE;
    }

References platoon_strategic_ihp::NO_RESPONSE.

Referenced by handle_mob_req().

Here is the caller graph for this function:

◆ mob_req_cb_candidateleader()

MobilityRequestResponse platoon_strategic_ihp::PlatoonStrategicIHPPlugin::mob_req_cb_candidateleader ( const carma_v2x_msgs::msg::MobilityRequest & msg )

private

Function to process mobility request in candidateleader state.

Parameters

msg	incoming mobility request.

Returns: ACK, NACK, or No response.

Definition at line 1830 of file platoon_strategic_ihp.cpp.

    {
        bool isTargetVehicle = msg.m_header.sender_id == pm_.current_plan.peerId; // need to check: senderID (old leader)
        bool isCandidateJoin = msg.plan_type.type == carma_v2x_msgs::msg::PlanType::PLATOON_FRONT_JOIN;
 
        lanelet::BasicPoint2d incoming_pose = ecef_to_map_point(msg.location);
        double obj_cross_track = wm_->routeTrackPos(incoming_pose).crosstrack;
        bool inTheSameLane = abs(obj_cross_track - current_crosstrack_) < config_.maxCrosstrackError;
        RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "current_cross_track error = " << abs(obj_cross_track - current_crosstrack_));
        RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "obj_cross_track = " << obj_cross_track);
        RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "current_crosstrack_ = " << current_crosstrack_);
        RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "inTheSameLane = " << inTheSameLane);
        RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "isTargetVehicle = " << isTargetVehicle);
        RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "isCandidateJoin = " << isCandidateJoin);
        if (isCandidateJoin && inTheSameLane  &&  isTargetVehicle)
        {
            RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "Old platoon leader " << pm_.current_plan.peerId << " has agreed to joining.");
            RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "Changing to PlatoonLeaderState and send ACK to the previous leader vehicle");
            pm_.current_platoon_state = PlatoonState::LEADER;
 
            // Clean up planning info
            pm_.clearActionPlan();
            pm_.platoonLeaderID = config_.vehicleID;
 
            // Clean up neighbor platoon info since we just joined it
            pm_.resetNeighborPlatoon();
 
            return MobilityRequestResponse::ACK;
        }
        else
        {
            RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "Received platoon request with vehicle id = " << msg.m_header.sender_id);
            RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "The request type is " << msg.plan_type.type << " and we choose to ignore");
            pm_.clearActionPlan();
            pm_.resetHostPlatoon(); //ASSUMES host is a solo joiner
 
            // return to leader state as a solo vehicle
            pm_.current_platoon_state = PlatoonState::LEADER;
            return MobilityRequestResponse::NACK;
        }
    }

Referenced by handle_mob_req().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ mob_req_cb_follower()

MobilityRequestResponse platoon_strategic_ihp::PlatoonStrategicIHPPlugin::mob_req_cb_follower ( const carma_v2x_msgs::msg::MobilityRequest & msg )

private

Function to process mobility request in follower state.

Parameters

msg	incoming mobility request

Returns: ACK, NACK, or No response

For cut-in join, the gap rear vehicle need to slow down once they received the request from platoon leader. Note:1. (cut-in) When joiner is in position, the leader will send the request to relevent platoon member. So no need to check for "in-position" one more time.

(cut-in) If a new member is added in middle, "mob_op_cb_STATUS" will update and resort all vehicle information. So the ordering of the platoon can be updated.
(depart) If the current leader is departing, the closest follower (i.e., 2nd in platoon) will become the new leader, and switch to candidate follower state. While the previous leader depart and operating in single leader state.

Definition at line 1431 of file platoon_strategic_ihp.cpp.

    {
        carma_v2x_msgs::msg::PlanType plan_type = msg.plan_type;
        std::string reccipientID = msg.m_header.recipient_id;
        std::string reqSenderID = msg.m_header.sender_id;
 
        // Check joining plan type.
        bool isCutInJoin = plan_type.type == carma_v2x_msgs::msg::PlanType::PLATOON_CUT_IN_JOIN;
        bool isGapCreated = plan_type.type == carma_v2x_msgs::msg::PlanType::STOP_CREATE_GAP;
        // TODO: Place holder for departure
 
        // Check if host is intended recipient
        bool isHostRecipent = pm_.getHostStaticID() == reccipientID;
 
        if (isCutInJoin && isHostRecipent)
        {
            // Read requesting vehicle's joining index
            std::string strategyParams = msg.strategy_params;
            std::vector<std::string> inputsParams;
            boost::algorithm::split(inputsParams, strategyParams, boost::is_any_of(","));
            std::vector<std::string> join_index_parsed;
            boost::algorithm::split(join_index_parsed, inputsParams[5], boost::is_any_of(":"));
            int req_sender_join_index = std::stoi(join_index_parsed[1]);
            RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "Requesting join_index parsed: " << req_sender_join_index);
 
            // Control vehicle speed based on cut-in type
            // 1. cut-in from rear
            if (static_cast<size_t>(req_sender_join_index) == pm_.host_platoon_.size()-1)
            {
                // Accept plan and idle (becasue rear join, gap leading vehicle do not need to slow down).
                RCLCPP_WARN(rclcpp::get_logger("platoon_strategic_ihp"),"Requested cut-in from rear, start approve cut-in and wait for lane change.");
                RCLCPP_WARN(rclcpp::get_logger("platoon_strategic_ihp"),"Due to the rear join nature, there is no need to slow down or create gap.");
                return MobilityRequestResponse::ACK;
 
            }
            // 2. cut-in from middle
            else if (req_sender_join_index >= 0  &&  static_cast<size_t>(req_sender_join_index) < pm_.host_platoon_.size()-1)
            {
                // Accept plan and slow down to create gap.
                pm_.isCreateGap = true;
                RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "Requested cut-in index is: " << req_sender_join_index << ", approve cut-in and start create gap.");
                return MobilityRequestResponse::ACK;
            }
            // 3. Abnormal join index
            else
            {
                // Note: Leader will abort plan if reponse is not ACK for plantype "PLATOON_CUT_IN_JOIN".
                RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "Abnormal cut-in index, abort operation.");
                return MobilityRequestResponse::NACK;
            }
        }
 
 
        // 4. Reset to normal speed once the gap is created.
        else if (isGapCreated)
        {
            RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "Gap is created, revert to normal operating speed.");
            // Only reset create gap indicator, no need to send response.
            pm_.isCreateGap = false;
            return MobilityRequestResponse::NO_RESPONSE;
        }
 
        // 5. Place holder for departure (host is follower)
 
        // 6. Same-lane join, no need to respond.
        else
        {
            return MobilityRequestResponse::NO_RESPONSE;
        }
    }

References platoon_strategic_ihp::ACK, platoon_strategic_ihp::PlatoonManager::getHostStaticID(), platoon_strategic_ihp::PlatoonManager::host_platoon_, platoon_strategic_ihp::PlatoonManager::isCreateGap, platoon_strategic_ihp::NACK, platoon_strategic_ihp::NO_RESPONSE, pm_, and process_traj_logs::split.

Referenced by handle_mob_req().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ mob_req_cb_leader()

MobilityRequestResponse platoon_strategic_ihp::PlatoonStrategicIHPPlugin::mob_req_cb_leader ( const carma_v2x_msgs::msg::MobilityRequest & msg )

private

Function to process mobility request in leader state.

Parameters

msg	incoming mobility request

Returns: ACK, NACK, or No response

UCLA implementation note:

Here the mobility requests of joining platoon (front and rear) get processed and responded.
1. The host is the leader of the existing platoon or single vehicle in default leader state.
2. Request sender is the joiner.
3. when two single vehicle meet, only allow backjoin.

Note: For front and rear jon, the JOIN_PARAMS format: JOIN_PARAMS| --> "SIZE:%1%,SPEED:%2%,ECEFX:%3%,ECEFY:%4%,ECEFZ:%5%,JOINIDX:%6%" |----—0---— –1------—2------—3------—4-------—5----—|

Note: JOIN_FROM_FRONT indicate a same-lane front join. JOIN_PLATOON_AT_REAR indicate a same-lane rear join. PLATOON_CUT_IN_JOIN indicate a cut-in join, which include three cut-in methods: cut-in front, cut-in middle, and cut-in rear.

Definition at line 1574 of file platoon_strategic_ihp.cpp.

    {
        // Check joining plan type.
        carma_v2x_msgs::msg::PlanType plan_type = msg.plan_type;
        bool isFrontJoin = plan_type.type == carma_v2x_msgs::msg::PlanType::JOIN_PLATOON_FROM_FRONT;
        bool isRearJoin = plan_type.type == carma_v2x_msgs::msg::PlanType::JOIN_PLATOON_AT_REAR;
        bool isCutInJoin = plan_type.type == carma_v2x_msgs::msg::PlanType::PLATOON_CUT_IN_JOIN;
        bool isDepart = (plan_type.type == carma_v2x_msgs::msg::PlanType::PLATOON_DEPARTURE);
 
        // Ignore the request if we are already working with a join/departure process or if no join type was requested (prevents multiple applicants)
        if (isFrontJoin  ||  isRearJoin  ||  isCutInJoin  ||  isDepart)
        {
            if (pm_.current_plan.valid){
                RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "Ignoring incoming request since we are already negotiating a join.");
                return MobilityRequestResponse::NO_RESPONSE; //TODO: replace with NACK that indicates to ask me later
            }
        }
        else
        {
            RCLCPP_WARN_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "Received request with bogus message type " << plan_type.type << "; ignoring");
            return MobilityRequestResponse::NO_RESPONSE;
        }
 
        // TODO: Generalize - We currently ignore the lane information for now and assume the applicant is in the same lane with us.
        // Determine intra-platoon conditions
        // We are currently checking two basic JOIN conditions:
        //     1. The size limitation on current platoon based on the plugin's parameters.
        //     2. Calculate how long that vehicle can be in a reasonable distance to actually join us.
        carma_v2x_msgs::msg::MobilityHeader msgHeader = msg.m_header;
        std::string params = msg.strategy_params;
        std::string applicantId = msgHeader.sender_id;
        RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "Received mobility JOIN request from " << applicantId << " and PlanId = " << msgHeader.plan_id);
        RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "The strategy parameters are " << params);
        if (params.length() == 0)
        {
            RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "The strategy parameters are empty, return no response");
            return MobilityRequestResponse::NO_RESPONSE;
        }
 
        // The incoming message is "mobility Request", which has a location category.
        std::vector<std::string> inputsParams;
        boost::algorithm::split(inputsParams, params, boost::is_any_of(","));
 
        // Parse applicantSize
        std::vector<std::string> applicantSize_parsed;
        boost::algorithm::split(applicantSize_parsed, inputsParams[0], boost::is_any_of(":"));
        int applicantSize = std::stoi(applicantSize_parsed[1]);
        RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "applicantSize: " << applicantSize);
 
        // Parse applicant Current Speed in m/s
        std::vector<std::string> applicantCurrentSpeed_parsed;
        boost::algorithm::split(applicantCurrentSpeed_parsed, inputsParams[1], boost::is_any_of(":"));
        double applicantCurrentSpeed = std::stod(applicantCurrentSpeed_parsed[1]);
        RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "applicantCurrentSpeed: " << applicantCurrentSpeed);
 
        // Calculate downtrack (m) based on incoming pose.
        lanelet::BasicPoint2d incoming_pose = ecef_to_map_point(msg.location);
        double applicantCurrentDtd = wm_->routeTrackPos(incoming_pose).downtrack;
        RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "applicantCurrentmemberUpdates from ecef pose: " << applicantCurrentDtd);
 
        // Calculate crosstrack (m) based on incoming pose.
        double applicantCurrentCtd = wm_->routeTrackPos(incoming_pose).crosstrack;
        RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "applicantCurrentCtd from ecef pose: " << applicantCurrentCtd);
        bool isInLane = abs(applicantCurrentCtd - current_crosstrack_) < config_.maxCrosstrackError;
        RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "isInLane = " << isInLane);
 
        // Check if we have enough room for that applicant
        int currentPlatoonSize = pm_.getHostPlatoonSize();
        bool hasEnoughRoomInPlatoon = applicantSize + currentPlatoonSize <= config_.maxPlatoonSize;
 
        // rear join; platoon leader --> leader waiting
        if (isRearJoin)
        {
            // Log the request  type
            RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "The received mobility JOIN request from " << applicantId << " and PlanId = " << msgHeader.plan_id << " is a same-lane REAR-JOIN request !");
 
            // -- core condition to decided accept joiner or not
            if (hasEnoughRoomInPlatoon && isInLane)
            {
                RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "The current platoon has enough room for the applicant with size " << applicantSize);
                double currentRearDtd = pm_.getPlatoonRearDowntrackDistance();
                RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "The current platoon rear dtd is " << currentRearDtd);
                double currentGap = currentRearDtd - applicantCurrentDtd - config_.vehicleLength;
                double currentTimeGap = currentGap / applicantCurrentSpeed;
                RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "The gap between current platoon rear and applicant is " << currentGap << "m or " << currentTimeGap << "s");
                if (currentGap < config_.minAllowedJoinGap)
                {
                    RCLCPP_WARN(rclcpp::get_logger("platoon_strategic_ihp"),"We should not receive any request from the vehicle in front of us. NACK it.");
                    return MobilityRequestResponse::NACK;
                }
 
                // Check if the applicant can join based on max timeGap/gap
                bool isDistanceCloseEnough = currentGap <= config_.maxAllowedJoinGap  ||  currentTimeGap <= config_.maxAllowedJoinTimeGap;
                if (isDistanceCloseEnough)
                {
                    RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "The applicant is close enough and we will allow it to try to join");
                    RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "Change to LeaderWaitingState and waiting for " << msg.m_header.sender_id << " to join");
 
                    // change state to leaderwaiting !
                    pm_.current_platoon_state = PlatoonState::LEADERWAITING;
                    waitingStartTime = timer_factory_->now().nanoseconds() / 1000000;
                    pm_.current_plan = ActionPlan(true, waitingStartTime, msgHeader.plan_id, applicantId);
                    pm_.platoonLeaderID = pm_.HostMobilityId;
                    return MobilityRequestResponse::ACK;
                }
                else
                {
                    RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "The applicant is too far away from us. NACK.");
                    return MobilityRequestResponse::NACK; //TODO: add reason & request to try again
                }
            }
            else
            {
                RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "The current platoon does not have enough room for applicant of size " << applicantSize << ". NACK");
                return MobilityRequestResponse::NACK;
            }
        }
 
        // front join; platoon leader --> leader aborting
        else if (isFrontJoin)
        {
            // Log the request  type
            RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "The received mobility JOIN request from " << applicantId << " and PlanId = " << msgHeader.plan_id << " is a same-lane FRONT-JOIN request !");
 
            // -- core condition to decided accept joiner or not
            if (hasEnoughRoomInPlatoon && isInLane)
            {
                RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "The current platoon has enough room for the applicant with size " << applicantSize);
 
                // UCLA: change to read platoon front info
                double currentFrontDtd = pm_.getPlatoonFrontDowntrackDistance();
                RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "The current platoon front dtd is " << currentFrontDtd);
                // UCLA: adjust for calculating gap between new leader and old leader
                double currentGap =  applicantCurrentDtd - currentFrontDtd - config_.vehicleLength;
                double currentTimeGap = currentGap / applicantCurrentSpeed;
                RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "The gap between current platoon front and applicant is " << currentGap << "m or " << currentTimeGap << "s");
 
                if (currentGap < config_.minAllowedJoinGap)
                {
                    RCLCPP_WARN(rclcpp::get_logger("platoon_strategic_ihp"),"The current time gap is not suitable for frontal join. NACK it.");
                    return MobilityRequestResponse::NACK;
                }
 
                // Check if the applicant can join based on max timeGap/gap
                bool isDistanceCloseEnough = currentGap <= config_.maxAllowedJoinGap  ||  currentTimeGap <= config_.maxAllowedJoinTimeGap;
 
                // UCLA: add condition: only allow front join when host platoon size >= 2 (make sure when two single vehicle join, only use back join)
                bool isPlatoonNotSingle = pm_.getHostPlatoonSize() >= 2 || config_.test_front_join;
 
                if (isDistanceCloseEnough && isPlatoonNotSingle)
                {
                    RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "The applicant is close enough for frontal join, send acceptance response");
                    RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "Change to LeaderAborting state and waiting for " << msg.m_header.sender_id << " to join as the new platoon leader");
 
                    // ----------------- give up leader position and look for new leader --------------------------
 
                    // adjust for frontal join. Platoon info is related to the platoon at back of the candidate leader vehicle.
                    // Don't want an action plan here
                    pm_.current_platoon_state = PlatoonState::LEADERABORTING;
                    candidatestateStartTime = timer_factory_->now().nanoseconds() / 1000000;
 
                    // If we are testing with a single vehicle representing this platoon, then we don't yet have a platoon ID,
                    // so use the ID for the proposed joining action plan
                    if (config_.test_front_join)
                    {
                        pm_.currentPlatoonID = msgHeader.plan_id;
                    }
 
                    // Store the leader ID as that of the joiner to allow run_leader_aborting to work correctly
                    pm_.platoonLeaderID = applicantId;
 
                    waitingStartTime = timer_factory_->now().nanoseconds() / 1000000;
                    pm_.current_plan.valid = false;
                    return MobilityRequestResponse::ACK;
                }
                else
                {
                    RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "The joining gap (" << currentGap << " m) is too far away from us or the target platoon size (" << pm_.getHostPlatoonSize() << ") is one. NACK.");
                    return MobilityRequestResponse::NACK;  //TODO: add reason & request to try again
                }
            }
            else
            {
                RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "The current platoon does not have enough room for applicant of size " << applicantSize << ". NACK");
                return MobilityRequestResponse::NACK;
            }
        }
 
        // UCLA: conditions for cut-in join; platoon leader --> leading with operation
        else if (isCutInJoin)
        {
            // Log the request  type
            RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "The received mobility JOIN request from " << applicantId << " and PlanId = " << msgHeader.plan_id << " is a CUT-IN-JOIN request !");
 
            // -- core condition to decided accept joiner or not. It is necessary leader only process the first cut-in joining request.
            // Note: The host is the platoon leader, need to use a different method to determine if joining vehicle is nearby.
            if (hasEnoughRoomInPlatoon && isJoiningVehicleNearPlatoon(applicantCurrentDtd, applicantCurrentCtd))
            {
                RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "The current platoon has enough room for the applicant with size " << applicantSize);
                RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "The applicant is close enough for cut-in join, send acceptance response");
                RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "Change to Leading with operation state and waiting for " << msg.m_header.sender_id << " to change lane");
                // change state to lead with operation
                pm_.current_platoon_state = PlatoonState::LEADWITHOPERATION;
                waitingStartTime = timer_factory_->now().nanoseconds() / 1000000;
                pm_.current_plan = ActionPlan(true, waitingStartTime, msgHeader.plan_id, applicantId);
                pm_.platoonLeaderID = pm_.HostMobilityId;
                return MobilityRequestResponse::ACK;
            }
            else
            {
                RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "The current platoon does not have enough room or the applicant is too far away from us. NACK the request.");
                RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "The current applicant size: " << applicantSize << ".");
                RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "The applicant downtrack is: " << current_downtrack_ << ".");
                RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "The applicant crosstrack is: " << current_crosstrack_ << ".");
                return MobilityRequestResponse::NACK;
            }
        }
 
        // TODO: Place holder for deaprture.
 
        // no response
        else
        {
            RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "Received mobility request with type " << msg.plan_type.type << " and ignored.");
            return MobilityRequestResponse::NO_RESPONSE;
        }
    }

Referenced by handle_mob_req().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ mob_req_cb_leaderaborting()

MobilityRequestResponse platoon_strategic_ihp::PlatoonStrategicIHPPlugin::mob_req_cb_leaderaborting ( const carma_v2x_msgs::msg::MobilityRequest & msg )

private

Function to process mobility request in leaderaborting state.

Parameters

msg	incoming mobility request.

Returns: ACK, NACK, or No response.

Definition at line 1821 of file platoon_strategic_ihp.cpp.

    {
        // This state does not handle any mobility request for now
        // TODO Maybe it should handle some ABORT request from a candidate leader
        RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "Received mobility request with type " << msg.plan_type.type << " but ignored.");
        return MobilityRequestResponse::NO_RESPONSE;
    }

References platoon_strategic_ihp::NO_RESPONSE.

Referenced by handle_mob_req().

Here is the caller graph for this function:

◆ mob_req_cb_leaderwaiting()

MobilityRequestResponse platoon_strategic_ihp::PlatoonStrategicIHPPlugin::mob_req_cb_leaderwaiting ( const carma_v2x_msgs::msg::MobilityRequest & msg )

private

Function to process mobility request in leader waiting state.

Parameters

msg	incoming mobility request

Returns: ACK, NACK, or No response

Definition at line 1513 of file platoon_strategic_ihp.cpp.

    {
        bool isTargetVehicle = msg.m_header.sender_id == pm_.current_plan.peerId;
        bool isCandidateJoin = msg.plan_type.type == carma_v2x_msgs::msg::PlanType::PLATOON_FOLLOWER_JOIN;
 
        lanelet::BasicPoint2d incoming_pose = ecef_to_map_point(msg.location);
        double obj_cross_track = wm_->routeTrackPos(incoming_pose).crosstrack;
        bool inTheSameLane = abs(obj_cross_track - current_crosstrack_) < config_.maxCrosstrackError;
        RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "current_cross_track error = " << abs(obj_cross_track - current_crosstrack_));
        RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "inTheSameLane = " << inTheSameLane);
 
        // If everything is agreeable then approve the request; if it is from an unexpected vehicle or
        // is not a candidate join request, then we can just ignore it with no action
        MobilityRequestResponse response = MobilityRequestResponse::NO_RESPONSE;
        if (isTargetVehicle && isCandidateJoin)
        {
            if (inTheSameLane)
            {
                RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "Target vehicle " << pm_.current_plan.peerId << " is actually joining.");
                RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "Changing to PlatoonLeaderState and send ACK to target vehicle");
 
                // Change state to LEADER
                pm_.current_platoon_state = PlatoonState::LEADER;
 
                // If we are not already in a platoon, then use this activity plan ID for the newly formed platoon
                if (pm_.currentPlatoonID.compare(pm_.dummyID) == 0)
                {
                    pm_.currentPlatoonID = msg.m_header.plan_id;
                }
 
                // Add the joiner to our platoon record (ASSUMES that we have not yet received a mob_op STATUS msg from joiner!)
                PlatoonMember newMember = PlatoonMember();
                newMember.staticId = msg.m_header.sender_id;
                newMember.vehiclePosition = wm_->routeTrackPos(incoming_pose).downtrack;
                RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "New member being added to platoon vector whose size is currently " << pm_.host_platoon_.size());
                pm_.host_platoon_.push_back(newMember);
                RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "pm_ now thinks platoon size is " << pm_.getHostPlatoonSize());
 
                // Send approval of the request
                response = MobilityRequestResponse::ACK;
                // Indicate the current join activity is complete
                pm_.clearActionPlan();
            }
            else //correct vehicle and intent, but it's in the wrong lane
            {
                RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "Received platoon request with vehicle id = " << msg.m_header.sender_id << " but in wrong lane. NACK");
                response = MobilityRequestResponse::NACK;
 
                // // Remove the candidate joiner from the platoon structure
                // if (!pm_.removeMemberById(msg.m_header.sender_id))
                // {
                //     RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "Failed to remove candidate joiner from platoon record: " << msg.m_header.sender_id);
                // }
            }
        }
 
 
        return response;
    }

Referenced by handle_mob_req().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ mob_req_cb_leadwithoperation()

MobilityRequestResponse platoon_strategic_ihp::PlatoonStrategicIHPPlugin::mob_req_cb_leadwithoperation ( const carma_v2x_msgs::msg::MobilityRequest & msg )

private

Function to process mobility request in leadwithoperation state (cut-in join)

Parameters

msg	incoming mobility request

Returns: ACK, NACK, or No response

Definition at line 1873 of file platoon_strategic_ihp.cpp.

    {
        /*
        *   Current leader change state to lead with opertaion once the cut-in join request is accepeted.
            For cut-in front, the leading vehicle will open gap for the joining vehicle.
            For cut-in middle, the leading vechile will request gap following vehcile to create gap.
            The host leading vheicle will also send lane cut-in approval response to the joining vehicle.
            The host leading vehicle will change to same-lane state once the lanechange completion plan was recieved.
 
        */
 
        // Check request plan type
        carma_v2x_msgs::msg::PlanType plan_type = msg.plan_type;
        std::string strategyParams = msg.strategy_params;
        std::string reqSenderID = msg.m_header.sender_id;
 
        // Calculate downtrack (m) based on ecef.
        lanelet::BasicPoint2d incoming_pose = ecef_to_map_point(msg.location);
        // read downtrack
        double applicantCurrentDtd = wm_->routeTrackPos(incoming_pose).downtrack;
        RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "Applicant downtrack from ecef pose: " << applicantCurrentDtd);
 
        // Read requesting join index
        std::vector<std::string> inputsParams;
        boost::algorithm::split(inputsParams, strategyParams, boost::is_any_of(","));
 
        std::vector<std::string> join_index_parsed;
        boost::algorithm::split(join_index_parsed, inputsParams[5], boost::is_any_of(":"));
        int req_sender_join_index = std::stoi(join_index_parsed[1]);
        RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "Requesting join_index parsed: " << req_sender_join_index);
 
        if (plan_type.type == carma_v2x_msgs::msg::PlanType::PLATOON_CUT_IN_JOIN)
        {
            // Send response
            // ----- CUT-IN front -----
            if (req_sender_join_index == -1)
            {
                // determine if joining vehicle in position for cut-in front - this value must be > 0
                double cutinDtdDifference = applicantCurrentDtd - current_downtrack_ - config_.vehicleLength;
                // check dtd between current leader (host) and joining vehicle is far enough away to avoid a collision
                bool isFrontJoinerInPosition = cutinDtdDifference >= 1.5*config_.vehicleLength;
 
                if (isFrontJoinerInPosition)
                {
                    RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "The joining vehicle is cutting in from front.  Gap is already sufficient.");
                    return MobilityRequestResponse::ACK;
                }
                else if (cutinDtdDifference > 0.0  &&  cutinDtdDifference < 1.5*config_.vehicleLength)
                {
                    // slow down leader to allow joiner cut-in
                    pm_.isCreateGap = true;
                    RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "The joining vehicle is cutting in from front.");
                    RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "Host (leader) slow down notified, joining vehicle can prepare to join");
                    return MobilityRequestResponse::ACK;
                }
 
                else
                {
                    RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "Front join geometry violation. NACK.  cutinDtdDifference = " << cutinDtdDifference);
                    pm_.current_platoon_state = PlatoonState::LEADER;
                    return MobilityRequestResponse::NACK;
                }
            }
 
            // ----- CUT-IN rear -----
            else if (static_cast<size_t>(req_sender_join_index) == pm_.host_platoon_.size()-1)
            {
                // determine if joining vehicle in position for cut-in rear
                // To pass, the joining vehicle need to be behind the last member, within three vehicle length.
                //TODO: should it also test CTE to ensure vehicle is in same lane?
                double platoonEndVehicleDtd = pm_.host_platoon_[pm_.host_platoon_.size()-1].vehiclePosition - config_.vehicleLength;
                double rearGap = platoonEndVehicleDtd - applicantCurrentDtd;
                bool isRearJoinerInPosition = rearGap >= 0  &&  rearGap <= config_.maxCutinGap;//3*config_.vehicleLength; TODO: temporary increase
 
                if (isRearJoinerInPosition)
                {
                    RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "Published Mobility cut-in-rear-Join request to relavent platoon member, host is leader.");
                    RCLCPP_WARN(rclcpp::get_logger("platoon_strategic_ihp"),"Published Mobility cut-in-rear-Join request to relavent platoon members to signal gap creation.");
                    pm_.isCreateGap = true;
                    return MobilityRequestResponse::ACK;
                }
                else
                {
                    RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "Rear join geometry violation. NACK. rearGap = " << rearGap);
                    pm_.current_platoon_state = PlatoonState::LEADER;
                    return MobilityRequestResponse::NACK;
                }
            }
            // ----- CUT-IN middle -----
            else //any other join_index value
            {
                // determine if joining vehicle in position for cut-in mid
                // To pass, the joining vehicle should be in front of the gap following vehicle, within three vehicle length.
                double gapFollowingVehicleDtd = pm_.host_platoon_[req_sender_join_index].vehiclePosition;
                double gapFollowerDiff = applicantCurrentDtd - gapFollowingVehicleDtd;
                bool isMidJoinerInPosition = gapFollowerDiff >=0  &&  gapFollowerDiff <= 3*config_.vehicleLength;
 
                // Task 4: send request to index member to slow down (i.e., set isCreateGap to true)
                // Note: Request recieving vehicle set pm_.isCreateGap
                if (isMidJoinerInPosition)
                {
                    // compose request (Note: The recipient should be the gap following vehicle.)
                    carma_v2x_msgs::msg::MobilityRequest request;
                    request.m_header.plan_id = boost::uuids::to_string(boost::uuids::random_generator()());
                    // Note: For cut-in mid, notify gap rear member to create/increase gap.
                    std::string recipient_ID = pm_.host_platoon_[req_sender_join_index+1].staticId;
                    request.m_header.sender_id = config_.vehicleID;
                    request.m_header.timestamp = timer_factory_->now().nanoseconds() / 1000000;;
                    // UCLA: add plan type, add this in cav_mwgs/plan_type
                    request.plan_type.type = carma_v2x_msgs::msg::PlanType::PLATOON_CUT_IN_JOIN;
                    request.strategy = PLATOONING_STRATEGY;
 
                    double platoon_size = pm_.getHostPlatoonSize();
 
                    boost::format fmter(JOIN_PARAMS);   // Note: Front and rear join uses same params, hence merge to one param for both condition.
                    fmter %platoon_size;                //  index = 0
                    fmter %current_speed_;              //  index = 1, in m/s
                    fmter %pose_ecef_point_.ecef_x;     //  index = 2
                    fmter %pose_ecef_point_.ecef_y;     //  index = 3
                    fmter %pose_ecef_point_.ecef_z;     //  index = 4
                    fmter %req_sender_join_index;       //  index = 5
 
                    request.strategy_params = fmter.str();
                    request.urgency = 50;
                    request.location = pose_to_ecef(pose_msg_);
                    // note: for rear join, cut-in index == host_platoon_.size()-1; for join from front, index == -1
                    //       for cut-in in middle, index indicate the gap leading vehicle's index
                    mobility_request_publisher_(request);
                    RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "Published Mobility cut-in-mid-Join request to relavent platoon members to signal gap creation, host is leader.");
                    RCLCPP_WARN(rclcpp::get_logger("platoon_strategic_ihp"),"Published Mobility cut-in-mid-Join request to relavent platoon members to signal gap creation.");
                    RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "The joining vehicle is cutting in at index: "<< req_sender_join_index <<". Notify gap rear vehicle with ID: " << recipient_ID << " to slow down");
                    return MobilityRequestResponse::ACK;
                }
                else
                {
                    RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "Mid join geometry violation. NACK. gapFollwerDiff = " << gapFollowerDiff);
                    pm_.current_platoon_state = PlatoonState::LEADER;
                    return MobilityRequestResponse::NACK;
                }
            }
        }
 
        // task 2: For cut-in from front, the leader need to stop creating gap
        else if (plan_type.type == carma_v2x_msgs::msg::PlanType::STOP_CREATE_GAP)
        {
            // reset create gap indicator
            pm_.isCreateGap = false;
            // no need to response, simple reset the indicator
            return MobilityRequestResponse::NO_RESPONSE;
        }
 
        // task 3 cut-in front: After creating gap, revert back to same-lane operation
        else if (plan_type.type == carma_v2x_msgs::msg::PlanType::CUT_IN_FRONT_DONE)
        {
            RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "Cut-in from front lane change finished, leader revert to same-lane maneuver.");
            pm_.current_platoon_state = PlatoonState::LEADERABORTING;
            candidatestateStartTime = timer_factory_->now().nanoseconds() / 1000000;
            // if testing with two vehicles, use plan id as platoon id
            if (pm_.currentPlatoonID.compare(pm_.dummyID) == 0)
            // if (config_.allowCutinJoin)
            {
                pm_.currentPlatoonID = msg.m_header.plan_id;
            }
            // Store the leader ID as that of the joiner to allow run_leader_aborting to work correctly
            pm_.platoonLeaderID = msg.m_header.sender_id;
            pm_.current_plan.valid = false;
            return MobilityRequestResponse::ACK;
        }
 
        // task 4 cut-in from middle/rear
        else if (plan_type.type == carma_v2x_msgs::msg::PlanType::CUT_IN_MID_OR_REAR_DONE)
        {
            RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "Cut-in from mid/rear lane change finished, leader revert to same-lane maneuver.");
            pm_.current_platoon_state = PlatoonState::LEADERWAITING;
            waitingStartTime = timer_factory_->now().nanoseconds() / 1000000;
            return MobilityRequestResponse::ACK;
        }
 
 
        // task 5: if other joining vehicle send joning request, NACK it since there is already a cut-in join going on.
        else
        {
            RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "CUT-IN join maneuver is already in operation, NACK incoming join requests from other candidates.");
            RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "Plan Type: " << plan_type.type );
            return MobilityRequestResponse::NACK;
        }
 
        // this statement should never be reached, but will ensure reasonable behavior in case of coding error above
        RCLCPP_WARN_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "End of method reached! Apparent logic fault above.");
        RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "End of method reached! Apparent logic fault above."); //since WARN doesn't always print
        return MobilityRequestResponse::NO_RESPONSE;
    }

Referenced by handle_mob_req().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ mob_req_cb_preparetojoin()

MobilityRequestResponse platoon_strategic_ihp::PlatoonStrategicIHPPlugin::mob_req_cb_preparetojoin ( const carma_v2x_msgs::msg::MobilityRequest & msg )

private

Function to process mobility request in preparetojoin state (cut-in join)

Parameters

msg	incoming mobility request

Returns: ACK, NACK, or No response

Definition at line 2067 of file platoon_strategic_ihp.cpp.

    {
        // This state does not handle any mobility request for now
        // TODO: if joining vehicle need to adjust speed, the leader should request it and the request should be handled here.
        RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "Received mobility request with type " << msg.plan_type.type << " but ignored.");
        return MobilityRequestResponse::NO_RESPONSE;
    }

References platoon_strategic_ihp::NO_RESPONSE.

Referenced by handle_mob_req().

Here is the caller graph for this function:

◆ mob_req_cb_standby()

MobilityRequestResponse platoon_strategic_ihp::PlatoonStrategicIHPPlugin::mob_req_cb_standby ( const carma_v2x_msgs::msg::MobilityRequest & msg )

private

Function to process mobility request in standby state.

Parameters

msg	incoming mobility request

Returns: ACK, NACK, or No response

Definition at line 1416 of file platoon_strategic_ihp.cpp.

    {
        // In standby state, the plugin is not responsible for replying to any request messages
        RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "STANDBY state does nothing with msg from " << msg.m_header.sender_id);
        return MobilityRequestResponse::NO_RESPONSE;
    }

References platoon_strategic_ihp::NO_RESPONSE.

Referenced by handle_mob_req().

Here is the caller graph for this function:

◆ mob_resp_cb()

void platoon_strategic_ihp::PlatoonStrategicIHPPlugin::mob_resp_cb ( const carma_v2x_msgs::msg::MobilityResponse::UniquePtr msg )

Callback function for Mobility Response Message.

Parameters

msg	Mobility Response Message

If any of the three condition (i.e., isCurrPlanValid, isForCurrentPlan and isFromTargetVehicle) was not satisfied, return ignore as this message was not intended for the host.

Definition at line 2080 of file platoon_strategic_ihp.cpp.

    {
        // Firstly, check eligibility of the received message.
        bool isCurrPlanValid = pm_.current_plan.valid;                          // Check if current plan is still valid (i.e., not timed out).
        bool isForCurrentPlan = msg->m_header.plan_id == pm_.current_plan.planId;  // Check if plan Id matches.
        bool isFromTargetVehicle = msg->m_header.sender_id == pm_.current_plan.peerId;  // Check if expected peer ID and sender ID matches.
        RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "mob_resp_cb: isCurrPlanValid = " << isCurrPlanValid << ", isForCurrentPlan = " <<
                        isForCurrentPlan << ", isFromTargetVehicle = " << isFromTargetVehicle);
        RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "sender ID = " << msg->m_header.sender_id << ", current peer ID = " << pm_.current_plan.peerId);
        RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "incoming plan ID = " << msg->m_header.plan_id << "current plan ID = " << pm_.current_plan.planId);
 
        if (!(isCurrPlanValid && isForCurrentPlan && isFromTargetVehicle))
        {
            RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), " Ignore the received response message as it was not intended for the host vehicle.");
            return;
        }
        else if (pm_.current_platoon_state == PlatoonState::LEADER)
        {
            mob_resp_cb_leader(*msg);
        }
        else if (pm_.current_platoon_state == PlatoonState::FOLLOWER)
        {
            mob_resp_cb_follower(*msg);
        }
        else if (pm_.current_platoon_state == PlatoonState::CANDIDATEFOLLOWER)
        {
            mob_resp_cb_candidatefollower(*msg);
        }
        else if (pm_.current_platoon_state == PlatoonState::LEADERWAITING)
        {
            mob_resp_cb_leaderwaiting(*msg);
        }
        else if (pm_.current_platoon_state == PlatoonState::STANDBY)
        {
            mob_resp_cb_standby(*msg);
        }
        // UCLA: add leader aboorting
        else if (pm_.current_platoon_state == PlatoonState::LEADERABORTING)
        {
            mob_resp_cb_leaderaborting(*msg);
        }
        //UCLA: add candidate leader
        else if (pm_.current_platoon_state == PlatoonState::CANDIDATELEADER)
        {
            mob_resp_cb_candidateleader(*msg);
        }
        // UCLA: add lead with operation for cut-in join
        else if (pm_.current_platoon_state == PlatoonState::LEADWITHOPERATION)
        {
            mob_resp_cb_leadwithoperation(*msg);
        }
        // UCLA: add prepare to join for cut-in join
        else if (pm_.current_platoon_state == PlatoonState::PREPARETOJOIN)
        {
            mob_resp_cb_preparetojoin(*msg);
        }
        // TODO: Place holder for departure.
    }

Referenced by platoon_strategic_ihp::Node::on_configure_plugin().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ mob_resp_cb_candidatefollower()

void platoon_strategic_ihp::PlatoonStrategicIHPPlugin::mob_resp_cb_candidatefollower ( const carma_v2x_msgs::msg::MobilityResponse & msg )

private

Function to process mobility response in candidate follower state.

Parameters

msg	incoming mobility response

Definition at line 2149 of file platoon_strategic_ihp.cpp.

    {
        RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "Callback for candidate follower ");
 
        // Check if current plan is still valid (i.e., not timed out)
        if (pm_.current_plan.valid)
        {
            bool isForCurrentPlan = msg.m_header.plan_id == pm_.current_plan.planId;
            RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "isForCurrentPlan " << isForCurrentPlan);
 
            // Check the response is received correctly (i.e., host vehicle is the desired receiver).
            if (isForCurrentPlan)
            {
                if (msg.is_accepted)
                {
                    // We change to follower state and start to actually follow that leader
                    // The platoon manager also need to change the platoon Id to the one that the target leader is using
                    pm_.current_platoon_state = PlatoonState::FOLLOWER;
                    RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "pm_.currentPlatoonID: " << pm_.currentPlatoonID);
                    RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "pm_.targetPlatoonID: " << pm_.currentPlatoonID);
 
                    if (pm_.targetPlatoonID.compare(pm_.dummyID) != 0)
                    {
                        pm_.currentPlatoonID = pm_.targetPlatoonID;
                        RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "pm_.currentPlatoonID now: " << pm_.currentPlatoonID);
                        pm_.resetNeighborPlatoon();
                    }
                    else
                    {
                        pm_.currentPlatoonID = msg.m_header.plan_id;
                    }
 
                    pm_.changeFromLeaderToFollower(pm_.currentPlatoonID, msg.m_header.sender_id);
                    RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "The leader " << msg.m_header.sender_id << " agreed on our join. Change to follower state.");
                    RCLCPP_WARN(rclcpp::get_logger("platoon_strategic_ihp"),"changed to follower");
                    pm_.clearActionPlan();
                }
                else
                {
                    // We change back to normal leader state and try to join other platoons
                    RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "The leader " << msg.m_header.sender_id << " does not agree on our join. Change back to leader state.");
                    RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "Trying again..");
                    // join plan failed, but we still need the peerid
                    pm_.current_plan.valid = false;
 
                    // Clear out any platooning plan we don't need
                    if (pm_.getHostPlatoonSize() == 1)
                    {
                        pm_.resetHostPlatoon();
                    }
                }
 
            }
            else
            {
                RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "Ignore received response message because it is not for the current plan.");
            }
        }
        else
        {
            RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "Ignore received response message because we are not in any negotiation process.");
        }
    }

Referenced by mob_resp_cb().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ mob_resp_cb_candidateleader()

void platoon_strategic_ihp::PlatoonStrategicIHPPlugin::mob_resp_cb_candidateleader ( const carma_v2x_msgs::msg::MobilityResponse & msg )

private

Function to process mobility response in candidateleader state.

Parameters

msg	incoming mobility response.

Definition at line 2446 of file platoon_strategic_ihp.cpp.

    {
        RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "CANDIDATELEADER state does nothing with msg from " << msg.m_header.sender_id);
    }

Referenced by mob_resp_cb().

Here is the caller graph for this function:

◆ mob_resp_cb_follower()

void platoon_strategic_ihp::PlatoonStrategicIHPPlugin::mob_resp_cb_follower ( const carma_v2x_msgs::msg::MobilityResponse & msg )

private

Function to process mobility response in follower state.

Parameters

msg	incoming mobility response

UCLA Note:

This method was implemented with the purpose of updating the platoon related references (i.e., platoon leader, platoon Id) to the new leader that joined from front. Changing the existing follower to candidate follower state will initiate a member update and therefore point all refernce to the new leader.

For rear join, since the existing follower already following the platoon leader. There is no need to establish communication hence no response will be handled for rear-join.

Definition at line 2223 of file platoon_strategic_ihp.cpp.

    {
        // UCLA: read plan type
        carma_v2x_msgs::msg::PlanType plan_type = msg.plan_type;
 
        // UCLA: determine joining type
        bool isFrontJoin = (plan_type.type == carma_v2x_msgs::msg::PlanType::JOIN_PLATOON_FROM_FRONT);
 
        //TODO: when would this code block ever be used? A normal follower would have to talk to a front joiner.
        // UCLA: add response so follower can change to candidate follower, then change leader
        if (isFrontJoin && msg.is_accepted)
        {
            // if frontal join is accepted, change followers to candidate follower to update leader
            RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "Received positive response for front-join plan id = " << pm_.current_plan.planId);
            RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "Change to CandidateFollower state and prepare to update platoon information");
            // Change to candidate follower state and request a new plan to catch up with the front platoon
            pm_.current_platoon_state = PlatoonState::CANDIDATEFOLLOWER;
            candidatestateStartTime = timer_factory_->now().nanoseconds() / 1000000;
        }
 
        // TODO: Place holder for follower departure.
    }

References platoon_strategic_ihp::CANDIDATEFOLLOWER, candidatestateStartTime, platoon_strategic_ihp::PlatoonManager::current_plan, platoon_strategic_ihp::PlatoonManager::current_platoon_state, platoon_strategic_ihp::ActionPlan::planId, pm_, and timer_factory_.

Referenced by mob_resp_cb().

Here is the caller graph for this function:

◆ mob_resp_cb_leader()

void platoon_strategic_ihp::PlatoonStrategicIHPPlugin::mob_resp_cb_leader ( const carma_v2x_msgs::msg::MobilityResponse & msg )

private

Function to process mobility response in leader state.

Parameters

msg	incoming mobility response

UCLA implementation note: This is where the Mobility response gets processed for leader state.

If the host is a single vehicle in the leader state, then the host vehicle is the joiner vehicle (frontal join: candidate leader; back join: candidate follower), and the response sender is the existing platoon leader (front join: aborting leader, back join: waiting leader).

If the host is the current platoon leader, all three case will be false and no further action is needed.

Disclaimer: Currently, if the host vehicle is platoon leader, there is no further action needed when receiving the mobility response. However, future development may add functions in this mehtod.

Definition at line 2259 of file platoon_strategic_ihp.cpp.

    {
        // UCLA: read plan type
        carma_v2x_msgs::msg::PlanType plan_type = msg.plan_type;
        RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "plan_type.type = " << plan_type.type);
 
        // UCLA: determine joining type
        bool isCutInJoin = plan_type.type == carma_v2x_msgs::msg::PlanType::PLATOON_CUT_IN_JOIN      &&  !config_.test_front_join;
        bool isRearJoin = plan_type.type == carma_v2x_msgs::msg::PlanType::JOIN_PLATOON_AT_REAR      &&  !config_.test_front_join;
        bool isFrontJoin = plan_type.type == carma_v2x_msgs::msg::PlanType::JOIN_PLATOON_FROM_FRONT  ||  config_.test_front_join;
        RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "Joining type: isRearJoin = " << isRearJoin);
        RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "Joining type: isFrontJoin = " << isFrontJoin);
        RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "Joining type: isCutInJoin = " << isCutInJoin);
 
        // Check if current plan is still valid (i.e., not timed out).
        if (pm_.current_plan.valid)
        {
            RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "My plan id = " << pm_.current_plan.planId << " and response plan Id = " << msg.m_header.plan_id);
            RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "Expected peer id = " << pm_.current_plan.peerId << " and response sender Id = " << msg.m_header.sender_id);
 
            // Check the response is received correctly (i.e., host vehicle is the desired receiver).
            if (pm_.current_plan.planId == msg.m_header.plan_id && pm_.current_plan.peerId == msg.m_header.sender_id)
            {
                // rear join
                if (isRearJoin && msg.is_accepted)
                {
                    RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "Received positive response for plan id = " << pm_.current_plan.planId);
                    RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "Change to CandidateFollower state and notify trajectory failure in order to replan");
 
                    // Change to candidate follower state and wait to catch up with the front platoon
                    pm_.current_platoon_state = PlatoonState::CANDIDATEFOLLOWER;
                    candidatestateStartTime = timer_factory_->now().nanoseconds() / 1000000;
                    pm_.current_plan.valid = false; //but leave peerId intact for use in second request
                }
 
                // UCLA: frontal join (candidate leader, inherited from leaderwaiting)
                else if (isFrontJoin && msg.is_accepted)
                {
                    RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "Received positive response for plan id = " << pm_.current_plan.planId);
                    RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "Change to CandidateLeader state and prepare to become new leader. ");
 
                    // Change to candidate leader and idle
                    pm_.current_platoon_state = PlatoonState::CANDIDATELEADER;
                    candidatestateStartTime = timer_factory_->now().nanoseconds() / 1000000;
                    pm_.current_plan.valid = false; //but leave peerId intact for use in second request
 
                    // Set the platoon ID to that of the target platoon even though we haven't yet joined;
                    // for front join this is necessary for the aborting leader to recognize us as an incoming
                    // member (via our published op STATUS messages)
                    pm_.currentPlatoonID = pm_.targetPlatoonID;
                }
 
                // UCLA: CutIn join
                else if (isCutInJoin && msg.is_accepted)
                {
                    RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "Received positive response for plan id = " << pm_.current_plan.planId);
                    RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "Change to Prepare to join state and prepare to change lane. ");
 
                    // Change to candidate leader and idle
                    pm_.current_platoon_state = PlatoonState::PREPARETOJOIN;
                    pm_.neighbor_platoon_leader_id_ = msg.m_header.sender_id;
                    candidatestateStartTime = timer_factory_->now().nanoseconds() / 1000000;
                    pm_.current_plan.valid = false; //but leave peerId intact for use in second request
                }
 
                // Current leader of an actual platoon (to be in this method host is in leader state)
                else if(pm_.getHostPlatoonSize() >= 2)
                {
                    //TODO future: add logic here to allow two platoons to join together
 
                    // Keep the leader idling, since this must be a bogus response
                    RCLCPP_WARN_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "Host received response for joining vehicles, remain idling as the host is a current platoon leader.");
                }
                else
                {
                    RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "Received negative response for plan id = " << pm_.current_plan.planId << ". Resetting plan & platoon info.");
                    // Forget about the previous plan totally
                    pm_.clearActionPlan();
                    pm_.resetHostPlatoon();
                }
            }
            else
            {
                RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "Ignore the response message because planID match: " << (pm_.current_plan.planId == msg.m_header.plan_id));
                RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "My plan id = " << pm_.current_plan.planId << " and response plan Id = " << msg.m_header.plan_id);
                RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "And peer id match " << (pm_.current_plan.peerId == msg.m_header.sender_id));
                RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "Expected peer id = " << pm_.current_plan.peerId << " and response sender Id = " << msg.m_header.sender_id);
            }
        }
    }

Referenced by mob_resp_cb().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ mob_resp_cb_leaderaborting()

void platoon_strategic_ihp::PlatoonStrategicIHPPlugin::mob_resp_cb_leaderaborting ( const carma_v2x_msgs::msg::MobilityResponse & msg )

private

Function to process mobility response in leaderaborting state.

Parameters

msg	incoming mobility response.

UCLA implementation note: This state is the middle state to handle the leader aborting process of the previous platoon leader in a front join scenario. Within this state, the previous leader will check for front joining vehicle's position and will not handle any further mobility requests.

Note: As the previous leader will join the new leader (joined from front), the corresponding join request will be send out by the previos leader.

Definition at line 2365 of file platoon_strategic_ihp.cpp.

    {
        RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "Callback for leader aborting !");
 
        // Check if current plan is still valid (i.e., not timed out).
        if (pm_.current_plan.valid)
        {
            bool isForCurrentPlan = msg.m_header.plan_id == pm_.current_plan.planId;
            bool isForFrontJoin = msg.plan_type.type == carma_v2x_msgs::msg::PlanType::PLATOON_FRONT_JOIN;
 
            if (msg.plan_type.type == carma_v2x_msgs::msg::PlanType::UNKNOWN){
                RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "*** plan type UNKNOWN");
            }else if (msg.plan_type.type == carma_v2x_msgs::msg::PlanType::JOIN_PLATOON_FROM_FRONT){
                RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "*** plan type JOIN_PLATOON_FROM_FRONT");
            }else if (msg.plan_type.type == carma_v2x_msgs::msg::PlanType::PLATOON_CUT_IN_JOIN){
                RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "*** plan type PLATOON_CUT_IN_JOIN");
            }else {
                RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "*** plan type not captured.");
            }
 
            bool isFromTargetVehicle = msg.m_header.sender_id == pm_.current_plan.peerId;
            RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "msg.m_header.sender_id " << msg.m_header.sender_id);
            RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "Plan Type " << msg.plan_type.type);
            RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "isForFrontJoin " << isForFrontJoin);
            RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "isForCurrentPlan " << isForCurrentPlan);
            RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "isFromTargetVehicle " << isFromTargetVehicle);
 
            // Check the response is received correctly (i.e., host vehicle is the desired receiver).
            if (isForCurrentPlan && isFromTargetVehicle && isForFrontJoin)
            {
                if (msg.is_accepted)
                {
                    // We change to follower state and start to actually follow the new leader
                    // The platoon manager also need to change the platoon Id to the one that the target leader is using
                    pm_.current_platoon_state = PlatoonState::FOLLOWER;
                    pm_.changeFromLeaderToFollower(pm_.currentPlatoonID, msg.m_header.sender_id);
                    RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "The new leader " << msg.m_header.sender_id << " agreed on the frontal join. Change to follower state.");
                    RCLCPP_WARN(rclcpp::get_logger("platoon_strategic_ihp"),"changed to follower");
 
                    // reset leader aborting request marker
                    numLeaderAbortingCalls_ = 0;
                }
                else
                {
                    // We change back to normal leader state
                    RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "The new leader " << msg.m_header.sender_id << " does not agree on the frontal join. Change back to leader state.");
                    pm_.current_platoon_state = PlatoonState::LEADER;
                    // We were already leading a platoon, so don't erase any of that info. But we need to remove the erstwhile candidate
                    // leader from our platoon roster; we know it is in position 0, so just remove that element
                    if (!pm_.removeMember(0))
                    {
                        RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "Failed to remove candidate leader from the platoon!");
                    }
                }
 
                // Clean up the joining plan
                pm_.clearActionPlan();
            }
            else
            {
                RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "Ignore received response message because it is not for the current plan.");
            }
        }
        else
        {
            RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "Ignore received response message because we are not in any negotiation process.");
        }
    }

Referenced by mob_resp_cb().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ mob_resp_cb_leaderwaiting()

void platoon_strategic_ihp::PlatoonStrategicIHPPlugin::mob_resp_cb_leaderwaiting ( const carma_v2x_msgs::msg::MobilityResponse & msg )

private

Function to process mobility response in leader waiting state.

Parameters

msg	incoming mobility response

Leader waiting is the state to check joining vehicle is in proper position and to prevent platoon leader from receiving messages from other CAVs in leader state. There was no response involved in this state, hence no action needed in this section.

Definition at line 2213 of file platoon_strategic_ihp.cpp.

    {
        RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "LEADERWAITING state does nothing with msg from " << msg.m_header.sender_id);
    }

Referenced by mob_resp_cb().

Here is the caller graph for this function:

◆ mob_resp_cb_leadwithoperation()

void platoon_strategic_ihp::PlatoonStrategicIHPPlugin::mob_resp_cb_leadwithoperation ( const carma_v2x_msgs::msg::MobilityResponse & msg )

private

Function to process mobility response in leadwithoperation state (cut-in join)

Parameters

msg	incoming mobility response

Definition at line 2452 of file platoon_strategic_ihp.cpp.

    {
        RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "LEADWITHOPERATION state does nothing with msg from " << msg.m_header.sender_id);
    }

Referenced by mob_resp_cb().

Here is the caller graph for this function:

◆ mob_resp_cb_preparetojoin()

void platoon_strategic_ihp::PlatoonStrategicIHPPlugin::mob_resp_cb_preparetojoin ( const carma_v2x_msgs::msg::MobilityResponse & msg )

private

Function to process mobility response in preparetojoin state.

Parameters

msg	incoming mobility response

Definition at line 2458 of file platoon_strategic_ihp.cpp.

    {
        /*
            If leader notify the member to slow down and ACK the request,
            start to check the gap and change lane when gap is large enough
        */
 
        carma_v2x_msgs::msg::PlanType plan_type = msg.plan_type;
        bool isCreatingGap = plan_type.type == carma_v2x_msgs::msg::PlanType::PLATOON_CUT_IN_JOIN;
        bool isFinishLaneChangeFront = plan_type.type == carma_v2x_msgs::msg::PlanType::CUT_IN_FRONT_DONE;
        bool isFinishLaneChangeMidorRear = plan_type.type == carma_v2x_msgs::msg::PlanType::CUT_IN_MID_OR_REAR_DONE;
        RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "isCreatingGap = " << isCreatingGap << ", is_neighbor_record_complete = " << pm_.is_neighbor_record_complete_);
 
        if (!msg.is_accepted)
        {
            RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "Request " << msg.m_header.plan_id << " was rejected by leader.");
            RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "Action Plan reset.");
            RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "Trying again....");
            pm_.current_plan.valid = false;
            pm_.current_platoon_state = PlatoonState::LEADER;
            return;
        }
        // UCLA: Create Gap or perform a rear join (no gap creation necessary)
        RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "pm_.is_neighbor_record_complete_ " << pm_.is_neighbor_record_complete_);
        if (isCreatingGap  &&  pm_.is_neighbor_record_complete_)
        {
 
            // task 2: set indicator if gap is safe
            safeToLaneChange_ = true;
            RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "Gap is now sufficiently large.");
            RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "in mob_resp_cb safeToLaneChange_: " << safeToLaneChange_);
 
            // task 3: notify gap-rear vehicle to stop slowing down
            carma_v2x_msgs::msg::MobilityRequest request;
            request.m_header.plan_id = boost::uuids::to_string(boost::uuids::random_generator()());
            request.m_header.recipient_id = pm_.current_plan.peerId;
            request.m_header.sender_id = config_.vehicleID;
            request.m_header.timestamp = timer_factory_->now().nanoseconds() / 1000000;;
            // UCLA: A new plan type to stop creat gap.
            request.plan_type.type = carma_v2x_msgs::msg::PlanType::STOP_CREATE_GAP;
            request.strategy = PLATOONING_STRATEGY;
            request.urgency = 50;
            request.location = pose_to_ecef(pose_msg_);
            double platoon_size = pm_.getHostPlatoonSize();
 
            boost::format fmter(JOIN_PARAMS);
            fmter %platoon_size;                    //  index = 0
            fmter %current_speed_;                  //  index = 1, in m/s
            fmter %pose_ecef_point_.ecef_x;         //  index = 2
            fmter %pose_ecef_point_.ecef_y;         //  index = 3
            fmter %pose_ecef_point_.ecef_z;         //  index = 4
            fmter %target_join_index_;              //  index = 5
 
            request.strategy_params = fmter.str();
            mobility_request_publisher_(request);
            RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "Published Mobility Candidate-Join request to the leader to stop creating gap");
        }
 
        // UCLA: Revert to same-lane for cut-in front
        else if (isFinishLaneChangeFront)
        {
            RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "Cut-in from front lane change finished, the joining vehicle revert to same-lane maneuver.");
            pm_.current_platoon_state = PlatoonState::CANDIDATELEADER;
            candidatestateStartTime = timer_factory_->now().nanoseconds() / 1000000;
            RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "pm_.currentPlatoonID: " << pm_.currentPlatoonID);
            RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "pm_.targetPlatoonID: " << pm_.targetPlatoonID);
            if (pm_.targetPlatoonID.compare(pm_.dummyID) != 0)
            {
                pm_.currentPlatoonID = pm_.targetPlatoonID;
                RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "pm_.currentPlatoonID now: " << pm_.currentPlatoonID);
            }
 
            pm_.current_plan.valid = false; //but leave peerId intact for use in second request
        }
 
        // UCLA: Revert to same-lane operation for cut-in from middle/rear
        else if (isFinishLaneChangeMidorRear)
        {
            RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "Cut-in from mid or rear, the lane change finished, the joining vehicle revert to same-lane maneuver.");
            pm_.current_platoon_state = PlatoonState::CANDIDATEFOLLOWER;
            candidatestateStartTime = timer_factory_->now().nanoseconds() / 1000000;
            pm_.current_plan.valid = false; //but leave peerId intact for use in second request
 
        }
 
        else
        {
            RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "End of mob_resp_cb_preparetojoin");
        }
    }

Referenced by mob_resp_cb().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ mob_resp_cb_standby()

void platoon_strategic_ihp::PlatoonStrategicIHPPlugin::mob_resp_cb_standby ( const carma_v2x_msgs::msg::MobilityResponse & msg )

private

Function to process mobility response in standby state.

Parameters

msg	incoming mobility response

Definition at line 2143 of file platoon_strategic_ihp.cpp.

    {
        // In standby state, it will not send out any requests so it will also ignore all responses
        RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "STANDBY state does nothing with msg from " << msg.m_header.sender_id);
    }

Referenced by mob_resp_cb().

Here is the caller graph for this function:

◆ onSpin()

bool platoon_strategic_ihp::PlatoonStrategicIHPPlugin::onSpin ( )

Spin callback function.

Definition at line 3063 of file platoon_strategic_ihp.cpp.

    {
        // Update the platoon manager for host's current location & speeds
        pm_.updateHostPose(current_downtrack_, current_crosstrack_);
        pm_.updateHostSpeeds(current_speed_, cmd_speed_);
 
        if (pm_.current_platoon_state == PlatoonState::LEADER)
        {
            run_leader();
        }
        else if (pm_.current_platoon_state == PlatoonState::FOLLOWER)
        {
            run_follower();
        }
        else if (pm_.current_platoon_state == PlatoonState::CANDIDATEFOLLOWER)
        {
            run_candidate_follower();
        }
        else if (pm_.current_platoon_state == PlatoonState::LEADERWAITING)
        {
            run_leader_waiting();
        }
        // UCLA: added for frontal join
        else if (pm_.current_platoon_state == PlatoonState::LEADERABORTING)
        {
            run_leader_aborting();
        }
        // UCLA: added for frontal join
        else if (pm_.current_platoon_state == PlatoonState::CANDIDATELEADER)
        {
            run_candidate_leader();
        }
        // UCLA: added lead with operationfor CUT-IN join
        else if (pm_.current_platoon_state == PlatoonState::LEADWITHOPERATION)
        {
            run_lead_with_operation();
        }
        // UCLA: added prepare to join for CUT-IN join
        else if (pm_.current_platoon_state == PlatoonState::PREPARETOJOIN)
        {
            run_prepare_to_join();
        }
        else if (pm_.current_platoon_state == PlatoonState::STANDBY)
        {
            RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "standby state, nothing to do");
        }
        // coding oversight
        else
        {
            RCLCPP_ERROR_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "///// unhandled state " << pm_.current_platoon_state);
        }
        // TODO: Place holder for departure
 
        carma_planning_msgs::msg::PlatooningInfo platoon_status = composePlatoonInfoMsg();
        platooning_info_publisher_(platoon_status);
 
        return true;
    }

Referenced by platoon_strategic_ihp::Node::on_configure_plugin().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ plan_maneuver_cb()

bool platoon_strategic_ihp::PlatoonStrategicIHPPlugin::plan_maneuver_cb	(	carma_planning_msgs::srv::PlanManeuvers::Request &	req,
		carma_planning_msgs::srv::PlanManeuvers::Response &	resp
	)

Callback function to the maneuver request.

Parameters

req	Maneuver service request
resp	Maneuver service response

Returns: Mobility response message

Definition at line 3224 of file platoon_strategic_ihp.cpp.

    {
        // use current position to find lanelet ID
        lanelet::BasicPoint2d current_loc(pose_msg_.pose.position.x, pose_msg_.pose.position.y);
 
        // *** get the actually closest lanelets that relate to current location (n=10) ***//
        auto current_lanelets = lanelet::geometry::findNearest(wm_->getMap()->laneletLayer, current_loc, 10);
 
        lanelet::ConstLanelet current_lanelet;
 
        // To avoid overlapping lanelets, compare the nearest lanelets with the route
        for (auto llt: current_lanelets)
        {
            if (wm_->getRoute()->contains(llt.second))
            {
                current_lanelet = llt.second;
                break;
            }
        }
 
        // raise warn if no path was found
        if(current_lanelets.size() == 0)
        {
            RCLCPP_WARN_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "Cannot find any lanelet in map!");
            return true;
        }
 
        // locate lanelet on shortest path
        auto shortest_path = wm_->getRoute()->shortestPath(); // find path amoung route
 
        // read status data
        double current_progress = wm_->routeTrackPos(current_loc).downtrack;
        double speed_progress = current_speed_;
        rclcpp::Time time_progress = timer_factory_->now();
 
        // ---------------- use IHP platoon trajectory regulation here --------------------
        // Note: The desired gap will be adjusted and send to control plugin (via platoon_info_msg) where gap creation will be handled.
        double target_speed;
 
        // Note: gap regulation has moved to control plug-in, no need to adjust speed here.
        target_speed = findSpeedLimit(current_lanelet);   //get Speed Limit
        double total_maneuver_length = current_progress + config_.time_step * target_speed;
        // ----------------------------------------------------------------
 
        // pick smaller length, accomendate when host is close to the route end
        double route_length =  wm_->getRouteEndTrackPos().downtrack;
        total_maneuver_length = std::min(total_maneuver_length, route_length);
 
        // Update current status based on prior plan
        if(req.prior_plan.maneuvers.size()!= 0)
        {
            RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "Provided with initial plan...");
            time_progress = req.prior_plan.planning_completion_time;
            int end_lanelet = 0;
            updateCurrentStatus(req.prior_plan.maneuvers.back(), speed_progress, current_progress, end_lanelet);
        }
 
        RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "Starting Loop");
        RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "total_maneuver_length: " << total_maneuver_length << " route_length: " << route_length);
 
 
        RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "in mvr  callback safeToLaneChange: " << safeToLaneChange_);
 
        // Note: Use current_lanlet list (which was determined based on vehicle pose) to find current lanelet ID.
        lanelet::Id current_lanelet_id = current_lanelet.id();
 
 
 
        RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "current_lanelet_id: " << current_lanelet_id);
        // lane change maneuver
        if (safeToLaneChange_)
        {
            // for testing purpose only, check lane change status
            double target_crosstrack = wm_->routeTrackPos(target_cutin_pose_).crosstrack;
            RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "target_crosstrack: " << target_crosstrack);
            double crosstrackDiff = current_crosstrack_ - target_crosstrack;
            bool isLaneChangeFinished = abs(crosstrackDiff) <= config_.maxCrosstrackError;
            RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "crosstrackDiff: " << crosstrackDiff);
            RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "isLaneChangeFinished: " << isLaneChangeFinished);
 
            // lane change not finished, use lane change plan
            if(!isLaneChangeFinished)
            {
                // send out lane change plan
                while (current_progress < total_maneuver_length)
                {
                    RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "Lane Change Maneuver for Cut-in join ! ");
                    RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "current_progress: "<< current_progress);
                    RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "speed_progress: " << speed_progress);
                    RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "target_speed: " << target_speed);
                    RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "time_progress: " << rclcpp::Time(time_progress).seconds());
 
                    // set to next lane destination, consider sending ecef instead of dtd
                    double end_dist = total_maneuver_length;
                    RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "end_dist: " << end_dist);
                    // consider calculate dtd_diff and ctd_diff
                    double dist_diff = end_dist - current_progress;
                    RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "dist_diff: " << dist_diff);
 
 
                    //TODO: target_cutin_pose_ represents the platoon leader. It seems this may be the wrong answer for mid- or rear-cutins?
                    //SAINA: currently, the functions do not provide the correct point of rear vehicle of the platoon
                    double lc_end_dist = wm_->routeTrackPos(target_cutin_pose_).downtrack;
                    RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "lc_end_dist before buffer: " << lc_end_dist);
                    lc_end_dist = std::max(lc_end_dist, current_progress + config_.maxCutinGap);
                    RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "lc_end_dist after buffer: " << lc_end_dist);
 
                    //TODO: target_cutin_pose_ represents the platoon leader. Is this the best pose to use here?
                    // get the actually closest lanelets,
                    auto target_lanelets = lanelet::geometry::findNearest(wm_->getMap()->laneletLayer, target_cutin_pose_, 1);
                    if (target_lanelets.empty())
                    {
                        RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "The target cutin pose is not on a valid lanelet. So no lane change!");
                        break;
                    }
                    lanelet::Id target_lanelet_id = target_lanelets[0].second.id();
                    RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "target_lanelet_id: " << target_lanelet_id);
 
                    lanelet::ConstLanelet starting_lanelet = wm_->getMap()->laneletLayer.get(current_lanelet_id);
                    lanelet::ConstLanelet ending_lanelet = wm_->getMap()->laneletLayer.get(target_lanelet_id);
 
                    bool lanechangePossible = is_lanechange_possible(current_lanelet_id, target_lanelet_id);
                    RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "lanechangePossible: " << lanechangePossible);
 
                    if (lanechangePossible)
                    {
                        RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "Lane change possible, planning it.. " );
                        resp.new_plan.maneuvers.push_back(composeLaneChangeManeuverMessage(current_downtrack_, lc_end_dist,
                                            speed_progress, target_speed, current_lanelet_id, target_lanelet_id , time_progress));
 
                    }
                    else
                    {
                        RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "Lane change impossible, planning lanefollow instead ... " );
                        resp.new_plan.maneuvers.push_back(composeManeuverMessage(current_downtrack_, end_dist,
                                            speed_progress, target_speed, current_lanelet_id, time_progress));
                    }
 
 
 
                    current_progress += dist_diff;
                    // read lane change maneuver end time as time progress
                    time_progress = resp.new_plan.maneuvers.back().lane_change_maneuver.end_time;
                    speed_progress = target_speed;
                    if(current_progress >= total_maneuver_length)
                    {
                        break;
                    }
 
                }
            }
 
            // lane change finished, use lane following plan
            else
            {
                // send out lane following plan
                while (current_progress < total_maneuver_length)
                {
                    RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "Same Lane Maneuver for platoon join ! ");
                    RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "current_progress: "<< current_progress);
                    RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "speed_progress: " << speed_progress);
                    RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "target_speed: " << target_speed);
                    RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "time_progress: " << rclcpp::Time(time_progress).seconds());
                    double end_dist = total_maneuver_length;
                    RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "end_dist: " << end_dist);
                    double dist_diff = end_dist - current_progress;
                    RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "dist_diff: " << dist_diff);
                    if(end_dist < current_progress)
                    {
                        break;
                    }
                    // Note: The previous plan was generated at the beginning of the trip. It is necessary to update
                    //       it as the lane ID and lanelet Index are different.
                    resp.new_plan.maneuvers.push_back(composeManeuverMessage(current_downtrack_, end_dist,
                                            speed_progress, target_speed, current_lanelet_id, time_progress));
 
                    current_progress += dist_diff;
                    time_progress = resp.new_plan.maneuvers.back().lane_following_maneuver.end_time;
                    speed_progress = target_speed;
                    if(current_progress >= total_maneuver_length)
                    {
                        break;
                    }
                }
            }
        }
 
        // same-lane maneuver
        else
        {
            RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "Planning Same Lane Maneuver! ");
            while (current_progress < total_maneuver_length)
            {
                RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "Same Lane Maneuver for platoon join ! ");
                RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "current_progress: "<< current_progress);
                RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "speed_progress: " << speed_progress);
                RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "target_speed: " << target_speed);
                RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "time_progress: " << rclcpp::Time(time_progress).seconds());
                double end_dist = total_maneuver_length;
                RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "end_dist: " << end_dist);
                double dist_diff = end_dist - current_progress;
                RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "dist_diff: " << dist_diff);
                if (end_dist < current_progress)
                {
                    break;
                }
 
                resp.new_plan.maneuvers.push_back(composeManeuverMessage(current_progress, end_dist,
                                        speed_progress, target_speed,current_lanelet_id, time_progress));
 
 
                current_progress += dist_diff;
                time_progress = resp.new_plan.maneuvers.back().lane_following_maneuver.end_time;
                speed_progress = target_speed;
                if(current_progress >= total_maneuver_length)
                {
                    break;
                }
            }
        }
 
 
        if(resp.new_plan.maneuvers.size() == 0)
        {
            RCLCPP_WARN_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "Cannot plan maneuver because no route is found");
        }
 
 
        if (pm_.getHostPlatoonSize() < 2 && !safeToLaneChange_)
        {
            resp.new_plan.maneuvers = {};
            RCLCPP_WARN_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "Platoon size 1 so Empty maneuver sent");
        }
        else
        {
            RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "Planning maneuvers: ");
            RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "safeToLaneChange_: " << safeToLaneChange_);
            RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "pm_.getHostPlatoonSize(): " << pm_.getHostPlatoonSize());
        }
 
        if (pm_.current_platoon_state == PlatoonState::STANDBY)
        {
            pm_.current_platoon_state = PlatoonState::LEADER;
            RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "change the state from standby to leader at start-up");
        }
 
        RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "current_downtrack: " << current_downtrack_);
 
        return true;
    }

References composeLaneChangeManeuverMessage(), composeManeuverMessage(), config_, current_crosstrack_, current_downtrack_, platoon_strategic_ihp::PlatoonManager::current_platoon_state, current_speed_, findSpeedLimit(), platoon_strategic_ihp::PlatoonManager::getHostPlatoonSize(), is_lanechange_possible(), platoon_strategic_ihp::LEADER, platoon_strategic_ihp::PlatoonPluginConfig::maxCrosstrackError, platoon_strategic_ihp::PlatoonPluginConfig::maxCutinGap, pm_, pose_msg_, safeToLaneChange_, platoon_strategic_ihp::STANDBY, target_cutin_pose_, platoon_strategic_ihp::PlatoonPluginConfig::time_step, timer_factory_, updateCurrentStatus(), and wm_.

Here is the call graph for this function:

◆ pose_cb()

void platoon_strategic_ihp::PlatoonStrategicIHPPlugin::pose_cb ( const geometry_msgs::msg::PoseStamped::UniquePtr msg )

Callback function for current pose.

Parameters

msg	PoseStamped msg

Definition at line 115 of file platoon_strategic_ihp.cpp.

    {
        pose_msg_ = geometry_msgs::msg::PoseStamped(*msg);
 
        if (pm_.current_platoon_state != PlatoonState::STANDBY)
        {
            lanelet::BasicPoint2d current_loc(pose_msg_.pose.position.x, pose_msg_.pose.position.y);
            carma_wm::TrackPos tc = wm_->routeTrackPos(current_loc);
 
            // update host's DtD and CtD
            current_downtrack_ = tc.downtrack;
            current_crosstrack_ = tc.crosstrack;
            // RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "current_downtrack_ = " << current_downtrack_ << ", current_crosstrack_ = " << current_crosstrack_);
            pm_.updateHostPose(current_downtrack_, current_crosstrack_);
 
            // note: the ecef read from "pose_ecef_point" is in cm.
            carma_v2x_msgs::msg::LocationECEF pose_ecef_point = pose_to_ecef(pose_msg_);
            setHostECEF(pose_ecef_point);
        }
    }

References carma_wm::TrackPos::crosstrack, current_crosstrack_, current_downtrack_, platoon_strategic_ihp::PlatoonManager::current_platoon_state, carma_wm::TrackPos::downtrack, pm_, pose_msg_, pose_to_ecef(), setHostECEF(), platoon_strategic_ihp::STANDBY, platoon_strategic_ihp::PlatoonManager::updateHostPose(), and wm_.

Referenced by platoon_strategic_ihp::Node::on_configure_plugin().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ pose_to_ecef()

carma_v2x_msgs::msg::LocationECEF platoon_strategic_ihp::PlatoonStrategicIHPPlugin::pose_to_ecef ( geometry_msgs::msg::PoseStamped pose_msg )

private

Function to convert pose from map frame to ecef location.

Parameters

pose_msg pose message

Returns: mobility operation msg

Definition at line 64 of file platoon_strategic_ihp.cpp.

    {
 
        if (!map_projector_)
        {
            throw std::invalid_argument("No map projector available for ecef conversion");
        }
 
        carma_v2x_msgs::msg::LocationECEF location;
 
        // note: ecef point read from map projector is in m.
        lanelet::BasicPoint3d ecef_point = map_projector_->projectECEF({pose_msg.pose.position.x, pose_msg.pose.position.y, 0.0}, 1);
        location.ecef_x = ecef_point.x() * 100.0;
        location.ecef_y = ecef_point.y() * 100.0;
        location.ecef_z = ecef_point.z() * 100.0;
 
 
        // RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "location.ecef_x: " << location.ecef_x);
        // RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "location.ecef_y: " << location.ecef_y);
        // RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "location.ecef_z: " << location.ecef_z);
 
        // note: the returned ecef is in cm.
        return location;
    }

References map_projector_.

Referenced by mob_op_cb_leader(), mob_op_cb_preparetojoin(), mob_req_cb_leadwithoperation(), mob_resp_cb_preparetojoin(), pose_cb(), run_candidate_follower(), run_leader_aborting(), and run_prepare_to_join().

Here is the caller graph for this function:

◆ run_candidate_follower()

void platoon_strategic_ihp::PlatoonStrategicIHPPlugin::run_candidate_follower ( )

Run Candidate Follower State.

Definition at line 2725 of file platoon_strategic_ihp.cpp.

    {
        long tsStart = timer_factory_->now().nanoseconds() / 1000000;
 
        // Task 1: state timeout
        bool isCurrentStateTimeout = (tsStart - candidatestateStartTime) > waitingStateTimeout * 1000;
        RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "timeout1: " << tsStart - candidatestateStartTime);
        RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "waitingStateTimeout: " << waitingStateTimeout * 1000);
        if (isCurrentStateTimeout)
        {
            RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "The current candidate follower state is timeout. Change back to leader state.");
            pm_.current_platoon_state = PlatoonState::LEADER;
            pm_.clearActionPlan();
        }
 
        // Task 2: plan timeout, check if current plan is still valid (i.e., not timed out).
        if (pm_.current_plan.valid)
        {
            RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "pm_.current_plan.planStartTime: " << pm_.current_plan.planStartTime);
            RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "timeout2: " << tsStart - pm_.current_plan.planStartTime);
            RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "NEGOTIATION_TIMEOUT: " << NEGOTIATION_TIMEOUT);
            bool isPlanTimeout = tsStart - pm_.current_plan.planStartTime > NEGOTIATION_TIMEOUT;
            if (isPlanTimeout)
            {
                pm_.current_platoon_state = PlatoonState::LEADER;
                pm_.clearActionPlan();
                RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "The current plan did not receive any response. Abort and change to leader state.");
                RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "Changed the state back to Leader");
            }
        }
 
        // Task 3: update plan calculate gap, update plan: send PLATOON_FOLLOWER_JOIN request with new gap
        double desiredJoinGap2 = config_.desiredJoinTimeGap * current_speed_;
        double maxJoinGap = std::max(config_.desiredJoinGap, desiredJoinGap2);
        double currentGap = 0.0;
        if (!pm_.neighbor_platoon_.empty())
        {
            currentGap = pm_.neighbor_platoon_.back().vehiclePosition - current_downtrack_;
            RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "curent gap calculated from back of neighbor platoon: " << currentGap);
            RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "pm_.neighbor_platoon_.back().vehiclePosition " << pm_.neighbor_platoon_.back().vehiclePosition);
 
        }
        else
        {
            currentGap = pm_.getDistanceToPredVehicle();
            RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "curent gap when there is no neighbor platoon: " << currentGap);
        }
 
        RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "Based on desired join time gap, the desired join distance gap is " << desiredJoinGap2 << " ms");
        RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "Since we have max allowed gap as " << config_.desiredJoinGap << " m then max join gap became " << maxJoinGap << " m");
        RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "The current gap from radar is " << currentGap << " m");
        if (currentGap <= maxJoinGap  &&  !pm_.current_plan.valid)
        {
            carma_v2x_msgs::msg::MobilityRequest request;
            std::string planId = boost::uuids::to_string(boost::uuids::random_generator()());
            long currentTime = timer_factory_->now().nanoseconds() / 1000000;
            request.m_header.plan_id = planId;
            request.m_header.recipient_id = pm_.current_plan.peerId;
            request.m_header.sender_id = config_.vehicleID;
            request.m_header.timestamp = currentTime;
 
            request.plan_type.type = carma_v2x_msgs::msg::PlanType::PLATOON_FOLLOWER_JOIN;
            request.strategy = PLATOONING_STRATEGY;
            request.strategy_params = ""; //params will not be read by receiver since this is 2nd msg in sequence
            request.urgency = 50;
            request.location = pose_to_ecef(pose_msg_);
            mobility_request_publisher_(request);
            RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "Published Mobility Candidate-Join request to the leader");
            RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "current plan peer id: " << pm_.current_plan.peerId);
 
            // Update the local record of the new activity plan and now establish that we have a platoon plan as well,
            // which allows us to start sending necessary op STATUS messages
            pm_.current_plan = ActionPlan(true, currentTime, planId, pm_.current_plan.peerId);
            pm_.platoonLeaderID = pm_.current_plan.peerId;
 
            // Initialize counter to delay transmission of first operation STATUS message
            candidate_follower_delay_count_ = 0;
        }
 
        //Task 4: publish platoon status message (as single joiner)
        if (pm_.current_plan.valid)
        {
            // Don't want to do this until after the above MobReq message is delivered, otherwise recipient will double-count us in their platoon
            if (++candidate_follower_delay_count_ > 2)
            {
                carma_v2x_msgs::msg::MobilityOperation status;
                status = composeMobilityOperationCandidateFollower();
                mobility_operation_publisher_(status);
                RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "Published platoon STATUS operation message as Candidate Follower");
            }
        }
 
        long tsEnd = timer_factory_->now().nanoseconds() / 1000000;
        long sleepDuration = std::max((int32_t)(statusMessageInterval_ - (tsEnd - tsStart)), 0);
        // TODO this solution is not sim-time complient and should be replaced with one which is when possible
        // https://github.com/usdot-fhwa-stol/carma-platform/issues/1888
        std::this_thread::sleep_for(std::chrono::milliseconds(sleepDuration));
    }

Referenced by onSpin().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ run_candidate_leader()

void platoon_strategic_ihp::PlatoonStrategicIHPPlugin::run_candidate_leader ( )

Run Candidate Leader State.

Definition at line 2921 of file platoon_strategic_ihp.cpp.

    {
        RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "Run Candidate Leader State ");
        long tsStart = timer_factory_->now().nanoseconds() / 1000000;
        // Task 1: State time out
        if (tsStart - candidatestateStartTime > waitingStateTimeout * 1000)
        {
            //TODO if the current state timeouts, we need to have a kind of ABORT message to inform the applicant
            RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "CandidateLeader state is timeout, changing back to PlatoonLeaderState.");
            pm_.current_platoon_state = PlatoonState::LEADER;
            pm_.clearActionPlan();
            pm_.resetHostPlatoon();
        }
 
        // Task 2: publish status message
        carma_v2x_msgs::msg::MobilityOperation status;
        status = composeMobilityOperationCandidateLeader();
        mobility_operation_publisher_(status);
        RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "publish status message");
        long tsEnd = timer_factory_->now().nanoseconds() / 1000000;
        long sleepDuration = std::max((int32_t)(statusMessageInterval_ - (tsEnd - tsStart)), 0);
        // TODO this solution is not sim-time complient and should be replaced with one which is when possible
        // https://github.com/usdot-fhwa-stol/carma-platform/issues/1888
        std::this_thread::sleep_for(std::chrono::milliseconds(sleepDuration));
    }

References candidatestateStartTime, platoon_strategic_ihp::PlatoonManager::clearActionPlan(), composeMobilityOperationCandidateLeader(), platoon_strategic_ihp::PlatoonManager::current_platoon_state, platoon_strategic_ihp::LEADER, mobility_operation_publisher_, pm_, platoon_strategic_ihp::PlatoonManager::resetHostPlatoon(), statusMessageInterval_, timer_factory_, and waitingStateTimeout.

Referenced by onSpin().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ run_follower()

void platoon_strategic_ihp::PlatoonStrategicIHPPlugin::run_follower ( )

Run Follower State.

Definition at line 2682 of file platoon_strategic_ihp.cpp.

    {
        RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "run follower");
        // This is a interrupted-safe loop.
        // This loop has four tasks:
        // 1. Check the state start time, if it exceeds a limit it will give up current plan and change back to leader state
        // 2. Abort current request if we wait for long enough time for response from leader and change back to leader state
 
        long tsStart = timer_factory_->now().nanoseconds() / 1000000;
        // Job 1
        carma_v2x_msgs::msg::MobilityOperation status;
        status = composeMobilityOperationFollower();
        mobility_operation_publisher_(status);
        // Job 2
        // Get the number of vehicles in this platoon who is in front of us
        int vehicleInFront = pm_.getNumberOfVehicleInFront();
        if (vehicleInFront == 0)
        {
            noLeaderUpdatesCounter++;
            if (noLeaderUpdatesCounter >= LEADER_TIMEOUT_COUNTER_LIMIT)
            {
                RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "noLeaderUpdatesCounter = " << noLeaderUpdatesCounter << " and change to leader state");
                pm_.changeFromFollowerToLeader();
                pm_.current_platoon_state = PlatoonState::LEADER;
                noLeaderUpdatesCounter = 0;
            }
        }
        else
        {
            // reset counter to zero when we get updates again
            noLeaderUpdatesCounter = 0;
        }
        long tsEnd = timer_factory_->now().nanoseconds() / 1000000;
        long sleepDuration = std::max((int32_t)(statusMessageInterval_ - (tsEnd - tsStart)), 0);
        // TODO this solution is not sim-time complient and should be replaced with one which is when possible
        // https://github.com/usdot-fhwa-stol/carma-platform/issues/1888
        std::this_thread::sleep_for(std::chrono::milliseconds(sleepDuration));
 
        // Job 3: Dissoleve request.
        //TODO: set departure indicator
 
    }

References platoon_strategic_ihp::PlatoonManager::changeFromFollowerToLeader(), composeMobilityOperationFollower(), platoon_strategic_ihp::PlatoonManager::current_platoon_state, platoon_strategic_ihp::PlatoonManager::getNumberOfVehicleInFront(), platoon_strategic_ihp::LEADER, LEADER_TIMEOUT_COUNTER_LIMIT, mobility_operation_publisher_, noLeaderUpdatesCounter, pm_, statusMessageInterval_, and timer_factory_.

Referenced by onSpin().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ run_lead_with_operation()

void platoon_strategic_ihp::PlatoonStrategicIHPPlugin::run_lead_with_operation ( )

UCLA Run lead with operation State.

Definition at line 2948 of file platoon_strategic_ihp.cpp.

    {
        long tsStart = timer_factory_->now().nanoseconds() / 1000000;
        // Task 1: heart beat timeout: constantly send INFO mob_op
        bool isTimeForHeartBeat = tsStart - prevHeartBeatTime_ >= infoMessageInterval_;
        RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "time since last heart beat: " << tsStart - prevHeartBeatTime_);
        if (isTimeForHeartBeat)
        {
            carma_v2x_msgs::msg::MobilityOperation infoOperation;
            infoOperation = composeMobilityOperationLeadWithOperation(OPERATION_INFO_TYPE);
            mobility_operation_publisher_(infoOperation);
            prevHeartBeatTime_ = timer_factory_->now().nanoseconds() / 1000000;
        }
 
 
        // Task 4: STATUS msgs
        bool hasFollower = pm_.getHostPlatoonSize() > 1  ||  config_.test_cutin_join;
        // if has follower, publish platoon message as STATUS mob_op
        if (hasFollower)
        {
            carma_v2x_msgs::msg::MobilityOperation statusOperation;
            statusOperation = composeMobilityOperationLeadWithOperation(OPERATION_STATUS_TYPE);
            mobility_operation_publisher_(statusOperation);
        }
        long tsEnd = timer_factory_->now().nanoseconds() / 1000000;
        long sleepDuration = std::max((int32_t)(statusMessageInterval_ - (tsEnd - tsStart)), 0);
        // TODO this solution is not sim-time complient and should be replaced with one which is when possible
        // https://github.com/usdot-fhwa-stol/carma-platform/issues/1888
        std::this_thread::sleep_for(std::chrono::milliseconds(sleepDuration));
    }

References composeMobilityOperationLeadWithOperation(), config_, platoon_strategic_ihp::PlatoonManager::getHostPlatoonSize(), infoMessageInterval_, mobility_operation_publisher_, OPERATION_INFO_TYPE, OPERATION_STATUS_TYPE, pm_, prevHeartBeatTime_, statusMessageInterval_, platoon_strategic_ihp::PlatoonPluginConfig::test_cutin_join, and timer_factory_.

Referenced by onSpin().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ run_leader()

void platoon_strategic_ihp::PlatoonStrategicIHPPlugin::run_leader ( )

Run Leader State.

Definition at line 2625 of file platoon_strategic_ihp.cpp.

    {
        unsigned long tsStart = timer_factory_->now().nanoseconds() / 1000000;
 
        // If vehicle is not rolling then return
        if (current_speed_ <= STOPPED_SPEED)
        {
            return;
        }
 
        // Task 1: heart beat timeout: send INFO mob_op
        bool isTimeForHeartBeat = tsStart - prevHeartBeatTime_ >= infoMessageInterval_;
        RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "time since last heart beat: " << tsStart - prevHeartBeatTime_);
        if (isTimeForHeartBeat)
        {
            carma_v2x_msgs::msg::MobilityOperation infoOperation;
            infoOperation = composeMobilityOperationLeader(OPERATION_INFO_TYPE);
            mobility_operation_publisher_(infoOperation);
            prevHeartBeatTime_ = timer_factory_->now().nanoseconds() / 1000000;
            RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "Published heart beat platoon INFO mobility operation message");
        }
 
        // Task 3: plan time out, check if any current join plan is still valid (i.e., not timed out).
        if (pm_.current_plan.valid)
        {
            bool isCurrentPlanTimeout = tsStart - pm_.current_plan.planStartTime > NEGOTIATION_TIMEOUT;
            if (isCurrentPlanTimeout)
            {
                RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "Give up current on waiting plan with planId: " << pm_.current_plan.planId);
                pm_.clearActionPlan();
            }
        }
 
        // Task 4: STATUS msgs
        bool hasFollower = pm_.getHostPlatoonSize() > 1  ||  config_.test_cutin_join;
        RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "hasFollower" << hasFollower);
        // if has follower, publish platoon message as STATUS mob_op
        if (hasFollower)
        {
            carma_v2x_msgs::msg::MobilityOperation statusOperation;
            statusOperation = composeMobilityOperationLeader(OPERATION_STATUS_TYPE);
            // mob_op_pub_.publish(statusOperation);
            mobility_operation_publisher_(statusOperation);
            RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "Published platoon STATUS operation message as a Leader with Follower");
        }
 
        long tsEnd = timer_factory_->now().nanoseconds() / 1000000;
        long sleepDuration = std::max((int32_t)(statusMessageInterval_ - (tsEnd - tsStart)), 0);
        // TODO this solution is not sim-time complient and should be replaced with one which is when possible
        // https://github.com/usdot-fhwa-stol/carma-platform/issues/1888
        std::this_thread::sleep_for(std::chrono::milliseconds(sleepDuration));
 
        // Job 5: Dissoleve request.
        // TODO: Place holder for departure. Need to change to departing state and tracking departng ID.
 
    }

References platoon_strategic_ihp::PlatoonManager::clearActionPlan(), composeMobilityOperationLeader(), config_, platoon_strategic_ihp::PlatoonManager::current_plan, current_speed_, platoon_strategic_ihp::PlatoonManager::getHostPlatoonSize(), infoMessageInterval_, mobility_operation_publisher_, NEGOTIATION_TIMEOUT, OPERATION_INFO_TYPE, OPERATION_STATUS_TYPE, platoon_strategic_ihp::ActionPlan::planId, platoon_strategic_ihp::ActionPlan::planStartTime, pm_, prevHeartBeatTime_, statusMessageInterval_, STOPPED_SPEED, platoon_strategic_ihp::PlatoonPluginConfig::test_cutin_join, timer_factory_, and platoon_strategic_ihp::ActionPlan::valid.

Referenced by onSpin().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ run_leader_aborting()

void platoon_strategic_ihp::PlatoonStrategicIHPPlugin::run_leader_aborting ( )

Run Leader Aborting State.

Definition at line 2825 of file platoon_strategic_ihp.cpp.

    {
        /*
            UCLA implementation note:
            1. this function  send step plan type: "PLATOON_FRONT_JOIN"
            2. the sender of the plan (host vehicle) is the previous leader, it wil prepare to follow front joiner (new leader)
        */
        long tsStart = timer_factory_->now().nanoseconds() / 1000000;
        // Task 1: state timeout
        bool isCurrentStateTimeout = (tsStart - candidatestateStartTime) > waitingStateTimeout * 1000;
        RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "timeout1: " << tsStart - candidatestateStartTime);
        RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "waitingStateTimeout: " << waitingStateTimeout * 1000);
        if (isCurrentStateTimeout)
        {
            RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "The current leader aborting state is timeout. Change back to leader state.");
            pm_.current_platoon_state = PlatoonState::LEADER;
 
            //clear plan validity & end; leave platoon info alone, as we may still be leading a valid platoon
            pm_.clearActionPlan();
            return;
        }
 
        // Task 3: update plan: PLATOON_FRONT_JOIN with new gap
        double desiredJoinGap2 = config_.desiredJoinTimeGap * current_speed_;
        double maxJoinGap = std::max(config_.desiredJoinGap, desiredJoinGap2);
 
        // check if compatible for front join --> return front gap, no veh type check, is compatible
        // Note that pm_ only represents host's platoon members. As the aborting leader, the only vehicle
        // preceding host is the candidate joiner. For this code to work, it depends on the candidate to publish
        // mobility operation STATUS messages so that host can include it in the pm_ platoon membership.
        double currentGap = pm_.getDistanceToPredVehicle(); //returns 0 if we haven't received op STATUS from joiner yet
        RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "Based on desired join time gap, the desired join distance gap is " << desiredJoinGap2 << " m");
        RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "Since we have max allowed gap as " << config_.desiredJoinGap << " m then max join gap became " << maxJoinGap << " m");
        RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "The current gap to joiner is " << currentGap << " m");
 
        // NOTE: The front join depends upon the joiner to publish op STATUS messages with this platoon's ID, then host receives at least one
        // and thereby adds the joiner to the platoon record. This process requires host's mob_req_cb_leader() to ACK the join request, then
        // the remote vehicle to handle that ACK and broadcast its first op STATUS, then host to receive and process it. All that has to happen
        // before the below code block runs, even though this method starts to spin immediately after we send out the mentioned ACK. To avoid
        // a race condition, we must wait a few cycles to ensure the 2-way messaging has completed. The race is that above calculation for
        // current gap will be returned as 0 until we have received said STATUS message from the joiner, which would prematurely trigger the
        // process to move forward.
 
        // Check if gap is big enough and if there is no currently active plan and this method has been called several times
        // Add a condition to prevent sending repeated requests (Note: This is a same-lane maneuver, so no need to consider lower bound of joining gap.)
        ++numLeaderAbortingCalls_;
        RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "numLeaderAbortingCalls = " << numLeaderAbortingCalls_ << ", max = " << config_.maxLeaderAbortingCalls);
        if (currentGap <= maxJoinGap  &&  !pm_.current_plan.valid  &&  numLeaderAbortingCalls_ > config_.maxLeaderAbortingCalls)
        {
            // compose frontal joining plan, senderID is the old leader
            carma_v2x_msgs::msg::MobilityRequest request;
            std::string planId = boost::uuids::to_string(boost::uuids::random_generator()());
            long currentTime = timer_factory_->now().nanoseconds() / 1000000;
            request.m_header.plan_id = planId;
            request.m_header.recipient_id = pm_.platoonLeaderID; //the new joiner
            request.m_header.sender_id = config_.vehicleID;
            request.m_header.timestamp = currentTime;
 
            int platoon_size = pm_.getHostPlatoonSize(); //depends on joiner to send op STATUS messages while joining
 
            boost::format fmter(JOIN_PARAMS); // Note: Front and rear join uses same params, hence merge to one param for both condition.
            int dummy_join_index = -2; //leader aborting doesn't need join_index so use default value
            fmter %platoon_size;                //  index = 0
            fmter %current_speed_;              //  index = 1, in m/s
            fmter %pose_ecef_point_.ecef_x;     //  index = 2
            fmter %pose_ecef_point_.ecef_y;     //  index = 3
            fmter %pose_ecef_point_.ecef_z;     //  index = 4
            fmter %dummy_join_index;            //  index = 5
            request.strategy_params = fmter.str();
 
            // assign a new plan type
            request.plan_type.type = carma_v2x_msgs::msg::PlanType::PLATOON_FRONT_JOIN;
            request.strategy = PLATOONING_STRATEGY;
            request.urgency = 50;
            request.location = pose_to_ecef(pose_msg_);
            mobility_request_publisher_(request);
            RCLCPP_WARN(rclcpp::get_logger("platoon_strategic_ihp"),"Published Mobility Candidate-Join request to the new leader");
 
            // Create a new join action plan
            pm_.current_plan = ActionPlan(true, currentTime, planId, pm_.platoonLeaderID);
        }
 
        //Task 4: publish platoon status message
        carma_v2x_msgs::msg::MobilityOperation status;
        status = composeMobilityOperationLeaderAborting();
        mobility_operation_publisher_(status);
        RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "Published platoon STATUS operation message");
 
        long tsEnd = timer_factory_->now().nanoseconds() / 1000000;
        long sleepDuration = std::max((int32_t)(statusMessageInterval_ - (tsEnd - tsStart)), 0);
        // TODO this solution is not sim-time complient and should be replaced with one which is when possible
        // https://github.com/usdot-fhwa-stol/carma-platform/issues/1888
        std::this_thread::sleep_for(std::chrono::milliseconds(sleepDuration));
    }

Referenced by onSpin().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ run_leader_waiting()

void platoon_strategic_ihp::PlatoonStrategicIHPPlugin::run_leader_waiting ( )

Run Leader Waiting State.

Definition at line 2600 of file platoon_strategic_ihp.cpp.

    {
        RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "Run LeaderWaiting State ");
        long tsStart = timer_factory_->now().nanoseconds() / 1000000;
        // Task 1
        if (tsStart - waitingStartTime > waitingStateTimeout * 1000)
        {
            //TODO if the current state timeouts, we need to have a kind of ABORT message to inform the applicant
            RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "LeaderWaitingState is timeout, changing back to PlatoonLeaderState.");
            pm_.current_platoon_state = PlatoonState::LEADER;
            pm_.clearActionPlan();
        }
        // Task 2
        carma_v2x_msgs::msg::MobilityOperation status;
        status = composeMobilityOperationLeaderWaiting();
        mobility_operation_publisher_(status);
        RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "publish status message");
        long tsEnd = timer_factory_->now().nanoseconds() / 1000000;
        long sleepDuration = std::max((int32_t)(statusMessageInterval_ - (tsEnd - tsStart)), 0);
 
        // TODO this solution is not sim-time complient and should be replaced with one which is when possible
        // https://github.com/usdot-fhwa-stol/carma-platform/issues/1888
        std::this_thread::sleep_for(std::chrono::milliseconds(sleepDuration));
    }

References platoon_strategic_ihp::PlatoonManager::clearActionPlan(), composeMobilityOperationLeaderWaiting(), platoon_strategic_ihp::PlatoonManager::current_platoon_state, platoon_strategic_ihp::LEADER, mobility_operation_publisher_, pm_, statusMessageInterval_, timer_factory_, waitingStartTime, and waitingStateTimeout.

Referenced by onSpin().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ run_prepare_to_join()

void platoon_strategic_ihp::PlatoonStrategicIHPPlugin::run_prepare_to_join ( )

UCLA Run prepare to join State.

Definition at line 2980 of file platoon_strategic_ihp.cpp.

    {
        /*
        * The prepare join state should have the following tasks:
        *   1. Compose mobility operation param: status.
        *   2. Time out check.
        *   3. Calculate proper cut_in index
        *   4. Send out lane change intend to leader.
        *   Note: 1. safeToLaneChange_ monitors the gap condition.
        *         2. Once it is safe to lane change, the updated plan will send-out in "plan_maneuver_cb"
        */
 
        // Task 2.1: state timeout
        long tsStart = timer_factory_->now().nanoseconds() / 1000000;
        bool isCurrentStateTimeout = (tsStart - candidatestateStartTime) > waitingStateTimeout * 1000;
        RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "timeout1: " << tsStart - candidatestateStartTime);
        RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "waitingStateTimeout: " << waitingStateTimeout * 1000);
        if (isCurrentStateTimeout)
        {
            RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "The current prepare to join state is timeout. Change back to leader state and abort lane change.");
            pm_.current_platoon_state = PlatoonState::LEADER;
            safeToLaneChange_ = false;
            pm_.clearActionPlan();
            pm_.resetHostPlatoon();
            // Leave neighbor platoon info in place, as we may retry the join later
        }
 
        // TODO: Plan timeout is not needed for this state
 
        // If we aren't already waiting on a response to one of these plans, create one once neighbor info is available
        RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "current_plan.valid = " << pm_.current_plan.valid << ", is_neighbor_record_complete = " << pm_.is_neighbor_record_complete_);
 
        if (!pm_.current_plan.valid  &&  pm_.is_neighbor_record_complete_)
        {
            // Task 1: compose mobility operation (status)
            carma_v2x_msgs::msg::MobilityOperation status;
            status = composeMobilityOperationPrepareToJoin(); //TODO: I bet we could consolidate a lot of these compose methods
            mobility_operation_publisher_(status);
            RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "Published platoon STATUS operation message");
 
            // Task 3: Calculate proper cut_in index
            // Note: The cut-in index is zero-based and points to the gap-leading vehicle's index. For cut-in from front, the join index = -1.
            double joinerDtD = current_downtrack_;
            target_join_index_ = pm_.getClosestIndex(joinerDtD);
            RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "calculated join index: " << target_join_index_);
 
            // Task 4: Send out request to leader about cut-in position
            carma_v2x_msgs::msg::MobilityRequest request;
            std::string planId = boost::uuids::to_string(boost::uuids::random_generator()());
            long currentTime = timer_factory_->now().nanoseconds() / 1000000;
            request.m_header.plan_id = planId;
            request.m_header.recipient_id = pm_.neighbor_platoon_leader_id_;
            request.m_header.sender_id = config_.vehicleID;
            request.m_header.timestamp = currentTime;
            request.plan_type.type = carma_v2x_msgs::msg::PlanType::PLATOON_CUT_IN_JOIN;
            request.strategy = PLATOONING_STRATEGY;
            request.urgency = 50;
            request.location = pose_to_ecef(pose_msg_);
            double platoon_size = pm_.getHostPlatoonSize();
 
            boost::format fmter(JOIN_PARAMS);
            fmter %platoon_size;                //  index = 0
            fmter %current_speed_;              //  index = 1, in m/s
            fmter %pose_ecef_point_.ecef_x;     //  index = 2
            fmter %pose_ecef_point_.ecef_y;     //  index = 3
            fmter %pose_ecef_point_.ecef_z;     //  index = 4
            fmter %target_join_index_;          //  index = 5
            request.strategy_params = fmter.str();
            mobility_request_publisher_(request);
            RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "Published Mobility cut-in join request to leader " << request.m_header.recipient_id << " with planId = " << planId);
 
            // Create a new join action plan
            pm_.current_plan = ActionPlan(true, currentTime, planId, pm_.neighbor_platoon_leader_id_);
        }
    }

Referenced by onSpin().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ setConfig()

void platoon_strategic_ihp::PlatoonStrategicIHPPlugin::setConfig ( const PlatoonPluginConfig & config )

Set the current config.

Definition at line 3520 of file platoon_strategic_ihp.cpp.

    {
        config_ = config;
    }

References config_.

◆ setHostECEF()

void platoon_strategic_ihp::PlatoonStrategicIHPPlugin::setHostECEF ( carma_v2x_msgs::msg::LocationECEF pose_ecef_point )

UCLA Update the private variable pose_ecef_point_.

Definition at line 90 of file platoon_strategic_ihp.cpp.

    {
        // Note, the ecef here is in cm.
        pose_ecef_point_ = pose_ecef_point;
    }

References pose_ecef_point_.

Referenced by pose_cb().

Here is the caller graph for this function:

◆ setPMState()

void platoon_strategic_ihp::PlatoonStrategicIHPPlugin::setPMState ( PlatoonState desiredState )

UCLA Setter: function to set pm_.platoon_state.

Definition at line 103 of file platoon_strategic_ihp.cpp.

    {
        pm_.current_platoon_state = desiredState;
    }

References platoon_strategic_ihp::PlatoonManager::current_platoon_state, and pm_.

◆ twist_cb()

void platoon_strategic_ihp::PlatoonStrategicIHPPlugin::twist_cb ( const geometry_msgs::msg::TwistStamped::UniquePtr msg )

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

Parameters

msg	Latest twist message

Definition at line 143 of file platoon_strategic_ihp.cpp.

    {
        current_speed_ = msg->twist.linear.x;
        if (current_speed_ < STOPPED_SPEED)
        {
            current_speed_ = 0.0;
        }
    }

References current_speed_, and STOPPED_SPEED.

Referenced by platoon_strategic_ihp::Node::on_configure_plugin().

Here is the caller graph for this function:

◆ updateCurrentStatus()

void platoon_strategic_ihp::PlatoonStrategicIHPPlugin::updateCurrentStatus	(	carma_planning_msgs::msg::Maneuver	maneuver,
		double &	speed,
		double &	current_progress,
		int &	lane_id
	)

Update maneuver status based on prior plan.

Parameters

maneuver	maneuver
speed	speed
current_progress	current progress
lane_id	lanelet ud

Definition at line 3206 of file platoon_strategic_ihp.cpp.

    {
        if(maneuver.type == carma_planning_msgs::msg::Maneuver::LANE_FOLLOWING){
            speed =  maneuver.lane_following_maneuver.end_speed;
            current_progress =  maneuver.lane_following_maneuver.end_dist;
            if (maneuver.lane_following_maneuver.lane_ids.empty())
            {
                RCLCPP_WARN_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "Lane id of lane following maneuver not set. Using 0");
                lane_id = 0;
            }
            else
            {
                lane_id =  stoi(maneuver.lane_following_maneuver.lane_ids[0]);
            }
        }
    }

Referenced by plan_maneuver_cb().

Here is the caller graph for this function:

◆ updatePlatoonList()

void platoon_strategic_ihp::PlatoonStrategicIHPPlugin::updatePlatoonList ( std::vector< PlatoonMember > platoon_list )

UCLA Setter: Update platoon list (Unit Test).

Definition at line 109 of file platoon_strategic_ihp.cpp.

    {
        pm_.host_platoon_ = platoon_list;
    }

References platoon_strategic_ihp::PlatoonManager::host_platoon_, and pm_.

Member Data Documentation

◆ candidate_follower_delay_count_

int platoon_strategic_ihp::PlatoonStrategicIHPPlugin::candidate_follower_delay_count_ = 0

private

Definition at line 786 of file platoon_strategic_ihp.h.

Referenced by run_candidate_follower().

◆ candidatestateStartTime

long platoon_strategic_ihp::PlatoonStrategicIHPPlugin::candidatestateStartTime = 0

private

Definition at line 336 of file platoon_strategic_ihp.h.

Referenced by mob_req_cb_leader(), mob_req_cb_leadwithoperation(), mob_resp_cb_follower(), mob_resp_cb_leader(), mob_resp_cb_preparetojoin(), run_candidate_follower(), run_candidate_leader(), run_leader_aborting(), and run_prepare_to_join().

◆ cmd_speed_

double platoon_strategic_ihp::PlatoonStrategicIHPPlugin::cmd_speed_ = 0

private

Definition at line 326 of file platoon_strategic_ihp.h.

Referenced by cmd_cb(), composeMobilityOperationSTATUS(), composePlatoonInfoMsg(), and onSpin().

◆ config_

PlatoonPluginConfig platoon_strategic_ihp::PlatoonStrategicIHPPlugin::config_

private

◆ current_crosstrack_

double platoon_strategic_ihp::PlatoonStrategicIHPPlugin::current_crosstrack_ = 0

private

Definition at line 332 of file platoon_strategic_ihp.h.

Referenced by isJoiningVehicleNearPlatoon(), isVehicleNearTargetPlatoon(), isVehicleRightBehind(), isVehicleRightInFront(), mob_op_cb_preparetojoin(), mob_req_cb_candidateleader(), mob_req_cb_leader(), mob_req_cb_leaderwaiting(), onSpin(), plan_maneuver_cb(), and pose_cb().

◆ current_downtrack_

double platoon_strategic_ihp::PlatoonStrategicIHPPlugin::current_downtrack_ = 0

private

Definition at line 330 of file platoon_strategic_ihp.h.

Referenced by composePlatoonInfoMsg(), isJoiningVehicleNearPlatoon(), isVehicleNearTargetPlatoon(), isVehicleRightBehind(), isVehicleRightInFront(), mob_op_cb_preparetojoin(), mob_req_cb_leader(), mob_req_cb_leadwithoperation(), onSpin(), plan_maneuver_cb(), pose_cb(), run_candidate_follower(), and run_prepare_to_join().

◆ current_speed_

double platoon_strategic_ihp::PlatoonStrategicIHPPlugin::current_speed_ = 0

private

Definition at line 328 of file platoon_strategic_ihp.h.

Referenced by composeMobilityOperationINFO(), composeMobilityOperationSTATUS(), composePlatoonInfoMsg(), mob_op_cb(), mob_op_cb_leader(), mob_op_cb_preparetojoin(), mob_req_cb(), mob_req_cb_leadwithoperation(), mob_resp_cb_preparetojoin(), onSpin(), plan_maneuver_cb(), run_candidate_follower(), run_leader(), run_leader_aborting(), run_prepare_to_join(), and twist_cb().

◆ desiredJoinGap

double platoon_strategic_ihp::PlatoonStrategicIHPPlugin::desiredJoinGap = 30.0

private

Definition at line 800 of file platoon_strategic_ihp.h.

◆ desiredJoinTimeGap

double platoon_strategic_ihp::PlatoonStrategicIHPPlugin::desiredJoinTimeGap = 4.0

private

Definition at line 801 of file platoon_strategic_ihp.h.

◆ georeference_

std::string platoon_strategic_ihp::PlatoonStrategicIHPPlugin::georeference_ {""}

private

Definition at line 773 of file platoon_strategic_ihp.h.

Referenced by georeference_cb().

◆ HostMobilityId

std::string platoon_strategic_ihp::PlatoonStrategicIHPPlugin::HostMobilityId = "hostid"

private

Definition at line 339 of file platoon_strategic_ihp.h.

◆ infoMessageInterval_

unsigned long platoon_strategic_ihp::PlatoonStrategicIHPPlugin::infoMessageInterval_ = 200

private

Definition at line 792 of file platoon_strategic_ihp.h.

Referenced by run_lead_with_operation(), and run_leader().

◆ JOIN_PARAMS

const std::string platoon_strategic_ihp::PlatoonStrategicIHPPlugin::JOIN_PARAMS = "SIZE:%1%,SPEED:%2%,ECEFX:%3%,ECEFY:%4%,ECEFZ:%5%,JOINIDX:%6%"

private

index = 0, SIZE, number of members. index = 1, SPEED, in m/s. index = 2, ECEFX, in cm. index = 3, ECEFY, in cm. index = 4, ECEFZ, in cm. index = 5, JOINIDX, index of the position in platoon the incoming vehicle tries to join note: The cut-in index is zero-based and points to the gap-leading vehicle's index. eg: for rear join, cut-in index == platoon.size()-1; for join from front, index == -1; for cut-in in middle, index indicate the gap leading vehicle's index.

Definition at line 859 of file platoon_strategic_ihp.h.

Referenced by mob_op_cb_leader(), mob_op_cb_preparetojoin(), mob_req_cb_leadwithoperation(), mob_resp_cb_preparetojoin(), run_leader_aborting(), and run_prepare_to_join().

◆ LANE_CHANGE_TIMEOUT

unsigned long platoon_strategic_ihp::PlatoonStrategicIHPPlugin::LANE_CHANGE_TIMEOUT = 300000

private

Definition at line 796 of file platoon_strategic_ihp.h.

◆ LEADER_TIMEOUT_COUNTER_LIMIT

int platoon_strategic_ihp::PlatoonStrategicIHPPlugin::LEADER_TIMEOUT_COUNTER_LIMIT = 5

private

Definition at line 798 of file platoon_strategic_ihp.h.

Referenced by run_follower().

◆ map_loaded_

bool platoon_strategic_ihp::PlatoonStrategicIHPPlugin::map_loaded_ = false

private

Definition at line 777 of file platoon_strategic_ihp.h.

◆ map_projector_

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

private

Definition at line 774 of file platoon_strategic_ihp.h.

Referenced by ecef_to_map_point(), georeference_cb(), and pose_to_ecef().

◆ maxAllowedJoinGap_

double platoon_strategic_ihp::PlatoonStrategicIHPPlugin::maxAllowedJoinGap_ = 90

private

Definition at line 789 of file platoon_strategic_ihp.h.

◆ maxAllowedJoinTimeGap_

double platoon_strategic_ihp::PlatoonStrategicIHPPlugin::maxAllowedJoinTimeGap_ = 15.0

private

Definition at line 788 of file platoon_strategic_ihp.h.

◆ maxPlatoonSize_

int platoon_strategic_ihp::PlatoonStrategicIHPPlugin::maxPlatoonSize_ = 10

private

Definition at line 790 of file platoon_strategic_ihp.h.

◆ mobility_operation_publisher_

MobilityOperationCB platoon_strategic_ihp::PlatoonStrategicIHPPlugin::mobility_operation_publisher_

private

Definition at line 310 of file platoon_strategic_ihp.h.

Referenced by run_candidate_follower(), run_candidate_leader(), run_follower(), run_lead_with_operation(), run_leader(), run_leader_aborting(), run_leader_waiting(), and run_prepare_to_join().

◆ mobility_request_publisher_

MobilityRequestCB platoon_strategic_ihp::PlatoonStrategicIHPPlugin::mobility_request_publisher_

private

Definition at line 308 of file platoon_strategic_ihp.h.

Referenced by mob_op_cb_leader(), mob_op_cb_preparetojoin(), mob_req_cb_leadwithoperation(), mob_resp_cb_preparetojoin(), run_candidate_follower(), run_leader_aborting(), and run_prepare_to_join().

◆ mobility_response_publisher_

MobilityResponseCB platoon_strategic_ihp::PlatoonStrategicIHPPlugin::mobility_response_publisher_

private

Definition at line 309 of file platoon_strategic_ihp.h.

Referenced by mob_req_cb().

◆ NEGOTIATION_TIMEOUT

unsigned long platoon_strategic_ihp::PlatoonStrategicIHPPlugin::NEGOTIATION_TIMEOUT = 15000

private

Definition at line 795 of file platoon_strategic_ihp.h.

Referenced by run_candidate_follower(), and run_leader().

◆ noLeaderUpdatesCounter

int platoon_strategic_ihp::PlatoonStrategicIHPPlugin::noLeaderUpdatesCounter = 0

private

Definition at line 797 of file platoon_strategic_ihp.h.

Referenced by run_follower().

◆ numLeaderAbortingCalls_

int platoon_strategic_ihp::PlatoonStrategicIHPPlugin::numLeaderAbortingCalls_ = 0

private

Definition at line 783 of file platoon_strategic_ihp.h.

Referenced by mob_resp_cb_leaderaborting(), and run_leader_aborting().

◆ OPERATION_INFO_PARAMS

const std::string platoon_strategic_ihp::PlatoonStrategicIHPPlugin::OPERATION_INFO_PARAMS = "INFO|LENGTH:%.2f,SPEED:%.2f,SIZE:%d,ECEFX:%.2f,ECEFY:%.2f,ECEFZ:%.2f"

private

index = 0, LENGTH, physical length of the platoon, in m. index = 1, SPEED, in m/s. index = 2, SIZE, number of members. index = 3, ECEFX, in cm. index = 4, ECEFY, in cm. index = 5, ECEFZ, in cm.

Definition at line 834 of file platoon_strategic_ihp.h.

Referenced by composeMobilityOperationINFO().

◆ OPERATION_INFO_TYPE

const std::string platoon_strategic_ihp::PlatoonStrategicIHPPlugin::OPERATION_INFO_TYPE = "INFO"

private

Definition at line 822 of file platoon_strategic_ihp.h.

Referenced by composeMobilityOperationLeader(), composeMobilityOperationLeadWithOperation(), mob_op_cb(), mob_op_cb_leader(), mob_op_cb_preparetojoin(), run_lead_with_operation(), and run_leader().

◆ OPERATION_STATUS_PARAMS

const std::string platoon_strategic_ihp::PlatoonStrategicIHPPlugin::OPERATION_STATUS_PARAMS = "STATUS|CMDSPEED:%1%,SPEED:%2%,ECEFX:%3%,ECEFY:%4%,ECEFZ:%5%"

private

index = 0, CMDSPEED, in m/s. index = 1, SPEED, in m/s. index = 2, ECEFX, in cm. index = 3, ECEFY, in cm. index = 4, ECEFZ, in cm.

Definition at line 843 of file platoon_strategic_ihp.h.

Referenced by composeMobilityOperationSTATUS().

◆ OPERATION_STATUS_TYPE

const std::string platoon_strategic_ihp::PlatoonStrategicIHPPlugin::OPERATION_STATUS_TYPE = "STATUS"

private

Definition at line 823 of file platoon_strategic_ihp.h.

Referenced by composeMobilityOperationLeader(), composeMobilityOperationLeadWithOperation(), mob_op_cb(), mob_op_cb_STATUS(), run_lead_with_operation(), and run_leader().

◆ platooning_info_publisher_

PlatooningInfoCB platoon_strategic_ihp::PlatoonStrategicIHPPlugin::platooning_info_publisher_

private

Definition at line 311 of file platoon_strategic_ihp.h.

Referenced by onSpin().

◆ PLATOONING_STRATEGY

const std::string platoon_strategic_ihp::PlatoonStrategicIHPPlugin::PLATOONING_STRATEGY = "Carma/Platooning"

private

Definition at line 821 of file platoon_strategic_ihp.h.

Referenced by composeMobilityOperationINFO(), composeMobilityOperationSTATUS(), handle_mob_req(), mob_op_cb(), mob_op_cb_leader(), mob_op_cb_preparetojoin(), mob_req_cb(), mob_req_cb_leadwithoperation(), mob_resp_cb_preparetojoin(), run_candidate_follower(), run_leader_aborting(), and run_prepare_to_join().

◆ plugin_discovery_msg_

carma_planning_msgs::msg::Plugin platoon_strategic_ihp::PlatoonStrategicIHPPlugin::plugin_discovery_msg_

private

Definition at line 780 of file platoon_strategic_ihp.h.

Referenced by PlatoonStrategicIHPPlugin().

◆ pm_

PlatoonManager platoon_strategic_ihp::PlatoonStrategicIHPPlugin::pm_

private

Definition at line 818 of file platoon_strategic_ihp.h.

Referenced by PlatoonStrategicIHPPlugin(), composeMobilityOperationINFO(), composeMobilityOperationSTATUS(), composePlatoonInfoMsg(), getHostPM(), handle_mob_req(), isJoiningVehicleNearPlatoon(), mob_op_cb(), mob_op_cb_leader(), mob_op_cb_preparetojoin(), mob_op_cb_STATUS(), mob_req_cb_candidateleader(), mob_req_cb_follower(), mob_req_cb_leader(), mob_req_cb_leaderwaiting(), mob_req_cb_leadwithoperation(), mob_resp_cb(), mob_resp_cb_candidatefollower(), mob_resp_cb_follower(), mob_resp_cb_leader(), mob_resp_cb_leaderaborting(), mob_resp_cb_preparetojoin(), onSpin(), plan_maneuver_cb(), pose_cb(), run_candidate_follower(), run_candidate_leader(), run_follower(), run_lead_with_operation(), run_leader(), run_leader_aborting(), run_leader_waiting(), run_prepare_to_join(), setPMState(), and updatePlatoonList().

◆ pose_ecef_point_

carma_v2x_msgs::msg::LocationECEF platoon_strategic_ihp::PlatoonStrategicIHPPlugin::pose_ecef_point_

Definition at line 257 of file platoon_strategic_ihp.h.

Referenced by composeMobilityOperationINFO(), composeMobilityOperationSTATUS(), mob_op_cb_leader(), mob_op_cb_preparetojoin(), mob_req_cb_leadwithoperation(), mob_resp_cb_preparetojoin(), run_leader_aborting(), run_prepare_to_join(), and setHostECEF().

◆ pose_msg_

geometry_msgs::msg::PoseStamped platoon_strategic_ihp::PlatoonStrategicIHPPlugin::pose_msg_

private

Definition at line 322 of file platoon_strategic_ihp.h.

Referenced by composePlatoonInfoMsg(), findLaneWidth(), mob_op_cb_leader(), mob_op_cb_preparetojoin(), mob_req_cb_leadwithoperation(), mob_resp_cb_preparetojoin(), plan_maneuver_cb(), pose_cb(), run_candidate_follower(), run_leader_aborting(), and run_prepare_to_join().

◆ prevHeartBeatTime_

unsigned long platoon_strategic_ihp::PlatoonStrategicIHPPlugin::prevHeartBeatTime_ = 0.0

private

Definition at line 793 of file platoon_strategic_ihp.h.

Referenced by run_lead_with_operation(), and run_leader().

◆ safeToLaneChange_

bool platoon_strategic_ihp::PlatoonStrategicIHPPlugin::safeToLaneChange_ = false

private

Definition at line 812 of file platoon_strategic_ihp.h.

Referenced by mob_op_cb_preparetojoin(), mob_resp_cb_preparetojoin(), plan_maneuver_cb(), and run_prepare_to_join().

◆ statusMessageInterval_

int platoon_strategic_ihp::PlatoonStrategicIHPPlugin::statusMessageInterval_ = 100

private

Definition at line 794 of file platoon_strategic_ihp.h.

Referenced by run_candidate_follower(), run_candidate_leader(), run_follower(), run_lead_with_operation(), run_leader(), run_leader_aborting(), and run_leader_waiting().

◆ STOPPED_SPEED

const double platoon_strategic_ihp::PlatoonStrategicIHPPlugin::STOPPED_SPEED = 0.5

private

Definition at line 806 of file platoon_strategic_ihp.h.

Referenced by mob_op_cb(), run_leader(), and twist_cb().

◆ target_cutin_pose_

lanelet::BasicPoint2d platoon_strategic_ihp::PlatoonStrategicIHPPlugin::target_cutin_pose_

private

Definition at line 803 of file platoon_strategic_ihp.h.

Referenced by mob_op_cb_leader(), and plan_maneuver_cb().

◆ target_join_index_

int platoon_strategic_ihp::PlatoonStrategicIHPPlugin::target_join_index_ = -2

private

Definition at line 809 of file platoon_strategic_ihp.h.

Referenced by mob_op_cb_preparetojoin(), mob_resp_cb_preparetojoin(), and run_prepare_to_join().

◆ timer_factory_

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

private

◆ vehicleLength_

double platoon_strategic_ihp::PlatoonStrategicIHPPlugin::vehicleLength_ = 5.0

private

Definition at line 791 of file platoon_strategic_ihp.h.

◆ waitingStartTime

long platoon_strategic_ihp::PlatoonStrategicIHPPlugin::waitingStartTime = 0

private

Definition at line 334 of file platoon_strategic_ihp.h.

Referenced by mob_req_cb_leader(), mob_req_cb_leadwithoperation(), and run_leader_waiting().

◆ waitingStateTimeout

double platoon_strategic_ihp::PlatoonStrategicIHPPlugin::waitingStateTimeout = 25.0

private

Definition at line 799 of file platoon_strategic_ihp.h.

Referenced by run_candidate_follower(), run_candidate_leader(), run_leader_aborting(), run_leader_waiting(), and run_prepare_to_join().

◆ wm_

carma_wm::WorldModelConstPtr platoon_strategic_ihp::PlatoonStrategicIHPPlugin::wm_

private

Definition at line 315 of file platoon_strategic_ihp.h.

Referenced by composeManeuverMessage(), composePlatoonInfoMsg(), findLaneWidth(), findSpeedLimit(), is_lanechange_possible(), mob_op_cb_leader(), mob_op_cb_preparetojoin(), mob_op_cb_STATUS(), mob_req_cb_candidateleader(), mob_req_cb_leader(), mob_req_cb_leaderwaiting(), mob_req_cb_leadwithoperation(), plan_maneuver_cb(), and pose_cb().

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

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

Public Member Functions

Public Attributes

Private Member Functions

Private Attributes

Detailed Description

Constructor & Destructor Documentation

◆ PlatoonStrategicIHPPlugin()

Member Function Documentation

◆ cmd_cb()

◆ composeLaneChangeManeuverMessage()

◆ composeManeuverMessage()

◆ composeMobilityOperationCandidateFollower()

◆ composeMobilityOperationCandidateLeader()

◆ composeMobilityOperationFollower()

◆ composeMobilityOperationINFO()

◆ composeMobilityOperationLeader()

◆ composeMobilityOperationLeaderAborting()

◆ composeMobilityOperationLeaderWaiting()

◆ composeMobilityOperationLeadWithOperation()

◆ composeMobilityOperationPrepareToJoin()

◆ composeMobilityOperationSTATUS()

◆ composePlatoonInfoMsg()

◆ ecef_to_map_point()

◆ find_target_lanelet_id()

◆ findLaneletIndexFromPath()

◆ findLaneWidth()

◆ findSpeedLimit()

◆ FRIEND_TEST() [1/2]

◆ FRIEND_TEST() [2/2]

◆ georeference_cb()

◆ getHostPM()

◆ handle_mob_req()

◆ is_lanechange_possible()

◆ isJoiningVehicleNearPlatoon()

◆ isVehicleNearTargetPlatoon()

◆ isVehicleRightBehind()

◆ isVehicleRightInFront()

◆ mob_op_cb()

◆ mob_op_cb_candidatefollower()

◆ mob_op_cb_candidateleader()

◆ mob_op_cb_follower()

◆ mob_op_cb_leader()

◆ mob_op_cb_leaderaborting()

◆ mob_op_cb_leaderwaiting()

◆ mob_op_cb_leadwithoperation()

◆ mob_op_cb_preparetojoin()

◆ mob_op_cb_standby()

◆ mob_op_cb_STATUS()

◆ mob_op_find_ecef_from_INFO_params()

◆ mob_op_find_ecef_from_STATUS_params()

◆ mob_op_find_platoon_length_from_INFO_params()

◆ mob_req_cb()

◆ mob_req_cb_candidatefollower()

◆ mob_req_cb_candidateleader()

◆ mob_req_cb_follower()

◆ mob_req_cb_leader()

◆ mob_req_cb_leaderaborting()

◆ mob_req_cb_leaderwaiting()

◆ mob_req_cb_leadwithoperation()

◆ mob_req_cb_preparetojoin()

◆ mob_req_cb_standby()

◆ mob_resp_cb()

◆ mob_resp_cb_candidatefollower()

◆ mob_resp_cb_candidateleader()

◆ mob_resp_cb_follower()

◆ mob_resp_cb_leader()

◆ mob_resp_cb_leaderaborting()

◆ mob_resp_cb_leaderwaiting()

◆ mob_resp_cb_leadwithoperation()

◆ mob_resp_cb_preparetojoin()

◆ mob_resp_cb_standby()

◆ onSpin()

◆ plan_maneuver_cb()

◆ pose_cb()

◆ pose_to_ecef()

◆ run_candidate_follower()

◆ run_candidate_leader()

◆ run_follower()

◆ run_lead_with_operation()

◆ run_leader()