Merge branch 'main' into mau/ref/localization/pose_instability_detector

.github/CODEOWNERS

@@ -87,7 +87,7 @@ localization/autoware_stop_filter/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier
 localization/autoware_twist2accel/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp
 localization/ekf_localizer/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp takamasa.horibe@tier4.jp takeshi.ishita@tier4.jp yamato.ando@tier4.jp
 localization/autoware_geo_pose_projector/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp
-localization/gyro_odometer/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp
+localization/autoware_gyro_odometer/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp
 localization/autoware_localization_error_monitor/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp
 localization/localization_util/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp
 localization/ndt_scan_matcher/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp

control/autoware_autonomous_emergency_braking/src/node.cpp

@@ -348,21 +348,30 @@ bool AEB::fetchLatestData()
     return missing("object detection method (pointcloud or predicted objects)");
   }
 
-  const auto imu_ptr = sub_imu_.takeData();
-  if (use_imu_path_) {
+  const bool has_imu_path = std::invoke([&]() {
+    if (!use_imu_path_) return false;
+    const auto imu_ptr = sub_imu_.takeData();
     if (!imu_ptr) {
       return missing("imu message");
     }
     // imu_ptr is valid
     onImu(imu_ptr);
-  }
-  if (use_imu_path_ && !angular_velocity_ptr_) {
-    return missing("imu");
-  }
+    return (!angular_velocity_ptr_) ? missing("imu") : true;
+  });
 
-  predicted_traj_ptr_ = sub_predicted_traj_.takeData();
-  if (use_predicted_trajectory_ && !predicted_traj_ptr_) {
-    return missing("control predicted trajectory");
+  const bool has_predicted_path = std::invoke([&]() {
+    if (!use_predicted_trajectory_) {
+      return false;
+    }
+    predicted_traj_ptr_ = sub_predicted_traj_.takeData();
+    return (!predicted_traj_ptr_) ? missing("control predicted trajectory") : true;
+  });
+
+  if (!has_imu_path && !has_predicted_path) {
+    RCLCPP_INFO_SKIPFIRST_THROTTLE(
+      get_logger(), *get_clock(), 5000,
+      "[AEB] At least one path (IMU or predicted trajectory) is required for operation");
+    return false;
   }
 
   autoware_state_ = sub_autoware_state_.takeData();
@@ -482,7 +491,7 @@ bool AEB::checkCollision(MarkerArray & debug_markers)
 
   // step3. make function to check collision with ego path created with sensor data
   const auto has_collision_ego = [&](pcl::PointCloud<pcl::PointXYZ>::Ptr filtered_objects) -> bool {
-    if (!use_imu_path_) return false;
+    if (!use_imu_path_ || !angular_velocity_ptr_) return false;
     const double current_w = angular_velocity_ptr_->z;
     constexpr colorTuple debug_color = {0.0 / 256.0, 148.0 / 256.0, 205.0 / 256.0, 0.999};
     const std::string ns = "ego";

control/autoware_vehicle_cmd_gate/config/vehicle_cmd_gate.param.yaml

@@ -4,15 +4,13 @@
     system_emergency_heartbeat_timeout: 0.5
     use_emergency_handling: false
     check_external_emergency_heartbeat: $(var check_external_emergency_heartbeat)
-    use_start_request: false
     enable_cmd_limit_filter: true
     filter_activated_count_threshold: 5
     filter_activated_velocity_threshold: 1.0
     external_emergency_stop_heartbeat_timeout: 0.0
     stop_hold_acceleration: -1.5
     emergency_acceleration: -2.4
     moderate_stop_service_acceleration: -1.5
-    stopped_state_entry_duration_time: 0.1
     stop_check_duration: 1.0
     nominal:
       vel_lim: 25.0

launch/tier4_localization_launch/launch/pose_twist_estimator/gyro_odometer.launch.xml

@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <launch>
   <group>
-    <include file="$(find-pkg-share gyro_odometer)/launch/gyro_odometer.launch.xml">
+    <include file="$(find-pkg-share autoware_gyro_odometer)/launch/gyro_odometer.launch.xml">
       <arg name="input_vehicle_twist_with_covariance_topic" value="/sensing/vehicle_velocity_converter/twist_with_covariance"/>
       <arg name="output_twist_with_covariance_topic" value="/localization/twist_estimator/twist_with_covariance"/>
       <arg name="output_twist_with_covariance_raw_topic" value="/localization/twist_estimator/twist_with_covariance_raw"/>

launch/tier4_localization_launch/launch/pose_twist_estimator/pose_twist_estimator.launch.xml

@@ -93,7 +93,7 @@
 
   <!-- Pose Estimator Arbiter Launch -->
   <group if="$(var multi_localizer_mode)">
-    <include file="$(find-pkg-share pose_estimator_arbiter)/launch/pose_estimator_arbiter.launch.xml">
+    <include file="$(find-pkg-share autoware_pose_estimator_arbiter)/launch/pose_estimator_arbiter.launch.xml">
       <arg name="pose_sources" value="$(var pose_sources)"/>
       <arg name="input_pointcloud" value="$(var input_pointcloud)"/>
     </include>

launch/tier4_localization_launch/package.xml

@@ -20,15 +20,15 @@
   <exec_depend>automatic_pose_initializer</exec_depend>
   <exec_depend>autoware_ar_tag_based_localizer</exec_depend>
   <exec_depend>autoware_geo_pose_projector</exec_depend>
+  <exec_depend>autoware_gyro_odometer</exec_depend>
   <exec_depend>autoware_pointcloud_preprocessor</exec_depend>
+  <exec_depend>autoware_pose_estimator_arbiter</exec_depend>
   <exec_depend>autoware_pose_instability_detector</exec_depend>
   <exec_depend>eagleye_geo_pose_fusion</exec_depend>
   <exec_depend>eagleye_gnss_converter</exec_depend>
   <exec_depend>eagleye_rt</exec_depend>
   <exec_depend>ekf_localizer</exec_depend>
-  <exec_depend>gyro_odometer</exec_depend>
   <exec_depend>ndt_scan_matcher</exec_depend>
-  <exec_depend>pose_estimator_arbiter</exec_depend>
   <exec_depend>pose_initializer</exec_depend>
   <exec_depend>topic_tools</exec_depend>
   <exec_depend>yabloc_common</exec_depend>

localization/gyro_odometer/CMakeLists.txt → localization/autoware_gyro_odometer/CMakeLists.txt

@@ -1,5 +1,5 @@
 cmake_minimum_required(VERSION 3.14)
-project(gyro_odometer)
+project(autoware_gyro_odometer)
 
 find_package(autoware_cmake REQUIRED)
 autoware_package()
@@ -22,6 +22,9 @@ if(BUILD_TESTING)
   target_link_libraries(test_gyro_odometer
     ${PROJECT_NAME}
   )
+  target_include_directories(test_gyro_odometer PRIVATE
+    src
+  )
 endif()
 
 rclcpp_components_register_node(${PROJECT_NAME}

localization/gyro_odometer/README.md → localization/autoware_gyro_odometer/README.md

@@ -1,8 +1,8 @@
-# gyro_odometer
+# autoware_gyro_odometer
 
 ## Purpose
 
-`gyro_odometer` is the package to estimate twist by combining imu and vehicle speed.
+`autoware_gyro_odometer` is the package to estimate twist by combining imu and vehicle speed.
 
 ## Inputs / Outputs
 
@@ -21,7 +21,7 @@
 
 ## Parameters
 
-{{ json_to_markdown("localization/gyro_odometer/schema/gyro_odometer.schema.json") }}
+{{ json_to_markdown("localization/autoware_gyro_odometer/schema/gyro_odometer.schema.json") }}
 
 ## Assumptions / Known limits
 

localization/gyro_odometer/launch/gyro_odometer.launch.xml → localization/autoware_gyro_odometer/launch/gyro_odometer.launch.xml

@@ -9,9 +9,9 @@
   <arg name="output_twist_topic" default="gyro_twist" description="output twist topic name"/>
   <arg name="output_twist_with_covariance_topic" default="gyro_twist_with_covariance" description="output twist with covariance topic name"/>
 
-  <arg name="config_file" default="$(find-pkg-share gyro_odometer)/config/gyro_odometer.param.yaml"/>
+  <arg name="config_file" default="$(find-pkg-share autoware_gyro_odometer)/config/gyro_odometer.param.yaml"/>
 
-  <node pkg="gyro_odometer" exec="gyro_odometer_node" output="both">
+  <node pkg="autoware_gyro_odometer" exec="autoware_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 → localization/autoware_gyro_odometer/package.xml

@@ -1,9 +1,9 @@
 <?xml version="1.0"?>
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
-  <name>gyro_odometer</name>
+  <name>autoware_gyro_odometer</name>
   <version>0.1.0</version>
-  <description>The gyro_odometer package as a ROS 2 node</description>
+  <description>The autoware_gyro_odometer package as a ROS 2 node</description>
   <maintainer email="yamato.ando@tier4.jp">Yamato Ando</maintainer>
   <maintainer email="masahiro.sakamoto@tier4.jp">Masahiro Sakamoto</maintainer>
   <maintainer email="kento.yabuuchi.2@tier4.jp">Kento Yabuuchi</maintainer>

localization/gyro_odometer/src/gyro_odometer_core.cpp → localization/autoware_gyro_odometer/src/gyro_odometer_core.cpp

@@ -12,7 +12,7 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#include "gyro_odometer/gyro_odometer_core.hpp"
+#include "gyro_odometer_core.hpp"
 
 #include <rclcpp/rclcpp.hpp>
 
@@ -23,6 +23,7 @@
 #endif
 #include <fmt/core.h>
 
+#include <algorithm>
 #include <cmath>
 #include <memory>
 #include <sstream>

localization/gyro_odometer/include/gyro_odometer/gyro_odometer_core.hpp → localization/autoware_gyro_odometer/src/gyro_odometer_core.hpp

@@ -12,8 +12,8 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef GYRO_ODOMETER__GYRO_ODOMETER_CORE_HPP_
-#define GYRO_ODOMETER__GYRO_ODOMETER_CORE_HPP_
+#ifndef GYRO_ODOMETER_CORE_HPP_
+#define GYRO_ODOMETER_CORE_HPP_
 
 #include "autoware/universe_utils/ros/logger_level_configure.hpp"
 #include "autoware/universe_utils/ros/msg_covariance.hpp"
@@ -85,4 +85,4 @@ class GyroOdometerNode : public rclcpp::Node
 
 }  // namespace autoware::gyro_odometer
 
-#endif  // GYRO_ODOMETER__GYRO_ODOMETER_CORE_HPP_
+#endif  // GYRO_ODOMETER_CORE_HPP_

localization/gyro_odometer/test/test_gyro_odometer_pubsub.cpp → localization/autoware_gyro_odometer/test/test_gyro_odometer_pubsub.cpp

@@ -12,7 +12,7 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#include "gyro_odometer/gyro_odometer_core.hpp"
+#include "gyro_odometer_core.hpp"
 #include "test_gyro_odometer_helper.hpp"
 
 #include <rclcpp/rclcpp.hpp>

localization/pose_estimator_arbiter/CMakeLists.txt → localization/autoware_pose_estimator_arbiter/CMakeLists.txt

@@ -1,22 +1,25 @@
 cmake_minimum_required(VERSION 3.14)
-project(pose_estimator_arbiter)
+project(autoware_pose_estimator_arbiter)
 
 find_package(autoware_cmake REQUIRED)
 autoware_package()
 
 find_package(PCL REQUIRED COMPONENTS common)
-include_directories(SYSTEM ${PCL_INCLUDE_DIRS})
+include_directories(
+  SYSTEM ${PCL_INCLUDE_DIRS}
+  src
+)
 
 # ==============================
 # pose estimator arbiter node
 ament_auto_add_library(${PROJECT_NAME} SHARED
-  src/pose_estimator_arbiter/pose_estimator_arbiter_core.cpp
-  src/pose_estimator_arbiter/switch_rule/enable_all_rule.cpp
+  src/pose_estimator_arbiter_core.cpp
+  src/switch_rule/enable_all_rule.cpp
 )
 target_include_directories(${PROJECT_NAME} PUBLIC src)
 
 rclcpp_components_register_node(${PROJECT_NAME}
-  PLUGIN "pose_estimator_arbiter::PoseEstimatorArbiter"
+  PLUGIN "autoware::pose_estimator_arbiter::PoseEstimatorArbiter"
   EXECUTABLE ${PROJECT_NAME}_node
   EXECUTOR MultiThreadedExecutor
 )

localization/pose_estimator_arbiter/README.md → localization/autoware_pose_estimator_arbiter/README.md

@@ -1,4 +1,4 @@
-# pose_estimator_arbiter
+# autoware_pose_estimator_arbiter
 
 Table of contents:
 
@@ -35,7 +35,7 @@ Also, even if both can be activated at the same time, the Kalman Filter may be a
 - [ndt_scan_matcher](https://github.com/autowarefoundation/autoware.universe/tree/main/localization/ndt_scan_matcher)
 - [eagleye](https://autowarefoundation.github.io/autoware-documentation/main/how-to-guides/integrating-autoware/launch-autoware/localization/eagleye/)
 - [yabloc](https://github.com/autowarefoundation/autoware.universe/tree/main/localization/yabloc)
-- [landmark_based_localizer](https://github.com/autowarefoundation/autoware.universe/tree/main/localization/landmark_based_localizer)
+- [landmark_based_localizer](https://github.com/autowarefoundation/autoware.universe/tree/main/localization/autoware_landmark_based_localizer)
 
 ### Demonstration
 
@@ -117,8 +117,8 @@ If it does not seems to work, users can get more information in the following wa
 > [!TIP]
 >
 > ```bash
-> ros2 service call /localization/pose_estimator_arbiter/config_logger logging_demo/srv/ConfigLogger \
->   '{logger_name: localization.pose_estimator_arbiter, level: debug}'
+> ros2 service call /localization/autoware_pose_estimator_arbiter/config_logger logging_demo/srv/ConfigLogger \
+>   '{logger_name: localization.autoware_pose_estimator_arbiter, level: debug}'
 > ```
 
 ## Architecture
@@ -135,7 +135,7 @@ Following figure shows the node configuration when NDT, YabLoc Eagleye and AR-Ta
 
 ### Case of running multiple pose estimators
 
-When running multiple pose_estimators, pose_estimator_arbiter is executed.
+When running multiple pose_estimators, autoware_pose_estimator_arbiter is executed.
 It comprises a **switching rule** and **stoppers** corresponding to each pose_estimator.
 
 - Stoppers control the pose_estimator activity by relaying inputs or outputs, or by requesting a suspend service.
@@ -187,14 +187,14 @@ ros2 launch autoware_launch logging_simulator.launch.xml \
 Even if `pose_source` includes an unexpected string, it will be filtered appropriately.
 Please see the table below for details.
 
-| given runtime argument                      | parsed pose_estimator_arbiter's param (pose_sources) |
-| ------------------------------------------- | ---------------------------------------------------- |
-| `pose_source:=ndt`                          | `["ndt"]`                                            |
-| `pose_source:=nan`                          | `[]`                                                 |
-| `pose_source:=yabloc_ndt`                   | `["ndt","yabloc"]`                                   |
-| `pose_source:=yabloc_ndt_ndt_ndt`           | `["ndt","yabloc"]`                                   |
-| `pose_source:=ndt_yabloc_eagleye`           | `["ndt","yabloc","eagleye"]`                         |
-| `pose_source:=ndt_yabloc_nan_eagleye_artag` | `["ndt","yabloc","eagleye","artag"]`                 |
+| given runtime argument                      | parsed autoware_pose_estimator_arbiter's param (pose_sources) |
+| ------------------------------------------- | ------------------------------------------------------------- |
+| `pose_source:=ndt`                          | `["ndt"]`                                                     |
+| `pose_source:=nan`                          | `[]`                                                          |
+| `pose_source:=yabloc_ndt`                   | `["ndt","yabloc"]`                                            |
+| `pose_source:=yabloc_ndt_ndt_ndt`           | `["ndt","yabloc"]`                                            |
+| `pose_source:=ndt_yabloc_eagleye`           | `["ndt","yabloc","eagleye"]`                                  |
+| `pose_source:=ndt_yabloc_nan_eagleye_artag` | `["ndt","yabloc","eagleye","artag"]`                          |
 
 </details>
 

localization/pose_estimator_arbiter/example_rule/CMakeLists.txt → localization/autoware_pose_estimator_arbiter/example_rule/CMakeLists.txt

@@ -1,13 +1,13 @@
 # example switch rule library
 ament_auto_add_library(example_rule
   SHARED
-  src/pose_estimator_arbiter/switch_rule/pcd_map_based_rule.cpp
-  src/pose_estimator_arbiter/switch_rule/vector_map_based_rule.cpp
-  src/pose_estimator_arbiter/rule_helper/pcd_occupancy.cpp
-  src/pose_estimator_arbiter/rule_helper/pose_estimator_area.cpp
+  src/switch_rule/pcd_map_based_rule.cpp
+  src/switch_rule/vector_map_based_rule.cpp
+  src/rule_helper/pcd_occupancy.cpp
+  src/rule_helper/pose_estimator_area.cpp
 )
 target_include_directories(example_rule PUBLIC src example_rule/src)
-target_link_libraries(example_rule pose_estimator_arbiter)
+target_link_libraries(example_rule autoware_pose_estimator_arbiter)
 
 # ==============================
 # define test definition macro
@@ -16,7 +16,7 @@ function(add_testcase filepath)
   string(REGEX REPLACE ".cpp" "" test_name ${filename})
 
   ament_add_gtest(${test_name} ${filepath})
-  target_link_libraries("${test_name}" pose_estimator_arbiter example_rule)
+  target_link_libraries("${test_name}" autoware_pose_estimator_arbiter example_rule)
   target_include_directories(${test_name} PUBLIC src)
   ament_target_dependencies(${test_name} ${${PROJECT_NAME}_FOUND_BUILD_DEPENDS})
 endfunction()

localization/pose_estimator_arbiter/example_rule/src/pose_estimator_arbiter/rule_helper/grid_key.hpp → localization/autoware_pose_estimator_arbiter/example_rule/src/rule_helper/grid_key.hpp

@@ -12,15 +12,15 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef POSE_ESTIMATOR_ARBITER__RULE_HELPER__GRID_KEY_HPP_
-#define POSE_ESTIMATOR_ARBITER__RULE_HELPER__GRID_KEY_HPP_
+#ifndef RULE_HELPER__GRID_KEY_HPP_
+#define RULE_HELPER__GRID_KEY_HPP_
 #include <boost/functional/hash.hpp>
 
 #include <pcl/point_types.h>
 
 #include <functional>
 
-namespace pose_estimator_arbiter::rule_helper
+namespace autoware::pose_estimator_arbiter::rule_helper
 {
 struct GridKey
 {
@@ -46,16 +46,16 @@ struct GridKey
   friend bool operator!=(const GridKey & one, const GridKey & other) { return !(one == other); }
 };
 
-}  // namespace pose_estimator_arbiter::rule_helper
+}  // namespace autoware::pose_estimator_arbiter::rule_helper
 
 // This is for unordered_map and unordered_set
 namespace std
 {
 template <>
-struct hash<pose_estimator_arbiter::rule_helper::GridKey>
+struct hash<autoware::pose_estimator_arbiter::rule_helper::GridKey>
 {
 public:
-  size_t operator()(const pose_estimator_arbiter::rule_helper::GridKey & grid) const
+  size_t operator()(const autoware::pose_estimator_arbiter::rule_helper::GridKey & grid) const
   {
     std::size_t seed = 0;
     boost::hash_combine(seed, grid.x);
@@ -65,4 +65,4 @@ struct hash<pose_estimator_arbiter::rule_helper::GridKey>
 };
 }  // namespace std
 
-#endif  // POSE_ESTIMATOR_ARBITER__RULE_HELPER__GRID_KEY_HPP_
+#endif  // RULE_HELPER__GRID_KEY_HPP_