26#include <gtest/gtest_prod.h>
29#include <carma_planning_msgs/msg/trajectory_plan.hpp>
30#include <carma_planning_msgs/msg/trajectory_plan_point.hpp>
31#include <carma_planning_msgs/msg/plugin.hpp>
32#include <boost/shared_ptr.hpp>
33#include <boost/uuid/uuid_generators.hpp>
34#include <boost/uuid/uuid_io.hpp>
35#include <boost/format.hpp>
36#include <geometry_msgs/msg/pose_stamped.hpp>
37#include <geometry_msgs/msg/twist_stamped.hpp>
38#include <carma_planning_msgs/srv/plan_maneuvers.hpp>
39#include <geometry_msgs/msg/pose_stamped.hpp>
40#include <carma_v2x_msgs/msg/mobility_operation.hpp>
41#include <carma_v2x_msgs/msg/mobility_request.hpp>
42#include <carma_v2x_msgs/msg/mobility_response.hpp>
43#include <carma_planning_msgs/msg/platooning_info.hpp>
44#include <carma_v2x_msgs/msg/plan_type.hpp>
46#include <lanelet2_core/geometry/Lanelet.h>
47#include <lanelet2_core/geometry/BoundingBox.h>
48#include <lanelet2_extension/traffic_rules/CarmaUSTrafficRules.h>
49#include <lanelet2_extension/regulatory_elements/DigitalMinimumGap.h>
52#include <tf2_ros/transform_listener.h>
53#include <tf2/LinearMath/Transform.h>
54#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
55#include <lanelet2_extension/projection/local_frame_projector.h>
56#include <std_msgs/msg/string.hpp>
57#include <rclcpp/time.hpp>
64 using PlatooningInfoCB = std::function<void(
const carma_planning_msgs::msg::PlatooningInfo&)>;
82 std::shared_ptr<carma_ros2_utils::timers::TimerFactory> timer_factory);
89 void mob_op_cb(
const carma_v2x_msgs::msg::MobilityOperation::UniquePtr msg);
96 void mob_req_cb(
const carma_v2x_msgs::msg::MobilityRequest::UniquePtr msg);
103 void mob_resp_cb(
const carma_v2x_msgs::msg::MobilityResponse::UniquePtr msg);
122 bool plan_maneuver_cb(carma_planning_msgs::srv::PlanManeuvers::Request &req, carma_planning_msgs::srv::PlanManeuvers::Response &resp);
153 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);
168 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);
178 void updateCurrentStatus(carma_planning_msgs::msg::Maneuver maneuver,
double& speed,
double& current_progress,
int& lane_id);
185 void pose_cb(
const geometry_msgs::msg::PoseStamped::UniquePtr msg);
217 void twist_cb(
const geometry_msgs::msg::TwistStamped::UniquePtr msg);
223 void cmd_cb(
const geometry_msgs::msg::TwistStamped::UniquePtr msg);
284 void setHostECEF(carma_v2x_msgs::msg::LocationECEF pose_ecef_point);
514 carma_v2x_msgs::msg::LocationECEF
pose_to_ecef(geometry_msgs::msg::PoseStamped pose_msg);
523 lanelet::BasicPoint2d
ecef_to_map_point(carma_v2x_msgs::msg::LocationECEF ecef_point);
859 const std::string
JOIN_PARAMS =
"SIZE:%1%,SPEED:%2%,ECEFX:%3%,ECEFY:%4%,ECEFZ:%5%,JOINIDX:%6%";
Class containing the logic for platoon manager. It is responsible for keeping track of the platoon me...
Class containing the logic for Strategic IHP Plugin. This class is responsible for all the negotiatio...
const std::string OPERATION_STATUS_TYPE
const std::string JOIN_PARAMS
void mob_op_cb_preparetojoin(const carma_v2x_msgs::msg::MobilityOperation &msg)
Function to process mobility operation in PrepareToJoin state (cut-in join)
void setPMState(PlatoonState desiredState)
UCLA Setter: function to set pm_.platoon_state.
bool isVehicleRightBehind(double downtrack, double crosstrack)
Function to determine if the given downtrack distance (m) is behind the host vehicle.
void georeference_cb(const std_msgs::msg::String::UniquePtr msg)
Callback for the georeference.
const std::string OPERATION_INFO_PARAMS
const std::string OPERATION_INFO_TYPE
void mob_resp_cb_leadwithoperation(const carma_v2x_msgs::msg::MobilityResponse &msg)
Function to process mobility response in leadwithoperation state (cut-in join)
MobilityResponseCB mobility_response_publisher_
MobilityRequestResponse mob_req_cb_candidatefollower(const carma_v2x_msgs::msg::MobilityRequest &msg)
Function to process mobility request in candidate follower state.
void setHostECEF(carma_v2x_msgs::msg::LocationECEF pose_ecef_point)
UCLA Update the private variable pose_ecef_point_.
carma_v2x_msgs::msg::MobilityOperation composeMobilityOperationLeadWithOperation(const std::string &type)
Function to compose mobility operation in LeadWithOperation (cut-in join)
void run_leader()
Run Leader State.
carma_v2x_msgs::msg::MobilityOperation composeMobilityOperationCandidateLeader()
Function to compose mobility operation in CandidateLeader.
void mob_resp_cb_leaderwaiting(const carma_v2x_msgs::msg::MobilityResponse &msg)
Function to process mobility response in leader waiting state.
MobilityRequestResponse mob_req_cb_preparetojoin(const carma_v2x_msgs::msg::MobilityRequest &msg)
Function to process mobility request in preparetojoin state (cut-in join)
int LEADER_TIMEOUT_COUNTER_LIMIT
void run_follower()
Run Follower State.
unsigned long NEGOTIATION_TIMEOUT
void mob_resp_cb_follower(const carma_v2x_msgs::msg::MobilityResponse &msg)
Function to process mobility response in follower state.
std::string HostMobilityId
long candidatestateStartTime
void updateCurrentStatus(carma_planning_msgs::msg::Maneuver maneuver, double &speed, double ¤t_progress, int &lane_id)
Update maneuver status based on prior plan.
double maxAllowedJoinGap_
std::string georeference_
void mob_op_cb_leader(const carma_v2x_msgs::msg::MobilityOperation &msg)
Function to process mobility operation in leader state.
carma_v2x_msgs::msg::MobilityOperation composeMobilityOperationINFO()
Function to compose mobility operation message with INFO params.
MobilityRequestResponse mob_req_cb_candidateleader(const carma_v2x_msgs::msg::MobilityRequest &msg)
Function to process mobility request in candidateleader state.
const std::string PLATOONING_STRATEGY
void mob_resp_cb(const carma_v2x_msgs::msg::MobilityResponse::UniquePtr msg)
Callback function for Mobility Response Message.
double desiredJoinTimeGap
std::shared_ptr< lanelet::projection::LocalFrameProjector > map_projector_
void mob_resp_cb_leaderaborting(const carma_v2x_msgs::msg::MobilityResponse &msg)
Function to process mobility response in leaderaborting state.
void mob_resp_cb_candidatefollower(const carma_v2x_msgs::msg::MobilityResponse &msg)
Function to process mobility response in candidate follower state.
geometry_msgs::msg::PoseStamped pose_msg_
carma_v2x_msgs::msg::MobilityOperation composeMobilityOperationFollower()
Function to compose mobility operation in follower state.
void mob_op_cb_leaderwaiting(const carma_v2x_msgs::msg::MobilityOperation &msg)
Function to process mobility operation in leader waiting state.
PlatooningStrategicIHPPlugin(carma_wm::WorldModelConstPtr wm, PlatooningPluginConfig 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.
MobilityOperationCB mobility_operation_publisher_
lanelet::BasicPoint2d target_cutin_pose_
carma_v2x_msgs::msg::LocationECEF pose_to_ecef(geometry_msgs::msg::PoseStamped pose_msg)
Function to convert pose from map frame to ecef location.
MobilityRequestResponse mob_req_cb_leader(const carma_v2x_msgs::msg::MobilityRequest &msg)
Function to process mobility request in leader state.
void mob_req_cb(const carma_v2x_msgs::msg::MobilityRequest::UniquePtr msg)
Callback function for Mobility Request Message.
bool isVehicleRightInFront(double downtrack, double crosstrack)
Function to determine if a target vehicle is in the front of host vehicle note: This is only applicab...
carma_planning_msgs::msg::Plugin plugin_discovery_msg_
FRIEND_TEST(PlatooningStrategicIHPPlugin, is_lanechange_possible)
void run_prepare_to_join()
UCLA Run prepare to join State.
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 ¤t_time)
Find start(current) and target(end) lanelet index from path to generate lane change maneuver message.
carma_v2x_msgs::msg::MobilityOperation composeMobilityOperationLeaderAborting()
Function to compose mobility operation in LeaderAborting state.
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 ...
void mob_op_cb_candidatefollower(const carma_v2x_msgs::msg::MobilityOperation &msg)
Function to process mobility operation in candidate follower state.
int statusMessageInterval_
void run_leader_aborting()
Run Leader Aborting State.
carma_planning_msgs::msg::PlatooningInfo composePlatoonInfoMsg()
Compose Platoon information message.
void mob_resp_cb_preparetojoin(const carma_v2x_msgs::msg::MobilityResponse &msg)
Function to process mobility response in preparetojoin state.
double current_downtrack_
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 v...
void run_leader_waiting()
Run Leader Waiting State.
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 scenar...
double current_crosstrack_
carma_planning_msgs::msg::Maneuver composeManeuverMessage(double current_dist, double end_dist, double current_speed, double target_speed, int lane_id, rclcpp::Time ¤t_time)
Find lanelet index from path.
MobilityRequestResponse mob_req_cb_leaderwaiting(const carma_v2x_msgs::msg::MobilityRequest &msg)
Function to process mobility request in leader waiting state.
void mob_resp_cb_leader(const carma_v2x_msgs::msg::MobilityResponse &msg)
Function to process mobility response in leader state.
carma_v2x_msgs::msg::MobilityOperation composeMobilityOperationLeaderWaiting()
Function to compose mobility operation in leader waiting state.
unsigned long LANE_CHANGE_TIMEOUT
const std::string OPERATION_STATUS_PARAMS
double maxAllowedJoinTimeGap_
int noLeaderUpdatesCounter
double findLaneWidth()
Find lanelet width from local position.
MobilityRequestResponse mob_req_cb_follower(const carma_v2x_msgs::msg::MobilityRequest &msg)
Function to process mobility request in follower state.
double findSpeedLimit(const lanelet::ConstLanelet &llt)
Function to find speed limit of a lanelet.
void mob_resp_cb_candidateleader(const carma_v2x_msgs::msg::MobilityResponse &msg)
Function to process mobility response in candidateleader state.
bool is_lanechange_possible(lanelet::Id start_lanelet_id, lanelet::Id target_lanelet_id)
Function to check if lanechange is possible.
PlatooningManager getHostPM()
UCLA Getter: for PlatooningManager class.
carma_wm::WorldModelConstPtr wm_
carma_v2x_msgs::msg::MobilityOperation composeMobilityOperationSTATUS()
Function to compose mobility operation message with STATUS params.
void twist_cb(const geometry_msgs::msg::TwistStamped::UniquePtr msg)
Callback for the twist subscriber, which will store latest twist locally.
void mob_op_cb_follower(const carma_v2x_msgs::msg::MobilityOperation &msg)
Function to process mobility operation in follower state.
unsigned long prevHeartBeatTime_
double waitingStateTimeout
carma_v2x_msgs::msg::MobilityOperation composeMobilityOperationLeader(const std::string &type)
Function to compose mobility operation in leader state.
void run_candidate_leader()
Run Candidate Leader State.
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 plat...
void mob_op_cb_standby(const carma_v2x_msgs::msg::MobilityOperation &msg)
Function to process mobility operation in standby state.
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.
carma_v2x_msgs::msg::MobilityOperation composeMobilityOperationPrepareToJoin()
Function to compose mobility operation in PrepareToJoin (cut-in join)
carma_v2x_msgs::msg::MobilityOperation composeMobilityOperationCandidateFollower()
Function to compose mobility operation in candidate follower state.
PlatooningPluginConfig config_
void mob_resp_cb_standby(const carma_v2x_msgs::msg::MobilityResponse &msg)
Function to process mobility response in standby state.
void run_candidate_follower()
Run Candidate Follower State.
MobilityRequestResponse mob_req_cb_leadwithoperation(const carma_v2x_msgs::msg::MobilityRequest &msg)
Function to process mobility request in leadwithoperation state (cut-in join)
carma_v2x_msgs::msg::LocationECEF pose_ecef_point_
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.
int candidate_follower_delay_count_
void updatePlatoonList(std::vector< PlatoonMember > platoon_list)
UCLA Setter: Update platoon list (Unit Test).
int numLeaderAbortingCalls_
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.
void run_lead_with_operation()
UCLA Run lead with operation State.
void cmd_cb(const geometry_msgs::msg::TwistStamped::UniquePtr msg)
Callback for the control command.
carma_v2x_msgs::msg::LocationECEF mob_op_find_ecef_from_STATUS_params(std::string strategyParams)
Function to process mobility operation for STATUS params.
bool onSpin()
Spin callback function.
PlatooningInfoCB platooning_info_publisher_
void setConfig(const PlatooningPluginConfig &config)
Set the current config.
void mob_op_cb_leaderaborting(const carma_v2x_msgs::msg::MobilityOperation &msg)
Function to process mobility operation in leaderaborting state.
void mob_op_cb_leadwithoperation(const carma_v2x_msgs::msg::MobilityOperation &msg)
Function to process mobility operation in LeadWithOperation state (cut-in join)
MobilityRequestCB mobility_request_publisher_
MobilityRequestResponse mob_req_cb_leaderaborting(const carma_v2x_msgs::msg::MobilityRequest &msg)
Function to process mobility request in leaderaborting state.
bool plan_maneuver_cb(carma_planning_msgs::srv::PlanManeuvers::Request &req, carma_planning_msgs::srv::PlanManeuvers::Response &resp)
Callback function to the maneuver request.
unsigned long infoMessageInterval_
void pose_cb(const geometry_msgs::msg::PoseStamped::UniquePtr msg)
Callback function for current pose.
const double STOPPED_SPEED
MobilityRequestResponse mob_req_cb_standby(const carma_v2x_msgs::msg::MobilityRequest &msg)
Function to process mobility request in standby state.
int findLaneletIndexFromPath(int target_id, lanelet::routing::LaneletPath &path)
Find lanelet index from path.
FRIEND_TEST(PlatooningStrategicIHPPlugin, platoon_info_pub_front)
void mob_op_cb(const carma_v2x_msgs::msg::MobilityOperation::UniquePtr msg)
Callback function for Mobility Operation Message.
MobilityRequestResponse handle_mob_req(const carma_v2x_msgs::msg::MobilityRequest &msg)
Function to the process and respond to the mobility request.
void mob_op_cb_candidateleader(const carma_v2x_msgs::msg::MobilityOperation &msg)
Function to process mobility operation in candidateleader state.
std::shared_ptr< carma_ros2_utils::timers::TimerFactory > timer_factory_
std::shared_ptr< const WorldModel > WorldModelConstPtr
MobilityRequestResponse
A response to an MobilityRequest message. ACK - indicates that the plugin accepts the MobilityRequest...
std::function< void(const carma_v2x_msgs::msg::MobilityRequest &)> MobilityRequestCB
std::function< void(const carma_planning_msgs::msg::PlatooningInfo &)> PlatooningInfoCB
PlatoonState
Platoon States. UCLA: Added additional states (i.e., LEADERABORTING and CANDIDATELEADER) for same-lan...
std::function< void(const carma_v2x_msgs::msg::MobilityOperation &)> MobilityOperationCB
std::function< void(const carma_v2x_msgs::msg::MobilityResponse &)> MobilityResponseCB
Stuct containing the algorithm configuration values for the yield_pluginConfig.