Merge branch 'tier4/proposal' into 1-add-rviz-plugin-packages

simulator/fault_injection/CMakeLists.txt

@@ -0,0 +1,62 @@
+cmake_minimum_required(VERSION 3.5)
+project(fault_injection)
+
+## Compile options
+if(NOT CMAKE_CXX_STANDARD)
+  set(CMAKE_CXX_STANDARD 17)
+  set(CMAKE_CXX_STANDARD_REQUIRED ON)
+  set(CMAKE_CXX_EXTENSIONS OFF)
+endif()
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic -Werror)
+endif()
+
+## Dependencies
+find_package(ament_cmake_auto REQUIRED)
+ament_auto_find_build_dependencies()
+
+## Targets
+ament_auto_add_library(fault_injection_node_component SHARED
+  src/fault_injection_node/fault_injection_node.cpp
+)
+
+rclcpp_components_register_node(fault_injection_node_component
+  PLUGIN "fault_injection::FaultInjectionNode"
+  EXECUTABLE fault_injection_node
+)
+
+## Tests
+if(BUILD_TESTING)
+  # Linter
+  find_package(ament_lint_auto REQUIRED)
+  ament_lint_auto_find_test_dependencies()
+
+  # gtest
+  ament_add_gtest(test_fault_injection_node_component
+    test/src/main.cpp
+    test/src/test_diagnostic_storage.cpp
+  )
+
+  target_link_libraries(test_fault_injection_node_component
+    fault_injection_node_component
+  )
+
+  # TODO: Comment out until the error in CI is resolved.
+  # launch_testing
+  # find_package(launch_testing_ament_cmake REQUIRED)
+  # add_launch_test(test/test_fault_injection_node.test.py)
+
+  # install(DIRECTORY
+  #   test/config
+  #   test/launch
+  #   DESTINATION share/${PROJECT_NAME}/
+  # )
+endif()
+
+## Package
+ament_auto_package(
+  INSTALL_TO_SHARE
+    config
+    launch
+)

simulator/fault_injection/README.md

@@ -0,0 +1,46 @@
+# fault_injection
+
+## Purpose
+
+This package is used to convert pseudo system faults from PSim to Diagnostics and notify Autoware.
+The component diagram is as follows:
+
+![Component diagram of fault_injection package](img/component.drawio.svg)
+
+## Test
+
+```bash
+source install/setup.bash
+cd fault_injection
+launch_test test/test_fault_injection_node.test.py
+```
+
+## Inner-workings / Algorithms
+
+## Inputs / Outputs
+
+### Input
+
+| Name                        | Type                                              | Description       |
+| --------------------------- | ------------------------------------------------- | ----------------- |
+| `~/input/simulation_events` | `autoware_simulation_msgs::msg::SimulationEvents` | simulation events |
+
+### Output
+
+None.
+
+## Parameters
+
+None.
+
+### Node Parameters
+
+None.
+
+### Core Parameters
+
+None.
+
+## Assumptions / Known limits
+
+TBD.

simulator/fault_injection/config/fault_injection.param.yaml

@@ -0,0 +1,37 @@
+/**:
+  ros__parameters:
+    event_diag_list:
+      vehicle_is_out_of_lane: "lane_departure"
+      trajectory_deviation_is_high: "trajectory_deviation"
+      localization_matching_score_is_low: "ndt_scan_matcher"
+      localization_accuracy_is_low: "localization_accuracy"
+      map_version_is_different: "map_version"
+      trajectory_is_invalid: "trajectory_point_validation"
+      cpu_temperature_is_high: "CPU Temperature"
+      cpu_usage_is_high: "CPU Usage"
+      cpu_is_in_thermal_throttling: "CPU Thermal Throttling"
+      storage_temperature_is_high: "HDD Temperature"
+      storage_usage_is_high: "HDD Usage"
+      network_usage_is_high: "Network Usage"
+      clock_error_is_large: "NTP Offset"
+      gpu_temperature_is_high: "GPU Temperature"
+      gpu_usage_is_high: "GPU Usage"
+      gpu_memory_usage_is_high: "GPU Memory Usage"
+      gpu_is_in_thermal_throttling: "GPU Thermal Throttling"
+      driving_recorder_storage_error: "driving_recorder"
+      debug_data_logger_storage_error: "bagpacker"
+      emergency_stop_operation: "emergency_stop_operation"
+      vehicle_error_occurred: "vehicle_errors"
+      vehicle_ecu_connection_is_lost: "can_bus_connection"
+      obstacle_crash_sensor_is_activated: "obstacle_crash"
+      /control/command_gate/node_alive_monitoring: "vehicle_cmd_gate: heartbeat"
+      /control/autonomous_driving/node_alive_monitoring: "control_topic_status"
+      /control/external_command_selector/node_alive_monitoring: "external_cmd_selector: heartbeat"
+      /localization/node_alive_monitoring: "localization_topic_status"
+      /map/node_alive_monitoring: "map_topic_status"
+      /planning/node_alive_monitoring: "planning_topic_status"
+      /sensing/lidar/node_alive_monitoring: "lidar_topic_status"
+      /sensing/imu/node_alive_monitoring: "imu_connection"
+      /sensing/gnss/node_alive_monitoring: "gnss_connection"
+      /system/node_alive_monitoring: "system_topic_status"
+      /vehicle/node_alive_monitoring: "vehicle_topic_status"

simulator/fault_injection/include/fault_injection/diagnostic_storage.hpp

@@ -0,0 +1,70 @@
+// Copyright 2021 Tier IV, Inc.
+//
+// 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 FAULT_INJECTION__DIAGNOSTIC_STORAGE_HPP__
+#define FAULT_INJECTION__DIAGNOSTIC_STORAGE_HPP__
+
+#include <diagnostic_aggregator/status_item.hpp>
+
+#include <diagnostic_msgs/msg/diagnostic_status.hpp>
+
+#include <string>
+#include <unordered_map>
+
+namespace fault_injection
+{
+using diagnostic_msgs::msg::DiagnosticStatus;
+
+struct DiagConfig
+{
+  std::string sim_name;   // Event name used in the simulation
+  std::string diag_name;  // Name of the diagnostic
+};
+
+class DiagnosticStorage
+{
+public:
+  void registerEvent(const DiagConfig & diag_config)
+  {
+    DiagnosticStatus status;
+    status.name = diag_config.diag_name;
+    status.hardware_id = "";
+    status.level = DiagnosticStatus::OK;
+    status.message = "OK";
+    event_diag_map_[diag_config.sim_name] = status;
+  }
+
+  bool isEventRegistered(const std::string & event_name)
+  {
+    return event_diag_map_.find(event_name) != event_diag_map_.end();
+  }
+
+  void updateLevel(const std::string & event_name, const int8_t level)
+  {
+    event_diag_map_.at(event_name).level = diagnostic_aggregator::valToLevel(level);
+    event_diag_map_.at(event_name).message = diagnostic_aggregator::valToMsg(level);
+  }
+
+  DiagnosticStatus getDiag(const std::string & event_name)
+  {
+    return event_diag_map_.at(event_name);
+  }
+
+private:
+  std::unordered_map<std::string, DiagnosticStatus> event_diag_map_;
+};
+
+}  // namespace fault_injection
+
+#endif  // FAULT_INJECTION__DIAGNOSTIC_STORAGE_HPP__

simulator/fault_injection/include/fault_injection/fault_injection_node.hpp

@@ -0,0 +1,62 @@
+// Copyright 2021 Tier IV, Inc.
+//
+// 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 FAULT_INJECTION__FAULT_INJECTION_NODE_HPP__
+#define FAULT_INJECTION__FAULT_INJECTION_NODE_HPP__
+
+#include "fault_injection/diagnostic_storage.hpp"
+
+#include <diagnostic_updater/diagnostic_updater.hpp>
+#include <rclcpp/rclcpp.hpp>
+
+#include <autoware_simulation_msgs/msg/simulation_events.hpp>
+
+#include <string>
+#include <vector>
+
+namespace fault_injection
+{
+using autoware_simulation_msgs::msg::SimulationEvents;
+
+class FaultInjectionNode : public rclcpp::Node
+{
+public:
+  explicit FaultInjectionNode(rclcpp::NodeOptions node_options);
+
+private:
+  // Subscribers
+  void onSimulationEvents(const SimulationEvents::ConstSharedPtr msg);
+  rclcpp::Subscription<SimulationEvents>::SharedPtr sub_simulation_events_;
+
+  // Parameter Server
+  rcl_interfaces::msg::SetParametersResult onSetParam(
+    const std::vector<rclcpp::Parameter> & params);
+  OnSetParametersCallbackHandle::SharedPtr set_param_res_;
+
+  void updateEventDiag(
+    diagnostic_updater::DiagnosticStatusWrapper & wrap, const std::string & event_name);
+
+  void addDiag(
+    const diagnostic_msgs::msg::DiagnosticStatus & status, diagnostic_updater::Updater & updater);
+
+  std::vector<DiagConfig> readEventDiagList();
+
+  diagnostic_updater::Updater updater_;
+
+  DiagnosticStorage diagnostic_storage_;
+};
+
+}  // namespace fault_injection
+
+#endif  // FAULT_INJECTION__FAULT_INJECTION_NODE_HPP__

simulator/fault_injection/launch/fault_injection.launch.xml

@@ -0,0 +1,10 @@
+<launch>
+  <arg name="input/simulation_events" default="/simulation/events"/>
+  <arg name="config_file" default="$(find-pkg-share fault_injection)/config/fault_injection.param.yaml" />
+  <arg name="log-level" default="info"/>
+
+  <node pkg="fault_injection" exec="fault_injection_node" name="fault_injection" output="screen" args="--ros-args --log-level fault_injection:=$(var log-level)">
+    <remap from="~/input/simulation_events" to="$(var input/simulation_events)"/>
+    <param from="$(var config_file)" />
+  </node>
+</launch>

simulator/fault_injection/package.xml

@@ -0,0 +1,32 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>fault_injection</name>
+  <version>0.0.1</version>
+  <description>fault_injection</description>
+  <maintainer email="keisuke.shima@tier4.jp">Keisuke Shima</maintainer>
+  <license>Apache License 2.0</license>
+
+  <buildtool_depend>ament_cmake_auto</buildtool_depend>
+
+  <depend>autoware_simulation_msgs</depend>
+  <depend>autoware_utils</depend>
+  <depend>diagnostic_aggregator</depend>
+  <depend>diagnostic_msgs</depend>
+  <depend>diagnostic_updater</depend>
+  <depend>rclcpp</depend>
+  <depend>rclcpp_components</depend>
+
+  <test_depend>ament_cmake_gtest</test_depend>
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>autoware_lint_common</test_depend>
+  <test_depend>launch</test_depend>
+  <test_depend>launch_ros</test_depend>
+  <test_depend>launch_testing</test_depend>
+  <test_depend>launch_testing_ament_cmake</test_depend>
+  <test_depend>launch_testing_ros</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>