Merge pull request #802 from tier4/sync-upstream

chore: sync upstream

.cspell-partial.json

@@ -1,5 +1,9 @@
 {
-  "ignorePaths": ["**/perception/**", "**/sensing/**", "perception/bytetrack/lib/**"],
+  "ignorePaths": [
+    "**/perception/**",
+    "sensing/tier4_pcl_extensions/include/**",
+    "perception/bytetrack/lib/**"
+  ],
   "ignoreRegExpList": [],
   "words": []
 }

common/perception_utils/package.xml

@@ -4,7 +4,6 @@
   <name>perception_utils</name>
   <version>0.1.0</version>
   <description>The perception_utils package</description>
-  <maintainer email="[email protected]">Mingyu Li</maintainer>
   <maintainer email="[email protected]">Satoshi Tanaka</maintainer>
   <maintainer email="[email protected]">Yusuke Muramatsu</maintainer>
   <maintainer email="[email protected]">Shunsuke Miura</maintainer>

control/operation_mode_transition_manager/src/state.cpp

@@ -122,14 +122,24 @@ bool AutonomousMode::isModeChangeCompleted()
   const auto closest_point = trajectory_.points.at(*closest_idx);
 
   // check for lateral deviation
-  const auto dist_deviation = calcDistance2d(closest_point.pose, kinematics_.pose.pose);
+  const auto dist_deviation =
+    motion_utils::calcLateralOffset(trajectory_.points, kinematics_.pose.pose.position);
+  if (std::isnan(dist_deviation)) {
+    RCLCPP_INFO(logger_, "Not stable yet: lateral offset calculation failed.");
+    return unstable();
+  }
   if (dist_deviation > stable_check_param_.dist_threshold) {
     RCLCPP_INFO(logger_, "Not stable yet: distance deviation is too large: %f", dist_deviation);
     return unstable();
   }
 
   // check for yaw deviation
-  const auto yaw_deviation = calcYawDeviation(closest_point.pose, kinematics_.pose.pose);
+  const auto yaw_deviation =
+    motion_utils::calcYawDeviation(trajectory_.points, kinematics_.pose.pose);
+  if (std::isnan(yaw_deviation)) {
+    RCLCPP_INFO(logger_, "Not stable yet: lateral offset calculation failed.");
+    return unstable();
+  }
   if (yaw_deviation > stable_check_param_.yaw_threshold) {
     RCLCPP_INFO(logger_, "Not stable yet: yaw deviation is too large: %f", yaw_deviation);
     return unstable();

localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp

@@ -270,7 +270,8 @@ void NDTScanMatcher::timer_diagnostic()
     }
     if (
       state_ptr_->count("skipping_publish_num") &&
-      std::stoi((*state_ptr_)["skipping_publish_num"]) > 1) {
+      std::stoi((*state_ptr_)["skipping_publish_num"]) > 1 &&
+      std::stoi((*state_ptr_)["skipping_publish_num"]) < 5) {
       diag_status_msg.level = diagnostic_msgs::msg::DiagnosticStatus::WARN;
       diag_status_msg.message += "skipping_publish_num > 1. ";
     }

perception/crosswalk_traffic_light_estimator/include/crosswalk_traffic_light_estimator/node.hpp

@@ -19,7 +19,8 @@
 #include <lanelet2_extension/utility/query.hpp>
 #include <lanelet2_extension/utility/utilities.hpp>
 #include <rclcpp/rclcpp.hpp>
-#include <tier4_autoware_utils/tier4_autoware_utils.hpp>
+#include <tier4_autoware_utils/ros/debug_publisher.hpp>
+#include <tier4_autoware_utils/system/stop_watch.hpp>
 
 #include <autoware_auto_mapping_msgs/msg/had_map_bin.hpp>
 #include <autoware_perception_msgs/msg/traffic_signal_array.hpp>

perception/crosswalk_traffic_light_estimator/package.xml

@@ -5,6 +5,8 @@
   <description>The crosswalk_traffic_light_estimator package</description>
 
   <maintainer email="[email protected]">Satoshi Ota</maintainer>
+  <maintainer email="[email protected]">Shunsuke Miura</maintainer>
+  <maintainer email="[email protected]">Tao Zhong</maintainer>
   <license>Apache License 2.0</license>
 
   <buildtool_depend>ament_cmake_auto</buildtool_depend>

perception/detected_object_validation/include/obstacle_pointcloud_based_validator/debugger.hpp

@@ -16,7 +16,6 @@
 #define OBSTACLE_POINTCLOUD_BASED_VALIDATOR__DEBUGGER_HPP_
 
 #include <rclcpp/rclcpp.hpp>
-#include <tier4_autoware_utils/tier4_autoware_utils.hpp>
 
 #include <autoware_auto_perception_msgs/msg/detected_objects.hpp>
 #include <sensor_msgs/msg/point_cloud2.hpp>

perception/detected_object_validation/src/object_lanelet_filter.cpp

@@ -15,7 +15,7 @@
 #include "detected_object_filter/object_lanelet_filter.hpp"
 
 #include <object_recognition_utils/object_recognition_utils.hpp>
-#include <tier4_autoware_utils/tier4_autoware_utils.hpp>
+#include <tier4_autoware_utils/geometry/geometry.hpp>
 
 #include <boost/geometry/algorithms/convex_hull.hpp>
 #include <boost/geometry/algorithms/disjoint.hpp>

perception/detected_object_validation/src/object_position_filter.cpp

@@ -14,8 +14,6 @@
 
 #include "detected_object_filter/object_position_filter.hpp"
 
-#include <tier4_autoware_utils/tier4_autoware_utils.hpp>
-
 namespace object_position_filter
 {
 ObjectPositionFilterNode::ObjectPositionFilterNode(const rclcpp::NodeOptions & node_options)

perception/detected_object_validation/src/obstacle_pointcloud_based_validator.cpp

@@ -15,7 +15,7 @@
 #include "obstacle_pointcloud_based_validator/obstacle_pointcloud_based_validator.hpp"
 
 #include <object_recognition_utils/object_recognition_utils.hpp>
-#include <tier4_autoware_utils/tier4_autoware_utils.hpp>
+#include <tier4_autoware_utils/geometry/boost_polygon_utils.hpp>
 
 #include <boost/geometry.hpp>
 

perception/detected_object_validation/src/occupancy_grid_based_validator.cpp

@@ -16,7 +16,7 @@
 
 #include <object_recognition_utils/object_classification.hpp>
 #include <object_recognition_utils/object_recognition_utils.hpp>
-#include <tier4_autoware_utils/tier4_autoware_utils.hpp>
+#include <tier4_autoware_utils/geometry/boost_polygon_utils.hpp>
 
 #include <boost/optional.hpp>
 

perception/detection_by_tracker/include/detection_by_tracker/debugger.hpp

@@ -20,7 +20,6 @@
 #include <euclidean_cluster/voxel_grid_based_euclidean_cluster.hpp>
 #include <rclcpp/rclcpp.hpp>
 #include <shape_estimation/shape_estimator.hpp>
-#include <tier4_autoware_utils/tier4_autoware_utils.hpp>
 
 #include <autoware_auto_perception_msgs/msg/detected_objects.hpp>
 #include <autoware_auto_perception_msgs/msg/tracked_objects.hpp>

perception/detection_by_tracker/include/detection_by_tracker/detection_by_tracker_core.hpp

@@ -22,7 +22,6 @@
 #include <euclidean_cluster/voxel_grid_based_euclidean_cluster.hpp>
 #include <rclcpp/rclcpp.hpp>
 #include <shape_estimation/shape_estimator.hpp>
-#include <tier4_autoware_utils/tier4_autoware_utils.hpp>
 
 #include <autoware_auto_perception_msgs/msg/detected_objects.hpp>
 #include <autoware_auto_perception_msgs/msg/tracked_objects.hpp>

perception/detection_by_tracker/src/detection_by_tracker_core.cpp

@@ -16,6 +16,9 @@
 
 #include "object_recognition_utils/object_recognition_utils.hpp"
 
+#include <tier4_autoware_utils/geometry/geometry.hpp>
+#include <tier4_autoware_utils/math/unit_conversion.hpp>
+
 #include <chrono>
 #include <memory>
 #include <optional>

perception/front_vehicle_velocity_estimator/include/front_vehicle_velocity_estimator/front_vehicle_velocity_estimator.hpp

@@ -19,7 +19,7 @@
 #include "pcl/point_types.h"
 #include "pcl_conversions/pcl_conversions.h"
 #include "rclcpp/logger.hpp"
-#include "tier4_autoware_utils/tier4_autoware_utils.hpp"
+#include "tier4_autoware_utils/geometry/boost_geometry.hpp"
 
 #include "autoware_auto_perception_msgs/msg/detected_objects.hpp"
 #include "nav_msgs/msg/odometry.hpp"

perception/front_vehicle_velocity_estimator/src/front_vehicle_velocity_estimator_node/front_vehicle_velocity_estimator.cpp

@@ -14,6 +14,8 @@
 
 #include "front_vehicle_velocity_estimator/front_vehicle_velocity_estimator.hpp"
 
+#include "tier4_autoware_utils/geometry/geometry.hpp"
+
 #include <boost/geometry.hpp>
 
 #include <memory>

perception/heatmap_visualizer/src/utils.cpp

@@ -15,7 +15,8 @@
 #include "heatmap_visualizer/utils.hpp"
 
 #include <rclcpp/rclcpp.hpp>
-#include <tier4_autoware_utils/tier4_autoware_utils.hpp>
+#include <tier4_autoware_utils/math/normalization.hpp>
+#include <tier4_autoware_utils/math/unit_conversion.hpp>
 
 #include <tf2/utils.h>
 

perception/lidar_centerpoint/lib/postprocess/non_maximum_suppression.cpp

@@ -16,8 +16,7 @@
 
 #include "object_recognition_utils/geometry.hpp"
 #include "object_recognition_utils/object_recognition_utils.hpp"
-#include "tier4_autoware_utils/tier4_autoware_utils.hpp"
-
+#include "tier4_autoware_utils/geometry/geometry.hpp"
 namespace centerpoint
 {
 

perception/multi_object_tracker/include/multi_object_tracker/utils/utils.hpp

@@ -21,14 +21,16 @@
 
 #include <Eigen/Core>
 #include <Eigen/Geometry>
-#include <tier4_autoware_utils/tier4_autoware_utils.hpp>
 
 #include <autoware_auto_perception_msgs/msg/detected_object.hpp>
 #include <autoware_auto_perception_msgs/msg/shape.hpp>
 #include <autoware_auto_perception_msgs/msg/tracked_object.hpp>
 #include <geometry_msgs/msg/polygon.hpp>
+#include <geometry_msgs/msg/transform.hpp>
 #include <geometry_msgs/msg/vector3.hpp>
 
+#include <tf2/utils.h>
+
 #include <algorithm>
 #include <cmath>
 #include <tuple>

perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp

@@ -20,7 +20,9 @@
 
 #include "multi_object_tracker/utils/utils.hpp"
 
-#include <tier4_autoware_utils/tier4_autoware_utils.hpp>
+#include <tier4_autoware_utils/geometry/boost_polygon_utils.hpp>
+#include <tier4_autoware_utils/math/normalization.hpp>
+#include <tier4_autoware_utils/math/unit_conversion.hpp>
 
 #include <bits/stdc++.h>
 #include <tf2/LinearMath/Matrix3x3.h>

perception/multi_object_tracker/src/tracker/model/big_vehicle_tracker.cpp

@@ -34,7 +34,9 @@
 
 #include <Eigen/Core>
 #include <Eigen/Geometry>
-#include <tier4_autoware_utils/tier4_autoware_utils.hpp>
+#include <tier4_autoware_utils/geometry/boost_polygon_utils.hpp>
+#include <tier4_autoware_utils/math/normalization.hpp>
+#include <tier4_autoware_utils/math/unit_conversion.hpp>
 
 using Label = autoware_auto_perception_msgs::msg::ObjectClassification;
 

perception/multi_object_tracker/src/tracker/model/multiple_vehicle_tracker.cpp

@@ -18,8 +18,6 @@
 
 #include "multi_object_tracker/tracker/model/multiple_vehicle_tracker.hpp"
 
-#include <tier4_autoware_utils/tier4_autoware_utils.hpp>
-
 using Label = autoware_auto_perception_msgs::msg::ObjectClassification;
 
 MultipleVehicleTracker::MultipleVehicleTracker(

perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp

@@ -34,7 +34,9 @@
 
 #include <Eigen/Core>
 #include <Eigen/Geometry>
-#include <tier4_autoware_utils/tier4_autoware_utils.hpp>
+#include <tier4_autoware_utils/geometry/boost_polygon_utils.hpp>
+#include <tier4_autoware_utils/math/normalization.hpp>
+#include <tier4_autoware_utils/math/unit_conversion.hpp>
 
 using Label = autoware_auto_perception_msgs::msg::ObjectClassification;
 

perception/multi_object_tracker/src/tracker/model/pass_through_tracker.cpp

@@ -34,7 +34,6 @@
 
 #include <Eigen/Core>
 #include <Eigen/Geometry>
-#include <tier4_autoware_utils/tier4_autoware_utils.hpp>
 
 PassThroughTracker::PassThroughTracker(
   const rclcpp::Time & time, const autoware_auto_perception_msgs::msg::DetectedObject & object,

perception/multi_object_tracker/src/tracker/model/pedestrian_and_bicycle_tracker.cpp

@@ -18,8 +18,6 @@
 
 #include "multi_object_tracker/tracker/model/pedestrian_and_bicycle_tracker.hpp"
 
-#include <tier4_autoware_utils/tier4_autoware_utils.hpp>
-
 using Label = autoware_auto_perception_msgs::msg::ObjectClassification;
 
 PedestrianAndBicycleTracker::PedestrianAndBicycleTracker(

perception/multi_object_tracker/src/tracker/model/pedestrian_tracker.cpp

@@ -21,7 +21,9 @@
 #include "multi_object_tracker/utils/utils.hpp"
 #include "object_recognition_utils/object_recognition_utils.hpp"
 
-#include <tier4_autoware_utils/tier4_autoware_utils.hpp>
+#include <tier4_autoware_utils/geometry/boost_polygon_utils.hpp>
+#include <tier4_autoware_utils/math/normalization.hpp>
+#include <tier4_autoware_utils/math/unit_conversion.hpp>
 
 #include <bits/stdc++.h>
 #include <tf2/LinearMath/Matrix3x3.h>

perception/multi_object_tracker/src/tracker/model/unknown_tracker.cpp

@@ -30,7 +30,9 @@
 
 #include <Eigen/Core>
 #include <Eigen/Geometry>
-#include <tier4_autoware_utils/tier4_autoware_utils.hpp>
+#include <tier4_autoware_utils/geometry/boost_polygon_utils.hpp>
+#include <tier4_autoware_utils/math/normalization.hpp>
+#include <tier4_autoware_utils/math/unit_conversion.hpp>
 
 UnknownTracker::UnknownTracker(
   const rclcpp::Time & time, const autoware_auto_perception_msgs::msg::DetectedObject & object,

perception/object_merger/include/object_association_merger/utils/utils.hpp

@@ -19,8 +19,6 @@
 #ifndef OBJECT_ASSOCIATION_MERGER__UTILS__UTILS_HPP_
 #define OBJECT_ASSOCIATION_MERGER__UTILS__UTILS_HPP_
 
-#include <tier4_autoware_utils/tier4_autoware_utils.hpp>
-
 #include <autoware_auto_perception_msgs/msg/detected_object.hpp>
 #include <autoware_auto_perception_msgs/msg/shape.hpp>
 #include <geometry_msgs/msg/polygon.hpp>

perception/object_merger/src/object_association_merger/node.cpp

@@ -16,7 +16,7 @@
 
 #include "object_association_merger/utils/utils.hpp"
 #include "object_recognition_utils/object_recognition_utils.hpp"
-#include "tier4_autoware_utils/tier4_autoware_utils.hpp"
+#include "tier4_autoware_utils/geometry/geometry.hpp"
 
 #include <boost/optional.hpp>
 

perception/object_velocity_splitter/src/object_velocity_splitter_node/object_velocity_splitter_node.cpp

@@ -14,7 +14,7 @@
 
 #include "object_velocity_splitter/object_velocity_splitter_node.hpp"
 
-#include "tier4_autoware_utils/tier4_autoware_utils.hpp"
+#include "tier4_autoware_utils/geometry/geometry.hpp"
 
 #include <memory>
 #include <string>

perception/occupancy_grid_map_outlier_filter/src/occupancy_grid_map_outlier_filter_nodelet.cpp

@@ -15,7 +15,9 @@
 #include "occupancy_grid_map_outlier_filter/occupancy_grid_map_outlier_filter_nodelet.hpp"
 
 #include <pcl_ros/transforms.hpp>
-#include <tier4_autoware_utils/tier4_autoware_utils.hpp>
+#include <tier4_autoware_utils/geometry/geometry.hpp>
+#include <tier4_autoware_utils/ros/debug_publisher.hpp>
+#include <tier4_autoware_utils/system/stop_watch.hpp>
 
 #include <boost/optional.hpp>
 

perception/probabilistic_occupancy_grid_map/include/utils/utils.hpp

@@ -18,7 +18,6 @@
 #include <builtin_interfaces/msg/time.hpp>
 #include <pcl_ros/transforms.hpp>
 #include <rclcpp/rclcpp.hpp>
-#include <tier4_autoware_utils/tier4_autoware_utils.hpp>
 
 #include <geometry_msgs/msg/pose.hpp>
 #include <geometry_msgs/msg/transform_stamped.hpp>

perception/probabilistic_occupancy_grid_map/src/laserscan_based_occupancy_grid_map/laserscan_based_occupancy_grid_map_node.cpp

@@ -18,7 +18,6 @@
 #include "utils/utils.hpp"
 
 #include <pcl_ros/transforms.hpp>
-#include <tier4_autoware_utils/tier4_autoware_utils.hpp>
 
 #include <nav_msgs/msg/occupancy_grid.hpp>
 

perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_base.cpp

@@ -56,7 +56,6 @@
 
 #include <grid_map_costmap_2d/grid_map_costmap_2d.hpp>
 #include <pcl_ros/transforms.hpp>
-#include <tier4_autoware_utils/tier4_autoware_utils.hpp>
 
 #include <sensor_msgs/point_cloud2_iterator.hpp>
 

perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_fixed.cpp

@@ -19,7 +19,7 @@
 
 #include <grid_map_costmap_2d/grid_map_costmap_2d.hpp>
 #include <pcl_ros/transforms.hpp>
-#include <tier4_autoware_utils/tier4_autoware_utils.hpp>
+#include <tier4_autoware_utils/math/unit_conversion.hpp>
 
 #include <sensor_msgs/point_cloud2_iterator.hpp>
 

perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_projective.cpp

@@ -20,7 +20,7 @@
 #include <grid_map_costmap_2d/grid_map_costmap_2d.hpp>
 #include <grid_map_ros/grid_map_ros.hpp>
 #include <pcl_ros/transforms.hpp>
-#include <tier4_autoware_utils/tier4_autoware_utils.hpp>
+#include <tier4_autoware_utils/math/unit_conversion.hpp>
 
 #include <sensor_msgs/point_cloud2_iterator.hpp>
 

perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/pointcloud_based_occupancy_grid_map_node.cpp

@@ -20,7 +20,8 @@
 #include "utils/utils.hpp"
 
 #include <pcl_ros/transforms.hpp>
-#include <tier4_autoware_utils/tier4_autoware_utils.hpp>
+#include <tier4_autoware_utils/ros/debug_publisher.hpp>
+#include <tier4_autoware_utils/system/stop_watch.hpp>
 
 #include <nav_msgs/msg/occupancy_grid.hpp>
 

perception/probabilistic_occupancy_grid_map/src/utils/utils.cpp

@@ -14,6 +14,8 @@
 
 #include "utils/utils.hpp"
 
+#include <tier4_autoware_utils/geometry/geometry.hpp>
+
 #include <string>
 
 namespace utils

perception/radar_fusion_to_detected_object/include/radar_fusion_to_detected_object.hpp

@@ -16,8 +16,7 @@
 #define RADAR_FUSION_TO_DETECTED_OBJECT_HPP_
 
 #include "rclcpp/logger.hpp"
-#include "tier4_autoware_utils/tier4_autoware_utils.hpp"
-
+#include "tier4_autoware_utils/geometry/boost_geometry.hpp"
 #define EIGEN_MPL2_ONLY
 #include <Eigen/Core>
 #include <Eigen/Geometry>