port_drayage_worker.hpp

Go to the documentation of this file.

#pragma once
 
/*
 * Copyright (C) 2020-2022 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.
 */
 
#include <rclcpp/rclcpp.hpp>
#include <functional>
#include <carma_v2x_msgs/msg/mobility_operation.hpp>
#include <carma_planning_msgs/msg/guidance_state.hpp>
#include <carma_planning_msgs/msg/route_event.hpp>
#include <carma_planning_msgs/srv/set_active_route.hpp>
#include <carma_msgs/msg/ui_instructions.hpp>
#include <geometry_msgs/msg/twist_stamped.hpp>
#include <geometry_msgs/msg/pose_stamped.hpp>
#include "std_msgs/msg/string.hpp"
#include "port_drayage_plugin/port_drayage_state_machine.hpp"
 
#include <lanelet2_extension/projection/local_frame_projector.h>
#include <boost/optional.hpp>
#include <boost/property_tree/ptree.hpp>
#include <boost/property_tree/json_parser.hpp>
 
namespace port_drayage_plugin
{
    struct PortDrayageMobilityOperationMsg
    {
        boost::optional<std::string> cargo_id;
        std::string operation = "";
        boost::optional<std::string> current_action_id; // Identifier for the action this message is related to
        boost::optional<double> dest_longitude;  // Destination longitude for the carma vehicle
        boost::optional<double> dest_latitude;   // Destination latitude for the carma vehicle
        boost::optional<double> start_longitude; // Starting longitude of the carma vehicle
        boost::optional<double> start_latitude;  // Starting latitude of the carma vehicle
    };
 
    struct LatLonCoordinate
    {
        double latitude = 0.0;
        double longitude = 0.0;
    };
 
    class OperationID
    {
        public:
            enum Operation {
                PICKUP,
                DROPOFF,
                ENTER_STAGING_AREA,
                EXIT_STAGING_AREA,
                ENTER_PORT,
                EXIT_PORT,
                PORT_CHECKPOINT,
                HOLDING_AREA,
                DEFAULT_OPERATION
            };
 
            OperationID(enum Operation op) :
                operation_enum_(op) {};
 
            OperationID::Operation getOperationID() const;
 
            std::string operationToString() const;
 
            friend std::ostream& operator<<(std::ostream& output, const OperationID& oid){
                return output << oid.operationToString();
            }
 
            friend bool operator==(const std::string& lhs, const OperationID& rhs) {
                return lhs == rhs.operationToString();
            }
 
        private:
            // Data member containing this object's Operation enum value
            const Operation operation_enum_ = Operation::DEFAULT_OPERATION;
    };
 
    class PortDrayageWorker
    {
        private:
            std::shared_ptr<carma_planning_msgs::msg::RouteEvent> latest_route_event_ = nullptr;
            PortDrayageStateMachine pdsm_;
            std::string previously_completed_operation_;
            std::string cmv_id_;
            std::string cargo_id_; // Empty if CMV is not currently carrying cargo
            std::function<void(carma_v2x_msgs::msg::MobilityOperation)> publish_mobility_operation_;
            std::function<void(carma_msgs::msg::UIInstructions)> publish_ui_instructions_;
            std::function<bool(std::shared_ptr<carma_planning_msgs::srv::SetActiveRoute::Request>)> call_set_active_route_service_;
            std::shared_ptr<lanelet::projection::LocalFrameProjector> map_projector_ = nullptr;
            std::string georeference_{""};
            bool starting_at_staging_area_; // Flag indicating CMV's first destination; 'true' indicates Staging Area Entrance; 'false' indicates Port Entrance.
            bool enable_port_drayage_; // Flag to enable to port drayage operations. If false, state machine will remain in 'INACTIVE' state
 
            // Data member for storing the strategy_params field of the last processed port drayage MobilityOperation message intended for this vehicle's cmv_id
            std::string previous_strategy_params_;
 
            // Constants
            const std::string PORT_DRAYAGE_PLUGIN_ID = "port_drayage_plugin";
            const std::string PORT_DRAYAGE_STRATEGY_ID = "carma/port_drayage";
            const std::string SET_GUIDANCE_ACTIVE_SERVICE_ID = "/guidance/set_guidance_active";
 
            // Clock for this object
            rclcpp::Clock::SharedPtr clock_;
 
            // Logger interface for this object
            rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr logger_;
 
        public:
 
            PortDrayageWorker(rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr logger,
                rclcpp::Clock::SharedPtr clock,
                std::function<void(carma_v2x_msgs::msg::MobilityOperation)> mobility_operations_publisher,
                std::function<void(carma_msgs::msg::UIInstructions)> ui_instructions_publisher,
                std::function<bool(std::shared_ptr<carma_planning_msgs::srv::SetActiveRoute::Request>)> call_set_active_route);
 
            void setVehicleID(const std::string& cmv_id);
 
            void setCargoID(const std::string& cargo_id);
 
            void setEnablePortDrayageFlag(bool enable_port_drayage);
 
            void setStartingAtStagingAreaFlag(bool starting_at_staging_area);
 
            void onArrivedAtDestination();
 
            void onReceivedNewDestination();
 
            std::shared_ptr<carma_planning_msgs::srv::SetActiveRoute::Request> composeSetActiveRouteRequest(boost::optional<double> dest_latitude, boost::optional<double> dest_longitude) const;
 
            carma_msgs::msg::UIInstructions composeUIInstructions(const std::string& current_operation, const std::string& previous_operation);
 
            carma_v2x_msgs::msg::MobilityOperation composeArrivalMessage() const;
 
            void onNewGeoreference(std_msgs::msg::String::UniquePtr msg);
 
            void onNewPose(geometry_msgs::msg::PoseStamped::UniquePtr msg);
 
            void onInboundMobilityOperation(carma_v2x_msgs::msg::MobilityOperation::UniquePtr msg);
 
            void updateCargoInformationAfterActionCompletion(const PortDrayageMobilityOperationMsg& previous_port_drayage_msg);
 
            void mobilityOperationMessageParser(std::string mobility_operation_strategy_params);
 
            void onGuidanceState(const carma_planning_msgs::msg::GuidanceState::UniquePtr msg);
 
            void onRouteEvent(const carma_planning_msgs::msg::RouteEvent::UniquePtr msg);
 
            PortDrayageState getPortDrayageState();
 
            // PortDrayageMobilityOperationMsg object for storing strategy_params data of a received port drayage MobilityOperation message intended for this vehicle's cmv_id
            PortDrayageMobilityOperationMsg latest_mobility_operation_msg_;
 
            // LatLonCoordinate object for storing the vehicle's current gps latitude/longitude coordinates
            LatLonCoordinate current_gps_position_;
    };
} // namespace port_drayage_plugin