lightbar_manager_worker.hpp

Go to the documentation of this file.

/*
 * Copyright (C) 2023 LEIDOS.
 *
 * Licensed under the Apache License, Version 2.0 (the "License"); you may not
 * use this file except in compliance with the License. You may obtain a copy of
 * the License at
 *
 * http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
 * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
 * License for the specific language governing permissions and limitations under
 * the License.
 */
 
#pragma once
#include <gtest/gtest.h>
#include <string>
#include <rclcpp/rclcpp.hpp>
#include <carma_ros2_utils/carma_lifecycle_node.hpp>
 
#include <vector>
#include <map>
 
#include <carma_msgs/msg/light_bar_cda_type.hpp>
#include <carma_msgs/msg/light_bar_indicator.hpp>
#include <carma_msgs/msg/light_bar_indicator_controllers.hpp>
#include <carma_driver_msgs/msg/light_bar_status.hpp>
#include <carma_planning_msgs/msg/guidance_state.hpp>
#include <automotive_platform_msgs/msg/turn_signal_command.hpp>
 
#include <carma_msgs/srv/request_indicator_control.hpp>
#include <carma_msgs/srv/release_indicator_control.hpp>
#include <carma_driver_msgs/srv/set_light_bar_indicator.hpp>
#include <carma_driver_msgs/srv/set_lights.hpp>
 
#include "lightbar_manager/lightbar_manager_sm.hpp"
#define INDICATOR_COUNT 8
#define CDA_MSG_TYPE_COUNT 4
 
namespace lightbar_manager
{
 
// forward declaration
class LightBarManagerStateMachine;
 
class LightBarManagerWorker
{
    public:
 
        LightBarManagerWorker();
 
        LightBarState getCurrentState();
 
        void next(const LightBarEvent& event);
 
        void handleStateChange(const carma_planning_msgs::msg::GuidanceState& msg);
 
        std::vector<lightbar_manager::LightBarIndicator> handleTurnSignal(const automotive_platform_msgs::msg::TurnSignalCommand& msg);
 
        void releaseControl(std::vector<LightBarIndicator> ind_list, std::string owner_name);
 
        std::vector<LightBarIndicator> requestControl(std::vector<LightBarIndicator> ind_list, std::string requester_name);
        
        std::vector<IndicatorStatus> setIndicator(LightBarIndicator ind, IndicatorStatus ind_status, std::string requester_name);
 
        LightBarIndicator getIndicatorFromCDAType(LightBarCDAType cda_type);
 
        LightBarCDAType getCDATypeFromIndicator(LightBarIndicator indicator);
 
        std::map<LightBarCDAType, LightBarIndicator> setIndicatorCDAMap(const std::vector<std::string>& lightbar_cda_table, const std::vector<std::string>& lightbar_ind_table);
 
        std::map<LightBarIndicator,std::string> getIndicatorControllers();
       
        void setIndicatorControllers();
 
        void setIndicatorControllers(std::map<LightBarIndicator, std::string> ind_ctrl_map);
 
        bool hasHigherPriority(std::string requester, std::string controller);
 
        void handleControlChange(LightBarIndicator indicator, std::string controller, IndicatorControlEvent event);
 
        carma_driver_msgs::msg::LightBarStatus getLightBarStatusMsg(std::vector<IndicatorStatus> indicators);
 
        std::vector<carma_msgs::msg::LightBarIndicator> getMsg(std::vector<LightBarIndicator> indicators);
 
        std::vector<carma_msgs::msg::LightBarCDAType> getMsg(std::vector<LightBarCDAType> cda_types);
 
        carma_msgs::msg::LightBarIndicatorControllers getMsg(std::map<LightBarIndicator, std::string> ind_ctrl_map);
 
        // Priorities of components/plugins read from ROSParamter
        std::vector<std::string> control_priorities;
 
        // LightBarStatus local copy in LightBarIndicator representation
        std::vector<IndicatorStatus> light_status;
 
    private:
        
 
        // LightBarManager state machine
        LightBarManagerStateMachine lbsm_;
 
        // Indicators and their owners' mapping
        std::map<LightBarIndicator, std::string> ind_ctrl_map_;
 
        // Indicators and their corresponding CDA msg type mapping
        std::map<LightBarCDAType, LightBarIndicator> cda_ind_map_;
 
        // Current turn signal
        uint8_t current_turn_signal_ = automotive_platform_msgs::msg::TurnSignalCommand::NONE;
 
        // Helper maps that convert string into enum representations when reading from ROSParameter
        std::map<std::string, LightBarCDAType> cda_type_dict_ = {
            {"TypeA",TYPE_A},
            {"TypeB",TYPE_B},
            {"TypeC",TYPE_C},
            {"TypeD",TYPE_D}};
            
        std::map<std::string, LightBarIndicator> ind_dict = {
            {"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}};
        
        FRIEND_TEST(LightBarManagerNodeTest, testSetIndicator);
        FRIEND_TEST(LightBarManagerNodeTest, testTurnOffAll);
        FRIEND_TEST(LightBarManagerNodeTest, testTurnSignalCallback);
}; //class LightBarManagerWorker
} // namespace lightbar_manager