#include <arbitrator_node.hpp>

Inheritance diagram for arbitrator::ArbitratorNode:

Collaboration diagram for arbitrator::ArbitratorNode:

Public Member Functions
	ArbitratorNode (const rclcpp::NodeOptions &options)
	Constructor. More...

carma_ros2_utils::CallbackReturn	handle_on_configure (const rclcpp_lifecycle::State &)

carma_ros2_utils::CallbackReturn	handle_on_activate (const rclcpp_lifecycle::State &)

Private Member Functions
std::map< std::string, double >	plugin_priorities_map_from_json (const std::string &json_string)

Private Attributes
carma_ros2_utils::SubPtr< geometry_msgs::msg::TwistStamped >	twist_sub_

Config	config_

std::shared_ptr< carma_wm::WMListener >	wm_listener_

carma_wm::WorldModelConstPtr	wm_

std::shared_ptr< Arbitrator >	arbitrator_

rclcpp::TimerBase::SharedPtr	bumper_pose_timer_

rclcpp::TimerBase::SharedPtr	arbitrator_run_

Detailed Description

An arbitrator node instance with currently used configuration/planning paradigms

Governs the interactions of plugins during the maneuver planning phase of the CARMA planning process by internally using a worker class with generic planning interface.

Definition at line 49 of file arbitrator_node.hpp.

Constructor & Destructor Documentation

◆ ArbitratorNode()

arbitrator::ArbitratorNode::ArbitratorNode ( const rclcpp::NodeOptions & options )

explicit

Constructor.

Definition at line 22 of file arbitrator_node.cpp.

                                                                   : carma_ros2_utils::CarmaLifecycleNode(options)
    {
        // Create initial config
        config_ = Config();
 
        config_.min_plan_duration = declare_parameter<double>("min_plan_duration", config_.min_plan_duration);
        config_.target_plan_duration = declare_parameter<double>("target_plan_duration", config_.target_plan_duration);
        config_.planning_frequency = declare_parameter<double>("planning_frequency", config_.planning_frequency);
        config_.beam_width = declare_parameter<int>("beam_width", config_.beam_width);
        config_.use_fixed_costs = declare_parameter<bool>("use_fixed_costs", config_.use_fixed_costs);
        config_.plugin_priorities = plugin_priorities_map_from_json(declare_parameter<std::string>("plugin_priorities", ""));
    }

References arbitrator::Config::beam_width, config_, arbitrator::Config::min_plan_duration, arbitrator::Config::planning_frequency, arbitrator::Config::plugin_priorities, plugin_priorities_map_from_json(), arbitrator::Config::target_plan_duration, and arbitrator::Config::use_fixed_costs.

Here is the call graph for this function:

Member Function Documentation

◆ handle_on_activate()

carma_ros2_utils::CallbackReturn arbitrator::ArbitratorNode::handle_on_activate ( const rclcpp_lifecycle::State & )

Definition at line 96 of file arbitrator_node.cpp.

    {
        bumper_pose_timer_ = create_timer(get_clock(),
                                std::chrono::milliseconds(100),
                                [this]() {this->arbitrator_->bumper_pose_cb();});
 
        arbitrator_run_ = create_timer(get_clock(),
                                std::chrono::duration<double>(1/(config_.planning_frequency * 2 )), //there is waiting state between each planning state
                                [this]() {this->arbitrator_->run();});
        //The intention is to keep the arbitrator planning at intended frequency - 1s. Looks like ROS2 conversion kept the WAITING state from ROS1,
        //which occurs between PLANNING state in the state machine. So if the timer calls state callback each 1 second, the arbitrator ends up planning
        //every 2s due to PLANNING > WAITING > PLANNING transition. That's why the frequency is doubled.
        RCLCPP_INFO_STREAM(rclcpp::get_logger("arbitrator"), "Arbitrator started, beginning arbitrator state machine.");
        return CallbackReturn::SUCCESS;
    }

References arbitrator_, arbitrator_run_, bumper_pose_timer_, config_, and arbitrator::Config::planning_frequency.

◆ handle_on_configure()

carma_ros2_utils::CallbackReturn arbitrator::ArbitratorNode::handle_on_configure ( const rclcpp_lifecycle::State & )

Definition at line 35 of file arbitrator_node.cpp.

    {
        // Handle dependency injection
        auto ci = std::make_shared<arbitrator::CapabilitiesInterface>(shared_from_this());
        arbitrator::ArbitratorStateMachine sm;
 
        // Reset config
        config_ = Config();
 
        get_parameter<double>("min_plan_duration", config_.min_plan_duration);
        get_parameter<double>("target_plan_duration", config_.target_plan_duration);
        get_parameter<double>("planning_frequency", config_.planning_frequency);
        get_parameter<int>("beam_width", config_.beam_width);
        get_parameter<bool>("use_fixed_costs", config_.use_fixed_costs);
        std::string json_string;
        get_parameter<std::string>("plugin_priorities", json_string);
 
        config_.plugin_priorities = plugin_priorities_map_from_json(json_string);
 
        RCLCPP_INFO_STREAM(rclcpp::get_logger("arbitrator"), "Arbitrator Loaded Params: " << config_);
 
        std::shared_ptr<arbitrator::CostFunction> cf;
        arbitrator::CostSystemCostFunction cscf = arbitrator::CostSystemCostFunction();
 
        arbitrator::FixedPriorityCostFunction fpcf(config_.plugin_priorities);
        if (config_.use_fixed_costs) {
            cf = std::make_shared<arbitrator::FixedPriorityCostFunction>(fpcf);
        } else {
            cscf.init(shared_from_this());
            cf = std::make_shared<arbitrator::CostSystemCostFunction>(cscf);
        }
 
        auto bss = std::make_shared<arbitrator::BeamSearchStrategy>(config_.beam_width);
 
        auto png = std::make_shared<arbitrator::PluginNeighborGenerator<arbitrator::CapabilitiesInterface>>(ci);
        arbitrator::TreePlanner tp(cf, png, bss, rclcpp::Duration(config_.target_plan_duration* 1e9));
 
        wm_listener_ = std::make_shared<carma_wm::WMListener>(
            this->get_node_base_interface(), this->get_node_logging_interface(),
        this->get_node_topics_interface(), this->get_node_parameters_interface()
        );
 
        wm_ = wm_listener_->getWorldModel();
 
        arbitrator_ = std::make_shared<Arbitrator>(
            shared_from_this(),
            std::make_shared<ArbitratorStateMachine>(sm),
            ci,
            std::make_shared<TreePlanner>(tp),
            rclcpp::Duration(config_.min_plan_duration* 1e9),
            1/config_.planning_frequency,
            wm_ );
 
 
        twist_sub_ = create_subscription<geometry_msgs::msg::TwistStamped>("current_velocity", 1, std::bind(&Arbitrator::twist_cb, arbitrator_.get(), std::placeholders::_1));
 
        arbitrator_->initializeBumperTransformLookup();
 
        return CallbackReturn::SUCCESS;
    }

References arbitrator_, arbitrator::Config::beam_width, config_, arbitrator::CostSystemCostFunction::init(), arbitrator::Config::min_plan_duration, arbitrator::Config::planning_frequency, arbitrator::Config::plugin_priorities, plugin_priorities_map_from_json(), arbitrator::Config::target_plan_duration, arbitrator::Arbitrator::twist_cb(), twist_sub_, arbitrator::Config::use_fixed_costs, wm_, and wm_listener_.

Here is the call graph for this function:

◆ plugin_priorities_map_from_json()

std::map< std::string, double > arbitrator::ArbitratorNode::plugin_priorities_map_from_json ( const std::string & json_string )

private

Definition at line 112 of file arbitrator_node.cpp.

    {
        std::map<std::string, double> map;
 
        rapidjson::Document d;
        if(d.Parse(json_string.c_str()).HasParseError())
        {
            RCLCPP_WARN(rclcpp::get_logger("arbitrator"), "Failed to parse plugin_priorities map. Invalid json structure");
            return map;
        }
        if (!d.HasMember("plugin_priorities")) {
            RCLCPP_WARN(rclcpp::get_logger("arbitrator"), "No plugin_priorities found in arbitrator config");
            return map;
        }
        rapidjson::Value& map_value = d["plugin_priorities"];
 
        for (rapidjson::Value::ConstMemberIterator it = map_value.MemberBegin();
                                    it != map_value.MemberEnd(); it++)
        {
            map[it->name.GetString()] = it->value.GetDouble();
        }
        return map;
 
    }