Implementation of PlanningStrategy using a generic tree search algorithm. More...

#include <tree_planner.hpp>

Inheritance diagram for arbitrator::TreePlanner:

Collaboration diagram for arbitrator::TreePlanner:

Public Member Functions
	TreePlanner (std::shared_ptr< CostFunction > cf, std::shared_ptr< NeighborGenerator > ng, std::shared_ptr< SearchStrategy > ss, rclcpp::Duration target)
	Tree planner constructor. More...

carma_planning_msgs::msg::ManeuverPlan	generate_plan (const VehicleState &start_state)
	Utilize the configured cost function, neighbor generator, and search strategy, to generate a plan by means of tree search. More...

Public Member Functions inherited from arbitrator::PlanningStrategy
virtual carma_planning_msgs::msg::ManeuverPlan	generate_plan (const VehicleState &start_state)=0
	Generate a plausible maneuver plan. More...

virtual	~PlanningStrategy ()
	Virtual destructor provided for memory safety. More...

Protected Attributes
std::shared_ptr< CostFunction >	cost_function_

std::shared_ptr< NeighborGenerator >	neighbor_generator_

std::shared_ptr< SearchStrategy >	search_strategy_

rclcpp::Duration	target_plan_duration_

Detailed Description

Implementation of PlanningStrategy using a generic tree search algorithm.

The fundamental components of this tree search are individually injected into this class at construction time to allow for fine-tuning of the algorithm and ensure better testability and separation of algorithmic concerns

Definition at line 40 of file tree_planner.hpp.

Constructor & Destructor Documentation

◆ TreePlanner()

arbitrator::TreePlanner::TreePlanner	(	std::shared_ptr< CostFunction >	cf,
		std::shared_ptr< NeighborGenerator >	ng,
		std::shared_ptr< SearchStrategy >	ss,
		rclcpp::Duration	target
	)

inline

Tree planner constructor.

Parameters

cf	Shared ptr to a CostFunction implementation
ng	Shared ptr to a NeighborGenerator implementation
ss	Shared ptr to a SearchStrategy implementation
target	The desired duration of finished plans

Definition at line 50 of file tree_planner.hpp.

                                      :
                cost_function_(cf),
                neighbor_generator_(ng),
                search_strategy_(ss),
                target_plan_duration_(target) {};

Member Function Documentation

◆ generate_plan()

carma_planning_msgs::msg::ManeuverPlan arbitrator::TreePlanner::generate_plan ( const VehicleState & start_state )

virtual

Utilize the configured cost function, neighbor generator, and search strategy, to generate a plan by means of tree search.

Parameters

start_state The starting state of the vehicle to plan for

Implements arbitrator::PlanningStrategy.

Definition at line 25 of file tree_planner.cpp.

    {
        carma_planning_msgs::msg::ManeuverPlan root;
        std::vector<std::pair<carma_planning_msgs::msg::ManeuverPlan, double>> open_list_to_evaluate;
        std::vector<std::pair<carma_planning_msgs::msg::ManeuverPlan, double>> final_open_list;
 
        const double INF = std::numeric_limits<double>::infinity();
        open_list_to_evaluate.push_back(std::make_pair(root, INF));
 
        carma_planning_msgs::msg::ManeuverPlan longest_plan = root; // Track longest plan in case target length is never reached
        rclcpp::Duration longest_plan_duration = rclcpp::Duration(0);
 
 
        while (!open_list_to_evaluate.empty())
        {
            std::vector<std::pair<carma_planning_msgs::msg::ManeuverPlan, double>> temp_open_list;
 
            for (auto it = open_list_to_evaluate.begin(); it != open_list_to_evaluate.end(); it++)
            {
                // Pop the first element off the open list
                carma_planning_msgs::msg::ManeuverPlan cur_plan = it->first;
 
                RCLCPP_DEBUG_STREAM(rclcpp::get_logger("arbitrator"), "START");
 
                for (auto mvr : cur_plan.maneuvers)
                {
                    RCLCPP_DEBUG_STREAM(rclcpp::get_logger("arbitrator"), "Printing cur_plan: mvr: "<< (int)mvr.type);
                }
 
                RCLCPP_DEBUG_STREAM(rclcpp::get_logger("arbitrator"), "PRINT END");
 
                auto plan_duration = rclcpp::Duration(0, 0); // zero duration
 
                // If we're not at the root, plan_duration is nonzero (our plan should have maneuvers)
                if (!cur_plan.maneuvers.empty())
                {
                    // get plan duration
                    plan_duration = arbitrator_utils::get_plan_end_time(cur_plan) - arbitrator_utils::get_plan_start_time(cur_plan);
                }
 
                // save longest if none of the plans have enough target duration
                if (plan_duration > longest_plan_duration)
                {
                    longest_plan_duration = plan_duration;
                    longest_plan = cur_plan;
                }
 
                // Evaluate plan_duration is sufficient do not expand more
                if (plan_duration >= target_plan_duration_)
                {
                    final_open_list.push_back((*it));
                    RCLCPP_DEBUG_STREAM(rclcpp::get_logger("arbitrator"), "Has enough duration, skipping that which has following mvrs..:");
                    for (auto mvr : it->first.maneuvers)
                    {
                        RCLCPP_DEBUG_STREAM(rclcpp::get_logger("arbitrator"), "Printing mvr: "<< (int)mvr.type);
                    }
                    continue;
                }
 
                // Expand it, and reprioritize
                std::vector<carma_planning_msgs::msg::ManeuverPlan> children = neighbor_generator_->generate_neighbors(cur_plan, start_state);
 
                // Compute cost for each child and store in open list
                for (auto child = children.begin(); child != children.end(); child++)
                {
 
                    if (child->maneuvers.empty())
                    {
                        RCLCPP_WARN_STREAM(rclcpp::get_logger("arbitrator"), "Child was empty for id: " << std::string(child->maneuver_plan_id));
                        continue;
                    }
 
                    temp_open_list.push_back(std::make_pair(*child, cost_function_->compute_cost_per_unit_distance(*child)));
                }
            }
 
            open_list_to_evaluate = temp_open_list;
        }
 
        if (final_open_list.empty())
        {
            RCLCPP_ERROR_STREAM(rclcpp::get_logger("arbitrator"), "None of the strategic plugins generated any valid plans! Please check if any is turned and returning valid maneuvers...");
            throw std::runtime_error("None of the strategic plugins generated any valid plans! Please check if any is turned and returning valid maneuvers...");
        }
 
        final_open_list = search_strategy_->prioritize_plans(final_open_list);
 
        // now every plan has enough duration if possible and prioritized
        for (auto pair : final_open_list)
        {
            // Pop the first element off the open list
            carma_planning_msgs::msg::ManeuverPlan cur_plan = pair.first;
            rclcpp::Duration plan_duration(0,0); // zero duration
 
            // get plan duration
            plan_duration = arbitrator_utils::get_plan_end_time(cur_plan) - arbitrator_utils::get_plan_start_time(cur_plan);
 
            // Evaluate plan_duration is sufficient do not expand more
            if (plan_duration >= target_plan_duration_)
            {
                return cur_plan;
            }
        }
 
        // If no perfect match is found, return the longest plan that fit the criteria
        return longest_plan;
    }