From 23982c2b675b0d852548e39312cbf7d183c2a11f Mon Sep 17 00:00:00 2001 From: "Takagi, Isamu" Date: Wed, 10 Aug 2022 23:46:55 +0900 Subject: [PATCH 01/34] feat(autoware_ad_api_msgs): define operation mode interface Signed-off-by: Takagi, Isamu --- common/autoware_ad_api_specs/CMakeLists.txt | 7 ++ .../autoware_ad_api_specs/operation_mode.hpp | 73 +++++++++++++++++++ common/autoware_ad_api_specs/package.xml | 20 +++++ .../component_interface_specs/CMakeLists.txt | 7 ++ .../component_interface_specs/system.hpp | 50 +++++++++++++ common/component_interface_specs/package.xml | 20 +++++ 6 files changed, 177 insertions(+) create mode 100644 common/autoware_ad_api_specs/CMakeLists.txt create mode 100644 common/autoware_ad_api_specs/include/autoware_ad_api_specs/operation_mode.hpp create mode 100644 common/autoware_ad_api_specs/package.xml create mode 100644 common/component_interface_specs/CMakeLists.txt create mode 100644 common/component_interface_specs/include/component_interface_specs/system.hpp create mode 100644 common/component_interface_specs/package.xml diff --git a/common/autoware_ad_api_specs/CMakeLists.txt b/common/autoware_ad_api_specs/CMakeLists.txt new file mode 100644 index 0000000000000..5f02e45a9c6b2 --- /dev/null +++ b/common/autoware_ad_api_specs/CMakeLists.txt @@ -0,0 +1,7 @@ +cmake_minimum_required(VERSION 3.14) +project(autoware_ad_api_specs) + +find_package(autoware_cmake REQUIRED) +autoware_package() + +ament_auto_package() diff --git a/common/autoware_ad_api_specs/include/autoware_ad_api_specs/operation_mode.hpp b/common/autoware_ad_api_specs/include/autoware_ad_api_specs/operation_mode.hpp new file mode 100644 index 0000000000000..08528055d9966 --- /dev/null +++ b/common/autoware_ad_api_specs/include/autoware_ad_api_specs/operation_mode.hpp @@ -0,0 +1,73 @@ +// Copyright 2022 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 AUTOWARE_AD_API_SPECS__OPERATION_MODE_HPP_ +#define AUTOWARE_AD_API_SPECS__OPERATION_MODE_HPP_ + +#include + +#include +#include + +namespace autoware_ad_api::operation_mode +{ + +struct ChangeToStop +{ + using Service = autoware_ad_api_msgs::srv::ChangeOperationMode; + static constexpr char name[] = "/api/operation_mode/change_to_stop"; +}; + +struct ChangeToAutonomous +{ + using Service = autoware_ad_api_msgs::srv::ChangeOperationMode; + static constexpr char name[] = "/api/operation_mode/change_to_autonomous"; +}; + +struct ChangeToLocal +{ + using Service = autoware_ad_api_msgs::srv::ChangeOperationMode; + static constexpr char name[] = "/api/operation_mode/change_to_local"; +}; + +struct ChangeToRemote +{ + using Service = autoware_ad_api_msgs::srv::ChangeOperationMode; + static constexpr char name[] = "/api/operation_mode/change_to_remote"; +}; + +struct EnableAutowareControl +{ + using Service = autoware_ad_api_msgs::srv::ChangeOperationMode; + static constexpr char name[] = "/api/operation_mode/enable_autoware_control"; +}; + +struct DisableAutowareControl +{ + using Service = autoware_ad_api_msgs::srv::ChangeOperationMode; + static constexpr char name[] = "/api/operation_mode/disable_autoware_control"; +}; + +struct OperationModeState +{ + using Message = autoware_ad_api_msgs::msg::OperationModeState; + static constexpr char name[] = "/api/operation_mode/state"; + static constexpr size_t depth = 1; + static constexpr auto reliability = RMW_QOS_POLICY_RELIABILITY_RELIABLE; + static constexpr auto durability = RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL; +}; + +} // namespace autoware_ad_api::operation_mode + +#endif // AUTOWARE_AD_API_SPECS__OPERATION_MODE_HPP_ diff --git a/common/autoware_ad_api_specs/package.xml b/common/autoware_ad_api_specs/package.xml new file mode 100644 index 0000000000000..cc6b46b9c7210 --- /dev/null +++ b/common/autoware_ad_api_specs/package.xml @@ -0,0 +1,20 @@ + + + + autoware_ad_api_specs + 0.0.0 + The autoware_ad_api_specs package + Takagi, Isamu + Apache License 2.0 + + ament_cmake_auto + + autoware_cmake + + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/common/component_interface_specs/CMakeLists.txt b/common/component_interface_specs/CMakeLists.txt new file mode 100644 index 0000000000000..31c723600183f --- /dev/null +++ b/common/component_interface_specs/CMakeLists.txt @@ -0,0 +1,7 @@ +cmake_minimum_required(VERSION 3.14) +project(component_interface_specs) + +find_package(autoware_cmake REQUIRED) +autoware_package() + +ament_auto_package() diff --git a/common/component_interface_specs/include/component_interface_specs/system.hpp b/common/component_interface_specs/include/component_interface_specs/system.hpp new file mode 100644 index 0000000000000..2c82632d237f3 --- /dev/null +++ b/common/component_interface_specs/include/component_interface_specs/system.hpp @@ -0,0 +1,50 @@ +// Copyright 2022 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 COMPONENT_INTERFACE_SPECS__SYSTEM_HPP_ +#define COMPONENT_INTERFACE_SPECS__SYSTEM_HPP_ + +#include + +#include +#include +#include + +namespace system_interface +{ + +struct ChangeAutowareControl +{ + using Service = tier4_system_msgs::srv::ChangeAutowareControl; + static constexpr char name[] = "/system/operation_mode/change_autoware_control"; +}; + +struct ChangeOperationMode +{ + using Service = tier4_system_msgs::srv::ChangeOperationMode; + static constexpr char name[] = "/system/operation_mode/change_operation_mode"; +}; + +struct OperationModeState +{ + using Message = autoware_ad_api_msgs::msg::OperationModeState; + static constexpr char name[] = "/system/operation_mode/state"; + static constexpr size_t depth = 1; + static constexpr auto reliability = RMW_QOS_POLICY_RELIABILITY_RELIABLE; + static constexpr auto durability = RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL; +}; + +} // namespace system_interface + +#endif // COMPONENT_INTERFACE_SPECS__SYSTEM_HPP_ diff --git a/common/component_interface_specs/package.xml b/common/component_interface_specs/package.xml new file mode 100644 index 0000000000000..d0c12c83c2379 --- /dev/null +++ b/common/component_interface_specs/package.xml @@ -0,0 +1,20 @@ + + + + component_interface_specs + 0.0.0 + The component_interface_specs package + Takagi, Isamu + Apache License 2.0 + + ament_cmake_auto + + autoware_cmake + + ament_lint_auto + autoware_lint_common + + + ament_cmake + + From 00e79522b7b1617481134a6573dc9bf462d4c1ec Mon Sep 17 00:00:00 2001 From: "Takagi, Isamu" Date: Wed, 10 Aug 2022 23:47:56 +0900 Subject: [PATCH 02/34] feat(default_ad_api): add operation mode api Signed-off-by: Takagi, Isamu --- .../launch/autoware_api.launch.xml | 6 ++ system/default_ad_api/CMakeLists.txt | 6 +- .../launch/default_ad_api.launch.py | 1 + system/default_ad_api/package.xml | 3 + system/default_ad_api/src/interface.cpp | 14 +-- system/default_ad_api/src/interface.hpp | 3 +- system/default_ad_api/src/operation_mode.cpp | 98 +++++++++++++++++++ system/default_ad_api/src/operation_mode.hpp | 75 ++++++++++++++ system/default_ad_api/src/utils/types.hpp | 14 +-- 9 files changed, 198 insertions(+), 22 deletions(-) create mode 100644 system/default_ad_api/src/operation_mode.cpp create mode 100644 system/default_ad_api/src/operation_mode.hpp diff --git a/launch/tier4_autoware_api_launch/launch/autoware_api.launch.xml b/launch/tier4_autoware_api_launch/launch/autoware_api.launch.xml index 914ca0d46e2d0..a98ac01548732 100644 --- a/launch/tier4_autoware_api_launch/launch/autoware_api.launch.xml +++ b/launch/tier4_autoware_api_launch/launch/autoware_api.launch.xml @@ -2,10 +2,16 @@ + + + + + + diff --git a/system/default_ad_api/CMakeLists.txt b/system/default_ad_api/CMakeLists.txt index ae4125cc822ff..5730ca9fa9cba 100644 --- a/system/default_ad_api/CMakeLists.txt +++ b/system/default_ad_api/CMakeLists.txt @@ -6,9 +6,13 @@ autoware_package() ament_auto_add_library(${PROJECT_NAME} SHARED src/interface.cpp + src/operation_mode.cpp ) -rclcpp_components_register_nodes(${PROJECT_NAME} "default_ad_api::InterfaceNode") +rclcpp_components_register_nodes(${PROJECT_NAME} + "default_ad_api::InterfaceNode" + "default_ad_api::OperationModeNode" +) if(BUILD_TESTING) add_launch_test(test/main.test.py) diff --git a/system/default_ad_api/launch/default_ad_api.launch.py b/system/default_ad_api/launch/default_ad_api.launch.py index e9542328ed986..ea88c20209655 100644 --- a/system/default_ad_api/launch/default_ad_api.launch.py +++ b/system/default_ad_api/launch/default_ad_api.launch.py @@ -31,6 +31,7 @@ def _create_api_node(node_name, class_name, **kwargs): def generate_launch_description(): components = [ _create_api_node("interface", "InterfaceNode"), + _create_api_node("operation_mode", "OperationModeNode"), ] container = ComposableNodeContainer( namespace="default_ad_api", diff --git a/system/default_ad_api/package.xml b/system/default_ad_api/package.xml index 0bb71303d5466..c518f0392ebb3 100644 --- a/system/default_ad_api/package.xml +++ b/system/default_ad_api/package.xml @@ -12,9 +12,12 @@ autoware_cmake autoware_ad_api_msgs + autoware_ad_api_specs + component_interface_specs component_interface_utils rclcpp rclcpp_components + tier4_system_msgs python3-flask diff --git a/system/default_ad_api/src/interface.cpp b/system/default_ad_api/src/interface.cpp index bd54da361fe1b..1df98603ed7a7 100644 --- a/system/default_ad_api/src/interface.cpp +++ b/system/default_ad_api/src/interface.cpp @@ -19,16 +19,12 @@ namespace default_ad_api InterfaceNode::InterfaceNode(const rclcpp::NodeOptions & options) : Node("interface", options) { - using InterfaceVersion = autoware_ad_api_msgs::srv::InterfaceVersion; - - const auto on_interface_version = [](SERVICE_ARG_NO_REQ(InterfaceVersion)) { - response->major = 0; - response->minor = 1; - response->patch = 0; - }; - const auto node = component_interface_utils::NodeAdaptor(this); - node.init_srv(srv_, on_interface_version); + node.init_srv(srv_, [](auto, auto res) { + res->major = 0; + res->minor = 1; + res->patch = 0; + }); } } // namespace default_ad_api diff --git a/system/default_ad_api/src/interface.hpp b/system/default_ad_api/src/interface.hpp index 6a1bda419bb7d..f9fb25bd5136e 100644 --- a/system/default_ad_api/src/interface.hpp +++ b/system/default_ad_api/src/interface.hpp @@ -16,7 +16,6 @@ #define INTERFACE_HPP_ #include "default_ad_api/specs/interface/version.hpp" -#include "utils/types.hpp" #include #include @@ -30,7 +29,7 @@ class InterfaceNode : public rclcpp::Node explicit InterfaceNode(const rclcpp::NodeOptions & options); private: - Service::SharedPtr srv_; + component_interface_utils::Service::SharedPtr srv_; }; } // namespace default_ad_api diff --git a/system/default_ad_api/src/operation_mode.cpp b/system/default_ad_api/src/operation_mode.cpp new file mode 100644 index 0000000000000..caa044fe9e9da --- /dev/null +++ b/system/default_ad_api/src/operation_mode.cpp @@ -0,0 +1,98 @@ +// Copyright 2022 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 "operation_mode.hpp" + +#include + +namespace default_ad_api +{ + +using OperationModeRequest = system_interface::ChangeOperationMode::Service::Request; +using OperationMode = OperationModeRequest::_operation_type; +using AutowareControlRequest = system_interface::ChangeAutowareControl::Service::Request; + +OperationModeNode::OperationModeNode(const rclcpp::NodeOptions & options) : Node("routing", options) +{ + const auto adaptor = component_interface_utils::NodeAdaptor(this); + group_cli_ = create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + // adaptor.relay_message(pub_state_, sub_state_); + adaptor.init_srv(srv_stop_mode_, this, &OperationModeNode::on_change_to_stop); + adaptor.init_srv(srv_autonomous_mode_, this, &OperationModeNode::on_change_to_autonomous); + adaptor.init_srv(srv_local_mode_, this, &OperationModeNode::on_change_to_local); + adaptor.init_srv(srv_remote_mode_, this, &OperationModeNode::on_change_to_remote); + adaptor.init_srv(srv_enable_control_, this, &OperationModeNode::on_enable_autoware_control); + adaptor.init_srv(srv_disable_control_, this, &OperationModeNode::on_disable_autoware_control); + adaptor.init_cli(cli_mode_, group_cli_); + adaptor.init_cli(cli_control_, group_cli_); +} + +void OperationModeNode::on_change_to_stop( + const ChangeToStop::Service::Request::SharedPtr, + const ChangeToStop::Service::Response::SharedPtr res) +{ + const auto req = std::make_shared(); + req->operation.mode = OperationMode::STOP; + res->status = cli_mode_->call(req)->status; +} + +void OperationModeNode::on_change_to_autonomous( + const ChangeToAutonomous::Service::Request::SharedPtr, + const ChangeToAutonomous::Service::Response::SharedPtr res) +{ + const auto req = std::make_shared(); + req->operation.mode = OperationMode::AUTONOMOUS; + res->status = cli_mode_->call(req)->status; +} + +void OperationModeNode::on_change_to_local( + const ChangeToLocal::Service::Request::SharedPtr, + const ChangeToLocal::Service::Response::SharedPtr res) +{ + const auto req = std::make_shared(); + req->operation.mode = OperationMode::LOCAL; + res->status = cli_mode_->call(req)->status; +} + +void OperationModeNode::on_change_to_remote( + const ChangeToRemote::Service::Request::SharedPtr, + const ChangeToRemote::Service::Response::SharedPtr res) +{ + const auto req = std::make_shared(); + req->operation.mode = OperationMode::REMOTE; + res->status = cli_mode_->call(req)->status; +} + +void OperationModeNode::on_enable_autoware_control( + const EnableAutowareControl::Service::Request::SharedPtr, + const EnableAutowareControl::Service::Response::SharedPtr res) +{ + const auto req = std::make_shared(); + req->autoware_control = true; + res->status = cli_control_->call(req)->status; +} + +void OperationModeNode::on_disable_autoware_control( + const DisableAutowareControl::Service::Request::SharedPtr, + const DisableAutowareControl::Service::Response::SharedPtr res) +{ + const auto req = std::make_shared(); + req->autoware_control = false; + res->status = cli_control_->call(req)->status; +} + +} // namespace default_ad_api + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(default_ad_api::OperationModeNode) diff --git a/system/default_ad_api/src/operation_mode.hpp b/system/default_ad_api/src/operation_mode.hpp new file mode 100644 index 0000000000000..745dceef9dee1 --- /dev/null +++ b/system/default_ad_api/src/operation_mode.hpp @@ -0,0 +1,75 @@ +// Copyright 2022 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 OPERATION_MODE_HPP_ +#define OPERATION_MODE_HPP_ + +#include +#include +#include + +// This file should be included after messages. +#include "utils/types.hpp" + +namespace default_ad_api +{ + +class OperationModeNode : public rclcpp::Node +{ +public: + explicit OperationModeNode(const rclcpp::NodeOptions & options); + +private: + rclcpp::CallbackGroup::SharedPtr group_cli_; + Pub pub_state_; + Srv srv_stop_mode_; + Srv srv_autonomous_mode_; + Srv srv_local_mode_; + Srv srv_remote_mode_; + Srv srv_enable_control_; + Srv srv_disable_control_; + Sub sub_state_; + Cli cli_mode_; + Cli cli_control_; + + using ChangeToStop = autoware_ad_api::operation_mode::ChangeToStop; + using ChangeToAutonomous = autoware_ad_api::operation_mode::ChangeToAutonomous; + using ChangeToLocal = autoware_ad_api::operation_mode::ChangeToLocal; + using ChangeToRemote = autoware_ad_api::operation_mode::ChangeToRemote; + using EnableAutowareControl = autoware_ad_api::operation_mode::EnableAutowareControl; + using DisableAutowareControl = autoware_ad_api::operation_mode::DisableAutowareControl; + + void on_change_to_stop( + const ChangeToStop::Service::Request::SharedPtr req, + const ChangeToStop::Service::Response::SharedPtr res); + void on_change_to_autonomous( + const ChangeToAutonomous::Service::Request::SharedPtr req, + const ChangeToAutonomous::Service::Response::SharedPtr res); + void on_change_to_local( + const ChangeToLocal::Service::Request::SharedPtr req, + const ChangeToLocal::Service::Response::SharedPtr res); + void on_change_to_remote( + const ChangeToRemote::Service::Request::SharedPtr req, + const ChangeToRemote::Service::Response::SharedPtr res); + void on_enable_autoware_control( + const EnableAutowareControl::Service::Request::SharedPtr req, + const EnableAutowareControl::Service::Response::SharedPtr res); + void on_disable_autoware_control( + const DisableAutowareControl::Service::Request::SharedPtr req, + const DisableAutowareControl::Service::Response::SharedPtr res); +}; + +} // namespace default_ad_api + +#endif // OPERATION_MODE_HPP_ diff --git a/system/default_ad_api/src/utils/types.hpp b/system/default_ad_api/src/utils/types.hpp index b9a2036804d79..693e64a0dce55 100644 --- a/system/default_ad_api/src/utils/types.hpp +++ b/system/default_ad_api/src/utils/types.hpp @@ -17,23 +17,17 @@ #include -#define MESSAGE_ARG(Type) const Type::ConstSharedPtr message -#define SERVICE_ARG(Type) \ - const Type::Request::SharedPtr request, const Type::Response::SharedPtr response -#define SERVICE_ARG_NO_REQ(Type) \ - const Type::Request::SharedPtr, const Type::Response::SharedPtr response - namespace default_ad_api { template -using Publisher = component_interface_utils::Publisher; +using Pub = typename component_interface_utils::Publisher::SharedPtr; template -using Subscription = component_interface_utils::Subscription; +using Sub = typename component_interface_utils::Subscription::SharedPtr; template -using Client = component_interface_utils::Client; +using Cli = typename component_interface_utils::Client::SharedPtr; template -using Service = component_interface_utils::Service; +using Srv = typename component_interface_utils::Service::SharedPtr; } // namespace default_ad_api From 3772e2c332c17da3c6d722b415676b0e48d9f888 Mon Sep 17 00:00:00 2001 From: "Takagi, Isamu" Date: Wed, 10 Aug 2022 23:56:00 +0900 Subject: [PATCH 03/34] fix: add message Signed-off-by: Takagi, Isamu --- common/autoware_ad_api_msgs/CMakeLists.txt | 3 +++ .../autoware_ad_api_msgs/common/msg/ResponseStatus.msg | 4 ++++ .../operation_mode/msg/OperationMode.msg | 9 +++++++++ .../operation_mode/msg/OperationModeState.msg | 8 ++++++++ .../operation_mode/srv/ChangeOperationMode.srv | 4 ++++ 5 files changed, 28 insertions(+) create mode 100644 common/autoware_ad_api_msgs/operation_mode/msg/OperationMode.msg create mode 100644 common/autoware_ad_api_msgs/operation_mode/msg/OperationModeState.msg create mode 100644 common/autoware_ad_api_msgs/operation_mode/srv/ChangeOperationMode.srv diff --git a/common/autoware_ad_api_msgs/CMakeLists.txt b/common/autoware_ad_api_msgs/CMakeLists.txt index 031134af5a5de..fb675886636d1 100644 --- a/common/autoware_ad_api_msgs/CMakeLists.txt +++ b/common/autoware_ad_api_msgs/CMakeLists.txt @@ -7,6 +7,9 @@ autoware_package() rosidl_generate_interfaces(${PROJECT_NAME} common/msg/ResponseStatus.msg interface/srv/InterfaceVersion.srv + operation_mode/srv/ChangeOperationMode.srv + operation_mode/msg/OperationMode.msg + operation_mode/msg/OperationModeState.msg DEPENDENCIES builtin_interfaces std_msgs diff --git a/common/autoware_ad_api_msgs/common/msg/ResponseStatus.msg b/common/autoware_ad_api_msgs/common/msg/ResponseStatus.msg index 4fe0e5d4208e3..65d6be71240ed 100644 --- a/common/autoware_ad_api_msgs/common/msg/ResponseStatus.msg +++ b/common/autoware_ad_api_msgs/common/msg/ResponseStatus.msg @@ -3,6 +3,10 @@ uint16 DEPRECATED = 50000 uint16 SERVICE_UNREADY = 50001 uint16 SERVICE_TIMEOUT = 50002 uint16 TRANSFORM_ERROR = 50003 +uint16 PARAMETER_ERROR = 50004 + +# constants for code +uint16 NO_EFFECT = 60001 # variables bool success diff --git a/common/autoware_ad_api_msgs/operation_mode/msg/OperationMode.msg b/common/autoware_ad_api_msgs/operation_mode/msg/OperationMode.msg new file mode 100644 index 0000000000000..7d750bd71be70 --- /dev/null +++ b/common/autoware_ad_api_msgs/operation_mode/msg/OperationMode.msg @@ -0,0 +1,9 @@ +# constants for mode +uint16 UNKNOWN = 0 +uint16 STOP = 1 +uint16 AUTONOMOUS = 2 +uint16 LOCAL = 3 +uint16 REMOTE = 4 + +# variables +uint16 mode diff --git a/common/autoware_ad_api_msgs/operation_mode/msg/OperationModeState.msg b/common/autoware_ad_api_msgs/operation_mode/msg/OperationModeState.msg new file mode 100644 index 0000000000000..4ec8180acda5a --- /dev/null +++ b/common/autoware_ad_api_msgs/operation_mode/msg/OperationModeState.msg @@ -0,0 +1,8 @@ +builtin_interfaces/Time stamp +autoware_ad_api_msgs/OperationMode operation +bool is_autoware_control_enabled +bool is_in_transition +bool change_to_stop +bool change_to_autonomous +bool change_to_local +bool change_to_remote diff --git a/common/autoware_ad_api_msgs/operation_mode/srv/ChangeOperationMode.srv b/common/autoware_ad_api_msgs/operation_mode/srv/ChangeOperationMode.srv new file mode 100644 index 0000000000000..2c0723843d927 --- /dev/null +++ b/common/autoware_ad_api_msgs/operation_mode/srv/ChangeOperationMode.srv @@ -0,0 +1,4 @@ +--- +uint16 ERROR_NOT_AVAILABLE = 1 +uint16 ERROR_IN_TRANSITION = 2 +autoware_ad_api_msgs/ResponseStatus status From c27f603e197b31285ba03173213d40a8f8940b56 Mon Sep 17 00:00:00 2001 From: "Takagi, Isamu" <43976882+isamu-takagi@users.noreply.github.com> Date: Thu, 11 Aug 2022 16:57:33 +0900 Subject: [PATCH 04/34] Update common/autoware_ad_api_msgs/operation_mode/msg/OperationModeState.msg Co-authored-by: Kenji Miyake <31987104+kenji-miyake@users.noreply.github.com> --- .../operation_mode/msg/OperationModeState.msg | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/common/autoware_ad_api_msgs/operation_mode/msg/OperationModeState.msg b/common/autoware_ad_api_msgs/operation_mode/msg/OperationModeState.msg index 4ec8180acda5a..9062580c78ed9 100644 --- a/common/autoware_ad_api_msgs/operation_mode/msg/OperationModeState.msg +++ b/common/autoware_ad_api_msgs/operation_mode/msg/OperationModeState.msg @@ -2,7 +2,7 @@ builtin_interfaces/Time stamp autoware_ad_api_msgs/OperationMode operation bool is_autoware_control_enabled bool is_in_transition -bool change_to_stop -bool change_to_autonomous -bool change_to_local -bool change_to_remote +bool is_stop_mode_available +bool is_autonomous_mode_available +bool is_local_mode_available +bool is_remote_mode_available From 3ab7ff0e5d5ee4c521d00150a3b018a8101ccd73 Mon Sep 17 00:00:00 2001 From: "Takagi, Isamu" <43976882+isamu-takagi@users.noreply.github.com> Date: Thu, 11 Aug 2022 16:57:49 +0900 Subject: [PATCH 05/34] Update common/autoware_ad_api_msgs/operation_mode/msg/OperationModeState.msg Co-authored-by: Kenji Miyake <31987104+kenji-miyake@users.noreply.github.com> --- .../operation_mode/msg/OperationModeState.msg | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/common/autoware_ad_api_msgs/operation_mode/msg/OperationModeState.msg b/common/autoware_ad_api_msgs/operation_mode/msg/OperationModeState.msg index 9062580c78ed9..0ca9ff31be842 100644 --- a/common/autoware_ad_api_msgs/operation_mode/msg/OperationModeState.msg +++ b/common/autoware_ad_api_msgs/operation_mode/msg/OperationModeState.msg @@ -1,5 +1,5 @@ builtin_interfaces/Time stamp -autoware_ad_api_msgs/OperationMode operation +autoware_ad_api_msgs/OperationMode mode bool is_autoware_control_enabled bool is_in_transition bool is_stop_mode_available From 325dfab43e8b70d10ae1bfc702a28e2b04bf913d Mon Sep 17 00:00:00 2001 From: "Takagi, Isamu" Date: Fri, 12 Aug 2022 11:40:37 +0900 Subject: [PATCH 06/34] fix: add message callback Signed-off-by: Takagi, Isamu --- .../component_interface_utils/rclcpp.hpp | 21 ++++++++++++++++--- system/default_ad_api/src/operation_mode.cpp | 8 ++++++- system/default_ad_api/src/operation_mode.hpp | 3 +++ 3 files changed, 28 insertions(+), 4 deletions(-) diff --git a/common/component_interface_utils/include/component_interface_utils/rclcpp.hpp b/common/component_interface_utils/include/component_interface_utils/rclcpp.hpp index e295173ac6e52..3916a76e6f611 100644 --- a/common/component_interface_utils/include/component_interface_utils/rclcpp.hpp +++ b/common/component_interface_utils/include/component_interface_utils/rclcpp.hpp @@ -32,6 +32,10 @@ class NodeAdaptor private: using CallbackGroup = rclcpp::CallbackGroup::SharedPtr; + template + using MessageCallback = + void (InstanceT::*)(const typename SharedPtrT::element_type::SpecType::Message::ConstSharedPtr); + template using ServiceCallback = void (InstanceT::*)( const typename SharedPtrT::element_type::SpecType::Service::Request::SharedPtr, @@ -94,14 +98,25 @@ class NodeAdaptor srv, [cli, timeout](ReqT req, ResT res) { *res = *cli->call(req, timeout); }, group); } + /// Create a subscription wrapper. + template + void init_sub( + SharedPtrT & sub, InstanceT * instance, + MessageCallback && callback) const + { + using std::placeholders::_1; + init_sub(sub, std::bind(callback, instance, _1)); + } + /// Create a service wrapper for logging. template void init_srv( - SharedPtrT & srv, InstanceT * instance, ServiceCallback callback, + SharedPtrT & srv, InstanceT * instance, ServiceCallback && callback, CallbackGroup group = nullptr) const { - init_srv( - srv, [instance, callback](auto req, auto res) { (instance->*callback)(req, res); }, group); + using std::placeholders::_1; + using std::placeholders::_2; + init_srv(srv, std::bind(callback, instance, _1, _2), group); } private: diff --git a/system/default_ad_api/src/operation_mode.cpp b/system/default_ad_api/src/operation_mode.cpp index caa044fe9e9da..21269bffd47e4 100644 --- a/system/default_ad_api/src/operation_mode.cpp +++ b/system/default_ad_api/src/operation_mode.cpp @@ -27,7 +27,8 @@ OperationModeNode::OperationModeNode(const rclcpp::NodeOptions & options) : Node { const auto adaptor = component_interface_utils::NodeAdaptor(this); group_cli_ = create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); - // adaptor.relay_message(pub_state_, sub_state_); + adaptor.init_sub(sub_state_, this, &OperationModeNode::on_state); + adaptor.init_pub(pub_state_); adaptor.init_srv(srv_stop_mode_, this, &OperationModeNode::on_change_to_stop); adaptor.init_srv(srv_autonomous_mode_, this, &OperationModeNode::on_change_to_autonomous); adaptor.init_srv(srv_local_mode_, this, &OperationModeNode::on_change_to_local); @@ -92,6 +93,11 @@ void OperationModeNode::on_disable_autoware_control( res->status = cli_control_->call(req)->status; } +void OperationModeNode::on_state(const OperationModeState::Message::ConstSharedPtr msg) +{ + pub_state_->publish(*msg); +} + } // namespace default_ad_api #include diff --git a/system/default_ad_api/src/operation_mode.hpp b/system/default_ad_api/src/operation_mode.hpp index 745dceef9dee1..8d141c66e7412 100644 --- a/system/default_ad_api/src/operation_mode.hpp +++ b/system/default_ad_api/src/operation_mode.hpp @@ -43,6 +43,7 @@ class OperationModeNode : public rclcpp::Node Cli cli_mode_; Cli cli_control_; + using OperationModeState = autoware_ad_api::operation_mode::OperationModeState; using ChangeToStop = autoware_ad_api::operation_mode::ChangeToStop; using ChangeToAutonomous = autoware_ad_api::operation_mode::ChangeToAutonomous; using ChangeToLocal = autoware_ad_api::operation_mode::ChangeToLocal; @@ -68,6 +69,8 @@ class OperationModeNode : public rclcpp::Node void on_disable_autoware_control( const DisableAutowareControl::Service::Request::SharedPtr req, const DisableAutowareControl::Service::Response::SharedPtr res); + + void on_state(const OperationModeState::Message::ConstSharedPtr msg); }; } // namespace default_ad_api From 053f249873acc19e6ffd62a6acc652da62e1d4db Mon Sep 17 00:00:00 2001 From: "Takagi, Isamu" Date: Fri, 12 Aug 2022 15:39:10 +0900 Subject: [PATCH 07/34] feat: add topic monitoring Signed-off-by: Takagi, Isamu --- system/default_ad_api/CMakeLists.txt | 2 +- .../default_ad_api/config/topic_monitor.csv | 14 +++++ .../launch/topic_monitor.launch.py | 55 +++++++++++++++++++ .../launch/topic_monitor.launch.xml | 7 +++ 4 files changed, 77 insertions(+), 1 deletion(-) create mode 100644 system/default_ad_api/config/topic_monitor.csv create mode 100644 system/default_ad_api/launch/topic_monitor.launch.py create mode 100644 system/default_ad_api/launch/topic_monitor.launch.xml diff --git a/system/default_ad_api/CMakeLists.txt b/system/default_ad_api/CMakeLists.txt index 5730ca9fa9cba..5b51cc71e064e 100644 --- a/system/default_ad_api/CMakeLists.txt +++ b/system/default_ad_api/CMakeLists.txt @@ -23,4 +23,4 @@ install( DESTINATION lib/${PROJECT_NAME} ) -ament_auto_package(INSTALL_TO_SHARE launch test) +ament_auto_package(INSTALL_TO_SHARE config launch test) diff --git a/system/default_ad_api/config/topic_monitor.csv b/system/default_ad_api/config/topic_monitor.csv new file mode 100644 index 0000000000000..220f86a8f256e --- /dev/null +++ b/system/default_ad_api/config/topic_monitor.csv @@ -0,0 +1,14 @@ +skip_psim,module,suffix,topic,topic_type,timeout,warn_rate,error_rate,transient_local,best_effort +,map,vector_map,/map/vector_map,autoware_auto_mapping_msgs/msg/HADMapBin,0.0,0.0,0.0,true,false +true,map,pointcloud_map,/map/pointcloud_map,sensor_msgs/msg/PointCloud2,0.0,0.0,0.0,true,false +,localization,initialpose3d,/initialpose3d,geometry_msgs/msg/PoseWithCovarianceStamped,0.0,0.0,0.0,false,false +true,localization,pose_twist_fusion_filter_pose,/localization/pose_twist_fusion_filter/pose,geometry_msgs/msg/PoseStamped,1.0,5.0,1.0,false,false +true,sensing,obstacle_segmentation_pointcloud,/perception/obstacle_segmentation/pointcloud,sensor_msgs/msg/PointCloud2,1.0,5.0,1.0,false,true +,perception,object_recognition_objects,/perception/object_recognition/objects,autoware_auto_perception_msgs/msg/PredictedObjects,1.0,5.0,1.0,false,false +,planning,mission_planning_route,/planning/mission_planning/route,autoware_auto_planning_msgs/msg/HADMapRoute,0.0,0.0,0.0,true,false +,planning,scenario_planning_trajectory,/planning/scenario_planning/trajectory,autoware_auto_planning_msgs/msg/Trajectory,1.0,5.0,1.0,false,false +,control,trajectory_follower_control_cmd,/control/trajectory_follower/control_cmd,autoware_auto_control_msgs/msg/AckermannControlCommand,1.0,5.0,1.0,false,false +,control,control_command_control_cmd,/control/command/control_cmd,autoware_auto_control_msgs/msg/AckermannControlCommand,1.0,5.0,1.0,false,false +,vehicle,vehicle_status_velocity_status,/vehicle/status/velocity_status,autoware_auto_vehicle_msgs/msg/VelocityReport,1.0,5.0,1.0,false,false +,vehicle,vehicle_status_steering_status,/vehicle/status/steering_status,autoware_auto_vehicle_msgs/msg/SteeringReport,1.0,5.0,1.0,false,false +,system,system_emergency_control_cmd,/system/emergency/control_cmd,autoware_auto_control_msgs/msg/AckermannControlCommand,1.0,5.0,1.0,false,false diff --git a/system/default_ad_api/launch/topic_monitor.launch.py b/system/default_ad_api/launch/topic_monitor.launch.py new file mode 100644 index 0000000000000..101b091803ace --- /dev/null +++ b/system/default_ad_api/launch/topic_monitor.launch.py @@ -0,0 +1,55 @@ +# Copyright 2022 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. + +import csv + +import launch +from launch.actions import DeclareLaunchArgument +from launch.actions import IncludeLaunchDescription +from launch.actions import OpaqueFunction +from launch.substitutions import LaunchConfiguration +from launch.substitutions import PathJoinSubstitution +from launch_ros.substitutions import FindPackageShare + + +def create_node(row): + package = FindPackageShare("topic_state_monitor") + include = PathJoinSubstitution([package, "launch/topic_state_monitor.launch.xml"]) + arguments = [ + ("diag_name", "default_ad_api/topic_monitor/" + row["module"]), + ("node_name_suffix", row["suffix"]), + ("topic", row["topic"]), + ("topic_type", row["topic_type"]), + ("timeout", row["timeout"]), + ("warn_rate", row["warn_rate"]), + ("error_rate", row["error_rate"]), + ("best_effort", row["best_effort"]), + ("transient_local", row["transient_local"]), + ] + return IncludeLaunchDescription(include, launch_arguments=arguments) + + +def launch_setup(context, *args, **kwargs): + with open(LaunchConfiguration("config_file").perform(context)) as fp: + nodes = [create_node(row) for row in csv.DictReader(fp)] + return [node for node in nodes if node] + + +def generate_launch_description(): + return launch.LaunchDescription( + [ + DeclareLaunchArgument("config_file"), + OpaqueFunction(function=launch_setup), + ] + ) diff --git a/system/default_ad_api/launch/topic_monitor.launch.xml b/system/default_ad_api/launch/topic_monitor.launch.xml new file mode 100644 index 0000000000000..80a47d39f5adf --- /dev/null +++ b/system/default_ad_api/launch/topic_monitor.launch.xml @@ -0,0 +1,7 @@ + + + + + + + From 1c79563a51991d8847207c264be916c839bd0eb9 Mon Sep 17 00:00:00 2001 From: "Takagi, Isamu" Date: Sat, 13 Aug 2022 02:05:22 +0900 Subject: [PATCH 08/34] feat: use topic monitoring Signed-off-by: Takagi, Isamu --- .../launch/autoware_api.launch.xml | 4 +- system/default_ad_api/CMakeLists.txt | 1 + .../default_ad_api/config/topic_monitor.csv | 28 +++---- .../launch/default_ad_api.launch.py | 65 +++++++++++++--- .../launch/topic_monitor.launch.py | 55 -------------- .../launch/topic_monitor.launch.xml | 7 -- system/default_ad_api/package.xml | 1 + system/default_ad_api/src/diagnostics.cpp | 63 ++++++++++++++++ system/default_ad_api/src/diagnostics.hpp | 44 +++++++++++ system/default_ad_api/src/operation_mode.cpp | 74 +++++++++++++++---- system/default_ad_api/src/operation_mode.hpp | 34 +++++++-- 11 files changed, 267 insertions(+), 109 deletions(-) delete mode 100644 system/default_ad_api/launch/topic_monitor.launch.py delete mode 100644 system/default_ad_api/launch/topic_monitor.launch.xml create mode 100644 system/default_ad_api/src/diagnostics.cpp create mode 100644 system/default_ad_api/src/diagnostics.hpp diff --git a/launch/tier4_autoware_api_launch/launch/autoware_api.launch.xml b/launch/tier4_autoware_api_launch/launch/autoware_api.launch.xml index a98ac01548732..c4ef8869456f9 100644 --- a/launch/tier4_autoware_api_launch/launch/autoware_api.launch.xml +++ b/launch/tier4_autoware_api_launch/launch/autoware_api.launch.xml @@ -9,7 +9,9 @@ - + + + diff --git a/system/default_ad_api/CMakeLists.txt b/system/default_ad_api/CMakeLists.txt index 5b51cc71e064e..1efdcf2556502 100644 --- a/system/default_ad_api/CMakeLists.txt +++ b/system/default_ad_api/CMakeLists.txt @@ -5,6 +5,7 @@ find_package(autoware_cmake REQUIRED) autoware_package() ament_auto_add_library(${PROJECT_NAME} SHARED + src/diagnostics.cpp src/interface.cpp src/operation_mode.cpp ) diff --git a/system/default_ad_api/config/topic_monitor.csv b/system/default_ad_api/config/topic_monitor.csv index 220f86a8f256e..aa1f2d510e5e4 100644 --- a/system/default_ad_api/config/topic_monitor.csv +++ b/system/default_ad_api/config/topic_monitor.csv @@ -1,14 +1,14 @@ -skip_psim,module,suffix,topic,topic_type,timeout,warn_rate,error_rate,transient_local,best_effort -,map,vector_map,/map/vector_map,autoware_auto_mapping_msgs/msg/HADMapBin,0.0,0.0,0.0,true,false -true,map,pointcloud_map,/map/pointcloud_map,sensor_msgs/msg/PointCloud2,0.0,0.0,0.0,true,false -,localization,initialpose3d,/initialpose3d,geometry_msgs/msg/PoseWithCovarianceStamped,0.0,0.0,0.0,false,false -true,localization,pose_twist_fusion_filter_pose,/localization/pose_twist_fusion_filter/pose,geometry_msgs/msg/PoseStamped,1.0,5.0,1.0,false,false -true,sensing,obstacle_segmentation_pointcloud,/perception/obstacle_segmentation/pointcloud,sensor_msgs/msg/PointCloud2,1.0,5.0,1.0,false,true -,perception,object_recognition_objects,/perception/object_recognition/objects,autoware_auto_perception_msgs/msg/PredictedObjects,1.0,5.0,1.0,false,false -,planning,mission_planning_route,/planning/mission_planning/route,autoware_auto_planning_msgs/msg/HADMapRoute,0.0,0.0,0.0,true,false -,planning,scenario_planning_trajectory,/planning/scenario_planning/trajectory,autoware_auto_planning_msgs/msg/Trajectory,1.0,5.0,1.0,false,false -,control,trajectory_follower_control_cmd,/control/trajectory_follower/control_cmd,autoware_auto_control_msgs/msg/AckermannControlCommand,1.0,5.0,1.0,false,false -,control,control_command_control_cmd,/control/command/control_cmd,autoware_auto_control_msgs/msg/AckermannControlCommand,1.0,5.0,1.0,false,false -,vehicle,vehicle_status_velocity_status,/vehicle/status/velocity_status,autoware_auto_vehicle_msgs/msg/VelocityReport,1.0,5.0,1.0,false,false -,vehicle,vehicle_status_steering_status,/vehicle/status/steering_status,autoware_auto_vehicle_msgs/msg/SteeringReport,1.0,5.0,1.0,false,false -,system,system_emergency_control_cmd,/system/emergency/control_cmd,autoware_auto_control_msgs/msg/AckermannControlCommand,1.0,5.0,1.0,false,false +suffix,topic,topic_type,timeout,warn_rate,error_rate,transient_local,best_effort +vector_map,/map/vector_map,autoware_auto_mapping_msgs/msg/HADMapBin,0.0,0.0,0.0,true,false +pointcloud_map,/map/pointcloud_map,sensor_msgs/msg/PointCloud2,0.0,0.0,0.0,true,false +initialpose3d,/initialpose3d,geometry_msgs/msg/PoseWithCovarianceStamped,0.0,0.0,0.0,false,false +pose_twist_fusion_filter_pose,/localization/pose_twist_fusion_filter/pose,geometry_msgs/msg/PoseStamped,1.0,5.0,1.0,false,false +obstacle_segmentation_pointcloud,/perception/obstacle_segmentation/pointcloud,sensor_msgs/msg/PointCloud2,1.0,5.0,1.0,false,true +object_recognition_objects,/perception/object_recognition/objects,autoware_auto_perception_msgs/msg/PredictedObjects,1.0,5.0,1.0,false,false +mission_planning_route,/planning/mission_planning/route,autoware_auto_planning_msgs/msg/HADMapRoute,0.0,0.0,0.0,true,false +scenario_planning_trajectory,/planning/scenario_planning/trajectory,autoware_auto_planning_msgs/msg/Trajectory,1.0,5.0,1.0,false,false +trajectory_follower_control_cmd,/control/trajectory_follower/control_cmd,autoware_auto_control_msgs/msg/AckermannControlCommand,1.0,5.0,1.0,false,false +control_command_control_cmd,/control/command/control_cmd,autoware_auto_control_msgs/msg/AckermannControlCommand,1.0,5.0,1.0,false,false +vehicle_status_velocity_status,/vehicle/status/velocity_status,autoware_auto_vehicle_msgs/msg/VelocityReport,1.0,5.0,1.0,false,false +vehicle_status_steering_status,/vehicle/status/steering_status,autoware_auto_vehicle_msgs/msg/SteeringReport,1.0,5.0,1.0,false,false +system_emergency_control_cmd,/system/emergency/control_cmd,autoware_auto_control_msgs/msg/AckermannControlCommand,1.0,5.0,1.0,false,false diff --git a/system/default_ad_api/launch/default_ad_api.launch.py b/system/default_ad_api/launch/default_ad_api.launch.py index ea88c20209655..a56bbfe884553 100644 --- a/system/default_ad_api/launch/default_ad_api.launch.py +++ b/system/default_ad_api/launch/default_ad_api.launch.py @@ -12,13 +12,42 @@ # See the License for the specific language governing permissions and # limitations under the License. +import csv + import launch +from launch.actions import DeclareLaunchArgument +from launch.actions import IncludeLaunchDescription +from launch.actions import OpaqueFunction +from launch.substitutions import LaunchConfiguration +from launch.substitutions import PathJoinSubstitution from launch_ros.actions import ComposableNodeContainer from launch_ros.actions import Node from launch_ros.descriptions import ComposableNode +from launch_ros.substitutions import FindPackageShare + + +def create_topic_monitor_name(row, diag_name): + return "topic_state_monitor_{}: {}".format(row["suffix"], diag_name) + + +def create_topic_monitor_node(row, diag_name): + package = FindPackageShare("topic_state_monitor") + include = PathJoinSubstitution([package, "launch/topic_state_monitor.launch.xml"]) + arguments = [ + ("diag_name", diag_name), + ("node_name_suffix", row["suffix"]), + ("topic", row["topic"]), + ("topic_type", row["topic_type"]), + ("timeout", row["timeout"]), + ("warn_rate", row["warn_rate"]), + ("error_rate", row["error_rate"]), + ("best_effort", row["best_effort"]), + ("transient_local", row["transient_local"]), + ] + return IncludeLaunchDescription(include, launch_arguments=arguments) -def _create_api_node(node_name, class_name, **kwargs): +def create_api_node(node_name, class_name, **kwargs): return ComposableNode( namespace="default_ad_api/node", name=node_name, @@ -28,10 +57,19 @@ def _create_api_node(node_name, class_name, **kwargs): ) -def generate_launch_description(): +def launch_setup(context, *args, **kwargs): + # create topic monitors + with open(LaunchConfiguration("config_file").perform(context)) as fp: + rows = list(csv.DictReader(fp)) + diag_name = "default_ad_api" + topic_monitor_nodes = [create_topic_monitor_node(row, diag_name) for row in rows] + topic_monitor_names = [create_topic_monitor_name(row, diag_name) for row in rows] + params_operation_mode = [{"topic_monitor_names": topic_monitor_names}] + + # create api components components = [ - _create_api_node("interface", "InterfaceNode"), - _create_api_node("operation_mode", "OperationModeNode"), + create_api_node("interface", "InterfaceNode"), + create_api_node("operation_mode", "OperationModeNode", parameters=params_operation_mode), ] container = ComposableNodeContainer( namespace="default_ad_api", @@ -40,9 +78,18 @@ def generate_launch_description(): executable="component_container_mt", composable_node_descriptions=components, ) - web_server = Node( - package="default_ad_api", - name="web_server", - executable="web_server.py", + return [container, *topic_monitor_nodes] + + +def generate_launch_description(): + return launch.LaunchDescription( + [ + DeclareLaunchArgument("config_file"), + OpaqueFunction(function=launch_setup), + Node( + package="default_ad_api", + name="web_server", + executable="web_server.py", + ), + ] ) - return launch.LaunchDescription([container, web_server]) diff --git a/system/default_ad_api/launch/topic_monitor.launch.py b/system/default_ad_api/launch/topic_monitor.launch.py deleted file mode 100644 index 101b091803ace..0000000000000 --- a/system/default_ad_api/launch/topic_monitor.launch.py +++ /dev/null @@ -1,55 +0,0 @@ -# Copyright 2022 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. - -import csv - -import launch -from launch.actions import DeclareLaunchArgument -from launch.actions import IncludeLaunchDescription -from launch.actions import OpaqueFunction -from launch.substitutions import LaunchConfiguration -from launch.substitutions import PathJoinSubstitution -from launch_ros.substitutions import FindPackageShare - - -def create_node(row): - package = FindPackageShare("topic_state_monitor") - include = PathJoinSubstitution([package, "launch/topic_state_monitor.launch.xml"]) - arguments = [ - ("diag_name", "default_ad_api/topic_monitor/" + row["module"]), - ("node_name_suffix", row["suffix"]), - ("topic", row["topic"]), - ("topic_type", row["topic_type"]), - ("timeout", row["timeout"]), - ("warn_rate", row["warn_rate"]), - ("error_rate", row["error_rate"]), - ("best_effort", row["best_effort"]), - ("transient_local", row["transient_local"]), - ] - return IncludeLaunchDescription(include, launch_arguments=arguments) - - -def launch_setup(context, *args, **kwargs): - with open(LaunchConfiguration("config_file").perform(context)) as fp: - nodes = [create_node(row) for row in csv.DictReader(fp)] - return [node for node in nodes if node] - - -def generate_launch_description(): - return launch.LaunchDescription( - [ - DeclareLaunchArgument("config_file"), - OpaqueFunction(function=launch_setup), - ] - ) diff --git a/system/default_ad_api/launch/topic_monitor.launch.xml b/system/default_ad_api/launch/topic_monitor.launch.xml deleted file mode 100644 index 80a47d39f5adf..0000000000000 --- a/system/default_ad_api/launch/topic_monitor.launch.xml +++ /dev/null @@ -1,7 +0,0 @@ - - - - - - - diff --git a/system/default_ad_api/package.xml b/system/default_ad_api/package.xml index c518f0392ebb3..f37e980506b02 100644 --- a/system/default_ad_api/package.xml +++ b/system/default_ad_api/package.xml @@ -15,6 +15,7 @@ autoware_ad_api_specs component_interface_specs component_interface_utils + diagnostic_msgs rclcpp rclcpp_components tier4_system_msgs diff --git a/system/default_ad_api/src/diagnostics.cpp b/system/default_ad_api/src/diagnostics.cpp new file mode 100644 index 0000000000000..88354a42ecd9f --- /dev/null +++ b/system/default_ad_api/src/diagnostics.cpp @@ -0,0 +1,63 @@ +// Copyright 2022 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 "diagnostics.hpp" + +#include +#include + +namespace default_ad_api +{ + +using diagnostic_msgs::msg::DiagnosticStatus; + +DiagnosticsMonitor::DiagnosticsMonitor(rclcpp::Node * node) +{ + const auto names = node->declare_parameter>("topic_monitor_names"); + for (const auto & name : names) { + levels_[name] = DiagnosticStatus::STALE; + } + + sub_diag_ = node->create_subscription( + "/diagnostics", 50, std::bind(&DiagnosticsMonitor::on_diag, this, std::placeholders::_1)); +} + +bool DiagnosticsMonitor::is_ok() +{ + for (const auto & level : levels_) { + if (level.second != DiagnosticStatus::OK) { + return false; + } + } + return true; +} + +void DiagnosticsMonitor::on_diag(const DiagnosticArray::ConstSharedPtr msg) +{ + const auto is_target = [](const std::string & name) { + const std::string suffix = "default_ad_api"; + if (name.size() < suffix.size()) return false; + return std::equal(suffix.rbegin(), suffix.rend(), name.rbegin()); + }; + + for (const auto & status : msg->status) { + if (status.hardware_id == "topic_state_monitor") { + if (is_target(status.name)) { + levels_[status.name] = status.level; + } + } + } +} + +} // namespace default_ad_api diff --git a/system/default_ad_api/src/diagnostics.hpp b/system/default_ad_api/src/diagnostics.hpp new file mode 100644 index 0000000000000..a2a628a587e80 --- /dev/null +++ b/system/default_ad_api/src/diagnostics.hpp @@ -0,0 +1,44 @@ +// Copyright 2022 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 DIAGNOSTICS_HPP_ +#define DIAGNOSTICS_HPP_ + +#include + +#include + +#include +#include + +namespace default_ad_api +{ + +class DiagnosticsMonitor +{ +public: + explicit DiagnosticsMonitor(rclcpp::Node * node); + bool is_ok(); + +private: + using DiagnosticArray = diagnostic_msgs::msg::DiagnosticArray; + using DiagnosticLevel = DiagnosticArray::_status_type::value_type::_level_type; + rclcpp::Subscription::SharedPtr sub_diag_; + std::unordered_map levels_; + void on_diag(const DiagnosticArray::ConstSharedPtr msg); +}; + +} // namespace default_ad_api + +#endif // DIAGNOSTICS_HPP_ diff --git a/system/default_ad_api/src/operation_mode.cpp b/system/default_ad_api/src/operation_mode.cpp index 21269bffd47e4..598031d48394b 100644 --- a/system/default_ad_api/src/operation_mode.cpp +++ b/system/default_ad_api/src/operation_mode.cpp @@ -20,10 +20,11 @@ namespace default_ad_api { using OperationModeRequest = system_interface::ChangeOperationMode::Service::Request; -using OperationMode = OperationModeRequest::_operation_type; using AutowareControlRequest = system_interface::ChangeAutowareControl::Service::Request; +using ServiceResponse = autoware_ad_api_msgs::srv::ChangeOperationMode::Response; -OperationModeNode::OperationModeNode(const rclcpp::NodeOptions & options) : Node("routing", options) +OperationModeNode::OperationModeNode(const rclcpp::NodeOptions & options) +: Node("operation_mode", options), diagnostics_(this) { const auto adaptor = component_interface_utils::NodeAdaptor(this); group_cli_ = create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); @@ -37,48 +38,67 @@ OperationModeNode::OperationModeNode(const rclcpp::NodeOptions & options) : Node adaptor.init_srv(srv_disable_control_, this, &OperationModeNode::on_disable_autoware_control); adaptor.init_cli(cli_mode_, group_cli_); adaptor.init_cli(cli_control_, group_cli_); + + timer_ = rclcpp::create_timer( + this, get_clock(), rclcpp::Rate(1.0).period(), std::bind(&OperationModeNode::on_timer, this)); + + curr_state_.mode.mode = OperationMode::UNKNOWN; + prev_state_.mode.mode = OperationMode::UNKNOWN; + mode_available_[OperationMode::UNKNOWN] = false; + mode_available_[OperationMode::STOP] = true; + mode_available_[OperationMode::AUTONOMOUS] = false; + mode_available_[OperationMode::LOCAL] = true; + mode_available_[OperationMode::REMOTE] = true; +} + +template +void OperationModeNode::change_mode(const ResponseT res, const OperationMode::_mode_type mode) +{ + if (!mode_available_[mode]) { + throw component_interface_utils::ServiceException( + ServiceResponse::ERROR_NOT_AVAILABLE, "The mode change condition is not satisfied."); + } + const auto req = std::make_shared(); + req->operation.mode = mode; + res->status = cli_mode_->call(req)->status; } void OperationModeNode::on_change_to_stop( const ChangeToStop::Service::Request::SharedPtr, const ChangeToStop::Service::Response::SharedPtr res) { - const auto req = std::make_shared(); - req->operation.mode = OperationMode::STOP; - res->status = cli_mode_->call(req)->status; + change_mode(res, OperationMode::STOP); } void OperationModeNode::on_change_to_autonomous( const ChangeToAutonomous::Service::Request::SharedPtr, const ChangeToAutonomous::Service::Response::SharedPtr res) { - const auto req = std::make_shared(); - req->operation.mode = OperationMode::AUTONOMOUS; - res->status = cli_mode_->call(req)->status; + change_mode(res, OperationMode::AUTONOMOUS); } void OperationModeNode::on_change_to_local( const ChangeToLocal::Service::Request::SharedPtr, const ChangeToLocal::Service::Response::SharedPtr res) { - const auto req = std::make_shared(); - req->operation.mode = OperationMode::LOCAL; - res->status = cli_mode_->call(req)->status; + change_mode(res, OperationMode::LOCAL); } void OperationModeNode::on_change_to_remote( const ChangeToRemote::Service::Request::SharedPtr, const ChangeToRemote::Service::Response::SharedPtr res) { - const auto req = std::make_shared(); - req->operation.mode = OperationMode::REMOTE; - res->status = cli_mode_->call(req)->status; + change_mode(res, OperationMode::REMOTE); } void OperationModeNode::on_enable_autoware_control( const EnableAutowareControl::Service::Request::SharedPtr, const EnableAutowareControl::Service::Response::SharedPtr res) { + if (!mode_available_[curr_state_.mode.mode]) { + throw component_interface_utils::ServiceException( + ServiceResponse::ERROR_NOT_AVAILABLE, "The mode change condition is not satisfied."); + } const auto req = std::make_shared(); req->autoware_control = true; res->status = cli_control_->call(req)->status; @@ -95,7 +115,31 @@ void OperationModeNode::on_disable_autoware_control( void OperationModeNode::on_state(const OperationModeState::Message::ConstSharedPtr msg) { - pub_state_->publish(*msg); + curr_state_ = *msg; + update_state(); +} + +void OperationModeNode::on_timer() +{ + mode_available_[OperationMode::AUTONOMOUS] = diagnostics_.is_ok(); + update_state(); +} + +void OperationModeNode::update_state() +{ + // Clear stamp to compare other fields. + OperationModeState::Message state = curr_state_; + state.stamp = builtin_interfaces::msg::Time(); + state.is_stop_mode_available &= mode_available_[OperationMode::STOP]; + state.is_autonomous_mode_available &= mode_available_[OperationMode::AUTONOMOUS]; + state.is_local_mode_available &= mode_available_[OperationMode::LOCAL]; + state.is_remote_mode_available &= mode_available_[OperationMode::REMOTE]; + + if (prev_state_ != state) { + prev_state_ = state; + state.stamp = now(); + pub_state_->publish(state); + } } } // namespace default_ad_api diff --git a/system/default_ad_api/src/operation_mode.hpp b/system/default_ad_api/src/operation_mode.hpp index 8d141c66e7412..e1edd4b1a70bd 100644 --- a/system/default_ad_api/src/operation_mode.hpp +++ b/system/default_ad_api/src/operation_mode.hpp @@ -15,23 +15,44 @@ #ifndef OPERATION_MODE_HPP_ #define OPERATION_MODE_HPP_ +#include "diagnostics.hpp" + #include #include #include +#include + // This file should be included after messages. #include "utils/types.hpp" namespace default_ad_api { +class DiagnosticsMonitor; + class OperationModeNode : public rclcpp::Node { public: explicit OperationModeNode(const rclcpp::NodeOptions & options); private: + using OperationModeState = autoware_ad_api::operation_mode::OperationModeState; + using OperationMode = OperationModeState::Message::_mode_type; + using ChangeToStop = autoware_ad_api::operation_mode::ChangeToStop; + using ChangeToAutonomous = autoware_ad_api::operation_mode::ChangeToAutonomous; + using ChangeToLocal = autoware_ad_api::operation_mode::ChangeToLocal; + using ChangeToRemote = autoware_ad_api::operation_mode::ChangeToRemote; + using EnableAutowareControl = autoware_ad_api::operation_mode::EnableAutowareControl; + using DisableAutowareControl = autoware_ad_api::operation_mode::DisableAutowareControl; + + DiagnosticsMonitor diagnostics_; + OperationModeState::Message curr_state_; + OperationModeState::Message prev_state_; + std::unordered_map mode_available_; + rclcpp::CallbackGroup::SharedPtr group_cli_; + rclcpp::TimerBase::SharedPtr timer_; Pub pub_state_; Srv srv_stop_mode_; Srv srv_autonomous_mode_; @@ -43,14 +64,6 @@ class OperationModeNode : public rclcpp::Node Cli cli_mode_; Cli cli_control_; - using OperationModeState = autoware_ad_api::operation_mode::OperationModeState; - using ChangeToStop = autoware_ad_api::operation_mode::ChangeToStop; - using ChangeToAutonomous = autoware_ad_api::operation_mode::ChangeToAutonomous; - using ChangeToLocal = autoware_ad_api::operation_mode::ChangeToLocal; - using ChangeToRemote = autoware_ad_api::operation_mode::ChangeToRemote; - using EnableAutowareControl = autoware_ad_api::operation_mode::EnableAutowareControl; - using DisableAutowareControl = autoware_ad_api::operation_mode::DisableAutowareControl; - void on_change_to_stop( const ChangeToStop::Service::Request::SharedPtr req, const ChangeToStop::Service::Response::SharedPtr res); @@ -71,6 +84,11 @@ class OperationModeNode : public rclcpp::Node const DisableAutowareControl::Service::Response::SharedPtr res); void on_state(const OperationModeState::Message::ConstSharedPtr msg); + void on_timer(); + void update_state(); + + template + void change_mode(const ResponseT res, const OperationMode::_mode_type mode); }; } // namespace default_ad_api From 92a1eb33afddfd057071a121749af704498ac938 Mon Sep 17 00:00:00 2001 From: "Takagi, Isamu" Date: Mon, 15 Aug 2022 16:23:25 +0900 Subject: [PATCH 09/34] feat: modify topic monitoring config Signed-off-by: Takagi, Isamu --- .../launch/autoware_api.launch.xml | 6 + .../launch/system.launch.xml | 13 -- .../default_ad_api/config/topic_monitor.csv | 14 -- .../default_ad_api/config/topic_monitor.yaml | 154 ++++++++++++++++++ .../launch/default_ad_api.launch.py | 39 ++--- 5 files changed, 177 insertions(+), 49 deletions(-) delete mode 100644 system/default_ad_api/config/topic_monitor.csv create mode 100644 system/default_ad_api/config/topic_monitor.yaml diff --git a/launch/tier4_autoware_api_launch/launch/autoware_api.launch.xml b/launch/tier4_autoware_api_launch/launch/autoware_api.launch.xml index c4ef8869456f9..2064f612f08cb 100644 --- a/launch/tier4_autoware_api_launch/launch/autoware_api.launch.xml +++ b/launch/tier4_autoware_api_launch/launch/autoware_api.launch.xml @@ -1,4 +1,5 @@ + @@ -7,9 +8,14 @@ + + + + + diff --git a/launch/tier4_system_launch/launch/system.launch.xml b/launch/tier4_system_launch/launch/system.launch.xml index 5e7153a09b11a..28c4c81a07fd9 100644 --- a/launch/tier4_system_launch/launch/system.launch.xml +++ b/launch/tier4_system_launch/launch/system.launch.xml @@ -23,19 +23,6 @@ - - - - - - - - - diff --git a/system/default_ad_api/config/topic_monitor.csv b/system/default_ad_api/config/topic_monitor.csv deleted file mode 100644 index aa1f2d510e5e4..0000000000000 --- a/system/default_ad_api/config/topic_monitor.csv +++ /dev/null @@ -1,14 +0,0 @@ -suffix,topic,topic_type,timeout,warn_rate,error_rate,transient_local,best_effort -vector_map,/map/vector_map,autoware_auto_mapping_msgs/msg/HADMapBin,0.0,0.0,0.0,true,false -pointcloud_map,/map/pointcloud_map,sensor_msgs/msg/PointCloud2,0.0,0.0,0.0,true,false -initialpose3d,/initialpose3d,geometry_msgs/msg/PoseWithCovarianceStamped,0.0,0.0,0.0,false,false -pose_twist_fusion_filter_pose,/localization/pose_twist_fusion_filter/pose,geometry_msgs/msg/PoseStamped,1.0,5.0,1.0,false,false -obstacle_segmentation_pointcloud,/perception/obstacle_segmentation/pointcloud,sensor_msgs/msg/PointCloud2,1.0,5.0,1.0,false,true -object_recognition_objects,/perception/object_recognition/objects,autoware_auto_perception_msgs/msg/PredictedObjects,1.0,5.0,1.0,false,false -mission_planning_route,/planning/mission_planning/route,autoware_auto_planning_msgs/msg/HADMapRoute,0.0,0.0,0.0,true,false -scenario_planning_trajectory,/planning/scenario_planning/trajectory,autoware_auto_planning_msgs/msg/Trajectory,1.0,5.0,1.0,false,false -trajectory_follower_control_cmd,/control/trajectory_follower/control_cmd,autoware_auto_control_msgs/msg/AckermannControlCommand,1.0,5.0,1.0,false,false -control_command_control_cmd,/control/command/control_cmd,autoware_auto_control_msgs/msg/AckermannControlCommand,1.0,5.0,1.0,false,false -vehicle_status_velocity_status,/vehicle/status/velocity_status,autoware_auto_vehicle_msgs/msg/VelocityReport,1.0,5.0,1.0,false,false -vehicle_status_steering_status,/vehicle/status/steering_status,autoware_auto_vehicle_msgs/msg/SteeringReport,1.0,5.0,1.0,false,false -system_emergency_control_cmd,/system/emergency/control_cmd,autoware_auto_control_msgs/msg/AckermannControlCommand,1.0,5.0,1.0,false,false diff --git a/system/default_ad_api/config/topic_monitor.yaml b/system/default_ad_api/config/topic_monitor.yaml new file mode 100644 index 0000000000000..83cabaaa077be --- /dev/null +++ b/system/default_ad_api/config/topic_monitor.yaml @@ -0,0 +1,154 @@ +- only_online_mode: false + args: + node_name_suffix: vector_map + topic: /map/vector_map + topic_type: autoware_auto_mapping_msgs/msg/HADMapBin + best_effort: false + transient_local: true + warn_rate: 0.0 + error_rate: 0.0 + timeout: 0.0 + +- only_online_mode: true + args: + node_name_suffix: pointcloud_map + topic: /map/pointcloud_map + topic_type: sensor_msgs/msg/PointCloud2 + best_effort: false + transient_local: true + warn_rate: 0.0 + error_rate: 0.0 + timeout: 0.0 + +- only_online_mode: false + args: + node_name_suffix: initialpose3d + topic: /initialpose3d + topic_type: geometry_msgs/msg/PoseWithCovarianceStamped + best_effort: false + transient_local: false + warn_rate: 0.0 + error_rate: 0.0 + timeout: 0.0 + +- only_online_mode: true + args: + node_name_suffix: pose_twist_fusion_filter_pose + topic: /localization/pose_twist_fusion_filter/pose + topic_type: geometry_msgs/msg/PoseStamped + best_effort: false + transient_local: false + warn_rate: 5.0 + error_rate: 1.0 + timeout: 1.0 + +- only_online_mode: true + args: + node_name_suffix: obstacle_segmentation_pointcloud + topic: /perception/obstacle_segmentation/pointcloud + topic_type: sensor_msgs/msg/PointCloud2 + best_effort: true + transient_local: false + warn_rate: 5.0 + error_rate: 1.0 + timeout: 1.0 + +- only_online_mode: false + args: + node_name_suffix: object_recognition_objects + topic: /perception/object_recognition/objects + topic_type: autoware_auto_perception_msgs/msg/PredictedObjects + best_effort: false + transient_local: false + warn_rate: 5.0 + error_rate: 1.0 + timeout: 1.0 + +- only_online_mode: false + args: + node_name_suffix: mission_planning_route + topic: /planning/mission_planning/route + topic_type: autoware_auto_planning_msgs/msg/HADMapRoute + best_effort: false + transient_local: true + warn_rate: 0.0 + error_rate: 0.0 + timeout: 0.0 + +- only_online_mode: false + args: + node_name_suffix: scenario_planning_trajectory + topic: /planning/scenario_planning/trajectory + topic_type: autoware_auto_planning_msgs/msg/Trajectory + best_effort: false + transient_local: false + warn_rate: 5.0 + error_rate: 1.0 + timeout: 1.0 + +- only_online_mode: false + args: + node_name_suffix: trajectory_follower_control_cmd + topic: /control/trajectory_follower/control_cmd + topic_type: autoware_auto_control_msgs/msg/AckermannControlCommand + best_effort: false + transient_local: false + warn_rate: 5.0 + error_rate: 1.0 + timeout: 1.0 + +- only_online_mode: false + args: + node_name_suffix: control_command_control_cmd + topic: /control/command/control_cmd + topic_type: autoware_auto_control_msgs/msg/AckermannControlCommand + best_effort: false + transient_local: false + warn_rate: 5.0 + error_rate: 1.0 + timeout: 1.0 + +- only_online_mode: false + args: + node_name_suffix: vehicle_status_velocity_status + topic: /vehicle/status/velocity_status + topic_type: autoware_auto_vehicle_msgs/msg/VelocityReport + best_effort: false + transient_local: false + warn_rate: 5.0 + error_rate: 1.0 + timeout: 1.0 + +- only_online_mode: false + args: + node_name_suffix: vehicle_status_steering_status + topic: /vehicle/status/steering_status + topic_type: autoware_auto_vehicle_msgs/msg/SteeringReport + best_effort: false + transient_local: false + warn_rate: 5.0 + error_rate: 1.0 + timeout: 1.0 + +- only_online_mode: false + args: + node_name_suffix: system_emergency_control_cmd + topic: /system/emergency/control_cmd + topic_type: autoware_auto_control_msgs/msg/AckermannControlCommand + best_effort: false + transient_local: false + warn_rate: 5.0 + error_rate: 1.0 + timeout: 1.0 + +- only_online_mode: false + args: + node_name_suffix: transform_map_to_base_link + topic: /tf + frame_id: map + child_frame_id: base_link + best_effort: false + transient_local: false + warn_rate: 5.0 + error_rate: 1.0 + timeout: 1.0 diff --git a/system/default_ad_api/launch/default_ad_api.launch.py b/system/default_ad_api/launch/default_ad_api.launch.py index a56bbfe884553..c7613aa1590c9 100644 --- a/system/default_ad_api/launch/default_ad_api.launch.py +++ b/system/default_ad_api/launch/default_ad_api.launch.py @@ -12,7 +12,8 @@ # See the License for the specific language governing permissions and # limitations under the License. -import csv + +from pathlib import Path import launch from launch.actions import DeclareLaunchArgument @@ -24,26 +25,18 @@ from launch_ros.actions import Node from launch_ros.descriptions import ComposableNode from launch_ros.substitutions import FindPackageShare +import yaml def create_topic_monitor_name(row, diag_name): - return "topic_state_monitor_{}: {}".format(row["suffix"], diag_name) + return "topic_state_monitor_{}: {}".format(row["args"]["node_name_suffix"], diag_name) def create_topic_monitor_node(row, diag_name): + tf_mode = "" if "topic_type" in row["args"] else "_tf" package = FindPackageShare("topic_state_monitor") - include = PathJoinSubstitution([package, "launch/topic_state_monitor.launch.xml"]) - arguments = [ - ("diag_name", diag_name), - ("node_name_suffix", row["suffix"]), - ("topic", row["topic"]), - ("topic_type", row["topic_type"]), - ("timeout", row["timeout"]), - ("warn_rate", row["warn_rate"]), - ("error_rate", row["error_rate"]), - ("best_effort", row["best_effort"]), - ("transient_local", row["transient_local"]), - ] + include = PathJoinSubstitution([package, f"launch/topic_state_monitor{tf_mode}.launch.xml"]) + arguments = [("diag_name", diag_name)] + [(k, str(v)) for k, v in row["args"].items()] return IncludeLaunchDescription(include, launch_arguments=arguments) @@ -53,23 +46,24 @@ def create_api_node(node_name, class_name, **kwargs): name=node_name, package="default_ad_api", plugin="default_ad_api::" + class_name, - **kwargs + **kwargs, ) def launch_setup(context, *args, **kwargs): # create topic monitors - with open(LaunchConfiguration("config_file").perform(context)) as fp: - rows = list(csv.DictReader(fp)) - diag_name = "default_ad_api" - topic_monitor_nodes = [create_topic_monitor_node(row, diag_name) for row in rows] - topic_monitor_names = [create_topic_monitor_name(row, diag_name) for row in rows] - params_operation_mode = [{"topic_monitor_names": topic_monitor_names}] + mode = LaunchConfiguration("online_mode").perform(context) + rows = yaml.safe_load(Path(LaunchConfiguration("config_file").perform(context)).read_text()) + rows = rows if mode else [row for row in rows if not rows["only_online_mode"]] + name = "default_ad_api" + topic_monitor_nodes = [create_topic_monitor_node(row, name) for row in rows] + topic_monitor_names = [create_topic_monitor_name(row, name) for row in rows] + param_operation_mode = [{"topic_monitor_names": topic_monitor_names}] # create api components components = [ create_api_node("interface", "InterfaceNode"), - create_api_node("operation_mode", "OperationModeNode", parameters=params_operation_mode), + create_api_node("operation_mode", "OperationModeNode", parameters=param_operation_mode), ] container = ComposableNodeContainer( namespace="default_ad_api", @@ -85,6 +79,7 @@ def generate_launch_description(): return launch.LaunchDescription( [ DeclareLaunchArgument("config_file"), + DeclareLaunchArgument("online_mode"), OpaqueFunction(function=launch_setup), Node( package="default_ad_api", From 4cd3726e01303a07bb162fbc88ec517cbaf8c234 Mon Sep 17 00:00:00 2001 From: "Takagi, Isamu" Date: Mon, 15 Aug 2022 16:34:37 +0900 Subject: [PATCH 10/34] fix: config name Signed-off-by: Takagi, Isamu --- launch/tier4_autoware_api_launch/launch/autoware_api.launch.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/launch/tier4_autoware_api_launch/launch/autoware_api.launch.xml b/launch/tier4_autoware_api_launch/launch/autoware_api.launch.xml index 2064f612f08cb..9a16a2594337f 100644 --- a/launch/tier4_autoware_api_launch/launch/autoware_api.launch.xml +++ b/launch/tier4_autoware_api_launch/launch/autoware_api.launch.xml @@ -16,7 +16,7 @@ - + From dc55a9e22385c78ee315221d66824bf86858adcf Mon Sep 17 00:00:00 2001 From: "Takagi, Isamu" Date: Tue, 16 Aug 2022 10:55:28 +0900 Subject: [PATCH 11/34] feat: modify diag name Signed-off-by: Takagi, Isamu --- .../default_ad_api/config/topic_monitor.yaml | 42 ++++++++++++------- .../launch/default_ad_api.launch.py | 15 ++++--- 2 files changed, 38 insertions(+), 19 deletions(-) diff --git a/system/default_ad_api/config/topic_monitor.yaml b/system/default_ad_api/config/topic_monitor.yaml index 83cabaaa077be..32c0d49eda0c9 100644 --- a/system/default_ad_api/config/topic_monitor.yaml +++ b/system/default_ad_api/config/topic_monitor.yaml @@ -1,4 +1,5 @@ -- only_online_mode: false +- module: map + only_online_mode: false args: node_name_suffix: vector_map topic: /map/vector_map @@ -9,7 +10,8 @@ error_rate: 0.0 timeout: 0.0 -- only_online_mode: true +- module: map + only_online_mode: true args: node_name_suffix: pointcloud_map topic: /map/pointcloud_map @@ -20,7 +22,8 @@ error_rate: 0.0 timeout: 0.0 -- only_online_mode: false +- module: localization + only_online_mode: false args: node_name_suffix: initialpose3d topic: /initialpose3d @@ -31,7 +34,8 @@ error_rate: 0.0 timeout: 0.0 -- only_online_mode: true +- module: localization + only_online_mode: true args: node_name_suffix: pose_twist_fusion_filter_pose topic: /localization/pose_twist_fusion_filter/pose @@ -42,7 +46,8 @@ error_rate: 1.0 timeout: 1.0 -- only_online_mode: true +- module: sensing + only_online_mode: true args: node_name_suffix: obstacle_segmentation_pointcloud topic: /perception/obstacle_segmentation/pointcloud @@ -53,7 +58,8 @@ error_rate: 1.0 timeout: 1.0 -- only_online_mode: false +- module: perception + only_online_mode: false args: node_name_suffix: object_recognition_objects topic: /perception/object_recognition/objects @@ -64,7 +70,8 @@ error_rate: 1.0 timeout: 1.0 -- only_online_mode: false +- module: planning + only_online_mode: false args: node_name_suffix: mission_planning_route topic: /planning/mission_planning/route @@ -75,7 +82,8 @@ error_rate: 0.0 timeout: 0.0 -- only_online_mode: false +- module: planning + only_online_mode: false args: node_name_suffix: scenario_planning_trajectory topic: /planning/scenario_planning/trajectory @@ -86,7 +94,8 @@ error_rate: 1.0 timeout: 1.0 -- only_online_mode: false +- module: control + only_online_mode: false args: node_name_suffix: trajectory_follower_control_cmd topic: /control/trajectory_follower/control_cmd @@ -97,7 +106,8 @@ error_rate: 1.0 timeout: 1.0 -- only_online_mode: false +- module: control + only_online_mode: false args: node_name_suffix: control_command_control_cmd topic: /control/command/control_cmd @@ -108,7 +118,8 @@ error_rate: 1.0 timeout: 1.0 -- only_online_mode: false +- module: vehicle + only_online_mode: false args: node_name_suffix: vehicle_status_velocity_status topic: /vehicle/status/velocity_status @@ -119,7 +130,8 @@ error_rate: 1.0 timeout: 1.0 -- only_online_mode: false +- module: vehicle + only_online_mode: false args: node_name_suffix: vehicle_status_steering_status topic: /vehicle/status/steering_status @@ -130,7 +142,8 @@ error_rate: 1.0 timeout: 1.0 -- only_online_mode: false +- module: system + only_online_mode: false args: node_name_suffix: system_emergency_control_cmd topic: /system/emergency/control_cmd @@ -141,7 +154,8 @@ error_rate: 1.0 timeout: 1.0 -- only_online_mode: false +- module: localization + only_online_mode: false args: node_name_suffix: transform_map_to_base_link topic: /tf diff --git a/system/default_ad_api/launch/default_ad_api.launch.py b/system/default_ad_api/launch/default_ad_api.launch.py index c7613aa1590c9..1f259de90921d 100644 --- a/system/default_ad_api/launch/default_ad_api.launch.py +++ b/system/default_ad_api/launch/default_ad_api.launch.py @@ -28,14 +28,20 @@ import yaml -def create_topic_monitor_name(row, diag_name): +def create_diagnostic_name(row): + return '"default_ad_api: {}_topic_status"'.format(row["module"]) + + +def create_topic_monitor_name(row): + diag_name = create_diagnostic_name(row) return "topic_state_monitor_{}: {}".format(row["args"]["node_name_suffix"], diag_name) -def create_topic_monitor_node(row, diag_name): +def create_topic_monitor_node(row): tf_mode = "" if "topic_type" in row["args"] else "_tf" package = FindPackageShare("topic_state_monitor") include = PathJoinSubstitution([package, f"launch/topic_state_monitor{tf_mode}.launch.xml"]) + diag_name = create_diagnostic_name(row) arguments = [("diag_name", diag_name)] + [(k, str(v)) for k, v in row["args"].items()] return IncludeLaunchDescription(include, launch_arguments=arguments) @@ -55,9 +61,8 @@ def launch_setup(context, *args, **kwargs): mode = LaunchConfiguration("online_mode").perform(context) rows = yaml.safe_load(Path(LaunchConfiguration("config_file").perform(context)).read_text()) rows = rows if mode else [row for row in rows if not rows["only_online_mode"]] - name = "default_ad_api" - topic_monitor_nodes = [create_topic_monitor_node(row, name) for row in rows] - topic_monitor_names = [create_topic_monitor_name(row, name) for row in rows] + topic_monitor_nodes = [create_topic_monitor_node(row) for row in rows] + topic_monitor_names = [create_topic_monitor_name(row) for row in rows] param_operation_mode = [{"topic_monitor_names": topic_monitor_names}] # create api components From 95324055d55d4f1a31983b016736b38694c51427 Mon Sep 17 00:00:00 2001 From: "Takagi, Isamu" Date: Wed, 21 Sep 2022 17:03:59 +0900 Subject: [PATCH 12/34] feat: move adapi message Signed-off-by: Takagi, Isamu --- common/autoware_ad_api_msgs/CMakeLists.txt | 29 ----------------- common/autoware_ad_api_msgs/README.md | 31 ------------------- .../common/msg/ResponseStatus.msg | 14 --------- .../interface/srv/InterfaceVersion.srv | 4 --- .../msg/LocalizationInitializationState.msg | 7 ----- .../srv/InitializeLocalization.srv | 7 ----- .../operation_mode/msg/OperationMode.msg | 9 ------ .../operation_mode/msg/OperationModeState.msg | 8 ----- .../srv/ChangeOperationMode.srv | 4 --- common/autoware_ad_api_msgs/package.xml | 30 ------------------ .../routing/msg/Route.msg | 2 -- .../routing/msg/RouteData.msg | 3 -- .../routing/msg/RoutePrimitive.msg | 2 -- .../routing/msg/RouteSegment.msg | 2 -- .../routing/msg/RouteState.msg | 8 ----- .../routing/srv/ClearRoute.srv | 2 -- .../routing/srv/SetRoute.srv | 6 ---- .../routing/srv/SetRoutePoints.srv | 8 ----- 18 files changed, 176 deletions(-) delete mode 100644 common/autoware_ad_api_msgs/CMakeLists.txt delete mode 100644 common/autoware_ad_api_msgs/README.md delete mode 100644 common/autoware_ad_api_msgs/common/msg/ResponseStatus.msg delete mode 100644 common/autoware_ad_api_msgs/interface/srv/InterfaceVersion.srv delete mode 100644 common/autoware_ad_api_msgs/localization/msg/LocalizationInitializationState.msg delete mode 100644 common/autoware_ad_api_msgs/localization/srv/InitializeLocalization.srv delete mode 100644 common/autoware_ad_api_msgs/operation_mode/msg/OperationMode.msg delete mode 100644 common/autoware_ad_api_msgs/operation_mode/msg/OperationModeState.msg delete mode 100644 common/autoware_ad_api_msgs/operation_mode/srv/ChangeOperationMode.srv delete mode 100644 common/autoware_ad_api_msgs/package.xml delete mode 100644 common/autoware_ad_api_msgs/routing/msg/Route.msg delete mode 100644 common/autoware_ad_api_msgs/routing/msg/RouteData.msg delete mode 100644 common/autoware_ad_api_msgs/routing/msg/RoutePrimitive.msg delete mode 100644 common/autoware_ad_api_msgs/routing/msg/RouteSegment.msg delete mode 100644 common/autoware_ad_api_msgs/routing/msg/RouteState.msg delete mode 100644 common/autoware_ad_api_msgs/routing/srv/ClearRoute.srv delete mode 100644 common/autoware_ad_api_msgs/routing/srv/SetRoute.srv delete mode 100644 common/autoware_ad_api_msgs/routing/srv/SetRoutePoints.srv diff --git a/common/autoware_ad_api_msgs/CMakeLists.txt b/common/autoware_ad_api_msgs/CMakeLists.txt deleted file mode 100644 index fe3b695f1f266..0000000000000 --- a/common/autoware_ad_api_msgs/CMakeLists.txt +++ /dev/null @@ -1,29 +0,0 @@ -cmake_minimum_required(VERSION 3.14) -project(autoware_ad_api_msgs) - -find_package(autoware_cmake REQUIRED) -autoware_package() - -rosidl_generate_interfaces(${PROJECT_NAME} - common/msg/ResponseStatus.msg - interface/srv/InterfaceVersion.srv - localization/msg/LocalizationInitializationState.msg - localization/srv/InitializeLocalization.srv - operation_mode/srv/ChangeOperationMode.srv - operation_mode/msg/OperationMode.msg - operation_mode/msg/OperationModeState.msg - routing/msg/RouteState.msg - routing/msg/Route.msg - routing/msg/RouteData.msg - routing/msg/RoutePrimitive.msg - routing/msg/RouteSegment.msg - routing/srv/ClearRoute.srv - routing/srv/SetRoute.srv - routing/srv/SetRoutePoints.srv - DEPENDENCIES - builtin_interfaces - std_msgs - geometry_msgs -) - -ament_auto_package() diff --git a/common/autoware_ad_api_msgs/README.md b/common/autoware_ad_api_msgs/README.md deleted file mode 100644 index 58ccb8bbf2d2b..0000000000000 --- a/common/autoware_ad_api_msgs/README.md +++ /dev/null @@ -1,31 +0,0 @@ -# autoware_ad_api_msgs - -## ResponseStatus - -This message is a response status commonly used in the service type API. Each API can define its own status codes. -The status codes are primarily used to indicate the error cause, such as invalid parameter and timeout. -If the API succeeds, set success to true, code to zero, and message to the empty string. -Alternatively, codes and messages can be used for warnings or additional information. -If the API fails, set success to false, code to the related status code, and message to the information. -The status code zero is reserved for success. The status code 50000 or over are also reserved for typical cases. - -| Name | Code | Description | -| ---------- | ----: | ------------------------------------ | -| SUCCESS | 0 | This API has completed successfully. | -| DEPRECATED | 50000 | This API is deprecated. | - -## InterfaceVersion - -Considering the product life cycle, there may be multiple vehicles using different versions of the AD API due to changes in requirements or functional improvements. For example, one vehicle uses `v1` for stability and another vehicle uses `v2` to enable more advanced functionality. - -In that situation, the AD API users such as developers of a web service have to switch the application behavior based on the version that each vehicle uses. -The version of AD API follows [Semantic Versioning][semver] in order to provide an intuitive understanding of the changes between versions. - -## Routing - -The routing service support two formats. One uses pose and the other uses map dependent data directly. -The body part of the route message is optional, since the route does not exist when it is cleared by the service. - - - -[semver]: https://semver.org/ diff --git a/common/autoware_ad_api_msgs/common/msg/ResponseStatus.msg b/common/autoware_ad_api_msgs/common/msg/ResponseStatus.msg deleted file mode 100644 index 65d6be71240ed..0000000000000 --- a/common/autoware_ad_api_msgs/common/msg/ResponseStatus.msg +++ /dev/null @@ -1,14 +0,0 @@ -# constants for code -uint16 DEPRECATED = 50000 -uint16 SERVICE_UNREADY = 50001 -uint16 SERVICE_TIMEOUT = 50002 -uint16 TRANSFORM_ERROR = 50003 -uint16 PARAMETER_ERROR = 50004 - -# constants for code -uint16 NO_EFFECT = 60001 - -# variables -bool success -uint16 code -string message diff --git a/common/autoware_ad_api_msgs/interface/srv/InterfaceVersion.srv b/common/autoware_ad_api_msgs/interface/srv/InterfaceVersion.srv deleted file mode 100644 index 93ba2680f1859..0000000000000 --- a/common/autoware_ad_api_msgs/interface/srv/InterfaceVersion.srv +++ /dev/null @@ -1,4 +0,0 @@ ---- -uint16 major -uint16 minor -uint16 patch diff --git a/common/autoware_ad_api_msgs/localization/msg/LocalizationInitializationState.msg b/common/autoware_ad_api_msgs/localization/msg/LocalizationInitializationState.msg deleted file mode 100644 index 8a4d790dbc321..0000000000000 --- a/common/autoware_ad_api_msgs/localization/msg/LocalizationInitializationState.msg +++ /dev/null @@ -1,7 +0,0 @@ -uint16 UNKNOWN = 0 -uint16 UNINITIALIZED = 1 -uint16 INITIALIZING = 2 -uint16 INITIALIZED = 3 - -builtin_interfaces/Time stamp -uint16 state diff --git a/common/autoware_ad_api_msgs/localization/srv/InitializeLocalization.srv b/common/autoware_ad_api_msgs/localization/srv/InitializeLocalization.srv deleted file mode 100644 index d3ffaece7789a..0000000000000 --- a/common/autoware_ad_api_msgs/localization/srv/InitializeLocalization.srv +++ /dev/null @@ -1,7 +0,0 @@ -geometry_msgs/PoseWithCovarianceStamped[<=1] pose ---- -uint16 ERROR_UNSAFE = 1 -uint16 ERROR_GNSS_SUPPORT = 2 -uint16 ERROR_GNSS = 3 -uint16 ERROR_ESTIMATION = 4 -autoware_ad_api_msgs/ResponseStatus status diff --git a/common/autoware_ad_api_msgs/operation_mode/msg/OperationMode.msg b/common/autoware_ad_api_msgs/operation_mode/msg/OperationMode.msg deleted file mode 100644 index 7d750bd71be70..0000000000000 --- a/common/autoware_ad_api_msgs/operation_mode/msg/OperationMode.msg +++ /dev/null @@ -1,9 +0,0 @@ -# constants for mode -uint16 UNKNOWN = 0 -uint16 STOP = 1 -uint16 AUTONOMOUS = 2 -uint16 LOCAL = 3 -uint16 REMOTE = 4 - -# variables -uint16 mode diff --git a/common/autoware_ad_api_msgs/operation_mode/msg/OperationModeState.msg b/common/autoware_ad_api_msgs/operation_mode/msg/OperationModeState.msg deleted file mode 100644 index 0ca9ff31be842..0000000000000 --- a/common/autoware_ad_api_msgs/operation_mode/msg/OperationModeState.msg +++ /dev/null @@ -1,8 +0,0 @@ -builtin_interfaces/Time stamp -autoware_ad_api_msgs/OperationMode mode -bool is_autoware_control_enabled -bool is_in_transition -bool is_stop_mode_available -bool is_autonomous_mode_available -bool is_local_mode_available -bool is_remote_mode_available diff --git a/common/autoware_ad_api_msgs/operation_mode/srv/ChangeOperationMode.srv b/common/autoware_ad_api_msgs/operation_mode/srv/ChangeOperationMode.srv deleted file mode 100644 index 2c0723843d927..0000000000000 --- a/common/autoware_ad_api_msgs/operation_mode/srv/ChangeOperationMode.srv +++ /dev/null @@ -1,4 +0,0 @@ ---- -uint16 ERROR_NOT_AVAILABLE = 1 -uint16 ERROR_IN_TRANSITION = 2 -autoware_ad_api_msgs/ResponseStatus status diff --git a/common/autoware_ad_api_msgs/package.xml b/common/autoware_ad_api_msgs/package.xml deleted file mode 100644 index db10bd2cdb877..0000000000000 --- a/common/autoware_ad_api_msgs/package.xml +++ /dev/null @@ -1,30 +0,0 @@ - - - - autoware_ad_api_msgs - 0.0.0 - The autoware_ad_api_msgs package - Takagi, Isamu - Apache License 2.0 - - ament_cmake_auto - - autoware_cmake - builtin_interfaces - rosidl_default_generators - - geometry_msgs - std_msgs - - builtin_interfaces - rosidl_default_runtime - - ament_lint_auto - autoware_lint_common - - rosidl_interface_packages - - - ament_cmake - - diff --git a/common/autoware_ad_api_msgs/routing/msg/Route.msg b/common/autoware_ad_api_msgs/routing/msg/Route.msg deleted file mode 100644 index 3e6ff3a06e18b..0000000000000 --- a/common/autoware_ad_api_msgs/routing/msg/Route.msg +++ /dev/null @@ -1,2 +0,0 @@ -std_msgs/Header header -autoware_ad_api_msgs/RouteData[<=1] data diff --git a/common/autoware_ad_api_msgs/routing/msg/RouteData.msg b/common/autoware_ad_api_msgs/routing/msg/RouteData.msg deleted file mode 100644 index e5d03efb14008..0000000000000 --- a/common/autoware_ad_api_msgs/routing/msg/RouteData.msg +++ /dev/null @@ -1,3 +0,0 @@ -geometry_msgs/Pose start -geometry_msgs/Pose goal -autoware_ad_api_msgs/RouteSegment[] segments diff --git a/common/autoware_ad_api_msgs/routing/msg/RoutePrimitive.msg b/common/autoware_ad_api_msgs/routing/msg/RoutePrimitive.msg deleted file mode 100644 index f7d9312611e51..0000000000000 --- a/common/autoware_ad_api_msgs/routing/msg/RoutePrimitive.msg +++ /dev/null @@ -1,2 +0,0 @@ -int64 id -string type # The same id may be used for each type. diff --git a/common/autoware_ad_api_msgs/routing/msg/RouteSegment.msg b/common/autoware_ad_api_msgs/routing/msg/RouteSegment.msg deleted file mode 100644 index a3f3d12d3872d..0000000000000 --- a/common/autoware_ad_api_msgs/routing/msg/RouteSegment.msg +++ /dev/null @@ -1,2 +0,0 @@ -autoware_ad_api_msgs/RoutePrimitive preferred -autoware_ad_api_msgs/RoutePrimitive[] alternatives # Does not include the preferred primitive. diff --git a/common/autoware_ad_api_msgs/routing/msg/RouteState.msg b/common/autoware_ad_api_msgs/routing/msg/RouteState.msg deleted file mode 100644 index 9dfe77ddee175..0000000000000 --- a/common/autoware_ad_api_msgs/routing/msg/RouteState.msg +++ /dev/null @@ -1,8 +0,0 @@ -uint16 UNKNOWN = 0 -uint16 UNSET = 1 -uint16 SET = 2 -uint16 ARRIVED = 3 -uint16 CHANGING = 4 - -builtin_interfaces/Time stamp -uint16 state diff --git a/common/autoware_ad_api_msgs/routing/srv/ClearRoute.srv b/common/autoware_ad_api_msgs/routing/srv/ClearRoute.srv deleted file mode 100644 index 38a3199e9d091..0000000000000 --- a/common/autoware_ad_api_msgs/routing/srv/ClearRoute.srv +++ /dev/null @@ -1,2 +0,0 @@ ---- -autoware_ad_api_msgs/ResponseStatus status diff --git a/common/autoware_ad_api_msgs/routing/srv/SetRoute.srv b/common/autoware_ad_api_msgs/routing/srv/SetRoute.srv deleted file mode 100644 index 7299b12fe8141..0000000000000 --- a/common/autoware_ad_api_msgs/routing/srv/SetRoute.srv +++ /dev/null @@ -1,6 +0,0 @@ -std_msgs/Header header -geometry_msgs/Pose goal -autoware_ad_api_msgs/RouteSegment[] segments ---- -uint16 ERROR_ROUTE_EXISTS = 1 -autoware_ad_api_msgs/ResponseStatus status diff --git a/common/autoware_ad_api_msgs/routing/srv/SetRoutePoints.srv b/common/autoware_ad_api_msgs/routing/srv/SetRoutePoints.srv deleted file mode 100644 index c05a211fae4fb..0000000000000 --- a/common/autoware_ad_api_msgs/routing/srv/SetRoutePoints.srv +++ /dev/null @@ -1,8 +0,0 @@ -std_msgs/Header header -geometry_msgs/Pose goal -geometry_msgs/Pose[] waypoints ---- -uint16 ERROR_ROUTE_EXISTS = 1 -uint16 ERROR_PLANNER_UNREADY = 2 -uint16 ERROR_PLANNER_FAILED = 3 -autoware_ad_api_msgs/ResponseStatus status From 871bdc411669de47ff82f2f2192b27911dee6f02 Mon Sep 17 00:00:00 2001 From: "Takagi, Isamu" Date: Wed, 21 Sep 2022 17:19:58 +0900 Subject: [PATCH 13/34] feat: change message type Signed-off-by: Takagi, Isamu --- .../autoware_ad_api_specs/operation_mode.hpp | 18 +++++++++--------- .../component_interface_specs/system.hpp | 4 ++-- 2 files changed, 11 insertions(+), 11 deletions(-) diff --git a/common/autoware_ad_api_specs/include/autoware_ad_api_specs/operation_mode.hpp b/common/autoware_ad_api_specs/include/autoware_ad_api_specs/operation_mode.hpp index 08528055d9966..f52b66cc88d2a 100644 --- a/common/autoware_ad_api_specs/include/autoware_ad_api_specs/operation_mode.hpp +++ b/common/autoware_ad_api_specs/include/autoware_ad_api_specs/operation_mode.hpp @@ -17,51 +17,51 @@ #include -#include -#include +#include +#include namespace autoware_ad_api::operation_mode { struct ChangeToStop { - using Service = autoware_ad_api_msgs::srv::ChangeOperationMode; + using Service = autoware_adapi_v1_msgs::srv::ChangeOperationMode; static constexpr char name[] = "/api/operation_mode/change_to_stop"; }; struct ChangeToAutonomous { - using Service = autoware_ad_api_msgs::srv::ChangeOperationMode; + using Service = autoware_adapi_v1_msgs::srv::ChangeOperationMode; static constexpr char name[] = "/api/operation_mode/change_to_autonomous"; }; struct ChangeToLocal { - using Service = autoware_ad_api_msgs::srv::ChangeOperationMode; + using Service = autoware_adapi_v1_msgs::srv::ChangeOperationMode; static constexpr char name[] = "/api/operation_mode/change_to_local"; }; struct ChangeToRemote { - using Service = autoware_ad_api_msgs::srv::ChangeOperationMode; + using Service = autoware_adapi_v1_msgs::srv::ChangeOperationMode; static constexpr char name[] = "/api/operation_mode/change_to_remote"; }; struct EnableAutowareControl { - using Service = autoware_ad_api_msgs::srv::ChangeOperationMode; + using Service = autoware_adapi_v1_msgs::srv::ChangeOperationMode; static constexpr char name[] = "/api/operation_mode/enable_autoware_control"; }; struct DisableAutowareControl { - using Service = autoware_ad_api_msgs::srv::ChangeOperationMode; + using Service = autoware_adapi_v1_msgs::srv::ChangeOperationMode; static constexpr char name[] = "/api/operation_mode/disable_autoware_control"; }; struct OperationModeState { - using Message = autoware_ad_api_msgs::msg::OperationModeState; + using Message = autoware_adapi_v1_msgs::msg::OperationModeState; static constexpr char name[] = "/api/operation_mode/state"; static constexpr size_t depth = 1; static constexpr auto reliability = RMW_QOS_POLICY_RELIABILITY_RELIABLE; diff --git a/common/component_interface_specs/include/component_interface_specs/system.hpp b/common/component_interface_specs/include/component_interface_specs/system.hpp index 2c82632d237f3..e7143d64fa05a 100644 --- a/common/component_interface_specs/include/component_interface_specs/system.hpp +++ b/common/component_interface_specs/include/component_interface_specs/system.hpp @@ -17,7 +17,7 @@ #include -#include +#include #include #include @@ -38,7 +38,7 @@ struct ChangeOperationMode struct OperationModeState { - using Message = autoware_ad_api_msgs::msg::OperationModeState; + using Message = autoware_adapi_v1_msgs::msg::OperationModeState; static constexpr char name[] = "/system/operation_mode/state"; static constexpr size_t depth = 1; static constexpr auto reliability = RMW_QOS_POLICY_RELIABILITY_RELIABLE; From 5ddbd5b12d1c05f482ad18a6a17a683d063f88e9 Mon Sep 17 00:00:00 2001 From: "Takagi, Isamu" Date: Wed, 5 Oct 2022 16:02:18 +0900 Subject: [PATCH 14/34] fix: merge Signed-off-by: Takagi, Isamu --- .../include/component_interface_utils/rclcpp.hpp | 10 ---------- 1 file changed, 10 deletions(-) diff --git a/common/component_interface_utils/include/component_interface_utils/rclcpp.hpp b/common/component_interface_utils/include/component_interface_utils/rclcpp.hpp index cf8718951bf70..283b99b6f34cc 100644 --- a/common/component_interface_utils/include/component_interface_utils/rclcpp.hpp +++ b/common/component_interface_utils/include/component_interface_utils/rclcpp.hpp @@ -106,16 +106,6 @@ class NodeAdaptor init_sub(sub, std::bind(callback, instance, _1)); } - /// Create a subscription wrapper. - template - void init_sub( - SharedPtrT & sub, InstanceT * instance, - MessageCallback && callback) const - { - using std::placeholders::_1; - init_sub(sub, std::bind(callback, instance, _1)); - } - /// Create a service wrapper for logging. template void init_srv( From b932bacfa22b0376d59c9fc26f26f337ffa3c238 Mon Sep 17 00:00:00 2001 From: "Takagi, Isamu" Date: Wed, 5 Oct 2022 16:23:59 +0900 Subject: [PATCH 15/34] WIP Signed-off-by: Takagi, Isamu --- system/default_ad_api/src/operation_mode.cpp | 5 ++--- system/default_ad_api/src/operation_mode.hpp | 9 +++++---- 2 files changed, 7 insertions(+), 7 deletions(-) diff --git a/system/default_ad_api/src/operation_mode.cpp b/system/default_ad_api/src/operation_mode.cpp index 598031d48394b..3e348637594e4 100644 --- a/system/default_ad_api/src/operation_mode.cpp +++ b/system/default_ad_api/src/operation_mode.cpp @@ -19,8 +19,6 @@ namespace default_ad_api { -using OperationModeRequest = system_interface::ChangeOperationMode::Service::Request; -using AutowareControlRequest = system_interface::ChangeAutowareControl::Service::Request; using ServiceResponse = autoware_ad_api_msgs::srv::ChangeOperationMode::Response; OperationModeNode::OperationModeNode(const rclcpp::NodeOptions & options) @@ -52,7 +50,8 @@ OperationModeNode::OperationModeNode(const rclcpp::NodeOptions & options) } template -void OperationModeNode::change_mode(const ResponseT res, const OperationMode::_mode_type mode) +void OperationModeNode::change_mode( + const ResponseT res, const OperationModeRequest::_mode_type mode) { if (!mode_available_[mode]) { throw component_interface_utils::ServiceException( diff --git a/system/default_ad_api/src/operation_mode.hpp b/system/default_ad_api/src/operation_mode.hpp index e1edd4b1a70bd..95dfb5d60813e 100644 --- a/system/default_ad_api/src/operation_mode.hpp +++ b/system/default_ad_api/src/operation_mode.hpp @@ -38,13 +38,14 @@ class OperationModeNode : public rclcpp::Node private: using OperationModeState = autoware_ad_api::operation_mode::OperationModeState; - using OperationMode = OperationModeState::Message::_mode_type; + using EnableAutowareControl = autoware_ad_api::operation_mode::EnableAutowareControl; + using DisableAutowareControl = autoware_ad_api::operation_mode::DisableAutowareControl; using ChangeToStop = autoware_ad_api::operation_mode::ChangeToStop; using ChangeToAutonomous = autoware_ad_api::operation_mode::ChangeToAutonomous; using ChangeToLocal = autoware_ad_api::operation_mode::ChangeToLocal; using ChangeToRemote = autoware_ad_api::operation_mode::ChangeToRemote; - using EnableAutowareControl = autoware_ad_api::operation_mode::EnableAutowareControl; - using DisableAutowareControl = autoware_ad_api::operation_mode::DisableAutowareControl; + using OperationModeRequest = system_interface::ChangeOperationMode::Service::Request; + using AutowareControlRequest = system_interface::ChangeAutowareControl::Service::Request; DiagnosticsMonitor diagnostics_; OperationModeState::Message curr_state_; @@ -88,7 +89,7 @@ class OperationModeNode : public rclcpp::Node void update_state(); template - void change_mode(const ResponseT res, const OperationMode::_mode_type mode); + void change_mode(const ResponseT res, const OperationModeRequest::_mode_type mode); }; } // namespace default_ad_api From cee50d23241fddf46ee307337c403163ef8837e6 Mon Sep 17 00:00:00 2001 From: "Takagi, Isamu" Date: Fri, 7 Oct 2022 19:56:21 +0900 Subject: [PATCH 16/34] fix: fix build error Signed-off-by: Takagi, Isamu --- .../include/autoware_ad_api_specs/motion.hpp | 8 ++-- system/default_ad_api/src/interface.hpp | 1 - system/default_ad_api/src/localization.hpp | 1 - system/default_ad_api/src/operation_mode.cpp | 44 +++++++++---------- system/default_ad_api/src/operation_mode.hpp | 3 +- 5 files changed, 28 insertions(+), 29 deletions(-) diff --git a/common/autoware_ad_api_specs/include/autoware_ad_api_specs/motion.hpp b/common/autoware_ad_api_specs/include/autoware_ad_api_specs/motion.hpp index dcec2a4670f04..041baa9a67051 100644 --- a/common/autoware_ad_api_specs/include/autoware_ad_api_specs/motion.hpp +++ b/common/autoware_ad_api_specs/include/autoware_ad_api_specs/motion.hpp @@ -17,21 +17,21 @@ #include -#include -#include +#include +#include namespace autoware_ad_api::motion { struct AcceptStart { - using Service = autoware_ad_api_msgs::srv::AcceptStart; + using Service = autoware_adapi_v1_msgs::srv::AcceptStart; static constexpr char name[] = "/api/motion/accept_start"; }; struct State { - using Message = autoware_ad_api_msgs::msg::MotionState; + using Message = autoware_adapi_v1_msgs::msg::MotionState; static constexpr char name[] = "/api/motion/state"; static constexpr size_t depth = 1; static constexpr auto reliability = RMW_QOS_POLICY_RELIABILITY_RELIABLE; diff --git a/system/default_ad_api/src/interface.hpp b/system/default_ad_api/src/interface.hpp index 074b1454f6e61..46c70abf17f36 100644 --- a/system/default_ad_api/src/interface.hpp +++ b/system/default_ad_api/src/interface.hpp @@ -16,7 +16,6 @@ #define INTERFACE_HPP_ #include -#include #include // This file should be included after messages. diff --git a/system/default_ad_api/src/localization.hpp b/system/default_ad_api/src/localization.hpp index 077c60d86df01..71517c4c6c769 100644 --- a/system/default_ad_api/src/localization.hpp +++ b/system/default_ad_api/src/localization.hpp @@ -17,7 +17,6 @@ #include #include -#include #include // This file should be included after messages. diff --git a/system/default_ad_api/src/operation_mode.cpp b/system/default_ad_api/src/operation_mode.cpp index 3e348637594e4..b4886477903a9 100644 --- a/system/default_ad_api/src/operation_mode.cpp +++ b/system/default_ad_api/src/operation_mode.cpp @@ -19,7 +19,7 @@ namespace default_ad_api { -using ServiceResponse = autoware_ad_api_msgs::srv::ChangeOperationMode::Response; +using ServiceResponse = autoware_adapi_v1_msgs::srv::ChangeOperationMode::Response; OperationModeNode::OperationModeNode(const rclcpp::NodeOptions & options) : Node("operation_mode", options), diagnostics_(this) @@ -40,13 +40,13 @@ OperationModeNode::OperationModeNode(const rclcpp::NodeOptions & options) timer_ = rclcpp::create_timer( this, get_clock(), rclcpp::Rate(1.0).period(), std::bind(&OperationModeNode::on_timer, this)); - curr_state_.mode.mode = OperationMode::UNKNOWN; - prev_state_.mode.mode = OperationMode::UNKNOWN; - mode_available_[OperationMode::UNKNOWN] = false; - mode_available_[OperationMode::STOP] = true; - mode_available_[OperationMode::AUTONOMOUS] = false; - mode_available_[OperationMode::LOCAL] = true; - mode_available_[OperationMode::REMOTE] = true; + curr_state_.mode = OperationModeState::Message::UNKNOWN; + prev_state_.mode = OperationModeState::Message::UNKNOWN; + mode_available_[OperationModeState::Message::UNKNOWN] = false; + mode_available_[OperationModeState::Message::STOP] = true; + mode_available_[OperationModeState::Message::AUTONOMOUS] = false; + mode_available_[OperationModeState::Message::LOCAL] = true; + mode_available_[OperationModeState::Message::REMOTE] = true; } template @@ -58,49 +58,49 @@ void OperationModeNode::change_mode( ServiceResponse::ERROR_NOT_AVAILABLE, "The mode change condition is not satisfied."); } const auto req = std::make_shared(); - req->operation.mode = mode; - res->status = cli_mode_->call(req)->status; + req->mode = mode; + component_interface_utils::status::copy(cli_mode_->call(req), res); // NOLINT } void OperationModeNode::on_change_to_stop( const ChangeToStop::Service::Request::SharedPtr, const ChangeToStop::Service::Response::SharedPtr res) { - change_mode(res, OperationMode::STOP); + change_mode(res, OperationModeRequest::STOP); } void OperationModeNode::on_change_to_autonomous( const ChangeToAutonomous::Service::Request::SharedPtr, const ChangeToAutonomous::Service::Response::SharedPtr res) { - change_mode(res, OperationMode::AUTONOMOUS); + change_mode(res, OperationModeRequest::AUTONOMOUS); } void OperationModeNode::on_change_to_local( const ChangeToLocal::Service::Request::SharedPtr, const ChangeToLocal::Service::Response::SharedPtr res) { - change_mode(res, OperationMode::LOCAL); + change_mode(res, OperationModeRequest::LOCAL); } void OperationModeNode::on_change_to_remote( const ChangeToRemote::Service::Request::SharedPtr, const ChangeToRemote::Service::Response::SharedPtr res) { - change_mode(res, OperationMode::REMOTE); + change_mode(res, OperationModeRequest::REMOTE); } void OperationModeNode::on_enable_autoware_control( const EnableAutowareControl::Service::Request::SharedPtr, const EnableAutowareControl::Service::Response::SharedPtr res) { - if (!mode_available_[curr_state_.mode.mode]) { + if (!mode_available_[curr_state_.mode]) { throw component_interface_utils::ServiceException( ServiceResponse::ERROR_NOT_AVAILABLE, "The mode change condition is not satisfied."); } const auto req = std::make_shared(); req->autoware_control = true; - res->status = cli_control_->call(req)->status; + component_interface_utils::status::copy(cli_control_->call(req), res); // NOLINT } void OperationModeNode::on_disable_autoware_control( @@ -109,7 +109,7 @@ void OperationModeNode::on_disable_autoware_control( { const auto req = std::make_shared(); req->autoware_control = false; - res->status = cli_control_->call(req)->status; + component_interface_utils::status::copy(cli_control_->call(req), res); // NOLINT } void OperationModeNode::on_state(const OperationModeState::Message::ConstSharedPtr msg) @@ -120,7 +120,7 @@ void OperationModeNode::on_state(const OperationModeState::Message::ConstSharedP void OperationModeNode::on_timer() { - mode_available_[OperationMode::AUTONOMOUS] = diagnostics_.is_ok(); + mode_available_[OperationModeState::Message::AUTONOMOUS] = diagnostics_.is_ok(); update_state(); } @@ -129,10 +129,10 @@ void OperationModeNode::update_state() // Clear stamp to compare other fields. OperationModeState::Message state = curr_state_; state.stamp = builtin_interfaces::msg::Time(); - state.is_stop_mode_available &= mode_available_[OperationMode::STOP]; - state.is_autonomous_mode_available &= mode_available_[OperationMode::AUTONOMOUS]; - state.is_local_mode_available &= mode_available_[OperationMode::LOCAL]; - state.is_remote_mode_available &= mode_available_[OperationMode::REMOTE]; + state.is_stop_mode_available &= mode_available_[OperationModeState::Message::STOP]; + state.is_autonomous_mode_available &= mode_available_[OperationModeState::Message::AUTONOMOUS]; + state.is_local_mode_available &= mode_available_[OperationModeState::Message::LOCAL]; + state.is_remote_mode_available &= mode_available_[OperationModeState::Message::REMOTE]; if (prev_state_ != state) { prev_state_ = state; diff --git a/system/default_ad_api/src/operation_mode.hpp b/system/default_ad_api/src/operation_mode.hpp index 95dfb5d60813e..1c56fd050d09e 100644 --- a/system/default_ad_api/src/operation_mode.hpp +++ b/system/default_ad_api/src/operation_mode.hpp @@ -19,6 +19,7 @@ #include #include +#include #include #include @@ -50,7 +51,7 @@ class OperationModeNode : public rclcpp::Node DiagnosticsMonitor diagnostics_; OperationModeState::Message curr_state_; OperationModeState::Message prev_state_; - std::unordered_map mode_available_; + std::unordered_map mode_available_; rclcpp::CallbackGroup::SharedPtr group_cli_; rclcpp::TimerBase::SharedPtr timer_; From f3885fb2585860ae60d5720493e0a5271d80052e Mon Sep 17 00:00:00 2001 From: "Takagi, Isamu" Date: Tue, 18 Oct 2022 11:24:59 +0900 Subject: [PATCH 17/34] feat: move diagnostics Signed-off-by: Takagi, Isamu --- .../default_ad_api/config/topic_monitor.yaml | 168 ------------------ system/default_ad_api/src/diagnostics.cpp | 63 ------- system/default_ad_api/src/diagnostics.hpp | 44 ----- 3 files changed, 275 deletions(-) delete mode 100644 system/default_ad_api/config/topic_monitor.yaml delete mode 100644 system/default_ad_api/src/diagnostics.cpp delete mode 100644 system/default_ad_api/src/diagnostics.hpp diff --git a/system/default_ad_api/config/topic_monitor.yaml b/system/default_ad_api/config/topic_monitor.yaml deleted file mode 100644 index 32c0d49eda0c9..0000000000000 --- a/system/default_ad_api/config/topic_monitor.yaml +++ /dev/null @@ -1,168 +0,0 @@ -- module: map - only_online_mode: false - args: - node_name_suffix: vector_map - topic: /map/vector_map - topic_type: autoware_auto_mapping_msgs/msg/HADMapBin - best_effort: false - transient_local: true - warn_rate: 0.0 - error_rate: 0.0 - timeout: 0.0 - -- module: map - only_online_mode: true - args: - node_name_suffix: pointcloud_map - topic: /map/pointcloud_map - topic_type: sensor_msgs/msg/PointCloud2 - best_effort: false - transient_local: true - warn_rate: 0.0 - error_rate: 0.0 - timeout: 0.0 - -- module: localization - only_online_mode: false - args: - node_name_suffix: initialpose3d - topic: /initialpose3d - topic_type: geometry_msgs/msg/PoseWithCovarianceStamped - best_effort: false - transient_local: false - warn_rate: 0.0 - error_rate: 0.0 - timeout: 0.0 - -- module: localization - only_online_mode: true - args: - node_name_suffix: pose_twist_fusion_filter_pose - topic: /localization/pose_twist_fusion_filter/pose - topic_type: geometry_msgs/msg/PoseStamped - best_effort: false - transient_local: false - warn_rate: 5.0 - error_rate: 1.0 - timeout: 1.0 - -- module: sensing - only_online_mode: true - args: - node_name_suffix: obstacle_segmentation_pointcloud - topic: /perception/obstacle_segmentation/pointcloud - topic_type: sensor_msgs/msg/PointCloud2 - best_effort: true - transient_local: false - warn_rate: 5.0 - error_rate: 1.0 - timeout: 1.0 - -- module: perception - only_online_mode: false - args: - node_name_suffix: object_recognition_objects - topic: /perception/object_recognition/objects - topic_type: autoware_auto_perception_msgs/msg/PredictedObjects - best_effort: false - transient_local: false - warn_rate: 5.0 - error_rate: 1.0 - timeout: 1.0 - -- module: planning - only_online_mode: false - args: - node_name_suffix: mission_planning_route - topic: /planning/mission_planning/route - topic_type: autoware_auto_planning_msgs/msg/HADMapRoute - best_effort: false - transient_local: true - warn_rate: 0.0 - error_rate: 0.0 - timeout: 0.0 - -- module: planning - only_online_mode: false - args: - node_name_suffix: scenario_planning_trajectory - topic: /planning/scenario_planning/trajectory - topic_type: autoware_auto_planning_msgs/msg/Trajectory - best_effort: false - transient_local: false - warn_rate: 5.0 - error_rate: 1.0 - timeout: 1.0 - -- module: control - only_online_mode: false - args: - node_name_suffix: trajectory_follower_control_cmd - topic: /control/trajectory_follower/control_cmd - topic_type: autoware_auto_control_msgs/msg/AckermannControlCommand - best_effort: false - transient_local: false - warn_rate: 5.0 - error_rate: 1.0 - timeout: 1.0 - -- module: control - only_online_mode: false - args: - node_name_suffix: control_command_control_cmd - topic: /control/command/control_cmd - topic_type: autoware_auto_control_msgs/msg/AckermannControlCommand - best_effort: false - transient_local: false - warn_rate: 5.0 - error_rate: 1.0 - timeout: 1.0 - -- module: vehicle - only_online_mode: false - args: - node_name_suffix: vehicle_status_velocity_status - topic: /vehicle/status/velocity_status - topic_type: autoware_auto_vehicle_msgs/msg/VelocityReport - best_effort: false - transient_local: false - warn_rate: 5.0 - error_rate: 1.0 - timeout: 1.0 - -- module: vehicle - only_online_mode: false - args: - node_name_suffix: vehicle_status_steering_status - topic: /vehicle/status/steering_status - topic_type: autoware_auto_vehicle_msgs/msg/SteeringReport - best_effort: false - transient_local: false - warn_rate: 5.0 - error_rate: 1.0 - timeout: 1.0 - -- module: system - only_online_mode: false - args: - node_name_suffix: system_emergency_control_cmd - topic: /system/emergency/control_cmd - topic_type: autoware_auto_control_msgs/msg/AckermannControlCommand - best_effort: false - transient_local: false - warn_rate: 5.0 - error_rate: 1.0 - timeout: 1.0 - -- module: localization - only_online_mode: false - args: - node_name_suffix: transform_map_to_base_link - topic: /tf - frame_id: map - child_frame_id: base_link - best_effort: false - transient_local: false - warn_rate: 5.0 - error_rate: 1.0 - timeout: 1.0 diff --git a/system/default_ad_api/src/diagnostics.cpp b/system/default_ad_api/src/diagnostics.cpp deleted file mode 100644 index 88354a42ecd9f..0000000000000 --- a/system/default_ad_api/src/diagnostics.cpp +++ /dev/null @@ -1,63 +0,0 @@ -// Copyright 2022 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 "diagnostics.hpp" - -#include -#include - -namespace default_ad_api -{ - -using diagnostic_msgs::msg::DiagnosticStatus; - -DiagnosticsMonitor::DiagnosticsMonitor(rclcpp::Node * node) -{ - const auto names = node->declare_parameter>("topic_monitor_names"); - for (const auto & name : names) { - levels_[name] = DiagnosticStatus::STALE; - } - - sub_diag_ = node->create_subscription( - "/diagnostics", 50, std::bind(&DiagnosticsMonitor::on_diag, this, std::placeholders::_1)); -} - -bool DiagnosticsMonitor::is_ok() -{ - for (const auto & level : levels_) { - if (level.second != DiagnosticStatus::OK) { - return false; - } - } - return true; -} - -void DiagnosticsMonitor::on_diag(const DiagnosticArray::ConstSharedPtr msg) -{ - const auto is_target = [](const std::string & name) { - const std::string suffix = "default_ad_api"; - if (name.size() < suffix.size()) return false; - return std::equal(suffix.rbegin(), suffix.rend(), name.rbegin()); - }; - - for (const auto & status : msg->status) { - if (status.hardware_id == "topic_state_monitor") { - if (is_target(status.name)) { - levels_[status.name] = status.level; - } - } - } -} - -} // namespace default_ad_api diff --git a/system/default_ad_api/src/diagnostics.hpp b/system/default_ad_api/src/diagnostics.hpp deleted file mode 100644 index a2a628a587e80..0000000000000 --- a/system/default_ad_api/src/diagnostics.hpp +++ /dev/null @@ -1,44 +0,0 @@ -// Copyright 2022 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 DIAGNOSTICS_HPP_ -#define DIAGNOSTICS_HPP_ - -#include - -#include - -#include -#include - -namespace default_ad_api -{ - -class DiagnosticsMonitor -{ -public: - explicit DiagnosticsMonitor(rclcpp::Node * node); - bool is_ok(); - -private: - using DiagnosticArray = diagnostic_msgs::msg::DiagnosticArray; - using DiagnosticLevel = DiagnosticArray::_status_type::value_type::_level_type; - rclcpp::Subscription::SharedPtr sub_diag_; - std::unordered_map levels_; - void on_diag(const DiagnosticArray::ConstSharedPtr msg); -}; - -} // namespace default_ad_api - -#endif // DIAGNOSTICS_HPP_ From 3e0e5b1d2fb4399bb4a7b128e12a22492204a87f Mon Sep 17 00:00:00 2001 From: "Takagi, Isamu" Date: Tue, 8 Nov 2022 20:01:06 +0900 Subject: [PATCH 18/34] feat: remove diagnostics Signed-off-by: Takagi, Isamu --- system/default_ad_api/CMakeLists.txt | 3 +- .../launch/default_ad_api.launch.py | 61 +++---------------- system/default_ad_api/src/operation_mode.cpp | 27 +++++++- system/default_ad_api/src/operation_mode.hpp | 14 +++-- 4 files changed, 40 insertions(+), 65 deletions(-) diff --git a/system/default_ad_api/CMakeLists.txt b/system/default_ad_api/CMakeLists.txt index 8e720de4eb786..bea6cd19c7a7e 100644 --- a/system/default_ad_api/CMakeLists.txt +++ b/system/default_ad_api/CMakeLists.txt @@ -5,7 +5,6 @@ find_package(autoware_cmake REQUIRED) autoware_package() ament_auto_add_library(${PROJECT_NAME} SHARED - src/diagnostics.cpp src/interface.cpp src/localization.cpp src/motion.cpp @@ -31,4 +30,4 @@ install( DESTINATION lib/${PROJECT_NAME} ) -ament_auto_package(INSTALL_TO_SHARE config launch test) +ament_auto_package(INSTALL_TO_SHARE launch test) diff --git a/system/default_ad_api/launch/default_ad_api.launch.py b/system/default_ad_api/launch/default_ad_api.launch.py index 96b4c21b1c1da..5f127894d17f9 100644 --- a/system/default_ad_api/launch/default_ad_api.launch.py +++ b/system/default_ad_api/launch/default_ad_api.launch.py @@ -12,38 +12,10 @@ # See the License for the specific language governing permissions and # limitations under the License. - -from pathlib import Path - import launch -from launch.actions import DeclareLaunchArgument -from launch.actions import IncludeLaunchDescription -from launch.actions import OpaqueFunction -from launch.substitutions import LaunchConfiguration -from launch.substitutions import PathJoinSubstitution from launch_ros.actions import ComposableNodeContainer from launch_ros.actions import Node from launch_ros.descriptions import ComposableNode -from launch_ros.substitutions import FindPackageShare -import yaml - - -def create_diagnostic_name(row): - return '"default_ad_api: {}_topic_status"'.format(row["module"]) - - -def create_topic_monitor_name(row): - diag_name = create_diagnostic_name(row) - return "topic_state_monitor_{}: {}".format(row["args"]["node_name_suffix"], diag_name) - - -def create_topic_monitor_node(row): - tf_mode = "" if "topic_type" in row["args"] else "_tf" - package = FindPackageShare("topic_state_monitor") - include = PathJoinSubstitution([package, f"launch/topic_state_monitor{tf_mode}.launch.xml"]) - diag_name = create_diagnostic_name(row) - arguments = [("diag_name", diag_name)] + [(k, str(v)) for k, v in row["args"].items()] - return IncludeLaunchDescription(include, launch_arguments=arguments) def create_api_node(node_name, class_name, **kwargs): @@ -56,21 +28,12 @@ def create_api_node(node_name, class_name, **kwargs): ) -def launch_setup(context, *args, **kwargs): - # create topic monitors - mode = LaunchConfiguration("online_mode").perform(context) - rows = yaml.safe_load(Path(LaunchConfiguration("config_file").perform(context)).read_text()) - rows = rows if mode else [row for row in rows if not rows["only_online_mode"]] - topic_monitor_nodes = [create_topic_monitor_node(row) for row in rows] - topic_monitor_names = [create_topic_monitor_name(row) for row in rows] - param_operation_mode = [{"topic_monitor_names": topic_monitor_names}] - - # create api components +def generate_launch_description(): components = [ create_api_node("interface", "InterfaceNode"), create_api_node("localization", "LocalizationNode"), create_api_node("motion", "MotionNode", parameters=[{"require_accept_start": False}]), - create_api_node("operation_mode", "OperationModeNode", parameters=param_operation_mode), + create_api_node("operation_mode", "OperationModeNode"), create_api_node("routing", "RoutingNode"), ] container = ComposableNodeContainer( @@ -80,19 +43,9 @@ def launch_setup(context, *args, **kwargs): executable="component_container_mt", composable_node_descriptions=components, ) - return [container, *topic_monitor_nodes] - - -def generate_launch_description(): - return launch.LaunchDescription( - [ - DeclareLaunchArgument("config_file"), - DeclareLaunchArgument("online_mode"), - OpaqueFunction(function=launch_setup), - Node( - package="default_ad_api", - name="web_server", - executable="web_server.py", - ), - ] + web_server = Node( + package="default_ad_api", + name="web_server", + executable="web_server.py", ) + return launch.LaunchDescription([container, web_server]) diff --git a/system/default_ad_api/src/operation_mode.cpp b/system/default_ad_api/src/operation_mode.cpp index b4886477903a9..2b95cb11c1b21 100644 --- a/system/default_ad_api/src/operation_mode.cpp +++ b/system/default_ad_api/src/operation_mode.cpp @@ -15,6 +15,8 @@ #include "operation_mode.hpp" #include +#include +#include namespace default_ad_api { @@ -22,7 +24,7 @@ namespace default_ad_api using ServiceResponse = autoware_adapi_v1_msgs::srv::ChangeOperationMode::Response; OperationModeNode::OperationModeNode(const rclcpp::NodeOptions & options) -: Node("operation_mode", options), diagnostics_(this) +: Node("operation_mode", options) { const auto adaptor = component_interface_utils::NodeAdaptor(this); group_cli_ = create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); @@ -37,8 +39,22 @@ OperationModeNode::OperationModeNode(const rclcpp::NodeOptions & options) adaptor.init_cli(cli_mode_, group_cli_); adaptor.init_cli(cli_control_, group_cli_); + const std::vector module_names = { + "sensing", "perception", "map", "localization", "planning", "control", "vehicle", "system", + }; + + for (size_t i = 0; i < module_names.size(); ++i) { + const auto name = "/system/component_state_monitor/component/autonomous/" + module_names[i]; + const auto qos = rclcpp::QoS(1).transient_local(); + const auto callback = [this, i](const ModeChangeAvailable::ConstSharedPtr msg) { + module_states_[i] = msg->available; + }; + sub_module_states_.push_back(create_subscription(name, qos, callback)); + } + module_states_.resize(module_names.size()); + timer_ = rclcpp::create_timer( - this, get_clock(), rclcpp::Rate(1.0).period(), std::bind(&OperationModeNode::on_timer, this)); + this, get_clock(), rclcpp::Rate(5.0).period(), std::bind(&OperationModeNode::on_timer, this)); curr_state_.mode = OperationModeState::Message::UNKNOWN; prev_state_.mode = OperationModeState::Message::UNKNOWN; @@ -120,7 +136,12 @@ void OperationModeNode::on_state(const OperationModeState::Message::ConstSharedP void OperationModeNode::on_timer() { - mode_available_[OperationModeState::Message::AUTONOMOUS] = diagnostics_.is_ok(); + bool autonomous_available = true; + for (const auto & state : module_states_) { + autonomous_available &= state; + } + mode_available_[OperationModeState::Message::AUTONOMOUS] = autonomous_available; + update_state(); } diff --git a/system/default_ad_api/src/operation_mode.hpp b/system/default_ad_api/src/operation_mode.hpp index 1c56fd050d09e..5f5699f42cab7 100644 --- a/system/default_ad_api/src/operation_mode.hpp +++ b/system/default_ad_api/src/operation_mode.hpp @@ -15,23 +15,22 @@ #ifndef OPERATION_MODE_HPP_ #define OPERATION_MODE_HPP_ -#include "diagnostics.hpp" - #include #include #include #include #include +#include + +// TODO(Takagi, Isamu): define interface +#include // This file should be included after messages. #include "utils/types.hpp" namespace default_ad_api { - -class DiagnosticsMonitor; - class OperationModeNode : public rclcpp::Node { public: @@ -47,8 +46,8 @@ class OperationModeNode : public rclcpp::Node using ChangeToRemote = autoware_ad_api::operation_mode::ChangeToRemote; using OperationModeRequest = system_interface::ChangeOperationMode::Service::Request; using AutowareControlRequest = system_interface::ChangeAutowareControl::Service::Request; + using ModeChangeAvailable = tier4_system_msgs::msg::ModeChangeAvailable; - DiagnosticsMonitor diagnostics_; OperationModeState::Message curr_state_; OperationModeState::Message prev_state_; std::unordered_map mode_available_; @@ -66,6 +65,9 @@ class OperationModeNode : public rclcpp::Node Cli cli_mode_; Cli cli_control_; + std::vector module_states_; + std::vector::SharedPtr> sub_module_states_; + void on_change_to_stop( const ChangeToStop::Service::Request::SharedPtr req, const ChangeToStop::Service::Response::SharedPtr res); From 51c9ddd74c7efbc91b5c54bb808aaa4a1a6ba722 Mon Sep 17 00:00:00 2001 From: "Takagi, Isamu" Date: Thu, 10 Nov 2022 21:27:49 +0900 Subject: [PATCH 19/34] feat: modify error message Signed-off-by: Takagi, Isamu --- system/default_ad_api/src/operation_mode.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/system/default_ad_api/src/operation_mode.cpp b/system/default_ad_api/src/operation_mode.cpp index 2b95cb11c1b21..a61a1b75697ab 100644 --- a/system/default_ad_api/src/operation_mode.cpp +++ b/system/default_ad_api/src/operation_mode.cpp @@ -71,7 +71,7 @@ void OperationModeNode::change_mode( { if (!mode_available_[mode]) { throw component_interface_utils::ServiceException( - ServiceResponse::ERROR_NOT_AVAILABLE, "The mode change condition is not satisfied."); + ServiceResponse::ERROR_NOT_AVAILABLE, "The mode change is blocked by the system."); } const auto req = std::make_shared(); req->mode = mode; @@ -112,7 +112,7 @@ void OperationModeNode::on_enable_autoware_control( { if (!mode_available_[curr_state_.mode]) { throw component_interface_utils::ServiceException( - ServiceResponse::ERROR_NOT_AVAILABLE, "The mode change condition is not satisfied."); + ServiceResponse::ERROR_NOT_AVAILABLE, "The mode change is blocked by the system."); } const auto req = std::make_shared(); req->autoware_control = true; From 0afa8ad4efc0b7eaa576fa899283b86afd1899c6 Mon Sep 17 00:00:00 2001 From: "Takagi, Isamu" Date: Thu, 10 Nov 2022 22:38:14 +0900 Subject: [PATCH 20/34] feat: remove unused code Signed-off-by: Takagi, Isamu --- .../launch/autoware_api.launch.xml | 6 ------ launch/tier4_system_launch/launch/system.launch.xml | 1 - system/default_ad_api/package.xml | 1 - 3 files changed, 8 deletions(-) diff --git a/launch/tier4_autoware_api_launch/launch/autoware_api.launch.xml b/launch/tier4_autoware_api_launch/launch/autoware_api.launch.xml index 9c00fb16a4077..efd9db8fdbb38 100644 --- a/launch/tier4_autoware_api_launch/launch/autoware_api.launch.xml +++ b/launch/tier4_autoware_api_launch/launch/autoware_api.launch.xml @@ -1,5 +1,4 @@ - @@ -8,11 +7,6 @@ - - - - - diff --git a/launch/tier4_system_launch/launch/system.launch.xml b/launch/tier4_system_launch/launch/system.launch.xml index 8d2a4bcf0648a..5ee6d27c05fef 100644 --- a/launch/tier4_system_launch/launch/system.launch.xml +++ b/launch/tier4_system_launch/launch/system.launch.xml @@ -23,7 +23,6 @@ - diff --git a/system/default_ad_api/package.xml b/system/default_ad_api/package.xml index db6b302bb1257..bc0d1edecb8dd 100644 --- a/system/default_ad_api/package.xml +++ b/system/default_ad_api/package.xml @@ -18,7 +18,6 @@ autoware_planning_msgs component_interface_specs component_interface_utils - diagnostic_msgs motion_utils rclcpp rclcpp_components From 1f49b3292c451281b645683986ae92701a7f7eaf Mon Sep 17 00:00:00 2001 From: "Takagi, Isamu" Date: Tue, 15 Nov 2022 13:55:20 +0900 Subject: [PATCH 21/34] feat(default_ad_api): add autoware state Signed-off-by: Takagi, Isamu --- system/default_ad_api/CMakeLists.txt | 2 + .../launch/default_ad_api.launch.py | 1 + system/default_ad_api/package.xml | 1 + .../src/compatibility/autoware_state.cpp | 103 ++++++++++++++++++ .../src/compatibility/autoware_state.hpp | 62 +++++++++++ 5 files changed, 169 insertions(+) create mode 100644 system/default_ad_api/src/compatibility/autoware_state.cpp create mode 100644 system/default_ad_api/src/compatibility/autoware_state.hpp diff --git a/system/default_ad_api/CMakeLists.txt b/system/default_ad_api/CMakeLists.txt index bea6cd19c7a7e..4d9997c775a1a 100644 --- a/system/default_ad_api/CMakeLists.txt +++ b/system/default_ad_api/CMakeLists.txt @@ -11,9 +11,11 @@ ament_auto_add_library(${PROJECT_NAME} SHARED src/operation_mode.cpp src/routing.cpp src/utils/route_conversion.cpp + src/compatibility/autoware_state.cpp ) rclcpp_components_register_nodes(${PROJECT_NAME} + "default_ad_api::AutowareStateNode" "default_ad_api::InterfaceNode" "default_ad_api::LocalizationNode" "default_ad_api::MotionNode" diff --git a/system/default_ad_api/launch/default_ad_api.launch.py b/system/default_ad_api/launch/default_ad_api.launch.py index 5f127894d17f9..ea0b6977fef41 100644 --- a/system/default_ad_api/launch/default_ad_api.launch.py +++ b/system/default_ad_api/launch/default_ad_api.launch.py @@ -30,6 +30,7 @@ def create_api_node(node_name, class_name, **kwargs): def generate_launch_description(): components = [ + create_api_node("autoware_state", "AutowareStateNode"), create_api_node("interface", "InterfaceNode"), create_api_node("localization", "LocalizationNode"), create_api_node("motion", "MotionNode", parameters=[{"require_accept_start": False}]), diff --git a/system/default_ad_api/package.xml b/system/default_ad_api/package.xml index bc0d1edecb8dd..44e5798fd61cc 100644 --- a/system/default_ad_api/package.xml +++ b/system/default_ad_api/package.xml @@ -15,6 +15,7 @@ autoware_adapi_v1_msgs autoware_adapi_version_msgs autoware_auto_planning_msgs + autoware_auto_system_msgs autoware_planning_msgs component_interface_specs component_interface_utils diff --git a/system/default_ad_api/src/compatibility/autoware_state.cpp b/system/default_ad_api/src/compatibility/autoware_state.cpp new file mode 100644 index 0000000000000..2ef49cfa40504 --- /dev/null +++ b/system/default_ad_api/src/compatibility/autoware_state.cpp @@ -0,0 +1,103 @@ +// Copyright 2022 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 "autoware_state.hpp" + +#include +#include + +namespace default_ad_api +{ + +AutowareStateNode::AutowareStateNode(const rclcpp::NodeOptions & options) +: Node("autoware_state", options) +{ + const std::vector module_names = { + "sensing", "perception", "map", "localization", "planning", "control", "vehicle", "system", + }; + + for (size_t i = 0; i < module_names.size(); ++i) { + const auto name = "/system/component_state_monitor/component/launch/" + module_names[i]; + const auto qos = rclcpp::QoS(1).transient_local(); + const auto callback = [this, i](const ModeChangeAvailable::ConstSharedPtr msg) { + launch_states_[i] = msg->available; + }; + sub_launch_states_.push_back(create_subscription(name, qos, callback)); + } + launch_states_.resize(module_names.size()); + + const auto adaptor = component_interface_utils::NodeAdaptor(this); + adaptor.init_sub(sub_localization_, [this](const LocalizationState::ConstSharedPtr msg) { + localization_ = *msg; + }); + // adaptor.init_sub(sub_routing_, on_interface_version); + // adaptor.init_sub(sub_operation_mode_, on_interface_version); + + const auto rate = rclcpp::Rate(1.0); + // const auto rate = rclcpp::Rate(5.0); + timer_ = rclcpp::create_timer(this, get_clock(), rate.period(), [this]() { on_timer(); }); + + launch_ = LaunchState::Initializing; + localization_.state = LocalizationState::UNKNOWN; + routing_.state = RoutingState::UNKNOWN; + operation_mode_.mode = OperationModeState::UNKNOWN; +} + +void AutowareStateNode::on_timer() +{ + using autoware_auto_system_msgs::msg::AutowareState; + + const auto convert_state = [this]() -> uint8_t { + if (launch_ == LaunchState::Initializing) { + return AutowareState::INITIALIZING; + } + if (launch_ == LaunchState::Finalizing) { + return AutowareState::FINALIZING; + } + if (localization_.state != LocalizationState::INITIALIZED) { + return AutowareState::INITIALIZING; + } + if (routing_.state == RoutingState::UNSET) { + return AutowareState::WAITING_FOR_ROUTE; + } + if (routing_.state == RoutingState::ARRIVED) { + return AutowareState::ARRIVED_GOAL; + } + if (operation_mode_.mode != OperationModeState::STOP) { + return AutowareState::DRIVING; + } + if (operation_mode_.is_autonomous_mode_available) { + return AutowareState::WAITING_FOR_ENGAGE; + } + return AutowareState::PLANNING; + }; + + if (launch_ == LaunchState::Initializing) { + bool is_initialized = true; + for (const auto & state : launch_states_) { + is_initialized &= state; + } + if (is_initialized) { + launch_ = LaunchState::Running; + } + } + + const auto state = convert_state(); + RCLCPP_INFO_STREAM(get_logger(), (int)state); +} + +} // namespace default_ad_api + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(default_ad_api::AutowareStateNode) diff --git a/system/default_ad_api/src/compatibility/autoware_state.hpp b/system/default_ad_api/src/compatibility/autoware_state.hpp new file mode 100644 index 0000000000000..45563a072f740 --- /dev/null +++ b/system/default_ad_api/src/compatibility/autoware_state.hpp @@ -0,0 +1,62 @@ +// Copyright 2022 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 COMPATIBILITY__AUTOWARE_STATE_HPP_ +#define COMPATIBILITY__AUTOWARE_STATE_HPP_ + +#include +#include +#include + +#include + +// TODO(Takagi, Isamu): define interface +#include +#include + +// This file should be included after messages. +#include "../utils/types.hpp" + +namespace default_ad_api +{ + +class AutowareStateNode : public rclcpp::Node +{ +public: + explicit AutowareStateNode(const rclcpp::NodeOptions & options); + +private: + using ModeChangeAvailable = tier4_system_msgs::msg::ModeChangeAvailable; + rclcpp::TimerBase::SharedPtr timer_; + // emergency + Sub sub_localization_; + Sub sub_routing_; + Sub sub_operation_mode_; + std::vector launch_states_; + std::vector::SharedPtr> sub_launch_states_; + + using LocalizationState = autoware_ad_api::localization::InitializationState::Message; + using RoutingState = autoware_ad_api::routing::RouteState::Message; + using OperationModeState = autoware_ad_api::operation_mode::OperationModeState::Message; + enum class LaunchState { Initializing, Running, Finalizing }; + LaunchState launch_; + LocalizationState localization_; + RoutingState routing_; + OperationModeState operation_mode_; + void on_timer(); +}; + +} // namespace default_ad_api + +#endif // COMPATIBILITY__AUTOWARE_STATE_HPP_ From b128bf17b1a709ff74050f21bdaca18517cc57da Mon Sep 17 00:00:00 2001 From: "Takagi, Isamu" Date: Wed, 16 Nov 2022 15:05:25 +0900 Subject: [PATCH 22/34] feat: reproduce old state Signed-off-by: Takagi, Isamu --- .../src/compatibility/autoware_state.cpp | 97 +++++++++++++------ .../src/compatibility/autoware_state.hpp | 19 ++-- 2 files changed, 78 insertions(+), 38 deletions(-) diff --git a/system/default_ad_api/src/compatibility/autoware_state.cpp b/system/default_ad_api/src/compatibility/autoware_state.cpp index 2ef49cfa40504..053eca44a7782 100644 --- a/system/default_ad_api/src/compatibility/autoware_state.cpp +++ b/system/default_ad_api/src/compatibility/autoware_state.cpp @@ -31,70 +31,103 @@ AutowareStateNode::AutowareStateNode(const rclcpp::NodeOptions & options) const auto name = "/system/component_state_monitor/component/launch/" + module_names[i]; const auto qos = rclcpp::QoS(1).transient_local(); const auto callback = [this, i](const ModeChangeAvailable::ConstSharedPtr msg) { - launch_states_[i] = msg->available; + component_states_[i] = msg->available; }; - sub_launch_states_.push_back(create_subscription(name, qos, callback)); + sub_component_states_.push_back(create_subscription(name, qos, callback)); } - launch_states_.resize(module_names.size()); + + pub_autoware_state_ = create_publisher("/autoware/state2", 1); const auto adaptor = component_interface_utils::NodeAdaptor(this); - adaptor.init_sub(sub_localization_, [this](const LocalizationState::ConstSharedPtr msg) { - localization_ = *msg; - }); - // adaptor.init_sub(sub_routing_, on_interface_version); - // adaptor.init_sub(sub_operation_mode_, on_interface_version); - - const auto rate = rclcpp::Rate(1.0); - // const auto rate = rclcpp::Rate(5.0); + adaptor.init_sub(sub_localization_, this, &AutowareStateNode::on_localization); + adaptor.init_sub(sub_routing_, this, &AutowareStateNode::on_routing); + adaptor.init_sub(sub_operation_mode_, this, &AutowareStateNode::on_operation_mode); + + // TODO(Takagi, Isamu): remove default value + const auto rate = rclcpp::Rate(declare_parameter("update_rate", 10.0)); timer_ = rclcpp::create_timer(this, get_clock(), rate.period(), [this]() { on_timer(); }); - launch_ = LaunchState::Initializing; - localization_.state = LocalizationState::UNKNOWN; - routing_.state = RoutingState::UNKNOWN; - operation_mode_.mode = OperationModeState::UNKNOWN; + component_states_.resize(module_names.size()); + launch_state_ = LaunchState::Initializing; + localization_state_.state = LocalizationState::UNKNOWN; + routing_state_.state = RoutingState::UNKNOWN; + operation_mode_state_.mode = OperationModeState::UNKNOWN; } -void AutowareStateNode::on_timer() +void AutowareStateNode::on_localization(const LocalizationState::ConstSharedPtr msg) { - using autoware_auto_system_msgs::msg::AutowareState; + localization_state_ = *msg; +} +void AutowareStateNode::on_routing(const RoutingState::ConstSharedPtr msg) +{ + routing_state_ = *msg; +} +void AutowareStateNode::on_operation_mode(const OperationModeState::ConstSharedPtr msg) +{ + operation_mode_state_ = *msg; +} - const auto convert_state = [this]() -> uint8_t { - if (launch_ == LaunchState::Initializing) { +void AutowareStateNode::on_timer() +{ + const auto convert_state = [this]() { + if (launch_state_ == LaunchState::Finalizing) { + return AutowareState::FINALIZING; + } + if (launch_state_ == LaunchState::Initializing) { return AutowareState::INITIALIZING; } - if (launch_ == LaunchState::Finalizing) { - return AutowareState::FINALIZING; + if (localization_state_.state == LocalizationState::UNKNOWN) { + return AutowareState::INITIALIZING; + } + if (routing_state_.state == RoutingState::UNKNOWN) { + return AutowareState::INITIALIZING; } - if (localization_.state != LocalizationState::INITIALIZED) { + if (operation_mode_state_.mode == OperationModeState::UNKNOWN) { return AutowareState::INITIALIZING; } - if (routing_.state == RoutingState::UNSET) { + if (localization_state_.state != LocalizationState::INITIALIZED) { + return AutowareState::INITIALIZING; + } + if (routing_state_.state == RoutingState::UNSET) { return AutowareState::WAITING_FOR_ROUTE; } - if (routing_.state == RoutingState::ARRIVED) { + if (routing_state_.state == RoutingState::ARRIVED) { return AutowareState::ARRIVED_GOAL; } - if (operation_mode_.mode != OperationModeState::STOP) { - return AutowareState::DRIVING; + if (operation_mode_state_.mode != OperationModeState::STOP) { + if (operation_mode_state_.is_autoware_control_enabled) { + return AutowareState::DRIVING; + } } - if (operation_mode_.is_autonomous_mode_available) { + if (operation_mode_state_.is_autonomous_mode_available) { return AutowareState::WAITING_FOR_ENGAGE; } return AutowareState::PLANNING; }; - if (launch_ == LaunchState::Initializing) { + // Update launch state. + if (launch_state_ == LaunchState::Initializing) { bool is_initialized = true; - for (const auto & state : launch_states_) { + for (const auto & state : component_states_) { is_initialized &= state; } if (is_initialized) { - launch_ = LaunchState::Running; + launch_state_ = LaunchState::Running; + } + } + + // Update routing state to reproduce old logic. + if (routing_state_.state == RoutingState::ARRIVED) { + const auto duration = now() - rclcpp::Time(routing_state_.stamp); + if (2.0 < duration.seconds()) { + routing_state_.state = RoutingState::UNSET; } } - const auto state = convert_state(); - RCLCPP_INFO_STREAM(get_logger(), (int)state); + AutowareState msg; + msg.stamp = now(); + msg.state = convert_state(); + pub_autoware_state_->publish(msg); } } // namespace default_ad_api diff --git a/system/default_ad_api/src/compatibility/autoware_state.hpp b/system/default_ad_api/src/compatibility/autoware_state.hpp index 45563a072f740..486681041ab4e 100644 --- a/system/default_ad_api/src/compatibility/autoware_state.hpp +++ b/system/default_ad_api/src/compatibility/autoware_state.hpp @@ -43,18 +43,25 @@ class AutowareStateNode : public rclcpp::Node Sub sub_localization_; Sub sub_routing_; Sub sub_operation_mode_; - std::vector launch_states_; - std::vector::SharedPtr> sub_launch_states_; + using AutowareState = autoware_auto_system_msgs::msg::AutowareState; using LocalizationState = autoware_ad_api::localization::InitializationState::Message; using RoutingState = autoware_ad_api::routing::RouteState::Message; using OperationModeState = autoware_ad_api::operation_mode::OperationModeState::Message; + std::vector component_states_; + std::vector::SharedPtr> sub_component_states_; + rclcpp::Publisher::SharedPtr pub_autoware_state_; + enum class LaunchState { Initializing, Running, Finalizing }; - LaunchState launch_; - LocalizationState localization_; - RoutingState routing_; - OperationModeState operation_mode_; + LaunchState launch_state_; + LocalizationState localization_state_; + RoutingState routing_state_; + OperationModeState operation_mode_state_; + void on_timer(); + void on_localization(const LocalizationState::ConstSharedPtr msg); + void on_routing(const RoutingState::ConstSharedPtr msg); + void on_operation_mode(const OperationModeState::ConstSharedPtr msg); }; } // namespace default_ad_api From bc14e782946091255ea7def5d351c5b6772f5cf5 Mon Sep 17 00:00:00 2001 From: "Takagi, Isamu" Date: Wed, 16 Nov 2022 16:29:50 +0900 Subject: [PATCH 23/34] feat: add shutdown service Signed-off-by: Takagi, Isamu --- .clang-format | 3 +++ .../launch/ad_service_state_monitor.launch.xml | 8 ++++---- system/default_ad_api/package.xml | 1 + .../src/compatibility/autoware_state.cpp | 13 ++++++++++++- .../src/compatibility/autoware_state.hpp | 9 ++++++--- 5 files changed, 26 insertions(+), 8 deletions(-) diff --git a/.clang-format b/.clang-format index 7762ec9dfb800..c3b72db71b4ca 100644 --- a/.clang-format +++ b/.clang-format @@ -33,6 +33,9 @@ IncludeCategories: - Regex: .*_msgs/.* Priority: 3 CaseSensitive: true + - Regex: .*_srvs/.* + Priority: 3 + CaseSensitive: true # Other Package headers - Regex: <.*> Priority: 2 diff --git a/system/ad_service_state_monitor/launch/ad_service_state_monitor.launch.xml b/system/ad_service_state_monitor/launch/ad_service_state_monitor.launch.xml index a7cbf13c4a14c..ea1cfcaa0f5af 100644 --- a/system/ad_service_state_monitor/launch/ad_service_state_monitor.launch.xml +++ b/system/ad_service_state_monitor/launch/ad_service_state_monitor.launch.xml @@ -8,12 +8,12 @@ - - + + - - + + diff --git a/system/default_ad_api/package.xml b/system/default_ad_api/package.xml index 44e5798fd61cc..b03b630996bf6 100644 --- a/system/default_ad_api/package.xml +++ b/system/default_ad_api/package.xml @@ -22,6 +22,7 @@ motion_utils rclcpp rclcpp_components + std_srvs tier4_control_msgs tier4_system_msgs diff --git a/system/default_ad_api/src/compatibility/autoware_state.cpp b/system/default_ad_api/src/compatibility/autoware_state.cpp index 053eca44a7782..5778edf66559f 100644 --- a/system/default_ad_api/src/compatibility/autoware_state.cpp +++ b/system/default_ad_api/src/compatibility/autoware_state.cpp @@ -36,7 +36,10 @@ AutowareStateNode::AutowareStateNode(const rclcpp::NodeOptions & options) sub_component_states_.push_back(create_subscription(name, qos, callback)); } - pub_autoware_state_ = create_publisher("/autoware/state2", 1); + pub_autoware_state_ = create_publisher("/autoware/state", 1); + srv_autoware_shutdown_ = create_service( + "/autoware/shutdown", + std::bind(&AutowareStateNode::on_shutdown, this, std::placeholders::_1, std::placeholders::_2)); const auto adaptor = component_interface_utils::NodeAdaptor(this); adaptor.init_sub(sub_localization_, this, &AutowareStateNode::on_localization); @@ -67,6 +70,14 @@ void AutowareStateNode::on_operation_mode(const OperationModeState::ConstSharedP operation_mode_state_ = *msg; } +void AutowareStateNode::on_shutdown( + const Trigger::Request::SharedPtr, const Trigger::Response::SharedPtr res) +{ + launch_state_ = LaunchState::Finalizing; + res->success = true; + res->message = "Shutdown Autoware."; +} + void AutowareStateNode::on_timer() { const auto convert_state = [this]() { diff --git a/system/default_ad_api/src/compatibility/autoware_state.hpp b/system/default_ad_api/src/compatibility/autoware_state.hpp index 486681041ab4e..4cd6233f5df0a 100644 --- a/system/default_ad_api/src/compatibility/autoware_state.hpp +++ b/system/default_ad_api/src/compatibility/autoware_state.hpp @@ -19,12 +19,12 @@ #include #include -#include - -// TODO(Takagi, Isamu): define interface #include +#include #include +#include + // This file should be included after messages. #include "../utils/types.hpp" @@ -48,9 +48,11 @@ class AutowareStateNode : public rclcpp::Node using LocalizationState = autoware_ad_api::localization::InitializationState::Message; using RoutingState = autoware_ad_api::routing::RouteState::Message; using OperationModeState = autoware_ad_api::operation_mode::OperationModeState::Message; + using Trigger = std_srvs::srv::Trigger; std::vector component_states_; std::vector::SharedPtr> sub_component_states_; rclcpp::Publisher::SharedPtr pub_autoware_state_; + rclcpp::Service::SharedPtr srv_autoware_shutdown_; enum class LaunchState { Initializing, Running, Finalizing }; LaunchState launch_state_; @@ -62,6 +64,7 @@ class AutowareStateNode : public rclcpp::Node void on_localization(const LocalizationState::ConstSharedPtr msg); void on_routing(const RoutingState::ConstSharedPtr msg); void on_operation_mode(const OperationModeState::ConstSharedPtr msg); + void on_shutdown(const Trigger::Request::SharedPtr, const Trigger::Response::SharedPtr); }; } // namespace default_ad_api From a9bb80b3e8103cd77bccd07079fe27947fbad8ae Mon Sep 17 00:00:00 2001 From: "Takagi, Isamu" Date: Wed, 16 Nov 2022 20:09:44 +0900 Subject: [PATCH 24/34] feat: change operation mode to stop Signed-off-by: Takagi, Isamu --- system/default_ad_api/src/routing.cpp | 14 ++++++++++++++ system/default_ad_api/src/routing.hpp | 3 +++ 2 files changed, 17 insertions(+) diff --git a/system/default_ad_api/src/routing.cpp b/system/default_ad_api/src/routing.cpp index 914f882eb006d..dc232688ca2f6 100644 --- a/system/default_ad_api/src/routing.cpp +++ b/system/default_ad_api/src/routing.cpp @@ -29,16 +29,30 @@ RoutingNode::RoutingNode(const rclcpp::NodeOptions & options) : Node("routing", adaptor.init_pub(pub_route_); adaptor.init_sub(sub_state_, this, &RoutingNode::on_state); adaptor.init_sub(sub_route_, this, &RoutingNode::on_route); + adaptor.init_cli(cli_operation_mode_, group_cli_); adaptor.init_cli(cli_set_route_, group_cli_); adaptor.init_srv(srv_set_route_, this, &RoutingNode::on_set_route); adaptor.relay_service(cli_set_route_points_, srv_set_route_points_, group_cli_); adaptor.relay_service(cli_clear_route_, srv_clear_route_, group_cli_); } +void RoutingNode::change_stop_mode() +{ + using OperationModeRequest = system_interface::ChangeOperationMode::Service::Request; + const auto req = std::make_shared(); + req->mode = OperationModeRequest::STOP; + cli_operation_mode_->async_send_request(req); +} + void RoutingNode::on_state(const State::Message::ConstSharedPtr msg) { pub_state_->publish(*msg); + // Change operation mode to stop when the vehicle arrives. + if (msg->state == State::Message::ARRIVED) { + change_stop_mode(); + } + // TODO(Takagi, Isamu): Remove when the mission planner supports an empty route. if (msg->state == State::Message::UNSET) { pub_route_->publish(conversion::create_empty_route(msg->stamp)); diff --git a/system/default_ad_api/src/routing.hpp b/system/default_ad_api/src/routing.hpp index 8ba3831ee96fb..39fd14f4bbac0 100644 --- a/system/default_ad_api/src/routing.hpp +++ b/system/default_ad_api/src/routing.hpp @@ -17,6 +17,7 @@ #include #include +#include #include #include @@ -43,9 +44,11 @@ class RoutingNode : public rclcpp::Node Cli cli_set_route_points_; Cli cli_set_route_; Cli cli_clear_route_; + Cli cli_operation_mode_; using State = planning_interface::RouteState; using Route = planning_interface::Route; + void change_stop_mode(); void on_state(const State::Message::ConstSharedPtr msg); void on_route(const Route::Message::ConstSharedPtr msg); void on_set_route( From dd90de426d6d40141df08cf43c908dd21afe4fd1 Mon Sep 17 00:00:00 2001 From: "Takagi, Isamu" Date: Thu, 17 Nov 2022 16:19:37 +0900 Subject: [PATCH 25/34] feat: change operation mode to stop Signed-off-by: Takagi, Isamu --- system/default_ad_api/src/routing.cpp | 28 ++++++++++++++++++++++----- system/default_ad_api/src/routing.hpp | 7 +++++++ 2 files changed, 30 insertions(+), 5 deletions(-) diff --git a/system/default_ad_api/src/routing.cpp b/system/default_ad_api/src/routing.cpp index dc232688ca2f6..e4a6dae6e8e89 100644 --- a/system/default_ad_api/src/routing.cpp +++ b/system/default_ad_api/src/routing.cpp @@ -29,19 +29,29 @@ RoutingNode::RoutingNode(const rclcpp::NodeOptions & options) : Node("routing", adaptor.init_pub(pub_route_); adaptor.init_sub(sub_state_, this, &RoutingNode::on_state); adaptor.init_sub(sub_route_, this, &RoutingNode::on_route); - adaptor.init_cli(cli_operation_mode_, group_cli_); + adaptor.init_cli(cli_clear_route_, group_cli_); + adaptor.init_srv(srv_clear_route_, this, &RoutingNode::on_clear_route); adaptor.init_cli(cli_set_route_, group_cli_); adaptor.init_srv(srv_set_route_, this, &RoutingNode::on_set_route); adaptor.relay_service(cli_set_route_points_, srv_set_route_points_, group_cli_); - adaptor.relay_service(cli_clear_route_, srv_clear_route_, group_cli_); + + adaptor.init_cli(cli_operation_mode_, group_cli_); + adaptor.init_sub(sub_operation_mode_, this, &RoutingNode::on_operation_mode); } void RoutingNode::change_stop_mode() { using OperationModeRequest = system_interface::ChangeOperationMode::Service::Request; - const auto req = std::make_shared(); - req->mode = OperationModeRequest::STOP; - cli_operation_mode_->async_send_request(req); + if (is_auto_mode) { + const auto req = std::make_shared(); + req->mode = OperationModeRequest::STOP; + cli_operation_mode_->async_send_request(req); + } +} + +void RoutingNode::on_operation_mode(const OperationModeState::Message::ConstSharedPtr msg) +{ + is_auto_mode = msg->mode == OperationModeState::Message::AUTONOMOUS; } void RoutingNode::on_state(const State::Message::ConstSharedPtr msg) @@ -64,6 +74,14 @@ void RoutingNode::on_route(const Route::Message::ConstSharedPtr msg) pub_route_->publish(conversion::convert_route(*msg)); } +void RoutingNode::on_clear_route( + const autoware_ad_api::routing::ClearRoute::Service::Request::SharedPtr req, + const autoware_ad_api::routing::ClearRoute::Service::Response::SharedPtr res) +{ + change_stop_mode(); + *res = *cli_clear_route_->call(req); +} + void RoutingNode::on_set_route( const autoware_ad_api::routing::SetRoute::Service::Request::SharedPtr req, const autoware_ad_api::routing::SetRoute::Service::Response::SharedPtr res) diff --git a/system/default_ad_api/src/routing.hpp b/system/default_ad_api/src/routing.hpp index 39fd14f4bbac0..47d23378f2c12 100644 --- a/system/default_ad_api/src/routing.hpp +++ b/system/default_ad_api/src/routing.hpp @@ -45,12 +45,19 @@ class RoutingNode : public rclcpp::Node Cli cli_set_route_; Cli cli_clear_route_; Cli cli_operation_mode_; + Sub sub_operation_mode_; + bool is_auto_mode = false; + using OperationModeState = system_interface::OperationModeState; using State = planning_interface::RouteState; using Route = planning_interface::Route; void change_stop_mode(); + void on_operation_mode(const OperationModeState::Message::ConstSharedPtr msg); void on_state(const State::Message::ConstSharedPtr msg); void on_route(const Route::Message::ConstSharedPtr msg); + void on_clear_route( + const autoware_ad_api::routing::ClearRoute::Service::Request::SharedPtr req, + const autoware_ad_api::routing::ClearRoute::Service::Response::SharedPtr res); void on_set_route( const autoware_ad_api::routing::SetRoute::Service::Request::SharedPtr req, const autoware_ad_api::routing::SetRoute::Service::Response::SharedPtr res); From 2e19e65cb611fcc446516f6e45ba7ffcba6f4e3f Mon Sep 17 00:00:00 2001 From: "Takagi, Isamu" Date: Thu, 17 Nov 2022 18:40:19 +0900 Subject: [PATCH 26/34] feat: remove ad_service_state_monitor Signed-off-by: Takagi, Isamu --- .../ad_service_state_monitor/CMakeLists.txt | 17 - system/ad_service_state_monitor/README.md | 46 -- .../ad_service_state_monitor.param.yaml | 155 ----- ...ate_monitor.planning_simulation.param.yaml | 126 ---- .../ad_service_state.hpp | 90 --- .../ad_service_state_monitor_node.hpp | 148 ----- .../ad_service_state_monitor/config.hpp | 122 ---- .../ad_service_state_monitor/module_name.hpp | 30 - .../state_machine.hpp | 113 ---- .../ad_service_state_monitor.launch.xml | 54 -- system/ad_service_state_monitor/package.xml | 40 -- .../ad_service_state_monitor_node.cpp | 553 ------------------ .../diagnostics.cpp | 181 ------ .../ad_service_state_monitor_node/main.cpp | 31 - .../state_machine.cpp | 350 ----------- 15 files changed, 2056 deletions(-) delete mode 100644 system/ad_service_state_monitor/CMakeLists.txt delete mode 100644 system/ad_service_state_monitor/README.md delete mode 100644 system/ad_service_state_monitor/config/ad_service_state_monitor.param.yaml delete mode 100644 system/ad_service_state_monitor/config/ad_service_state_monitor.planning_simulation.param.yaml delete mode 100644 system/ad_service_state_monitor/include/ad_service_state_monitor/ad_service_state.hpp delete mode 100644 system/ad_service_state_monitor/include/ad_service_state_monitor/ad_service_state_monitor_node.hpp delete mode 100644 system/ad_service_state_monitor/include/ad_service_state_monitor/config.hpp delete mode 100644 system/ad_service_state_monitor/include/ad_service_state_monitor/module_name.hpp delete mode 100644 system/ad_service_state_monitor/include/ad_service_state_monitor/state_machine.hpp delete mode 100644 system/ad_service_state_monitor/launch/ad_service_state_monitor.launch.xml delete mode 100644 system/ad_service_state_monitor/package.xml delete mode 100644 system/ad_service_state_monitor/src/ad_service_state_monitor_node/ad_service_state_monitor_node.cpp delete mode 100644 system/ad_service_state_monitor/src/ad_service_state_monitor_node/diagnostics.cpp delete mode 100644 system/ad_service_state_monitor/src/ad_service_state_monitor_node/main.cpp delete mode 100644 system/ad_service_state_monitor/src/ad_service_state_monitor_node/state_machine.cpp diff --git a/system/ad_service_state_monitor/CMakeLists.txt b/system/ad_service_state_monitor/CMakeLists.txt deleted file mode 100644 index 92cf601fb482e..0000000000000 --- a/system/ad_service_state_monitor/CMakeLists.txt +++ /dev/null @@ -1,17 +0,0 @@ -cmake_minimum_required(VERSION 3.14) -project(ad_service_state_monitor) - -find_package(autoware_cmake REQUIRED) -autoware_package() - -ament_auto_add_executable(ad_service_state_monitor - src/ad_service_state_monitor_node/main.cpp - src/ad_service_state_monitor_node/ad_service_state_monitor_node.cpp - src/ad_service_state_monitor_node/state_machine.cpp - src/ad_service_state_monitor_node/diagnostics.cpp -) - -ament_auto_package(INSTALL_TO_SHARE - launch - config -) diff --git a/system/ad_service_state_monitor/README.md b/system/ad_service_state_monitor/README.md deleted file mode 100644 index 53ff7efc4502f..0000000000000 --- a/system/ad_service_state_monitor/README.md +++ /dev/null @@ -1,46 +0,0 @@ -# ad_service_state_monitor - -## Purpose - -This node manages AutowareState transitions. - -## Inner-workings / Algorithms - -## Inputs / Outputs - -### Input - -| Name | Type | Description | -| ---------------------------------- | ---------------------------------------------------- | ------------------------------------------------- | -| `/planning/mission_planning/route` | `autoware_auto_planning_msgs::msg::HADMapRoute` | Subscribe route | -| `/localization/kinematic_state` | `nav_msgs::msg::Odometry` | Used to decide whether vehicle is stopped or not | -| `/vehicle/state_report` | `autoware_auto_vehicle_msgs::msg::ControlModeReport` | Used to check vehicle mode: autonomous or manual. | - -### Output - -| Name | Type | Description | -| ------------------ | ----------------------------------------------- | -------------------------------------------------- | -| `/autoware/engage` | `autoware_auto_vehicle_msgs::msg::Engage` | publish disengage flag on AutowareState transition | -| `/autoware/state` | `autoware_auto_system_msgs::msg::AutowareState` | publish AutowareState | - -## Parameters - -### Node Parameters - -| Name | Type | Default Value | Explanation | -| ------------- | ---- | ------------- | ---------------------- | -| `update_rate` | int | `10` | Timer callback period. | - -### Core Parameters - -| Name | Type | Default Value | Explanation | -| ------------------------- | ------ | ------------- | -------------------------------------------------------------------------- | -| `th_arrived_distance_m` | double | 1.0 | threshold distance to check if vehicle has arrived at the route's endpoint | -| `th_stopped_time_sec` | double | 1.0 | threshold time to check if vehicle is stopped | -| `th_stopped_velocity_mps` | double | 0.01 | threshold velocity to check if vehicle is stopped | -| `disengage_on_route` | bool | true | send disengage flag or not when the route is subscribed | -| `disengage_on_goal` | bool | true | send disengage flag or not when the vehicle is arrived goal | - -## Assumptions / Known limits - -TBD. diff --git a/system/ad_service_state_monitor/config/ad_service_state_monitor.param.yaml b/system/ad_service_state_monitor/config/ad_service_state_monitor.param.yaml deleted file mode 100644 index 9113e8a1f1206..0000000000000 --- a/system/ad_service_state_monitor/config/ad_service_state_monitor.param.yaml +++ /dev/null @@ -1,155 +0,0 @@ -# AD Service State Monitor Parameters -/**: - ros__parameters: - # modules_names: string array - module_names: [ - "map", - "sensing", - "perception", - "localization", - "planning", - "control", - "vehicle", - "system" - ] - - # Topic Config - ## names: string array - ## configs: top level - ### names used to declare parameter group - #### module: string - #### timeout[s]: double (0: none) - #### warn_rate[hz]: double (0: none) - topic_configs: - names: [ - "/map/vector_map", - "/map/pointcloud_map", - "/perception/obstacle_segmentation/pointcloud", - "/perception/object_recognition/objects", - "/initialpose3d", - "/localization/pose_twist_fusion_filter/pose", - "/planning/mission_planning/route", - "/planning/scenario_planning/trajectory", - "/control/trajectory_follower/control_cmd", - "/control/command/control_cmd", - "/vehicle/status/velocity_status", - "/vehicle/status/steering_status", - "/system/emergency/control_cmd" - ] - - configs: - /map/vector_map: - module: "map" - timeout: 0.0 - warn_rate: 0.0 - type: "autoware_auto_mapping_msgs/msg/HADMapBin" - transient_local: True - - /map/pointcloud_map: - module: "map" - timeout: 0.0 - warn_rate: 0.0 - type: "sensor_msgs/msg/PointCloud2" - transient_local: True - - /perception/obstacle_segmentation/pointcloud: - module: "sensing" - timeout: 1.0 - warn_rate: 5.0 - type: "sensor_msgs/msg/PointCloud2" - best_effort: True - - # Can be both with feature array or without - # In prediction.launch.xml this is set to without - /perception/object_recognition/objects: - module: "perception" - timeout: 1.0 - warn_rate: 5.0 - type: "autoware_auto_perception_msgs/msg/PredictedObjects" - # This topic could have two different types depending on the launch flags used. - # The implementation of subscriptions in ROS2 does not allow for multiple types - # to be defined for a topic. By default this is set to not use have features. - # type: ["tier4_perception_msgs/msg/DynamicObjectArray", "tier4_perception_msgs/msg/DynamicObjectWithFeatureArray"] - - /initialpose3d: - module: "localization" - timeout: 0.0 - warn_rate: 0.0 - type: "geometry_msgs/msg/PoseWithCovarianceStamped" - - /localization/pose_twist_fusion_filter/pose: - module: "localization" - timeout: 1.0 - warn_rate: 5.0 - type: "geometry_msgs/msg/PoseStamped" - - /planning/mission_planning/route: - module: "planning" - timeout: 0.0 - warn_rate: 0.0 - type: "autoware_auto_planning_msgs/msg/HADMapRoute" - transient_local: True - - /planning/scenario_planning/trajectory: - module: "planning" - timeout: 1.0 - warn_rate: 5.0 - type: "autoware_auto_planning_msgs/msg/Trajectory" - - /control/trajectory_follower/control_cmd: - module: "control" - timeout: 1.0 - warn_rate: 5.0 - type: "autoware_auto_control_msgs/msg/AckermannControlCommand" - - /control/command/control_cmd: - module: "control" - timeout: 1.0 - warn_rate: 5.0 - type: "autoware_auto_control_msgs/msg/AckermannControlCommand" - - /vehicle/status/velocity_status: - module: "vehicle" - timeout: 1.0 - warn_rate: 5.0 - type: "autoware_auto_vehicle_msgs/msg/VelocityReport" - - /vehicle/status/steering_status: - module: "vehicle" - timeout: 1.0 - warn_rate: 5.0 - type: "autoware_auto_vehicle_msgs/msg/SteeringReport" - - /system/emergency/control_cmd: - module: "system" - timeout: 1.0 - warn_rate: 5.0 - type: "autoware_auto_control_msgs/msg/AckermannControlCommand" - - # Param Config - ## names: string array - ## configs: top level - ### names used to declare parameter group - #### module: string - # param_configs: - # names: ["/vehicle_info"] - # configs: - # /vehicle_info: - # module: "vehicle" - - # TF Config - ## names: string array - ## configs: top level - ### names used to declare parameter group - #### module: string - #### from: string - #### to: string - #### timeout[s]: double (0: none) - tf_configs: - names: ["map_to_base_link"] - configs: - map_to_base_link: - module: "localization" - from: "map" - to: "base_link" - timeout: 1.0 diff --git a/system/ad_service_state_monitor/config/ad_service_state_monitor.planning_simulation.param.yaml b/system/ad_service_state_monitor/config/ad_service_state_monitor.planning_simulation.param.yaml deleted file mode 100644 index 51fbe99dd9bc3..0000000000000 --- a/system/ad_service_state_monitor/config/ad_service_state_monitor.planning_simulation.param.yaml +++ /dev/null @@ -1,126 +0,0 @@ -# AD Service State Monitor: Planning Simulator Parameters -/**: - ros__parameters: - # modules_names: string array - module_names: [ - "map", - "sensing", - "perception", - "localization", - "planning", - "control", - "vehicle", - "system" - ] - -# Topic Config - ## names: string array - ## configs: top level - ### names used to declare parameter group - #### module: string - #### timeout[s]: double (0: none) - #### warn_rate[hz]: double (0: none) - topic_configs: - names: [ - "/map/vector_map", - "/perception/object_recognition/objects", - "/initialpose3d", - "/planning/mission_planning/route", - "/planning/scenario_planning/trajectory", - "/control/trajectory_follower/control_cmd", - "/control/command/control_cmd", - "/vehicle/status/velocity_status", - "/vehicle/status/steering_status", - "/system/emergency/control_cmd" - ] - - configs: - /map/vector_map: - module: "map" - timeout: 0.0 - warn_rate: 0.0 - type: "autoware_auto_mapping_msgs/msg/HADMapBin" - transient_local: True - - /perception/object_recognition/objects: - module: "perception" - timeout: 1.0 - warn_rate: 5.0 - type: "autoware_auto_perception_msgs/msg/PredictedObjects" - - /initialpose3d: - module: "localization" - timeout: 0.0 - warn_rate: 0.0 - type: "geometry_msgs/msg/PoseWithCovarianceStamped" - - /planning/mission_planning/route: - module: "planning" - timeout: 0.0 - warn_rate: 0.0 - type: "autoware_auto_planning_msgs/msg/HADMapRoute" - transient_local: True - - /planning/scenario_planning/trajectory: - module: "planning" - timeout: 1.0 - warn_rate: 5.0 - type: "autoware_auto_planning_msgs/msg/Trajectory" - - /control/trajectory_follower/control_cmd: - module: "control" - timeout: 1.0 - warn_rate: 5.0 - type: "autoware_auto_control_msgs/msg/AckermannControlCommand" - - /control/command/control_cmd: - module: "control" - timeout: 1.0 - warn_rate: 5.0 - type: "autoware_auto_control_msgs/msg/AckermannControlCommand" - - /vehicle/status/velocity_status: - module: "vehicle" - timeout: 1.0 - warn_rate: 5.0 - type: "autoware_auto_vehicle_msgs/msg/VelocityReport" - - /vehicle/status/steering_status: - module: "vehicle" - timeout: 1.0 - warn_rate: 5.0 - type: "autoware_auto_vehicle_msgs/msg/SteeringReport" - - /system/emergency/control_cmd: - module: "system" - timeout: 1.0 - warn_rate: 5.0 - type: "autoware_auto_control_msgs/msg/AckermannControlCommand" - - # Param Config - ## names: string array - ## configs: top level - ### names used to declare parameter group - #### module: string - # param_configs: - # names: ["/vehicle_info"] - # configs: - # /vehicle_info: - # module: "vehicle" - - # TF Config - ## names: string array - ## configs: top level - ### names used to declare parameter group - #### module: string - #### from: string - #### to: string - #### timeout[s]: double (0: none) - tf_configs: - names: ["map_to_base_link"] - configs: - map_to_base_link: - module: "localization" - from: "map" - to: "base_link" - timeout: 1.0 diff --git a/system/ad_service_state_monitor/include/ad_service_state_monitor/ad_service_state.hpp b/system/ad_service_state_monitor/include/ad_service_state_monitor/ad_service_state.hpp deleted file mode 100644 index 000ab94812bcd..0000000000000 --- a/system/ad_service_state_monitor/include/ad_service_state_monitor/ad_service_state.hpp +++ /dev/null @@ -1,90 +0,0 @@ -// Copyright 2020 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 AD_SERVICE_STATE_MONITOR__AD_SERVICE_STATE_HPP_ -#define AD_SERVICE_STATE_MONITOR__AD_SERVICE_STATE_HPP_ - -#include - -#include - -enum class AutowareState : int8_t { - InitializingVehicle, - WaitingForRoute, - Planning, - WaitingForEngage, - Driving, - ArrivedGoal, - Finalizing, -}; - -inline AutowareState fromMsg(const int state) -{ - using StateMessage = autoware_auto_system_msgs::msg::AutowareState; - - if (state == StateMessage::INITIALIZING) { - return AutowareState::InitializingVehicle; - } - if (state == StateMessage::WAITING_FOR_ROUTE) { - return AutowareState::WaitingForRoute; - } - if (state == StateMessage::PLANNING) { - return AutowareState::Planning; - } - if (state == StateMessage::WAITING_FOR_ENGAGE) { - return AutowareState::WaitingForEngage; - } - if (state == StateMessage::DRIVING) { - return AutowareState::Driving; - } - if (state == StateMessage::ARRIVED_GOAL) { - return AutowareState::ArrivedGoal; - } - if (state == StateMessage::FINALIZING) { - return AutowareState::Finalizing; - } - - throw std::runtime_error("invalid state"); -} - -inline int toMsg(const AutowareState & state) -{ - using StateMessage = autoware_auto_system_msgs::msg::AutowareState; - - if (state == AutowareState::InitializingVehicle) { - return StateMessage::INITIALIZING; - } - if (state == AutowareState::WaitingForRoute) { - return StateMessage::WAITING_FOR_ROUTE; - } - if (state == AutowareState::Planning) { - return StateMessage::PLANNING; - } - if (state == AutowareState::WaitingForEngage) { - return StateMessage::WAITING_FOR_ENGAGE; - } - if (state == AutowareState::Driving) { - return StateMessage::DRIVING; - } - if (state == AutowareState::ArrivedGoal) { - return StateMessage::ARRIVED_GOAL; - } - if (state == AutowareState::Finalizing) { - return StateMessage::FINALIZING; - } - - throw std::runtime_error("invalid state"); -} - -#endif // AD_SERVICE_STATE_MONITOR__AD_SERVICE_STATE_HPP_ diff --git a/system/ad_service_state_monitor/include/ad_service_state_monitor/ad_service_state_monitor_node.hpp b/system/ad_service_state_monitor/include/ad_service_state_monitor/ad_service_state_monitor_node.hpp deleted file mode 100644 index cb7577cb85e17..0000000000000 --- a/system/ad_service_state_monitor/include/ad_service_state_monitor/ad_service_state_monitor_node.hpp +++ /dev/null @@ -1,148 +0,0 @@ -// Copyright 2020 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 AD_SERVICE_STATE_MONITOR__AD_SERVICE_STATE_MONITOR_NODE_HPP_ -#define AD_SERVICE_STATE_MONITOR__AD_SERVICE_STATE_MONITOR_NODE_HPP_ - -#include "ad_service_state_monitor/ad_service_state.hpp" -#include "ad_service_state_monitor/config.hpp" -#include "ad_service_state_monitor/state_machine.hpp" - -#include -#include -#include - -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include - -#include -#include -#include -#include -#include - -class AutowareStateMonitorNode : public rclcpp::Node -{ -public: - AutowareStateMonitorNode(); - -private: - // Parameter - double update_rate_; - bool disengage_on_route_; - bool disengage_on_goal_; - - std::vector topic_configs_; - std::vector param_configs_; - std::vector tf_configs_; - - // TF - tf2_ros::Buffer tf_buffer_; - tf2_ros::TransformListener tf_listener_; - geometry_msgs::msg::PoseStamped::ConstSharedPtr current_pose_; - - // Lanelet - std::shared_ptr lanelet_map_ptr_; - bool is_map_msg_ready_{false}; - - // CallbackGroups - rclcpp::CallbackGroup::SharedPtr callback_group_subscribers_; - rclcpp::CallbackGroup::SharedPtr callback_group_services_; - - // Subscriber - rclcpp::Subscription::SharedPtr sub_autoware_engage_; - rclcpp::Subscription::SharedPtr - sub_control_mode_; - rclcpp::Subscription::SharedPtr sub_route_; - rclcpp::Subscription::SharedPtr sub_modified_goal_; - rclcpp::Subscription::SharedPtr sub_map_; - rclcpp::Subscription::SharedPtr sub_odom_; - - void onAutowareEngage(const autoware_auto_vehicle_msgs::msg::Engage::ConstSharedPtr msg); - void onVehicleControlMode( - const autoware_auto_vehicle_msgs::msg::ControlModeReport::ConstSharedPtr msg); - void onMap(const autoware_auto_mapping_msgs::msg::HADMapBin::ConstSharedPtr msg); - void onRoute(const autoware_auto_planning_msgs::msg::HADMapRoute::ConstSharedPtr msg); - void onModifiedGoal(const geometry_msgs::msg::PoseStamped::ConstSharedPtr msg); - void onOdometry(const nav_msgs::msg::Odometry::ConstSharedPtr msg); - - // Topic Buffer - void onTopic( - const std::shared_ptr msg, const std::string & topic_name); - void registerTopicCallback( - const std::string & topic_name, const std::string & topic_type, const bool transient_local, - const bool best_effort); - - std::map sub_topic_map_; - std::map> topic_received_time_buffer_; - - // Service - rclcpp::Service::SharedPtr srv_shutdown_; - rclcpp::Service::SharedPtr srv_reset_route_; - - bool onShutdownService( - const std::shared_ptr request_header, - const std::shared_ptr request, - std::shared_ptr response); - bool onResetRouteService( - const std::shared_ptr request_header, - const std::shared_ptr request, - std::shared_ptr response); - - // Publisher - rclcpp::Publisher::SharedPtr pub_autoware_state_; - rclcpp::Publisher::SharedPtr pub_autoware_engage_; - - bool isEngaged(); - void setDisengage(); - - // Timer - void onTimer(); - rclcpp::TimerBase::SharedPtr timer_; - - // Stats - TopicStats getTopicStats() const; - ParamStats getParamStats() const; - TfStats getTfStats() const; - - // State Machine - std::shared_ptr state_machine_; - StateInput state_input_; - StateParam state_param_; - - // Lock Variables - std::mutex lock_state_machine_; - std::mutex lock_state_input_; - - // Diagnostic Updater - diagnostic_updater::Updater updater_; - - void setupDiagnosticUpdater(); - void checkTopicStatus( - diagnostic_updater::DiagnosticStatusWrapper & stat, const std::string & module_name); - void checkTFStatus( - diagnostic_updater::DiagnosticStatusWrapper & stat, const std::string & module_name); -}; - -#endif // AD_SERVICE_STATE_MONITOR__AD_SERVICE_STATE_MONITOR_NODE_HPP_ diff --git a/system/ad_service_state_monitor/include/ad_service_state_monitor/config.hpp b/system/ad_service_state_monitor/include/ad_service_state_monitor/config.hpp deleted file mode 100644 index 656d8233a89b2..0000000000000 --- a/system/ad_service_state_monitor/include/ad_service_state_monitor/config.hpp +++ /dev/null @@ -1,122 +0,0 @@ -// Copyright 2020 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 AD_SERVICE_STATE_MONITOR__CONFIG_HPP_ -#define AD_SERVICE_STATE_MONITOR__CONFIG_HPP_ - -#include -#include - -#include -#include -#include - -struct TopicConfig -{ - explicit TopicConfig( - rclcpp::node_interfaces::NodeParametersInterface::SharedPtr interface, - const std::string & namespace_prefix, const std::string & name) - : module( - interface->declare_parameter(namespace_prefix + ".module", - rclcpp::PARAMETER_STRING).get()), - name(name), - type( - interface->declare_parameter(namespace_prefix + ".type", - rclcpp::PARAMETER_STRING).get()), - transient_local( - interface->declare_parameter(namespace_prefix + ".transient_local", - static_cast(false)).get()), - best_effort( - interface->declare_parameter(namespace_prefix + ".best_effort", - static_cast(false)).get()), - timeout( - interface->declare_parameter(namespace_prefix + ".timeout", - rclcpp::PARAMETER_DOUBLE).get()), - warn_rate( - interface->declare_parameter(namespace_prefix + ".warn_rate", - rclcpp::PARAMETER_DOUBLE).get()) - { - } - - std::string module; - std::string name; - std::string type; - bool transient_local; - bool best_effort; - double timeout; - double warn_rate; -}; - -struct ParamConfig -{ - explicit ParamConfig( - rclcpp::node_interfaces::NodeParametersInterface::SharedPtr interface, - const std::string & namespace_prefix, const std::string & name) - : module(interface->declare_parameter(namespace_prefix + ".module", rclcpp::PARAMETER_STRING) - .get()), - name(name) - { - } - - std::string module; - std::string name; -}; - -struct TfConfig -{ - explicit TfConfig( - rclcpp::node_interfaces::NodeParametersInterface::SharedPtr interface, - const std::string & namespace_prefix, [[maybe_unused]] const std::string & name) - : module(interface->declare_parameter(namespace_prefix + ".module", rclcpp::PARAMETER_STRING) - .get()), - from(interface->declare_parameter(namespace_prefix + ".from", rclcpp::PARAMETER_STRING) - .get()), - to(interface->declare_parameter(namespace_prefix + ".to", rclcpp::PARAMETER_STRING) - .get()), - timeout(interface->declare_parameter(namespace_prefix + ".timeout", rclcpp::PARAMETER_DOUBLE) - .get()) - { - } - - std::string module; - std::string from; - std::string to; - double timeout; -}; - -struct TopicStats -{ - rclcpp::Time checked_time; - std::vector ok_list; - std::vector non_received_list; - std::vector> timeout_list; // pair - std::vector> slow_rate_list; // pair -}; - -struct ParamStats -{ - rclcpp::Time checked_time; - std::vector ok_list; - std::vector non_set_list; -}; - -struct TfStats -{ - rclcpp::Time checked_time; - std::vector ok_list; - std::vector non_received_list; - std::vector> timeout_list; // pair -}; - -#endif // AD_SERVICE_STATE_MONITOR__CONFIG_HPP_ diff --git a/system/ad_service_state_monitor/include/ad_service_state_monitor/module_name.hpp b/system/ad_service_state_monitor/include/ad_service_state_monitor/module_name.hpp deleted file mode 100644 index 4e2a7500f3ff6..0000000000000 --- a/system/ad_service_state_monitor/include/ad_service_state_monitor/module_name.hpp +++ /dev/null @@ -1,30 +0,0 @@ -// Copyright 2020 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 AD_SERVICE_STATE_MONITOR__MODULE_NAME_HPP_ -#define AD_SERVICE_STATE_MONITOR__MODULE_NAME_HPP_ - -struct ModuleName -{ - static constexpr const char * map = "map"; - static constexpr const char * sensing = "sensing"; - static constexpr const char * localization = "localization"; - static constexpr const char * perception = "perception"; - static constexpr const char * planning = "planning"; - static constexpr const char * control = "control"; - static constexpr const char * vehicle = "vehicle"; - static constexpr const char * system = "system"; -}; - -#endif // AD_SERVICE_STATE_MONITOR__MODULE_NAME_HPP_ diff --git a/system/ad_service_state_monitor/include/ad_service_state_monitor/state_machine.hpp b/system/ad_service_state_monitor/include/ad_service_state_monitor/state_machine.hpp deleted file mode 100644 index 5d503d420c092..0000000000000 --- a/system/ad_service_state_monitor/include/ad_service_state_monitor/state_machine.hpp +++ /dev/null @@ -1,113 +0,0 @@ -// Copyright 2020 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 AD_SERVICE_STATE_MONITOR__STATE_MACHINE_HPP_ -#define AD_SERVICE_STATE_MONITOR__STATE_MACHINE_HPP_ - -#include "ad_service_state_monitor/ad_service_state.hpp" -#include "ad_service_state_monitor/config.hpp" -#include "ad_service_state_monitor/module_name.hpp" -#include "tier4_autoware_utils/math/normalization.hpp" -#include "tier4_autoware_utils/math/unit_conversion.hpp" - -#include - -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include - -struct StateInput -{ - TopicStats topic_stats; - ParamStats param_stats; - TfStats tf_stats; - - rclcpp::Time current_time; - - geometry_msgs::msg::PoseStamped::ConstSharedPtr current_pose; - geometry_msgs::msg::Pose::ConstSharedPtr goal_pose; - geometry_msgs::msg::PoseStamped::ConstSharedPtr modified_goal_pose; - - autoware_auto_vehicle_msgs::msg::Engage::ConstSharedPtr autoware_engage; - autoware_auto_vehicle_msgs::msg::ControlModeReport::ConstSharedPtr control_mode_; - bool is_finalizing = false; - bool is_route_reset_required = false; - autoware_auto_planning_msgs::msg::HADMapRoute::ConstSharedPtr route; - nav_msgs::msg::Odometry::ConstSharedPtr odometry; - std::deque odometry_buffer; -}; - -struct StateParam -{ - double th_arrived_distance_m; - double th_arrived_angle; - double th_stopped_time_sec; - double th_stopped_velocity_mps; -}; - -struct Times -{ - rclcpp::Time arrived_goal; - rclcpp::Time initializing_completed; - rclcpp::Time planning_completed; -}; - -struct Flags -{ - bool waiting_after_initializing = false; - bool waiting_after_planning = false; -}; - -class StateMachine -{ -public: - explicit StateMachine(const StateParam & state_param) : state_param_(state_param) {} - - AutowareState getCurrentState() const { return autoware_state_; } - AutowareState updateState(const StateInput & state_input); - std::vector getMessages() const { return msgs_; } - -private: - AutowareState autoware_state_ = AutowareState::InitializingVehicle; - StateInput state_input_; - const StateParam state_param_; - - mutable std::vector msgs_; - mutable Times times_; - mutable Flags flags_; - mutable autoware_auto_planning_msgs::msg::HADMapRoute::ConstSharedPtr executing_route_ = nullptr; - - AutowareState judgeAutowareState() const; - - bool isModuleInitialized(const char * module_name) const; - bool isVehicleInitialized() const; - bool hasRoute() const; - bool isRouteReceived() const; - bool isPlanningCompleted() const; - bool isEngaged() const; - bool isOverridden() const; - bool hasArrivedGoal() const; - bool isFinalizing() const; - bool isRouteResetRequired() const; -}; - -#endif // AD_SERVICE_STATE_MONITOR__STATE_MACHINE_HPP_ diff --git a/system/ad_service_state_monitor/launch/ad_service_state_monitor.launch.xml b/system/ad_service_state_monitor/launch/ad_service_state_monitor.launch.xml deleted file mode 100644 index ea1cfcaa0f5af..0000000000000 --- a/system/ad_service_state_monitor/launch/ad_service_state_monitor.launch.xml +++ /dev/null @@ -1,54 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/system/ad_service_state_monitor/package.xml b/system/ad_service_state_monitor/package.xml deleted file mode 100644 index ed99be17cbc22..0000000000000 --- a/system/ad_service_state_monitor/package.xml +++ /dev/null @@ -1,40 +0,0 @@ - - - - ad_service_state_monitor - 0.1.1 - The ad_service_state_monitor package ROS2 - Kenji Miyake - Apache License 2.0 - - ament_cmake_auto - - autoware_cmake - - autoware_auto_control_msgs - autoware_auto_planning_msgs - autoware_auto_system_msgs - autoware_auto_vehicle_msgs - diagnostic_updater - fmt - geometry_msgs - lanelet2_extension - nav_msgs - rclcpp - std_msgs - std_srvs - tf2 - tf2_geometry_msgs - tf2_ros - tier4_autoware_utils - - sensor_msgs - tier4_perception_msgs - - ament_lint_auto - autoware_lint_common - - - ament_cmake - - diff --git a/system/ad_service_state_monitor/src/ad_service_state_monitor_node/ad_service_state_monitor_node.cpp b/system/ad_service_state_monitor/src/ad_service_state_monitor_node/ad_service_state_monitor_node.cpp deleted file mode 100644 index 662df2efbfe0d..0000000000000 --- a/system/ad_service_state_monitor/src/ad_service_state_monitor_node/ad_service_state_monitor_node.cpp +++ /dev/null @@ -1,553 +0,0 @@ -// Copyright 2020 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 "ad_service_state_monitor/ad_service_state_monitor_node.hpp" - -#include "lanelet2_extension/utility/message_conversion.hpp" -#include "lanelet2_extension/utility/route_checker.hpp" - -#ifdef ROS_DISTRO_GALACTIC -#include -#else -#include -#endif - -#include -#include -#include -#include -#include - -namespace -{ -template -std::vector getConfigs( - rclcpp::node_interfaces::NodeParametersInterface::SharedPtr interface, - const std::string & config_namespace) -{ - std::string names_key = config_namespace + ".names"; - interface->declare_parameter(names_key, rclcpp::PARAMETER_STRING_ARRAY); - std::vector config_names = interface->get_parameter(names_key).as_string_array(); - - std::vector configs; - configs.reserve(config_names.size()); - - for (auto config_name : config_names) { - configs.emplace_back(interface, config_namespace + ".configs." + config_name, config_name); - } - - return configs; -} - -double calcTopicRate(const std::deque & topic_received_time_buffer) -{ - assert(topic_received_time_buffer.size() >= 2); - - const auto & buf = topic_received_time_buffer; - const auto time_diff = buf.back() - buf.front(); - - return static_cast(buf.size() - 1) / time_diff.seconds(); -} - -geometry_msgs::msg::PoseStamped::SharedPtr getCurrentPose(const tf2_ros::Buffer & tf_buffer) -{ - geometry_msgs::msg::TransformStamped tf_current_pose; - - try { - tf_current_pose = tf_buffer.lookupTransform("map", "base_link", tf2::TimePointZero); - } catch (tf2::TransformException & ex) { - return nullptr; - } - - auto p = std::make_shared(); - p->header = tf_current_pose.header; - p->pose.orientation = tf_current_pose.transform.rotation; - p->pose.position.x = tf_current_pose.transform.translation.x; - p->pose.position.y = tf_current_pose.transform.translation.y; - p->pose.position.z = tf_current_pose.transform.translation.z; - - return p; -} - -} // namespace - -void AutowareStateMonitorNode::onAutowareEngage( - const autoware_auto_vehicle_msgs::msg::Engage::ConstSharedPtr msg) -{ - std::lock_guard lock(lock_state_input_); - state_input_.autoware_engage = msg; -} - -void AutowareStateMonitorNode::onVehicleControlMode( - const autoware_auto_vehicle_msgs::msg::ControlModeReport::ConstSharedPtr msg) -{ - std::lock_guard lock(lock_state_input_); - state_input_.control_mode_ = msg; -} - -void AutowareStateMonitorNode::onModifiedGoal( - const geometry_msgs::msg::PoseStamped::ConstSharedPtr msg) -{ - std::lock_guard lock(lock_state_input_); - state_input_.modified_goal_pose = msg; -} - -void AutowareStateMonitorNode::onMap( - const autoware_auto_mapping_msgs::msg::HADMapBin::ConstSharedPtr msg) -{ - lanelet_map_ptr_ = std::make_shared(); - lanelet::utils::conversion::fromBinMsg(*msg, lanelet_map_ptr_); - is_map_msg_ready_ = true; -} - -void AutowareStateMonitorNode::onRoute( - const autoware_auto_planning_msgs::msg::HADMapRoute::ConstSharedPtr msg) -{ - if (!is_map_msg_ready_) { - RCLCPP_WARN(this->get_logger(), "Map msg is not ready yet. Skip route msg."); - return; - } - bool is_route_valid = lanelet::utils::route::isRouteValid(*msg, lanelet_map_ptr_); - if (!is_route_valid) { - return; - } - - { - std::lock_guard lock(lock_state_input_); - state_input_.route = msg; - - // Get goal pose - geometry_msgs::msg::Pose::SharedPtr p = std::make_shared(); - *p = msg->goal_pose; - state_input_.goal_pose = geometry_msgs::msg::Pose::ConstSharedPtr(p); - } - - if (disengage_on_route_ && isEngaged()) { - RCLCPP_INFO(this->get_logger(), "new route received and disengage Autoware"); - setDisengage(); - } -} - -void AutowareStateMonitorNode::onOdometry(const nav_msgs::msg::Odometry::ConstSharedPtr msg) -{ - std::lock_guard lock(lock_state_input_); - - state_input_.odometry = msg; - - state_input_.odometry_buffer.push_back(msg); - - // Delete old data in buffer - while (true) { - const auto time_diff = rclcpp::Time(msg->header.stamp) - - rclcpp::Time(state_input_.odometry_buffer.front()->header.stamp); - - if (time_diff.seconds() < state_param_.th_stopped_time_sec) { - break; - } - - state_input_.odometry_buffer.pop_front(); - } - - constexpr size_t odometry_buffer_size = 200; // 40Hz * 5sec - if (state_input_.odometry_buffer.size() > odometry_buffer_size) { - state_input_.odometry_buffer.pop_front(); - } -} - -bool AutowareStateMonitorNode::onShutdownService( - const std::shared_ptr request_header, - [[maybe_unused]] const std::shared_ptr request, - const std::shared_ptr response) -{ - (void)request_header; - - { - std::lock_guard lock(lock_state_input_); - state_input_.is_finalizing = true; - } - - const auto t_start = this->get_clock()->now(); - constexpr double timeout = 3.0; - while (rclcpp::ok()) { - // rclcpp::spin_some(this->get_node_base_interface()); - - { - std::unique_lock lock(lock_state_machine_); - if (state_machine_->getCurrentState() == AutowareState::Finalizing) { - lock.unlock(); - - response->success = true; - response->message = "Shutdown Autoware."; - return true; - } - } - - if ((this->get_clock()->now() - t_start).seconds() > timeout) { - response->success = false; - response->message = "Shutdown timeout."; - return true; - } - - rclcpp::Rate(10.0).sleep(); - } - - response->success = false; - response->message = "Shutdown failure."; - return true; -} - -bool AutowareStateMonitorNode::onResetRouteService( - [[maybe_unused]] const std::shared_ptr request_header, - [[maybe_unused]] const std::shared_ptr request, - const std::shared_ptr response) -{ - { - std::unique_lock lock(lock_state_machine_); - if (state_machine_->getCurrentState() != AutowareState::WaitingForEngage) { - lock.unlock(); - - response->success = false; - response->message = "Reset route can be accepted only under WaitingForEngage."; - return true; - } - } - - { - std::lock_guard lock(lock_state_input_); - state_input_.is_route_reset_required = true; - } - - const auto t_start = this->now(); - constexpr double timeout = 3.0; - while (rclcpp::ok()) { - { - // To avoid dead lock, 2-phase lock is required here. - // If you change the order of the locks below, it may be dead-lock. - std::unique_lock lock_state_input(lock_state_input_); - std::unique_lock lock_state_machine(lock_state_machine_); - if (state_machine_->getCurrentState() == AutowareState::WaitingForRoute) { - state_input_.is_route_reset_required = false; - - lock_state_machine.unlock(); - lock_state_input.unlock(); - - response->success = true; - response->message = "Reset route."; - return true; - } - } - - if ((this->now() - t_start).seconds() > timeout) { - response->success = false; - response->message = "Reset route timeout."; - return true; - } - - rclcpp::Rate(10.0).sleep(); - } - - response->success = false; - response->message = "Reset route failure."; - return true; -} - -void AutowareStateMonitorNode::onTimer() -{ - AutowareState prev_autoware_state; - AutowareState autoware_state; - - { - std::unique_lock lock_state_input(lock_state_input_); - - // Prepare state input - state_input_.current_pose = getCurrentPose(tf_buffer_); - if (state_input_.current_pose == nullptr) { - RCLCPP_WARN_THROTTLE( - this->get_logger(), *this->get_clock(), 5000 /* ms */, - "Fail lookupTransform base_link to map"); - } - - state_input_.topic_stats = getTopicStats(); - state_input_.param_stats = getParamStats(); - state_input_.tf_stats = getTfStats(); - state_input_.current_time = this->now(); - - // Update state - // To avoid dead lock, 2-phase lock is required here. - std::lock_guard lock_state_machine(lock_state_machine_); - - prev_autoware_state = state_machine_->getCurrentState(); - autoware_state = state_machine_->updateState(state_input_); - } - - if (autoware_state != prev_autoware_state) { - RCLCPP_INFO( - this->get_logger(), "state changed: %i -> %i", toMsg(prev_autoware_state), - toMsg(autoware_state)); - } - - // Disengage on event - if (disengage_on_goal_ && isEngaged() && autoware_state == AutowareState::ArrivedGoal) { - RCLCPP_INFO(this->get_logger(), "arrived goal and disengage Autoware"); - setDisengage(); - } - - // Publish state message - { - autoware_auto_system_msgs::msg::AutowareState autoware_state_msg; - autoware_state_msg.state = toMsg(autoware_state); - - pub_autoware_state_->publish(autoware_state_msg); - } - - // Publish diag message - updater_.force_update(); -} - -// TODO(jilaada): Use generic subscription base -void AutowareStateMonitorNode::onTopic( - [[maybe_unused]] const std::shared_ptr msg, - const std::string & topic_name) -{ - const auto now = this->now(); - - auto & buf = topic_received_time_buffer_.at(topic_name); - buf.push_back(now); - - constexpr size_t topic_received_time_buffer_size = 10; - if (buf.size() > topic_received_time_buffer_size) { - buf.pop_front(); - } -} - -void AutowareStateMonitorNode::registerTopicCallback( - const std::string & topic_name, const std::string & topic_type, const bool transient_local, - const bool best_effort) -{ - // Initialize buffer - topic_received_time_buffer_[topic_name] = {}; - - // Register callback - using Callback = std::function)>; - const auto callback = static_cast( - std::bind(&AutowareStateMonitorNode::onTopic, this, std::placeholders::_1, topic_name)); - auto qos = rclcpp::QoS{1}; - if (transient_local) { - qos.transient_local(); - } - if (best_effort) { - qos.best_effort(); - } - - auto subscriber_option = rclcpp::SubscriptionOptions(); - subscriber_option.callback_group = callback_group_subscribers_; - - sub_topic_map_[topic_name] = - this->create_generic_subscription(topic_name, topic_type, qos, callback, subscriber_option); -} - -TopicStats AutowareStateMonitorNode::getTopicStats() const -{ - TopicStats topic_stats; - topic_stats.checked_time = this->now(); - - for (const auto & topic_config : topic_configs_) { - // Alias - const auto & buf = topic_received_time_buffer_.at(topic_config.name); - - // Check at least once received - if (buf.empty()) { - topic_stats.non_received_list.push_back(topic_config); - continue; - } - - // Check timeout - const auto last_received_time = buf.back(); - const auto time_diff = (topic_stats.checked_time - last_received_time).seconds(); - const auto is_timeout = (topic_config.timeout != 0) && (time_diff > topic_config.timeout); - if (is_timeout) { - topic_stats.timeout_list.emplace_back(topic_config, last_received_time); - continue; - } - - // Check topic rate - if (!is_timeout && buf.size() >= 2) { - const auto topic_rate = calcTopicRate(buf); - if (topic_config.warn_rate != 0 && topic_rate < topic_config.warn_rate) { - topic_stats.slow_rate_list.emplace_back(topic_config, topic_rate); - continue; - } - } - - // No error - topic_stats.ok_list.push_back(topic_config); - } - - return topic_stats; -} - -ParamStats AutowareStateMonitorNode::getParamStats() const -{ - ParamStats param_stats; - param_stats.checked_time = this->now(); - - for (const auto & param_config : param_configs_) { - const bool result = this->has_parameter("param_configs.configs." + param_config.name); - if (!result) { - param_stats.non_set_list.push_back(param_config); - continue; - } - - // No error - param_stats.ok_list.push_back(param_config); - } - - return param_stats; -} - -TfStats AutowareStateMonitorNode::getTfStats() const -{ - TfStats tf_stats; - tf_stats.checked_time = this->now(); - - for (const auto & tf_config : tf_configs_) { - try { - const auto transform = - tf_buffer_.lookupTransform(tf_config.from, tf_config.to, tf2::TimePointZero); - - const auto last_received_time = transform.header.stamp; - const auto time_diff = (tf_stats.checked_time - last_received_time).seconds(); - if (time_diff > tf_config.timeout) { - tf_stats.timeout_list.emplace_back(tf_config, last_received_time); - continue; - } - } catch (tf2::TransformException & ex) { - tf_stats.non_received_list.push_back(tf_config); - continue; - } - - // No error - tf_stats.ok_list.push_back(tf_config); - } - - return tf_stats; -} - -bool AutowareStateMonitorNode::isEngaged() -{ - std::lock_guard lock(lock_state_input_); - if (!state_input_.autoware_engage) { - return false; - } - - return state_input_.autoware_engage->engage; -} - -void AutowareStateMonitorNode::setDisengage() -{ - autoware_auto_vehicle_msgs::msg::Engage msg; - msg.stamp = this->now(); - msg.engage = false; - pub_autoware_engage_->publish(msg); -} - -AutowareStateMonitorNode::AutowareStateMonitorNode() -: Node("ad_service_state_monitor"), - tf_buffer_(this->get_clock()), - tf_listener_(tf_buffer_), - updater_(this) -{ - using std::placeholders::_1; - using std::placeholders::_2; - using std::placeholders::_3; - // Parameter - update_rate_ = this->declare_parameter("update_rate", 10.0); - disengage_on_route_ = this->declare_parameter("disengage_on_route", true); - disengage_on_goal_ = this->declare_parameter("disengage_on_goal", true); - - // Parameter for StateMachine - state_param_.th_arrived_distance_m = this->declare_parameter("th_arrived_distance_m", 1.0); - const auto th_arrived_angle_deg = this->declare_parameter("th_arrived_angle_deg", 45.0); - state_param_.th_arrived_angle = tier4_autoware_utils::deg2rad(th_arrived_angle_deg); - state_param_.th_stopped_time_sec = this->declare_parameter("th_stopped_time_sec", 1.0); - state_param_.th_stopped_velocity_mps = this->declare_parameter("th_stopped_velocity_mps", 0.01); - - // State Machine - state_machine_ = std::make_shared(state_param_); - - // Config - topic_configs_ = getConfigs(this->get_node_parameters_interface(), "topic_configs"); - tf_configs_ = getConfigs(this->get_node_parameters_interface(), "tf_configs"); - - // Callback Groups - callback_group_subscribers_ = - this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); - callback_group_services_ = - this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); - auto subscriber_option = rclcpp::SubscriptionOptions(); - subscriber_option.callback_group = callback_group_subscribers_; - - // Topic Callback - for (const auto & topic_config : topic_configs_) { - registerTopicCallback( - topic_config.name, topic_config.type, topic_config.transient_local, topic_config.best_effort); - } - - // Subscriber - sub_autoware_engage_ = this->create_subscription( - "input/autoware_engage", 1, std::bind(&AutowareStateMonitorNode::onAutowareEngage, this, _1), - subscriber_option); - sub_control_mode_ = this->create_subscription( - "input/control_mode", 1, std::bind(&AutowareStateMonitorNode::onVehicleControlMode, this, _1), - subscriber_option); - sub_map_ = create_subscription( - "input/vector_map", rclcpp::QoS{1}.transient_local(), - std::bind(&AutowareStateMonitorNode::onMap, this, _1), subscriber_option); - sub_route_ = this->create_subscription( - "input/route", rclcpp::QoS{1}.transient_local(), - std::bind(&AutowareStateMonitorNode::onRoute, this, _1), subscriber_option); - sub_modified_goal_ = this->create_subscription( - "input/modified_goal", 1, std::bind(&AutowareStateMonitorNode::onModifiedGoal, this, _1), - subscriber_option); - sub_odom_ = this->create_subscription( - "input/odometry", 100, std::bind(&AutowareStateMonitorNode::onOdometry, this, _1), - subscriber_option); - - // Service - srv_shutdown_ = this->create_service( - "service/shutdown", std::bind(&AutowareStateMonitorNode::onShutdownService, this, _1, _2, _3), - rmw_qos_profile_services_default, callback_group_services_); - srv_reset_route_ = this->create_service( - "service/reset_route", - std::bind(&AutowareStateMonitorNode::onResetRouteService, this, _1, _2, _3), - rmw_qos_profile_services_default, callback_group_services_); - - // Publisher - pub_autoware_state_ = this->create_publisher( - "output/autoware_state", 1); - pub_autoware_engage_ = - this->create_publisher("output/autoware_engage", 1); - - // Diagnostic Updater - setupDiagnosticUpdater(); - - // Wait for first topics - std::this_thread::sleep_for(std::chrono::milliseconds(1000)); - - // Timer - const auto period_ns = rclcpp::Rate(update_rate_).period(); - timer_ = rclcpp::create_timer( - this, get_clock(), period_ns, std::bind(&AutowareStateMonitorNode::onTimer, this), - callback_group_subscribers_); -} diff --git a/system/ad_service_state_monitor/src/ad_service_state_monitor_node/diagnostics.cpp b/system/ad_service_state_monitor/src/ad_service_state_monitor_node/diagnostics.cpp deleted file mode 100644 index 503547370029b..0000000000000 --- a/system/ad_service_state_monitor/src/ad_service_state_monitor_node/diagnostics.cpp +++ /dev/null @@ -1,181 +0,0 @@ -// Copyright 2020 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 "ad_service_state_monitor/ad_service_state_monitor_node.hpp" - -#include -#include - -#define FMT_HEADER_ONLY -#include - -void AutowareStateMonitorNode::setupDiagnosticUpdater() -{ - updater_.setHardwareID("ad_service_state_monitor"); - - const auto module_names = this->declare_parameter>("module_names"); - - // Topic - for (const auto & module_name : module_names) { - const auto diag_name = fmt::format("{}_topic_status", module_name); - - updater_.add( - diag_name, - std::bind( - &AutowareStateMonitorNode::checkTopicStatus, this, std::placeholders::_1, module_name)); - } - - // TF - updater_.add( - "localization_tf_status", - std::bind( - &AutowareStateMonitorNode::checkTFStatus, this, std::placeholders::_1, "localization")); -} - -void AutowareStateMonitorNode::checkTopicStatus( - diagnostic_updater::DiagnosticStatusWrapper & stat, const std::string & module_name) -{ - int8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; - - std::lock_guard lock(lock_state_input_); - const auto & topic_stats = state_input_.topic_stats; - - // OK - for (const auto & topic_config : topic_stats.ok_list) { - if (topic_config.module != module_name) { - continue; - } - - stat.add(fmt::format("{} status", topic_config.name), "OK"); - } - - // Check topic received - for (const auto & topic_config : topic_stats.non_received_list) { - if (topic_config.module != module_name) { - continue; - } - - stat.add(fmt::format("{} status", topic_config.name), "Not Received"); - - level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; - } - - // Check topic rate - for (const auto & topic_config_pair : topic_stats.slow_rate_list) { - const auto & topic_config = topic_config_pair.first; - const auto & topic_rate = topic_config_pair.second; - - if (topic_config.module != module_name) { - continue; - } - - const auto & name = topic_config.name; - stat.add(fmt::format("{} status", name), "Slow Rate"); - stat.addf(fmt::format("{} warn_rate", name), "%.2f [Hz]", topic_config.warn_rate); - stat.addf(fmt::format("{} measured_rate", name), "%.2f [Hz]", topic_rate); - - level = diagnostic_msgs::msg::DiagnosticStatus::WARN; - } - - // Check topic timeout - for (const auto & topic_config_pair : topic_stats.timeout_list) { - const auto & topic_config = topic_config_pair.first; - const auto & last_received_time = topic_config_pair.second; - - if (topic_config.module != module_name) { - continue; - } - - const auto & name = topic_config.name; - stat.add(fmt::format("{} status", name), "Timeout"); - stat.addf(fmt::format("{} timeout", name), "%.2f [s]", topic_config.timeout); - stat.addf(fmt::format("{} checked_time", name), "%.2f [s]", topic_stats.checked_time.seconds()); - stat.addf(fmt::format("{} last_received_time", name), "%.2f [s]", last_received_time.seconds()); - - level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; - } - - // Create message - std::string msg; - if (level == diagnostic_msgs::msg::DiagnosticStatus::OK) { - msg = "OK"; - } else if (level == diagnostic_msgs::msg::DiagnosticStatus::WARN) { - msg = "Warn"; - } else if (level == diagnostic_msgs::msg::DiagnosticStatus::ERROR) { - msg = "Error"; - } - - stat.summary(level, msg); -} - -void AutowareStateMonitorNode::checkTFStatus( - diagnostic_updater::DiagnosticStatusWrapper & stat, const std::string & module_name) -{ - int8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; - - std::lock_guard lock(lock_state_input_); - const auto & tf_stats = state_input_.tf_stats; - - // OK - for (const auto & tf_config : tf_stats.ok_list) { - if (tf_config.module != module_name) { - continue; - } - - const auto name = fmt::format("{}2{}", tf_config.from, tf_config.to); - stat.add(fmt::format("{} status", name), "OK"); - } - - // Check tf received - for (const auto & tf_config : tf_stats.non_received_list) { - if (tf_config.module != module_name) { - continue; - } - - const auto name = fmt::format("{}2{}", tf_config.from, tf_config.to); - stat.add(fmt::format("{} status", name), "Not Received"); - - level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; - } - - // Check tf timeout - for (const auto & tf_config_pair : tf_stats.timeout_list) { - const auto & tf_config = tf_config_pair.first; - const auto & last_received_time = tf_config_pair.second; - - if (tf_config.module != module_name) { - continue; - } - - const auto name = fmt::format("{}2{}", tf_config.from, tf_config.to); - stat.add(fmt::format("{} status", name), "Timeout"); - stat.addf(fmt::format("{} timeout", name), "%.2f [s]", tf_config.timeout); - stat.addf(fmt::format("{} checked_time", name), "%.2f [s]", tf_stats.checked_time.seconds()); - stat.addf(fmt::format("{} last_received_time", name), "%.2f [s]", last_received_time.seconds()); - - level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; - } - - // Create message - std::string msg; - if (level == diagnostic_msgs::msg::DiagnosticStatus::OK) { - msg = "OK"; - } else if (level == diagnostic_msgs::msg::DiagnosticStatus::WARN) { - msg = "Warn"; - } else if (level == diagnostic_msgs::msg::DiagnosticStatus::ERROR) { - msg = "Error"; - } - - stat.summary(level, msg); -} diff --git a/system/ad_service_state_monitor/src/ad_service_state_monitor_node/main.cpp b/system/ad_service_state_monitor/src/ad_service_state_monitor_node/main.cpp deleted file mode 100644 index 84e28f401e7e6..0000000000000 --- a/system/ad_service_state_monitor/src/ad_service_state_monitor_node/main.cpp +++ /dev/null @@ -1,31 +0,0 @@ -// Copyright 2020 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 "ad_service_state_monitor/ad_service_state_monitor_node.hpp" - -#include - -#include - -int main(int argc, char * argv[]) -{ - rclcpp::init(argc, argv); - auto node = std::make_shared(); - auto executor = std::make_shared(); - executor->add_node(node); - executor->spin(); - rclcpp::shutdown(); - - return 0; -} diff --git a/system/ad_service_state_monitor/src/ad_service_state_monitor_node/state_machine.cpp b/system/ad_service_state_monitor/src/ad_service_state_monitor_node/state_machine.cpp deleted file mode 100644 index 8eada23c7c7eb..0000000000000 --- a/system/ad_service_state_monitor/src/ad_service_state_monitor_node/state_machine.cpp +++ /dev/null @@ -1,350 +0,0 @@ -// Copyright 2020 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 "ad_service_state_monitor/state_machine.hpp" - -#include -#include - -#define FMT_HEADER_ONLY // NOLINT -#include -#include - -#ifdef ROS_DISTRO_GALACTIC -#include -#else -#include -#endif - -namespace -{ -double calcDistance2d(const geometry_msgs::msg::Point & p1, const geometry_msgs::msg::Point & p2) -{ - return std::hypot(p1.x - p2.x, p1.y - p2.y); -} - -double calcDistance2d(const geometry_msgs::msg::Pose & p1, const geometry_msgs::msg::Pose & p2) -{ - return calcDistance2d(p1.position, p2.position); -} - -bool isValidAngle( - const geometry_msgs::msg::Pose & current_pose, const geometry_msgs::msg::Pose & ref_pose, - const double th_angle_rad) -{ - const double yaw_curr = tf2::getYaw(current_pose.orientation); - const double yaw_ref = tf2::getYaw(ref_pose.orientation); - const double yaw_diff = tier4_autoware_utils::normalizeRadian(yaw_curr - yaw_ref); - return std::fabs(yaw_diff) < th_angle_rad; -} - -bool isNearGoal( - const geometry_msgs::msg::Pose & current_pose, const geometry_msgs::msg::Pose & goal_pose, - const double th_dist) -{ - return calcDistance2d(current_pose, goal_pose) < th_dist; -} - -bool isStopped( - const std::deque & odometry_buffer, - const double th_stopped_velocity_mps) -{ - for (const auto & odometry : odometry_buffer) { - if (std::abs(odometry->twist.twist.linear.x) > th_stopped_velocity_mps) { - return false; - } - } - return true; -} - -template -std::vector filterConfigByModuleName(const std::vector & configs, const char * module_name) -{ - std::vector filtered; - - for (const auto & config : configs) { - if (config.module == module_name) { - filtered.push_back(config); - } - } - - return filtered; -} - -} // namespace - -bool StateMachine::isModuleInitialized(const char * module_name) const -{ - const auto non_received_topics = - filterConfigByModuleName(state_input_.topic_stats.non_received_list, module_name); - const auto non_set_params = - filterConfigByModuleName(state_input_.param_stats.non_set_list, module_name); - const auto non_received_tfs = - filterConfigByModuleName(state_input_.tf_stats.non_received_list, module_name); - - if (non_received_topics.empty() && non_set_params.empty() && non_received_tfs.empty()) { - return true; - } - - for (const auto & topic_config : non_received_topics) { - const auto msg = fmt::format("topic `{}` is not received yet", topic_config.name); - msgs_.push_back(msg); - } - - for (const auto & param_config : non_set_params) { - const auto msg = fmt::format("param `{}` is not set", param_config.name); - msgs_.push_back(msg); - } - - for (const auto & tf_config : non_received_tfs) { - const auto msg = - fmt::format("tf from `{}` to `{}` is not received yet", tf_config.from, tf_config.to); - msgs_.push_back(msg); - } - - { - const auto msg = fmt::format("module `{}` is not initialized", module_name); - msgs_.push_back(msg); - } - - return false; -} - -bool StateMachine::isVehicleInitialized() const -{ - if (!isModuleInitialized(ModuleName::map)) { - return false; - } - - if (!isModuleInitialized(ModuleName::vehicle)) { - return false; - } - - if (!isModuleInitialized(ModuleName::sensing)) { - return false; - } - - if (!isModuleInitialized(ModuleName::localization)) { - return false; - } - - if (!isModuleInitialized(ModuleName::perception)) { - return false; - } - - // TODO(Kenji Miyake): Check if the vehicle is on a lane? - - return true; -} - -bool StateMachine::hasRoute() const { return state_input_.route != nullptr; } - -bool StateMachine::isRouteReceived() const { return state_input_.route != executing_route_; } - -bool StateMachine::isPlanningCompleted() const -{ - if (!isModuleInitialized(ModuleName::planning)) { - return false; - } - - if (!isModuleInitialized(ModuleName::control)) { - return false; - } - - return true; -} - -bool StateMachine::isEngaged() const -{ - if (!state_input_.autoware_engage) { - return false; - } - - if (state_input_.autoware_engage->engage != 1) { - return false; - } - - if (!state_input_.control_mode_) { - return false; - } - - if ( - state_input_.control_mode_->mode == - autoware_auto_vehicle_msgs::msg::ControlModeReport::MANUAL) { - return false; - } - - return true; -} - -bool StateMachine::isOverridden() const { return !isEngaged(); } - -bool StateMachine::hasArrivedGoal() const -{ - geometry_msgs::msg::Pose goal_pose = *state_input_.goal_pose; - if ( - state_input_.modified_goal_pose != nullptr && - rclcpp::Time(state_input_.route->header.stamp).seconds() == - rclcpp::Time(state_input_.modified_goal_pose->header.stamp).seconds()) { - goal_pose = state_input_.modified_goal_pose->pose; - } - - const auto is_valid_goal_angle = - isValidAngle(state_input_.current_pose->pose, goal_pose, state_param_.th_arrived_angle); - const auto is_near_goal = - isNearGoal(state_input_.current_pose->pose, goal_pose, state_param_.th_arrived_distance_m); - const auto is_stopped = - isStopped(state_input_.odometry_buffer, state_param_.th_stopped_velocity_mps); - - if (is_valid_goal_angle && is_near_goal && is_stopped) { - return true; - } - - return false; -} - -bool StateMachine::isFinalizing() const { return state_input_.is_finalizing; } - -bool StateMachine::isRouteResetRequired() const { return state_input_.is_route_reset_required; } - -AutowareState StateMachine::updateState(const StateInput & state_input) -{ - msgs_ = {}; - state_input_ = state_input; - autoware_state_ = judgeAutowareState(); - return autoware_state_; -} - -AutowareState StateMachine::judgeAutowareState() const -{ - if (isFinalizing()) { - return AutowareState::Finalizing; - } - - switch (autoware_state_) { - case (AutowareState::InitializingVehicle): { - if (isVehicleInitialized()) { - if (!flags_.waiting_after_initializing) { - flags_.waiting_after_initializing = true; - times_.initializing_completed = state_input_.current_time; - break; - } - - // Wait after initialize completed to avoid sync error - constexpr double wait_time_after_initializing = 1.0; - const auto time_from_initializing = - state_input_.current_time - times_.initializing_completed; - if (time_from_initializing.seconds() > wait_time_after_initializing) { - flags_.waiting_after_initializing = false; - return AutowareState::WaitingForRoute; - } - } - - break; - } - - case (AutowareState::WaitingForRoute): { - if (isRouteReceived()) { - return AutowareState::Planning; - } - - if (hasRoute() && isEngaged() && !hasArrivedGoal()) { - return AutowareState::Driving; - } - - break; - } - - case (AutowareState::Planning): { - executing_route_ = state_input_.route; - - if (isPlanningCompleted()) { - if (!flags_.waiting_after_planning) { - flags_.waiting_after_planning = true; - times_.planning_completed = state_input_.current_time; - break; - } - - // Wait after planning completed to avoid sync error - constexpr double wait_time_after_planning = 3.0; - const auto time_from_planning = state_input_.current_time - times_.planning_completed; - if (time_from_planning.seconds() > wait_time_after_planning) { - flags_.waiting_after_planning = false; - return AutowareState::WaitingForEngage; - } - } - - break; - } - - case (AutowareState::WaitingForEngage): { - if (isRouteResetRequired()) { - return AutowareState::WaitingForRoute; - } - - if (isRouteReceived()) { - return AutowareState::Planning; - } - - if (isEngaged()) { - return AutowareState::Driving; - } - - if (hasArrivedGoal()) { - times_.arrived_goal = state_input_.current_time; - return AutowareState::ArrivedGoal; - } - - break; - } - - case (AutowareState::Driving): { - if (isRouteReceived()) { - return AutowareState::Planning; - } - - if (isOverridden()) { - return AutowareState::WaitingForEngage; - } - - if (hasArrivedGoal()) { - times_.arrived_goal = state_input_.current_time; - return AutowareState::ArrivedGoal; - } - - break; - } - - case (AutowareState::ArrivedGoal): { - constexpr double wait_time_after_arrived_goal = 2.0; - const auto time_from_arrived_goal = state_input_.current_time - times_.arrived_goal; - if (time_from_arrived_goal.seconds() > wait_time_after_arrived_goal) { - return AutowareState::WaitingForRoute; - } - - break; - } - - case (AutowareState::Finalizing): { - break; - } - - default: { - throw std::runtime_error("invalid state"); - } - } - - // continue previous state when break - return autoware_state_; -} From 5e9d0fa5cce1cbe2a9aa5a28fc74ae981c261840 Mon Sep 17 00:00:00 2001 From: "Takagi, Isamu" Date: Thu, 17 Nov 2022 18:42:47 +0900 Subject: [PATCH 27/34] feat: apply removing of ad_service_state_monitor Signed-off-by: Takagi, Isamu --- .github/CODEOWNERS | 1 - .../ad_service_state_monitor.param.yaml | 155 --------- ...ate_monitor.planning_simulation.param.yaml | 126 -------- .../component_state_monitor/topics.yaml | 184 +++++++++++ .../launch/system.launch.xml | 15 +- launch/tier4_system_launch/package.xml | 2 +- .../system_launch.drawio.svg | 301 +++++++----------- .../config/topics.yaml | 2 + 8 files changed, 299 insertions(+), 487 deletions(-) delete mode 100644 launch/tier4_system_launch/config/ad_service_state_monitor/ad_service_state_monitor.param.yaml delete mode 100644 launch/tier4_system_launch/config/ad_service_state_monitor/ad_service_state_monitor.planning_simulation.param.yaml create mode 100644 launch/tier4_system_launch/config/component_state_monitor/topics.yaml diff --git a/.github/CODEOWNERS b/.github/CODEOWNERS index c5f00c95dcb5e..c59a7cd754db5 100644 --- a/.github/CODEOWNERS +++ b/.github/CODEOWNERS @@ -142,7 +142,6 @@ sensing/vehicle_velocity_converter/** ryu.yamamoto@tier4.jp simulator/dummy_perception_publisher/** yukihiro.saito@tier4.jp simulator/fault_injection/** keisuke.shima@tier4.jp simulator/simple_planning_simulator/** takamasa.horibe@tier4.jp -system/ad_service_state_monitor/** kenji.miyake@tier4.jp system/bluetooth_monitor/** fumihito.ito@tier4.jp system/component_state_monitor/** isamu.takagi@tier4.jp system/default_ad_api/** isamu.takagi@tier4.jp diff --git a/launch/tier4_system_launch/config/ad_service_state_monitor/ad_service_state_monitor.param.yaml b/launch/tier4_system_launch/config/ad_service_state_monitor/ad_service_state_monitor.param.yaml deleted file mode 100644 index 7a16c35390490..0000000000000 --- a/launch/tier4_system_launch/config/ad_service_state_monitor/ad_service_state_monitor.param.yaml +++ /dev/null @@ -1,155 +0,0 @@ -# AD Service State Monitor Parameters -/**: - ros__parameters: - # modules_names: string array - module_names: [ - "map", - "sensing", - "perception", - "localization", - "planning", - "control", - "vehicle", - "system" - ] - - # Topic Config - ## names: string array - ## configs: top level - ### names used to declare parameter group - #### module: string - #### timeout[s]: double (0: none) - #### warn_rate[hz]: double (0: none) - topic_configs: - names: [ - "/map/vector_map", - "/map/pointcloud_map", - "/perception/obstacle_segmentation/pointcloud", - "/perception/object_recognition/objects", - "/initialpose3d", - "/localization/pose_twist_fusion_filter/pose", - "/planning/mission_planning/route", - "/planning/scenario_planning/trajectory", - "/control/trajectory_follower/control_cmd", - "/control/command/control_cmd", - "/vehicle/status/velocity_status", - "/vehicle/status/steering_status", - "/system/emergency/control_cmd" - ] - - configs: - /map/vector_map: - module: "map" - timeout: 0.0 - warn_rate: 0.0 - type: "autoware_auto_mapping_msgs/msg/HADMapBin" - transient_local: True - - /map/pointcloud_map: - module: "map" - timeout: 0.0 - warn_rate: 0.0 - type: "sensor_msgs/msg/PointCloud2" - transient_local: True - - /perception/obstacle_segmentation/pointcloud: - module: "sensing" - timeout: 1.0 - warn_rate: 5.0 - type: "sensor_msgs/msg/PointCloud2" - best_effort: True - - /initialpose3d: - module: "localization" - timeout: 0.0 - warn_rate: 0.0 - type: "geometry_msgs/msg/PoseWithCovarianceStamped" - - /localization/pose_twist_fusion_filter/pose: - module: "localization" - timeout: 1.0 - warn_rate: 5.0 - type: "geometry_msgs/msg/PoseStamped" - - # Can be both with feature array or without - # In prediction.launch.xml this is set to without - /perception/object_recognition/objects: - module: "perception" - timeout: 1.0 - warn_rate: 5.0 - type: "autoware_auto_perception_msgs/msg/PredictedObjects" - # This topic could have two different types depending on the launch flags used. - # The implementation of subscriptions in ROS2 does not allow for multiple types - # to be defined for a topic. By default this is set to not use have features. - # type: ["tier4_perception_msgs/msg/DynamicObjectArray", "tier4_perception_msgs/msg/DynamicObjectWithFeatureArray"] - - /planning/mission_planning/route: - module: "planning" - timeout: 0.0 - warn_rate: 0.0 - type: "autoware_auto_planning_msgs/msg/HADMapRoute" - transient_local: True - - /planning/scenario_planning/trajectory: - module: "planning" - timeout: 1.0 - warn_rate: 5.0 - type: "autoware_auto_planning_msgs/msg/Trajectory" - - /control/trajectory_follower/control_cmd: - module: "control" - timeout: 1.0 - warn_rate: 5.0 - type: "autoware_auto_control_msgs/msg/AckermannControlCommand" - - /control/command/control_cmd: - module: "control" - timeout: 1.0 - warn_rate: 5.0 - type: "autoware_auto_control_msgs/msg/AckermannControlCommand" - - /vehicle/status/velocity_status: - module: "vehicle" - timeout: 1.0 - warn_rate: 5.0 - type: "autoware_auto_vehicle_msgs/msg/VelocityReport" - - /vehicle/status/steering_status: - module: "vehicle" - timeout: 1.0 - warn_rate: 5.0 - type: "autoware_auto_vehicle_msgs/msg/SteeringReport" - - /system/emergency/control_cmd: - module: "system" - timeout: 1.0 - warn_rate: 5.0 - type: "autoware_auto_control_msgs/msg/AckermannControlCommand" - - # Param Config - ## names: string array - ## configs: top level - ### names used to declare parameter group - #### module: string - # param_configs: - # names: ["/vehicle_info"] - # configs: - # /vehicle_info: - # module: "vehicle" - - # TF Config - ## names: string array - ## configs: top level - ### names used to declare parameter group - #### module: string - #### from: string - #### to: string - #### timeout[s]: double (0: none) - tf_configs: - names: ["map_to_base_link"] - configs: - map_to_base_link: - module: "localization" - from: "map" - to: "base_link" - timeout: 1.0 diff --git a/launch/tier4_system_launch/config/ad_service_state_monitor/ad_service_state_monitor.planning_simulation.param.yaml b/launch/tier4_system_launch/config/ad_service_state_monitor/ad_service_state_monitor.planning_simulation.param.yaml deleted file mode 100644 index 51fbe99dd9bc3..0000000000000 --- a/launch/tier4_system_launch/config/ad_service_state_monitor/ad_service_state_monitor.planning_simulation.param.yaml +++ /dev/null @@ -1,126 +0,0 @@ -# AD Service State Monitor: Planning Simulator Parameters -/**: - ros__parameters: - # modules_names: string array - module_names: [ - "map", - "sensing", - "perception", - "localization", - "planning", - "control", - "vehicle", - "system" - ] - -# Topic Config - ## names: string array - ## configs: top level - ### names used to declare parameter group - #### module: string - #### timeout[s]: double (0: none) - #### warn_rate[hz]: double (0: none) - topic_configs: - names: [ - "/map/vector_map", - "/perception/object_recognition/objects", - "/initialpose3d", - "/planning/mission_planning/route", - "/planning/scenario_planning/trajectory", - "/control/trajectory_follower/control_cmd", - "/control/command/control_cmd", - "/vehicle/status/velocity_status", - "/vehicle/status/steering_status", - "/system/emergency/control_cmd" - ] - - configs: - /map/vector_map: - module: "map" - timeout: 0.0 - warn_rate: 0.0 - type: "autoware_auto_mapping_msgs/msg/HADMapBin" - transient_local: True - - /perception/object_recognition/objects: - module: "perception" - timeout: 1.0 - warn_rate: 5.0 - type: "autoware_auto_perception_msgs/msg/PredictedObjects" - - /initialpose3d: - module: "localization" - timeout: 0.0 - warn_rate: 0.0 - type: "geometry_msgs/msg/PoseWithCovarianceStamped" - - /planning/mission_planning/route: - module: "planning" - timeout: 0.0 - warn_rate: 0.0 - type: "autoware_auto_planning_msgs/msg/HADMapRoute" - transient_local: True - - /planning/scenario_planning/trajectory: - module: "planning" - timeout: 1.0 - warn_rate: 5.0 - type: "autoware_auto_planning_msgs/msg/Trajectory" - - /control/trajectory_follower/control_cmd: - module: "control" - timeout: 1.0 - warn_rate: 5.0 - type: "autoware_auto_control_msgs/msg/AckermannControlCommand" - - /control/command/control_cmd: - module: "control" - timeout: 1.0 - warn_rate: 5.0 - type: "autoware_auto_control_msgs/msg/AckermannControlCommand" - - /vehicle/status/velocity_status: - module: "vehicle" - timeout: 1.0 - warn_rate: 5.0 - type: "autoware_auto_vehicle_msgs/msg/VelocityReport" - - /vehicle/status/steering_status: - module: "vehicle" - timeout: 1.0 - warn_rate: 5.0 - type: "autoware_auto_vehicle_msgs/msg/SteeringReport" - - /system/emergency/control_cmd: - module: "system" - timeout: 1.0 - warn_rate: 5.0 - type: "autoware_auto_control_msgs/msg/AckermannControlCommand" - - # Param Config - ## names: string array - ## configs: top level - ### names used to declare parameter group - #### module: string - # param_configs: - # names: ["/vehicle_info"] - # configs: - # /vehicle_info: - # module: "vehicle" - - # TF Config - ## names: string array - ## configs: top level - ### names used to declare parameter group - #### module: string - #### from: string - #### to: string - #### timeout[s]: double (0: none) - tf_configs: - names: ["map_to_base_link"] - configs: - map_to_base_link: - module: "localization" - from: "map" - to: "base_link" - timeout: 1.0 diff --git a/launch/tier4_system_launch/config/component_state_monitor/topics.yaml b/launch/tier4_system_launch/config/component_state_monitor/topics.yaml new file mode 100644 index 0000000000000..ed8f201232410 --- /dev/null +++ b/launch/tier4_system_launch/config/component_state_monitor/topics.yaml @@ -0,0 +1,184 @@ +# This file is copied from component_state_monitor/config/topics.yaml. + +- module: map + mode: [online, planning_simulation] + type: launch + args: + node_name_suffix: vector_map + topic: /map/vector_map + topic_type: autoware_auto_mapping_msgs/msg/HADMapBin + best_effort: false + transient_local: true + warn_rate: 0.0 + error_rate: 0.0 + timeout: 0.0 + +- module: map + mode: [online] + type: launch + args: + node_name_suffix: pointcloud_map + topic: /map/pointcloud_map + topic_type: sensor_msgs/msg/PointCloud2 + best_effort: false + transient_local: true + warn_rate: 0.0 + error_rate: 0.0 + timeout: 0.0 + +- module: localization + mode: [online, planning_simulation] + type: autonomous + args: + node_name_suffix: initialpose3d + topic: /initialpose3d + topic_type: geometry_msgs/msg/PoseWithCovarianceStamped + best_effort: false + transient_local: false + warn_rate: 0.0 + error_rate: 0.0 + timeout: 0.0 + +- module: localization + mode: [online] + type: autonomous + args: + node_name_suffix: pose_twist_fusion_filter_pose + topic: /localization/pose_twist_fusion_filter/pose + topic_type: geometry_msgs/msg/PoseStamped + best_effort: false + transient_local: false + warn_rate: 5.0 + error_rate: 1.0 + timeout: 1.0 + +- module: perception + mode: [online] + type: launch + args: + node_name_suffix: obstacle_segmentation_pointcloud + topic: /perception/obstacle_segmentation/pointcloud + topic_type: sensor_msgs/msg/PointCloud2 + best_effort: true + transient_local: false + warn_rate: 5.0 + error_rate: 1.0 + timeout: 1.0 + +- module: perception + mode: [online, planning_simulation] + type: autonomous + args: + node_name_suffix: object_recognition_objects + topic: /perception/object_recognition/objects + topic_type: autoware_auto_perception_msgs/msg/PredictedObjects + best_effort: false + transient_local: false + warn_rate: 5.0 + error_rate: 1.0 + timeout: 1.0 + +- module: planning + mode: [online, planning_simulation] + type: autonomous + args: + node_name_suffix: mission_planning_route + topic: /planning/mission_planning/route + topic_type: autoware_auto_planning_msgs/msg/HADMapRoute + best_effort: false + transient_local: true + warn_rate: 0.0 + error_rate: 0.0 + timeout: 0.0 + +- module: planning + mode: [online, planning_simulation] + type: autonomous + args: + node_name_suffix: scenario_planning_trajectory + topic: /planning/scenario_planning/trajectory + topic_type: autoware_auto_planning_msgs/msg/Trajectory + best_effort: false + transient_local: false + warn_rate: 5.0 + error_rate: 1.0 + timeout: 1.0 + +- module: control + mode: [online, planning_simulation] + type: autonomous + args: + node_name_suffix: trajectory_follower_control_cmd + topic: /control/trajectory_follower/control_cmd + topic_type: autoware_auto_control_msgs/msg/AckermannControlCommand + best_effort: false + transient_local: false + warn_rate: 5.0 + error_rate: 1.0 + timeout: 1.0 + +- module: control + mode: [online, planning_simulation] + type: autonomous + args: + node_name_suffix: control_command_control_cmd + topic: /control/command/control_cmd + topic_type: autoware_auto_control_msgs/msg/AckermannControlCommand + best_effort: false + transient_local: false + warn_rate: 5.0 + error_rate: 1.0 + timeout: 1.0 + +- module: vehicle + mode: [online, planning_simulation] + type: autonomous + args: + node_name_suffix: vehicle_status_velocity_status + topic: /vehicle/status/velocity_status + topic_type: autoware_auto_vehicle_msgs/msg/VelocityReport + best_effort: false + transient_local: false + warn_rate: 5.0 + error_rate: 1.0 + timeout: 1.0 + +- module: vehicle + mode: [online, planning_simulation] + type: autonomous + args: + node_name_suffix: vehicle_status_steering_status + topic: /vehicle/status/steering_status + topic_type: autoware_auto_vehicle_msgs/msg/SteeringReport + best_effort: false + transient_local: false + warn_rate: 5.0 + error_rate: 1.0 + timeout: 1.0 + +- module: system + mode: [online, planning_simulation] + type: launch + args: + node_name_suffix: system_emergency_control_cmd + topic: /system/emergency/control_cmd + topic_type: autoware_auto_control_msgs/msg/AckermannControlCommand + best_effort: false + transient_local: false + warn_rate: 5.0 + error_rate: 1.0 + timeout: 1.0 + +- module: localization + mode: [online, planning_simulation] + type: autonomous + args: + node_name_suffix: transform_map_to_base_link + topic: /tf + frame_id: map + child_frame_id: base_link + best_effort: false + transient_local: false + warn_rate: 5.0 + error_rate: 1.0 + timeout: 1.0 diff --git a/launch/tier4_system_launch/launch/system.launch.xml b/launch/tier4_system_launch/launch/system.launch.xml index 258a9820a9565..2071faab3e247 100644 --- a/launch/tier4_system_launch/launch/system.launch.xml +++ b/launch/tier4_system_launch/launch/system.launch.xml @@ -23,24 +23,11 @@ - - - - - - - - - - + diff --git a/launch/tier4_system_launch/package.xml b/launch/tier4_system_launch/package.xml index f496ac854ace3..beb2a4a782e22 100644 --- a/launch/tier4_system_launch/package.xml +++ b/launch/tier4_system_launch/package.xml @@ -12,7 +12,7 @@ autoware_cmake - ad_service_state_monitor + component_state_monitor emergency_handler system_error_monitor system_monitor diff --git a/launch/tier4_system_launch/system_launch.drawio.svg b/launch/tier4_system_launch/system_launch.drawio.svg index 9b32dc4cc60d2..334012a85b351 100644 --- a/launch/tier4_system_launch/system_launch.drawio.svg +++ b/launch/tier4_system_launch/system_launch.drawio.svg @@ -3,43 +3,54 @@ xmlns="http://www.w3.org/2000/svg" xmlns:xlink="http://www.w3.org/1999/xlink" version="1.1" - width="1209px" - height="1051px" - viewBox="-0.5 -0.5 1209 1051" - content="<mxfile><diagram id="hw8opUG3N7cuFXbovIOC" name="ページ1">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</diagram></mxfile>" + width="1131px" + height="882px" + viewBox="-0.5 -0.5 1131 882" + content="<mxfile><diagram id="hw8opUG3N7cuFXbovIOC" name="ページ1">7VpLk6M2EP41rtocxoV5++h5JHvYVKVqDklOLhnEYxckIoRt9tdHAvEQAntmjedRMzMHo5Zoqfv7Wt2SvTDu0uMfBGTRn9iHyULX/OPCuF/oum6sTfbBJWUtWTtOLQhJ7NeiVSd4jH9CIdSEtIh9mEsDKcYJjTNZ6GGEoEclGSAEH+RhAU7kWTMQQkXw6IFElf4d+zSqpa7udPKvMA6jZuaVva57UtAMFpbkEfDxoScyHhbGHcGY1k/p8Q4m3HmNX+r3fp/obRdGIKJPecGxxDpo2RgHfWaraCKM2MdtRNOEtVbssR7Ox0zOKEQ5LognRukCJEBCKEYZrbWMJhCnkJKSDSEwATTey9qBwCtsx3UmsQdh1YSF9kkLMaERDjECyUMnvYrBtnahxdWrG0JA2RuQ4RjRvKf5Ly5gA5pgcwXTRKgphDgz3nC1gbfrFXS+b015EhyufhkcAUZUbAgra1Z43PcIj63PDI/xRuFxVu8RHteeFx7hmD1ICmFxXuYUpssEFMiLlkcGhG4nzGe3O8KeQlphMCXx4/1QlGcANbIMeD+qTLjh+FQTbeuJmtFsvf0XemJJ9YBRBBfIh9wkjXUfopjCRzYX7z2wgmHAqDhJ7nCCSfWu4XnQCgK+UErwD9jrMWxjbfiCgz25tuH/TL6HhMYsiW+SOESsL419v+IvEAKPcRKSdsUjJOUq4LEnUgkpegdEWAkiaIeuXmhlUb9WaCqDS3KeMUGTbYpRTDFp6JKVz2ZLD0f7v4KXKbfMI/RG+JAzJYEB7XpPkUks523Spvqbhwy2dp4Npnk1NpxOuaLGk73bbfTfMM6ET79DSkuxu4OCYtnjzD2k/Kff+JcrW1pN8/4olNetUrQm/TvIGpfu/cpmvWq9K2AxXUvWUeck8dqJbf+8pjqVKZp+IQUYanA3e7KGQApfLqLPpoymIS/sXeSMGYN/kAnMJyYCd47INxWu8DVvFO9ywGUXyq4Se0Tfr0Kk5M7JLDuGmYxqr3g0L3X+tLfdKznb0hRnI+wP2f8Zlm2QwZVvQWcsLNe2YwD7umH5mnFprRWqYBqx8PnkygRXGFNc3xzjiqvvDPvKXDFfkSu2uq14OM3Y7ovoNqeAwrdS1dsg5SCiXZ5NUGti4R+1MhgeCwz9JY8FjnvyWHD2qqet99uS/jn1/nOvhGyxWunKbj3HueC5d0ItIgI1y5z3jscZSQ4oiVm1NQbXN7CDiQzM00syAvP4J9hV+jg0wnSm3LpdWPeLqfs8heLtVy1C2aL9gqMPqzNxxSo0aUtTX68l34qT64UHvZvmQkRovVm5sgocBDm89GDWbPave3U6FifOq3zzMIwTR5v5qrrZb7s4yRKAUIzCbR6nBbcNo3cfNO4EeA23taXmyFlklpjRZaWN1lkjpuFqD8GFbn7ZA14IkwKx+sCHv6kJP8LprsjPJ/sZUrRhDzZ79yVLPzUT8Gu3AzNoCwnBZFj5/cr1/zwXuuPrmqrg5jy6qGZdYza2m+Z1P4vZIA63rJDkp6JKldbMwQO/GtKUi50aIwiqwlHVPI4oYy1IlyXoEGVm1fo/aHVsjoTeFatj9dj1GXsfJfbUMuIzHpV4tEduQa4Yj2q1B1PIalzkldsIIFabvYFQVJb04VjhaldkBWt2P42rC87uB4bGw/8=</diagram></mxfile>" > - + + + + + + + + + - -
-
-
+ +
+
+
system.launch.xml

- package: tier4_system_launch + package: system_launch
- system.launch.xml... + system.launch.xml... - + - +
-
-
+
+
system_monitor.launch.py

@@ -48,70 +59,16 @@
- system_monitor.launch.py... + system_monitor.launch.py... - - - - - - + - -
-
-
- online -
-
-
- - online - - - - - - - -
-
-
- planning_simulation -
-
-
- - planning_simulation - - - - - - -
-
-
- $(var run_mode) -
-
-
- - $(var run_mode) - - - - - - -
-
-
+ +
+
+
launch name

@@ -122,32 +79,34 @@
- launch name... + launch name... - + - -
-
-
ex:
+ +
+
+
+ ex: +
- ex: + ex: - + - +
-
-
+
+
node name

@@ -158,19 +117,19 @@
- node name... + node name... - + - +
-
-
+
+
other name

@@ -181,189 +140,153 @@
- other name... - - - - - - -
-
-
- ad_service_state_monitor.launch.xml -
-
-
package: ad_service_state_monitor
-
-
-
-
- args: config_file = - ad_service_state_monitor.param.yaml -
-
-
-
- - ad_service_state_monitor.launch.xml... + other name... - + - +
-
-
- ad_service_state_monitor.launch.xml +
+
+ component_state_monitor.launch.py

-
package: ad_service_state_monitor
-
-
-
- args: config_file = - ad_service_state_monitor.planning_simulation.param.yaml + package: + component_state_monitor
- ad_service_state_monitor.launch.xml... + component_state_monitor.launch.py... - - - - + + - -
-
-
+ +
+
+
online
- online + online - - + + - -
-
-
+ +
+
+
planning_simulation
- planning_simulation + planning_simulation - + - +
-
-
+
+
$(var run_mode)
- $(var run_mode) + $(var run_mode) - + - +
-
-
- system_error_monitor.launch.xml +
+
+ autoware_error_monitor.launch.xml

-
package: system_error_monitor
+
package: autoware_error_monitor

args: config_file = - system_error_monitor.param.yaml + autoware_error_monitor.param.yaml
- system_error_monitor.launch.xml... + autoware_error_monitor.launch.xml... - + - +
-
-
- system_error_monitor.launch.xml +
+
+ autoware_error_monitor.launch.xml

-
package: system_error_monitor
+
package: autoware_error_monitor

args: config_file = - system_error_monitor.planning_simulation.param.yaml + autoware_error_monitor.planning_simulation.param.yaml
- system_error_monitor.launch.xml... + autoware_error_monitor.launch.xml... - + - +
-
-
+
+
emergency_handler.launch.xml

@@ -372,11 +295,9 @@
- emergency_handler.launch.xml... + emergency_handler.launch.xml... - - diff --git a/system/component_state_monitor/config/topics.yaml b/system/component_state_monitor/config/topics.yaml index b55dfa5aecd9f..32a2ea82d11e5 100644 --- a/system/component_state_monitor/config/topics.yaml +++ b/system/component_state_monitor/config/topics.yaml @@ -1,3 +1,5 @@ +# This file needs to be copied to tier4_system_launch/config/component_state_monitor/topics.yaml. + - module: map mode: [online, planning_simulation] type: launch From 39a914c8254306aebd5006f276d0415ac96ad2ed Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Thu, 17 Nov 2022 09:47:21 +0000 Subject: [PATCH 28/34] ci(pre-commit): autofix --- control/vehicle_cmd_gate/src/vehicle_cmd_gate.hpp | 2 +- .../ekf_localizer/include/ekf_localizer/ekf_localizer.hpp | 2 +- .../include/ndt_scan_matcher/ndt_scan_matcher_core.hpp | 2 +- .../src/pose_initializer/localization_trigger_module.hpp | 1 + .../include/system_error_monitor/system_error_monitor_core.hpp | 2 +- 5 files changed, 5 insertions(+), 4 deletions(-) diff --git a/control/vehicle_cmd_gate/src/vehicle_cmd_gate.hpp b/control/vehicle_cmd_gate/src/vehicle_cmd_gate.hpp index 533e49b3aef3a..2813af0261641 100644 --- a/control/vehicle_cmd_gate/src/vehicle_cmd_gate.hpp +++ b/control/vehicle_cmd_gate/src/vehicle_cmd_gate.hpp @@ -20,7 +20,6 @@ #include #include -#include #include #include @@ -33,6 +32,7 @@ #include #include #include +#include #include #include #include diff --git a/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp b/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp index 08a286c06c106..8f53e5a63af58 100644 --- a/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp +++ b/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp @@ -21,7 +21,6 @@ #include #include #include -#include #include #include @@ -32,6 +31,7 @@ #include #include #include +#include #include #include diff --git a/localization/ndt_scan_matcher/include/ndt_scan_matcher/ndt_scan_matcher_core.hpp b/localization/ndt_scan_matcher/include/ndt_scan_matcher/ndt_scan_matcher_core.hpp index ed791bd6cda16..1fb11cba71b72 100644 --- a/localization/ndt_scan_matcher/include/ndt_scan_matcher/ndt_scan_matcher_core.hpp +++ b/localization/ndt_scan_matcher/include/ndt_scan_matcher/ndt_scan_matcher_core.hpp @@ -22,13 +22,13 @@ #include "ndt_scan_matcher/tf2_listener_module.hpp" #include -#include #include #include #include #include #include +#include #include #include #include diff --git a/localization/pose_initializer/src/pose_initializer/localization_trigger_module.hpp b/localization/pose_initializer/src/pose_initializer/localization_trigger_module.hpp index 528fa11857075..3e32518c89698 100644 --- a/localization/pose_initializer/src/pose_initializer/localization_trigger_module.hpp +++ b/localization/pose_initializer/src/pose_initializer/localization_trigger_module.hpp @@ -16,6 +16,7 @@ #define POSE_INITIALIZER__LOCALIZATION_TRIGGER_MODULE_HPP_ #include + #include class LocalizationTriggerModule diff --git a/system/system_error_monitor/include/system_error_monitor/system_error_monitor_core.hpp b/system/system_error_monitor/include/system_error_monitor/system_error_monitor_core.hpp index a170fa8b90742..102986eff5809 100644 --- a/system/system_error_monitor/include/system_error_monitor/system_error_monitor_core.hpp +++ b/system/system_error_monitor/include/system_error_monitor/system_error_monitor_core.hpp @@ -17,12 +17,12 @@ #include #include -#include #include #include #include #include +#include #include #include From 840703857711644e048502847ec8a2852823068e Mon Sep 17 00:00:00 2001 From: "Takagi, Isamu" Date: Fri, 18 Nov 2022 14:01:52 +0900 Subject: [PATCH 29/34] fix: remove comment for sync-file Signed-off-by: Takagi, Isamu --- .../config/component_state_monitor/topics.yaml | 2 -- system/component_state_monitor/config/topics.yaml | 2 -- 2 files changed, 4 deletions(-) diff --git a/launch/tier4_system_launch/config/component_state_monitor/topics.yaml b/launch/tier4_system_launch/config/component_state_monitor/topics.yaml index ed8f201232410..b55dfa5aecd9f 100644 --- a/launch/tier4_system_launch/config/component_state_monitor/topics.yaml +++ b/launch/tier4_system_launch/config/component_state_monitor/topics.yaml @@ -1,5 +1,3 @@ -# This file is copied from component_state_monitor/config/topics.yaml. - - module: map mode: [online, planning_simulation] type: launch diff --git a/system/component_state_monitor/config/topics.yaml b/system/component_state_monitor/config/topics.yaml index 32a2ea82d11e5..b55dfa5aecd9f 100644 --- a/system/component_state_monitor/config/topics.yaml +++ b/system/component_state_monitor/config/topics.yaml @@ -1,5 +1,3 @@ -# This file needs to be copied to tier4_system_launch/config/component_state_monitor/topics.yaml. - - module: map mode: [online, planning_simulation] type: launch From 120d4f8d1aee93d7cbb29cc9bfbbbc52fe12cbf6 Mon Sep 17 00:00:00 2001 From: "Takagi, Isamu" Date: Tue, 22 Nov 2022 03:12:40 +0900 Subject: [PATCH 30/34] feat: discard sensing topic rate status Signed-off-by: Takagi, Isamu --- .../config/diagnostic_aggregator/sensing.param.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/system/system_error_monitor/config/diagnostic_aggregator/sensing.param.yaml b/system/system_error_monitor/config/diagnostic_aggregator/sensing.param.yaml index 887e671ff072c..118dcb802a3fa 100644 --- a/system/system_error_monitor/config/diagnostic_aggregator/sensing.param.yaml +++ b/system/system_error_monitor/config/diagnostic_aggregator/sensing.param.yaml @@ -9,7 +9,7 @@ path: node_alive_monitoring analyzers: topic_status: - type: diagnostic_aggregator/GenericAnalyzer + type: diagnostic_aggregator/DiscardAnalyzer path: topic_status contains: [": sensing_topic_status"] timeout: 1.0 From dbcdca47a123d48635874033eb2e3d321412e195 Mon Sep 17 00:00:00 2001 From: "Takagi, Isamu" Date: Tue, 22 Nov 2022 11:47:34 +0900 Subject: [PATCH 31/34] Revert "feat: discard sensing topic rate status" This reverts commit 120d4f8d1aee93d7cbb29cc9bfbbbc52fe12cbf6. --- .../config/diagnostic_aggregator/sensing.param.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/system/system_error_monitor/config/diagnostic_aggregator/sensing.param.yaml b/system/system_error_monitor/config/diagnostic_aggregator/sensing.param.yaml index 118dcb802a3fa..887e671ff072c 100644 --- a/system/system_error_monitor/config/diagnostic_aggregator/sensing.param.yaml +++ b/system/system_error_monitor/config/diagnostic_aggregator/sensing.param.yaml @@ -9,7 +9,7 @@ path: node_alive_monitoring analyzers: topic_status: - type: diagnostic_aggregator/DiscardAnalyzer + type: diagnostic_aggregator/GenericAnalyzer path: topic_status contains: [": sensing_topic_status"] timeout: 1.0 From 46d9d4a495b6bc1ee86dcd2e71b5df346e8f1f6b Mon Sep 17 00:00:00 2001 From: "Takagi, Isamu" Date: Tue, 22 Nov 2022 12:33:15 +0900 Subject: [PATCH 32/34] feat: add dummy topic rate check for alive monitoring Signed-off-by: Takagi, Isamu --- .../launch/component_state_monitor.launch.py | 31 +++++++++++++++++++ 1 file changed, 31 insertions(+) diff --git a/system/component_state_monitor/launch/component_state_monitor.launch.py b/system/component_state_monitor/launch/component_state_monitor.launch.py index 6fcb4e6f13020..a98454e55ff36 100644 --- a/system/component_state_monitor/launch/component_state_monitor.launch.py +++ b/system/component_state_monitor/launch/component_state_monitor.launch.py @@ -46,6 +46,24 @@ def create_topic_monitor_node(row): return IncludeLaunchDescription(include, launch_arguments=arguments) +def create_dummy_topic_monitor(group, module): + package = FindPackageShare("topic_state_monitor") + include = PathJoinSubstitution([package, "launch/topic_state_monitor.launch.xml"]) + arguments = { + "diag_name": f"{module}_topic_status", + "node_name_suffix": f"component_state_monitor_{group}_{module}", + "topic": f"/system/component_state_monitor/component/{group}/{module}", + "topic_type": "tier4_system_msgs/msg/ModeChangeAvailable", + "best_effort": False, + "transient_local": True, + "warn_rate": 0.0, + "error_rate": 0.0, + "timeout": 0.0, + } + arguments = [(k, str(v)) for k, v in arguments.items()] + return IncludeLaunchDescription(include, launch_arguments=arguments) + + def launch_setup(context, *args, **kwargs): # create topic monitors mode = LaunchConfiguration("mode").perform(context) @@ -58,6 +76,19 @@ def launch_setup(context, *args, **kwargs): topic_monitor_param[row["type"]][row["module"]].append(create_topic_monitor_name(row)) topic_monitor_param = {name: dict(module) for name, module in topic_monitor_param.items()} + # create dummy diagnostics for node alive monitoring + modules = ( + "sensing", + "perception", + "map", + "localization", + "planning", + "control", + "vehicle", + "system", + ) + topic_monitor_nodes.extend(create_dummy_topic_monitor("launch", module) for module in modules) + # create component component = ComposableNode( namespace="component_state_monitor", From 3b8f672dd9caae6358004c8f87edba4af0196ad3 Mon Sep 17 00:00:00 2001 From: "Takagi, Isamu" Date: Tue, 22 Nov 2022 20:38:42 +0900 Subject: [PATCH 33/34] Revert "feat: add dummy topic rate check for alive monitoring" This reverts commit 46d9d4a495b6bc1ee86dcd2e71b5df346e8f1f6b. --- .../launch/component_state_monitor.launch.py | 31 ------------------- 1 file changed, 31 deletions(-) diff --git a/system/component_state_monitor/launch/component_state_monitor.launch.py b/system/component_state_monitor/launch/component_state_monitor.launch.py index a98454e55ff36..6fcb4e6f13020 100644 --- a/system/component_state_monitor/launch/component_state_monitor.launch.py +++ b/system/component_state_monitor/launch/component_state_monitor.launch.py @@ -46,24 +46,6 @@ def create_topic_monitor_node(row): return IncludeLaunchDescription(include, launch_arguments=arguments) -def create_dummy_topic_monitor(group, module): - package = FindPackageShare("topic_state_monitor") - include = PathJoinSubstitution([package, "launch/topic_state_monitor.launch.xml"]) - arguments = { - "diag_name": f"{module}_topic_status", - "node_name_suffix": f"component_state_monitor_{group}_{module}", - "topic": f"/system/component_state_monitor/component/{group}/{module}", - "topic_type": "tier4_system_msgs/msg/ModeChangeAvailable", - "best_effort": False, - "transient_local": True, - "warn_rate": 0.0, - "error_rate": 0.0, - "timeout": 0.0, - } - arguments = [(k, str(v)) for k, v in arguments.items()] - return IncludeLaunchDescription(include, launch_arguments=arguments) - - def launch_setup(context, *args, **kwargs): # create topic monitors mode = LaunchConfiguration("mode").perform(context) @@ -76,19 +58,6 @@ def launch_setup(context, *args, **kwargs): topic_monitor_param[row["type"]][row["module"]].append(create_topic_monitor_name(row)) topic_monitor_param = {name: dict(module) for name, module in topic_monitor_param.items()} - # create dummy diagnostics for node alive monitoring - modules = ( - "sensing", - "perception", - "map", - "localization", - "planning", - "control", - "vehicle", - "system", - ) - topic_monitor_nodes.extend(create_dummy_topic_monitor("launch", module) for module in modules) - # create component component = ComposableNode( namespace="component_state_monitor", From 52ae523af94ce870add69a540608c624d8c95a16 Mon Sep 17 00:00:00 2001 From: "Takagi, Isamu" Date: Tue, 22 Nov 2022 21:16:47 +0900 Subject: [PATCH 34/34] feat: remove sensing alive monitoring Signed-off-by: Takagi, Isamu --- .../system_error_monitor.param.yaml | 2 +- ...m_error_monitor.planning_simulation.param.yaml | 2 +- .../diagnostic_aggregator/sensing.param.yaml | 15 +-------------- .../config/system_error_monitor.param.yaml | 2 +- ...m_error_monitor.planning_simulation.param.yaml | 2 +- 5 files changed, 5 insertions(+), 18 deletions(-) diff --git a/launch/tier4_system_launch/config/system_error_monitor/system_error_monitor.param.yaml b/launch/tier4_system_launch/config/system_error_monitor/system_error_monitor.param.yaml index b9a5a4fa79f52..b9de5cc4f2e13 100644 --- a/launch/tier4_system_launch/config/system_error_monitor/system_error_monitor.param.yaml +++ b/launch/tier4_system_launch/config/system_error_monitor/system_error_monitor.param.yaml @@ -32,7 +32,7 @@ /autoware/planning/node_alive_monitoring: default /autoware/planning/performance_monitoring/trajectory_validation: default - /autoware/sensing/node_alive_monitoring: default + # /autoware/sensing/node_alive_monitoring: default /autoware/system/node_alive_monitoring: default /autoware/system/emergency_stop_operation: default diff --git a/launch/tier4_system_launch/config/system_error_monitor/system_error_monitor.planning_simulation.param.yaml b/launch/tier4_system_launch/config/system_error_monitor/system_error_monitor.planning_simulation.param.yaml index ce081b75b2836..bf40c334f6498 100644 --- a/launch/tier4_system_launch/config/system_error_monitor/system_error_monitor.planning_simulation.param.yaml +++ b/launch/tier4_system_launch/config/system_error_monitor/system_error_monitor.planning_simulation.param.yaml @@ -32,7 +32,7 @@ /autoware/planning/node_alive_monitoring: default /autoware/planning/performance_monitoring/trajectory_validation: default - /autoware/sensing/node_alive_monitoring: default + # /autoware/sensing/node_alive_monitoring: default /autoware/system/node_alive_monitoring: default /autoware/system/emergency_stop_operation: default diff --git a/system/system_error_monitor/config/diagnostic_aggregator/sensing.param.yaml b/system/system_error_monitor/config/diagnostic_aggregator/sensing.param.yaml index 887e671ff072c..27dc4518d4f30 100644 --- a/system/system_error_monitor/config/diagnostic_aggregator/sensing.param.yaml +++ b/system/system_error_monitor/config/diagnostic_aggregator/sensing.param.yaml @@ -1,15 +1,2 @@ /**: - ros__parameters: - sensing: - type: diagnostic_aggregator/AnalyzerGroup - path: sensing - analyzers: - node_alive_monitoring: - type: diagnostic_aggregator/AnalyzerGroup - path: node_alive_monitoring - analyzers: - topic_status: - type: diagnostic_aggregator/GenericAnalyzer - path: topic_status - contains: [": sensing_topic_status"] - timeout: 1.0 + ros__parameters: {} diff --git a/system/system_error_monitor/config/system_error_monitor.param.yaml b/system/system_error_monitor/config/system_error_monitor.param.yaml index b9a5a4fa79f52..b9de5cc4f2e13 100644 --- a/system/system_error_monitor/config/system_error_monitor.param.yaml +++ b/system/system_error_monitor/config/system_error_monitor.param.yaml @@ -32,7 +32,7 @@ /autoware/planning/node_alive_monitoring: default /autoware/planning/performance_monitoring/trajectory_validation: default - /autoware/sensing/node_alive_monitoring: default + # /autoware/sensing/node_alive_monitoring: default /autoware/system/node_alive_monitoring: default /autoware/system/emergency_stop_operation: default diff --git a/system/system_error_monitor/config/system_error_monitor.planning_simulation.param.yaml b/system/system_error_monitor/config/system_error_monitor.planning_simulation.param.yaml index ce081b75b2836..bf40c334f6498 100644 --- a/system/system_error_monitor/config/system_error_monitor.planning_simulation.param.yaml +++ b/system/system_error_monitor/config/system_error_monitor.planning_simulation.param.yaml @@ -32,7 +32,7 @@ /autoware/planning/node_alive_monitoring: default /autoware/planning/performance_monitoring/trajectory_validation: default - /autoware/sensing/node_alive_monitoring: default + # /autoware/sensing/node_alive_monitoring: default /autoware/system/node_alive_monitoring: default /autoware/system/emergency_stop_operation: default