Merge branch 'awf-latest' into RT1-6335-route-handler-overlap-removal…

…-is-too-conservative
autowarefoundation · May 28, 2024 · 47d214d · 47d214d
2 parents faf748e + 6744b1a
commit 47d214d
Show file tree

Hide file tree

Showing 119 changed files with 1,748 additions and 1,071 deletions.
diff --git a/.github/CODEOWNERS b/.github/CODEOWNERS
@@ -21,35 +21,25 @@ common/goal_distance_calculator/** [email protected]
 common/grid_map_utils/** [email protected]
 common/interpolation/** [email protected] [email protected]
 common/kalman_filter/** [email protected] [email protected] [email protected]
-common/mission_planner_rviz_plugin/** [email protected]
 common/motion_utils/** [email protected] [email protected] [email protected] [email protected] [email protected] [email protected] [email protected] [email protected]
 common/object_recognition_utils/** [email protected] [email protected] [email protected] [email protected]
 common/osqp_interface/** [email protected] [email protected] [email protected] [email protected]
 common/path_distance_calculator/** [email protected]
 common/perception_utils/** [email protected] [email protected] [email protected]
 common/polar_grid/** [email protected]
 common/qp_interface/** [email protected] [email protected] [email protected] [email protected]
-common/rtc_manager_rviz_plugin/** [email protected] [email protected]
 common/signal_processing/** [email protected] [email protected] [email protected] [email protected] [email protected]
 common/tensorrt_common/** [email protected] [email protected]
 common/tier4_adapi_rviz_plugin/** [email protected] [email protected] [email protected]
 common/tier4_api_utils/** [email protected]
-common/tier4_automatic_goal_rviz_plugin/** [email protected] [email protected] [email protected] [email protected]
 common/tier4_autoware_utils/** [email protected] [email protected] [email protected]
-common/tier4_calibration_rviz_plugin/** [email protected]
 common/tier4_camera_view_rviz_plugin/** [email protected] [email protected]
-common/tier4_control_rviz_plugin/** [email protected]
 common/tier4_datetime_rviz_plugin/** [email protected]
-common/tier4_debug_rviz_plugin/** [email protected]
 common/tier4_localization_rviz_plugin/** [email protected] [email protected]
-common/tier4_logging_level_configure_rviz_plugin/** [email protected] [email protected] [email protected]
 common/tier4_perception_rviz_plugin/** [email protected]
 common/tier4_planning_rviz_plugin/** [email protected] [email protected]
-common/tier4_screen_capture_rviz_plugin/** [email protected] [email protected] [email protected]
-common/tier4_simulated_clock_rviz_plugin/** [email protected]
-common/tier4_state_rviz_plugin/** [email protected] [email protected]
+common/tier4_state_rviz_plugin/** [email protected] [email protected] [email protected]
 common/tier4_system_rviz_plugin/** [email protected]
-common/tier4_target_object_type_rviz_plugin/** [email protected]
 common/tier4_traffic_light_rviz_plugin/** [email protected]
 common/tier4_vehicle_rviz_plugin/** [email protected]
 common/time_utils/** [email protected] [email protected] [email protected]
@@ -157,6 +147,7 @@ perception/traffic_light_multi_camera_fusion/** [email protected] tao.zhon
 perception/traffic_light_occlusion_predictor/** [email protected] [email protected]
 perception/traffic_light_visualization/** [email protected] [email protected]
 planning/autoware_behavior_path_external_request_lane_change_module/** [email protected] [email protected] [email protected] [email protected] [email protected] [email protected]
+planning/autoware_behavior_velocity_planner/** [email protected] [email protected] [email protected] [email protected] [email protected] [email protected] [email protected] [email protected] [email protected] [email protected] [email protected]
 planning/autoware_planning_test_manager/** [email protected] [email protected]
 planning/autoware_remaining_distance_time_calculator/** [email protected]
 planning/autoware_static_centerline_generator/** [email protected] [email protected]
@@ -179,7 +170,6 @@ planning/behavior_velocity_no_drivable_lane_module/** [email protected]
 planning/behavior_velocity_no_stopping_area_module/** [email protected] [email protected] [email protected]
 planning/behavior_velocity_occlusion_spot_module/** [email protected] [email protected] [email protected]
 planning/behavior_velocity_out_of_lane_module/** [email protected] [email protected] [email protected] [email protected]
-planning/behavior_velocity_planner/** [email protected] [email protected] [email protected] [email protected] [email protected] [email protected] [email protected] [email protected] [email protected] [email protected] [email protected]
 planning/behavior_velocity_planner_common/** [email protected] [email protected] [email protected] [email protected] [email protected]
 planning/behavior_velocity_run_out_module/** [email protected] [email protected] [email protected] [email protected] [email protected] [email protected]
 planning/behavior_velocity_speed_bump_module/** [email protected] [email protected] [email protected]

diff --git a/common/autoware_ad_api_specs/include/autoware_ad_api_specs/system.hpp b/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_
diff --git a/common/tier4_autoware_utils/include/tier4_autoware_utils/math/trigonometry.hpp b/common/tier4_autoware_utils/include/tier4_autoware_utils/math/trigonometry.hpp
@@ -15,13 +15,17 @@
 #ifndef TIER4_AUTOWARE_UTILS__MATH__TRIGONOMETRY_HPP_
 #define TIER4_AUTOWARE_UTILS__MATH__TRIGONOMETRY_HPP_
 
+#include <utility>
+
 namespace tier4_autoware_utils
 {
 
 float sin(float radian);
 
 float cos(float radian);
 
+std::pair<float, float> sin_and_cos(float radian);
+
 }  // namespace tier4_autoware_utils
 
 #endif  // TIER4_AUTOWARE_UTILS__MATH__TRIGONOMETRY_HPP_
diff --git a/common/tier4_autoware_utils/src/math/trigonometry.cpp b/common/tier4_autoware_utils/src/math/trigonometry.cpp
@@ -49,4 +49,27 @@ float cos(float radian)
   return sin(radian + static_cast<float>(tier4_autoware_utils::pi) / 2.f);
 }
 
+std::pair<float, float> sin_and_cos(float radian)
+{
+  constexpr float tmp =
+    (180.f / static_cast<float>(tier4_autoware_utils::pi)) * (discrete_arcs_num_360 / 360.f);
+  const float degree = radian * tmp;
+  size_t idx =
+    (static_cast<int>(std::round(degree)) % discrete_arcs_num_360 + discrete_arcs_num_360) %
+    discrete_arcs_num_360;
+
+  if (idx < discrete_arcs_num_90) {
+    return {g_sin_table[idx], g_sin_table[discrete_arcs_num_90 - idx]};
+  } else if (discrete_arcs_num_90 <= idx && idx < 2 * discrete_arcs_num_90) {
+    idx = 2 * discrete_arcs_num_90 - idx;
+    return {g_sin_table[idx], -g_sin_table[discrete_arcs_num_90 - idx]};
+  } else if (2 * discrete_arcs_num_90 <= idx && idx < 3 * discrete_arcs_num_90) {
+    idx = idx - 2 * discrete_arcs_num_90;
+    return {-g_sin_table[idx], -g_sin_table[discrete_arcs_num_90 - idx]};
+  } else {  // 3 * discrete_arcs_num_90 <= idx && idx < 4 * discrete_arcs_num_90
+    idx = 4 * discrete_arcs_num_90 - idx;
+    return {-g_sin_table[idx], g_sin_table[discrete_arcs_num_90 - idx]};
+  }
+}
+
 }  // namespace tier4_autoware_utils
diff --git a/common/tier4_autoware_utils/test/src/math/test_trigonometry.cpp b/common/tier4_autoware_utils/test/src/math/test_trigonometry.cpp
@@ -40,3 +40,13 @@ TEST(trigonometry, cos)
         tier4_autoware_utils::cos(x * static_cast<float>(i))) < 10e-5);
   }
 }
+
+TEST(trigonometry, sin_and_cos)
+{
+  float x = 4.f * tier4_autoware_utils::pi / 128.f;
+  for (int i = 0; i < 128; i++) {
+    const auto sin_and_cos = tier4_autoware_utils::sin_and_cos(x * static_cast<float>(i));
+    EXPECT_TRUE(std::abs(std::sin(x * static_cast<float>(i)) - sin_and_cos.first) < 10e-7);
+    EXPECT_TRUE(std::abs(std::cos(x * static_cast<float>(i)) - sin_and_cos.second) < 10e-7);
+  }
+}
diff --git a/evaluator/perception_online_evaluator/param/perception_online_evaluator.defaults.yaml b/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

diff --git a/...unch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml b/...unch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml
@@ -220,7 +220,7 @@
       <extra_arg name="use_intra_process_comms" value="false"/>
     </composable_node>
 
-    <composable_node pkg="behavior_velocity_planner" plugin="behavior_velocity_planner::BehaviorVelocityPlannerNode" name="behavior_velocity_planner" namespace="">
+    <composable_node pkg="autoware_behavior_velocity_planner" plugin="autoware::behavior_velocity_planner::BehaviorVelocityPlannerNode" name="behavior_velocity_planner" namespace="">
       <!-- topic remap -->
       <remap from="~/input/path_with_lane_id" to="path_with_lane_id"/>
       <remap from="~/input/vector_map" to="$(var input_vector_map_topic_name)"/>

diff --git a/launch/tier4_planning_launch/package.xml b/launch/tier4_planning_launch/package.xml
@@ -57,9 +57,9 @@
   <buildtool_depend>ament_cmake_auto</buildtool_depend>
   <buildtool_depend>autoware_cmake</buildtool_depend>
 
+  <exec_depend>autoware_behavior_velocity_planner</exec_depend>
   <exec_depend>autoware_remaining_distance_time_calculator</exec_depend>
   <exec_depend>behavior_path_planner</exec_depend>
-  <exec_depend>behavior_velocity_planner</exec_depend>
   <exec_depend>costmap_generator</exec_depend>
   <exec_depend>external_cmd_selector</exec_depend>
   <exec_depend>external_velocity_limit_selector</exec_depend>

diff --git a/launch/tier4_system_launch/launch/system.launch.xml b/launch/tier4_system_launch/launch/system.launch.xml
@@ -25,11 +25,11 @@
   <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"/>
 
   <let name="sensor_launch_pkg" value="$(find-pkg-share $(var sensor_model)_launch)"/>
 
@@ -57,6 +57,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 +77,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 +104,31 @@
     </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)"/>
+      <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 -->

diff --git a/localization/geo_pose_projector/CMakeLists.txt b/localization/geo_pose_projector/CMakeLists.txt
@@ -4,11 +4,15 @@ project(geo_pose_projector)
 find_package(autoware_cmake REQUIRED)
 autoware_package()
 
-ament_auto_add_executable(geo_pose_projector
-  src/geo_pose_projector_node.cpp
+ament_auto_add_library(${PROJECT_NAME} SHARED
   src/geo_pose_projector.cpp
 )
-ament_target_dependencies(geo_pose_projector)
+
+rclcpp_components_register_node(${PROJECT_NAME}
+  PLUGIN "GeoPoseProjector"
+  EXECUTABLE ${PROJECT_NAME}_node
+  EXECUTOR SingleThreadedExecutor
+)
 
 ament_auto_package(
   INSTALL_TO_SHARE

diff --git a/localization/geo_pose_projector/launch/geo_pose_projector.launch.xml b/localization/geo_pose_projector/launch/geo_pose_projector.launch.xml
@@ -4,7 +4,7 @@
   <arg name="output_pose" default="/pose_with_covariance"/>
   <arg name="param_path" default="$(find-pkg-share geo_pose_projector)/config/geo_pose_projector.param.yaml"/>
 
-  <node pkg="geo_pose_projector" exec="geo_pose_projector" name="geo_pose_projector" output="screen">
+  <node pkg="geo_pose_projector" exec="geo_pose_projector_node" output="both">
     <remap from="input_geo_pose" to="$(var input_geo_pose)"/>
     <remap from="output_pose" to="$(var output_pose)"/>
 

diff --git a/localization/geo_pose_projector/package.xml b/localization/geo_pose_projector/package.xml
@@ -24,6 +24,7 @@
   <depend>geography_utils</depend>
   <depend>geometry_msgs</depend>
   <depend>rclcpp</depend>
+  <depend>rclcpp_components</depend>
   <depend>tf2_geometry_msgs</depend>
   <depend>tf2_ros</depend>
 

diff --git a/localization/geo_pose_projector/src/geo_pose_projector.cpp b/localization/geo_pose_projector/src/geo_pose_projector.cpp
@@ -25,8 +25,8 @@
 
 #include <string>
 
-GeoPoseProjector::GeoPoseProjector()
-: Node("geo_pose_projector"), publish_tf_(declare_parameter<bool>("publish_tf"))
+GeoPoseProjector::GeoPoseProjector(const rclcpp::NodeOptions & options)
+: rclcpp::Node("geo_pose_projector", options), publish_tf_(declare_parameter<bool>("publish_tf"))
 {
   // Subscribe to map_projector_info topic
   const auto adaptor = component_interface_utils::NodeAdaptor(this);
@@ -102,3 +102,6 @@ void GeoPoseProjector::on_geo_pose(const GeoPoseWithCovariance::ConstSharedPtr m
     tf_broadcaster_->sendTransform(transform_stamped);
   }
 }
+
+#include <rclcpp_components/register_node_macro.hpp>
+RCLCPP_COMPONENTS_REGISTER_NODE(GeoPoseProjector)
diff --git a/localization/geo_pose_projector/src/geo_pose_projector.hpp b/localization/geo_pose_projector/src/geo_pose_projector.hpp
@@ -36,7 +36,7 @@ class GeoPoseProjector : public rclcpp::Node
   using MapProjectorInfo = map_interface::MapProjectorInfo;
 
 public:
-  GeoPoseProjector();
+  explicit GeoPoseProjector(const rclcpp::NodeOptions & options);
 
 private:
   void on_geo_pose(const GeoPoseWithCovariance::ConstSharedPtr msg);

diff --git a/localization/gyro_odometer/CMakeLists.txt b/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