diff --git a/subsystem_controllers/config/drivers_controller_config.yaml b/subsystem_controllers/config/drivers_controller_config.yaml index 185d29c9c0..29d0b7e4cc 100644 --- a/subsystem_controllers/config/drivers_controller_config.yaml +++ b/subsystem_controllers/config/drivers_controller_config.yaml @@ -14,20 +14,17 @@ # Required subsystem controller nodes for the overall system to be functional and will have their lifecycle's managed required_subsystem_nodes: [''] - # List of nodes which will not be directly managed by this subsystem controller + # List of nodes which will not be directly managed by this subsystem controller # but which are required to be operational for the subsystem to function unmanaged_required_nodes: [''] # List of nodes in the namespace which will not be managed by this subsystem controller # Specifically includes the lidar and gps nodes which are handled in other subsystem controllers excluded_namespace_nodes : [''] - + # List of ros1 controller drivers (node name) to consider required and who's failure shall result in automation abort ros1_required_drivers: [''] - # List of ros1 camera drivers (node name) to consider required and who's failure shall result in automation abort. - ros1_camera_drivers: [''] - # Boolean: If this flag is true then all nodes under subsystem_namespace are treated as required in addition to any nodes in required_subsystem_nodes full_subsystem_required: false @@ -35,4 +32,4 @@ startup_duration: 30 # Double: The timeout threshold for essential drivers in ms - required_driver_timeout: 3000.0 \ No newline at end of file + required_driver_timeout: 3000.0 diff --git a/subsystem_controllers/include/subsystem_controllers/drivers_controller/driver_manager.hpp b/subsystem_controllers/include/subsystem_controllers/drivers_controller/driver_manager.hpp index 1cdd9dca9f..81bfebfbf5 100644 --- a/subsystem_controllers/include/subsystem_controllers/drivers_controller/driver_manager.hpp +++ b/subsystem_controllers/include/subsystem_controllers/drivers_controller/driver_manager.hpp @@ -16,89 +16,84 @@ * the License. */ -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include +#include #include + +#include +#include #include -#include "entry_manager.hpp" +#include +#include +#include + #include "entry.hpp" +#include "entry_manager.hpp" +#include + #include #include -#include - +#include +#include +#include +#include namespace subsystem_controllers { - - /** - * \brief The DriverManager serves as a component to manage CARMA required ROS1 Drivers - */ - class DriverManager - { - public: - - /*! - * \brief Default constructor for DriverManager with driver_timeout_ = 1000ms - */ - DriverManager(); - - /** - * \brief Constructor for DriverManager - * - * \param critical_driver_names The set of drivers which will be treated as required. A failure in these plugins will result in an exception - * \param camera_entries The set of camera drivers. - * \param driver_timeout The timeout threshold for essential drivers - */ - DriverManager(const std::vector& critical_driver_names, - const std::vector& camera_entries, - const long driver_timeout); - - - /*! - * \brief Update driver status - */ - void update_driver_status(const carma_driver_msgs::msg::DriverStatus::SharedPtr msg, long current_time); - - /*! - * \brief Check if all critical drivers are operational - */ - std::string are_critical_drivers_operational(long current_time); - - /*! - * \brief Evaluate if the sensor is available - */ - void evaluate_sensor(int &sensor_input,bool available,long current_time,long timestamp,long driver_timeout); - - /*! - * \brief Handle the spin and publisher - */ - carma_msgs::msg::SystemAlert handle_spin(long time_now,long start_up_timestamp,long startup_duration); - - protected: - - //list of critical drivers - std::vector critical_drivers_; - - //list of camera entries - std::vector camera_entries_; - - //! Entry manager to keep track of detected plugins - std::shared_ptr em_; - - // timeout for critical driver timeout in milliseconds - long driver_timeout_ = 1000; - - bool starting_up_ = true; - - FRIEND_TEST(DriverManagerTest, testCarTruckHandleSpinFatalUnknown); - - }; -} \ No newline at end of file +/** + * \brief The DriverManager serves as a component to manage CARMA required ROS1 Drivers + */ +class DriverManager +{ +public: + /*! + * \brief Default constructor for DriverManager with driver_timeout_ = 1000ms + */ + DriverManager(); + + /** + * \brief Constructor for DriverManager + * + * \param critical_driver_names The set of drivers which will be treated as required. A failure in + * these plugins will result in an exception \param driver_timeout The timeout threshold for + * essential drivers + */ + DriverManager(const std::vector & critical_driver_names, const long driver_timeout); + + /*! + * \brief Update driver status + */ + void update_driver_status( + const carma_driver_msgs::msg::DriverStatus::SharedPtr msg, long current_time); + + /*! + * \brief Check if all critical drivers are operational + */ + std::string are_critical_drivers_operational(long current_time); + + /*! + * \brief Evaluate if the sensor is available + */ + void evaluate_sensor( + int & sensor_input, bool available, long current_time, long timestamp, long driver_timeout); + + /*! + * \brief Handle the spin and publisher + */ + carma_msgs::msg::SystemAlert handle_spin( + long time_now, long start_up_timestamp, long startup_duration); + +protected: + // list of critical drivers + std::vector critical_drivers_; + + //! Entry manager to keep track of detected plugins + std::shared_ptr em_; + + // timeout for critical driver timeout in milliseconds + long driver_timeout_ = 1000; + + bool starting_up_ = true; + + FRIEND_TEST(DriverManagerTest, testCarTruckHandleSpinFatalUnknown); +}; +} // namespace subsystem_controllers diff --git a/subsystem_controllers/include/subsystem_controllers/drivers_controller/drivers_controller_config.hpp b/subsystem_controllers/include/subsystem_controllers/drivers_controller/drivers_controller_config.hpp index fa07914fbb..8feeec5a46 100644 --- a/subsystem_controllers/include/subsystem_controllers/drivers_controller/drivers_controller_config.hpp +++ b/subsystem_controllers/include/subsystem_controllers/drivers_controller/drivers_controller_config.hpp @@ -21,51 +21,40 @@ namespace subsystem_controllers { - /** - * \brief Stuct containing the algorithm configuration values for the GuidanceController - */ - struct DriversControllerConfig - +/** + * \brief Stuct containing the algorithm configuration values for the GuidanceController + */ +struct DriversControllerConfig + +{ + //! List of ros1 controller drivers (node name) to consider required and who's failure shall + //! result in automation abort. + std::vector ros1_required_drivers_; + //! List of nodes in the namespace which will not be managed by this subsystem controller + std::vector excluded_namespace_nodes_; + //! The time allocated for system startup in seconds + int startup_duration_; + //! The timeout threshold for essential drivers in ms + double driver_timeout_ = 1000; + + // Stream operator for this config + friend std::ostream & operator<<(std::ostream & output, const DriversControllerConfig & c) { - //! List of ros1 controller drivers (node name) to consider required and who's failure shall result in automation abort. - std::vector ros1_required_drivers_; - //! List of ros1 camera drivers (node name) to consider required and who's failure shall result in automation abort. - std::vector ros1_camera_drivers_; - //! List of nodes in the namespace which will not be managed by this subsystem controller - std::vector excluded_namespace_nodes_; - //! The time allocated for system startup in seconds - int startup_duration_; - //! The timeout threshold for essential drivers in ms - double driver_timeout_ = 1000; - + output << "DriversControllerConfig { " << std::endl << "ros1_required_drivers: [ " << std::endl; - // Stream operator for this config - friend std::ostream &operator<<(std::ostream &output, const DriversControllerConfig &c) - { - - output << "DriversControllerConfig { " << std::endl - << "ros1_required_drivers: [ " << std::endl; - - for (auto node : c.ros1_required_drivers_) - output << node << " "; + for (auto node : c.ros1_required_drivers_) output << node << " "; - output << "] " << std::endl << "ros1_camera_drivers: [ "; + output << "] " << std::endl << "excluded_namespace_nodes: [ "; - for (auto node : c.ros1_camera_drivers_) - output << node << " "; - - output << "] " << std::endl << "excluded_namespace_nodes: [ "; + for (auto node : c.excluded_namespace_nodes_) output << node << " "; - for (auto node : c.excluded_namespace_nodes_) - output << node << " "; + output << "] " << std::endl << "startup_duration: " << c.startup_duration_ << std::endl; - output<< "] " << std::endl << "startup_duration: "<< c.startup_duration_ << std::endl; + output << "driver_timeout: " << c.driver_timeout_ << std::endl - output <<"driver_timeout: "<< c.driver_timeout_ << std::endl - - << "}" << std::endl; - return output; - } - }; + << "}" << std::endl; + return output; + } +}; -} // namespace subsystem_controllers +} // namespace subsystem_controllers diff --git a/subsystem_controllers/include/subsystem_controllers/drivers_controller/drivers_controller_node.hpp b/subsystem_controllers/include/subsystem_controllers/drivers_controller/drivers_controller_node.hpp index 349a4602c5..5d22197eda 100644 --- a/subsystem_controllers/include/subsystem_controllers/drivers_controller/drivers_controller_node.hpp +++ b/subsystem_controllers/include/subsystem_controllers/drivers_controller/drivers_controller_node.hpp @@ -35,14 +35,14 @@ namespace subsystem_controllers class DriversControllerNode : public BaseSubsystemController { public: - + DriversControllerNode() = delete; ~DriversControllerNode() = default; /** * \brief Constructor. Set explicitly to support node composition. - * + * * \param options The node options to use for configuring this node */ explicit DriversControllerNode(const rclcpp::NodeOptions &options); @@ -62,7 +62,7 @@ namespace subsystem_controllers // message/service callbacks void driver_discovery_cb(const carma_driver_msgs::msg::DriverStatus::SharedPtr msg); - //! Timer callback function to check status of required ros1 drivers + //! Timer callback function to check status of required ros1 drivers void timer_callback(); carma_ros2_utils::CallbackReturn handle_on_configure(const rclcpp_lifecycle::State &prev_state); @@ -70,7 +70,6 @@ namespace subsystem_controllers //! ROS parameters std::vector ros1_required_drivers_; - std::vector ros1_camera_drivers_; // record of startup timestamp long start_up_timestamp_; @@ -83,4 +82,3 @@ namespace subsystem_controllers }; } // namespace v2x_controller - diff --git a/subsystem_controllers/include/subsystem_controllers/drivers_controller/entry_manager.hpp b/subsystem_controllers/include/subsystem_controllers/drivers_controller/entry_manager.hpp index 46a3f924ae..0be3d5eb73 100644 --- a/subsystem_controllers/include/subsystem_controllers/drivers_controller/entry_manager.hpp +++ b/subsystem_controllers/include/subsystem_controllers/drivers_controller/entry_manager.hpp @@ -16,79 +16,62 @@ * the License. */ -#include -#include #include -#include "entry.hpp" +#include +#include "entry.hpp" +#include namespace subsystem_controllers { - /** - * \brief The EntryManager serves as a component to track the status of each required CARMA ROS 1 driver - */ - class EntryManager - { - - public: - - /*! - * \brief Default constructor for EntryManager. - */ - EntryManager(); - /*! - * \brief Constructor for EntryManager to set required entries. - */ - EntryManager(std::vector required_entries); - - /*! - * \brief Constructor for EntryManager to set required entries and camera entires. - */ - EntryManager(std::vector required_entries, std::vector camera_entries); - - /*! - * \brief Add a new entry if the given name does not exist. - * Update an existing entry if the given name exists. - */ - void update_entry(const Entry& entry); - - /*! - * \brief Get all registed entries as a list. - */ - std::vector get_entries() const; - - /*! - * \brief Get a entry using name as the key. - */ - boost::optional get_entry_by_name(const std::string& name) const; - - /*! - * \brief Delete an entry using the given name as the key. - */ - void delete_entry(const std::string& name); - - /*! - * \brief Check if the entry is required - */ - bool is_entry_required(const std::string& name) const; - - /*! - * \brief Check if the entry is a required camera entry - */ - int is_camera_entry_required(const std::string& name) const; - - private: - - //! private list to keep track of all entries - std::vector entry_list_; - - //list of required entries - std::vector required_entries_; - - //list of camera entries - std::vector camera_entries_; - - }; - - -} \ No newline at end of file +/** + * \brief The EntryManager serves as a component to track the status of each required CARMA ROS 1 + * driver + */ +class EntryManager +{ +public: + /*! + * \brief Default constructor for EntryManager. + */ + EntryManager(); + /*! + * \brief Constructor for EntryManager to set required entries. + */ + EntryManager(std::vector required_entries); + + /*! + * \brief Add a new entry if the given name does not exist. + * Update an existing entry if the given name exists. + */ + void update_entry(const Entry & entry); + + /*! + * \brief Get all registed entries as a list. + */ + std::vector get_entries() const; + + /*! + * \brief Get a entry using name as the key. + */ + boost::optional get_entry_by_name(const std::string & name) const; + + /*! + * \brief Delete an entry using the given name as the key. + */ + void delete_entry(const std::string & name); + + /*! + * \brief Check if the entry is required + */ + bool is_entry_required(const std::string & name) const; + +private: + //! private list to keep track of all entries + std::vector entry_list_; + + // list of required entries + std::vector required_entries_; +}; + +} // namespace subsystem_controllers diff --git a/subsystem_controllers/src/drivers_controller/driver_manager.cpp b/subsystem_controllers/src/drivers_controller/driver_manager.cpp index f4a5aff32a..bdd68ee545 100644 --- a/subsystem_controllers/src/drivers_controller/driver_manager.cpp +++ b/subsystem_controllers/src/drivers_controller/driver_manager.cpp @@ -14,141 +14,105 @@ * the License. */ +#include "subsystem_controllers/drivers_controller/driver_manager.hpp" #include -#include #include #include -#include "subsystem_controllers/drivers_controller/driver_manager.hpp" +#include using std_msec = std::chrono::milliseconds; namespace subsystem_controllers { - DriverManager::DriverManager() {} - - DriverManager::DriverManager(const std::vector& critical_driver_names, - const std::vector& camera_entries, - const long driver_timeout) - : critical_drivers_(critical_driver_names.begin(), critical_driver_names.end()), - camera_entries_(camera_entries.begin(), camera_entries.end()), - driver_timeout_(driver_timeout) - { - // Intialize entry manager - em_ = std::make_shared(critical_driver_names, camera_entries); - - } - - - carma_msgs::msg::SystemAlert DriverManager::handle_spin(long time_now,long start_up_timestamp,long startup_duration) - { - carma_msgs::msg::SystemAlert alert; - - std::string status = are_critical_drivers_operational(time_now); - if(status.compare("s_1_c_1") == 0) - { - starting_up_ = false; - alert.description = "All essential drivers are ready"; - alert.type = carma_msgs::msg::SystemAlert::DRIVERS_READY; - return alert; - } - else if(starting_up_ && (time_now - start_up_timestamp <= startup_duration)) - { - alert.description = "System is starting up..."; - alert.type = carma_msgs::msg::SystemAlert::NOT_READY; - return alert; - } - else if(status.compare("s_1_c_0")==0) - { - alert.description = "Camera Failed"; - alert.type = carma_msgs::msg::SystemAlert::SHUTDOWN; - return alert; - } - else if(status.compare("s_0") == 0) - { - alert.description = "SSC Failed"; - alert.type = carma_msgs::msg::SystemAlert::SHUTDOWN; - return alert; - } - else - { - alert.description = "Unknown problem assessing essential driver availability"; - alert.type = carma_msgs::msg::SystemAlert::FATAL; - return alert; - } - - } - - - void DriverManager::update_driver_status(const carma_driver_msgs::msg::DriverStatus::SharedPtr msg, long current_time) - { - // update driver status is only called in response to a message received on driver_discovery. This topic is only being published in ros1 - Entry driver_status(msg->status == carma_driver_msgs::msg::DriverStatus::OPERATIONAL || msg->status == carma_driver_msgs::msg::DriverStatus::DEGRADED, - msg->name,current_time); - - em_->update_entry(driver_status); - } - - - void DriverManager::evaluate_sensor(int &sensor_input,bool available,long current_time,long timestamp,long driver_timeout) - { - - if((!available) || (current_time-timestamp > driver_timeout)) - { - sensor_input=0; - } - else - { - sensor_input=1; - } - - } - - std::string DriverManager::are_critical_drivers_operational(long current_time) - { - int ssc=0; - int camera=0; - - std::vector driver_list = em_->get_entries(); //Real time driver list from driver status - for(auto i = driver_list.begin(); i < driver_list.end(); ++i) - { - RCLCPP_DEBUG_STREAM(rclcpp::get_logger("subsystem_controller"), "i->name_: " << i->name_ - << ", current_time: " << current_time - << ", i->timestamp_: " << i->timestamp_ - << ", difference: " << current_time-(i->timestamp_) ); - if(em_->is_entry_required(i->name_)) - { - evaluate_sensor(ssc,i->available_,current_time,i->timestamp_,driver_timeout_); - } - else if(em_->is_camera_entry_required(i->name_)==0) - { - evaluate_sensor(camera,i->available_,current_time,i->timestamp_,driver_timeout_); - } - } - - // Manual disable of ssc entry in case ssc wrapper is in ros2 - if (critical_drivers_.empty()) - { - ssc = 1; - } - - ///////////////////// - //Decision making - if (ssc == 1 && camera == 1) - { - return "s_1_c_1"; - } - else if (ssc == 1 && camera == 0) - { - return "s_1_c_0"; - } - else{ - return "s_0"; - } +DriverManager::DriverManager() {} +DriverManager::DriverManager( + const std::vector & critical_driver_names, const long driver_timeout) +: critical_drivers_(critical_driver_names.begin(), critical_driver_names.end()), + driver_timeout_(driver_timeout) +{ + // Intialize entry manager + em_ = std::make_shared(critical_driver_names); +} +carma_msgs::msg::SystemAlert DriverManager::handle_spin( + long time_now, long start_up_timestamp, long startup_duration) +{ + carma_msgs::msg::SystemAlert alert; + + std::string status = are_critical_drivers_operational(time_now); + if (status.compare("s_1_c_1") == 0) { + starting_up_ = false; + alert.description = "All essential drivers are ready"; + alert.type = carma_msgs::msg::SystemAlert::DRIVERS_READY; + return alert; + } else if (starting_up_ && (time_now - start_up_timestamp <= startup_duration)) { + alert.description = "System is starting up..."; + alert.type = carma_msgs::msg::SystemAlert::NOT_READY; + return alert; + } else if (status.compare("s_0") == 0) { + alert.description = "SSC Failed"; + alert.type = carma_msgs::msg::SystemAlert::SHUTDOWN; + return alert; + } else { + alert.description = "Unknown problem assessing essential driver availability"; + alert.type = carma_msgs::msg::SystemAlert::FATAL; + return alert; + } +} + +void DriverManager::update_driver_status( + const carma_driver_msgs::msg::DriverStatus::SharedPtr msg, long current_time) +{ + // update driver status is only called in response to a message received on driver_discovery. This + // topic is only being published in ros1 + Entry driver_status( + msg->status == carma_driver_msgs::msg::DriverStatus::OPERATIONAL || + msg->status == carma_driver_msgs::msg::DriverStatus::DEGRADED, + msg->name, current_time); + + em_->update_entry(driver_status); +} + +void DriverManager::evaluate_sensor( + int & sensor_input, bool available, long current_time, long timestamp, long driver_timeout) +{ + if ((!available) || (current_time - timestamp > driver_timeout)) { + sensor_input = 0; + } else { + sensor_input = 1; + } +} + +std::string DriverManager::are_critical_drivers_operational(long current_time) +{ + int ssc = 0; + + std::vector driver_list = em_->get_entries(); // Real time driver list from driver status + for (auto i = driver_list.begin(); i < driver_list.end(); ++i) { + RCLCPP_DEBUG_STREAM( + rclcpp::get_logger("subsystem_controller"), + "i->name_: " << i->name_ << ", current_time: " << current_time << ", i->timestamp_: " + << i->timestamp_ << ", difference: " << current_time - (i->timestamp_)); + if (em_->is_entry_required(i->name_)) { + evaluate_sensor(ssc, i->available_, current_time, i->timestamp_, driver_timeout_); } - - -} \ No newline at end of file + } + + // Manual disable of ssc entry in case ssc wrapper is in ros2 + if (critical_drivers_.empty()) { + ssc = 1; + } + + ///////////////////// + // Decision making + if (ssc == 1) { + return "s_1"; + } else { + return "s_0"; + } +} + +} // namespace subsystem_controllers diff --git a/subsystem_controllers/src/drivers_controller/drivers_controller_node.cpp b/subsystem_controllers/src/drivers_controller/drivers_controller_node.cpp index 55afc07a86..5295e46375 100644 --- a/subsystem_controllers/src/drivers_controller/drivers_controller_node.cpp +++ b/subsystem_controllers/src/drivers_controller/drivers_controller_node.cpp @@ -19,146 +19,148 @@ namespace subsystem_controllers { - DriversControllerNode::DriversControllerNode(const rclcpp::NodeOptions &options) - : BaseSubsystemController(options) - { - // Don't automatically trigger state transitions from base class on configure - // In this class the managed nodes list first needs to be modified then the transition will be triggered manually - trigger_managed_nodes_configure_from_base_class_ = false; - - config_.startup_duration_ = declare_parameter("startup_duration", config_.startup_duration_); - config_.driver_timeout_ = declare_parameter("required_driver_timeout", config_.driver_timeout_); - - // carma-config parameters - config_.ros1_required_drivers_ = declare_parameter>("ros1_required_drivers", config_.ros1_required_drivers_); - config_.ros1_camera_drivers_ = declare_parameter>("ros1_camera_drivers", config_.ros1_camera_drivers_); - config_.excluded_namespace_nodes_ = declare_parameter>("excluded_namespace_nodes", config_.excluded_namespace_nodes_); +DriversControllerNode::DriversControllerNode(const rclcpp::NodeOptions & options) +: BaseSubsystemController(options) +{ + // Don't automatically trigger state transitions from base class on configure + // In this class the managed nodes list first needs to be modified then the transition will be + // triggered manually + trigger_managed_nodes_configure_from_base_class_ = false; + + config_.startup_duration_ = declare_parameter("startup_duration", config_.startup_duration_); + config_.driver_timeout_ = + declare_parameter("required_driver_timeout", config_.driver_timeout_); + + // carma-config parameters + config_.ros1_required_drivers_ = declare_parameter>( + "ros1_required_drivers", config_.ros1_required_drivers_); + config_.excluded_namespace_nodes_ = declare_parameter>( + "excluded_namespace_nodes", config_.excluded_namespace_nodes_); +} + +carma_ros2_utils::CallbackReturn DriversControllerNode::handle_on_configure( + const rclcpp_lifecycle::State & prev_state) +{ + auto base_return = BaseSubsystemController::handle_on_configure(prev_state); + if (base_return != carma_ros2_utils::CallbackReturn::SUCCESS) { + RCLCPP_ERROR(get_logger(), "Drivers Controller could not configure"); + return base_return; } - carma_ros2_utils::CallbackReturn DriversControllerNode::handle_on_configure(const rclcpp_lifecycle::State &prev_state) - { - auto base_return = BaseSubsystemController::handle_on_configure(prev_state); - - if (base_return != carma_ros2_utils::CallbackReturn::SUCCESS) { - RCLCPP_ERROR(get_logger(), "Drivers Controller could not configure"); - return base_return; - } - - config_ = DriversControllerConfig(); - - // Load required plugins and default enabled plugins - get_parameter>("ros1_required_drivers", config_.ros1_required_drivers_); - get_parameter>("ros1_camera_drivers", config_.ros1_camera_drivers_); - get_parameter("startup_duration", config_.startup_duration_); - get_parameter("required_driver_timeout", config_.driver_timeout_); - get_parameter>("excluded_namespace_nodes", config_.excluded_namespace_nodes_); - - RCLCPP_INFO_STREAM(get_logger(), "Config: " << config_); - - // Handle fact that parameter vectors cannot be empty - if (config_.ros1_required_drivers_.size() == 1 && config_.ros1_required_drivers_[0].empty()) { - config_.ros1_required_drivers_.clear(); - } - if (config_.ros1_camera_drivers_.size() == 1 && config_.ros1_camera_drivers_[0].empty()) { - config_.ros1_camera_drivers_.clear(); - } - if (config_.excluded_namespace_nodes_.size() == 1 && config_.excluded_namespace_nodes_[0].empty()) { - config_.excluded_namespace_nodes_.clear(); - } - - auto base_managed_nodes = lifecycle_mgr_.get_managed_nodes(); - // Update managed nodes - // Collect namespace nodes not managed by other subsystem controllers - manually specified in carma-config - auto updated_managed_nodes = get_non_intersecting_set(base_managed_nodes, config_.excluded_namespace_nodes_); - - lifecycle_mgr_.set_managed_nodes(updated_managed_nodes); - - driver_manager_ = std::make_shared( - config_.ros1_required_drivers_, - config_.ros1_camera_drivers_, - config_.driver_timeout_ - ); - - // record starup time - start_up_timestamp_ = this->now().nanoseconds() / 1e6; - - driver_status_sub_ = create_subscription("driver_discovery", 20, std::bind(&DriversControllerNode::driver_discovery_cb, this, std::placeholders::_1)); + config_ = DriversControllerConfig(); - timer_ = create_timer(get_clock(), std::chrono::milliseconds(1000), std::bind(&DriversControllerNode::timer_callback,this)); + // Load required plugins and default enabled plugins + get_parameter>("ros1_required_drivers", config_.ros1_required_drivers_); + get_parameter("startup_duration", config_.startup_duration_); + get_parameter("required_driver_timeout", config_.driver_timeout_); + get_parameter>( + "excluded_namespace_nodes", config_.excluded_namespace_nodes_); - // Configure our drivers - bool success = lifecycle_mgr_.configure(std::chrono::milliseconds(base_config_.service_timeout_ms), std::chrono::milliseconds(base_config_.call_timeout_ms)).empty(); - - if (success) - { - RCLCPP_INFO_STREAM(get_logger(), "Subsystem able to configure"); - return CallbackReturn::SUCCESS; - } - else - { - RCLCPP_INFO_STREAM(get_logger(), "Subsystem unable to configure"); - return CallbackReturn::FAILURE; - } + RCLCPP_INFO_STREAM(get_logger(), "Config: " << config_); + // Handle fact that parameter vectors cannot be empty + if (config_.ros1_required_drivers_.size() == 1 && config_.ros1_required_drivers_[0].empty()) { + config_.ros1_required_drivers_.clear(); } - - carma_ros2_utils::CallbackReturn DriversControllerNode::handle_on_activate(const rclcpp_lifecycle::State &prev_state) - { - - // Reset to automatically trigger state transitions from base class - trigger_managed_nodes_configure_from_base_class_ = true; - - // return only either SUCCESS or FAILURE - auto base_return = BaseSubsystemController::handle_on_activate(prev_state); // This will activate all base_managed_nodes - - if (base_return == CallbackReturn::SUCCESS) { - RCLCPP_INFO(get_logger(), "Driver Controller activated!"); - } - else if (base_return == CallbackReturn::FAILURE) - { - RCLCPP_ERROR(get_logger(), "Driver Controller encountered FAILURE when trying to activate the subsystems... please check which driver failed to activate..."); - } - - return base_return; - + if ( + config_.excluded_namespace_nodes_.size() == 1 && config_.excluded_namespace_nodes_[0].empty()) { + config_.excluded_namespace_nodes_.clear(); } - void DriversControllerNode::timer_callback() - { - - long time_now = this->now().nanoseconds() / 1e6; - rclcpp::Duration sd(config_.startup_duration_, 0); - long start_duration = sd.nanoseconds()/1e6; - - auto dm = driver_manager_->handle_spin(time_now, start_up_timestamp_, start_duration); - - //Wait for node to be activated - if (get_current_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE){ - if (!prev_alert) { - prev_alert = dm; - publish_system_alert(dm); - } - else if ( prev_alert->type == dm.type && prev_alert->description.compare(dm.description) == 0) { // Do not publish duplicate alerts - RCLCPP_DEBUG_STREAM(get_logger(), "No change to alert status"); - } - else{ - prev_alert = dm; - publish_system_alert(dm); - } - } + auto base_managed_nodes = lifecycle_mgr_.get_managed_nodes(); + // Update managed nodes + // Collect namespace nodes not managed by other subsystem controllers - manually specified in + // carma-config + auto updated_managed_nodes = + get_non_intersecting_set(base_managed_nodes, config_.excluded_namespace_nodes_); + + lifecycle_mgr_.set_managed_nodes(updated_managed_nodes); + + driver_manager_ = std::make_shared( + config_.ros1_required_drivers_, config_.driver_timeout_); + + // record starup time + start_up_timestamp_ = this->now().nanoseconds() / 1e6; + + driver_status_sub_ = create_subscription( + "driver_discovery", 20, + std::bind(&DriversControllerNode::driver_discovery_cb, this, std::placeholders::_1)); + + timer_ = create_timer( + get_clock(), std::chrono::milliseconds(1000), + std::bind(&DriversControllerNode::timer_callback, this)); + + // Configure our drivers + bool success = lifecycle_mgr_ + .configure( + std::chrono::milliseconds(base_config_.service_timeout_ms), + std::chrono::milliseconds(base_config_.call_timeout_ms)) + .empty(); + + if (success) { + RCLCPP_INFO_STREAM(get_logger(), "Subsystem able to configure"); + return CallbackReturn::SUCCESS; + } else { + RCLCPP_INFO_STREAM(get_logger(), "Subsystem unable to configure"); + return CallbackReturn::FAILURE; + } +} +carma_ros2_utils::CallbackReturn DriversControllerNode::handle_on_activate( + const rclcpp_lifecycle::State & prev_state) +{ + // Reset to automatically trigger state transitions from base class + trigger_managed_nodes_configure_from_base_class_ = true; + + // return only either SUCCESS or FAILURE + auto base_return = BaseSubsystemController::handle_on_activate( + prev_state); // This will activate all base_managed_nodes + + if (base_return == CallbackReturn::SUCCESS) { + RCLCPP_INFO(get_logger(), "Driver Controller activated!"); + } else if (base_return == CallbackReturn::FAILURE) { + RCLCPP_ERROR( + get_logger(), + "Driver Controller encountered FAILURE when trying to activate the subsystems... please " + "check which driver failed to activate..."); } + return base_return; +} - void DriversControllerNode::driver_discovery_cb(const carma_driver_msgs::msg::DriverStatus::SharedPtr msg) - { - // Driver discovery only published by ros1 drivers - driver_manager_->update_driver_status(msg, this->now().nanoseconds()/1e6); +void DriversControllerNode::timer_callback() +{ + long time_now = this->now().nanoseconds() / 1e6; + rclcpp::Duration sd(config_.startup_duration_, 0); + long start_duration = sd.nanoseconds() / 1e6; + + auto dm = driver_manager_->handle_spin(time_now, start_up_timestamp_, start_duration); + + // Wait for node to be activated + if (get_current_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) { + if (!prev_alert) { + prev_alert = dm; + publish_system_alert(dm); + } else if ( + prev_alert->type == dm.type && + prev_alert->description.compare(dm.description) == 0) { // Do not publish duplicate alerts + RCLCPP_DEBUG_STREAM(get_logger(), "No change to alert status"); + } else { + prev_alert = dm; + publish_system_alert(dm); + } } +} +void DriversControllerNode::driver_discovery_cb( + const carma_driver_msgs::msg::DriverStatus::SharedPtr msg) +{ + // Driver discovery only published by ros1 drivers + driver_manager_->update_driver_status(msg, this->now().nanoseconds() / 1e6); +} -} // namespace subsystem_controllers +} // namespace subsystem_controllers #include "rclcpp_components/register_node_macro.hpp" diff --git a/subsystem_controllers/src/drivers_controller/entry_manager.cpp b/subsystem_controllers/src/drivers_controller/entry_manager.cpp index 02bdce3da0..693578d933 100644 --- a/subsystem_controllers/src/drivers_controller/entry_manager.cpp +++ b/subsystem_controllers/src/drivers_controller/entry_manager.cpp @@ -15,88 +15,66 @@ */ #include "subsystem_controllers/drivers_controller/entry_manager.hpp" + #include namespace subsystem_controllers { - EntryManager::EntryManager() {} - - EntryManager::EntryManager(std::vector required_entries):required_entries_(required_entries) {} - - EntryManager::EntryManager(std::vector required_entries, std::vector camera_entries) - :required_entries_(required_entries), camera_entries_(camera_entries){} +EntryManager::EntryManager() {} - void EntryManager::update_entry(const Entry& entry) - { - for(auto i = entry_list_.begin(); i < entry_list_.end(); ++i) - { - if(i->name_.compare(entry.name_) == 0) - { - // name entry wont change - i->available_ = entry.available_; - i->timestamp_ = entry.timestamp_; - return; - } - } - entry_list_.push_back(entry); - } +EntryManager::EntryManager(std::vector required_entries) +: required_entries_(required_entries) +{ +} - std::vector EntryManager::get_entries() const - { - // returns the copy of the original list - return std::vector(entry_list_); +void EntryManager::update_entry(const Entry & entry) +{ + for (auto i = entry_list_.begin(); i < entry_list_.end(); ++i) { + if (i->name_.compare(entry.name_) == 0) { + // name entry wont change + i->available_ = entry.available_; + i->timestamp_ = entry.timestamp_; + return; } + } + entry_list_.push_back(entry); +} - void EntryManager::delete_entry(const std::string& name) - { - for(auto i = entry_list_.begin(); i < entry_list_.end(); ++i) - { - if(i->name_.compare(name) == 0) - { - entry_list_.erase(i); - return; - } - } - } +std::vector EntryManager::get_entries() const +{ + // returns the copy of the original list + return std::vector(entry_list_); +} - boost::optional EntryManager::get_entry_by_name(const std::string& name) const - { - for(auto i = entry_list_.begin(); i < entry_list_.end(); ++i) - { - if(i->name_.compare(name) == 0) - { - return *i; - } - } - // use boost::optional because requested entry might not exist - return boost::none; +void EntryManager::delete_entry(const std::string & name) +{ + for (auto i = entry_list_.begin(); i < entry_list_.end(); ++i) { + if (i->name_.compare(name) == 0) { + entry_list_.erase(i); + return; } + } +} - bool EntryManager::is_entry_required(const std::string& name) const - { - for(auto i = required_entries_.begin(); i < required_entries_.end(); ++i) - { - if(i->compare(name) == 0) - { - return true; - } - } - return false; +boost::optional EntryManager::get_entry_by_name(const std::string & name) const +{ + for (auto i = entry_list_.begin(); i < entry_list_.end(); ++i) { + if (i->name_.compare(name) == 0) { + return *i; } + } + // use boost::optional because requested entry might not exist + return boost::none; +} - int EntryManager::is_camera_entry_required(const std::string& name) const - { - for(int i = 0;i < camera_entries_.size(); i++) - { - if(camera_entries_[i].compare(name) == 0) - { - return i; - } - } - - // default like above? - return -1; +bool EntryManager::is_entry_required(const std::string & name) const +{ + for (auto i = required_entries_.begin(); i < required_entries_.end(); ++i) { + if (i->compare(name) == 0) { + return true; } + } + return false; +} - -} \ No newline at end of file +} // namespace subsystem_controllers diff --git a/subsystem_controllers/test/test_driver_subsystem/test_driver_manager.cpp b/subsystem_controllers/test/test_driver_subsystem/test_driver_manager.cpp index e02fedce84..3bc249e635 100644 --- a/subsystem_controllers/test/test_driver_subsystem/test_driver_manager.cpp +++ b/subsystem_controllers/test/test_driver_subsystem/test_driver_manager.cpp @@ -14,250 +14,165 @@ * the License. */ -#include #include +#include namespace subsystem_controllers { - TEST(DriverManagerTest, testCarNormalDriverStatus) - { - // inordinary case where no critical drivers are specified - DriverManager dm0({"ssc"}, {}, 0); - carma_driver_msgs::msg::DriverStatus msg0; - msg0.controller = true; - msg0.name = "controller"; - msg0.status = carma_driver_msgs::msg::DriverStatus::OPERATIONAL; - - auto msg0_pointer = std::make_shared(msg0); - - dm0.update_driver_status(msg0_pointer, 1000); - - EXPECT_EQ("s_0", dm0.are_critical_drivers_operational(1500)); - - std::vector required_drivers{"controller"}; - std::vector camera_drivers{"camera"}; - - DriverManager dm(required_drivers, camera_drivers, 1000L); - - carma_driver_msgs::msg::DriverStatus msg1; - msg1.controller = true; - msg1.name = "controller"; - msg1.status = carma_driver_msgs::msg::DriverStatus::OPERATIONAL; - carma_driver_msgs::msg::DriverStatus::SharedPtr msg1_pointer(new carma_driver_msgs::msg::DriverStatus(msg1)); - dm.update_driver_status(msg1_pointer, 1000); - - carma_driver_msgs::msg::DriverStatus msg2; - msg2.gnss = true; - msg2.name = "camera"; - msg2.status = carma_driver_msgs::msg::DriverStatus::OPERATIONAL; - carma_driver_msgs::msg::DriverStatus::SharedPtr msg2_pointer(new carma_driver_msgs::msg::DriverStatus(msg2)); - dm.update_driver_status(msg2_pointer, 1000); - - EXPECT_EQ("s_1_c_1", dm.are_critical_drivers_operational(1500)); - } - - TEST(DriverManagerTest, testSsc_1_Camera_0) - { - std::vector required_drivers{"controller"}; - std::vector camera_drivers{"camera"}; - - - DriverManager dm(required_drivers,camera_drivers, 1000L); - - carma_driver_msgs::msg::DriverStatus msg1; - msg1.controller = true; - msg1.name = "controller"; - msg1.status = carma_driver_msgs::msg::DriverStatus::OPERATIONAL; - carma_driver_msgs::msg::DriverStatus::SharedPtr msg1_pointer(new carma_driver_msgs::msg::DriverStatus(msg1)); - dm.update_driver_status(msg1_pointer, 1000); - - - carma_driver_msgs::msg::DriverStatus msg2; - msg2.gnss = true; - msg2.name = "camera"; - msg2.status = carma_driver_msgs::msg::DriverStatus::OFF; - carma_driver_msgs::msg::DriverStatus::SharedPtr msg2_pointer(new carma_driver_msgs::msg::DriverStatus(msg2)); - dm.update_driver_status(msg2_pointer, 1000); - - EXPECT_EQ("s_1_c_0", dm.are_critical_drivers_operational(1500)); - } - - TEST(DriverManagerTest, testCarErrorDriverStatusTimeOut) - { - std::vector required_drivers{"controller"}; - std::vector camera_drivers{"camera"}; - - DriverManager dm(required_drivers, camera_drivers, 1000L); - - carma_driver_msgs::msg::DriverStatus msg1; - msg1.controller = true; - msg1.name = "controller"; - msg1.status = carma_driver_msgs::msg::DriverStatus::OPERATIONAL; - carma_driver_msgs::msg::DriverStatus::SharedPtr msg1_pointer(new carma_driver_msgs::msg::DriverStatus(msg1)); - dm.update_driver_status(msg1_pointer, 1000); - - carma_driver_msgs::msg::DriverStatus msg2; - msg2.gnss = true; - msg2.name = "camera"; - msg2.status = carma_driver_msgs::msg::DriverStatus::DEGRADED; - carma_driver_msgs::msg::DriverStatus::SharedPtr msg2_pointer(new carma_driver_msgs::msg::DriverStatus(msg2)); - dm.update_driver_status(msg2_pointer, 1000); - - EXPECT_EQ("s_0", dm.are_critical_drivers_operational(2100)); - } - - TEST(DriverManagerTest, testCarHandleSpinDriversReady) - { - std::vector required_drivers{"controller"}; - std::vector camera_drivers{"camera"}; - - DriverManager dm(required_drivers,camera_drivers, 1000L); - - carma_driver_msgs::msg::DriverStatus msg1; - msg1.controller = true; - msg1.name = "controller"; - msg1.status = carma_driver_msgs::msg::DriverStatus::OPERATIONAL; - carma_driver_msgs::msg::DriverStatus::SharedPtr msg1_pointer(new carma_driver_msgs::msg::DriverStatus(msg1)); - dm.update_driver_status(msg1_pointer, 1000); - - - carma_driver_msgs::msg::DriverStatus msg2; - msg2.gnss = true; - msg2.name = "camera"; - msg2.status = carma_driver_msgs::msg::DriverStatus::OPERATIONAL; - carma_driver_msgs::msg::DriverStatus::SharedPtr msg2_pointer(new carma_driver_msgs::msg::DriverStatus(msg2)); - dm.update_driver_status(msg2_pointer, 1000); - - carma_msgs::msg::SystemAlert alert; - alert=dm.handle_spin(1500,150,750); - - EXPECT_EQ(5, alert.type); - } - - TEST(DriverManagerTest, testCarHandleSpinFatalSscWorking) - { - std::vector required_drivers{"controller"}; - std::vector camera_drivers{"camera"}; - - DriverManager dm(required_drivers, camera_drivers, 1000L); - - carma_driver_msgs::msg::DriverStatus msg1; - msg1.controller = true; - msg1.name = "controller"; - msg1.status = carma_driver_msgs::msg::DriverStatus::OFF; - carma_driver_msgs::msg::DriverStatus::SharedPtr msg1_pointer(new carma_driver_msgs::msg::DriverStatus(msg1)); - dm.update_driver_status(msg1_pointer, 1000); - - - carma_driver_msgs::msg::DriverStatus msg2; - msg2.gnss = true; - msg2.name = "camera"; - msg2.status = carma_driver_msgs::msg::DriverStatus::DEGRADED; - carma_driver_msgs::msg::DriverStatus::SharedPtr msg2_pointer(new carma_driver_msgs::msg::DriverStatus(msg2)); - dm.update_driver_status(msg2_pointer, 1000); - - - carma_msgs::msg::SystemAlert alert; - alert=dm.handle_spin(1500,150,750); - - EXPECT_EQ(6, alert.type); - } - - TEST(DriverManagerTest, testCarHandleSpinFatalUnknownInside) - { - std::vector required_drivers{"controller"}; - std::vector camera_drivers{"camera"}; - - DriverManager dm(required_drivers,camera_drivers, 1000L); - - carma_msgs::msg::SystemAlert alert; - alert=dm.handle_spin(1500,150,750); - - EXPECT_EQ(6, alert.type); - } - - - TEST(DriverManagerTest, testCarHandleSpinNotReadyCase1) - { - std::vector required_drivers{"controller"}; - std::vector camera_drivers{"camera"}; - - DriverManager dm(required_drivers,camera_drivers, 1000L); - - carma_driver_msgs::msg::DriverStatus msg1; - msg1.controller = true; - msg1.name = "controller"; - msg1.status = carma_driver_msgs::msg::DriverStatus::OFF; - carma_driver_msgs::msg::DriverStatus::SharedPtr msg1_pointer(new carma_driver_msgs::msg::DriverStatus(msg1)); - dm.update_driver_status(msg1_pointer, 1000); - - - carma_driver_msgs::msg::DriverStatus msg2; - msg2.gnss = true; - msg2.name = "camera"; - msg2.status = carma_driver_msgs::msg::DriverStatus::DEGRADED; - carma_driver_msgs::msg::DriverStatus::SharedPtr msg2_pointer(new carma_driver_msgs::msg::DriverStatus(msg2)); - dm.update_driver_status(msg2_pointer, 1000); - - - carma_msgs::msg::SystemAlert alert; - alert=dm.handle_spin(1500,1000,750); - - EXPECT_EQ(4, alert.type); - } - - TEST(DriverManagerTest, testCarHandleSpinNotReadyCase2) - { - std::vector required_drivers{"controller"}; - std::vector camera_drivers{"camera"}; - - DriverManager dm(required_drivers,camera_drivers, 1000L); - - carma_driver_msgs::msg::DriverStatus msg1; - msg1.controller = true; - msg1.name = "controller"; - msg1.status = carma_driver_msgs::msg::DriverStatus::OFF; - carma_driver_msgs::msg::DriverStatus::SharedPtr msg1_pointer(new carma_driver_msgs::msg::DriverStatus(msg1)); - dm.update_driver_status(msg1_pointer, 1000); - - carma_driver_msgs::msg::DriverStatus msg2; - msg2.gnss = true; - msg2.name = "camera"; - msg2.status = carma_driver_msgs::msg::DriverStatus::DEGRADED; - carma_driver_msgs::msg::DriverStatus::SharedPtr msg2_pointer(new carma_driver_msgs::msg::DriverStatus(msg2)); - dm.update_driver_status(msg2_pointer, 1000); - - carma_msgs::msg::SystemAlert alert; - alert=dm.handle_spin(1500,5000,750); - - EXPECT_EQ(4, alert.type); - } - - TEST(DriverManagerTest, testCarTruckHandleSpinFatalUnknown) - { - std::vector required_drivers{"controller"}; - std::vector camera_drivers{"camera"}; - - DriverManager dm(required_drivers,camera_drivers, 1000L); - - carma_driver_msgs::msg::DriverStatus msg1; - msg1.controller = true; - msg1.name = "controller"; - msg1.status = carma_driver_msgs::msg::DriverStatus::OFF; - carma_driver_msgs::msg::DriverStatus::SharedPtr msg1_pointer(new carma_driver_msgs::msg::DriverStatus(msg1)); - dm.update_driver_status(msg1_pointer, 1000); - - carma_driver_msgs::msg::DriverStatus msg2; - msg2.gnss = true; - msg2.name = "camera"; - msg2.status = carma_driver_msgs::msg::DriverStatus::OFF; - carma_driver_msgs::msg::DriverStatus::SharedPtr msg2_pointer(new carma_driver_msgs::msg::DriverStatus(msg2)); - dm.update_driver_status(msg2_pointer, 1000); - - carma_msgs::msg::SystemAlert alert; - alert=dm.handle_spin(1500,150,750); - - EXPECT_EQ(6, alert.type); - } -} \ No newline at end of file +TEST(DriverManagerTest, testCarNormalDriverStatus) +{ + // inordinary case where no critical drivers are specified + DriverManager dm0({"ssc"}, 0); + carma_driver_msgs::msg::DriverStatus msg0; + msg0.controller = true; + msg0.name = "controller"; + msg0.status = carma_driver_msgs::msg::DriverStatus::OPERATIONAL; + + auto msg0_pointer = std::make_shared(msg0); + + dm0.update_driver_status(msg0_pointer, 1000); + + EXPECT_EQ("s_0", dm0.are_critical_drivers_operational(1500)); + + std::vector required_drivers{"controller"}; + + carma_driver_msgs::msg::DriverStatus msg1; + msg1.controller = true; + msg1.name = "controller"; + msg1.status = carma_driver_msgs::msg::DriverStatus::OPERATIONAL; + carma_driver_msgs::msg::DriverStatus::SharedPtr msg1_pointer( + new carma_driver_msgs::msg::DriverStatus(msg1)); + dm0.update_driver_status(msg1_pointer, 1000); + + EXPECT_EQ("s_1", dm0.are_critical_drivers_operational(1500)); +} + +TEST(DriverManagerTest, testCarErrorDriverStatusTimeOut) +{ + std::vector required_drivers{"controller"}; + + DriverManager dm(required_drivers, 1000L); + + carma_driver_msgs::msg::DriverStatus msg1; + msg1.controller = true; + msg1.name = "controller"; + msg1.status = carma_driver_msgs::msg::DriverStatus::OPERATIONAL; + carma_driver_msgs::msg::DriverStatus::SharedPtr msg1_pointer( + new carma_driver_msgs::msg::DriverStatus(msg1)); + dm.update_driver_status(msg1_pointer, 1000); + + EXPECT_EQ("s_0", dm.are_critical_drivers_operational(2100)); +} + +TEST(DriverManagerTest, testCarHandleSpinDriversReady) +{ + std::vector required_drivers{"controller"}; + + DriverManager dm(required_drivers, 1000L); + + carma_driver_msgs::msg::DriverStatus msg1; + msg1.controller = true; + msg1.name = "controller"; + msg1.status = carma_driver_msgs::msg::DriverStatus::OPERATIONAL; + carma_driver_msgs::msg::DriverStatus::SharedPtr msg1_pointer( + new carma_driver_msgs::msg::DriverStatus(msg1)); + dm.update_driver_status(msg1_pointer, 1000); + + carma_msgs::msg::SystemAlert alert; + alert = dm.handle_spin(1500, 150, 750); + + EXPECT_EQ(5, alert.type); +} + +TEST(DriverManagerTest, testCarHandleSpinFatalSscWorking) +{ + std::vector required_drivers{"controller"}; + + DriverManager dm(required_drivers, 1000L); + + carma_driver_msgs::msg::DriverStatus msg1; + msg1.controller = true; + msg1.name = "controller"; + msg1.status = carma_driver_msgs::msg::DriverStatus::OFF; + carma_driver_msgs::msg::DriverStatus::SharedPtr msg1_pointer( + new carma_driver_msgs::msg::DriverStatus(msg1)); + dm.update_driver_status(msg1_pointer, 1000); + + carma_msgs::msg::SystemAlert alert; + alert = dm.handle_spin(1500, 150, 750); + + EXPECT_EQ(6, alert.type); +} + +TEST(DriverManagerTest, testCarHandleSpinFatalUnknownInside) +{ + std::vector required_drivers{"controller"}; + DriverManager dm(required_drivers, 1000L); + + carma_msgs::msg::SystemAlert alert; + alert = dm.handle_spin(1500, 150, 750); + + EXPECT_EQ(6, alert.type); +} + +TEST(DriverManagerTest, testCarHandleSpinNotReadyCase1) +{ + std::vector required_drivers{"controller"}; + + DriverManager dm(required_drivers, 1000L); + + carma_driver_msgs::msg::DriverStatus msg1; + msg1.controller = true; + msg1.name = "controller"; + msg1.status = carma_driver_msgs::msg::DriverStatus::OFF; + carma_driver_msgs::msg::DriverStatus::SharedPtr msg1_pointer( + new carma_driver_msgs::msg::DriverStatus(msg1)); + dm.update_driver_status(msg1_pointer, 1000); + + carma_msgs::msg::SystemAlert alert; + alert = dm.handle_spin(1500, 1000, 750); + + EXPECT_EQ(4, alert.type); +} + +TEST(DriverManagerTest, testCarHandleSpinNotReadyCase2) +{ + std::vector required_drivers{"controller"}; + + DriverManager dm(required_drivers, 1000L); + + carma_driver_msgs::msg::DriverStatus msg1; + msg1.controller = true; + msg1.name = "controller"; + msg1.status = carma_driver_msgs::msg::DriverStatus::OFF; + carma_driver_msgs::msg::DriverStatus::SharedPtr msg1_pointer( + new carma_driver_msgs::msg::DriverStatus(msg1)); + dm.update_driver_status(msg1_pointer, 1000); + + carma_msgs::msg::SystemAlert alert; + alert = dm.handle_spin(1500, 5000, 750); + + EXPECT_EQ(4, alert.type); +} + +TEST(DriverManagerTest, testCarTruckHandleSpinFatalUnknown) +{ + std::vector required_drivers{"controller"}; + + DriverManager dm(required_drivers, 1000L); + + carma_driver_msgs::msg::DriverStatus msg1; + msg1.controller = true; + msg1.name = "controller"; + msg1.status = carma_driver_msgs::msg::DriverStatus::OFF; + carma_driver_msgs::msg::DriverStatus::SharedPtr msg1_pointer( + new carma_driver_msgs::msg::DriverStatus(msg1)); + dm.update_driver_status(msg1_pointer, 1000); + + carma_msgs::msg::SystemAlert alert; + alert = dm.handle_spin(1500, 150, 750); + + EXPECT_EQ(6, alert.type); +} +} // namespace subsystem_controllers diff --git a/subsystem_controllers/test/test_entry_manager.cpp b/subsystem_controllers/test/test_entry_manager.cpp index b76f093572..22417af33b 100644 --- a/subsystem_controllers/test/test_entry_manager.cpp +++ b/subsystem_controllers/test/test_entry_manager.cpp @@ -14,149 +14,139 @@ * the License. */ -#include "entry_manager.h" #include + #include +#include "entry_manager.h" + namespace subsystem_controllers { - - TEST(EntryManagerTest, testAddNewEntry) - { - EntryManager em; - // params: bool available, bool active, std::string name, long timestamp, uint8_t type - Entry entry(true, false, "cruising_plugin", 1000, 1, ""); - em.update_entry(entry); - std::vector res = em.get_entries(); - EXPECT_EQ(1, res.size()); - EXPECT_EQ(true, res.begin()->available_); - EXPECT_EQ(false, res.begin()->active_); - EXPECT_EQ("cruising_plugin", res.begin()->name_); - EXPECT_EQ(1000, res.begin()->timestamp_); - EXPECT_EQ(1, res.begin()->type_); - } - - TEST(EntryManagerTest, testUpdateEntry) - { - EntryManager em; - // params: bool available, bool active, std::string name, long timestamp, uint8_t type - Entry entry(true, false, "cruising_plugin", 1000, 1, ""); - em.update_entry(entry); - Entry new_entry(false, true, "cruising_plugin", 2000, 3, ""); - em.update_entry(new_entry); - std::vector res = em.get_entries(); - EXPECT_EQ(1, res.size()); - EXPECT_EQ(false, res.begin()->available_); - EXPECT_EQ(true, res.begin()->active_); - EXPECT_EQ(2000, res.begin()->timestamp_); - // the following two attributes should not change once set - EXPECT_EQ("cruising_plugin", res.begin()->name_); - EXPECT_EQ(1, res.begin()->type_); - } - - TEST(EntryManagerTest, testDeleteEntry) - { - EntryManager em; - // params: bool available, bool active, std::string name, long timestamp, uint8_t type - Entry entry(true, false, "cruising_plugin", 1000, 1, ""); - em.update_entry(entry); - Entry new_entry(false, true, "cruising_plugin_2", 2000, 3, ""); - em.update_entry(new_entry); - em.delete_entry("cruising_plugin"); - std::vector res = em.get_entries(); - EXPECT_EQ(1, res.size()); - EXPECT_EQ(false, res.begin()->available_); - EXPECT_EQ(true, res.begin()->active_); - EXPECT_EQ(2000, res.begin()->timestamp_); - // the following two attributes should not change once set - EXPECT_EQ("cruising_plugin_2", res.begin()->name_); - EXPECT_EQ(3, res.begin()->type_); - } - - TEST(EntryManagerTest, testGetEntryByValidName) - { - EntryManager em; - // params: bool available, bool active, std::string name, long timestamp, uint8_t type - Entry entry(true, false, "cruising_plugin", 1000, 1, ""); - em.update_entry(entry); - boost::optional res = em.get_entry_by_name("cruising_plugin"); - EXPECT_EQ(true, res->available_); - EXPECT_EQ(false, res->active_); - EXPECT_EQ(1000, res->timestamp_); - EXPECT_EQ(1, res->type_); - EXPECT_EQ("cruising_plugin", res->name_); - } - - TEST(EntryManagerTest, testGetEntryByInvalidName) - { - EntryManager em; - // params: bool available, bool active, std::string name, long timestamp, uint8_t type - Entry entry(true, false, "cruising_plugin", 1000, 1, ""); - em.update_entry(entry); - boost::optional res = em.get_entry_by_name("plugin"); - if(!res) - { - ASSERT_TRUE(1 == 1); - } else - { - ASSERT_TRUE(1 == 2); - } - } - - TEST(EntryManagerTest, testRequiredEntryCheck) - { - std::vector required_names; - required_names.push_back("cruising"); - required_names.push_back("cruising_plugin"); - EntryManager em(required_names); - Entry entry(true, false, "cruising_plugin", 1000, 1, ""); - em.update_entry(entry); - Entry new_entry(false, true, "cruising_plugin_2", 2000, 3, ""); - em.update_entry(new_entry); - EXPECT_EQ(true, em.is_entry_required("cruising_plugin")); - EXPECT_EQ(true, em.is_entry_required("cruising")); - EXPECT_EQ(false, em.is_entry_required("cruising_plugin_2")); - } - - TEST(EntryManagerTest, testTruckLidarGpsEntryCheck) - { - std::vector required_entries; - required_entries.push_back("ssc"); - - std::vector lidar_gps_entries; - lidar_gps_entries.push_back("lidar1"); - lidar_gps_entries.push_back("lidar2"); - lidar_gps_entries.push_back("gps"); - - std::vector camera_entries; - camera_entries.push_back("camera"); - - - EntryManager em(required_entries,lidar_gps_entries,camera_entries); - - EXPECT_EQ(0, em.is_lidar_gps_entry_required("lidar1")); - EXPECT_EQ(1, em.is_lidar_gps_entry_required("lidar2")); - EXPECT_EQ(2, em.is_lidar_gps_entry_required("gps")); - EXPECT_EQ(0, em.is_camera_entry_required("camera")); - } - - TEST(EntryManagerTest, testCarLidarGpsEntryCheck) - { - std::vector required_entries; - required_entries.push_back("ssc"); - - std::vector lidar_gps_entries; - lidar_gps_entries.push_back("lidar"); - lidar_gps_entries.push_back("gps"); - - std::vector camera_entries; - camera_entries.push_back("camera"); - - EntryManager em(required_entries,lidar_gps_entries,camera_entries); - - EXPECT_EQ(0, em.is_lidar_gps_entry_required("lidar")); - EXPECT_EQ(1, em.is_lidar_gps_entry_required("gps")); - EXPECT_EQ(0, em.is_camera_entry_required("camera")); - } - +TEST(EntryManagerTest, testAddNewEntry) +{ + EntryManager em; + // params: bool available, bool active, std::string name, long timestamp, uint8_t type + Entry entry(true, false, "cruising_plugin", 1000, 1, ""); + em.update_entry(entry); + std::vector res = em.get_entries(); + EXPECT_EQ(1, res.size()); + EXPECT_EQ(true, res.begin()->available_); + EXPECT_EQ(false, res.begin()->active_); + EXPECT_EQ("cruising_plugin", res.begin()->name_); + EXPECT_EQ(1000, res.begin()->timestamp_); + EXPECT_EQ(1, res.begin()->type_); +} + +TEST(EntryManagerTest, testUpdateEntry) +{ + EntryManager em; + // params: bool available, bool active, std::string name, long timestamp, uint8_t type + Entry entry(true, false, "cruising_plugin", 1000, 1, ""); + em.update_entry(entry); + Entry new_entry(false, true, "cruising_plugin", 2000, 3, ""); + em.update_entry(new_entry); + std::vector res = em.get_entries(); + EXPECT_EQ(1, res.size()); + EXPECT_EQ(false, res.begin()->available_); + EXPECT_EQ(true, res.begin()->active_); + EXPECT_EQ(2000, res.begin()->timestamp_); + // the following two attributes should not change once set + EXPECT_EQ("cruising_plugin", res.begin()->name_); + EXPECT_EQ(1, res.begin()->type_); +} + +TEST(EntryManagerTest, testDeleteEntry) +{ + EntryManager em; + // params: bool available, bool active, std::string name, long timestamp, uint8_t type + Entry entry(true, false, "cruising_plugin", 1000, 1, ""); + em.update_entry(entry); + Entry new_entry(false, true, "cruising_plugin_2", 2000, 3, ""); + em.update_entry(new_entry); + em.delete_entry("cruising_plugin"); + std::vector res = em.get_entries(); + EXPECT_EQ(1, res.size()); + EXPECT_EQ(false, res.begin()->available_); + EXPECT_EQ(true, res.begin()->active_); + EXPECT_EQ(2000, res.begin()->timestamp_); + // the following two attributes should not change once set + EXPECT_EQ("cruising_plugin_2", res.begin()->name_); + EXPECT_EQ(3, res.begin()->type_); +} + +TEST(EntryManagerTest, testGetEntryByValidName) +{ + EntryManager em; + // params: bool available, bool active, std::string name, long timestamp, uint8_t type + Entry entry(true, false, "cruising_plugin", 1000, 1, ""); + em.update_entry(entry); + boost::optional res = em.get_entry_by_name("cruising_plugin"); + EXPECT_EQ(true, res->available_); + EXPECT_EQ(false, res->active_); + EXPECT_EQ(1000, res->timestamp_); + EXPECT_EQ(1, res->type_); + EXPECT_EQ("cruising_plugin", res->name_); +} + +TEST(EntryManagerTest, testGetEntryByInvalidName) +{ + EntryManager em; + // params: bool available, bool active, std::string name, long timestamp, uint8_t type + Entry entry(true, false, "cruising_plugin", 1000, 1, ""); + em.update_entry(entry); + boost::optional res = em.get_entry_by_name("plugin"); + if (!res) { + ASSERT_TRUE(1 == 1); + } else { + ASSERT_TRUE(1 == 2); + } } + +TEST(EntryManagerTest, testRequiredEntryCheck) +{ + std::vector required_names; + required_names.push_back("cruising"); + required_names.push_back("cruising_plugin"); + EntryManager em(required_names); + Entry entry(true, false, "cruising_plugin", 1000, 1, ""); + em.update_entry(entry); + Entry new_entry(false, true, "cruising_plugin_2", 2000, 3, ""); + em.update_entry(new_entry); + EXPECT_EQ(true, em.is_entry_required("cruising_plugin")); + EXPECT_EQ(true, em.is_entry_required("cruising")); + EXPECT_EQ(false, em.is_entry_required("cruising_plugin_2")); +} + +TEST(EntryManagerTest, testTruckLidarGpsEntryCheck) +{ + std::vector required_entries; + required_entries.push_back("ssc"); + + std::vector lidar_gps_entries; + lidar_gps_entries.push_back("lidar1"); + lidar_gps_entries.push_back("lidar2"); + lidar_gps_entries.push_back("gps"); + + EntryManager em(required_entries, lidar_gps_entries); + + EXPECT_EQ(0, em.is_lidar_gps_entry_required("lidar1")); + EXPECT_EQ(1, em.is_lidar_gps_entry_required("lidar2")); + EXPECT_EQ(2, em.is_lidar_gps_entry_required("gps")); +} + +TEST(EntryManagerTest, testCarLidarGpsEntryCheck) +{ + std::vector required_entries; + required_entries.push_back("ssc"); + + std::vector lidar_gps_entries; + lidar_gps_entries.push_back("lidar"); + lidar_gps_entries.push_back("gps"); + + EntryManager em(required_entries, lidar_gps_entries); + + EXPECT_EQ(0, em.is_lidar_gps_entry_required("lidar")); + EXPECT_EQ(1, em.is_lidar_gps_entry_required("gps")); +} + +} // namespace subsystem_controllers