platoon_strategic_ihp.h

Go to the documentation of this file.

/*
 * Copyright (C) 2019-2021 LEIDOS.
 *
 * Licensed under the Apache License, Version 2.0 (the "License"); you may not
 * use this file except in compliance with the License. You may obtain a copy of
 * the License at
 *
 * http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
 * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
 * License for the specific language governing permissions and limitations under
 * the License.
 */
 
/*
 * Developed by the UCLA Mobility Lab, 10/20/2021.
 *
 * Creator: Xu Han
 * Author: Xu Han, Xin Xia, Jiaqi Ma
 */
 
#pragma once
 
#include <gtest/gtest_prod.h>
#include <vector>
#include <math.h>
#include <carma_planning_msgs/msg/trajectory_plan.hpp>
#include <carma_planning_msgs/msg/trajectory_plan_point.hpp>
#include <carma_planning_msgs/msg/plugin.hpp>
#include <boost/shared_ptr.hpp>
#include <boost/uuid/uuid_generators.hpp>
#include <boost/uuid/uuid_io.hpp>
#include <boost/format.hpp>
#include <geometry_msgs/msg/pose_stamped.hpp>
#include <geometry_msgs/msg/twist_stamped.hpp>
#include <carma_planning_msgs/srv/plan_maneuvers.hpp>
#include <geometry_msgs/msg/pose_stamped.hpp>
#include <carma_v2x_msgs/msg/mobility_operation.hpp>
#include <carma_v2x_msgs/msg/mobility_request.hpp>
#include <carma_v2x_msgs/msg/mobility_response.hpp>
#include <carma_planning_msgs/msg/platooning_info.hpp>
#include <carma_v2x_msgs/msg/plan_type.hpp>
#include <carma_wm/WorldModel.hpp>
#include <lanelet2_core/geometry/Lanelet.h>
#include <lanelet2_core/geometry/BoundingBox.h>
#include <lanelet2_extension/traffic_rules/CarmaUSTrafficRules.h>
#include <lanelet2_extension/regulatory_elements/DigitalMinimumGap.h>
#include "platoon_config_ihp.h"
#include "platoon_manager_ihp.h"
#include <tf2_ros/transform_listener.h>
#include <tf2/LinearMath/Transform.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
#include <lanelet2_extension/projection/local_frame_projector.h>
#include <std_msgs/msg/string.hpp>
#include <rclcpp/time.hpp>
 
namespace platoon_strategic_ihp
{
    using MobilityResponseCB = std::function<void(const carma_v2x_msgs::msg::MobilityResponse&)>;
    using MobilityRequestCB = std::function<void(const carma_v2x_msgs::msg::MobilityRequest&)>;
    using MobilityOperationCB = std::function<void(const carma_v2x_msgs::msg::MobilityOperation&)>;
    using PlatooningInfoCB = std::function<void(const carma_planning_msgs::msg::PlatooningInfo&)>;
 
    class PlatoonStrategicIHPPlugin
    {
        public:
 
            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);
 
            void mob_op_cb(const carma_v2x_msgs::msg::MobilityOperation::UniquePtr msg);
 
            void mob_req_cb(const carma_v2x_msgs::msg::MobilityRequest::UniquePtr msg);
 
            void mob_resp_cb(const carma_v2x_msgs::msg::MobilityResponse::UniquePtr msg);
 
            MobilityRequestResponse handle_mob_req(const carma_v2x_msgs::msg::MobilityRequest& msg);
 
            bool plan_maneuver_cb(carma_planning_msgs::srv::PlanManeuvers::Request &req, carma_planning_msgs::srv::PlanManeuvers::Response &resp);
 
            int findLaneletIndexFromPath(int target_id, lanelet::routing::LaneletPath& path);
 
            double findLaneWidth();
 
            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);
 
            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);
 
            void updateCurrentStatus(carma_planning_msgs::msg::Maneuver maneuver, double& speed, double& current_progress, int& lane_id);
 
            void pose_cb(const geometry_msgs::msg::PoseStamped::UniquePtr msg);
 
            carma_planning_msgs::msg::PlatooningInfo composePlatoonInfoMsg();
 
            void twist_cb(const geometry_msgs::msg::TwistStamped::UniquePtr msg);
 
            void cmd_cb(const geometry_msgs::msg::TwistStamped::UniquePtr msg);
 
            void georeference_cb(const std_msgs::msg::String::UniquePtr msg);
 
            bool onSpin();
 
            void run_leader();
 
            void run_leader_waiting();
 
            void run_candidate_follower();
 
            void run_follower();
 
            // ECEF position of the host vehicle
            carma_v2x_msgs::msg::LocationECEF pose_ecef_point_;
 
            // -------------- UCLA: add two states for frontal join ------------------
            void run_leader_aborting();
 
            void run_candidate_leader();
 
            // -------------- UCLA: add two states for cut-in join ------------------
            void run_lead_with_operation();
 
            void run_prepare_to_join();
 
            void setHostECEF(carma_v2x_msgs::msg::LocationECEF pose_ecef_point);
 
            PlatoonManager getHostPM();
 
            void setPMState(PlatoonState desiredState);
 
            void updatePlatoonList(std::vector<PlatoonMember> platoon_list);
 
            void setConfig(const PlatoonPluginConfig& config);
 
        private:
 
            MobilityRequestCB mobility_request_publisher_;
            MobilityResponseCB mobility_response_publisher_;
            MobilityOperationCB mobility_operation_publisher_;
            PlatooningInfoCB platooning_info_publisher_;
 
 
            // pointer to the actual wm object
            carma_wm::WorldModelConstPtr wm_;
 
            // local copy of configuration file
            PlatoonPluginConfig config_;
 
            // local copy of pose
            // Current vehicle pose in map
            geometry_msgs::msg::PoseStamped pose_msg_;
 
            //Internal Variables used in unit tests
            // Current vehicle command speed, m/s
            double cmd_speed_ = 0;
            // Current vehicle measured speed, m/s
            double current_speed_ = 0;
            // Current vehicle downtrack distance in route, m
            double current_downtrack_ = 0;
            // Current vehicle crosstrack distance in route, m
            double current_crosstrack_ = 0;
            // start time of leaderwaiting state, s
            long waitingStartTime = 0;
            // start time of candidate follower state, s
            long candidatestateStartTime = 0;
 
            // Host Mobility ID
            std::string HostMobilityId = "hostid";
 
 
            bool isVehicleRightInFront(double downtrack, double crosstrack);
 
            double findSpeedLimit(const lanelet::ConstLanelet& llt);
 
            MobilityRequestResponse mob_req_cb_leader(const carma_v2x_msgs::msg::MobilityRequest& msg);
 
            MobilityRequestResponse mob_req_cb_leaderwaiting(const carma_v2x_msgs::msg::MobilityRequest& msg);
 
            MobilityRequestResponse mob_req_cb_follower(const carma_v2x_msgs::msg::MobilityRequest& msg);
 
            MobilityRequestResponse mob_req_cb_candidatefollower(const carma_v2x_msgs::msg::MobilityRequest& msg);
 
            MobilityRequestResponse mob_req_cb_standby(const carma_v2x_msgs::msg::MobilityRequest& msg);
 
            void mob_resp_cb_leader(const carma_v2x_msgs::msg::MobilityResponse& msg);
 
            void mob_resp_cb_leaderwaiting(const carma_v2x_msgs::msg::MobilityResponse& msg);
 
            void mob_resp_cb_follower(const carma_v2x_msgs::msg::MobilityResponse& msg);
 
            void mob_resp_cb_candidatefollower(const carma_v2x_msgs::msg::MobilityResponse& msg);
 
            void mob_resp_cb_standby(const carma_v2x_msgs::msg::MobilityResponse& msg);
 
            void mob_op_cb_leader(const carma_v2x_msgs::msg::MobilityOperation& msg);
 
            void mob_op_cb_leaderwaiting(const carma_v2x_msgs::msg::MobilityOperation& msg);
 
            void mob_op_cb_follower(const carma_v2x_msgs::msg::MobilityOperation& msg);
 
            void mob_op_cb_candidatefollower(const carma_v2x_msgs::msg::MobilityOperation& msg);
 
            void mob_op_cb_standby(const carma_v2x_msgs::msg::MobilityOperation& msg);
 
            carma_v2x_msgs::msg::MobilityOperation composeMobilityOperationLeader(const std::string& type);
 
            carma_v2x_msgs::msg::MobilityOperation composeMobilityOperationFollower();
 
            carma_v2x_msgs::msg::MobilityOperation composeMobilityOperationLeaderWaiting();
 
            carma_v2x_msgs::msg::MobilityOperation composeMobilityOperationCandidateFollower();
 
            carma_v2x_msgs::msg::LocationECEF pose_to_ecef(geometry_msgs::msg::PoseStamped pose_msg);
 
            lanelet::BasicPoint2d ecef_to_map_point(carma_v2x_msgs::msg::LocationECEF ecef_point);
 
            // -------------- UCLA implemented functions -----------------------
 
            // ----- 0. Extract data -------
 
            bool isVehicleRightBehind(double downtrack, double crosstrack);
 
            bool isJoiningVehicleNearPlatoon(double joining_downtrack, double joining_crosstrack);
 
            bool isVehicleNearTargetPlatoon(double rearVehicleDtd, double frontVehicleDtd, double frontVehicleCtd);
 
            int find_target_lanelet_id(double start_downtrack, double end_downtrack);
 
            // ----- 1. compose message ---------
 
            carma_v2x_msgs::msg::MobilityOperation composeMobilityOperationSTATUS();
 
            carma_v2x_msgs::msg::MobilityOperation composeMobilityOperationINFO();
 
            carma_v2x_msgs::msg::MobilityOperation composeMobilityOperationLeaderAborting();
 
            carma_v2x_msgs::msg::MobilityOperation composeMobilityOperationCandidateLeader();
 
            carma_v2x_msgs::msg::MobilityOperation composeMobilityOperationLeadWithOperation(const std::string& type);
 
            carma_v2x_msgs::msg::MobilityOperation composeMobilityOperationPrepareToJoin();
 
            // --------- 2. Mobility operation callback -----------
 
            void mob_op_cb_STATUS(const carma_v2x_msgs::msg::MobilityOperation& msg);
 
            double mob_op_find_platoon_length_from_INFO_params(std::string strategyParams);
 
            carma_v2x_msgs::msg::LocationECEF mob_op_find_ecef_from_INFO_params(std::string strategyParams);
 
            carma_v2x_msgs::msg::LocationECEF mob_op_find_ecef_from_STATUS_params(std::string strategyParams);
 
            void mob_op_cb_leaderaborting(const carma_v2x_msgs::msg::MobilityOperation& msg);
 
            void mob_op_cb_candidateleader(const carma_v2x_msgs::msg::MobilityOperation& msg);
 
            void mob_op_cb_leadwithoperation(const carma_v2x_msgs::msg::MobilityOperation& msg);
 
            void mob_op_cb_preparetojoin(const carma_v2x_msgs::msg::MobilityOperation& msg);
 
            //------- 3. Mobility request callback -----------
 
            MobilityRequestResponse mob_req_cb_leaderaborting(const carma_v2x_msgs::msg::MobilityRequest& msg);
 
            MobilityRequestResponse mob_req_cb_candidateleader(const carma_v2x_msgs::msg::MobilityRequest& msg);
 
            MobilityRequestResponse mob_req_cb_leadwithoperation(const carma_v2x_msgs::msg::MobilityRequest& msg);
 
            MobilityRequestResponse mob_req_cb_preparetojoin(const carma_v2x_msgs::msg::MobilityRequest& msg);
 
            // ------ 4. Mobility response callback ------
            void mob_resp_cb_leaderaborting(const carma_v2x_msgs::msg::MobilityResponse& msg);
 
            void mob_resp_cb_candidateleader(const carma_v2x_msgs::msg::MobilityResponse& msg);
 
            void mob_resp_cb_leadwithoperation(const carma_v2x_msgs::msg::MobilityResponse& msg);
 
            void mob_resp_cb_preparetojoin(const carma_v2x_msgs::msg::MobilityResponse& msg);
 
 
            bool is_lanechange_possible(lanelet::Id start_lanelet_id, lanelet::Id target_lanelet_id);
 
            // Pointer for map projector
            std::string georeference_{""};
            std::shared_ptr<lanelet::projection::LocalFrameProjector> map_projector_;
 
            // flag to check if map is loaded
            bool map_loaded_ = false;
 
            // Plugin discovery message
            carma_planning_msgs::msg::Plugin plugin_discovery_msg_;
 
            // Number of calls to the run_leader_aborting() method
            int numLeaderAbortingCalls_ = 0;
 
            // Number of calls to run_candidate_follower() method after MobReq message sent
            int candidate_follower_delay_count_ = 0;
 
            double maxAllowedJoinTimeGap_ = 15.0;
            double maxAllowedJoinGap_ = 90;
            int maxPlatoonSize_ = 10;
            double vehicleLength_ = 5.0;
            unsigned long infoMessageInterval_ = 200; // ms
            unsigned long prevHeartBeatTime_ = 0.0;
            int statusMessageInterval_ = 100; // ms
            unsigned long NEGOTIATION_TIMEOUT = 15000;  // ms
            unsigned long LANE_CHANGE_TIMEOUT = 300000; // ms (5 min)
            int noLeaderUpdatesCounter = 0;
            int LEADER_TIMEOUT_COUNTER_LIMIT = 5;
            double waitingStateTimeout = 25.0; // s
            double desiredJoinGap = 30.0; // m
            double desiredJoinTimeGap = 4.0; // s
 
            lanelet::BasicPoint2d target_cutin_pose_;
 
            // Speed below which platooning will not be attempted; non-zero value allows for sensor noise
            const double STOPPED_SPEED = 0.5; // m/s
 
            // UCLA: add member variable for state prepare to join (default -2, front join -1, mid/rear join other integer)
            int target_join_index_ = -2;
 
            // Is there a sufficient gap open in the platoon for a cut-in join?
            bool safeToLaneChange_ = false;
 
            // Interface for getting current time
            std::shared_ptr<carma_ros2_utils::timers::TimerFactory> timer_factory_;
 
            // Platoon Manager Object
            PlatoonManager pm_;
 
            // Strategy types
            const std::string PLATOONING_STRATEGY = "Carma/Platooning";
            const std::string OPERATION_INFO_TYPE = "INFO";
            const std::string OPERATION_STATUS_TYPE = "STATUS";
 
            // UCLA: edit INFO params to store platoon front info (front join needs 10 info params)
            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%";
 
            // JOIN Strategy Params
            const std::string JOIN_PARAMS = "SIZE:%1%,SPEED:%2%,ECEFX:%3%,ECEFY:%4%,ECEFZ:%5%,JOINIDX:%6%";
 
            // Unit Test Accessors
            FRIEND_TEST(PlatoonStrategicIHPPlugin, platoon_info_pub_front);
            FRIEND_TEST(PlatoonStrategicIHPPlugin, is_lanechange_possible);
    };
}