tree_planner.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_TREE_PLANNER_HPP__
#define __ARBITRATOR_INCLUDE_TREE_PLANNER_HPP__
 
#include <memory>
#include <carma_planning_msgs/msg/maneuver_plan.hpp>
#include "planning_strategy.hpp"
#include "cost_function.hpp"
#include "neighbor_generator.hpp"
#include "search_strategy.hpp"
#include "vehicle_state.hpp"
#include <boost/uuid/uuid.hpp>
#include <boost/uuid/uuid_io.hpp>
#include <boost/uuid/uuid_generators.hpp>
#include <rclcpp/rclcpp.hpp>
 
namespace arbitrator
{
    inline std::string maneuver_type_to_string(uint8_t type) {
        switch(type) {
            case carma_planning_msgs::msg::Maneuver::LANE_FOLLOWING:
                return "LANE_FOLLOWING";
            case carma_planning_msgs::msg::Maneuver::LANE_CHANGE:
                return "LANE_CHANGE";
            case carma_planning_msgs::msg::Maneuver::INTERSECTION_TRANSIT_STRAIGHT:
                return "INTERSECTION_TRANSIT_STRAIGHT";
            case carma_planning_msgs::msg::Maneuver::INTERSECTION_TRANSIT_LEFT_TURN:
                return "INTERSECTION_TRANSIT_LEFT_TURN";
            case carma_planning_msgs::msg::Maneuver::INTERSECTION_TRANSIT_RIGHT_TURN:
                return "INTERSECTION_TRANSIT_RIGHT_TURN";
            case carma_planning_msgs::msg::Maneuver::STOP_AND_WAIT:
                return "STOP_AND_WAIT";
            default:
                return "UNKNOWN";
        }
    }
    class TreePlanner : public PlanningStrategy
    {
        public:
            TreePlanner(std::shared_ptr<CostFunction> cf,
                std::shared_ptr<NeighborGenerator> ng,
                std::shared_ptr<SearchStrategy> ss,
                rclcpp::Duration target):
                cost_function_(cf),
                neighbor_generator_(ng),
                search_strategy_(ss),
                target_plan_duration_(target) {};
 
            carma_planning_msgs::msg::ManeuverPlan generate_plan(const VehicleState& start_state);
        protected:
            std::shared_ptr<CostFunction> cost_function_;
            std::shared_ptr<NeighborGenerator> neighbor_generator_;
            std::shared_ptr<SearchStrategy> search_strategy_;
            rclcpp::Duration target_plan_duration_;
    };
}
 
#endif //__ARBITRATOR_INCLUDE_TREE_PLANNER_HPP__