From 26c537b4b2455db7271860e00520c07df6cd4fbb Mon Sep 17 00:00:00 2001 From: veqcc Date: Fri, 15 Dec 2023 00:19:16 +0900 Subject: [PATCH] add redundant_autoware_state_checker node Signed-off-by: veqcc --- .../launch/system.launch.xml | 1 + .../CMakeLists.txt | 14 + .../README.md | 1 + ...edundant_autoware_state_checker.param.yaml | 5 + ...edundant_autoware_state_checker.launch.xml | 6 + .../package.xml | 23 ++ .../src/redundant_autoware_state_checker.cpp | 309 ++++++++++++++++++ .../src/redundant_autoware_state_checker.hpp | 114 +++++++ 8 files changed, 473 insertions(+) create mode 100644 system/redundant_autoware_state_checker/CMakeLists.txt create mode 100644 system/redundant_autoware_state_checker/README.md create mode 100644 system/redundant_autoware_state_checker/config/redundant_autoware_state_checker.param.yaml create mode 100644 system/redundant_autoware_state_checker/launch/redundant_autoware_state_checker.launch.xml create mode 100644 system/redundant_autoware_state_checker/package.xml create mode 100644 system/redundant_autoware_state_checker/src/redundant_autoware_state_checker.cpp create mode 100644 system/redundant_autoware_state_checker/src/redundant_autoware_state_checker.hpp diff --git a/launch/tier4_system_launch/launch/system.launch.xml b/launch/tier4_system_launch/launch/system.launch.xml index 016d613cfa72d..baad5b207b17b 100644 --- a/launch/tier4_system_launch/launch/system.launch.xml +++ b/launch/tier4_system_launch/launch/system.launch.xml @@ -18,6 +18,7 @@ + diff --git a/system/redundant_autoware_state_checker/CMakeLists.txt b/system/redundant_autoware_state_checker/CMakeLists.txt new file mode 100644 index 0000000000000..02da221690b7c --- /dev/null +++ b/system/redundant_autoware_state_checker/CMakeLists.txt @@ -0,0 +1,14 @@ +cmake_minimum_required(VERSION 3.14) +project(redundant_autoware_state_checker) + +find_package(autoware_cmake REQUIRED) +autoware_package() + +ament_auto_add_executable(redundant_autoware_state_checker + src/redundant_autoware_state_checker.cpp +) + +ament_auto_package(INSTALL_TO_SHARE + launch + config +) \ No newline at end of file diff --git a/system/redundant_autoware_state_checker/README.md b/system/redundant_autoware_state_checker/README.md new file mode 100644 index 0000000000000..76b5f4ec2cc07 --- /dev/null +++ b/system/redundant_autoware_state_checker/README.md @@ -0,0 +1 @@ +# redundant_autoware_state_checker \ No newline at end of file diff --git a/system/redundant_autoware_state_checker/config/redundant_autoware_state_checker.param.yaml b/system/redundant_autoware_state_checker/config/redundant_autoware_state_checker.param.yaml new file mode 100644 index 0000000000000..89f82f1024545 --- /dev/null +++ b/system/redundant_autoware_state_checker/config/redundant_autoware_state_checker.param.yaml @@ -0,0 +1,5 @@ +/**: + ros__parameters: + states_equality_timeout: 1.0 + update_rate_hz: 10.0 + pose_distance_threshold: 1.0 \ No newline at end of file diff --git a/system/redundant_autoware_state_checker/launch/redundant_autoware_state_checker.launch.xml b/system/redundant_autoware_state_checker/launch/redundant_autoware_state_checker.launch.xml new file mode 100644 index 0000000000000..dcf53dfb8590b --- /dev/null +++ b/system/redundant_autoware_state_checker/launch/redundant_autoware_state_checker.launch.xml @@ -0,0 +1,6 @@ + + + + + + \ No newline at end of file diff --git a/system/redundant_autoware_state_checker/package.xml b/system/redundant_autoware_state_checker/package.xml new file mode 100644 index 0000000000000..0947e862175de --- /dev/null +++ b/system/redundant_autoware_state_checker/package.xml @@ -0,0 +1,23 @@ + + + + redundant_autoware_state_checker + 0.1.0 + The redundant_autoware_state_checker ROS 2 package + Ryuta Kambe + Apache License 2.0 + + ament_cmake_auto + autoware_cmake + + autoware_adapi_v1_msgs + diagnostic_msgs + rclcpp + + ament_lint_auto + autoware_lint_common + + + ament_cmake + + \ No newline at end of file diff --git a/system/redundant_autoware_state_checker/src/redundant_autoware_state_checker.cpp b/system/redundant_autoware_state_checker/src/redundant_autoware_state_checker.cpp new file mode 100644 index 0000000000000..6e37a504435b2 --- /dev/null +++ b/system/redundant_autoware_state_checker/src/redundant_autoware_state_checker.cpp @@ -0,0 +1,309 @@ +// Copyright 2023 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. + +#include "redundant_autoware_state_checker.hpp" + +namespace redundant_autoware_state_checker +{ + +RedundantAutowareStateChecker::RedundantAutowareStateChecker() : Node("RedundantAutowareStateChecker") +{ + using std::placeholders::_1; + + // Params + states_equality_timeout = + this->declare_parameter("states_equality_timeout", 1.0); // 1.0s + update_rate_hz = this->declare_parameter("update_rate_hz", 10.0); // 10hz + pose_distance_threshold = this->declare_parameter("pose_distance_threshold", 1.0); // 1.0m + + // Publishers + pub_diagnostic = create_publisher( + "/diagnostics/supervisor", rclcpp::QoS(1)); + + // Subscribers + sub_overall_mrm_state = create_subscription( + "/supervisor/fail_safe/overall/mrm_state", rclcpp::QoS(1), + std::bind(&RedundantAutowareStateChecker::on_mrm_state, this, _1)); + sub_main_pose_with_covariance = create_subscription( + "/main/localization/pose_with_covariance", rclcpp::QoS(1), + std::bind(&RedundantAutowareStateChecker::on_main_pose_with_covariance, this, _1)); + sub_sub_pose_with_covariance = create_subscription( + "/sub/localization/pose_with_covariance", rclcpp::QoS(1), + std::bind(&RedundantAutowareStateChecker::on_sub_pose_with_covariance, this, _1)); + sub_main_operation_mode_state = create_subscription( + "/main/api/operation_mode/state", rclcpp::QoS(1), + std::bind(&RedundantAutowareStateChecker::on_main_operation_mode_state, this, _1)); + sub_sub_operation_mode_state = create_subscription( + "/sub/api/operation_mode/state", rclcpp::QoS(1), + std::bind(&RedundantAutowareStateChecker::on_sub_operation_mode_state, this, _1)); + sub_main_localization_initialization_state = create_subscription( + "/main/api/localization/initialization_state", rclcpp::QoS(1), + std::bind(&RedundantAutowareStateChecker::on_main_localization_initialization_state, this, _1)); + sub_sub_localization_initialization_state = create_subscription( + "/sub/api/localization/initialization_state", rclcpp::QoS(1), + std::bind(&RedundantAutowareStateChecker::on_sub_localization_initialization_state, this, _1)); + sub_main_route_state = create_subscription( + "/main/api/routing/state", rclcpp::QoS(1), + std::bind(&RedundantAutowareStateChecker::on_main_route_state, this, _1)); + sub_sub_route_state = create_subscription( + "/sub/api/routing/state", rclcpp::QoS(1), + std::bind(&RedundantAutowareStateChecker::on_sub_route_state, this, _1)); + sub_main_route = create_subscription( + "/main/api/routing/route", rclcpp::QoS(1), + std::bind(&RedundantAutowareStateChecker::on_main_route, this, _1)); + sub_sub_route = create_subscription( + "/sub/api/routing/route", rclcpp::QoS(1), + std::bind(&RedundantAutowareStateChecker::on_sub_route, this, _1)); + + // Timer + const auto period_ns = rclcpp::Rate(update_rate_hz).period(); + timer_ = rclcpp::create_timer(this, get_clock(), period_ns, std::bind(&RedundantAutowareStateChecker::onTimer, this)); + + // Variables + is_autonomous = true; + last_time_pose_with_covariance_is_equal = this->now(); + last_time_operation_mode_state_is_equal = this->now(); + last_time_localization_initialization_state_is_equal = this->now(); + last_time_route_state_is_equal = this->now(); + last_time_route_is_equal = this->now(); +} + +void RedundantAutowareStateChecker::on_mrm_state( + const autoware_adapi_v1_msgs::msg::MrmState::ConstSharedPtr msg) +{ + is_autonomous = ( + msg->state == autoware_adapi_v1_msgs::msg::MrmState::NORMAL && + msg->behavior == autoware_adapi_v1_msgs::msg::MrmState::NONE); +} + +void RedundantAutowareStateChecker::on_main_pose_with_covariance( + const geometry_msgs::msg::PoseWithCovarianceStamped::ConstSharedPtr msg) +{ + has_subscribed_main_pose_with_covariance = true; + main_pose_with_covariance = msg; +} + +void RedundantAutowareStateChecker::on_sub_pose_with_covariance( + const geometry_msgs::msg::PoseWithCovarianceStamped::ConstSharedPtr msg) +{ + has_subscribed_sub_pose_with_covariance = true; + sub_pose_with_covariance = msg; +} + +void RedundantAutowareStateChecker::on_main_operation_mode_state( + const autoware_adapi_v1_msgs::msg::OperationModeState::ConstSharedPtr msg) +{ + has_subscribed_main_operation_mode_state = true; + main_operation_mode_state = msg; +} + +void RedundantAutowareStateChecker::on_sub_operation_mode_state( + const autoware_adapi_v1_msgs::msg::OperationModeState::ConstSharedPtr msg) +{ + has_subscribed_sub_operation_mode_state = true; + sub_operation_mode_state = msg; +} + +void RedundantAutowareStateChecker::on_main_localization_initialization_state( + const autoware_adapi_v1_msgs::msg::LocalizationInitializationState::ConstSharedPtr msg) +{ + has_subscribed_main_localization_initialization_state = true; + main_localization_initialization_state = msg; +} + +void RedundantAutowareStateChecker::on_sub_localization_initialization_state( + const autoware_adapi_v1_msgs::msg::LocalizationInitializationState::ConstSharedPtr msg) +{ + has_subscribed_sub_localization_initialization_state = true; + sub_localization_initialization_state = msg; +} + +void RedundantAutowareStateChecker::on_main_route_state( + const autoware_adapi_v1_msgs::msg::RouteState::ConstSharedPtr msg) +{ + has_subscribed_main_route_state = true; + main_route_state = msg; +} + +void RedundantAutowareStateChecker::on_sub_route_state( + const autoware_adapi_v1_msgs::msg::RouteState::ConstSharedPtr msg) +{ + has_subscribed_sub_route_state = true; + sub_route_state = msg; +} + +void RedundantAutowareStateChecker::on_main_route( + const autoware_adapi_v1_msgs::msg::Route::ConstSharedPtr msg) +{ + has_subscribed_main_route = true; + main_route = msg; +} + +void RedundantAutowareStateChecker::on_sub_route( + const autoware_adapi_v1_msgs::msg::Route::ConstSharedPtr msg) +{ + has_subscribed_sub_route = true; + sub_route = msg; +} + +void RedundantAutowareStateChecker::onTimer() +{ + if (!is_autonomous) return; + + const auto now = this->now(); + + diagnostic_msgs::msg::DiagnosticArray diag_msg; + diag_msg.header.stamp = now; + + diagnostic_msgs::msg::DiagnosticStatus diag_status_msg; + diag_status_msg.name = "redundant_autoware_state_checker"; + diag_status_msg.hardware_id = ""; + diag_status_msg.level = diagnostic_msgs::msg::DiagnosticStatus::OK; + diag_status_msg.message = ""; + + // Check the pose with covariance equality + // Note that only this check raises WARN, not ERROR + if (is_equal_pose_with_covariance()) { + last_time_pose_with_covariance_is_equal = now; + } else { + if ((now - last_time_pose_with_covariance_is_equal).seconds() > states_equality_timeout) { + diag_status_msg.level = diagnostic_msgs::msg::DiagnosticStatus::WARN; + diag_status_msg.message += "Main and Sub ECUs' pose with covariance are different."; + } + } + + // Check the operation mode state equality + if (is_equal_operation_mode_stete()) { + last_time_operation_mode_state_is_equal = now; + } else { + if ((now - last_time_operation_mode_state_is_equal).seconds() > states_equality_timeout) { + diag_status_msg.level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; + diag_status_msg.message += "Main and Sub ECUs' operation mode states are different."; + } + } + + // Check the localization initialization state equality + if (is_equal_localization_initialization_state()) { + last_time_localization_initialization_state_is_equal = now; + } else { + if ((now - last_time_localization_initialization_state_is_equal).seconds() > states_equality_timeout) { + diag_status_msg.level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; + diag_status_msg.message += "Main and Sub ECUs' localization initialization states are different."; + } + } + + // Check the route state equality + if (is_equal_route_stete()) { + last_time_route_state_is_equal = now; + } else { + if ((now - last_time_route_state_is_equal).seconds() > states_equality_timeout) { + diag_status_msg.level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; + diag_status_msg.message += "Main and Sub ECUs' route states are different."; + } + } + + // Check the route equality + if (is_equal_route()) { + last_time_route_is_equal = now; + } else { + if ((now - last_time_route_is_equal).seconds() > states_equality_timeout) { + diag_status_msg.level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; + diag_status_msg.message += "Main and Sub ECUs' routes are different."; + } + } + + diag_msg.status.push_back(diag_status_msg); + pub_diagnostic->publish(diag_msg); +} + +bool RedundantAutowareStateChecker::is_equal_pose_with_covariance() +{ + // If ether state is not subscribed, then assume the states are equal + if (!has_subscribed_main_pose_with_covariance) return true; + if (!has_subscribed_sub_pose_with_covariance) return true; + + // Currently, Main and Sub ECUs poses are used to check the equality + const auto main_pose = main_pose_with_covariance->pose.pose; + const auto sub_pose = sub_pose_with_covariance->pose.pose; + const auto squared_distance = + (main_pose.position.x - sub_pose.position.x) * (main_pose.position.x - sub_pose.position.x) + + (main_pose.position.y - sub_pose.position.y) * (main_pose.position.y - sub_pose.position.y); + if (squared_distance > pose_distance_threshold * pose_distance_threshold) return false; + + return true; +} + +bool RedundantAutowareStateChecker::is_equal_operation_mode_stete() +{ + // If ether state is not subscribed, then assume the states are equal + if (!has_subscribed_main_operation_mode_state) return true; + if (!has_subscribed_sub_operation_mode_state) return true; + + // Currently, following menbers are used to check the equality + if (main_operation_mode_state->mode != sub_operation_mode_state->mode) return false; + if (main_operation_mode_state->is_autoware_control_enabled != sub_operation_mode_state->is_autoware_control_enabled) return false; + + return true; +} + +bool RedundantAutowareStateChecker::is_equal_localization_initialization_state() +{ + // If ether state is not subscribed, then assume the states are equal + if (!has_subscribed_main_localization_initialization_state) return true; + if (!has_subscribed_sub_localization_initialization_state) return true; + + // Currently, following menbers are used to check the equality + if (main_localization_initialization_state->state != main_localization_initialization_state->state) return false; + + return true; +} + +bool RedundantAutowareStateChecker::is_equal_route_stete() +{ + // If ether state is not subscribed, then assume the states are equal + if (!has_subscribed_main_route_state) return true; + if (!has_subscribed_sub_route_state) return true; + + // Currently, following menbers are used to check the equality + if (main_route_state->state != sub_route_state->state) return false; + + return true; +} + +bool RedundantAutowareStateChecker::is_equal_route() +{ + // If ether state is not subscribed, then assume the states are equal + if (!has_subscribed_main_route) return true; + if (!has_subscribed_sub_route) return true; + + // Currently, following menbers are used to check the equality + if (main_route->data != sub_route->data) return false; + + return true; +} + +} // namespace redundant_autoware_state_checker + + +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + rclcpp::executors::SingleThreadedExecutor executor; + auto node = std::make_shared(); + executor.add_node(node); + executor.spin(); + executor.remove_node(node); + rclcpp::shutdown(); + return 0; +} \ No newline at end of file diff --git a/system/redundant_autoware_state_checker/src/redundant_autoware_state_checker.hpp b/system/redundant_autoware_state_checker/src/redundant_autoware_state_checker.hpp new file mode 100644 index 0000000000000..00988eae171c4 --- /dev/null +++ b/system/redundant_autoware_state_checker/src/redundant_autoware_state_checker.hpp @@ -0,0 +1,114 @@ +// Copyright 2023 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 REDUNDANT_AUTOWARE_STATE_CHECKER_ +#define REDUNDANT_AUTOWARE_STATE_CHECKER_ + +#include +#include +#include +#include +#include +#include +#include + +#include "rclcpp/rclcpp.hpp" + +namespace redundant_autoware_state_checker +{ + +class RedundantAutowareStateChecker : public rclcpp::Node +{ +public: + explicit RedundantAutowareStateChecker(); + +private: + // Publishers + rclcpp::Publisher::SharedPtr pub_diagnostic; + + // Subscribers + rclcpp::Subscription::SharedPtr sub_overall_mrm_state; + rclcpp::Subscription::SharedPtr sub_main_pose_with_covariance; + rclcpp::Subscription::SharedPtr sub_sub_pose_with_covariance; + rclcpp::Subscription::SharedPtr sub_main_operation_mode_state; + rclcpp::Subscription::SharedPtr sub_sub_operation_mode_state; + rclcpp::Subscription::SharedPtr sub_main_localization_initialization_state; + rclcpp::Subscription::SharedPtr sub_sub_localization_initialization_state; + rclcpp::Subscription::SharedPtr sub_main_route_state; + rclcpp::Subscription::SharedPtr sub_sub_route_state; + rclcpp::Subscription::SharedPtr sub_main_route; + rclcpp::Subscription::SharedPtr sub_sub_route; + + // Callbacks + void onTimer(); + void on_main_pose_with_covariance(const geometry_msgs::msg::PoseWithCovarianceStamped::ConstSharedPtr msg); + void on_sub_pose_with_covariance(const geometry_msgs::msg::PoseWithCovarianceStamped::ConstSharedPtr msg); + void on_mrm_state(const autoware_adapi_v1_msgs::msg::MrmState::ConstSharedPtr msg); + void on_main_operation_mode_state(const autoware_adapi_v1_msgs::msg::OperationModeState::ConstSharedPtr msg); + void on_sub_operation_mode_state(const autoware_adapi_v1_msgs::msg::OperationModeState::ConstSharedPtr msg); + void on_main_localization_initialization_state(const autoware_adapi_v1_msgs::msg::LocalizationInitializationState::ConstSharedPtr msg); + void on_sub_localization_initialization_state(const autoware_adapi_v1_msgs::msg::LocalizationInitializationState::ConstSharedPtr msg); + void on_main_route_state(const autoware_adapi_v1_msgs::msg::RouteState::ConstSharedPtr msg); + void on_sub_route_state(const autoware_adapi_v1_msgs::msg::RouteState::ConstSharedPtr msg); + void on_main_route(const autoware_adapi_v1_msgs::msg::Route::ConstSharedPtr msg); + void on_sub_route(const autoware_adapi_v1_msgs::msg::Route::ConstSharedPtr msg); + + // Variables + bool is_autonomous; + rclcpp::TimerBase::SharedPtr timer_; + + bool has_subscribed_main_pose_with_covariance = false; + bool has_subscribed_sub_pose_with_covariance = false; + bool has_subscribed_main_operation_mode_state = false; + bool has_subscribed_sub_operation_mode_state = false; + bool has_subscribed_main_localization_initialization_state = false; + bool has_subscribed_sub_localization_initialization_state = false; + bool has_subscribed_main_route_state = false; + bool has_subscribed_sub_route_state = false; + bool has_subscribed_main_route = false; + bool has_subscribed_sub_route = false; + + geometry_msgs::msg::PoseWithCovarianceStamped::ConstSharedPtr main_pose_with_covariance; + geometry_msgs::msg::PoseWithCovarianceStamped::ConstSharedPtr sub_pose_with_covariance; + autoware_adapi_v1_msgs::msg::OperationModeState::ConstSharedPtr main_operation_mode_state; + autoware_adapi_v1_msgs::msg::OperationModeState::ConstSharedPtr sub_operation_mode_state; + autoware_adapi_v1_msgs::msg::LocalizationInitializationState::ConstSharedPtr main_localization_initialization_state; + autoware_adapi_v1_msgs::msg::LocalizationInitializationState::ConstSharedPtr sub_localization_initialization_state; + autoware_adapi_v1_msgs::msg::RouteState::ConstSharedPtr main_route_state; + autoware_adapi_v1_msgs::msg::RouteState::ConstSharedPtr sub_route_state; + autoware_adapi_v1_msgs::msg::Route::ConstSharedPtr main_route; + autoware_adapi_v1_msgs::msg::Route::ConstSharedPtr sub_route; + + rclcpp::Time last_time_pose_with_covariance_is_equal; + rclcpp::Time last_time_operation_mode_state_is_equal; + rclcpp::Time last_time_localization_initialization_state_is_equal; + rclcpp::Time last_time_route_state_is_equal; + rclcpp::Time last_time_route_is_equal; + + // Params + double states_equality_timeout; + double update_rate_hz; + double pose_distance_threshold; + + // Functions + bool is_equal_pose_with_covariance(); + bool is_equal_operation_mode_stete(); + bool is_equal_localization_initialization_state(); + bool is_equal_route_stete(); + bool is_equal_route(); +}; + +} // namespace redundant_autoware_state_checker + +#endif // REDUNDANT_AUTOWARE_STATE_CHECKER_ \ No newline at end of file