lightbar_manager_node.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 <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_worker.hpp"
 
namespace lightbar_manager
{
 
struct Config
{
    double spin_rate_hz = 10.0;
    bool normal_operation = true;   // if false, other plugins are able to take control over the lightbar status
    std::vector<std::string> lightbar_priorities = {"lightbar_manager"}; // Priority in descending order. Higher priority takes over lightbar
    std::vector<std::string> lightbar_cda_table = {"TypeA", "TypeB", "TypeC", "TypeD"}; // Keys for lightbar_cda_to_ind_table, 1-to-1 with lightbar_ind_table
    std::vector<std::string> lightbar_ind_table = {"YELLOW_DIM","YELLOW_DIM","YELLOW_FLASH","YELLOW_SIDES"}; // Values for lightbar_cda_to_ind_table, 1-to-1 with lightbar_cda_table
 
    // Stream operator for this config
    friend std::ostream &operator<<(std::ostream &output, const Config &c)
    {
      output << "LightBarManager::Config { " << std::endl
           << "spin_rate_hz: " << c.spin_rate_hz << std::endl
           << "lightbar_priorities.size(): " << c.lightbar_priorities.size() << std::endl
           << "lightbar_cda_table.size(): " << c.lightbar_cda_table.size() << std::endl
           << "lightbar_ind_table.size(): " << c.lightbar_ind_table.size() << std::endl
           << "}" << std::endl;
      return output;
    }
 
};
 
class LightBarManager : public carma_ros2_utils::CarmaLifecycleNode
{
    public:
 
        explicit LightBarManager(const rclcpp::NodeOptions &);
 
        std::shared_ptr<LightBarManagerWorker> getWorker();
 
        void turnOffAll();
 
        int setIndicator(LightBarIndicator ind, IndicatorStatus ind_status, const std::string& requester_name);
 
        void turnSignalCallback(automotive_platform_msgs::msg::TurnSignalCommand::UniquePtr msg_ptr);
 
        // Overrides
        carma_ros2_utils::CallbackReturn handle_on_configure(const rclcpp_lifecycle::State &);
        carma_ros2_utils::CallbackReturn handle_on_activate(const rclcpp_lifecycle::State &);
        carma_ros2_utils::CallbackReturn handle_on_deactivate(const rclcpp_lifecycle::State &);
        carma_ros2_utils::CallbackReturn handle_on_cleanup(const rclcpp_lifecycle::State &);        
        carma_ros2_utils::CallbackReturn handle_on_error(const rclcpp_lifecycle::State &);
        carma_ros2_utils::CallbackReturn handle_on_shutdown(const rclcpp_lifecycle::State &);
 
    private:
 
        Config config_;
 
        void processTurnSignal(const automotive_platform_msgs::msg::TurnSignalCommand& msg);
 
        // Node Data
        std::string node_name_ = "lightbar_manager";
        double spin_rate_ = 10.0;
        std::map<lightbar_manager::LightBarIndicator, std::string> prev_owners_before_turn_;
 
        // spin callback function
        bool spinCallBack();
 
        // Message/service callbacks
        bool requestControlCallBack(const std::shared_ptr<rmw_request_id_t>,
                                const std::shared_ptr<carma_msgs::srv::RequestIndicatorControl::Request> req,
                                std::shared_ptr<carma_msgs::srv::RequestIndicatorControl::Response> resp);
        bool releaseControlCallBack(const std::shared_ptr<rmw_request_id_t>,
                                const std::shared_ptr<carma_msgs::srv::ReleaseIndicatorControl::Request> req,
                                std::shared_ptr<carma_msgs::srv::ReleaseIndicatorControl::Response> resp);
        bool setIndicatorCallBack(const std::shared_ptr<rmw_request_id_t>,
                                const std::shared_ptr<carma_driver_msgs::srv::SetLightBarIndicator::Request> req,
                                std::shared_ptr<carma_driver_msgs::srv::SetLightBarIndicator::Response> resp);
        void stateChangeCallBack(carma_planning_msgs::msg::GuidanceState::UniquePtr msg_ptr);
 
        // Service servers/clients
        carma_ros2_utils::ServicePtr<carma_msgs::srv::RequestIndicatorControl> request_control_server_;
        carma_ros2_utils::ServicePtr<carma_msgs::srv::ReleaseIndicatorControl> release_control_server_;
        carma_ros2_utils::ServicePtr<carma_driver_msgs::srv::SetLightBarIndicator> set_indicator_server_;
        carma_ros2_utils::ClientPtr<carma_driver_msgs::srv::SetLights> lightbar_driver_client_;
 
        // Publishers
        carma_ros2_utils::PubPtr<carma_msgs::msg::LightBarIndicatorControllers> indicator_control_publisher_;
 
        // Subscribers
        carma_ros2_utils::SubPtr<carma_planning_msgs::msg::GuidanceState> guidance_state_subscriber_;
        carma_ros2_utils::SubPtr<automotive_platform_msgs::msg::TurnSignalCommand> turn_signal_subscriber_;
 
        // LightBarManager Worker
        std::shared_ptr<LightBarManagerWorker> lbm_;
 
        rclcpp::TimerBase::SharedPtr pub_timer_;
 
        FRIEND_TEST(LightBarManagerNodeTest, testSetIndicator);
        FRIEND_TEST(LightBarManagerNodeTest, testTurnOffAll);
        FRIEND_TEST(LightBarManagerNodeTest, testTurnSignalCallback);
 
}; //class LightBarManagerNode
} // namespace lightbar_manager
 
#include "rclcpp_components/register_node_macro.hpp"
 
// Register the component with class_loader
RCLCPP_COMPONENTS_REGISTER_NODE(lightbar_manager::LightBarManager)