#include <lightbar_manager_worker.hpp>

Collaboration diagram for lightbar_manager::LightBarManagerWorker:

Public Member Functions
	LightBarManagerWorker ()
	Default constructor for LightBarManager. More...

LightBarState	getCurrentState ()
	Get current state of the LightBarStateMachine. More...

void	next (const LightBarEvent &event)
	Transition to the next state of the LightBarStateMachine Not used at the moment as state machine fully handles everything. More...

void	handleStateChange (const carma_planning_msgs::msg::GuidanceState &msg)
	This function relays the state change msg to the state maching. It triggers the transitioning to the next state in LightBarStateMachine based on the guidance state change. More...

std::vector< lightbar_manager::LightBarIndicator >	handleTurnSignal (const automotive_platform_msgs::msg::TurnSignalCommand &msg)
	This function checks if the turn signal should be changed on the lightbar. More...

void	releaseControl (std::vector< LightBarIndicator > ind_list, std::string owner_name)
	Releases the specified owner plugin or component's control of the given indicator list. This function handles successful transitioning of next ownership when that happens. More...

std::vector< LightBarIndicator >	requestControl (std::vector< LightBarIndicator > ind_list, std::string requester_name)
	Requests the control of the given list of indicators to the requester. This function handles successful transitioning of next ownership and illogical requests such as mutually exclusive indicators. More...

std::vector< IndicatorStatus >	setIndicator (LightBarIndicator ind, IndicatorStatus ind_status, std::string requester_name)
	Try to turn the given indicator ON or OFF (locally) upon the given component's request. More...

LightBarIndicator	getIndicatorFromCDAType (LightBarCDAType cda_type)
	Helper functions that translates an indicator to its corresponding CDA msg type The mapping between indicators and CDA types are configured through ROS params. More...

LightBarCDAType	getCDATypeFromIndicator (LightBarIndicator indicator)
	Helper functions that translates a CDA msg type to its corresponding indicator The mapping between indicators and CDA types are configured through ROS params. More...

std::map< LightBarCDAType, LightBarIndicator >	setIndicatorCDAMap (const std::vector< std::string > &lightbar_cda_table, const std::vector< std::string > &lightbar_ind_table)
	Helper function that initializes CDAType to Indicator Mapping (updates internal copy) More...

std::map< LightBarIndicator, std::string >	getIndicatorControllers ()
	Helper function that gets all current owners of the indicator. More...

void	setIndicatorControllers ()
	Helper function that initializes supporting Indicators and their owner mapping as empty strings. More...

void	setIndicatorControllers (std::map< LightBarIndicator, std::string > ind_ctrl_map)
	Helper function that resets Indicators and their owner mapping. More...

bool	hasHigherPriority (std::string requester, std::string controller)
	Helper function that checks if the first input component has higher priority than the second. More...

void	handleControlChange (LightBarIndicator indicator, std::string controller, IndicatorControlEvent event)
	Helper function that handles control lost/gained event of a component. This function registers/removes controller's name to all mutually inexclusive indicators. e.g. If registering a component as controller of YELLOW_ARROW_OUT, it will be registered for YELLOW_ARROW_LEFT/RIGHT/FLASH too. More...

carma_driver_msgs::msg::LightBarStatus	getLightBarStatusMsg (std::vector< IndicatorStatus > indicators)
	Helper function that translates IndicatorStatus vector into LightBarStatus.msg, a lightbar driver compatible msg. More...

std::vector< carma_msgs::msg::LightBarIndicator >	getMsg (std::vector< LightBarIndicator > indicators)
	Helper function that translates LightBarIndicator vector into LightBarIndicator.msg vector. More...

std::vector< carma_msgs::msg::LightBarCDAType >	getMsg (std::vector< LightBarCDAType > cda_types)
	Helper function that translates LightBarIndicator vector into LightBarIndicator.msg vector. More...

carma_msgs::msg::LightBarIndicatorControllers	getMsg (std::map< LightBarIndicator, std::string > ind_ctrl_map)
	Helper function that translates mapping of indicators to their owners into Msg. More...

Public Attributes
std::vector< std::string >	control_priorities

std::vector< IndicatorStatus >	light_status

Private Member Functions
	FRIEND_TEST (LightBarManagerNodeTest, testSetIndicator)

	FRIEND_TEST (LightBarManagerNodeTest, testTurnOffAll)

	FRIEND_TEST (LightBarManagerNodeTest, testTurnSignalCallback)

Private Attributes
LightBarManagerStateMachine	lbsm_

std::map< LightBarIndicator, std::string >	ind_ctrl_map_

std::map< LightBarCDAType, LightBarIndicator >	cda_ind_map_

uint8_t	current_turn_signal_ = automotive_platform_msgs::msg::TurnSignalCommand::NONE

std::map< std::string, LightBarCDAType >	cda_type_dict_

std::map< std::string, LightBarIndicator >	ind_dict

Detailed Description

Definition at line 48 of file lightbar_manager_worker.hpp.

Constructor & Destructor Documentation

◆ LightBarManagerWorker()

lightbar_manager::LightBarManagerWorker::LightBarManagerWorker ( )

Default constructor for LightBarManager.

Definition at line 22 of file lightbar_manager_worker.cpp.

22{};

Member Function Documentation

◆ FRIEND_TEST() [1/3]

lightbar_manager::LightBarManagerWorker::FRIEND_TEST	(	LightBarManagerNodeTest	,
		testSetIndicator
	)

private

◆ FRIEND_TEST() [2/3]

lightbar_manager::LightBarManagerWorker::FRIEND_TEST	(	LightBarManagerNodeTest	,
		testTurnOffAll
	)

private

◆ FRIEND_TEST() [3/3]

lightbar_manager::LightBarManagerWorker::FRIEND_TEST	(	LightBarManagerNodeTest	,
		testTurnSignalCallback
	)

private

◆ getCDATypeFromIndicator()

LightBarCDAType lightbar_manager::LightBarManagerWorker::getCDATypeFromIndicator ( LightBarIndicator indicator )

Helper functions that translates a CDA msg type to its corresponding indicator
The mapping between indicators and CDA types are configured through ROS params.

Returns: Returns the CDA msg type that the indicator represents

Definition at line 390 of file lightbar_manager_worker.cpp.

    {
        for (std::pair<LightBarCDAType,LightBarIndicator> element : cda_ind_map_) 
        {
            if (element.second == indicator)
                return element.first;
        }
        // if the indicator does not have any mapped CDA Msg Type, throw and handle outside
        throw INDICATOR_NOT_MAPPED(std::string("Specified indicator does not have a mapped CDA Msg Type. Skipping...")); 
    }

References cda_ind_map_.

◆ getCurrentState()

LightBarState lightbar_manager::LightBarManagerWorker::getCurrentState ( )

Get current state of the LightBarStateMachine.

Returns: current state of the lightbar state machine

Definition at line 29 of file lightbar_manager_worker.cpp.

    {
        return lbsm_.getCurrentState();
    }

References lightbar_manager::LightBarManagerStateMachine::getCurrentState(), and lbsm_.

Here is the call graph for this function:

◆ getIndicatorControllers()

std::map< LightBarIndicator, std::string > lightbar_manager::LightBarManagerWorker::getIndicatorControllers ( )

Helper function that gets all current owners of the indicator.

Returns: return the mapping of indicators to their owners

Definition at line 106 of file lightbar_manager_worker.cpp.

    {
        return ind_ctrl_map_;
    }

References ind_ctrl_map_.

◆ getIndicatorFromCDAType()

LightBarIndicator lightbar_manager::LightBarManagerWorker::getIndicatorFromCDAType ( LightBarCDAType cda_type )

Helper functions that translates an indicator to its corresponding CDA msg type
The mapping between indicators and CDA types are configured through ROS params.

Returns: Returns the indicator corresponding to the msg type.

Definition at line 385 of file lightbar_manager_worker.cpp.

    {
        return cda_ind_map_[cda_type];
    }

References cda_ind_map_.

◆ getLightBarStatusMsg()

carma_driver_msgs::msg::LightBarStatus lightbar_manager::LightBarManagerWorker::getLightBarStatusMsg ( std::vector< IndicatorStatus > indicators )

Helper function that translates IndicatorStatus vector into LightBarStatus.msg, a lightbar driver compatible msg.

Returns: light bar status

Definition at line 349 of file lightbar_manager_worker.cpp.

    {
        // it is assumed that mutually exclusive cases are handled properly.
        carma_driver_msgs::msg::LightBarStatus msg;
        msg.green_solid = indicators[GREEN_SOLID] == ON ? carma_driver_msgs::msg::LightBarStatus::ON : carma_driver_msgs::msg::LightBarStatus::OFF;
        msg.green_flash = indicators[GREEN_FLASH]== ON ? carma_driver_msgs::msg::LightBarStatus::ON : carma_driver_msgs::msg::LightBarStatus::OFF;
        msg.sides_solid = indicators[YELLOW_SIDES]== ON ? carma_driver_msgs::msg::LightBarStatus::ON : carma_driver_msgs::msg::LightBarStatus::OFF;
        msg.yellow_solid = indicators[YELLOW_DIM]== ON ? carma_driver_msgs::msg::LightBarStatus::ON : carma_driver_msgs::msg::LightBarStatus::OFF;
        msg.flash = indicators[YELLOW_FLASH]== ON ? carma_driver_msgs::msg::LightBarStatus::ON : carma_driver_msgs::msg::LightBarStatus::OFF;
 
        msg.left_arrow = indicators[YELLOW_ARROW_LEFT]== ON ? carma_driver_msgs::msg::LightBarStatus::ON : carma_driver_msgs::msg::LightBarStatus::OFF;
        msg.right_arrow = indicators[YELLOW_ARROW_RIGHT]== ON ? carma_driver_msgs::msg::LightBarStatus::ON : carma_driver_msgs::msg::LightBarStatus::OFF;
        // for YELLOW_ARROW_OUT set left and right
        if (indicators[YELLOW_ARROW_OUT] == ON)
        {
            msg.left_arrow = carma_driver_msgs::msg::LightBarStatus::ON;
            msg.right_arrow = carma_driver_msgs::msg::LightBarStatus::ON;
        }
        return msg;
    }

References lightbar_manager::GREEN_FLASH, lightbar_manager::GREEN_SOLID, lightbar_manager::OFF, lightbar_manager::ON, lightbar_manager::YELLOW_ARROW_LEFT, lightbar_manager::YELLOW_ARROW_OUT, lightbar_manager::YELLOW_ARROW_RIGHT, lightbar_manager::YELLOW_DIM, lightbar_manager::YELLOW_FLASH, and lightbar_manager::YELLOW_SIDES.

◆ getMsg() [1/3]

carma_msgs::msg::LightBarIndicatorControllers lightbar_manager::LightBarManagerWorker::getMsg ( std::map< LightBarIndicator, std::string > ind_ctrl_map )

Helper function that translates mapping of indicators to their owners into Msg.

Returns: LightBarIndicatorControlllers.msg

Definition at line 57 of file lightbar_manager_worker.cpp.

    {
        carma_msgs::msg::LightBarIndicatorControllers curr;
        curr.green_solid_owner = ind_ctrl_map[GREEN_SOLID];
        curr.green_flash_owner = ind_ctrl_map[GREEN_FLASH];
        curr.yellow_sides_owner= ind_ctrl_map[YELLOW_SIDES];
        curr.yellow_dim_owner = ind_ctrl_map[YELLOW_DIM];
        curr.yellow_flash_owner = ind_ctrl_map[YELLOW_FLASH];
        curr.yellow_arrow_left_owner = ind_ctrl_map[YELLOW_ARROW_LEFT];
        curr.yellow_arrow_right_owner = ind_ctrl_map[YELLOW_ARROW_RIGHT];
        curr.yellow_arrow_out_owner = ind_ctrl_map[YELLOW_ARROW_OUT];
 
        return curr;
    }

References lightbar_manager::GREEN_FLASH, lightbar_manager::GREEN_SOLID, lightbar_manager::YELLOW_ARROW_LEFT, lightbar_manager::YELLOW_ARROW_OUT, lightbar_manager::YELLOW_ARROW_RIGHT, lightbar_manager::YELLOW_DIM, lightbar_manager::YELLOW_FLASH, and lightbar_manager::YELLOW_SIDES.

◆ getMsg() [2/3]

std::vector< carma_msgs::msg::LightBarCDAType > lightbar_manager::LightBarManagerWorker::getMsg ( std::vector< LightBarCDAType > cda_types )

Helper function that translates LightBarIndicator vector into LightBarIndicator.msg vector.

Returns: LightBarCDAType.msg vector

Definition at line 46 of file lightbar_manager_worker.cpp.

    {
        std::vector<carma_msgs::msg::LightBarCDAType> return_msg;
        for (auto cda_type : cda_types)
        {
            carma_msgs::msg::LightBarCDAType msg;
            msg.type = cda_type;
            return_msg.push_back(msg);
        }
        return return_msg;
    }

◆ getMsg() [3/3]

std::vector< carma_msgs::msg::LightBarIndicator > lightbar_manager::LightBarManagerWorker::getMsg ( std::vector< LightBarIndicator > indicators )

Helper function that translates LightBarIndicator vector into LightBarIndicator.msg vector.

Returns: LightBarIndicator.msg vector

Definition at line 34 of file lightbar_manager_worker.cpp.

    {
        std::vector<carma_msgs::msg::LightBarIndicator> return_msg;
        for (auto indicator : indicators)
        {
            carma_msgs::msg::LightBarIndicator msg;
            msg.indicator = indicator;
            return_msg.push_back(msg);
        }
        return return_msg;
    }

◆ handleControlChange()

void lightbar_manager::LightBarManagerWorker::handleControlChange	(	LightBarIndicator	indicator,
		std::string	controller,
		IndicatorControlEvent	event
	)

Helper function that handles control lost/gained event of a component. This function registers/removes controller's name to all mutually inexclusive indicators. e.g. If registering a component as controller of YELLOW_ARROW_OUT, it will be registered for YELLOW_ARROW_LEFT/RIGHT/FLASH too.

Definition at line 262 of file lightbar_manager_worker.cpp.

    {
        // Pick new owner name depending on losing or gaining control
        std::string new_owner;
        if (event == CONTROL_GAINED)
            new_owner = controller;
        else
            new_owner = "";
        
        // Handle mutually in-exclusive indicators
        // These are indicators that are controlled indirectly due to change in one indicator
        // Priority of every one of those indicators, must be compared to that of requester to change it.
        // e.g. if A>B and YELLOW_RIGHT= "" & YELLOW_LEFT = A (means YELLOW_FLASH = A & YELLOW_OUT = A)
        // Then if YELLOW_RIGHT = "" -> YELLOW_RIGHT = "B", B should not be able to control YELLOW_FLASH nor YELLOW_OUT
        switch (indicator)
        {
            case YELLOW_ARROW_LEFT:
            case YELLOW_ARROW_RIGHT:
                ind_ctrl_map_[YELLOW_ARROW_OUT] = 
                    hasHigherPriority(controller, ind_ctrl_map_[YELLOW_ARROW_OUT]) ? new_owner :ind_ctrl_map_[YELLOW_ARROW_OUT];
                ind_ctrl_map_[YELLOW_FLASH] = ind_ctrl_map_[YELLOW_ARROW_OUT]; //they always have same owner
                ind_ctrl_map_[indicator] = new_owner;
                break;
            case YELLOW_ARROW_OUT:
            case YELLOW_FLASH:
                ind_ctrl_map_[YELLOW_ARROW_LEFT] = 
                    hasHigherPriority(controller, ind_ctrl_map_[YELLOW_ARROW_LEFT]) ? new_owner :ind_ctrl_map_[YELLOW_ARROW_LEFT];
                ind_ctrl_map_[YELLOW_ARROW_RIGHT] = 
                    hasHigherPriority(controller, ind_ctrl_map_[YELLOW_ARROW_RIGHT]) ? new_owner :ind_ctrl_map_[YELLOW_ARROW_RIGHT];
                ind_ctrl_map_[YELLOW_ARROW_OUT] = 
                    hasHigherPriority(controller, ind_ctrl_map_[YELLOW_ARROW_OUT]) ? new_owner :ind_ctrl_map_[YELLOW_ARROW_OUT];
                ind_ctrl_map_[YELLOW_FLASH] = ind_ctrl_map_[YELLOW_ARROW_OUT]; //they always have same owner
                break;
            case GREEN_FLASH:
            case GREEN_SOLID:
                ind_ctrl_map_[GREEN_FLASH] = 
                    hasHigherPriority(controller, ind_ctrl_map_[GREEN_FLASH]) ? new_owner :ind_ctrl_map_[GREEN_FLASH];
                ind_ctrl_map_[GREEN_SOLID] = ind_ctrl_map_[GREEN_FLASH]; //they always have same owner
                break;
            default:
                ind_ctrl_map_[indicator] = new_owner;
                break;
        }
        return;
    }

References lightbar_manager::CONTROL_GAINED, lightbar_manager::GREEN_FLASH, lightbar_manager::GREEN_SOLID, hasHigherPriority(), ind_ctrl_map_, lightbar_manager::YELLOW_ARROW_LEFT, lightbar_manager::YELLOW_ARROW_OUT, lightbar_manager::YELLOW_ARROW_RIGHT, and lightbar_manager::YELLOW_FLASH.

Referenced by releaseControl(), and requestControl().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ handleStateChange()

void lightbar_manager::LightBarManagerWorker::handleStateChange ( const carma_planning_msgs::msg::GuidanceState & msg )

This function relays the state change msg to the state maching. It triggers the transitioning to the next state in LightBarStateMachine based on the guidance state change.

Definition at line 72 of file lightbar_manager_worker.cpp.

    {
        lbsm_.handleStateChange(msg);
        return;
    }

References lightbar_manager::LightBarManagerStateMachine::handleStateChange(), and lbsm_.

Here is the call graph for this function:

◆ handleTurnSignal()

std::vector< lightbar_manager::LightBarIndicator > lightbar_manager::LightBarManagerWorker::handleTurnSignal ( const automotive_platform_msgs::msg::TurnSignalCommand & msg )

This function checks if the turn signal should be changed on the lightbar.

Returns: size one vector of turn signal, empty if no change is required

Definition at line 78 of file lightbar_manager_worker.cpp.

    {
        std::vector<lightbar_manager::LightBarIndicator> turn_signal;
        if (msg.turn_signal == current_turn_signal_)
        {
            return {};
        }
        if (msg.turn_signal == automotive_platform_msgs::msg::TurnSignalCommand::LEFT) //NONE -> LEFT
        {
            turn_signal.push_back(lightbar_manager::LightBarIndicator::YELLOW_ARROW_LEFT);  
        }
        else if (msg.turn_signal == automotive_platform_msgs::msg::TurnSignalCommand::RIGHT) //NONE -> RIGHT
        {
            turn_signal.push_back(lightbar_manager::LightBarIndicator::YELLOW_ARROW_RIGHT);
        }
        else if (msg.turn_signal == automotive_platform_msgs::msg::TurnSignalCommand::NONE) 
        {
            // check previous signal
            if (current_turn_signal_ == automotive_platform_msgs::msg::TurnSignalCommand::RIGHT) // RIGHT -> NONE
                turn_signal.push_back(lightbar_manager::LightBarIndicator::YELLOW_ARROW_RIGHT);
            else if (current_turn_signal_ == automotive_platform_msgs::msg::TurnSignalCommand::LEFT) // LEFT -> NONE
                turn_signal.push_back(lightbar_manager::LightBarIndicator::YELLOW_ARROW_LEFT);
        }
 
        current_turn_signal_ = msg.turn_signal;
 
        return turn_signal;
    }

References current_turn_signal_, lightbar_manager::YELLOW_ARROW_LEFT, and lightbar_manager::YELLOW_ARROW_RIGHT.

◆ hasHigherPriority()

bool lightbar_manager::LightBarManagerWorker::hasHigherPriority	(	std::string	requester,
		std::string	controller
	)

Helper function that checks if the first input component has higher priority than the second.

Returns: true if the requester has higher priority than the controller

Definition at line 167 of file lightbar_manager_worker.cpp.

    {
        auto start = control_priorities.begin();
        auto end = control_priorities.end();
        
        auto requesterPriority = std::find(start, end, requester);
        auto controllerPriority = std::find(start, end, controller);
 
        // Components not in the priority list are assumed to have the lowest priority
        if (requesterPriority == end)
        {
            RCLCPP_WARN_STREAM(rclcpp::get_logger("lightbar_manager"),requester << " is referenced in lightbar_manager, but is not in the priority list");
            return false;
        }  
        else if (controllerPriority == end)
        {
            RCLCPP_WARN_STREAM(rclcpp::get_logger("lightbar_manager"),controller << " is referenced in lightbar_manager and is controlling an indicator, but is not in the priority list");
            return true;
        }
        return (requesterPriority - end) <= (controllerPriority - end);
    }

References control_priorities.

Referenced by handleControlChange(), and requestControl().

Here is the caller graph for this function:

◆ next()

void lightbar_manager::LightBarManagerWorker::next ( const LightBarEvent & event )

Transition to the next state of the LightBarStateMachine Not used at the moment as state machine fully handles everything.

Definition at line 24 of file lightbar_manager_worker.cpp.

    {
        lbsm_.next(event);
    }

References lbsm_, and lightbar_manager::LightBarManagerStateMachine::next().

Here is the call graph for this function:

◆ releaseControl()

void lightbar_manager::LightBarManagerWorker::releaseControl	(	std::vector< LightBarIndicator >	ind_list,
		std::string	owner_name
	)

Releases the specified owner plugin or component's control of the given indicator list. This function handles successful transitioning of next ownership when that happens.

Definition at line 233 of file lightbar_manager_worker.cpp.

    {
        std::string current_owner;
        
        // Attempt to release control of all indicators
        for (LightBarIndicator indicator: ind_list) 
        {
            // Attempt release control
            try
            {
                current_owner = ind_ctrl_map_.at(indicator);
            }
            catch(const std::exception& e)
            {
                RCLCPP_WARN_STREAM(rclcpp::get_logger("lightbar_manager"),"In function: " << __FUNCTION__ << ", the component, " << owner_name 
                    << ", requested a release of an invalid indicator. Skipping with WARNING:" << e.what() << "\n");
                continue;
            }
 
            // Lose control only if the requester is currently controlling it
            if (current_owner == owner_name) 
            {   
                handleControlChange(indicator,owner_name, CONTROL_LOST);
            }
        }
 
        return;
    }

References lightbar_manager::CONTROL_LOST, handleControlChange(), and ind_ctrl_map_.

Here is the call graph for this function:

◆ requestControl()

std::vector< LightBarIndicator > lightbar_manager::LightBarManagerWorker::requestControl	(	std::vector< LightBarIndicator >	ind_list,
		std::string	requester_name
	)

Requests the control of the given list of indicators to the requester. This function handles successful transitioning of next ownership and illogical requests such as mutually exclusive indicators.

Returns: Returns the list of the indicators that were denied for the requester

Definition at line 189 of file lightbar_manager_worker.cpp.

    {
        std::vector<LightBarIndicator> denied_list;
        std::string indicator_owner;
        // Attempt to acquire control of every indicators
        for (LightBarIndicator indicator: ind_list) 
        {
            // Attempt control
            try
            {
                indicator_owner = ind_ctrl_map_.at(indicator);
            }
            catch(const std::exception& e)
            {
                RCLCPP_WARN_STREAM(rclcpp::get_logger("lightbar_manager"),"In function: " << __FUNCTION__ << ", the component, " << requester_name 
                    << ", requested a control of invalid indicator. Skipping with WARNING:" << e.what() << "\n");
                continue;
            }
        
            if (indicator_owner == "") 
            {   
                // Add new controller If no other component has claimed this indicator
                handleControlChange(indicator, requester_name, CONTROL_GAINED);
            } 
            else if (indicator_owner != requester_name) 
            {   
                // If this indicator is already controlled
                // If the requesting component has higher priority it may take control of this indicator
                if (hasHigherPriority(requester_name, indicator_owner)) 
                {
                    // Handle previous controller
                    handleControlChange(indicator, indicator_owner, CONTROL_LOST);
                    // Add new controller
                    handleControlChange(indicator, requester_name, CONTROL_GAINED);
                } 
                else 
                {
                    denied_list.push_back(indicator); // Notify caller of failure to take control of component
                }
            }
        }
        return denied_list;
    }   

References lightbar_manager::CONTROL_GAINED, lightbar_manager::CONTROL_LOST, handleControlChange(), hasHigherPriority(), and ind_ctrl_map_.

Here is the call graph for this function:

◆ setIndicator()

std::vector< IndicatorStatus > lightbar_manager::LightBarManagerWorker::setIndicator	(	LightBarIndicator	ind,
		IndicatorStatus	ind_status,
		std::string	requester_name
	)

Try to turn the given indicator ON or OFF (locally) upon the given component's request.

Returns: Returns the changed vector of indicator status to be set by the driver client

Definition at line 308 of file lightbar_manager_worker.cpp.

    {
        std::string current_controller = ind_ctrl_map_[ind];
 
        // Use a local copy in case manager fails to set the light
        std::vector<IndicatorStatus> light_status_copy = light_status;
 
        // Handle mutually non-exclusive cases
        // If desired indicator is already at the status do not change any indicators 
        if (ind_status != light_status_copy[ind])
        {
            switch(ind)
            {
                case YELLOW_ARROW_LEFT:
                case YELLOW_ARROW_RIGHT:
                    light_status_copy[YELLOW_ARROW_OUT] = OFF;
                    light_status_copy[YELLOW_FLASH] = OFF;
                    break;
                case YELLOW_ARROW_OUT:
                case YELLOW_FLASH:
                    light_status_copy[YELLOW_ARROW_OUT] = OFF;
                    light_status_copy[YELLOW_FLASH] = OFF;
                    light_status_copy[YELLOW_ARROW_LEFT] = OFF;
                    light_status_copy[YELLOW_ARROW_RIGHT] = OFF;
                    break;
                case GREEN_FLASH:
                case GREEN_SOLID:
                    light_status_copy[GREEN_FLASH] = OFF;
                    light_status_copy[GREEN_SOLID] = OFF;
                    break;
                default:
                    break;
            }
        }
        
        // Set the desired indicator now that there is no conflict.
        light_status_copy[ind] = ind_status;
        return light_status_copy;
 
    }

References lightbar_manager::GREEN_FLASH, lightbar_manager::GREEN_SOLID, ind_ctrl_map_, light_status, lightbar_manager::OFF, lightbar_manager::YELLOW_ARROW_LEFT, lightbar_manager::YELLOW_ARROW_OUT, lightbar_manager::YELLOW_ARROW_RIGHT, and lightbar_manager::YELLOW_FLASH.

◆ setIndicatorCDAMap()

std::map< LightBarCDAType, LightBarIndicator > lightbar_manager::LightBarManagerWorker::setIndicatorCDAMap	(	const std::vector< std::string > &	lightbar_cda_table,
		const std::vector< std::string > &	lightbar_ind_table
	)

Helper function that initializes CDAType to Indicator Mapping (updates internal copy)

Returns: return the mapping for debug purposes.

Definition at line 112 of file lightbar_manager_worker.cpp.

    {
        // In case if the parameter is not loaded corretly and there is an error, return this
        std::map<std::string, std::string> raw_map;
        for (size_t i = 0; i < lightbar_cda_table.size(); i ++)
        {
            raw_map[lightbar_cda_table[i]] = lightbar_ind_table[i];
        }
        
        std::map<LightBarCDAType, LightBarIndicator> default_map;
        
        default_map[TYPE_A] = YELLOW_DIM;
        default_map[TYPE_B] = YELLOW_DIM;
        default_map[TYPE_C] = YELLOW_FLASH;
        default_map[TYPE_D] = YELLOW_SIDES;
 
        if (raw_map.size() < 4)
        {
            RCLCPP_WARN_STREAM(rclcpp::get_logger("lightbar_manager"),"In function: " << __FUNCTION__ << ": LightBarManager's CDAType to Indicator table is not configured correctly. Using default mapping...");
            cda_ind_map_ = default_map;
            return cda_ind_map_;
        }
        
        // Convert from string:string mapping to correct enums
        for (std::pair<std::string, std::string> element : raw_map)
        {
            LightBarCDAType cda_type;
            LightBarIndicator indicator;
            try
            {
                cda_type = cda_type_dict_.at(element.first);
            }
            catch(const std::exception& e)
            {
                RCLCPP_WARN_STREAM(rclcpp::get_logger("lightbar_manager"),"In function: " << __FUNCTION__ << 
                ": LightBarManager Received unknown CDA Msg Type. Using default mapping for cda-indicators...");
                cda_ind_map_ = default_map;
                return cda_ind_map_;
            }
            try
            {
                indicator = ind_dict.at(element.second);
            }
            catch(const std::exception& e)
            {
                RCLCPP_WARN_STREAM(rclcpp::get_logger("lightbar_manager"),"In function: " << __FUNCTION__ << 
                ": LightBarManager Received unknown indicator type. Using default mapping for cda-indicators...");
                cda_ind_map_ = default_map;
                return cda_ind_map_;
            }
            cda_ind_map_.emplace(cda_type, indicator);
        }
        return cda_ind_map_;
    }

References cda_ind_map_, cda_type_dict_, process_bag::i, ind_dict, lightbar_manager::TYPE_A, lightbar_manager::TYPE_B, lightbar_manager::TYPE_C, lightbar_manager::TYPE_D, lightbar_manager::YELLOW_DIM, lightbar_manager::YELLOW_FLASH, and lightbar_manager::YELLOW_SIDES.

◆ setIndicatorControllers() [1/2]

void lightbar_manager::LightBarManagerWorker::setIndicatorControllers ( )

Helper function that initializes supporting Indicators and their owner mapping as empty strings.

Definition at line 370 of file lightbar_manager_worker.cpp.

    {
        for (int i = 0; i <= INDICATOR_COUNT; i++ )
        {
            LightBarIndicator indicator = static_cast<LightBarIndicator>(i);
            // initialize the owner as empty string
            ind_ctrl_map_[indicator] = "";
        }
    }

References process_bag::i, ind_ctrl_map_, and INDICATOR_COUNT.

◆ setIndicatorControllers() [2/2]

void lightbar_manager::LightBarManagerWorker::setIndicatorControllers ( std::map< LightBarIndicator, std::string > ind_ctrl_map )

Helper function that resets Indicators and their owner mapping.

Definition at line 380 of file lightbar_manager_worker.cpp.

    {
        ind_ctrl_map_ = ind_ctrl_map;
    }

References ind_ctrl_map_.

Member Data Documentation

◆ cda_ind_map_

std::map<LightBarCDAType, LightBarIndicator> lightbar_manager::LightBarManagerWorker::cda_ind_map_

private

Definition at line 189 of file lightbar_manager_worker.hpp.

Referenced by getCDATypeFromIndicator(), getIndicatorFromCDAType(), and setIndicatorCDAMap().

◆ cda_type_dict_

std::map<std::string, LightBarCDAType> lightbar_manager::LightBarManagerWorker::cda_type_dict_

private

Initial value:

= {
            {"TypeA",TYPE_A},
            {"TypeB",TYPE_B},
            {"TypeC",TYPE_C},
            {"TypeD",TYPE_D}}

Definition at line 195 of file lightbar_manager_worker.hpp.

Referenced by setIndicatorCDAMap().

◆ control_priorities

std::vector<std::string> lightbar_manager::LightBarManagerWorker::control_priorities

Definition at line 174 of file lightbar_manager_worker.hpp.

Referenced by hasHigherPriority().

◆ current_turn_signal_

uint8_t lightbar_manager::LightBarManagerWorker::current_turn_signal_ = automotive_platform_msgs::msg::TurnSignalCommand::NONE

private

Definition at line 192 of file lightbar_manager_worker.hpp.

Referenced by handleTurnSignal().

◆ ind_ctrl_map_

std::map<LightBarIndicator, std::string> lightbar_manager::LightBarManagerWorker::ind_ctrl_map_

private

Definition at line 186 of file lightbar_manager_worker.hpp.

Referenced by getIndicatorControllers(), handleControlChange(), releaseControl(), requestControl(), setIndicator(), and setIndicatorControllers().

◆ ind_dict

std::map<std::string, LightBarIndicator> lightbar_manager::LightBarManagerWorker::ind_dict

private

Initial value:

= {
            {"GREEN_SOLID", GREEN_SOLID},
            {"GREEN_FLASH", GREEN_FLASH},
            {"YELLOW_SIDES", YELLOW_SIDES},
            {"YELLOW_DIM", YELLOW_DIM},
            {"YELLOW_FLASH", YELLOW_FLASH},
            {"YELLOW_ARROW_LEFT", YELLOW_ARROW_LEFT},
            {"YELLOW_ARROW_RIGHT", YELLOW_ARROW_RIGHT},
            {"YELLOW_ARROW_OUT", YELLOW_ARROW_OUT}}

Definition at line 201 of file lightbar_manager_worker.hpp.

Referenced by setIndicatorCDAMap().

◆ lbsm_

LightBarManagerStateMachine lightbar_manager::LightBarManagerWorker::lbsm_

private

Definition at line 183 of file lightbar_manager_worker.hpp.

Referenced by getCurrentState(), handleStateChange(), and next().

◆ light_status

std::vector<IndicatorStatus> lightbar_manager::LightBarManagerWorker::light_status

Definition at line 177 of file lightbar_manager_worker.hpp.

Referenced by setIndicator().

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

lightbar_manager/include/lightbar_manager/lightbar_manager_worker.hpp
lightbar_manager/src/lightbar_manager_worker.cpp

Public Member Functions

Public Attributes

Private Member Functions

Private Attributes

Detailed Description

Constructor & Destructor Documentation

◆ LightBarManagerWorker()

Member Function Documentation

◆ FRIEND_TEST() [1/3]

◆ FRIEND_TEST() [2/3]

◆ FRIEND_TEST() [3/3]

◆ getCDATypeFromIndicator()

◆ getCurrentState()

◆ getIndicatorControllers()

◆ getIndicatorFromCDAType()

◆ getLightBarStatusMsg()

◆ getMsg() [1/3]

◆ getMsg() [2/3]

◆ getMsg() [3/3]

◆ handleControlChange()

◆ handleStateChange()

◆ handleTurnSignal()

◆ hasHigherPriority()

◆ next()

◆ releaseControl()

◆ requestControl()

◆ setIndicator()

◆ setIndicatorCDAMap()

◆ setIndicatorControllers() [1/2]

◆ setIndicatorControllers() [2/2]

Member Data Documentation

◆ cda_ind_map_

◆ cda_type_dict_

◆ control_priorities

◆ current_turn_signal_

◆ ind_ctrl_map_

◆ ind_dict

◆ lbsm_

◆ light_status