#include <arbitrator.hpp>

Collaboration diagram for arbitrator::Arbitrator:

Public Member Functions
	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)
	Constructor for arbitrator class taking in dependencies via dependency injection. More...

void	run ()
	Begin the operation of the arbitrator. More...

void	twist_cb (geometry_msgs::msg::TwistStamped::UniquePtr msg)
	Callback for the twist subscriber, which will store latest twist locally. More...

void	bumper_pose_cb ()
	Callback for the front bumper pose transform. More...

void	initializeBumperTransformLookup ()
	Initialize transform Lookup from front bumper to map. More...

Protected Member Functions
void	initial_state ()
	Function to be executed during the initial state of the Arbitrator. More...

void	planning_state ()
	Function to be called when the Arbitrator begins planning. More...

void	waiting_state ()
	Function to be executed when the Arbitrator has finished planning and is awaiting another planning cycle. More...

void	paused_state ()
	Function to be executed when the Arbitrator is not planning but also not awaiting a new plan cycle. More...

void	shutdown_state ()
	Function to be executed when the Arbitrator is to clean up and shutdown. More...

void	guidance_state_cb (carma_planning_msgs::msg::GuidanceState::UniquePtr msg)
	Callback for receiving Guidance state machine updates. More...

Private Attributes
VehicleState	vehicle_state_

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_

tf2_ros::Buffer	tf2_buffer_

std::unique_ptr< tf2_ros::TransformListener >	tf2_listener_

tf2::Stamped< tf2::Transform >	bumper_transform_

bool	planning_in_progress_ = false

carma_planning_msgs::msg::GuidanceState::_state_type	previous_guidance_state_

Detailed Description

Primary work class for the Arbitrator package

Governs the interactions of plugins during the maneuver planning phase of the CARMA planning process. Utilizes a generic planning interface to allow for reconfiguration with other paradigms in the future.

Definition at line 71 of file arbitrator.hpp.

Constructor & Destructor Documentation

◆ Arbitrator()

arbitrator::Arbitrator::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
	)

inline

Constructor for arbitrator class taking in dependencies via dependency injection.

Parameters

nh	A CarmaLifecycleNode node pointer
sm	An ArbitratorStateMachine instance for regulating the states of the Arbitrator
ci	A CapabilitiesInterface for querying plugins
planning_strategy	A planning strategy implementation for generating plans
min_plan_duration	The minimum acceptable length of a plan
planning_frequency	The frequency at which to generate high-level plans when engaged
wm	pointer to an inialized world model.

Definition at line 84 of file arbitrator.hpp.

                                              :
                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()) {};

Member Function Documentation

◆ bumper_pose_cb()

void arbitrator::Arbitrator::bumper_pose_cb ( )

Callback for the front bumper pose transform.

Definition at line 187 of file arbitrator.cpp.

    {
        try
        {
            tf_ = tf2_buffer_.lookupTransform("map", "vehicle_front", rclcpp::Time(0), rclcpp::Duration(1.0, 0)); //save to local copy of transform 1 sec timeout
            tf2::fromMsg(tf_, bumper_transform_);
            vehicle_state_.stamp = tf_.header.stamp;
        }
        catch (const tf2::TransformException &ex)
        {
            RCLCPP_WARN_STREAM(nh_->get_logger(), ex.what());
        }
 
        vehicle_state_.x = bumper_transform_.getOrigin().getX();
        vehicle_state_.y = bumper_transform_.getOrigin().getY();
 
        // If the route is available then set the downtrack and lane id
        if (wm_->getRoute()) {
 
            vehicle_state_.downtrack = wm_->routeTrackPos( { vehicle_state_.x, vehicle_state_.y } ).downtrack;
 
            auto lanelets = wm_->getLaneletsBetween(vehicle_state_.downtrack, vehicle_state_.downtrack, true);
 
            if (lanelets.empty()) {
 
                RCLCPP_WARN_STREAM(nh_->get_logger(), "Vehicle is not in a lanelet.");
                vehicle_state_.lane_id = lanelet::InvalId;
 
            } else {
 
                vehicle_state_.lane_id = lanelets[0].id();
            }
        }
 
    }

References bumper_transform_, arbitrator::VehicleState::downtrack, arbitrator::VehicleState::lane_id, nh_, arbitrator::VehicleState::stamp, tf2_buffer_, tf_, vehicle_state_, wm_, arbitrator::VehicleState::x, and arbitrator::VehicleState::y.

◆ guidance_state_cb()

void arbitrator::Arbitrator::guidance_state_cb ( carma_planning_msgs::msg::GuidanceState::UniquePtr msg )

protected

Callback for receiving Guidance state machine updates.

Parameters

msg	The new GuidanceState message

Definition at line 67 of file arbitrator.cpp.

                                                                                       {
        // Flag to determine if this is a new state
        bool is_new_state = (msg->state != previous_guidance_state_);
 
        switch (msg->state) {
            case carma_planning_msgs::msg::GuidanceState::STARTUP:
                // NO-OP
                break;
            case carma_planning_msgs::msg::GuidanceState::DRIVERS_READY:
                if(sm_->get_state() == ArbitratorState::PLANNING ||
                   sm_->get_state() == ArbitratorState::WAITING) {
                    if (is_new_state) {
                        RCLCPP_INFO_STREAM(nh_->get_logger(),
                            "Guidance has been restarted, pausing arbitrator...");
                    }
                    sm_->submit_event(ArbitratorEvent::ARBITRATOR_PAUSED);
                }
                break;
            case carma_planning_msgs::msg::GuidanceState::ACTIVE:
                // NO-OP
                break;
            case carma_planning_msgs::msg::GuidanceState::ENGAGED:
                if (is_new_state) {
                    RCLCPP_INFO_STREAM(nh_->get_logger(),
                        "Guidance has been engaged!");
                }
                if (sm_->get_state() == ArbitratorState::INITIAL) {
                    sm_->submit_event(ArbitratorEvent::SYSTEM_STARTUP_COMPLETE);
                } else if (sm_->get_state() == ArbitratorState::PAUSED) {
                    sm_->submit_event(ArbitratorEvent::ARBITRATOR_RESUMED);
                }
                break;
            case carma_planning_msgs::msg::GuidanceState::INACTIVE:
                if (is_new_state) {
                    RCLCPP_INFO_STREAM(nh_->get_logger(),
                        "Guidance has been disengaged, pausing arbitrator.");
                }
                sm_->submit_event(ArbitratorEvent::ARBITRATOR_PAUSED);
                break;
            case carma_planning_msgs::msg::GuidanceState::SHUTDOWN:
                if (is_new_state) {
                    RCLCPP_INFO_STREAM(nh_->get_logger(),
                        "Guidance has been shutdown, shutting down arbitrator...");
                }
                sm_->submit_event(ArbitratorEvent::SYSTEM_SHUTDOWN_INITIATED);
                break;
            default:
                break;
        }
 
        // Update previous state
        previous_guidance_state_ = msg->state;
    }

References lightbar_manager::ACTIVE, arbitrator::ARBITRATOR_PAUSED, arbitrator::ARBITRATOR_RESUMED, lightbar_manager::ENGAGED, port_drayage_plugin::INACTIVE, arbitrator::INITIAL, nh_, arbitrator::PAUSED, arbitrator::PLANNING, previous_guidance_state_, arbitrator::SHUTDOWN, sm_, arbitrator::SYSTEM_SHUTDOWN_INITIATED, arbitrator::SYSTEM_STARTUP_COMPLETE, and arbitrator::WAITING.

Referenced by initial_state().

Here is the caller graph for this function:

◆ initial_state()

void arbitrator::Arbitrator::initial_state ( )

protected

Function to be executed during the initial state of the Arbitrator.

Definition at line 121 of file arbitrator.cpp.

    {
        if(!initialized_)
        {
            RCLCPP_INFO_STREAM(nh_->get_logger(), "Arbitrator initializing on first initial state spin...");
            final_plan_pub_ = nh_->create_publisher<carma_planning_msgs::msg::ManeuverPlan>("final_maneuver_plan", 5);
            guidance_state_sub_ = nh_->create_subscription<carma_planning_msgs::msg::GuidanceState>("guidance_state", 5, std::bind(&Arbitrator::guidance_state_cb, this, std::placeholders::_1));
            final_plan_pub_->on_activate();
 
            initialized_ = true;
        }
 
        std::this_thread::sleep_for(std::chrono::milliseconds(100));
    }

References final_plan_pub_, guidance_state_cb(), guidance_state_sub_, initialized_, and nh_.

Referenced by run().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ initializeBumperTransformLookup()

void arbitrator::Arbitrator::initializeBumperTransformLookup ( )

Initialize transform Lookup from front bumper to map.

Definition at line 228 of file arbitrator.cpp.

    {
        tf2_listener_.reset(new tf2_ros::TransformListener(tf2_buffer_));
        tf2_buffer_.setUsingDedicatedThread(true);
    }

References tf2_buffer_, and tf2_listener_.

◆ paused_state()

void arbitrator::Arbitrator::paused_state ( )

protected

Function to be executed when the Arbitrator is not planning but also not awaiting a new plan cycle.

Definition at line 176 of file arbitrator.cpp.

    {
       std::this_thread::sleep_for(std::chrono::milliseconds(100));
    }

Referenced by run().

Here is the caller graph for this function:

◆ planning_state()

void arbitrator::Arbitrator::planning_state ( )

protected

Function to be called when the Arbitrator begins planning.

Definition at line 136 of file arbitrator.cpp.

    {
        rclcpp::Time planning_process_start = nh_->get_clock()->now();
 
        carma_planning_msgs::msg::ManeuverPlan plan = planning_strategy_->generate_plan(vehicle_state_);
 
        if (!plan.maneuvers.empty())
        {
            rclcpp::Time plan_end_time = arbitrator_utils::get_plan_end_time(plan);
            rclcpp::Time plan_start_time = arbitrator_utils::get_plan_start_time(plan);
            rclcpp::Duration plan_duration = plan_end_time - plan_start_time;
 
            if (plan_duration < min_plan_duration_)
            {
                RCLCPP_WARN_STREAM(nh_->get_logger(), "Arbitrator is unable to generate a plan with minimum plan duration requirement!");
            }
            else
            {
                RCLCPP_DEBUG_STREAM(nh_->get_logger(),
                    "Arbitrator is publishing plan id: "
                    << std::string(plan.maneuver_plan_id)
                    << " of duration " << plan_duration.seconds() << " as current maneuver plan");
            }
            final_plan_pub_->publish(plan);
        }
        else
        {
            RCLCPP_WARN_STREAM(nh_->get_logger(), "Arbitrator was unable to generate a plan!");
        }
 
        next_planning_process_start_ = planning_process_start + time_between_plans_;
 
        sm_->submit_event(ArbitratorEvent::PLANNING_COMPLETE);
    }

References final_plan_pub_, arbitrator_utils::get_plan_end_time(), arbitrator_utils::get_plan_start_time(), min_plan_duration_, next_planning_process_start_, nh_, arbitrator::PLANNING_COMPLETE, planning_strategy_, sm_, time_between_plans_, and vehicle_state_.

Referenced by run().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ run()

void arbitrator::Arbitrator::run ( )

Begin the operation of the arbitrator.

Loops internally via rclcpp::Duration sleeps and spins

Definition at line 27 of file arbitrator.cpp.

    {
        if (!planning_in_progress_ && nh_->get_current_state().id() != lifecycle_msgs::msg::State::TRANSITION_STATE_SHUTTINGDOWN)
        {
            planning_in_progress_ = true;
 
            switch (sm_->get_state())
            {
                case INITIAL:
                    RCLCPP_INFO_STREAM(nh_->get_logger(), "Arbitrator spinning in INITIAL state.");
                    initial_state();
                    break;
                case PLANNING:
                    RCLCPP_INFO_STREAM(nh_->get_logger(), "Arbitrator spinning in PLANNING state.");
                    planning_state();
                    break;
                case WAITING:
                    RCLCPP_INFO_STREAM(nh_->get_logger(), "Arbitrator spinning in WAITING state.");
                    waiting_state();
                    break;
                case PAUSED:
                    RCLCPP_INFO_STREAM(nh_->get_logger(), "Arbitrator spinning in PAUSED state.");
                    paused_state();
                    break;
                case SHUTDOWN:
                    RCLCPP_INFO_STREAM(nh_->get_logger(), "Arbitrator shutting down after being commanded to shutdown!");
                    rclcpp::shutdown();
                    exit(0);
                    break;
                default:
                    throw std::invalid_argument("State machine attempting to process an illegal state value");
            }
        }
        else
        {
            return;
        }
        planning_in_progress_ = false;
    }

References arbitrator::INITIAL, initial_state(), nh_, arbitrator::PAUSED, paused_state(), arbitrator::PLANNING, planning_in_progress_, planning_state(), arbitrator::SHUTDOWN, sm_, arbitrator::WAITING, and waiting_state().

Here is the call graph for this function:

◆ shutdown_state()

void arbitrator::Arbitrator::shutdown_state ( )

protected

Function to be executed when the Arbitrator is to clean up and shutdown.

Definition at line 181 of file arbitrator.cpp.

    {
        RCLCPP_INFO_STREAM(nh_->get_logger(), "Arbitrator shutting down...");
        rclcpp::shutdown(); // Will stop upper level spin and shutdown node
    }

References nh_.

◆ twist_cb()

void arbitrator::Arbitrator::twist_cb ( geometry_msgs::msg::TwistStamped::UniquePtr msg )

Callback for the twist subscriber, which will store latest twist locally.

Parameters

msg	Latest twist message

Definition at line 223 of file arbitrator.cpp.

    {
        vehicle_state_.velocity = msg->twist.linear.x;
    }

References vehicle_state_, and arbitrator::VehicleState::velocity.

Referenced by arbitrator::ArbitratorNode::handle_on_configure().

Here is the caller graph for this function:

◆ waiting_state()

void arbitrator::Arbitrator::waiting_state ( )

protected

Function to be executed when the Arbitrator has finished planning and is awaiting another planning cycle.

Definition at line 171 of file arbitrator.cpp.

    {
        sm_->submit_event(ArbitratorEvent::PLANNING_TIMER_TRIGGER);
    }

References arbitrator::PLANNING_TIMER_TRIGGER, and sm_.

Referenced by run().

Here is the caller graph for this function:

Member Data Documentation

◆ bumper_transform_

tf2::Stamped<tf2::Transform> arbitrator::Arbitrator::bumper_transform_

private

Definition at line 179 of file arbitrator.hpp.

Referenced by bumper_pose_cb().

◆ capabilities_interface_

std::shared_ptr<CapabilitiesInterface> arbitrator::Arbitrator::capabilities_interface_

private

Definition at line 169 of file arbitrator.hpp.

◆ final_plan_pub_

carma_ros2_utils::PubPtr<carma_planning_msgs::msg::ManeuverPlan> arbitrator::Arbitrator::final_plan_pub_

private

Definition at line 163 of file arbitrator.hpp.

Referenced by initial_state(), and planning_state().

◆ guidance_state_sub_

carma_ros2_utils::SubPtr<carma_planning_msgs::msg::GuidanceState> arbitrator::Arbitrator::guidance_state_sub_

private

Definition at line 164 of file arbitrator.hpp.

Referenced by initial_state().

◆ initialized_

bool arbitrator::Arbitrator::initialized_

private

Definition at line 171 of file arbitrator.hpp.

Referenced by initial_state().

◆ min_plan_duration_

rclcpp::Duration arbitrator::Arbitrator::min_plan_duration_

private

Definition at line 166 of file arbitrator.hpp.

Referenced by planning_state().

◆ next_planning_process_start_

rclcpp::Time arbitrator::Arbitrator::next_planning_process_start_

private

Definition at line 168 of file arbitrator.hpp.

Referenced by planning_state().

◆ nh_

std::shared_ptr<carma_ros2_utils::CarmaLifecycleNode> arbitrator::Arbitrator::nh_

private

Definition at line 165 of file arbitrator.hpp.

Referenced by bumper_pose_cb(), guidance_state_cb(), initial_state(), planning_state(), run(), and shutdown_state().

◆ planning_in_progress_

bool arbitrator::Arbitrator::planning_in_progress_ = false

private

Definition at line 180 of file arbitrator.hpp.

Referenced by run().

◆ planning_strategy_

std::shared_ptr<PlanningStrategy> arbitrator::Arbitrator::planning_strategy_

private

Definition at line 170 of file arbitrator.hpp.

Referenced by planning_state().

◆ previous_guidance_state_

carma_planning_msgs::msg::GuidanceState::_state_type arbitrator::Arbitrator::previous_guidance_state_

private

Initial value:

=

carma_planning_msgs::msg::GuidanceState::STARTUP

Definition at line 181 of file arbitrator.hpp.

Referenced by guidance_state_cb().

◆ sm_

std::shared_ptr<ArbitratorStateMachine> arbitrator::Arbitrator::sm_

private

Definition at line 162 of file arbitrator.hpp.

Referenced by guidance_state_cb(), planning_state(), run(), and waiting_state().

◆ tf2_buffer_

tf2_ros::Buffer arbitrator::Arbitrator::tf2_buffer_

private

Definition at line 176 of file arbitrator.hpp.

Referenced by bumper_pose_cb(), and initializeBumperTransformLookup().

◆ tf2_listener_

std::unique_ptr<tf2_ros::TransformListener> arbitrator::Arbitrator::tf2_listener_

private

Definition at line 177 of file arbitrator.hpp.

Referenced by initializeBumperTransformLookup().

◆ tf_

geometry_msgs::msg::TransformStamped arbitrator::Arbitrator::tf_

private

Definition at line 174 of file arbitrator.hpp.

Referenced by bumper_pose_cb().

◆ time_between_plans_

rclcpp::Duration arbitrator::Arbitrator::time_between_plans_

private

Definition at line 167 of file arbitrator.hpp.

Referenced by planning_state().

◆ vehicle_state_

VehicleState arbitrator::Arbitrator::vehicle_state_

private

Definition at line 160 of file arbitrator.hpp.

Referenced by bumper_pose_cb(), planning_state(), and twist_cb().

◆ wm_

carma_wm::WorldModelConstPtr arbitrator::Arbitrator::wm_

private

Definition at line 172 of file arbitrator.hpp.

Referenced by bumper_pose_cb().

The documentation for this class was generated from the following files:

arbitrator/include/arbitrator.hpp
arbitrator/src/arbitrator.cpp

Public Member Functions

Protected Member Functions

Private Attributes

Detailed Description

Constructor & Destructor Documentation

◆ Arbitrator()

Member Function Documentation

◆ bumper_pose_cb()

◆ guidance_state_cb()

◆ initial_state()

◆ initializeBumperTransformLookup()

◆ paused_state()

◆ planning_state()

◆ run()

◆ shutdown_state()

◆ twist_cb()

◆ waiting_state()

Member Data Documentation

◆ bumper_transform_

◆ capabilities_interface_

◆ final_plan_pub_

◆ guidance_state_sub_

◆ initialized_

◆ min_plan_duration_

◆ next_planning_process_start_

◆ nh_

◆ planning_in_progress_

◆ planning_strategy_

◆ previous_guidance_state_

◆ sm_

◆ tf2_buffer_

◆ tf2_listener_

◆ tf_

◆ time_between_plans_

◆ vehicle_state_

◆ wm_