From 26c537b4b2455db7271860e00520c07df6cd4fbb Mon Sep 17 00:00:00 2001 From: veqcc Date: Fri, 15 Dec 2023 00:19:16 +0900 Subject: [PATCH 01/11] 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 From 5a35f28eb3e486c4b334a73a9af7ddbe5435de33 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Thu, 14 Dec 2023 15:25:28 +0000 Subject: [PATCH 02/11] style(pre-commit): autofix --- .../CMakeLists.txt | 2 +- .../README.md | 2 +- ...edundant_autoware_state_checker.param.yaml | 2 +- ...edundant_autoware_state_checker.launch.xml | 2 +- .../package.xml | 2 +- .../src/redundant_autoware_state_checker.cpp | 78 +++++++++++-------- .../src/redundant_autoware_state_checker.hpp | 66 +++++++++------- 7 files changed, 92 insertions(+), 62 deletions(-) diff --git a/system/redundant_autoware_state_checker/CMakeLists.txt b/system/redundant_autoware_state_checker/CMakeLists.txt index 02da221690b7c..0634e51ee4eb9 100644 --- a/system/redundant_autoware_state_checker/CMakeLists.txt +++ b/system/redundant_autoware_state_checker/CMakeLists.txt @@ -11,4 +11,4 @@ ament_auto_add_executable(redundant_autoware_state_checker 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 index 76b5f4ec2cc07..230a24ebcc00d 100644 --- a/system/redundant_autoware_state_checker/README.md +++ b/system/redundant_autoware_state_checker/README.md @@ -1 +1 @@ -# redundant_autoware_state_checker \ No newline at end of file +# redundant_autoware_state_checker 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 index 89f82f1024545..1a3d3e2fb6d00 100644 --- 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 @@ -2,4 +2,4 @@ ros__parameters: states_equality_timeout: 1.0 update_rate_hz: 10.0 - pose_distance_threshold: 1.0 \ No newline at end of file + pose_distance_threshold: 1.0 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 index dcf53dfb8590b..14ff5756644c7 100644 --- 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 @@ -3,4 +3,4 @@ - \ No newline at end of file + diff --git a/system/redundant_autoware_state_checker/package.xml b/system/redundant_autoware_state_checker/package.xml index 0947e862175de..3f4ea7aefc4de 100644 --- a/system/redundant_autoware_state_checker/package.xml +++ b/system/redundant_autoware_state_checker/package.xml @@ -20,4 +20,4 @@ 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 index 6e37a504435b2..75fdcca09d5be 100644 --- a/system/redundant_autoware_state_checker/src/redundant_autoware_state_checker.cpp +++ b/system/redundant_autoware_state_checker/src/redundant_autoware_state_checker.cpp @@ -17,15 +17,16 @@ namespace redundant_autoware_state_checker { -RedundantAutowareStateChecker::RedundantAutowareStateChecker() : Node("RedundantAutowareStateChecker") +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 + 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( @@ -35,24 +36,31 @@ RedundantAutowareStateChecker::RedundantAutowareStateChecker() : Node("Redundant 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_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_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)); @@ -68,7 +76,8 @@ RedundantAutowareStateChecker::RedundantAutowareStateChecker() : Node("Redundant // 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)); + timer_ = rclcpp::create_timer( + this, get_clock(), period_ns, std::bind(&RedundantAutowareStateChecker::onTimer, this)); // Variables is_autonomous = true; @@ -82,9 +91,9 @@ RedundantAutowareStateChecker::RedundantAutowareStateChecker() : Node("Redundant 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); + 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( @@ -197,9 +206,12 @@ void RedundantAutowareStateChecker::onTimer() 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) { + 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."; + diag_status_msg.message += + "Main and Sub ECUs' localization initialization states are different."; } } @@ -252,7 +264,10 @@ bool RedundantAutowareStateChecker::is_equal_operation_mode_stete() // 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; + if ( + main_operation_mode_state->is_autoware_control_enabled != + sub_operation_mode_state->is_autoware_control_enabled) + return false; return true; } @@ -264,7 +279,9 @@ bool RedundantAutowareStateChecker::is_equal_localization_initialization_state() 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; + if ( + main_localization_initialization_state->state != main_localization_initialization_state->state) + return false; return true; } @@ -293,8 +310,7 @@ bool RedundantAutowareStateChecker::is_equal_route() return true; } -} // namespace redundant_autoware_state_checker - +} // namespace redundant_autoware_state_checker int main(int argc, char ** argv) { @@ -306,4 +322,4 @@ int main(int argc, char ** argv) 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 index 00988eae171c4..bf9d665436feb 100644 --- a/system/redundant_autoware_state_checker/src/redundant_autoware_state_checker.hpp +++ b/system/redundant_autoware_state_checker/src/redundant_autoware_state_checker.hpp @@ -12,18 +12,18 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef REDUNDANT_AUTOWARE_STATE_CHECKER_ -#define REDUNDANT_AUTOWARE_STATE_CHECKER_ +#ifndef REDUNDANT_AUTOWARE_STATE_CHECKER_HPP_ +#define REDUNDANT_AUTOWARE_STATE_CHECKER_HPP_ -#include +#include "rclcpp/rclcpp.hpp" + +#include #include -#include #include -#include -#include #include - -#include "rclcpp/rclcpp.hpp" +#include +#include +#include namespace redundant_autoware_state_checker { @@ -36,29 +36,41 @@ class RedundantAutowareStateChecker : public rclcpp::Node 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_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_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_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); @@ -83,8 +95,10 @@ class RedundantAutowareStateChecker : public rclcpp::Node 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::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; @@ -109,6 +123,6 @@ class RedundantAutowareStateChecker : public rclcpp::Node bool is_equal_route(); }; -} // namespace redundant_autoware_state_checker +} // namespace redundant_autoware_state_checker -#endif // REDUNDANT_AUTOWARE_STATE_CHECKER_ \ No newline at end of file +#endif // REDUNDANT_AUTOWARE_STATE_CHECKER_HPP_ From 40b0e58bda0845a85b874e96de1dc75ea5882b29 Mon Sep 17 00:00:00 2001 From: veqcc Date: Fri, 15 Dec 2023 12:39:14 +0900 Subject: [PATCH 03/11] fix minor Signed-off-by: veqcc --- .../launch/redundant_autoware_state_checker.launch.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 index 14ff5756644c7..9b11fd979dd8f 100644 --- 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 @@ -1,6 +1,6 @@ - + From b52af00081db67ce18cdca802ebb58977d233847 Mon Sep 17 00:00:00 2001 From: TetsuKawa Date: Sat, 16 Dec 2023 16:39:13 +0900 Subject: [PATCH 04/11] feat: respond to spell checker --- .../src/redundant_autoware_state_checker.cpp | 16 ++++++++-------- .../src/redundant_autoware_state_checker.hpp | 4 ++-- 2 files changed, 10 insertions(+), 10 deletions(-) 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 index 75fdcca09d5be..7fae22a86499c 100644 --- a/system/redundant_autoware_state_checker/src/redundant_autoware_state_checker.cpp +++ b/system/redundant_autoware_state_checker/src/redundant_autoware_state_checker.cpp @@ -193,7 +193,7 @@ void RedundantAutowareStateChecker::onTimer() } // Check the operation mode state equality - if (is_equal_operation_mode_stete()) { + if (is_equal_operation_mode_state()) { last_time_operation_mode_state_is_equal = now; } else { if ((now - last_time_operation_mode_state_is_equal).seconds() > states_equality_timeout) { @@ -216,7 +216,7 @@ void RedundantAutowareStateChecker::onTimer() } // Check the route state equality - if (is_equal_route_stete()) { + if (is_equal_route_state()) { last_time_route_state_is_equal = now; } else { if ((now - last_time_route_state_is_equal).seconds() > states_equality_timeout) { @@ -256,13 +256,13 @@ bool RedundantAutowareStateChecker::is_equal_pose_with_covariance() return true; } -bool RedundantAutowareStateChecker::is_equal_operation_mode_stete() +bool RedundantAutowareStateChecker::is_equal_operation_mode_state() { // 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 + // Currently, following members 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 != @@ -278,7 +278,7 @@ bool RedundantAutowareStateChecker::is_equal_localization_initialization_state() 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 + // Currently, following members are used to check the equality if ( main_localization_initialization_state->state != main_localization_initialization_state->state) return false; @@ -286,13 +286,13 @@ bool RedundantAutowareStateChecker::is_equal_localization_initialization_state() return true; } -bool RedundantAutowareStateChecker::is_equal_route_stete() +bool RedundantAutowareStateChecker::is_equal_route_state() { // 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 + // Currently, following members are used to check the equality if (main_route_state->state != sub_route_state->state) return false; return true; @@ -304,7 +304,7 @@ bool RedundantAutowareStateChecker::is_equal_route() if (!has_subscribed_main_route) return true; if (!has_subscribed_sub_route) return true; - // Currently, following menbers are used to check the equality + // Currently, following members are used to check the equality if (main_route->data != sub_route->data) return false; return true; 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 index bf9d665436feb..cbb1b718def21 100644 --- a/system/redundant_autoware_state_checker/src/redundant_autoware_state_checker.hpp +++ b/system/redundant_autoware_state_checker/src/redundant_autoware_state_checker.hpp @@ -117,9 +117,9 @@ class RedundantAutowareStateChecker : public rclcpp::Node // Functions bool is_equal_pose_with_covariance(); - bool is_equal_operation_mode_stete(); + bool is_equal_operation_mode_state(); bool is_equal_localization_initialization_state(); - bool is_equal_route_stete(); + bool is_equal_route_state(); bool is_equal_route(); }; From a57dd7cf62760b7524ab3aee780d0e28cbadc91f Mon Sep 17 00:00:00 2001 From: TetsuKawa Date: Sat, 16 Dec 2023 16:50:40 +0900 Subject: [PATCH 05/11] fix: include memory and remove explicit --- .../src/redundant_autoware_state_checker.cpp | 2 ++ .../src/redundant_autoware_state_checker.hpp | 2 +- 2 files changed, 3 insertions(+), 1 deletion(-) 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 index 7fae22a86499c..887d35e57d15f 100644 --- a/system/redundant_autoware_state_checker/src/redundant_autoware_state_checker.cpp +++ b/system/redundant_autoware_state_checker/src/redundant_autoware_state_checker.cpp @@ -13,6 +13,8 @@ // limitations under the License. #include "redundant_autoware_state_checker.hpp" +#include + namespace redundant_autoware_state_checker { 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 index cbb1b718def21..8f0359c5b99f1 100644 --- a/system/redundant_autoware_state_checker/src/redundant_autoware_state_checker.hpp +++ b/system/redundant_autoware_state_checker/src/redundant_autoware_state_checker.hpp @@ -31,7 +31,7 @@ namespace redundant_autoware_state_checker class RedundantAutowareStateChecker : public rclcpp::Node { public: - explicit RedundantAutowareStateChecker(); + RedundantAutowareStateChecker(); private: // Publishers From f9e8775d3af96050b2916ba598ce3bc2470cf216 Mon Sep 17 00:00:00 2001 From: veqcc Date: Mon, 18 Dec 2023 12:17:24 +0900 Subject: [PATCH 06/11] remove unnecessary arg Signed-off-by: veqcc --- launch/tier4_system_launch/launch/system.launch.xml | 1 - 1 file changed, 1 deletion(-) diff --git a/launch/tier4_system_launch/launch/system.launch.xml b/launch/tier4_system_launch/launch/system.launch.xml index baad5b207b17b..016d613cfa72d 100644 --- a/launch/tier4_system_launch/launch/system.launch.xml +++ b/launch/tier4_system_launch/launch/system.launch.xml @@ -18,7 +18,6 @@ - From a8b47c6e8eeeb99279e5c535038d74d5dc7af35d Mon Sep 17 00:00:00 2001 From: veqcc Date: Mon, 18 Dec 2023 12:18:34 +0900 Subject: [PATCH 07/11] add maintainer Signed-off-by: veqcc --- system/redundant_autoware_state_checker/package.xml | 1 + 1 file changed, 1 insertion(+) diff --git a/system/redundant_autoware_state_checker/package.xml b/system/redundant_autoware_state_checker/package.xml index 3f4ea7aefc4de..c78b5b2df38cb 100644 --- a/system/redundant_autoware_state_checker/package.xml +++ b/system/redundant_autoware_state_checker/package.xml @@ -4,6 +4,7 @@ redundant_autoware_state_checker 0.1.0 The redundant_autoware_state_checker ROS 2 package + Makoto Kurihara Ryuta Kambe Apache License 2.0 From 821cd5bc44521ca9998ab78e8608c8f964f72dca Mon Sep 17 00:00:00 2001 From: veqcc Date: Mon, 18 Dec 2023 12:42:31 +0900 Subject: [PATCH 08/11] add trailing underscores to class variables Signed-off-by: veqcc --- .../src/redundant_autoware_state_checker.cpp | 150 +++++++++--------- .../src/redundant_autoware_state_checker.hpp | 86 +++++----- 2 files changed, 118 insertions(+), 118 deletions(-) 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 index 887d35e57d15f..a48a9b73619fa 100644 --- a/system/redundant_autoware_state_checker/src/redundant_autoware_state_checker.cpp +++ b/system/redundant_autoware_state_checker/src/redundant_autoware_state_checker.cpp @@ -25,75 +25,75 @@ RedundantAutowareStateChecker::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 = + 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( + pub_diagnostic_ = create_publisher( "/diagnostics/supervisor", rclcpp::QoS(1)); // Subscribers - sub_overall_mrm_state = create_subscription( + 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 = + 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_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 = + 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 = + 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 = + 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 = + 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( + 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_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( + 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_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(); + 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(); + 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 = + is_autonomous_ = (msg->state == autoware_adapi_v1_msgs::msg::MrmState::NORMAL && msg->behavior == autoware_adapi_v1_msgs::msg::MrmState::NONE); } @@ -101,76 +101,76 @@ void RedundantAutowareStateChecker::on_mrm_state( 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; + 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; + 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; + 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; + 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; + 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; + 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; + 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; + 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; + 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; + has_subscribed_sub_route_ = true; + sub_route_ = msg; } void RedundantAutowareStateChecker::onTimer() { - if (!is_autonomous) return; + if (!is_autonomous_) return; const auto now = this->now(); @@ -186,9 +186,9 @@ void RedundantAutowareStateChecker::onTimer() // 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; + last_time_pose_with_covariance_is_equal_ = now; } else { - if ((now - last_time_pose_with_covariance_is_equal).seconds() > states_equality_timeout) { + 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."; } @@ -196,9 +196,9 @@ void RedundantAutowareStateChecker::onTimer() // Check the operation mode state equality if (is_equal_operation_mode_state()) { - last_time_operation_mode_state_is_equal = now; + last_time_operation_mode_state_is_equal_ = now; } else { - if ((now - last_time_operation_mode_state_is_equal).seconds() > states_equality_timeout) { + 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."; } @@ -206,11 +206,11 @@ void RedundantAutowareStateChecker::onTimer() // Check the localization initialization state equality if (is_equal_localization_initialization_state()) { - last_time_localization_initialization_state_is_equal = now; + last_time_localization_initialization_state_is_equal_ = now; } else { if ( - (now - last_time_localization_initialization_state_is_equal).seconds() > - states_equality_timeout) { + (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."; @@ -219,9 +219,9 @@ void RedundantAutowareStateChecker::onTimer() // Check the route state equality if (is_equal_route_state()) { - last_time_route_state_is_equal = now; + last_time_route_state_is_equal_ = now; } else { - if ((now - last_time_route_state_is_equal).seconds() > states_equality_timeout) { + 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."; } @@ -229,31 +229,31 @@ void RedundantAutowareStateChecker::onTimer() // Check the route equality if (is_equal_route()) { - last_time_route_is_equal = now; + last_time_route_is_equal_ = now; } else { - if ((now - last_time_route_is_equal).seconds() > states_equality_timeout) { + 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); + 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; + 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 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; + if (squared_distance > pose_distance_threshold_ * pose_distance_threshold_) return false; return true; } @@ -261,14 +261,14 @@ bool RedundantAutowareStateChecker::is_equal_pose_with_covariance() bool RedundantAutowareStateChecker::is_equal_operation_mode_state() { // 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; + if (!has_subscribed_main_operation_mode_state_) return true; + if (!has_subscribed_sub_operation_mode_state_) return true; // Currently, following members are used to check the equality - if (main_operation_mode_state->mode != sub_operation_mode_state->mode) return false; + 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) + main_operation_mode_state_->is_autoware_control_enabled != + sub_operation_mode_state_->is_autoware_control_enabled) return false; return true; @@ -277,12 +277,12 @@ bool RedundantAutowareStateChecker::is_equal_operation_mode_state() 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; + if (!has_subscribed_main_localization_initialization_state_) return true; + if (!has_subscribed_sub_localization_initialization_state_) return true; // Currently, following members are used to check the equality if ( - main_localization_initialization_state->state != main_localization_initialization_state->state) + main_localization_initialization_state_->state != main_localization_initialization_state_->state) return false; return true; @@ -291,11 +291,11 @@ bool RedundantAutowareStateChecker::is_equal_localization_initialization_state() bool RedundantAutowareStateChecker::is_equal_route_state() { // 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; + if (!has_subscribed_main_route_state_) return true; + if (!has_subscribed_sub_route_state_) return true; // Currently, following members are used to check the equality - if (main_route_state->state != sub_route_state->state) return false; + if (main_route_state_->state != sub_route_state_->state) return false; return true; } @@ -303,11 +303,11 @@ bool RedundantAutowareStateChecker::is_equal_route_state() 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; + if (!has_subscribed_main_route_) return true; + if (!has_subscribed_sub_route_) return true; // Currently, following members are used to check the equality - if (main_route->data != sub_route->data) return false; + if (main_route_->data != sub_route_->data) return false; return true; } 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 index 8f0359c5b99f1..61bcc7c644022 100644 --- a/system/redundant_autoware_state_checker/src/redundant_autoware_state_checker.hpp +++ b/system/redundant_autoware_state_checker/src/redundant_autoware_state_checker.hpp @@ -35,26 +35,26 @@ class RedundantAutowareStateChecker : public rclcpp::Node private: // Publishers - rclcpp::Publisher::SharedPtr pub_diagnostic; + rclcpp::Publisher::SharedPtr pub_diagnostic_; // Subscribers - rclcpp::Subscription::SharedPtr sub_overall_mrm_state; + rclcpp::Subscription::SharedPtr sub_overall_mrm_state_; rclcpp::Subscription::SharedPtr - sub_main_pose_with_covariance; + sub_main_pose_with_covariance_; rclcpp::Subscription::SharedPtr - sub_sub_pose_with_covariance; + sub_sub_pose_with_covariance_; rclcpp::Subscription::SharedPtr - sub_main_operation_mode_state; + sub_main_operation_mode_state_; rclcpp::Subscription::SharedPtr - sub_sub_operation_mode_state; + sub_sub_operation_mode_state_; rclcpp::Subscription::SharedPtr - sub_main_localization_initialization_state; + 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; + 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(); @@ -77,43 +77,43 @@ class RedundantAutowareStateChecker : public rclcpp::Node void on_sub_route(const autoware_adapi_v1_msgs::msg::Route::ConstSharedPtr msg); // Variables - bool is_autonomous; + 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; + 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; + 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; + 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; + double states_equality_timeout_; + double update_rate_hz_; + double pose_distance_threshold_; // Functions bool is_equal_pose_with_covariance(); From 97145627c92b85ce2ba97e7d9d528f05af8d6e2b Mon Sep 17 00:00:00 2001 From: veqcc Date: Mon, 18 Dec 2023 12:44:54 +0900 Subject: [PATCH 09/11] change default value Signed-off-by: veqcc --- .../src/redundant_autoware_state_checker.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 index a48a9b73619fa..52ed9a53d3aff 100644 --- a/system/redundant_autoware_state_checker/src/redundant_autoware_state_checker.cpp +++ b/system/redundant_autoware_state_checker/src/redundant_autoware_state_checker.cpp @@ -82,7 +82,7 @@ RedundantAutowareStateChecker::RedundantAutowareStateChecker() this, get_clock(), period_ns, std::bind(&RedundantAutowareStateChecker::onTimer, this)); // Variables - is_autonomous_ = true; + is_autonomous_ = false; 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(); From f280c7f625b84af07921fdb5c681fac48d5d07c7 Mon Sep 17 00:00:00 2001 From: veqcc Date: Mon, 18 Dec 2023 12:59:14 +0900 Subject: [PATCH 10/11] implement route data equality check Signed-off-by: veqcc --- .../src/redundant_autoware_state_checker.cpp | 17 ++++++++++------- .../src/redundant_autoware_state_checker.hpp | 2 ++ 2 files changed, 12 insertions(+), 7 deletions(-) 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 index 52ed9a53d3aff..9cf5ba5e722e4 100644 --- a/system/redundant_autoware_state_checker/src/redundant_autoware_state_checker.cpp +++ b/system/redundant_autoware_state_checker/src/redundant_autoware_state_checker.cpp @@ -248,12 +248,7 @@ bool RedundantAutowareStateChecker::is_equal_pose_with_covariance() 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; + if (!is_close_to_each_other(main_pose_with_covariance_->pose.pose, sub_pose_with_covariance_->pose.pose)) return false; return true; } @@ -307,11 +302,19 @@ bool RedundantAutowareStateChecker::is_equal_route() if (!has_subscribed_sub_route_) return true; // Currently, following members are used to check the equality - if (main_route_->data != sub_route_->data) return false; + if (!is_close_to_each_other(main_route_->data[0].start, sub_route_->data[0].start)) return false; + if (!is_close_to_each_other(main_route_->data[0].goal, sub_route_->data[0].goal)) return false; return true; } +bool RedundantAutowareStateChecker::is_close_to_each_other(geometry_msgs::msg::Pose pose1, geometry_msgs::msg::Pose pose2) { + const auto squared_distance = + (pose1.position.x - pose2.position.x) * (pose1.position.x - pose2.position.x) + + (pose1.position.y - pose2.position.y) * (pose1.position.y - pose2.position.y); + return squared_distance <= pose_distance_threshold_ * pose_distance_threshold_; +} + } // namespace redundant_autoware_state_checker int main(int argc, char ** argv) 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 index 61bcc7c644022..02615ad29b9c3 100644 --- a/system/redundant_autoware_state_checker/src/redundant_autoware_state_checker.hpp +++ b/system/redundant_autoware_state_checker/src/redundant_autoware_state_checker.hpp @@ -121,6 +121,8 @@ class RedundantAutowareStateChecker : public rclcpp::Node bool is_equal_localization_initialization_state(); bool is_equal_route_state(); bool is_equal_route(); + + bool is_close_to_each_other(geometry_msgs::msg::Pose pose1, geometry_msgs::msg::Pose pose2); }; } // namespace redundant_autoware_state_checker From 8c06960d2001e03562b7d636f8748e13c93693d8 Mon Sep 17 00:00:00 2001 From: veqcc Date: Mon, 18 Dec 2023 13:30:36 +0900 Subject: [PATCH 11/11] modify onTimer function Signed-off-by: veqcc --- .../src/redundant_autoware_state_checker.cpp | 92 ++++++++++--------- 1 file changed, 47 insertions(+), 45 deletions(-) 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 index 9cf5ba5e722e4..2395ada31223f 100644 --- a/system/redundant_autoware_state_checker/src/redundant_autoware_state_checker.cpp +++ b/system/redundant_autoware_state_checker/src/redundant_autoware_state_checker.cpp @@ -170,8 +170,6 @@ void RedundantAutowareStateChecker::on_sub_route( void RedundantAutowareStateChecker::onTimer() { - if (!is_autonomous_) return; - const auto now = this->now(); diagnostic_msgs::msg::DiagnosticArray diag_msg; @@ -183,57 +181,61 @@ void RedundantAutowareStateChecker::onTimer() 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 equality only when is_autonomous is true + if (is_autonomous_) { + + // 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_state()) { - 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 operation mode state equality + if (is_equal_operation_mode_state()) { + 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 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_state()) { - 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 state equality + if (is_equal_route_state()) { + 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."; + // 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."; + } } }