Merge branch 'main' into fix/segmenation_pointcloud_filter_add_rle_fi…

…lter

.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_;

control/autoware_vehicle_cmd_gate/src/vehicle_cmd_gate.cpp

@@ -538,7 +538,6 @@ void VehicleCmdGate::publishControlCommands(const Commands & commands)
   // Set default commands
   {
     filtered_commands.control = commands.control;
-    filtered_commands.gear = commands.gear;  // tmp
   }
 
   if (moderate_stop_interface_->is_stop_requested()) {  // if stop requested, stop the vehicle
@@ -551,7 +550,6 @@ void VehicleCmdGate::publishControlCommands(const Commands & commands)
     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
   }
 
   // Check engage
@@ -903,7 +901,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(

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>

perception/autoware_bytetrack/lib/include/strack.h

@@ -38,8 +38,7 @@
 
 #pragma once
 
-// #include "kalman_filter.h"
-#include <kalman_filter/kalman_filter.hpp>
+#include <autoware/kalman_filter/kalman_filter.hpp>
 #include <opencv2/opencv.hpp>
 
 #include <boost/uuid/uuid.hpp>
@@ -49,6 +48,8 @@
 
 enum TrackState { New = 0, Tracked, Lost, Removed };
 
+using autoware::kalman_filter::KalmanFilter;
+
 /** manage one tracklet*/
 class STrack
 {

perception/autoware_bytetrack/package.xml

@@ -15,12 +15,12 @@
   <buildtool_export_depend>cudnn_cmake_module</buildtool_export_depend>
   <buildtool_export_depend>tensorrt_cmake_module</buildtool_export_depend>
 
+  <depend>autoware_kalman_filter</depend>
   <depend>autoware_perception_msgs</depend>
   <depend>cuda_utils</depend>
   <depend>cv_bridge</depend>
   <depend>eigen</depend>
   <depend>image_transport</depend>
-  <depend>kalman_filter</depend>
   <depend>libboost-system-dev</depend>
   <depend>libopencv-dev</depend>
   <depend>rclcpp</depend>

perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/utils/utils.hpp

@@ -41,6 +41,7 @@
 
 #include "autoware_perception_msgs/msg/shape.hpp"
 #include "tier4_perception_msgs/msg/detected_object_with_feature.hpp"
+#include <sensor_msgs/msg/camera_info.hpp>
 
 #include <image_geometry/pinhole_camera_model.h>
 #include <pcl/point_cloud.h>
@@ -62,6 +63,8 @@ struct PointData
   size_t orig_index;
 };
 
+bool checkCameraInfo(const sensor_msgs::msg::CameraInfo & camera_info);
+
 Eigen::Vector2d calcRawImageProjectedPoint(
   const image_geometry::PinholeCameraModel & pinhole_camera_model, const cv::Point3d & point3d);
 

perception/autoware_image_projection_based_fusion/src/pointpainting_fusion/node.cpp

@@ -271,6 +271,8 @@ void PointPaintingFusionNode::fuseOnSingleImage(
     return;
   }
 
+  if (!checkCameraInfo(camera_info)) return;
+
   std::vector<sensor_msgs::msg::RegionOfInterest> debug_image_rois;
   std::vector<Eigen::Vector2d> debug_image_points;
 

perception/autoware_image_projection_based_fusion/src/roi_cluster_fusion/node.cpp

@@ -83,6 +83,8 @@ void RoiClusterFusionNode::fuseOnSingleImage(
   const DetectedObjectsWithFeature & input_roi_msg,
   const sensor_msgs::msg::CameraInfo & camera_info, DetectedObjectsWithFeature & output_cluster_msg)
 {
+  if (!checkCameraInfo(camera_info)) return;
+
   image_geometry::PinholeCameraModel pinhole_camera_model;
   pinhole_camera_model.fromCameraInfo(camera_info);
 

perception/autoware_image_projection_based_fusion/src/roi_detected_object_fusion/node.cpp

@@ -75,6 +75,8 @@ void RoiDetectedObjectFusionNode::fuseOnSingleImage(
   const sensor_msgs::msg::CameraInfo & camera_info,
   DetectedObjects & output_object_msg __attribute__((unused)))
 {
+  if (!checkCameraInfo(camera_info)) return;
+
   Eigen::Affine3d object2camera_affine;
   {
     const auto transform_stamped_optional = getTransformStamped(

perception/autoware_image_projection_based_fusion/src/roi_pointcloud_fusion/node.cpp

@@ -82,6 +82,8 @@ void RoiPointCloudFusionNode::fuseOnSingleImage(
   if (input_pointcloud_msg.data.empty()) {
     return;
   }
+  if (!checkCameraInfo(camera_info)) return;
+
   std::vector<DetectedObjectWithFeature> output_objs;
   // select ROIs for fusion
   for (const auto & feature_obj : input_roi_msg.feature_objects) {

perception/autoware_image_projection_based_fusion/src/segmentation_pointcloud_fusion/node.cpp

@@ -60,6 +60,7 @@ void SegmentPointCloudFusionNode::fuseOnSingleImage(
   if (input_pointcloud_msg.data.empty()) {
     return;
   }
+  if (!checkCameraInfo(camera_info)) return;
   if (input_mask.height == 0 || input_mask.width == 0) {
     return;
   }

perception/autoware_image_projection_based_fusion/src/utils/utils.cpp

@@ -14,8 +14,32 @@
 
 #include "autoware/image_projection_based_fusion/utils/utils.hpp"
 
+#include <sensor_msgs/distortion_models.hpp>
+
 namespace autoware::image_projection_based_fusion
 {
+bool checkCameraInfo(const sensor_msgs::msg::CameraInfo & camera_info)
+{
+  const bool is_supported_model =
+    (camera_info.distortion_model == sensor_msgs::distortion_models::PLUMB_BOB ||
+     camera_info.distortion_model == sensor_msgs::distortion_models::RATIONAL_POLYNOMIAL);
+  if (!is_supported_model) {
+    RCLCPP_ERROR_STREAM(
+      rclcpp::get_logger("image_projection_based_fusion"),
+      "checkCameraInfo: Unsupported distortion model: " << camera_info.distortion_model);
+    return false;
+  }
+  const bool is_supported_distortion_param =
+    (camera_info.d.size() == 5 || camera_info.d.size() == 8);
+  if (!is_supported_distortion_param) {
+    RCLCPP_ERROR_STREAM(
+      rclcpp::get_logger("image_projection_based_fusion"),
+      "checkCameraInfo: Unsupported distortion coefficients size: " << camera_info.d.size());
+    return false;
+  }
+  return true;
+}
+
 Eigen::Vector2d calcRawImageProjectedPoint(
   const image_geometry::PinholeCameraModel & pinhole_camera_model, const cv::Point3d & point3d)
 {

perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/bicycle_tracker.hpp

@@ -19,10 +19,10 @@
 #ifndef AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MODEL__BICYCLE_TRACKER_HPP_
 #define AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MODEL__BICYCLE_TRACKER_HPP_
 
+#include "autoware/kalman_filter/kalman_filter.hpp"
 #include "autoware/multi_object_tracker/tracker/model/tracker_base.hpp"
 #include "autoware/multi_object_tracker/tracker/motion_model/bicycle_motion_model.hpp"
 #include "autoware/multi_object_tracker/tracker/object_model/object_model.hpp"
-#include "kalman_filter/kalman_filter.hpp"
 
 namespace autoware::multi_object_tracker
 {

perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/big_vehicle_tracker.hpp

@@ -19,10 +19,10 @@
 #ifndef AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MODEL__BIG_VEHICLE_TRACKER_HPP_
 #define AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MODEL__BIG_VEHICLE_TRACKER_HPP_
 
+#include "autoware/kalman_filter/kalman_filter.hpp"
 #include "autoware/multi_object_tracker/tracker/model/tracker_base.hpp"
 #include "autoware/multi_object_tracker/tracker/motion_model/bicycle_motion_model.hpp"
 #include "autoware/multi_object_tracker/tracker/object_model/object_model.hpp"
-#include "kalman_filter/kalman_filter.hpp"
 
 namespace autoware::multi_object_tracker
 {

perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/multiple_vehicle_tracker.hpp

@@ -19,10 +19,10 @@
 #ifndef AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MODEL__MULTIPLE_VEHICLE_TRACKER_HPP_
 #define AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MODEL__MULTIPLE_VEHICLE_TRACKER_HPP_
 
+#include "autoware/kalman_filter/kalman_filter.hpp"
 #include "autoware/multi_object_tracker/tracker/model/big_vehicle_tracker.hpp"
 #include "autoware/multi_object_tracker/tracker/model/normal_vehicle_tracker.hpp"
 #include "autoware/multi_object_tracker/tracker/model/tracker_base.hpp"
-#include "kalman_filter/kalman_filter.hpp"
 
 #include <rclcpp/time.hpp>