Merge branch 'main' into test/test_cuda_centerpoint

common/autoware_ad_api_specs/include/autoware_ad_api_specs/system.hpp

@@ -0,0 +1,36 @@
+// Copyright 2024 The Autoware Contributors
+//
+// 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__SYSTEM_HPP_
+#define AUTOWARE_AD_API_SPECS__SYSTEM_HPP_
+
+#include <rclcpp/qos.hpp>
+
+#include <autoware_adapi_v1_msgs/msg/heartbeat.hpp>
+
+namespace autoware_ad_api::system
+{
+
+struct Heartbeat
+{
+  using Message = autoware_adapi_v1_msgs::msg::Heartbeat;
+  static constexpr char name[] = "/api/system/heartbeat";
+  static constexpr size_t depth = 10;
+  static constexpr auto reliability = RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT;
+  static constexpr auto durability = RMW_QOS_POLICY_DURABILITY_VOLATILE;
+};
+
+}  // namespace autoware_ad_api::system
+
+#endif  // AUTOWARE_AD_API_SPECS__SYSTEM_HPP_

evaluator/perception_online_evaluator/param/perception_online_evaluator.defaults.yaml

@@ -13,7 +13,7 @@
     prediction_time_horizons: [1.0, 2.0, 3.0, 5.0]
 
     stopped_velocity_threshold: 1.0
-    detection_radius_list: [50.0, 100.0, 150.0, 200.0]
+    detection_radius_list: [20.0, 40.0, 60.0, 80.0, 100.0, 120.0, 140.0, 160.0, 180.0, 200.0]
     detection_height_list: [10.0]
     detection_count_purge_seconds: 36000.0
     objects_count_window_seconds: 1.0

launch/tier4_system_launch/launch/system.launch.xml

@@ -25,11 +25,12 @@
   <arg name="run_mode" default="online" description="options: online, logging_simulation, planning_simulation"/>
   <arg name="sensor_model" description="sensor model name"/>
 
-  <arg name="use_diagnostic_graph" default="false" description="use diagnostic graph packages"/>
-  <arg name="mrm_handler_param_path" if="$(var use_diagnostic_graph)"/>
-  <arg name="diagnostic_graph_aggregator_param_path" if="$(var use_diagnostic_graph)"/>
-  <arg name="diagnostic_graph_aggregator_graph_path" if="$(var use_diagnostic_graph)"/>
-  <arg name="diagnostic_graph_aggregator_planning_simulator_graph_path" if="$(var use_diagnostic_graph)"/>
+  <!-- Emergency handler will be replaced by MRM handler. -->
+  <arg name="use_emergency_handler" default="true" description="use emergency handler packages"/>
+  <arg name="mrm_handler_param_path"/>
+  <arg name="diagnostic_graph_aggregator_param_path"/>
+  <arg name="diagnostic_graph_aggregator_graph_path_main"/>
+  <arg name="diagnostic_graph_aggregator_graph_path_psim"/>
 
   <let name="sensor_launch_pkg" value="$(find-pkg-share $(var sensor_model)_launch)"/>
 
@@ -57,6 +58,13 @@
       </include>
     </group>
 
+    <!-- Duplicated Node Checker -->
+    <group>
+      <include file="$(find-pkg-share duplicated_node_checker)/launch/duplicated_node_checker.launch.xml">
+        <arg name="config_file" value="$(var duplicated_node_checker_param_path)"/>
+      </include>
+    </group>
+
     <!-- Service Log Checker -->
     <group>
       <include file="$(find-pkg-share component_interface_tools)/launch/service_log_checker.launch.xml"/>
@@ -70,8 +78,20 @@
       </include>
     </group>
 
+    <!-- MRM Operator -->
+    <group>
+      <include file="$(find-pkg-share mrm_comfortable_stop_operator)/launch/mrm_comfortable_stop_operator.launch.py">
+        <arg name="config_file" value="$(var mrm_comfortable_stop_operator_param_path)"/>
+      </include>
+    </group>
+    <group>
+      <include file="$(find-pkg-share mrm_emergency_stop_operator)/launch/mrm_emergency_stop_operator.launch.py">
+        <arg name="config_file" value="$(var mrm_emergency_stop_operator_param_path)"/>
+      </include>
+    </group>
+
     <!-- Error Monitor -->
-    <group unless="$(var use_diagnostic_graph)">
+    <group if="$(var use_emergency_handler)">
       <let name="config_file" value="$(var system_error_monitor_param_path)" if="$(eval &quot;'$(var run_mode)'=='online'&quot;)"/>
       <let name="config_file" value="$(var system_error_monitor_param_path)" if="$(eval &quot;'$(var run_mode)'=='logging_simulation'&quot;)"/>
       <let name="config_file" value="$(var system_error_monitor_planning_simulator_param_path)" if="$(eval &quot;'$(var run_mode)'=='planning_simulation'&quot;)"/>
@@ -85,52 +105,34 @@
     </group>
 
     <!-- Emergency Handler -->
-    <group unless="$(var use_diagnostic_graph)">
+    <group if="$(var use_emergency_handler)">
       <include file="$(find-pkg-share emergency_handler)/launch/emergency_handler.launch.xml">
         <arg name="config_file" value="$(var emergency_handler_param_path)"/>
       </include>
     </group>
 
+    <!-- Diagnostic Graph Aggregator -->
     <group>
-      <include file="$(find-pkg-share duplicated_node_checker)/launch/duplicated_node_checker.launch.xml">
-        <arg name="config_file" value="$(var duplicated_node_checker_param_path)"/>
+      <let name="diagnostic_graph_aggregator_graph_path" value="$(var diagnostic_graph_aggregator_graph_path_main)" if="$(eval &quot;'$(var run_mode)'=='online'&quot;)"/>
+      <let name="diagnostic_graph_aggregator_graph_path" value="$(var diagnostic_graph_aggregator_graph_path_main)" if="$(eval &quot;'$(var run_mode)'=='logging_simulation'&quot;)"/>
+      <let name="diagnostic_graph_aggregator_graph_path" value="$(var diagnostic_graph_aggregator_graph_path_psim)" if="$(eval &quot;'$(var run_mode)'=='planning_simulation'&quot;)"/>
+      <include file="$(find-pkg-share system_diagnostic_monitor)/launch/system_diagnostic_monitor.launch.xml">
+        <arg name="param_file" value="$(var diagnostic_graph_aggregator_param_path)"/>
+        <arg name="graph_file" value="$(var diagnostic_graph_aggregator_graph_path)"/>
       </include>
     </group>
 
-    <!-- MRM Operator -->
-    <group>
-      <include file="$(find-pkg-share mrm_comfortable_stop_operator)/launch/mrm_comfortable_stop_operator.launch.py">
-        <arg name="config_file" value="$(var mrm_comfortable_stop_operator_param_path)"/>
-      </include>
-    </group>
-    <group>
-      <include file="$(find-pkg-share mrm_emergency_stop_operator)/launch/mrm_emergency_stop_operator.launch.py">
-        <arg name="config_file" value="$(var mrm_emergency_stop_operator_param_path)"/>
-      </include>
+    <!-- Hazard Status Converter -->
+    <group unless="$(var use_emergency_handler)">
+      <include file="$(find-pkg-share hazard_status_converter)/launch/hazard_status_converter.launch.xml"/>
     </group>
 
     <!-- MRM Handler -->
-    <group if="$(var use_diagnostic_graph)">
+    <group unless="$(var use_emergency_handler)">
       <include file="$(find-pkg-share mrm_handler)/launch/mrm_handler.launch.xml">
         <arg name="config_file" value="$(var mrm_handler_param_path)"/>
       </include>
     </group>
-
-    <!-- Diagnostic Graph Aggregator -->
-    <group if="$(var use_diagnostic_graph)">
-      <let name="graph_file" value="$(var diagnostic_graph_aggregator_graph_path)" if="$(eval &quot;'$(var run_mode)'=='online'&quot;)"/>
-      <let name="graph_file" value="$(var diagnostic_graph_aggregator_graph_path)" if="$(eval &quot;'$(var run_mode)'=='logging_simulation'&quot;)"/>
-      <let name="graph_file" value="$(var diagnostic_graph_aggregator_planning_simulator_graph_path)" if="$(eval &quot;'$(var run_mode)'=='planning_simulation'&quot;)"/>
-      <include file="$(find-pkg-share diagnostic_graph_aggregator)/launch/aggregator.launch.xml">
-        <arg name="param_file" value="$(var diagnostic_graph_aggregator_param_path)"/>
-        <arg name="graph_file" value="$(var graph_file)"/>
-      </include>
-    </group>
-
-    <!-- Hazard Status Converter -->
-    <group if="$(var use_diagnostic_graph)">
-      <include file="$(find-pkg-share hazard_status_converter)/launch/hazard_status_converter.launch.xml"/>
-    </group>
   </group>
 
   <!-- Dummy Diag Publisher -->

localization/gyro_odometer/CMakeLists.txt

@@ -4,17 +4,12 @@ project(gyro_odometer)
 find_package(autoware_cmake REQUIRED)
 autoware_package()
 
-ament_auto_add_executable(${PROJECT_NAME}
-  src/gyro_odometer_node.cpp
+ament_auto_add_library(${PROJECT_NAME} SHARED
   src/gyro_odometer_core.cpp
 )
 
 target_link_libraries(${PROJECT_NAME} fmt)
 
-ament_auto_add_library(gyro_odometer_node SHARED
-  src/gyro_odometer_core.cpp
-)
-
 if(BUILD_TESTING)
   ament_add_ros_isolated_gtest(test_gyro_odometer
     test/test_main.cpp
@@ -25,10 +20,15 @@ if(BUILD_TESTING)
     rclcpp
   )
   target_link_libraries(test_gyro_odometer
-    gyro_odometer_node
+    ${PROJECT_NAME}
   )
 endif()
 
+rclcpp_components_register_node(${PROJECT_NAME}
+  PLUGIN "autoware::gyro_odometer::GyroOdometerNode"
+  EXECUTABLE ${PROJECT_NAME}_node
+  EXECUTOR SingleThreadedExecutor
+)
 
 ament_auto_package(INSTALL_TO_SHARE
   launch

localization/gyro_odometer/include/gyro_odometer/gyro_odometer_core.hpp

@@ -36,14 +36,17 @@
 #include <memory>
 #include <string>
 
-class GyroOdometer : public rclcpp::Node
+namespace autoware::gyro_odometer
+{
+
+class GyroOdometerNode : public rclcpp::Node
 {
 private:
   using COV_IDX = tier4_autoware_utils::xyz_covariance_index::XYZ_COV_IDX;
 
 public:
-  explicit GyroOdometer(const rclcpp::NodeOptions & options);
-  ~GyroOdometer();
+  explicit GyroOdometerNode(const rclcpp::NodeOptions & node_options);
+  ~GyroOdometerNode();
 
 private:
   void callbackVehicleTwist(
@@ -75,4 +78,6 @@ class GyroOdometer : public rclcpp::Node
   std::deque<sensor_msgs::msg::Imu> gyro_queue_;
 };
 
+}  // namespace autoware::gyro_odometer
+
 #endif  // GYRO_ODOMETER__GYRO_ODOMETER_CORE_HPP_

localization/gyro_odometer/launch/gyro_odometer.launch.xml

@@ -11,7 +11,7 @@
 
   <arg name="config_file" default="$(find-pkg-share gyro_odometer)/config/gyro_odometer.param.yaml"/>
 
-  <node pkg="gyro_odometer" exec="gyro_odometer" name="gyro_odometer" output="screen">
+  <node pkg="gyro_odometer" exec="gyro_odometer_node" output="both">
     <remap from="vehicle/twist_with_covariance" to="$(var input_vehicle_twist_with_covariance_topic)"/>
 
     <remap from="imu" to="$(var input_imu_topic)"/>

localization/gyro_odometer/package.xml

@@ -19,14 +19,13 @@
 
   <depend>fmt</depend>
   <depend>geometry_msgs</depend>
+  <depend>rclcpp</depend>
+  <depend>rclcpp_components</depend>
   <depend>sensor_msgs</depend>
   <depend>tf2</depend>
   <depend>tf2_geometry_msgs</depend>
   <depend>tier4_autoware_utils</depend>
 
-  <exec_depend>rclcpp</exec_depend>
-  <exec_depend>rclcpp_components</exec_depend>
-
   <test_depend>ament_cmake_ros</test_depend>
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>autoware_lint_common</test_depend>

localization/gyro_odometer/src/gyro_odometer_core.cpp

@@ -14,6 +14,8 @@
 
 #include "gyro_odometer/gyro_odometer_core.hpp"
 
+#include <rclcpp/rclcpp.hpp>
+
 #ifdef ROS_DISTRO_GALACTIC
 #include <tf2_geometry_msgs/tf2_geometry_msgs.h>
 #else
@@ -25,6 +27,42 @@
 #include <memory>
 #include <string>
 
+namespace autoware::gyro_odometer
+{
+
+GyroOdometerNode::GyroOdometerNode(const rclcpp::NodeOptions & node_options)
+: Node("gyro_odometer", node_options),
+  output_frame_(declare_parameter<std::string>("output_frame")),
+  message_timeout_sec_(declare_parameter<double>("message_timeout_sec")),
+  vehicle_twist_arrived_(false),
+  imu_arrived_(false)
+{
+  transform_listener_ = std::make_shared<tier4_autoware_utils::TransformListener>(this);
+  logger_configure_ = std::make_unique<tier4_autoware_utils::LoggerLevelConfigure>(this);
+
+  vehicle_twist_sub_ = create_subscription<geometry_msgs::msg::TwistWithCovarianceStamped>(
+    "vehicle/twist_with_covariance", rclcpp::QoS{100},
+    std::bind(&GyroOdometerNode::callbackVehicleTwist, this, std::placeholders::_1));
+
+  imu_sub_ = create_subscription<sensor_msgs::msg::Imu>(
+    "imu", rclcpp::QoS{100},
+    std::bind(&GyroOdometerNode::callbackImu, this, std::placeholders::_1));
+
+  twist_raw_pub_ = create_publisher<geometry_msgs::msg::TwistStamped>("twist_raw", rclcpp::QoS{10});
+  twist_with_covariance_raw_pub_ = create_publisher<geometry_msgs::msg::TwistWithCovarianceStamped>(
+    "twist_with_covariance_raw", rclcpp::QoS{10});
+
+  twist_pub_ = create_publisher<geometry_msgs::msg::TwistStamped>("twist", rclcpp::QoS{10});
+  twist_with_covariance_pub_ = create_publisher<geometry_msgs::msg::TwistWithCovarianceStamped>(
+    "twist_with_covariance", rclcpp::QoS{10});
+
+  // TODO(YamatoAndo) createTimer
+}
+
+GyroOdometerNode::~GyroOdometerNode()
+{
+}
+
 std::array<double, 9> transformCovariance(const std::array<double, 9> & cov)
 {
   using COV_IDX = tier4_autoware_utils::xyz_covariance_index::XYZ_COV_IDX;
@@ -100,39 +138,7 @@ geometry_msgs::msg::TwistWithCovarianceStamped concatGyroAndOdometer(
   return twist_with_cov;
 }
 
-GyroOdometer::GyroOdometer(const rclcpp::NodeOptions & options)
-: Node("gyro_odometer", options),
-  output_frame_(declare_parameter<std::string>("output_frame")),
-  message_timeout_sec_(declare_parameter<double>("message_timeout_sec")),
-  vehicle_twist_arrived_(false),
-  imu_arrived_(false)
-{
-  transform_listener_ = std::make_shared<tier4_autoware_utils::TransformListener>(this);
-  logger_configure_ = std::make_unique<tier4_autoware_utils::LoggerLevelConfigure>(this);
-
-  vehicle_twist_sub_ = create_subscription<geometry_msgs::msg::TwistWithCovarianceStamped>(
-    "vehicle/twist_with_covariance", rclcpp::QoS{100},
-    std::bind(&GyroOdometer::callbackVehicleTwist, this, std::placeholders::_1));
-
-  imu_sub_ = create_subscription<sensor_msgs::msg::Imu>(
-    "imu", rclcpp::QoS{100}, std::bind(&GyroOdometer::callbackImu, this, std::placeholders::_1));
-
-  twist_raw_pub_ = create_publisher<geometry_msgs::msg::TwistStamped>("twist_raw", rclcpp::QoS{10});
-  twist_with_covariance_raw_pub_ = create_publisher<geometry_msgs::msg::TwistWithCovarianceStamped>(
-    "twist_with_covariance_raw", rclcpp::QoS{10});
-
-  twist_pub_ = create_publisher<geometry_msgs::msg::TwistStamped>("twist", rclcpp::QoS{10});
-  twist_with_covariance_pub_ = create_publisher<geometry_msgs::msg::TwistWithCovarianceStamped>(
-    "twist_with_covariance", rclcpp::QoS{10});
-
-  // TODO(YamatoAndo) createTimer
-}
-
-GyroOdometer::~GyroOdometer()
-{
-}
-
-void GyroOdometer::callbackVehicleTwist(
+void GyroOdometerNode::callbackVehicleTwist(
   const geometry_msgs::msg::TwistWithCovarianceStamped::ConstSharedPtr vehicle_twist_ptr)
 {
   vehicle_twist_arrived_ = true;
@@ -173,7 +179,7 @@ void GyroOdometer::callbackVehicleTwist(
   gyro_queue_.clear();
 }
 
-void GyroOdometer::callbackImu(const sensor_msgs::msg::Imu::ConstSharedPtr imu_msg_ptr)
+void GyroOdometerNode::callbackImu(const sensor_msgs::msg::Imu::ConstSharedPtr imu_msg_ptr)
 {
   imu_arrived_ = true;
   if (!vehicle_twist_arrived_) {
@@ -241,7 +247,7 @@ void GyroOdometer::callbackImu(const sensor_msgs::msg::Imu::ConstSharedPtr imu_m
   gyro_queue_.clear();
 }
 
-void GyroOdometer::publishData(
+void GyroOdometerNode::publishData(
   const geometry_msgs::msg::TwistWithCovarianceStamped & twist_with_cov_raw)
 {
   geometry_msgs::msg::TwistStamped twist_raw;
@@ -269,3 +275,8 @@ void GyroOdometer::publishData(
   twist_pub_->publish(twist);
   twist_with_covariance_pub_->publish(twist_with_covariance);
 }
+
+}  // namespace autoware::gyro_odometer
+
+#include <rclcpp_components/register_node_macro.hpp>
+RCLCPP_COMPONENTS_REGISTER_NODE(autoware::gyro_odometer::GyroOdometerNode)