arbitrator.hpp

Go to the documentation of this file.

/*
 * Copyright (C) 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.
 */
 
#ifndef __ARBITRATOR_INCLUDE_ARBITRATOR_HPP__
#define __ARBITRATOR_INCLUDE_ARBITRATOR_HPP__
 
#include <rclcpp/rclcpp.hpp>
#include <carma_ros2_utils/carma_lifecycle_node.hpp>
#include <carma_planning_msgs/msg/guidance_state.hpp>
#include <carma_wm/WorldModel.hpp>
#include <geometry_msgs/msg/pose_stamped.hpp>
#include <geometry_msgs/msg/twist_stamped.hpp>
#include <tf2_ros/transform_listener.h>
#include <tf2_ros/buffer.h>
#include <tf2/LinearMath/Transform.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
#include <boost/uuid/uuid.hpp>
#include <boost/uuid/uuid_io.hpp>
#include <boost/uuid/uuid_generators.hpp>
#include "vehicle_state.hpp"
#include "arbitrator_state_machine.hpp"
#include "planning_strategy.hpp"
#include "capabilities_interface.hpp"
 
namespace arbitrator
{
    inline std::string guidance_state_to_string(uint8_t state) {
        switch(state) {
            case carma_planning_msgs::msg::GuidanceState::STARTUP:
                return "STARTUP";
            case carma_planning_msgs::msg::GuidanceState::DRIVERS_READY:
                return "DRIVERS_READY";
            case carma_planning_msgs::msg::GuidanceState::ACTIVE:
                return "ACTIVE";
            case carma_planning_msgs::msg::GuidanceState::ENGAGED:
                return "ENGAGED";
            case carma_planning_msgs::msg::GuidanceState::INACTIVE:
                return "INACTIVE";
            case carma_planning_msgs::msg::GuidanceState::SHUTDOWN:
                return "SHUTDOWN";
            default:
                return "UNKNOWN";
        }
    }
 
    class Arbitrator
    {
        public:
            Arbitrator(std::shared_ptr<carma_ros2_utils::CarmaLifecycleNode> nh,
                std::shared_ptr<ArbitratorStateMachine> sm,
                std::shared_ptr<CapabilitiesInterface> ci,
                std::shared_ptr<PlanningStrategy> planning_strategy,
                rclcpp::Duration min_plan_duration,
                double planning_period,
                carma_wm::WorldModelConstPtr wm):
                sm_(sm),
                nh_(nh),
                min_plan_duration_(min_plan_duration),
                time_between_plans_(rclcpp::Duration::from_seconds(planning_period)),
                capabilities_interface_(ci),
                planning_strategy_(planning_strategy),
                initialized_(false),
                wm_(wm),
                tf2_buffer_(nh_->get_clock()) {};
 
            void run();
 
            void twist_cb(geometry_msgs::msg::TwistStamped::UniquePtr msg);
 
            void bumper_pose_cb();
 
            void initializeBumperTransformLookup();
 
        protected:
            void initial_state();
 
            void planning_state();
 
            void waiting_state();
 
            void paused_state();
 
            void shutdown_state();
 
            void guidance_state_cb(carma_planning_msgs::msg::GuidanceState::UniquePtr msg);
 
        private:
 
            VehicleState vehicle_state_; // The current state of the vehicle for populating planning requests
 
            std::shared_ptr<ArbitratorStateMachine> sm_;
            carma_ros2_utils::PubPtr<carma_planning_msgs::msg::ManeuverPlan> final_plan_pub_;
            carma_ros2_utils::SubPtr<carma_planning_msgs::msg::GuidanceState> guidance_state_sub_;
            std::shared_ptr<carma_ros2_utils::CarmaLifecycleNode> nh_;
            rclcpp::Duration min_plan_duration_;
            rclcpp::Duration time_between_plans_;
            rclcpp::Time next_planning_process_start_;
            std::shared_ptr<CapabilitiesInterface> capabilities_interface_;
            std::shared_ptr<PlanningStrategy> planning_strategy_;
            bool initialized_;
            carma_wm::WorldModelConstPtr wm_;
 
            geometry_msgs::msg::TransformStamped tf_;
            // TF listenser
            tf2_ros::Buffer tf2_buffer_;
            std::unique_ptr<tf2_ros::TransformListener> tf2_listener_;
            // transform from front bumper to map
            tf2::Stamped<tf2::Transform> bumper_transform_;
            bool planning_in_progress_ = false;
            carma_planning_msgs::msg::GuidanceState::_state_type previous_guidance_state_ =
                carma_planning_msgs::msg::GuidanceState::STARTUP;  // Initialize to default value
    };
}
 
#endif