fix(costmap_generator): use vehicle frame for lidar height thresholds (…

…#9311) Signed-off-by: Maxime CLEMENT <[email protected]>
autowarefoundation · Nov 13, 2024 · e0002f3 · e0002f3
1 parent 96230fc
commit e0002f3
Show file tree

Hide file tree

Showing 4 changed files with 15 additions and 10 deletions.
diff --git a/planning/autoware_costmap_generator/README.md b/planning/autoware_costmap_generator/README.md
@@ -50,8 +50,8 @@ None
 | `grid_length_y`              | int    | size of gridmap for y direction                                                                |
 | `grid_position_x`            | int    | offset from coordinate in x direction                                                          |
 | `grid_position_y`            | int    | offset from coordinate in y direction                                                          |
-| `maximum_lidar_height_thres` | double | maximum height threshold for pointcloud data                                                   |
-| `minimum_lidar_height_thres` | double | minimum height threshold for pointcloud data                                                   |
+| `maximum_lidar_height_thres` | double | maximum height threshold for pointcloud data (relative to the vehicle_frame)                   |
+| `minimum_lidar_height_thres` | double | minimum height threshold for pointcloud data (relative to the vehicle_frame)                   |
 | `expand_rectangle_size`      | double | expand object's rectangle with this value                                                      |
 | `size_of_expansion_kernel`   | int    | kernel size for blurring effect on object's costmap                                            |
 

diff --git a/planning/autoware_costmap_generator/config/costmap_generator.param.yaml b/planning/autoware_costmap_generator/config/costmap_generator.param.yaml
@@ -12,8 +12,8 @@
     grid_length_y: 70.0
     grid_position_x: 0.0
     grid_position_y: 0.0
-    maximum_lidar_height_thres: 0.3
-    minimum_lidar_height_thres: -2.2
+    maximum_lidar_height_thres: 2.5  # [m] when use_points is true, ignore points with a z value above this (in the vehicle_frame)
+    minimum_lidar_height_thres: 0.0  # [m] when use_points is true, ignore points with a z value bellow this (in the vehicle_frame)
     use_wayarea: true
     use_parkinglot: true
     use_objects: true

diff --git a/planning/autoware_costmap_generator/include/autoware/costmap_generator/costmap_generator.hpp b/planning/autoware_costmap_generator/include/autoware/costmap_generator/costmap_generator.hpp
@@ -166,8 +166,9 @@ class CostmapGenerator : public rclcpp::Node
 
   /// \brief calculate cost from pointcloud data
   /// \param[in] in_points: subscribed pointcloud data
+  /// \param[in] vehicle_to_map_z: z value of the ego vehicle in the costmap frame
   grid_map::Matrix generatePointsCostmap(
-    const sensor_msgs::msg::PointCloud2::ConstSharedPtr & in_points);
+    const sensor_msgs::msg::PointCloud2::ConstSharedPtr & in_points, const double vehicle_to_map_z);
 
   /// \brief calculate cost from DynamicObjectArray
   /// \param[in] in_objects: subscribed DynamicObjectArray

diff --git a/planning/autoware_costmap_generator/src/costmap_generator.cpp b/planning/autoware_costmap_generator/src/costmap_generator.cpp
@@ -51,6 +51,7 @@
 #include <autoware_lanelet2_extension/utility/utilities.hpp>
 #include <autoware_lanelet2_extension/visualization/visualization.hpp>
 #include <pcl_ros/transforms.hpp>
+#include <rclcpp/logging.hpp>
 #include <tf2_eigen/tf2_eigen.hpp>
 
 #include <lanelet2_core/Forward.h>
@@ -303,7 +304,7 @@ void CostmapGenerator::onTimer()
 
   if (param_->use_points && points_) {
     autoware::universe_utils::ScopedTimeTrack st("generatePointsCostmap()", *time_keeper_);
-    costmap_[LayerName::points] = generatePointsCostmap(points_);
+    costmap_[LayerName::points] = generatePointsCostmap(points_, tf.transform.translation.z);
   }
 
   {
@@ -346,21 +347,24 @@ void CostmapGenerator::initGridmap()
 }
 
 grid_map::Matrix CostmapGenerator::generatePointsCostmap(
-  const sensor_msgs::msg::PointCloud2::ConstSharedPtr & in_points)
+  const sensor_msgs::msg::PointCloud2::ConstSharedPtr & in_points, const double vehicle_to_map_z)
 {
   geometry_msgs::msg::TransformStamped points2costmap;
   try {
     points2costmap = tf_buffer_.lookupTransform(
       param_->costmap_frame, in_points->header.frame_id, tf2::TimePointZero);
   } catch (const tf2::TransformException & ex) {
-    RCLCPP_ERROR(rclcpp::get_logger("costmap_generator"), "%s", ex.what());
+    RCLCPP_ERROR_THROTTLE(
+      rclcpp::get_logger("costmap_generator"), *get_clock(), 1000, "%s", ex.what());
   }
 
   const auto transformed_points = getTransformedPointCloud(*in_points, points2costmap.transform);
 
+  const auto maximum_height_thres = param_->maximum_lidar_height_thres + vehicle_to_map_z;
+  const auto minimum_height_thres = param_->minimum_lidar_height_thres + vehicle_to_map_z;
   grid_map::Matrix points_costmap = points2costmap_.makeCostmapFromPoints(
-    param_->maximum_lidar_height_thres, param_->minimum_lidar_height_thres, param_->grid_min_value,
-    param_->grid_max_value, costmap_, LayerName::points, transformed_points);
+    maximum_height_thres, minimum_height_thres, param_->grid_min_value, param_->grid_max_value,
+    costmap_, LayerName::points, transformed_points);
 
   return points_costmap;
 }