platoon_manager_ihp.h

Go to the documentation of this file.

#pragma once
 
/*
 * Copyright (C) 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
 */
 
 
#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_v2x_msgs/msg/plan_type.hpp>
#include <geometry_msgs/msg/pose_stamped.hpp>
#include <geometry_msgs/msg/twist_stamped.hpp>
#include <carma_wm/WMListener.hpp>
#include <carma_wm/WorldModel.hpp>
#include <boost/uuid/uuid_generators.hpp>
#include <boost/uuid/uuid_io.hpp>
#include <carma_ros2_utils/timers/TimerFactory.hpp>
#include "platoon_config_ihp.h"
 
namespace platoon_strategic_ihp
{
    struct ActionPlan 
    {
        bool            valid;          //is host currently negotiating/executing a join action?
        unsigned long   planStartTime;  //time that plan was initiated
        std::string     planId;         //ID of this action
        std::string     peerId;         //vehicle ID of the candidtate joining vehicle or leader of platoon we are joining
 
        ActionPlan() : valid(false), planStartTime(0), planId(""), peerId("") {}
 
        ActionPlan(bool valid, unsigned long planStartTime, std::string planId, std::string peerId): 
            valid(valid), planStartTime(planStartTime), planId(planId), peerId(peerId) {}  
    };
 
    enum MobilityRequestResponse 
    {
            ACK,
            NACK,
            NO_RESPONSE
    };
 
    enum PlatoonState
    {
        STANDBY,                    // 0;
        LEADERWAITING,              // 1;
        LEADER,                     // 2;
        CANDIDATEFOLLOWER,          // 3;
        FOLLOWER,                   // 4;
        //UCLA: FRONTAL JOIN STATE
        LEADERABORTING,             // 5;
        //UCLA: FRONTAL JOIN STATE
        CANDIDATELEADER,            // 6;
        //UCLA: CUT-IN JOIN STATE
        LEADWITHOPERATION,          // 7;
        //UCLA: CUT-IN JOIN STATE
        PREPARETOJOIN               // 8;
    };
 
    struct PlatoonMember
    {
        // Static ID is permanent ID for each vehicle
        std::string staticId;
        // Vehicle real time command speed in m/s.
        double commandSpeed;
        // Actual vehicle speed in m/s.
        double vehicleSpeed;
        // Vehicle current downtrack distance on the current route in m.
        double vehiclePosition;
        // Vehicle current crosstrack distance on the current route in m.
        double vehicleCrossTrack;
        // The local time stamp when the host vehicle update any informations of this member.
        long   timestamp;
 
        PlatoonMember(): staticId(""), commandSpeed(0.0), vehicleSpeed(0.0), vehiclePosition(0.0), vehicleCrossTrack(0.0), timestamp(0) {} 
 
        PlatoonMember(std::string staticId, double commandSpeed, double vehicleSpeed, double vehiclePosition, double vehicleCrossTrack, long timestamp): staticId(staticId),
            commandSpeed(commandSpeed), vehicleSpeed(vehicleSpeed), vehiclePosition(vehiclePosition), vehicleCrossTrack(vehicleCrossTrack), timestamp(timestamp) {}
    };
 
    class PlatoonManager
    {
    public:
 
        PlatoonManager(std::shared_ptr<carma_ros2_utils::timers::TimerFactory> timer_factory);
 
        void updateHostPose(const double downtrack, const double crosstrack);
 
        void updateHostSpeeds(const double cmdSpeed, const double actualSpeed);
 
        void hostMemberUpdates(const std::string& senderId, const std::string& platoonId, const std::string& params,
                               const double& DtD, const double& CtD);
 
        void neighborMemberUpdates(const std::string& senderId, const std::string& platoonId, const std::string& params,
                                   const double& DtD, const double& CtD);
 
        void updatesOrAddMemberInfo(std::vector<PlatoonMember>& platoon, std::string senderId, double cmdSpeed,
                                    double dtDistance, double ctDistance, double curSpeed);
        
        int getHostPlatoonSize();
 
        void clearActionPlan();
 
        void resetHostPlatoon();
 
        void resetNeighborPlatoon();
 
        bool removeMember(const size_t mem);
 
        bool removeMemberById(const std::string id);
        
        PlatoonMember getDynamicLeader();
 
        int allPredecessorFollowing();
 
        void changeFromFollowerToLeader();
        
        void changeFromLeaderToFollower(std::string newPlatoonId, std::string newLeaderId);
        
        int getNumberOfVehicleInFront();
        
        double getCurrentPlatoonLength();
 
        double getPlatoonRearDowntrackDistance();
        
        double getDistanceToPredVehicle();
        
        double getCurrentSpeed() const;
        
        double getCommandSpeed() const;
 
        double getCurrentDowntrackDistance() const;
 
        double getCurrentCrosstrackDistance() const;
 
        std::string getHostStaticID() const;
 
        double getPlatoonFrontDowntrackDistance();
 
        double getPredecessorTimeHeadwaySum();
        
        double getPredecessorSpeed();
 
        double getPredecessorPosition();
        
        double getIHPDesPosFollower(double dt);
 
        int getClosestIndex(double joinerDtD);
 
        double getCutInGap(const int gap_leading_index, const double joinerDtD);
 
        const std::string dummyID = "00000000-0000-0000-0000-000000000000";
 
        // List of members in the host's own platoon (host will always be represented, so size is never zero)
        std::vector<PlatoonMember> host_platoon_;
 
        // Platoon ID of the host's platoon
        std::string currentPlatoonID = dummyID; //dummy indicates not part of a platoon
 
        // Vehicle ID of the host's platoon leader (host may be the leader)
        std::string platoonLeaderID = dummyID;  //dummy indicates not part of a platoon
 
        // Current platooning state of the host vehicle
        PlatoonState current_platoon_state = PlatoonState::STANDBY;
 
        //index to the host_platoon_ vector that represents the host vehicle
        size_t hostPosInPlatoon_ = 0;
 
         // Plan that represents a joining activity only, it is NOT the ID of the platoon itself
        ActionPlan current_plan = ActionPlan();
 
        // Is the host a follower in its platoon?
        bool isFollower = false;
 
        // List of members in a detected neighbor platoon, which may be empty
        // CAUTION: we can only represent one neighbor platoon in this version, so if multiple platoons are nearby,
        //          code will get very confused and results are unpredictable.
        std::vector<PlatoonMember> neighbor_platoon_;
 
        // Num vehicles in the neighbor platoon, as indicated by the size field in the INFO message
        size_t neighbor_platoon_info_size_ = 0;
 
        // Platoon ID of the neighboring platoon
        std::string targetPlatoonID = dummyID;  //ID of a real platoon that we may be attempting to join
 
        std::string neighborPlatoonID = dummyID;  //ID of the neighbor platoon that we may be attempting to join (dummy if neighbor is a solo vehicle)
 
        // Vehicle ID of the neighbor platoon's leader
        std::string neighbor_platoon_leader_id_ = dummyID; //dummy indicates unknown
 
        // Is the record of neighbor platoon members complete (does it contain a record for every member)?
        bool is_neighbor_record_complete_ = false;
 
        // Current vehicle pose in map
        geometry_msgs::msg::PoseStamped pose_msg_;
 
        // host vehicle's static ID 
        std::string HostMobilityId = "default_host_id";
 
        // UCLA: add indicator of creating gap
        bool isCreateGap = false;
 
        size_t dynamic_leader_index_ = 0;
 
        // Timer factory used to get current time
        std::shared_ptr<carma_ros2_utils::timers::TimerFactory> timer_factory_;
        
    private:
 
        // local copy of configuration file
        PlatoonPluginConfig config_;
 
        std::string OPERATION_INFO_TYPE = "INFO";
        std::string OPERATION_STATUS_TYPE = "STATUS";
        std::string JOIN_AT_REAR_PARAMS = "SIZE:%1%,SPEED:%2%,DTD:%3%";
        // UCLA: add params for frontal join
        std::string JOIN_FROM_FRONT_PARAMS = "SIZE:%1%,SPEED:%2%,DTD:%3%";
        std::string  MOBILITY_STRATEGY = "Carma/Platooning";
 
        double minCutinGap_ = 22.0;                                  // m
        double maxCutinGap_ = 32.0;                                  // m
        std::string previousFunctionalDynamicLeaderID_ = "";
        int previousFunctionalDynamicLeaderIndex_ = -1;
 
       // note: APF related parameters are in config.h.
 
        double vehicleLength_ = 5.0;                            // the length of the vehicle, in m.
        double gapWithPred_ = 0.0;                              // time headway with predecessor, in s.
        double downtrack_progress_ = 0;                         // current downtrack distance, in m.
 
        // Note: Parameters for IHP platoon trajectory regulation are in config.h. 
 
        std::string algorithmType_ = "APF_ALGORITHM";           // a string that defines the current algorithm to determine the dynamic leader.
 
        bool insufficientGapWithPredecessor(double distanceToPredVehicle);
 
        std::vector<double> calculateTimeHeadway(std::vector<double> downtrackDistance, std::vector<double> speed) const;
 
        int determineDynamicLeaderBasedOnViolation(std::vector<double> timeHeadways);
        
        int findLowerBoundaryViolationClosestToTheHostVehicle(std::vector<double> timeHeadways) const;
 
        int findMaximumSpacingViolationClosestToTheHostVehicle(std::vector<double> timeHeadways) const;
 
        std::vector<double> getTimeHeadwayFromIndex(std::vector<double> timeHeadways, int start) const;
    };
}