guidance::GuidanceStateMachine Class Reference

#include <guidance_state_machine.hpp>

Collaboration diagram for guidance::GuidanceStateMachine:

Public Types
enum	Signal {   INITIALIZED = 0 , ACTIVATED = 1 , ENGAGE = 2 , DISENGAGED = 3 ,   SHUTDOWN = 4 , OVERRIDE = 5 , PARK = 6 }
enum	State {   OFF = 0 , STARTUP = 1 , DRIVERS_READY = 2 , ACTIVE = 3 ,   ENGAGED = 4 , INACTIVE = 5 , ENTER_PARK = 6 }

Public Member Functions
	GuidanceStateMachine (rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr logger)
	constructor for GuidanceStateMachine More...
void	onVehicleStatus (autoware_msgs::msg::VehicleStatus::UniquePtr msg)
	Handle vehicle status message from ROS network. More...
void	onGuidanceInitialized ()
	Updates Guidance State Machine with INITIALIZED signal. More...
void	onGuidanceShutdown ()
	Updates Guidance State Machine with SHUTDOWN signal. More...
void	onSetGuidanceActive (bool msg)
	Handle set_guidance_active service call from ROS network. More...
void	onRoboticStatus (carma_driver_msgs::msg::RobotEnabled::UniquePtr msg)
	Handle robotic_status message from ROS network. More...
void	onRouteEvent (carma_planning_msgs::msg::RouteEvent::UniquePtr msg)
	Handle route event message. More...
bool	shouldCallSetEnableRobotic ()
	Indicate if SetEnableRobotic needs to be called in ACTIVE state. More...
uint8_t	getCurrentState ()
	Get current state machine status. More...

Private Member Functions
void	onGuidanceSignal (Signal signal)
	Actual state machine logic driven by signal enum. More...

Private Attributes
State	current_guidance_state_ {State::STARTUP}
bool	robotic_active_status_ {false}
bool	called_robotic_engage_in_active_ {false}
double	current_velocity_ = 0.0
rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr	logger_

Detailed Description

Definition at line 28 of file guidance_state_machine.hpp.

Member Enumeration Documentation

Signal

enum guidance::GuidanceStateMachine::Signal

Enumerator
INITIALIZED
ACTIVATED
ENGAGE
DISENGAGED
SHUTDOWN
OVERRIDE
PARK

Definition at line 32 of file guidance_state_machine.hpp.

            {
                INITIALIZED = 0,
                ACTIVATED = 1,
                ENGAGE = 2,
                DISENGAGED = 3,
                SHUTDOWN = 4,
                OVERRIDE = 5,
                PARK = 6,
            };

State

enum guidance::GuidanceStateMachine::State

Enumerator
OFF
STARTUP
DRIVERS_READY
ACTIVE
ENGAGED
INACTIVE
ENTER_PARK

Definition at line 43 of file guidance_state_machine.hpp.

            {
                OFF = 0,
                STARTUP = 1,
                DRIVERS_READY = 2,
                ACTIVE = 3,
                ENGAGED = 4,
                INACTIVE = 5,
                ENTER_PARK = 6,
            };

Constructor & Destructor Documentation

GuidanceStateMachine()

guidance::GuidanceStateMachine::GuidanceStateMachine

(

rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr

logger

)

constructor for GuidanceStateMachine

Parameters

logger

The logger interface that will be used by this object

Definition at line 21 of file guidance_state_machine.cpp.

        : logger_(logger) {}

Member Function Documentation

getCurrentState()

uint8_t guidance::GuidanceStateMachine::getCurrentState

(

)

Get current state machine status.

Definition at line 157 of file guidance_state_machine.cpp.

    {
        return static_cast<uint8_t>(current_guidance_state_);
    }

References current_guidance_state_.

Referenced by guidance::GuidanceWorker::guidance_activation_cb(), and guidance::GuidanceWorker::spin_cb().

Here is the caller graph for this function:

onGuidanceInitialized()

void guidance::GuidanceStateMachine::onGuidanceInitialized

(

)

Updates Guidance State Machine with INITIALIZED signal.

Definition at line 103 of file guidance_state_machine.cpp.

    {
        onGuidanceSignal(Signal::INITIALIZED);
    }

References onGuidanceSignal().

Referenced by guidance::GuidanceWorker::handle_on_activate().

Here is the call graph for this function:

Here is the caller graph for this function:

onGuidanceShutdown()

void guidance::GuidanceStateMachine::onGuidanceShutdown

(

)

Updates Guidance State Machine with SHUTDOWN signal.

Definition at line 108 of file guidance_state_machine.cpp.

    {
        onGuidanceSignal(Signal::SHUTDOWN);
    }

References onGuidanceSignal(), and arbitrator::SHUTDOWN.

Referenced by guidance::GuidanceWorker::handle_on_shutdown().

Here is the call graph for this function:

Here is the caller graph for this function:

onGuidanceSignal()

void guidance::GuidanceStateMachine::onGuidanceSignal ( Signal  signal )

private

Actual state machine logic driven by signal enum.

Definition at line 24 of file guidance_state_machine.cpp.

    {
        // Set to OFF state of SHUTDOWN signal received
        if(signal == Signal::SHUTDOWN) {
            current_guidance_state_ = State::OFF;
            return;
        }
        switch(current_guidance_state_)
        {
            case State::STARTUP:
                if(signal == Signal::INITIALIZED) {
                    current_guidance_state_ = State::DRIVERS_READY;
                }
                break;
            case State::DRIVERS_READY:
                if(signal == Signal::ACTIVATED)
                {
                    current_guidance_state_ = State::ACTIVE;
                }
                break;
            case State::ACTIVE:
                if(signal == Signal::ENGAGE)
                {
                    current_guidance_state_ = State::ENGAGED;
                } else if(signal == Signal::DISENGAGED)
                {
                    current_guidance_state_ = State::DRIVERS_READY;
                }
                break;
            case State::ENGAGED:
                if(signal == Signal::DISENGAGED)
                {
                    current_guidance_state_ = State::DRIVERS_READY;
                } else if(signal == Signal::OVERRIDE)
                {
                    current_guidance_state_ = State::INACTIVE;
                } else if(signal == Signal::PARK)
                {
                    current_guidance_state_ = State::ENTER_PARK;
                }
                break;
            case State::INACTIVE:
                if(signal == Signal::ACTIVATED)
                {
                    current_guidance_state_ = State::ACTIVE;
                }else if(signal == Signal::INITIALIZED){
                    current_guidance_state_ = State::DRIVERS_READY;
                }
                break;
            case State::ENTER_PARK:
                if(signal == Signal::OVERRIDE)
                {
                    current_guidance_state_ = State::INACTIVE;
                } 
                else if(signal == Signal::DISENGAGED)
                {
                    current_guidance_state_ = State::DRIVERS_READY;
                }
                break;
            default:
                break;
        }
    }

References lightbar_manager::ACTIVE, current_guidance_state_, lightbar_manager::DISENGAGED, lightbar_manager::ENGAGED, port_drayage_plugin::INACTIVE, lightbar_manager::OFF, and arbitrator::SHUTDOWN.

Referenced by onGuidanceInitialized(), onGuidanceShutdown(), onRoboticStatus(), onRouteEvent(), onSetGuidanceActive(), and onVehicleStatus().

Here is the caller graph for this function:

onRoboticStatus()

void guidance::GuidanceStateMachine::onRoboticStatus

(

carma_driver_msgs::msg::RobotEnabled::UniquePtr

msg

)

Handle robotic_status message from ROS network.

Definition at line 125 of file guidance_state_machine.cpp.

    {
        // robotic status changes from false to true
        if(!robotic_active_status_ && msg->robot_active)
        {
            robotic_active_status_ = true;
            onGuidanceSignal(Signal::ENGAGE);
        }
        // robotic status changes from true to false
        else if(robotic_active_status_ && !msg->robot_active)
        {
            robotic_active_status_ = false;
            onGuidanceSignal(Signal::OVERRIDE);
        }
    }

References onGuidanceSignal(), and robotic_active_status_.

Referenced by guidance::GuidanceWorker::robot_status_cb().

Here is the call graph for this function:

Here is the caller graph for this function:

onRouteEvent()

void guidance::GuidanceStateMachine::onRouteEvent

(

carma_planning_msgs::msg::RouteEvent::UniquePtr

msg

)

Handle route event message.

Definition at line 141 of file guidance_state_machine.cpp.

    {
        if(msg->event == carma_planning_msgs::msg::RouteEvent::ROUTE_DEPARTED ||
           msg->event == carma_planning_msgs::msg::RouteEvent::ROUTE_ABORTED) {
               onGuidanceSignal(Signal::DISENGAGED);
           }
        else if(msg->event == carma_planning_msgs::msg::RouteEvent::ROUTE_COMPLETED){
            if (fabs(current_velocity_) < 0.001) { // Check we have successfully stopped
                onGuidanceSignal(Signal::PARK); // ENGAGED -> ENTER_PARK this state restricts transitioning out of ENTER_PARK until vehicle is shifted to PARK
            } else { // Vehicle was not able to stop transition to inactive
                RCLCPP_WARN_STREAM(logger_->get_logger(), "Vehicle failed to park on route completion because current velocity was " << current_velocity_);
                onGuidanceSignal(Signal::OVERRIDE);
            }
        }
    }

References current_velocity_, lightbar_manager::DISENGAGED, logger_, and onGuidanceSignal().

Referenced by guidance::GuidanceWorker::route_event_cb().

Here is the call graph for this function:

Here is the caller graph for this function:

onSetGuidanceActive()

void guidance::GuidanceStateMachine::onSetGuidanceActive

(

bool

msg

)

Handle set_guidance_active service call from ROS network.

Definition at line 113 of file guidance_state_machine.cpp.

    {
        if(msg)
        {
            onGuidanceSignal(Signal::ACTIVATED);
        } else
        {
            onGuidanceSignal(Signal::DISENGAGED);
        }
        
    }

References lightbar_manager::DISENGAGED, and onGuidanceSignal().

Referenced by guidance::GuidanceWorker::guidance_activation_cb().

Here is the call graph for this function:

Here is the caller graph for this function:

onVehicleStatus()

void guidance::GuidanceStateMachine::onVehicleStatus

(

autoware_msgs::msg::VehicleStatus::UniquePtr

msg

)

Handle vehicle status message from ROS network.

Definition at line 88 of file guidance_state_machine.cpp.

    {
        current_velocity_ = msg->speed * 0.277777; // Convert kilometers per hour to meters per second. Rounded down so that it comes under epsilon for parking check
        if (current_guidance_state_ == State::ENTER_PARK)
        {
            // '3' indicates vehicle gearshift is currently set to PARK
            if(msg->current_gear.gear== autoware_msgs::msg::Gear::PARK)
            {
                onGuidanceSignal(Signal::OVERRIDE); // Required for ENTER_PARK -> INACTIVE
 
                onGuidanceSignal(Signal::INITIALIZED); // Required for INACTIVE -> DRIVERS_READY
            }
        }
    }

References current_guidance_state_, current_velocity_, and onGuidanceSignal().

Referenced by guidance::GuidanceWorker::vehicle_status_cb().

Here is the call graph for this function:

Here is the caller graph for this function:

shouldCallSetEnableRobotic()

bool guidance::GuidanceStateMachine::shouldCallSetEnableRobotic

(

)

Indicate if SetEnableRobotic needs to be called in ACTIVE state.

Definition at line 162 of file guidance_state_machine.cpp.

    {
        // call SetEnableRobotic service once when we enter ACTIVE state
        if(current_guidance_state_ == GuidanceStateMachine::ACTIVE)
        {
            if(!called_robotic_engage_in_active_)
            {
                called_robotic_engage_in_active_ = true;
                return true;
            }
        } else {
            // Reset when we leave ACTIVE state 
            called_robotic_engage_in_active_ = false;
        }
        return false;
    }

References ACTIVE, called_robotic_engage_in_active_, and current_guidance_state_.

Referenced by guidance::GuidanceWorker::spin_cb().

Here is the caller graph for this function:

Member Data Documentation

called_robotic_engage_in_active_

bool guidance::GuidanceStateMachine::called_robotic_engage_in_active_ {false}

private

Definition at line 114 of file guidance_state_machine.hpp.

Referenced by shouldCallSetEnableRobotic().

current_guidance_state_

State guidance::GuidanceStateMachine::current_guidance_state_ {State::STARTUP}

private

Definition at line 108 of file guidance_state_machine.hpp.

Referenced by getCurrentState(), onGuidanceSignal(), onVehicleStatus(), and shouldCallSetEnableRobotic().

current_velocity_

double guidance::GuidanceStateMachine::current_velocity_ = 0.0

private

Definition at line 117 of file guidance_state_machine.hpp.

Referenced by onRouteEvent(), and onVehicleStatus().

logger_

rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr guidance::GuidanceStateMachine::logger_

private

Definition at line 120 of file guidance_state_machine.hpp.

Referenced by onRouteEvent().

robotic_active_status_

bool guidance::GuidanceStateMachine::robotic_active_status_ {false}

private

Definition at line 111 of file guidance_state_machine.hpp.

Referenced by onRoboticStatus().

---

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

• guidance/include/guidance/guidance_state_machine.hpp
• guidance/src/guidance_state_machine.cpp