Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

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

Merged
merged 2 commits into from
Nov 13, 2024
Merged

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

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
4f216b3
fix(costmap_generator): use correct frame for lidar height thresholds
maxime-clem Nov 13, 2024
5adbdc2
throttle error logging for baselink->map tf not found
maxime-clem Nov 13, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
4 changes: 2 additions & 2 deletions planning/autoware_costmap_generator/README.md
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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 |

Expand Down
4 changes: 2 additions & 2 deletions planning/autoware_costmap_generator/config/costmap_generator.param.yaml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
3 changes: 2 additions & 1 deletion planning/autoware_costmap_generator/include/autoware/costmap_generator/costmap_generator.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
14 changes: 9 additions & 5 deletions planning/autoware_costmap_generator/src/costmap_generator.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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>
Expand Down Expand Up @@ -303,7 +304,7 @@

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);
}

{
Expand Down Expand Up @@ -346,21 +347,24 @@
}

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(

Check warning on line 357 in planning/autoware_costmap_generator/src/costmap_generator.cpp

View check run for this annotation

Codecov / codecov/patch

planning/autoware_costmap_generator/src/costmap_generator.cpp#L357 

Added line #L357 was not covered by tests
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;

Check warning on line 364 in planning/autoware_costmap_generator/src/costmap_generator.cpp

View check run for this annotation

Codecov / codecov/patch

planning/autoware_costmap_generator/src/costmap_generator.cpp#L363-L364 

Added lines #L363 - L364 were not covered by tests
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;
}
Expand Down
Loading