add ros2 mock drivers

carma/launch/drivers.launch.py

@@ -24,6 +24,7 @@
 from launch_ros.descriptions import ComposableNode
 from carma_ros2_utils.launch.get_log_level import GetLogLevel
 from carma_ros2_utils.launch.get_current_namespace import GetCurrentNamespace
+from launch.conditions import IfCondition
 import os
 
 
@@ -89,6 +90,15 @@ def generate_launch_description():
         ]
     )
 
+    # if running in simulation with CARLA, publish driver status message automatically
+    ros2_mock_drivers = Node(
+        condition=IfCondition(use_sim_time),
+        package='ros2_mock_drivers',
+        name='carma_carla_driver_status',
+        executable='mock_driver_exec',
+        parameters=[{"use_sim_time" : use_sim_time}],
+    )
+
     # subsystem_controller which orchestrates the lifecycle of this subsystem's components
     subsystem_controller = Node(
         package='subsystem_controllers',
@@ -107,5 +117,6 @@ def generate_launch_description():
         declare_vehicle_config_param_file_arg,
         declare_use_sim_time_arg,
         lightbar_manager_container,
+        ros2_mock_drivers,
         subsystem_controller
     ])

mock_drivers/ros2_mock_drivers/CMakeLists.txt

@@ -0,0 +1,55 @@
+
+# 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.
+
+cmake_minimum_required(VERSION 3.5)
+project(ros2_mock_drivers)
+
+# Declare carma package and check ROS version
+find_package(carma_cmake_common REQUIRED)
+carma_check_ros_version(2)
+carma_package()
+
+## Find dependencies using ament auto
+find_package(ament_cmake_auto REQUIRED)
+ament_auto_find_build_dependencies()
+
+# Name build targets
+set(node_exec mock_driver_exec)
+set(node_lib mock_driver)
+
+# Includes
+include_directories(
+  include
+)
+
+# Build
+ament_auto_add_library(${node_lib} SHARED
+        src/mock_driver.cpp
+)
+
+ament_auto_add_executable(${node_exec}
+        src/main.cpp
+)
+
+# All locally created targets will need to be manually linked
+# ament auto will handle linking of external dependencies
+target_link_libraries(${node_exec}
+        ${node_lib}
+)
+
+# Install
+ament_auto_package(
+        INSTALL_TO_SHARE
+)

mock_drivers/ros2_mock_drivers/include/mock_driver.hpp

@@ -0,0 +1,47 @@
+// Copyright 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.
+
+#ifndef ROS2_MOCK_DRIVER_HPP_
+#define ROS2_MOCK_DRIVER_HPP_
+
+
+#include <memory>
+#include <rclcpp/rclcpp.hpp>
+#include <carma_driver_msgs/msg/driver_status.hpp>
+#include <carma_ros2_utils/carma_lifecycle_node.hpp>
+#include "mock_driver.hpp"
+
+namespace mock_driver{
+
+class DriverStatusPublisher : public rclcpp::Node {
+public:
+    DriverStatusPublisher(const rclcpp::NodeOptions &options) : rclcpp::Node("carla_carma_driver_status", options) {
+        init();
+    }
+
+private:
+    void init();
+    void publish_controller_driver_status();
+    void publish_camera_driver_status();
+    void publish_lidar_driver_status();
+    void publish_gnss_driver_status();
+
+    rclcpp::Publisher<carma_driver_msgs::msg::DriverStatus>::SharedPtr publisher_;
+    rclcpp::TimerBase::SharedPtr timer_;
+};
+
+
+}
+
+#endif  // ROS2_MOCK_DRIVER_HPP_

mock_drivers/ros2_mock_drivers/package.xml

@@ -0,0 +1,39 @@
+<?xml version="1.0"?>
+
+<!--
+ 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.
+-->
+
+<package format="3">
+  <name>ros2_mock_drivers</name>
+  <version>1.0.0</version>
+  <description>The ros2_mock_drivers package</description>
+
+  <maintainer email="carma@dot.gov">carma</maintainer>
+
+  <license>Apache 2.0</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+  <build_depend>carma_cmake_common</build_depend>
+  <build_depend>ament_auto_cmake</build_depend>
+
+  <depend>rclcpp</depend>
+  <depend>carma_ros2_utils</depend>
+  <depend>rclcpp_components</depend>
+  <depend>carma_driver_msgs</depend>
+  <exec_depend>launch</exec_depend>
+  <exec_depend>launch_ros</exec_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

mock_drivers/ros2_mock_drivers/src/main.cpp

@@ -0,0 +1,26 @@
+// Copyright 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.
+
+#include <rclcpp/rclcpp.hpp>
+#include "mock_driver.hpp"
+
+int main(int argc, char **argv) {
+    rclcpp::init(argc, argv);
+    rclcpp::executors::SingleThreadedExecutor executor;
+    auto node = std::make_shared<mock_driver::DriverStatusPublisher>(rclcpp::NodeOptions());
+    executor.add_node(node);
+    executor.spin();
+
+    return 0;
+}

mock_drivers/ros2_mock_drivers/src/mock_driver.cpp

@@ -0,0 +1,100 @@
+
+// Copyright 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.
+
+#include "mock_driver.hpp"
+
+namespace mock_driver
+{
+
+    void DriverStatusPublisher::init()
+    {
+        // Initialize publisher
+        RCLCPP_ERROR_STREAM(this->get_logger(), "Print1");
+        publisher_ = this->create_publisher<carma_driver_msgs::msg::DriverStatus>("/hardware_interface/driver_discovery", 10);
+
+        while (!this->get_clock()->ros_time_is_active())
+        {
+            RCLCPP_ERROR_STREAM(this->get_logger(), "Waiting for ROS time to be available");
+            rclcpp::sleep_for(std::chrono::seconds(1));
+        }
+        // Initialize parameters
+        this->declare_parameter("driver_status_pub_rate", 10);
+
+        RCLCPP_ERROR_STREAM(this->get_logger(), "Print2");
+
+        // Create timer
+        RCLCPP_ERROR_STREAM(this->get_logger(), "Print3");
+
+        int pub_rate = this->get_parameter("driver_status_pub_rate").as_int();
+        timer_ = rclcpp::create_timer(this,
+            get_clock(),
+            rclcpp::Duration(1000 / pub_rate * 1e6), [this]() -> void {
+            this->publish_controller_driver_status();
+            this->publish_lidar_driver_status();
+            this->publish_camera_driver_status();
+            this->publish_gnss_driver_status();
+        });
+        RCLCPP_ERROR_STREAM(this->get_logger(), "Print4");
+    }
+     void DriverStatusPublisher::publish_controller_driver_status() {
+        carma_driver_msgs::msg::DriverStatus message;
+        RCLCPP_ERROR_STREAM(this->get_logger(), "Print5");
+
+        // Set other message fields
+        message.name = "/hardware_interface/carla_driver";
+        message.status = carma_driver_msgs::msg::DriverStatus::OPERATIONAL;
+        message.controller = true;
+
+        // Publish message
+        publisher_->publish(message);
+        RCLCPP_ERROR_STREAM(this->get_logger(), "Print6");
+
+    }
+
+    void DriverStatusPublisher::publish_camera_driver_status() {
+        carma_driver_msgs::msg::DriverStatus message;
+
+        // Set other message fields
+        message.name = "/hardware_interface/carla_camera_driver";
+        message.status = carma_driver_msgs::msg::DriverStatus::OPERATIONAL;
+        message.camera = true;
+
+        // Publish message
+        publisher_->publish(message);
+    }
+
+    void DriverStatusPublisher::publish_lidar_driver_status() {
+        carma_driver_msgs::msg::DriverStatus message;
+
+        // Set other message fields
+        message.name = "/hardware_interface/carla_lidar_driver";
+        message.status = carma_driver_msgs::msg::DriverStatus::OPERATIONAL;
+        message.lidar = true;
+        // Publish message
+        publisher_->publish(message);
+    }
+
+    void DriverStatusPublisher::publish_gnss_driver_status() {
+        carma_driver_msgs::msg::DriverStatus message;
+
+        // Set other message fields
+        message.name = "/hardware_interface/carla_gnss_driver";
+        message.status = carma_driver_msgs::msg::DriverStatus::OPERATIONAL;
+        message.gnss = true;
+
+        // Publish message
+        publisher_->publish(message);
+    }
+}

subsystem_controllers/src/base_subsystem_controller/base_subsystem_controller.cpp

@@ -25,7 +25,7 @@ using std_msec = std::chrono::milliseconds;
 namespace subsystem_controllers
 {
   BaseSubsystemController::BaseSubsystemController(const rclcpp::NodeOptions &options)
-      : CarmaLifecycleNode(options), 
+      : CarmaLifecycleNode(options),
       lifecycle_mgr_(get_node_base_interface(), get_node_graph_interface(), get_node_logging_interface(), get_node_services_interface())
   {
     // Declare parameters
@@ -35,7 +35,7 @@ namespace subsystem_controllers
     base_config_.subsystem_namespace = this->declare_parameter<std::string>("subsystem_namespace", base_config_.subsystem_namespace);
     base_config_.full_subsystem_required = this->declare_parameter<bool>("full_subsystem_required", base_config_.full_subsystem_required);
     base_config_.unmanaged_required_nodes = this->declare_parameter<std::vector<std::string>>("unmanaged_required_nodes", base_config_.unmanaged_required_nodes);
-  
+
     // Handle fact that parameter vectors cannot be empty
     if (base_config_.required_subsystem_nodes.size() == 1 && base_config_.required_subsystem_nodes[0].empty()) {
       base_config_.required_subsystem_nodes.clear();
@@ -78,7 +78,7 @@ namespace subsystem_controllers
         alert.source_node =  get_node_base_interface()->get_fully_qualified_name();
         publish_system_alert(alert);
 
-        // TODO: It might be worth trying to deactivate or shutdown after alerting the larger system, 
+        // TODO: It might be worth trying to deactivate or shutdown after alerting the larger system,
         //       but not clear on if that will increase instability of shutdown process
       }
       else
@@ -93,7 +93,7 @@ namespace subsystem_controllers
   {
     RCLCPP_INFO_STREAM(get_logger(), "Subsystem trying to configure");
 
-    // Reset config 
+    // Reset config
     base_config_ = BaseSubSystemControllerConfig();
 
     // Load Parameters
@@ -141,7 +141,7 @@ namespace subsystem_controllers
 
       RCLCPP_INFO_STREAM(get_logger(), "New config: " << base_config_);
     }
-    
+
 
     if (!trigger_managed_nodes_configure_from_base_class_) {
       return CallbackReturn::SUCCESS;
@@ -279,7 +279,7 @@ namespace subsystem_controllers
   std::vector<std::string> BaseSubsystemController::get_nodes_in_namespace(const std::string &node_namespace) const
   {
 
-    // These two services are exposed by lifecycle nodes. 
+    // These two services are exposed by lifecycle nodes.
     static const std::string CHANGE_STATE_SRV = "/change_state";
     static const std::string GET_STATE_SRV = "/get_state";
 
@@ -307,8 +307,8 @@ namespace subsystem_controllers
 
         ////
         // In the following section of code we check if the current node in the target namespace is actually a lifecycle node
-        // This check is done by evaluating if that nodes exposes the change_state and get_state lifecycle services. 
-        // If the node does not expose these services then it cannot be managed by this component. 
+        // This check is done by evaluating if that nodes exposes the change_state and get_state lifecycle services.
+        // If the node does not expose these services then it cannot be managed by this component.
         // However, this does not result in an error as the node could be wrapped by a lifecycle component wrapper
         ////
 
@@ -332,7 +332,7 @@ namespace subsystem_controllers
         const std::string cs_srv = node + CHANGE_STATE_SRV;
         const std::string gs_srv = node + GET_STATE_SRV;
 
-        if (services_and_types.find(cs_srv) != services_and_types.end() 
+        if (services_and_types.find(cs_srv) != services_and_types.end()
           && services_and_types.find(gs_srv) != services_and_types.end()
           && std::find(services_and_types.at(cs_srv).begin(), services_and_types.at(cs_srv).end(), CHANGE_STATE_TYPE) != services_and_types.at(cs_srv).end()
           && std::find(services_and_types.at(gs_srv).begin(), services_and_types.at(gs_srv).end(), GET_STATE_TYPE) != services_and_types.at(gs_srv).end())
@@ -343,7 +343,7 @@ namespace subsystem_controllers
         } else {
           // Current node is not a lifecycle node so log a warning
           RCLCPP_WARN_STREAM(get_logger(), "Failed to find lifecycle services for node: " << node << " this node will not be managed. NOTE: If this node is wrapped by a lifecycle component that is managed then this is not an issue.");
-          
+
           continue;
 
         }