Handle only ROS1 SSC in drivers and remove camera related logics (#2444)

* buildable without carma references

* specialize in ssc

* typo

* address PR changes

* reduce nesting and reuse code

* rename to ros1_ssc_driver_timeout

subsystem_controllers/CMakeLists.txt

@@ -57,8 +57,7 @@ target_link_libraries(environment_perception_controller environment_perception_c
 # Driver Subsystem
 ament_auto_add_library(drivers_controller_core SHARED
   src/drivers_controller/drivers_controller_node.cpp
-  src/drivers_controller/entry_manager.cpp
-  src/drivers_controller/driver_manager.cpp
+  src/drivers_controller/ssc_driver_manager.cpp
   )
 rclcpp_components_register_nodes(drivers_controller_core "subsystem_controllers::DriversControllerNode")
 target_link_libraries(drivers_controller_core ${base_lib})

subsystem_controllers/config/drivers_controller_config.yaml

@@ -2,37 +2,34 @@
   ros__parameters:
     # Long: Timeout for each service to be detected as available in milliseconds
     # Units: milliseconds
-    service_timeout_ms : 200
+    service_timeout_ms: 200
 
     # Long: Timeout for each service call in milliseconds
     # Units: milliseconds
-    call_timeout_ms : 1000
+    call_timeout_ms: 1000
 
     # String: The namespace for nodes in this subsystem. All nodes under this namespace will have their lifecycle's managed by this controller
     subsystem_namespace: /hardware_interface
 
     # Required subsystem controller nodes for the overall system to be functional and will have their lifecycle's managed
-    required_subsystem_nodes: ['']
+    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: ['']
+    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: ['']
-
+    excluded_namespace_nodes: [""]
+
+    # ros1 controller driver (node name) to consider required and whose failure shall result in automation abort
+    ros1_ssc_driver_name: ""
+
     # 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
 
     # Int: The time allocated for system startup in seconds
     startup_duration: 30
 
     # Double: The timeout threshold for essential drivers in ms
-    required_driver_timeout: 3000.0
+    ros1_ssc_driver_timeout : 3000.0

subsystem_controllers/include/subsystem_controllers/drivers_controller/drivers_controller_config.hpp

@@ -21,51 +21,38 @@
 
 namespace subsystem_controllers
 {
- /**
-  * \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<std::string> ros1_required_drivers_;
-    //! List of ros1 camera drivers (node name) to consider required and who's failure shall result in automation abort.
-    std::vector<std::string> ros1_camera_drivers_;
-    //! List of nodes in the namespace which will not be managed by this subsystem controller
-    std::vector<std::string> 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)
-    {
-
-      output << "DriversControllerConfig { " << std::endl
-             << "ros1_required_drivers: [ " << std::endl;
-
-      for (auto node : c.ros1_required_drivers_)
-        output << node << " ";
+/**
+ * \brief Stuct containing the algorithm configuration values for the GuidanceController
+ */
+struct DriversControllerConfig
 
-      output << "] " << std::endl << "ros1_camera_drivers: [ ";
+{
+  //! ros1 controller driver (node name) to consider required and whose failure shall
+  //! result in automation abort.
+  std::string ros1_ssc_driver_name_;
+  //! List of nodes in the namespace which will not be managed by this subsystem controller
+  std::vector<std::string> 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)
+  {
+    output << "DriversControllerConfig { " << std::endl
+           << "ros1_ssc_driver_name: " << c.ros1_ssc_driver_name_ << std::endl
+           << "excluded_namespace_nodes: [  " << std::endl;
 
-      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

subsystem_controllers/include/subsystem_controllers/drivers_controller/drivers_controller_node.hpp

@@ -16,71 +16,63 @@
  * the License.
  */
 
-
-
 #include <memory>
 
-#include "carma_msgs/msg/system_alert.hpp"
-#include "ros2_lifecycle_manager/ros2_lifecycle_manager.hpp"
+#include "drivers_controller_config.hpp"
 #include "rclcpp/rclcpp.hpp"
-#include <carma_driver_msgs/msg/driver_status.hpp>
+#include "ros2_lifecycle_manager/ros2_lifecycle_manager.hpp"
+#include "ssc_driver_manager.hpp"
 #include "subsystem_controllers/base_subsystem_controller/base_subsystem_controller.hpp"
-#include "drivers_controller_config.hpp"
-#include "driver_manager.hpp"
 #include <boost/algorithm/string.hpp>
 
+#include "carma_msgs/msg/system_alert.hpp"
+#include <carma_driver_msgs/msg/driver_status.hpp>
+
 namespace subsystem_controllers
 {
+class DriversControllerNode : public BaseSubsystemController
+{
+public:
+  DriversControllerNode() = delete;
 
-  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);
-
-
-  private:
-
-    // DriverManager to handle all the driver specific discovery and reporting
-    std::shared_ptr<DriverManager> driver_manager_;
+  ~DriversControllerNode() = default;
 
-    //! Config for user provided parameters
-    DriversControllerConfig config_;
+  /**
+   * \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);
 
-    //! ROS handles
-    carma_ros2_utils::SubPtr<carma_driver_msgs::msg::DriverStatus> driver_status_sub_;
+private:
+  // SSCDriverManager to handle all the driver specific discovery and reporting
+  std::shared_ptr<SSCDriverManager> ssc_driver_manager_;
 
-    // message/service callbacks
-    void driver_discovery_cb(const carma_driver_msgs::msg::DriverStatus::SharedPtr msg);
+  //! Config for user provided parameters
+  DriversControllerConfig config_;
 
-    //! Timer callback function to check status of required ros1 drivers 
-    void timer_callback();
+  //! ROS handles
+  carma_ros2_utils::SubPtr<carma_driver_msgs::msg::DriverStatus> driver_status_sub_;
 
-    carma_ros2_utils::CallbackReturn handle_on_configure(const rclcpp_lifecycle::State &prev_state);
-    carma_ros2_utils::CallbackReturn handle_on_activate(const rclcpp_lifecycle::State &prev_state);
+  // message/service callbacks
+  void driver_discovery_cb(const carma_driver_msgs::msg::DriverStatus::SharedPtr msg);
 
-    //! ROS parameters
-    std::vector<std::string> ros1_required_drivers_;
-    std::vector<std::string> ros1_camera_drivers_;
+  //! Timer callback function to check status of required ros1 drivers
+  void critical_drivers_check_callback();
 
-    // record of startup timestamp
-    long start_up_timestamp_;
+  carma_ros2_utils::CallbackReturn handle_on_configure(const rclcpp_lifecycle::State & prev_state);
+  carma_ros2_utils::CallbackReturn handle_on_activate(const rclcpp_lifecycle::State & prev_state);
 
-    rclcpp::TimerBase::SharedPtr timer_;
+  //! ROS parameters
+  std::vector<std::string> ros1_ssc_driver_name_;
 
-    // Previously published alert message
-    boost::optional<carma_msgs::msg::SystemAlert> prev_alert;
+  // record of startup timestamp
+  long start_up_timestamp_;
 
-  };
+  rclcpp::TimerBase::SharedPtr ssc_status_check_timer_;
 
-} // namespace v2x_controller
+  // Previously published alert message
+  boost::optional<carma_msgs::msg::SystemAlert> prev_alert;
+};
 
+}  // namespace subsystem_controllers