diff --git a/system/autoware_state_monitor/CMakeLists.txt b/system/autoware_state_monitor/CMakeLists.txt new file mode 100644 index 0000000000000..081e6b757d92a --- /dev/null +++ b/system/autoware_state_monitor/CMakeLists.txt @@ -0,0 +1,40 @@ +cmake_minimum_required(VERSION 3.5) +project(autoware_state_monitor) + +### Compile options +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) + set(CMAKE_CXX_STANDARD_REQUIRED ON) + set(CMAKE_CXX_EXTENSIONS OFF) +endif() +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic -Werror) +endif() + +### Dependencies +find_package(ament_cmake_auto REQUIRED) +ament_auto_find_build_dependencies() + +# Target +## Set source files +set(AUTOWARE_STATE_MONITOR_SRC + src/autoware_state_monitor_node/autoware_state_monitor_node.cpp + src/autoware_state_monitor_node/state_machine.cpp + src/autoware_state_monitor_node/diagnostics.cpp +) + +## Add executables +ament_auto_add_executable(autoware_state_monitor + src/autoware_state_monitor_node/main.cpp + ${AUTOWARE_STATE_MONITOR_SRC} +) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() +endif() + +ament_auto_package(INSTALL_TO_SHARE + launch + config +) diff --git a/system/autoware_state_monitor/README.md b/system/autoware_state_monitor/README.md new file mode 100644 index 0000000000000..24a659a1eb459 --- /dev/null +++ b/system/autoware_state_monitor/README.md @@ -0,0 +1,46 @@ +# autoware_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/autoware_state_monitor/config/autoware_state_monitor.param.yaml b/system/autoware_state_monitor/config/autoware_state_monitor.param.yaml new file mode 100644 index 0000000000000..92221018a5b24 --- /dev/null +++ b/system/autoware_state_monitor/config/autoware_state_monitor.param.yaml @@ -0,0 +1,155 @@ +# Autoware 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", + "/initialpose3d", + "/localization/pose_twist_fusion_filter/pose", + "/perception/object_recognition/objects", + "/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: ["autoware_perception_msgs/msg/DynamicObjectArray", "autoware_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/system/autoware_state_monitor/config/autoware_state_monitor.planning_simulation.param.yaml b/system/autoware_state_monitor/config/autoware_state_monitor.planning_simulation.param.yaml new file mode 100644 index 0000000000000..41928923a0dfd --- /dev/null +++ b/system/autoware_state_monitor/config/autoware_state_monitor.planning_simulation.param.yaml @@ -0,0 +1,134 @@ +# Autoware 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", + "/map/pointcloud_map", + "/perception/object_recognition/objects", + "/initialpose2d", + "/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/object_recognition/objects: + module: "perception" + timeout: 1.0 + warn_rate: 5.0 + type: "autoware_auto_perception_msgs/msg/PredictedObjects" + + /initialpose2d: + 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/autoware_state_monitor/include/autoware_state_monitor/autoware_state.hpp b/system/autoware_state_monitor/include/autoware_state_monitor/autoware_state.hpp new file mode 100644 index 0000000000000..fc1c77f316287 --- /dev/null +++ b/system/autoware_state_monitor/include/autoware_state_monitor/autoware_state.hpp @@ -0,0 +1,90 @@ +// 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 AUTOWARE_STATE_MONITOR__AUTOWARE_STATE_HPP_ +#define AUTOWARE_STATE_MONITOR__AUTOWARE_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 // AUTOWARE_STATE_MONITOR__AUTOWARE_STATE_HPP_ diff --git a/system/autoware_state_monitor/include/autoware_state_monitor/autoware_state_monitor_node.hpp b/system/autoware_state_monitor/include/autoware_state_monitor/autoware_state_monitor_node.hpp new file mode 100644 index 0000000000000..e00dbcec288a4 --- /dev/null +++ b/system/autoware_state_monitor/include/autoware_state_monitor/autoware_state_monitor_node.hpp @@ -0,0 +1,134 @@ +// 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 AUTOWARE_STATE_MONITOR__AUTOWARE_STATE_MONITOR_NODE_HPP_ +#define AUTOWARE_STATE_MONITOR__AUTOWARE_STATE_MONITOR_NODE_HPP_ + +#include "autoware_state_monitor/autoware_state.hpp" +#include "autoware_state_monitor/config.hpp" +#include "autoware_state_monitor/state_machine.hpp" + +#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_; + + // 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_odom_; + + void onAutowareEngage(const autoware_auto_vehicle_msgs::msg::Engage::ConstSharedPtr msg); + void onVehicleControlMode( + const autoware_auto_vehicle_msgs::msg::ControlModeReport::ConstSharedPtr msg); + void onRoute(const autoware_auto_planning_msgs::msg::HADMapRoute::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_; + + // 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 // AUTOWARE_STATE_MONITOR__AUTOWARE_STATE_MONITOR_NODE_HPP_ diff --git a/system/autoware_state_monitor/include/autoware_state_monitor/config.hpp b/system/autoware_state_monitor/include/autoware_state_monitor/config.hpp new file mode 100644 index 0000000000000..e758fd2f6267a --- /dev/null +++ b/system/autoware_state_monitor/include/autoware_state_monitor/config.hpp @@ -0,0 +1,122 @@ +// 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 AUTOWARE_STATE_MONITOR__CONFIG_HPP_ +#define AUTOWARE_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 // AUTOWARE_STATE_MONITOR__CONFIG_HPP_ diff --git a/system/autoware_state_monitor/include/autoware_state_monitor/module_name.hpp b/system/autoware_state_monitor/include/autoware_state_monitor/module_name.hpp new file mode 100644 index 0000000000000..760d08e215721 --- /dev/null +++ b/system/autoware_state_monitor/include/autoware_state_monitor/module_name.hpp @@ -0,0 +1,30 @@ +// 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 AUTOWARE_STATE_MONITOR__MODULE_NAME_HPP_ +#define AUTOWARE_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 // AUTOWARE_STATE_MONITOR__MODULE_NAME_HPP_ diff --git a/system/autoware_state_monitor/include/autoware_state_monitor/state_machine.hpp b/system/autoware_state_monitor/include/autoware_state_monitor/state_machine.hpp new file mode 100644 index 0000000000000..68091312f1cde --- /dev/null +++ b/system/autoware_state_monitor/include/autoware_state_monitor/state_machine.hpp @@ -0,0 +1,109 @@ +// 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 AUTOWARE_STATE_MONITOR__STATE_MACHINE_HPP_ +#define AUTOWARE_STATE_MONITOR__STATE_MACHINE_HPP_ + +#include "autoware_state_monitor/autoware_state.hpp" +#include "autoware_state_monitor/config.hpp" +#include "autoware_state_monitor/module_name.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; + + 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_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 // AUTOWARE_STATE_MONITOR__STATE_MACHINE_HPP_ diff --git a/system/autoware_state_monitor/launch/autoware_state_monitor.launch.xml b/system/autoware_state_monitor/launch/autoware_state_monitor.launch.xml new file mode 100644 index 0000000000000..5c2d8d800da2f --- /dev/null +++ b/system/autoware_state_monitor/launch/autoware_state_monitor.launch.xml @@ -0,0 +1,48 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/system/autoware_state_monitor/package.xml b/system/autoware_state_monitor/package.xml new file mode 100644 index 0000000000000..7f2a44a1f5772 --- /dev/null +++ b/system/autoware_state_monitor/package.xml @@ -0,0 +1,37 @@ + + + + autoware_state_monitor + 0.1.1 + The autoware_state_monitor package ROS2 + Kenji Miyake + Apache License 2.0 + + ament_cmake_auto + + autoware_auto_control_msgs + autoware_auto_planning_msgs + autoware_auto_system_msgs + autoware_auto_vehicle_msgs + diagnostic_updater + fmt + geometry_msgs + nav_msgs + rclcpp + std_msgs + std_srvs + tf2 + tf2_geometry_msgs + tf2_ros + + autoware_lanelet2_msgs + autoware_perception_msgs + sensor_msgs + + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/system/autoware_state_monitor/src/autoware_state_monitor_node/autoware_state_monitor_node.cpp b/system/autoware_state_monitor/src/autoware_state_monitor_node/autoware_state_monitor_node.cpp new file mode 100644 index 0000000000000..6b3c8c61936ca --- /dev/null +++ b/system/autoware_state_monitor/src/autoware_state_monitor_node/autoware_state_monitor_node.cpp @@ -0,0 +1,469 @@ +// 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 "autoware_state_monitor/autoware_state_monitor_node.hpp" + +#include + +#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) +{ + state_input_.autoware_engage = msg; +} + +void AutowareStateMonitorNode::onVehicleControlMode( + const autoware_auto_vehicle_msgs::msg::ControlModeReport::ConstSharedPtr msg) +{ + state_input_.control_mode_ = msg; +} + +void AutowareStateMonitorNode::onRoute( + const autoware_auto_planning_msgs::msg::HADMapRoute::ConstSharedPtr msg) +{ + 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) +{ + 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(); + } +} + +bool AutowareStateMonitorNode::onShutdownService( + const std::shared_ptr request_header, + [[maybe_unused]] const std::shared_ptr request, + const std::shared_ptr response) +{ + (void)request_header; + 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()); + + if (state_machine_->getCurrentState() == AutowareState::Finalizing) { + 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) +{ + if (state_machine_->getCurrentState() != AutowareState::WaitingForEngage) { + response->success = false; + response->message = "Reset route can be accepted only under WaitingForEngage."; + return true; + } + + state_input_.is_route_reset_required = true; + + const auto t_start = this->now(); + constexpr double timeout = 3.0; + while (rclcpp::ok()) { + if (state_machine_->getCurrentState() == AutowareState::WaitingForRoute) { + state_input_.is_route_reset_required = false; + 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() +{ + // 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 + const auto prev_autoware_state = state_machine_->getCurrentState(); + const auto 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() +{ + 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("autoware_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); + 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_route_ = this->create_subscription( + "input/route", rclcpp::QoS{1}.transient_local(), + std::bind(&AutowareStateMonitorNode::onRoute, 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 + auto timer_callback = std::bind(&AutowareStateMonitorNode::onTimer, this); + auto period = std::chrono::duration_cast( + std::chrono::duration(1.0 / update_rate_)); + + timer_ = std::make_shared>( + this->get_clock(), period, std::move(timer_callback), + this->get_node_base_interface()->get_context()); + this->get_node_timers_interface()->add_timer(timer_, callback_group_subscribers_); +} diff --git a/system/autoware_state_monitor/src/autoware_state_monitor_node/diagnostics.cpp b/system/autoware_state_monitor/src/autoware_state_monitor_node/diagnostics.cpp new file mode 100644 index 0000000000000..db11d1fd41783 --- /dev/null +++ b/system/autoware_state_monitor/src/autoware_state_monitor_node/diagnostics.cpp @@ -0,0 +1,181 @@ +// 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 "autoware_state_monitor/autoware_state_monitor_node.hpp" + +#include + +#include +#include + +#define FMT_HEADER_ONLY +#include + +void AutowareStateMonitorNode::setupDiagnosticUpdater() +{ + updater_.setHardwareID("autoware_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; + + 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; + + 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/autoware_state_monitor/src/autoware_state_monitor_node/main.cpp b/system/autoware_state_monitor/src/autoware_state_monitor_node/main.cpp new file mode 100644 index 0000000000000..ad88c1a89b8ee --- /dev/null +++ b/system/autoware_state_monitor/src/autoware_state_monitor_node/main.cpp @@ -0,0 +1,31 @@ +// 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 "autoware_state_monitor/autoware_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/autoware_state_monitor/src/autoware_state_monitor_node/state_machine.cpp b/system/autoware_state_monitor/src/autoware_state_monitor_node/state_machine.cpp new file mode 100644 index 0000000000000..fa53c97a5a55c --- /dev/null +++ b/system/autoware_state_monitor/src/autoware_state_monitor_node/state_machine.cpp @@ -0,0 +1,323 @@ +// 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 +#include + +#define FMT_HEADER_ONLY +#include "autoware_state_monitor/state_machine.hpp" + +#include + +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 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 +{ + const auto is_near_goal = isNearGoal( + state_input_.current_pose->pose, *state_input_.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_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_; +}