Merge branch 'main' into obstacle_stop_planner

.github/CODEOWNERS

@@ -3,6 +3,7 @@ common/autoware_ad_api_specs/** [email protected] [email protected]
 common/autoware_auto_common/** [email protected] [email protected] [email protected] [email protected]
 common/autoware_geography_utils/** [email protected]
 common/autoware_grid_map_utils/** [email protected]
+common/autoware_kalman_filter/** [email protected] [email protected] [email protected]
 common/autoware_motion_utils/** [email protected] [email protected] [email protected] [email protected] [email protected] [email protected] [email protected] [email protected]
 common/autoware_overlay_rviz_plugin/autoware_mission_details_overlay_rviz_plugin/** [email protected]
 common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/** [email protected]
@@ -22,7 +23,6 @@ common/global_parameter_loader/** [email protected]
 common/glog_component/** [email protected]
 common/goal_distance_calculator/** [email protected]
 common/interpolation/** [email protected] [email protected]
-common/kalman_filter/** [email protected] [email protected] [email protected]
 common/object_recognition_utils/** [email protected] [email protected] [email protected]
 common/osqp_interface/** [email protected] [email protected] [email protected] [email protected]
 common/perception_utils/** [email protected] [email protected]

common/kalman_filter/CMakeLists.txt → common/autoware_kalman_filter/CMakeLists.txt

@@ -1,5 +1,5 @@
 cmake_minimum_required(VERSION 3.14)
-project(kalman_filter)
+project(autoware_kalman_filter)
 
 find_package(autoware_cmake REQUIRED)
 autoware_package()
@@ -12,18 +12,18 @@ include_directories(
     ${EIGEN3_INCLUDE_DIR}
 )
 
-ament_auto_add_library(kalman_filter SHARED
+ament_auto_add_library(${PROJECT_NAME} SHARED
   src/kalman_filter.cpp
   src/time_delay_kalman_filter.cpp
-  include/kalman_filter/kalman_filter.hpp
-  include/kalman_filter/time_delay_kalman_filter.hpp
+  include/autoware/kalman_filter/kalman_filter.hpp
+  include/autoware/kalman_filter/time_delay_kalman_filter.hpp
 )
 
 if(BUILD_TESTING)
   file(GLOB_RECURSE test_files test/*.cpp)
-  ament_add_ros_isolated_gtest(test_kalman_filter ${test_files})
+  ament_add_ros_isolated_gtest(test_${PROJECT_NAME} ${test_files})
 
-  target_link_libraries(test_kalman_filter kalman_filter)
+  target_link_libraries(test_${PROJECT_NAME} ${PROJECT_NAME})
 endif()
 
 ament_auto_package()

common/kalman_filter/include/kalman_filter/kalman_filter.hpp → common/autoware_kalman_filter/include/autoware/kalman_filter/kalman_filter.hpp

@@ -12,12 +12,15 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef KALMAN_FILTER__KALMAN_FILTER_HPP_
-#define KALMAN_FILTER__KALMAN_FILTER_HPP_
+#ifndef AUTOWARE__KALMAN_FILTER__KALMAN_FILTER_HPP_
+#define AUTOWARE__KALMAN_FILTER__KALMAN_FILTER_HPP_
 
 #include <Eigen/Core>
 #include <Eigen/LU>
 
+namespace autoware::kalman_filter
+{
+
 /**
  * @file kalman_filter.h
  * @brief kalman filter class
@@ -207,4 +210,5 @@ class KalmanFilter
   Eigen::MatrixXd R_;  //!< @brief covariance matrix for measurement model y[k] = C * x[k]
   Eigen::MatrixXd P_;  //!< @brief covariance of estimated state
 };
-#endif  // KALMAN_FILTER__KALMAN_FILTER_HPP_
+}  // namespace autoware::kalman_filter
+#endif  // AUTOWARE__KALMAN_FILTER__KALMAN_FILTER_HPP_

common/kalman_filter/include/kalman_filter/time_delay_kalman_filter.hpp → common/autoware_kalman_filter/include/autoware/kalman_filter/time_delay_kalman_filter.hpp

@@ -12,16 +12,18 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef KALMAN_FILTER__TIME_DELAY_KALMAN_FILTER_HPP_
-#define KALMAN_FILTER__TIME_DELAY_KALMAN_FILTER_HPP_
+#ifndef AUTOWARE__KALMAN_FILTER__TIME_DELAY_KALMAN_FILTER_HPP_
+#define AUTOWARE__KALMAN_FILTER__TIME_DELAY_KALMAN_FILTER_HPP_
 
-#include "kalman_filter/kalman_filter.hpp"
+#include "autoware/kalman_filter/kalman_filter.hpp"
 
 #include <Eigen/Core>
 #include <Eigen/LU>
 
 #include <iostream>
 
+namespace autoware::kalman_filter
+{
 /**
  * @file time_delay_kalman_filter.h
  * @brief kalman filter with delayed measurement class
@@ -83,4 +85,5 @@ class TimeDelayKalmanFilter : public KalmanFilter
   int dim_x_;           //!< @brief dimension of latest state
   int dim_x_ex_;        //!< @brief dimension of extended state with dime delay
 };
-#endif  // KALMAN_FILTER__TIME_DELAY_KALMAN_FILTER_HPP_
+}  // namespace autoware::kalman_filter
+#endif  // AUTOWARE__KALMAN_FILTER__TIME_DELAY_KALMAN_FILTER_HPP_

common/kalman_filter/package.xml → common/autoware_kalman_filter/package.xml

@@ -1,7 +1,7 @@
 <?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>kalman_filter</name>
+  <name>autoware_kalman_filter</name>
   <version>0.1.0</version>
   <description>The kalman filter package</description>
   <maintainer email="[email protected]">Yukihiro Saito</maintainer>

common/kalman_filter/src/kalman_filter.cpp → common/autoware_kalman_filter/src/kalman_filter.cpp

@@ -12,8 +12,10 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#include "kalman_filter/kalman_filter.hpp"
+#include "autoware/kalman_filter/kalman_filter.hpp"
 
+namespace autoware::kalman_filter
+{
 KalmanFilter::KalmanFilter()
 {
 }
@@ -156,3 +158,4 @@ bool KalmanFilter::update(const Eigen::MatrixXd & y)
 {
   return update(y, C_, R_);
 }
+}  // namespace autoware::kalman_filter

common/kalman_filter/src/time_delay_kalman_filter.cpp → common/autoware_kalman_filter/src/time_delay_kalman_filter.cpp

@@ -12,8 +12,10 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#include "kalman_filter/time_delay_kalman_filter.hpp"
+#include "autoware/kalman_filter/time_delay_kalman_filter.hpp"
 
+namespace autoware::kalman_filter
+{
 TimeDelayKalmanFilter::TimeDelayKalmanFilter()
 {
 }
@@ -102,3 +104,4 @@ bool TimeDelayKalmanFilter::updateWithDelay(
 
   return true;
 }
+}  // namespace autoware::kalman_filter

common/kalman_filter/test/test_kalman_filter.cpp → common/autoware_kalman_filter/test/test_kalman_filter.cpp

@@ -12,10 +12,12 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#include "kalman_filter/kalman_filter.hpp"
+#include "autoware/kalman_filter/kalman_filter.hpp"
 
 #include <gtest/gtest.h>
 
+using autoware::kalman_filter::KalmanFilter;
+
 TEST(kalman_filter, kf)
 {
   KalmanFilter kf_;

common/kalman_filter/test/test_time_delay_kalman_filter.cpp → common/autoware_kalman_filter/test/test_time_delay_kalman_filter.cpp

@@ -12,10 +12,12 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#include "kalman_filter/time_delay_kalman_filter.hpp"
+#include "autoware/kalman_filter/time_delay_kalman_filter.hpp"
 
 #include <gtest/gtest.h>
 
+using autoware::kalman_filter::TimeDelayKalmanFilter;
+
 TEST(time_delay_kalman_filter, td_kf)
 {
   TimeDelayKalmanFilter td_kf_;

common/perception_utils/CMakeLists.txt

@@ -4,8 +4,6 @@ project(perception_utils)
 find_package(autoware_cmake REQUIRED)
 autoware_package()
 
-ament_auto_package()
-
 ament_auto_add_library(${PROJECT_NAME} SHARED
   src/run_length_encoder.cpp
 )
@@ -14,3 +12,5 @@ find_package(OpenCV REQUIRED)
 target_link_libraries(${PROJECT_NAME}
   ${OpenCV_LIBS}
 )
+
+ament_auto_package()

control/autoware_vehicle_cmd_gate/src/vehicle_cmd_gate.cpp

@@ -533,62 +533,55 @@ void VehicleCmdGate::publishControlCommands(const Commands & commands)
     return;
   }
 
-  Commands filtered_commands;
-
   // Set default commands
-  {
-    filtered_commands.control = commands.control;
-    filtered_commands.gear = commands.gear;  // tmp
-  }
+  Control filtered_control = commands.control;
 
   if (moderate_stop_interface_->is_stop_requested()) {  // if stop requested, stop the vehicle
-    filtered_commands.control.longitudinal.velocity = 0.0;
-    filtered_commands.control.longitudinal.acceleration = moderate_stop_service_acceleration_;
+    filtered_control.longitudinal.velocity = 0.0;
+    filtered_control.longitudinal.acceleration = moderate_stop_service_acceleration_;
   }
 
   // Check emergency
   if (use_emergency_handling_ && is_system_emergency_) {
     RCLCPP_WARN_THROTTLE(
       get_logger(), *get_clock(), std::chrono::milliseconds(1000).count(), "Emergency!");
-    filtered_commands.control = emergency_commands_.control;
-    filtered_commands.gear = emergency_commands_.gear;  // tmp
+    filtered_control = emergency_commands_.control;
   }
 
   // Check engage
   if (!is_engaged_) {
-    filtered_commands.control.longitudinal = createLongitudinalStopControlCmd();
+    filtered_control.longitudinal = createLongitudinalStopControlCmd();
   }
 
   // Check pause. Place this check after all other checks as it needs the final output.
-  adapi_pause_->update(filtered_commands.control);
+  adapi_pause_->update(filtered_control);
   if (adapi_pause_->is_paused()) {
     if (is_engaged_) {
-      filtered_commands.control.longitudinal = createLongitudinalStopControlCmd();
+      filtered_control.longitudinal = createLongitudinalStopControlCmd();
     } else {
-      filtered_commands.control = createStopControlCmd();
+      filtered_control = createStopControlCmd();
     }
   }
 
   // Check if command filtering option is enable
   if (enable_cmd_limit_filter_) {
     // Apply limit filtering
-    filtered_commands.control = filterControlCommand(filtered_commands.control);
+    filtered_control = filterControlCommand(filtered_control);
   }
   // tmp: Publish vehicle emergency status
   VehicleEmergencyStamped vehicle_cmd_emergency;
   vehicle_cmd_emergency.emergency = (use_emergency_handling_ && is_system_emergency_);
-  vehicle_cmd_emergency.stamp = filtered_commands.control.stamp;
+  vehicle_cmd_emergency.stamp = filtered_control.stamp;
 
   // Publish commands
   vehicle_cmd_emergency_pub_->publish(vehicle_cmd_emergency);
-  control_cmd_pub_->publish(filtered_commands.control);
-  published_time_publisher_->publish_if_subscribed(
-    control_cmd_pub_, filtered_commands.control.stamp);
+  control_cmd_pub_->publish(filtered_control);
+  published_time_publisher_->publish_if_subscribed(control_cmd_pub_, filtered_control.stamp);
   adapi_pause_->publish();
   moderate_stop_interface_->publish();
 
   // Save ControlCmd to steering angle when disengaged
-  prev_control_cmd_ = filtered_commands.control;
+  prev_control_cmd_ = filtered_control;
 }
 
 void VehicleCmdGate::publishEmergencyStopControlCommands()
@@ -903,7 +896,6 @@ MarkerArray VehicleCmdGate::createMarkerArray(const IsFilterActivated & filter_a
   }
   if (filter_activated.is_activated_on_steering_rate) {
     reason += first_msg ? " steer_rate" : ", steer_rate";
-    first_msg = false;
   }
 
   msg.markers.emplace_back(createStringMarker(

launch/tier4_simulator_launch/launch/simulator.launch.xml

@@ -5,13 +5,18 @@
   <arg name="laserscan_based_occupancy_grid_map_param_path"/>
   <arg name="occupancy_grid_map_updater"/>
   <arg name="occupancy_grid_map_updater_param_path"/>
+
+  <arg name="localization_error_monitor_param_path"/>
+  <arg name="ekf_localizer_param_path"/>
   <arg name="pose_initializer_param_path"/>
+  <arg name="twist2accel_param_path"/>
+
   <arg name="launch_simulator_perception_modules" default="true"/>
   <arg name="launch_dummy_perception"/>
   <arg name="launch_dummy_vehicle"/>
   <arg
     name="localization_sim_mode"
-    description="Select localization mode. Options are 'none' or 'api'. 'api' starts an external API for initial position estimation. 'none' does not start any localization-related process."
+    description="Select localization mode. Options are 'none', 'api' or 'pose_twist_estimator'. 'pose_twist_estimator' starts most of the localization modules except for the ndt_scan_matcher. 'api' starts an external API for initial position estimation. 'none' does not start any localization-related process."
   />
   <arg name="launch_dummy_doors"/>
   <arg name="launch_diagnostic_converter"/>
@@ -130,7 +135,7 @@
     </group>
   </group>
 
-  <!-- Dummy localization -->
+  <!-- localization -->
   <group if="$(eval '&quot;$(var localization_sim_mode)&quot;==&quot;none&quot;')">
     <!-- Do nothing -->
   </group>
@@ -148,6 +153,50 @@
     </include>
   </group>
 
+  <group if="$(eval '&quot;$(var localization_sim_mode)&quot;==&quot;pose_twist_estimator&quot;')">
+    <group>
+      <!-- start name space localization -->
+      <push-ros-namespace namespace="localization"/>
+      <group>
+        <push-ros-namespace namespace="util"/>
+        <include file="$(find-pkg-share pose_initializer)/launch/pose_initializer.launch.xml">
+          <arg name="user_defined_initial_pose/enable" value="false"/>
+          <arg name="user_defined_initial_pose/pose" value="[0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0]"/>
+          <arg name="ndt_enabled" value="false"/>
+          <arg name="gnss_enabled" value="false"/>
+          <arg name="ekf_enabled" value="true"/>
+          <arg name="yabloc_enabled" value="false"/>
+          <arg name="stop_check_enabled" value="false"/>
+          <arg name="config_file" value="$(var pose_initializer_param_path)"/>
+        </include>
+      </group>
+
+      <group>
+        <push-ros-namespace namespace="twist_estimator"/>
+        <include file="$(find-pkg-share tier4_localization_launch)/launch/pose_twist_estimator/gyro_odometer.launch.xml"/>
+      </group>
+
+      <!-- pose_twist_fusion_filter module -->
+      <group>
+        <push-ros-namespace namespace="pose_twist_fusion_filter"/>
+        <include file="$(find-pkg-share tier4_localization_launch)/launch/pose_twist_fusion_filter/pose_twist_fusion_filter.launch.xml"/>
+      </group>
+    </group>
+    <!-- end name space localization -->
+    <group>
+      <push-ros-namespace namespace="sensing"/>
+      <arg name="input_vehicle_velocity_topic" default="/vehicle/status/velocity_status"/>
+      <arg name="output_twist_with_covariance" default="/sensing/vehicle_velocity_converter/twist_with_covariance"/>
+      <arg name="config_file" default="$(find-pkg-share vehicle_velocity_converter)/config/vehicle_velocity_converter.param.yaml"/>
+
+      <node pkg="vehicle_velocity_converter" exec="vehicle_velocity_converter_node" output="both">
+        <param from="$(var config_file)"/>
+        <remap from="velocity_status" to="$(var input_vehicle_velocity_topic)"/>
+        <remap from="twist_with_covariance" to="$(var output_twist_with_covariance)"/>
+      </node>
+    </group>
+  </group>
+
   <!-- Dummy doors -->
   <group if="$(var launch_dummy_doors)">
     <include file="$(find-pkg-share vehicle_door_simulator)/launch/vehicle_door_simulator.launch.xml"/>
@@ -163,11 +212,15 @@
   <!-- Simulator model -->
   <group if="$(var launch_dummy_vehicle)">
     <arg name="simulator_model" default="$(var vehicle_model_pkg)/config/simulator_model.param.yaml" description="path to the file of simulator model"/>
+    <!-- 'pose_only' mode publishes the pose topic as an alternative to ndt_localizer. 'full_motion' mode publishes the odometry and acceleration topics as an alternative to localization modules. -->
+    <let name="motion_publish_mode" value="pose_only" if="$(eval '&quot;$(var localization_sim_mode)&quot;==&quot;pose_twist_estimator&quot;')"/>
+    <let name="motion_publish_mode" value="full_motion" unless="$(eval '&quot;$(var localization_sim_mode)&quot;==&quot;pose_twist_estimator&quot;')"/>
     <include file="$(find-pkg-share simple_planning_simulator)/launch/simple_planning_simulator.launch.py">
       <arg name="vehicle_info_param_file" value="$(var vehicle_info_param_file)"/>
       <arg name="simulator_model_param_file" value="$(var simulator_model)"/>
       <arg name="initial_engage_state" value="$(var initial_engage_state)"/>
       <arg name="raw_vehicle_cmd_converter_param_path" value="$(var raw_vehicle_cmd_converter_param_path)"/>
+      <arg name="motion_publish_mode" value="$(var motion_publish_mode)"/>
     </include>
   </group>
 </launch>

localization/ekf_localizer/include/ekf_localizer/ekf_module.hpp

@@ -19,8 +19,8 @@
 #include "ekf_localizer/state_index.hpp"
 #include "ekf_localizer/warning.hpp"
 
-#include <kalman_filter/kalman_filter.hpp>
-#include <kalman_filter/time_delay_kalman_filter.hpp>
+#include <autoware/kalman_filter/kalman_filter.hpp>
+#include <autoware/kalman_filter/time_delay_kalman_filter.hpp>
 #include <rclcpp/rclcpp.hpp>
 
 #include <geometry_msgs/msg/pose_stamped.hpp>
@@ -34,6 +34,8 @@
 #include <memory>
 #include <vector>
 
+using autoware::kalman_filter::TimeDelayKalmanFilter;
+
 struct EKFDiagnosticInfo
 {
   size_t no_update_count{0};

localization/ekf_localizer/package.xml

@@ -22,11 +22,11 @@
 
   <build_depend>eigen</build_depend>
 
+  <depend>autoware_kalman_filter</depend>
   <depend>autoware_universe_utils</depend>
   <depend>diagnostic_msgs</depend>
   <depend>fmt</depend>
   <depend>geometry_msgs</depend>
-  <depend>kalman_filter</depend>
   <depend>localization_util</depend>
   <depend>nav_msgs</depend>
   <depend>rclcpp</depend>