From 458dbe5c5445fe95a62d9a4804509acbc024d703 Mon Sep 17 00:00:00 2001 From: Esteve Fernandez <33620+esteve@users.noreply.github.com> Date: Tue, 9 Jul 2024 07:45:46 +0200 Subject: [PATCH 1/8] chore(autoware_behavior_velocity_planner): remove no_prefix function from tests (#7589) Signed-off-by: Esteve Fernandez --- .../test/src/test_node_interface.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/test/src/test_node_interface.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/test/src/test_node_interface.cpp index 3482d7be8ec48..cc2f4bf3b96b4 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/test/src/test_node_interface.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/test/src/test_node_interface.cpp @@ -54,7 +54,6 @@ std::shared_ptr generateNode() const auto velocity_smoother_dir = ament_index_cpp::get_package_share_directory("autoware_velocity_smoother"); - // TODO(esteve): delete when all the modules are migrated to autoware_behavior_velocity_* const auto get_behavior_velocity_module_config = [](const std::string & module) { const auto package_name = "autoware_behavior_velocity_" + module + "_module"; const auto package_path = ament_index_cpp::get_package_share_directory(package_name); From f716b5b0ac4678d5d75ae06936d66e340292930d Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Tue, 9 Jul 2024 15:02:25 +0900 Subject: [PATCH 2/8] fix(euclidean_cluster): include autoware_universe_utils headers (#7908) fix: include autoware_universe_utils headers instead of compare_map_segmentation package Signed-off-by: Taekjin LEE --- perception/euclidean_cluster/package.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/perception/euclidean_cluster/package.xml b/perception/euclidean_cluster/package.xml index c5cdd9c54b988..2644786ead061 100644 --- a/perception/euclidean_cluster/package.xml +++ b/perception/euclidean_cluster/package.xml @@ -12,7 +12,7 @@ autoware_cmake autoware_perception_msgs - compare_map_segmentation + autoware_universe_utils geometry_msgs libpcl-all-dev pcl_conversions From d2f2fd9c1ab4e505758646bfce3d48ffc8192fb9 Mon Sep 17 00:00:00 2001 From: danielsanchezaran Date: Tue, 9 Jul 2024 18:46:05 +0900 Subject: [PATCH 3/8] feat(autonomous_emergency_braking): add virtual stop wall to aeb (#7894) * add virtual stop wall to aeb Signed-off-by: Daniel Sanchez * add maintainer Signed-off-by: Daniel Sanchez * add uppercase Signed-off-by: Daniel Sanchez * use motion utils function instead of shiftPose Signed-off-by: Daniel Sanchez --------- Signed-off-by: Daniel Sanchez --- .github/CODEOWNERS | 2 +- .../README.md | 1 + .../autonomous_emergency_braking.param.yaml | 1 + .../autonomous_emergency_braking/node.hpp | 1 + .../autonomous_emergency_braking/utils.hpp | 2 - .../package.xml | 1 + .../src/node.cpp | 38 ++++++++++++++++++- 7 files changed, 41 insertions(+), 5 deletions(-) diff --git a/.github/CODEOWNERS b/.github/CODEOWNERS index 0fe324f84f81a..ed496d7b7ba74 100644 --- a/.github/CODEOWNERS +++ b/.github/CODEOWNERS @@ -45,7 +45,7 @@ common/time_utils/** christopherj.ho@gmail.com shumpei.wakabayashi@tier4.jp tomo common/traffic_light_recognition_marker_publisher/** shumpei.wakabayashi@tier4.jp takeshi.miura@tier4.jp tomoya.kimura@tier4.jp common/traffic_light_utils/** kotaro.uetake@tier4.jp satoshi.ota@tier4.jp shunsuke.miura@tier4.jp common/tvm_utility/** ambroise.vincent@arm.com xinyu.wang@tier4.jp -control/autoware_autonomous_emergency_braking/** daniel.sanchez@tier4.jp mamoru.sobue@tier4.jp takamasa.horibe@tier4.jp tomoya.kimura@tier4.jp +control/autoware_autonomous_emergency_braking/** daniel.sanchez@tier4.jp mamoru.sobue@tier4.jp takamasa.horibe@tier4.jp tomoya.kimura@tier4.jp kyoichi.sugahara@tier4.jp control/autoware_control_validator/** kyoichi.sugahara@tier4.jp makoto.kurihara@tier4.jp mamoru.sobue@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp control/autoware_external_cmd_selector/** fumiya.watanabe@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp taiki.tanaka@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp control/autoware_joy_controller/** fumiya.watanabe@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp taiki.tanaka@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp diff --git a/control/autoware_autonomous_emergency_braking/README.md b/control/autoware_autonomous_emergency_braking/README.md index f040235f3f052..db27ca1b4f699 100644 --- a/control/autoware_autonomous_emergency_braking/README.md +++ b/control/autoware_autonomous_emergency_braking/README.md @@ -192,6 +192,7 @@ When vehicle odometry information is faulty, it is possible that the MPC fails t | Name | Unit | Type | Description | Default value | | :-------------------------------- | :----- | :----- | :---------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | :------------ | +| publish_debug_markers | [-] | bool | flag to publish debug markers | true | | publish_debug_pointcloud | [-] | bool | flag to publish the point cloud used for debugging | false | | use_predicted_trajectory | [-] | bool | flag to use the predicted path from the control module | true | | use_imu_path | [-] | bool | flag to use the predicted path generated by sensor data | true | diff --git a/control/autoware_autonomous_emergency_braking/config/autonomous_emergency_braking.param.yaml b/control/autoware_autonomous_emergency_braking/config/autonomous_emergency_braking.param.yaml index 482ddc50766f8..25db9448f758b 100644 --- a/control/autoware_autonomous_emergency_braking/config/autonomous_emergency_braking.param.yaml +++ b/control/autoware_autonomous_emergency_braking/config/autonomous_emergency_braking.param.yaml @@ -14,6 +14,7 @@ # Debug publish_debug_pointcloud: false + publish_debug_markers: true # Point cloud partitioning detection_range_min_height: 0.0 diff --git a/control/autoware_autonomous_emergency_braking/include/autoware/autonomous_emergency_braking/node.hpp b/control/autoware_autonomous_emergency_braking/include/autoware/autonomous_emergency_braking/node.hpp index f6df46440d841..9fd6d0192439c 100644 --- a/control/autoware_autonomous_emergency_braking/include/autoware/autonomous_emergency_braking/node.hpp +++ b/control/autoware_autonomous_emergency_braking/include/autoware/autonomous_emergency_braking/node.hpp @@ -329,6 +329,7 @@ class AEB : public rclcpp::Node // member variables bool publish_debug_pointcloud_; + bool publish_debug_markers_; bool use_predicted_trajectory_; bool use_imu_path_; bool use_pointcloud_data_; diff --git a/control/autoware_autonomous_emergency_braking/include/autoware/autonomous_emergency_braking/utils.hpp b/control/autoware_autonomous_emergency_braking/include/autoware/autonomous_emergency_braking/utils.hpp index 4a6bc0ce2c11f..d2ec1763ca4dd 100644 --- a/control/autoware_autonomous_emergency_braking/include/autoware/autonomous_emergency_braking/utils.hpp +++ b/control/autoware_autonomous_emergency_braking/include/autoware/autonomous_emergency_braking/utils.hpp @@ -33,8 +33,6 @@ #include #endif -#include - namespace autoware::motion::control::autonomous_emergency_braking::utils { using autoware::universe_utils::Polygon2d; diff --git a/control/autoware_autonomous_emergency_braking/package.xml b/control/autoware_autonomous_emergency_braking/package.xml index c822adb510805..0c64b9498486a 100644 --- a/control/autoware_autonomous_emergency_braking/package.xml +++ b/control/autoware_autonomous_emergency_braking/package.xml @@ -8,6 +8,7 @@ Tomoya Kimura Mamoru Sobue Daniel Sanchez + Kyoichi Sugahara Apache License 2.0 diff --git a/control/autoware_autonomous_emergency_braking/src/node.cpp b/control/autoware_autonomous_emergency_braking/src/node.cpp index fb0fc2d043cc5..69bedd02d201b 100644 --- a/control/autoware_autonomous_emergency_braking/src/node.cpp +++ b/control/autoware_autonomous_emergency_braking/src/node.cpp @@ -14,6 +14,7 @@ #include #include +#include #include #include #include @@ -41,7 +42,9 @@ #include #include +#include #include +#include #ifdef ROS_DISTRO_GALACTIC #include #include @@ -131,6 +134,7 @@ AEB::AEB(const rclcpp::NodeOptions & node_options) } // parameter publish_debug_pointcloud_ = declare_parameter("publish_debug_pointcloud"); + publish_debug_markers_ = declare_parameter("publish_debug_markers"); use_predicted_trajectory_ = declare_parameter("use_predicted_trajectory"); use_imu_path_ = declare_parameter("use_imu_path"); use_pointcloud_data_ = declare_parameter("use_pointcloud_data"); @@ -182,6 +186,7 @@ rcl_interfaces::msg::SetParametersResult AEB::onParameter( { using autoware::universe_utils::updateParam; updateParam(parameters, "publish_debug_pointcloud", publish_debug_pointcloud_); + updateParam(parameters, "publish_debug_markers", publish_debug_markers_); updateParam(parameters, "use_predicted_trajectory", use_predicted_trajectory_); updateParam(parameters, "use_imu_path", use_imu_path_); updateParam(parameters, "use_pointcloud_data", use_pointcloud_data_); @@ -378,7 +383,9 @@ void AEB::onCheckCollision(DiagnosticStatusWrapper & stat) stat.addf("RSS", "%.2f", data.rss); stat.addf("Distance", "%.2f", data.distance_to_object); stat.addf("Object Speed", "%.2f", data.velocity); - addCollisionMarker(data, debug_markers); + if (publish_debug_markers_) { + addCollisionMarker(data, debug_markers); + } } else { const std::string error_msg = "[AEB]: No Collision"; const auto diag_level = DiagnosticStatus::OK; @@ -455,7 +462,7 @@ bool AEB::checkCollision(MarkerArray & debug_markers) }); // Add debug markers - { + if (publish_debug_markers_) { const auto [color_r, color_g, color_b, color_a] = debug_colors; addMarker( this->get_clock()->now(), path, ego_polys, objects, closest_object_point, color_r, color_g, @@ -896,6 +903,33 @@ void AEB::addCollisionMarker(const ObjectData & data, MarkerArray & debug_marker autoware::universe_utils::createMarkerColor(1.0, 0.0, 0.0, 0.3)); point_marker.pose.position = data.position; debug_markers.markers.push_back(point_marker); + + const auto ego_map_pose = std::invoke([this]() -> std::optional { + geometry_msgs::msg::TransformStamped tf_current_pose; + geometry_msgs::msg::Pose p; + try { + tf_current_pose = tf_buffer_.lookupTransform( + "map", "base_link", rclcpp::Time(0), rclcpp::Duration::from_seconds(1.0)); + } catch (tf2::TransformException & ex) { + RCLCPP_ERROR(get_logger(), "%s", ex.what()); + return std::nullopt; + } + + p.orientation = tf_current_pose.transform.rotation; + p.position.x = tf_current_pose.transform.translation.x; + p.position.y = tf_current_pose.transform.translation.y; + p.position.z = tf_current_pose.transform.translation.z; + return std::make_optional(p); + }); + + if (ego_map_pose.has_value()) { + const double base_to_front_offset = vehicle_info_.max_longitudinal_offset_m; + const auto ego_front_pose = autoware::universe_utils::calcOffsetPose( + ego_map_pose.value(), base_to_front_offset, 0.0, 0.0, 0.0); + const auto virtual_stop_wall = autoware::motion_utils::createStopVirtualWallMarker( + ego_front_pose, "autonomous_emergency_braking", this->now(), 0); + autoware::universe_utils::appendMarkerArray(virtual_stop_wall, &debug_markers); + } } } // namespace autoware::motion::control::autonomous_emergency_braking From 48b7cee5f5e3fc3d07294d529758867231d113b0 Mon Sep 17 00:00:00 2001 From: Keisuke Shima <19993104+KeisukeShima@users.noreply.github.com> Date: Tue, 9 Jul 2024 18:47:11 +0900 Subject: [PATCH 4/8] docs(goal_planner): update parameter description (#7889) * docs(goal_planner): update parameter description Signed-off-by: Keisuke Shima <19993104+KeisukeShima@users.noreply.github.com> * style(pre-commit): autofix Signed-off-by: Keisuke Shima <19993104+KeisukeShima@users.noreply.github.com> --------- Signed-off-by: Keisuke Shima <19993104+KeisukeShima@users.noreply.github.com> Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- .../README.md | 27 +++++++++---------- 1 file changed, 13 insertions(+), 14 deletions(-) diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/README.md b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/README.md index 3e155aba0af2e..014aaf6db6657 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/README.md +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/README.md @@ -173,20 +173,19 @@ The gray numbers represent objects to avoid, and you can see that the goal in fr ### Parameters for goal search -| Name | Unit | Type | Description | Default value | -| :------------------------------ | :--- | :----- | :------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------ | :-------------------------- | -| goal_priority | [-] | string | In case `minimum_longitudinal_distance`, sort with smaller longitudinal distances taking precedence over smaller lateral distances. In case `minimum_weighted_distance`, sort with the sum of weighted lateral distance and longitudinal distance | `minimum_weighted_distance` | -| lateral_weight | [-] | double | Weight for lateral distance used when `minimum_weighted_distance` | 40.0 | -| prioritize_goals_before_objects | [-] | bool | If there are objects that may need to be avoided, prioritize the goal in front of them | true | -| forward_goal_search_length | [m] | double | length of forward range to be explored from the original goal | 20.0 | -| backward_goal_search_length | [m] | double | length of backward range to be explored from the original goal | 20.0 | -| goal_search_interval | [m] | double | distance interval for goal search | 2.0 | -| longitudinal_margin | [m] | double | margin between ego-vehicle at the goal position and obstacles | 3.0 | -| max_lateral_offset | [m] | double | maximum offset of goal search in the lateral direction | 0.5 | -| lateral_offset_interval | [m] | double | distance interval of goal search in the lateral direction | 0.25 | -| ignore_distance_from_lane_start | [m] | double | distance from start of pull over lanes for ignoring goal candidates | 0.0 | -| ignore_distance_from_lane_start | [m] | double | distance from start of pull over lanes for ignoring goal candidates | 0.0 | -| margin_from_boundary | [m] | double | distance margin from edge of the shoulder lane | 0.5 | +| Name | Unit | Type | Description | Default value | +| :------------------------------ | :--- | :----- | :------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | :-------------------------- | +| goal_priority | [-] | string | In case `minimum_longitudinal_distance`, sort with smaller longitudinal distances taking precedence over smaller lateral distances. In case `minimum_weighted_distance`, sort with the sum of weighted lateral distance and longitudinal distance | `minimum_weighted_distance` | +| lateral_weight | [-] | double | Weight for lateral distance used when `minimum_weighted_distance` | 40.0 | +| prioritize_goals_before_objects | [-] | bool | If there are objects that may need to be avoided, prioritize the goal in front of them | true | +| forward_goal_search_length | [m] | double | length of forward range to be explored from the original goal | 20.0 | +| backward_goal_search_length | [m] | double | length of backward range to be explored from the original goal | 20.0 | +| goal_search_interval | [m] | double | distance interval for goal search | 2.0 | +| longitudinal_margin | [m] | double | margin between ego-vehicle at the goal position and obstacles | 3.0 | +| max_lateral_offset | [m] | double | maximum offset of goal search in the lateral direction | 0.5 | +| lateral_offset_interval | [m] | double | distance interval of goal search in the lateral direction | 0.25 | +| ignore_distance_from_lane_start | [m] | double | This parameter ensures that the distance between the start of the shoulder lane and the goal is not less than the specified value. It's used to prevent setting goals too close to the beginning of the shoulder lane, which might lead to unsafe or impractical pull-over maneuvers. Increasing this value will force the system to ignore potential goal positions near the start of the shoulder lane, potentially leading to safer and more comfortable pull-over locations. | 0.0 | +| margin_from_boundary | [m] | double | distance margin from edge of the shoulder lane | 0.5 | ## **Pull Over** From f386fa8e611ec685c1703bed89e45a5d525c2282 Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Tue, 9 Jul 2024 19:07:17 +0900 Subject: [PATCH 5/8] fix(start/goal_planner): fix addition of duplicate segments in calcBeforeShiftedArcLength (#7902) * fix(start/goal_planner): fix addition of duplicate segments in calcBeforeShiftedArcLength Signed-off-by: kosuke55 * Update trajectory.hpp Co-authored-by: Kyoichi Sugahara * Update trajectory.hpp Co-authored-by: Kyoichi Sugahara --------- Signed-off-by: kosuke55 Co-authored-by: Kyoichi Sugahara --- .../motion_utils/trajectory/trajectory.hpp | 49 +++++++++++++------ .../src/trajectory/trajectory.cpp | 12 ++--- .../src/shift_pull_over.cpp | 15 +++++- .../src/shift_pull_out.cpp | 13 ++++- 4 files changed, 63 insertions(+), 26 deletions(-) diff --git a/common/autoware_motion_utils/include/autoware/motion_utils/trajectory/trajectory.hpp b/common/autoware_motion_utils/include/autoware/motion_utils/trajectory/trajectory.hpp index da37d04550f5e..126e66772920a 100644 --- a/common/autoware_motion_utils/include/autoware/motion_utils/trajectory/trajectory.hpp +++ b/common/autoware_motion_utils/include/autoware/motion_utils/trajectory/trajectory.hpp @@ -991,34 +991,51 @@ calcCurvature>( * curvature calculation */ template -std::vector> calcCurvatureAndArcLength(const T & points) +std::vector>> calcCurvatureAndSegmentLength( + const T & points) { - // Note that arclength is for the segment, not the sum. - std::vector> curvature_arc_length_vec; - curvature_arc_length_vec.emplace_back(0.0, 0.0); + // segment length is pair of segment length between {p1, p2} and {p2, p3} + std::vector>> curvature_and_segment_length_vec; + curvature_and_segment_length_vec.reserve(points.size()); + curvature_and_segment_length_vec.emplace_back(0.0, std::make_pair(0.0, 0.0)); for (size_t i = 1; i < points.size() - 1; ++i) { const auto p1 = autoware::universe_utils::getPoint(points.at(i - 1)); const auto p2 = autoware::universe_utils::getPoint(points.at(i)); const auto p3 = autoware::universe_utils::getPoint(points.at(i + 1)); const double curvature = autoware::universe_utils::calcCurvature(p1, p2, p3); - const double arc_length = - autoware::universe_utils::calcDistance2d(points.at(i - 1), points.at(i)) + - autoware::universe_utils::calcDistance2d(points.at(i), points.at(i + 1)); - curvature_arc_length_vec.emplace_back(curvature, arc_length); + + // The first point has only the next point, so put the distance to that point. + // In other words, assign the first segment length at the second point to the + // second_segment_length at the first point. + if (i == 1) { + curvature_and_segment_length_vec.at(0).second.second = + autoware::universe_utils::calcDistance2d(p1, p2); + } + + // The second_segment_length of the previous point and the first segment length of the current + // point are equal. + const std::pair arc_length{ + curvature_and_segment_length_vec.back().second.second, + autoware::universe_utils::calcDistance2d(p2, p3)}; + + curvature_and_segment_length_vec.emplace_back(curvature, arc_length); } - curvature_arc_length_vec.emplace_back(0.0, 0.0); - return curvature_arc_length_vec; + // set to the last point + curvature_and_segment_length_vec.emplace_back( + 0.0, std::make_pair(curvature_and_segment_length_vec.back().second.second, 0.0)); + + return curvature_and_segment_length_vec; } -extern template std::vector> -calcCurvatureAndArcLength>( +extern template std::vector>> +calcCurvatureAndSegmentLength>( const std::vector & points); -extern template std::vector> -calcCurvatureAndArcLength>( +extern template std::vector>> +calcCurvatureAndSegmentLength>( const std::vector & points); -extern template std::vector> -calcCurvatureAndArcLength>( +extern template std::vector>> +calcCurvatureAndSegmentLength>( const std::vector & points); /** diff --git a/common/autoware_motion_utils/src/trajectory/trajectory.cpp b/common/autoware_motion_utils/src/trajectory/trajectory.cpp index 5d536c0772fea..ce26952c3d634 100644 --- a/common/autoware_motion_utils/src/trajectory/trajectory.cpp +++ b/common/autoware_motion_utils/src/trajectory/trajectory.cpp @@ -238,14 +238,14 @@ calcCurvature>( const std::vector & points); // -template std::vector> -calcCurvatureAndArcLength>( +template std::vector>> +calcCurvatureAndSegmentLength>( const std::vector & points); -template std::vector> -calcCurvatureAndArcLength>( +template std::vector>> +calcCurvatureAndSegmentLength>( const std::vector & points); -template std::vector> -calcCurvatureAndArcLength>( +template std::vector>> +calcCurvatureAndSegmentLength>( const std::vector & points); // diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/shift_pull_over.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/shift_pull_over.cpp index 3d2bf9500bc78..9570621f0f717 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/shift_pull_over.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/shift_pull_over.cpp @@ -308,8 +308,19 @@ double ShiftPullOver::calcBeforeShiftedArcLength( double before_arc_length{0.0}; double after_arc_length{0.0}; - for (const auto & [k, segment_length] : - autoware::motion_utils::calcCurvatureAndArcLength(reversed_path.points)) { + + const auto curvature_and_segment_length = + autoware::motion_utils::calcCurvatureAndSegmentLength(reversed_path.points); + + for (size_t i = 0; i < curvature_and_segment_length.size(); ++i) { + const auto & [k, segment_length_pair] = curvature_and_segment_length[i]; + + // If it is the last point, add the lengths of the previous and next segments. + // For other points, only add the length of the previous segment. + const double segment_length = i == curvature_and_segment_length.size() - 1 + ? segment_length_pair.first + : segment_length_pair.first + segment_length_pair.second; + // after shifted segment length const double after_segment_length = k > 0 ? segment_length * (1 + k * dr) : segment_length / (1 - k * dr); diff --git a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/shift_pull_out.cpp b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/shift_pull_out.cpp index 7c78f62192105..f1e0f8abb2395 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/shift_pull_out.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/shift_pull_out.cpp @@ -423,8 +423,17 @@ double ShiftPullOut::calcBeforeShiftedArcLength( double before_arc_length{0.0}; double after_arc_length{0.0}; - for (const auto & [k, segment_length] : - autoware::motion_utils::calcCurvatureAndArcLength(path.points)) { + const auto curvatures_and_segment_lengths = + autoware::motion_utils::calcCurvatureAndSegmentLength(path.points); + for (size_t i = 0; i < curvatures_and_segment_lengths.size(); ++i) { + const auto & [k, segment_length_pair] = curvatures_and_segment_lengths.at(i); + + // If it is the last point, add the lengths of the previous and next segments. + // For other points, only add the length of the previous segment. + const double segment_length = i == curvatures_and_segment_lengths.size() - 1 + ? segment_length_pair.first + segment_length_pair.second + : segment_length_pair.first; + // after shifted segment length const double after_segment_length = k < 0 ? segment_length * (1 - k * dr) : segment_length / (1 + k * dr); From 1654a578acec1d033746200d5e052ba0d0da7355 Mon Sep 17 00:00:00 2001 From: Maxime CLEMENT <78338830+maxime-clem@users.noreply.github.com> Date: Tue, 9 Jul 2024 19:55:42 +0900 Subject: [PATCH 6/8] fix(frenet_planner): fix mistake in the curvature calculation (#7920) Signed-off-by: Maxime CLEMENT --- .../lane_driving/motion_planning/motion_planning.launch.xml | 1 + .../src/frenet_planner/frenet_planner.cpp | 4 ++-- 2 files changed, 3 insertions(+), 2 deletions(-) diff --git a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.xml b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.xml index d0c34ea01b2c6..63353459fa16c 100644 --- a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.xml +++ b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.xml @@ -90,6 +90,7 @@ + diff --git a/planning/sampling_based_planner/autoware_frenet_planner/src/frenet_planner/frenet_planner.cpp b/planning/sampling_based_planner/autoware_frenet_planner/src/frenet_planner/frenet_planner.cpp index 1f05fc93138a2..1e04db2a17eee 100644 --- a/planning/sampling_based_planner/autoware_frenet_planner/src/frenet_planner/frenet_planner.cpp +++ b/planning/sampling_based_planner/autoware_frenet_planner/src/frenet_planner/frenet_planner.cpp @@ -1,4 +1,4 @@ -// Copyright 2023 Tier IV, Inc. +// Copyright 2023-2024 Tier IV, Inc. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -186,7 +186,7 @@ void calculateCartesian( for (size_t i = 1; i < path.yaws.size(); ++i) { const auto dyaw = autoware::common::helper_functions::wrap_angle(path.yaws[i] - path.yaws[i - 1]); - path.curvatures.push_back(dyaw / (path.lengths[i - 1], path.lengths[i])); + path.curvatures.push_back(dyaw / (path.lengths[i] - path.lengths[i - 1])); } path.curvatures.push_back(path.curvatures.back()); } From 9b20ced4b6d976bc30e28e997269a0e0dea4bbfa Mon Sep 17 00:00:00 2001 From: "Yi-Hsiang Fang (Vivid)" <146902905+vividf@users.noreply.github.com> Date: Tue, 9 Jul 2024 21:02:02 +0900 Subject: [PATCH 7/8] fix(pointcloud_preprocessor): fix distortion corrector unit test (#7833) * fix: use pointcloud iterator instead of memcpy, remove reinterpret cast Signed-off-by: vividf * chore: set default debugging parameter to false Signed-off-by: vividf * style(pre-commit): autofix * chore: run precommit Signed-off-by: vividf * chore: fix TIER IV name Signed-off-by: vividf * chore: changed public variables to protected and add getters Signed-off-by: vividf * chore: add tolerance Signed-off-by: vividf * feat: add two tests for pure linear and rotation Signed-off-by: vividf * chore: change naming, fix tolerance value Signed-off-by: vividf * chore: add comment for quat Signed-off-by: vividf * fix: remove node_->get_clock() and use self-defined timestamp Signed-off-by: vividf * chore: remove redundant parameters Signed-off-by: vividf * chore: fix spell error and add tests in cmake Signed-off-by: vividf * chore: move all clock->now() to self-defined time Signed-off-by: vividf * chore: change function names Signed-off-by: vividf * chore: remove irrelevant variable Signed-off-by: vividf * chore: fix variables naming Signed-off-by: vividf * chore: change boolen naming: generate_points Signed-off-by: vividf * chore: add assert to make sure ms is not larger than a second Signed-off-by: vividf * chore: add note Signed-off-by: vividf * chore: add unifore initialization and semantic meaning for magic number Signed-off-by: vividf * chore: change vector to Eigen Signed-off-by: vividf * chore: fix explanation Signed-off-by: vividf * chore: fix more eigen Signed-off-by: vividf * chore: fix more magic numbers Signed-off-by: vividf * chore: add unit Signed-off-by: vividf * fix: use assert from gtest Signed-off-by: vividf --------- Signed-off-by: vividf Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> Co-authored-by: Kenzo Lobos Tsunekawa --- .../pointcloud_preprocessor/CMakeLists.txt | 4 + .../distortion_corrector.hpp | 9 + .../distortion_corrector.cpp | 29 +- .../test/test_distortion_corrector_node.cpp | 904 ++++++++++++++++++ 4 files changed, 945 insertions(+), 1 deletion(-) create mode 100644 sensing/pointcloud_preprocessor/test/test_distortion_corrector_node.cpp diff --git a/sensing/pointcloud_preprocessor/CMakeLists.txt b/sensing/pointcloud_preprocessor/CMakeLists.txt index 53b6aae55d47b..2d0649fe43954 100644 --- a/sensing/pointcloud_preprocessor/CMakeLists.txt +++ b/sensing/pointcloud_preprocessor/CMakeLists.txt @@ -231,8 +231,12 @@ if(BUILD_TESTING) ament_add_gtest(test_utilities test/test_utilities.cpp ) + ament_add_gtest(distortion_corrector_node_tests + test/test_distortion_corrector_node.cpp + ) target_link_libraries(test_utilities pointcloud_preprocessor_filter) + target_link_libraries(distortion_corrector_node_tests pointcloud_preprocessor_filter) endif() diff --git a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp index 8a7b7c90113da..e37329a9a4cc3 100644 --- a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp +++ b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp @@ -48,6 +48,11 @@ namespace pointcloud_preprocessor class DistortionCorrectorBase { public: + virtual bool pointcloud_transform_exists() = 0; + virtual bool pointcloud_transform_needed() = 0; + virtual std::deque get_twist_queue() = 0; + virtual std::deque get_angular_velocity_queue() = 0; + virtual void processTwistMessage( const geometry_msgs::msg::TwistWithCovarianceStamped::ConstSharedPtr twist_msg) = 0; virtual void processIMUMessage( @@ -96,6 +101,10 @@ class DistortionCorrector : public DistortionCorrectorBase : node_(node), tf_buffer_(node_->get_clock()), tf_listener_(tf_buffer_) { } + bool pointcloud_transform_exists(); + bool pointcloud_transform_needed(); + std::deque get_twist_queue(); + std::deque get_angular_velocity_queue(); void processTwistMessage( const geometry_msgs::msg::TwistWithCovarianceStamped::ConstSharedPtr twist_msg) override; diff --git a/sensing/pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp b/sensing/pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp index 8c8752c7e278e..f933d4c6f75d9 100644 --- a/sensing/pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp +++ b/sensing/pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp @@ -20,6 +20,31 @@ namespace pointcloud_preprocessor { + +template +bool DistortionCorrector::pointcloud_transform_exists() +{ + return pointcloud_transform_exists_; +} + +template +bool DistortionCorrector::pointcloud_transform_needed() +{ + return pointcloud_transform_needed_; +} + +template +std::deque DistortionCorrector::get_twist_queue() +{ + return twist_queue_; +} + +template +std::deque DistortionCorrector::get_angular_velocity_queue() +{ + return angular_velocity_queue_; +} + template void DistortionCorrector::processTwistMessage( const geometry_msgs::msg::TwistWithCovarianceStamped::ConstSharedPtr twist_msg) @@ -349,7 +374,9 @@ inline void DistortionCorrector2D::undistortPointImplementation( theta_ += w * time_offset; baselink_quat_.setValue( 0, 0, autoware::universe_utils::sin(theta_ * 0.5f), - autoware::universe_utils::cos(theta_ * 0.5f)); // baselink_quat.setRPY(0.0, 0.0, theta); + autoware::universe_utils::cos( + theta_ * + 0.5f)); // baselink_quat.setRPY(0.0, 0.0, theta); (Note that the value is slightly different) const float dis = v * time_offset; x_ += dis * autoware::universe_utils::cos(theta_); y_ += dis * autoware::universe_utils::sin(theta_); diff --git a/sensing/pointcloud_preprocessor/test/test_distortion_corrector_node.cpp b/sensing/pointcloud_preprocessor/test/test_distortion_corrector_node.cpp new file mode 100644 index 0000000000000..7997aaf43c202 --- /dev/null +++ b/sensing/pointcloud_preprocessor/test/test_distortion_corrector_node.cpp @@ -0,0 +1,904 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +// Note: To regenerate the ground truth (GT) for the expected undistorted point cloud values, +// set the "debug_" value to true to display the point cloud values. Then, +// replace the expected values with the newly displayed undistorted point cloud values. +// +// Also, make sure the point stamp, twist stamp, and imu stamp are not identical. +// In the current hardcoded design, timestamp of pointcloud, twist, and imu msg are listed below +// pointcloud (1 msgs, 10points): +// 10.10, 10.11, 10.12, 10.13, 10.14, 10.15, 10.16, 10.17, 10.18, 10.19 +// twist (6msgs): +// 10.095, 10.119, 10.143, 10.167, 10.191, 10.215 +// imu (6msgs): +// 10.09, 10.117, 10.144, 10.171, 10.198, 10.225 + +#include "autoware/universe_utils/math/trigonometry.hpp" +#include "pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp" + +#include + +#include +#include +#include +#include +#include + +#include +#include + +#include + +class DistortionCorrectorTest : public ::testing::Test +{ +protected: + void SetUp() override + { + node_ = std::make_shared("test_node"); + distortion_corrector_2d_ = + std::make_shared(node_.get()); + distortion_corrector_3d_ = + std::make_shared(node_.get()); + + // Setup TF + tf_broadcaster_ = std::make_shared(node_); + tf_broadcaster_->sendTransform(generateStaticTransformMsg()); + + // Spin the node for a while to ensure transforms are published + auto start = std::chrono::steady_clock::now(); + auto timeout = std::chrono::seconds(1); + while (std::chrono::steady_clock::now() - start < timeout) { + rclcpp::spin_some(node_); + std::this_thread::sleep_for(std::chrono::milliseconds(10)); + } + } + + void TearDown() override {} + + void checkInput(int ms) { ASSERT_LT(ms, 1000) << "ms should be less than a second."; } + + rclcpp::Time addMilliseconds(rclcpp::Time stamp, int ms) + { + checkInput(ms); + auto ms_in_ns = rclcpp::Duration(0, ms * 1000000); + return stamp + ms_in_ns; + } + + rclcpp::Time subtractMilliseconds(rclcpp::Time stamp, int ms) + { + checkInput(ms); + auto ms_in_ns = rclcpp::Duration(0, ms * 1000000); + return stamp - ms_in_ns; + } + + geometry_msgs::msg::TransformStamped generateTransformMsg( + const std::string & parent_frame, const std::string & child_frame, double x, double y, double z, + double qx, double qy, double qz, double qw) + { + rclcpp::Time timestamp(timestamp_seconds_, timestamp_nanoseconds_, RCL_ROS_TIME); + geometry_msgs::msg::TransformStamped tf_msg; + tf_msg.header.stamp = timestamp; + tf_msg.header.frame_id = parent_frame; + tf_msg.child_frame_id = child_frame; + tf_msg.transform.translation.x = x; + tf_msg.transform.translation.y = y; + tf_msg.transform.translation.z = z; + tf_msg.transform.rotation.x = qx; + tf_msg.transform.rotation.y = qy; + tf_msg.transform.rotation.z = qz; + tf_msg.transform.rotation.w = qw; + return tf_msg; + } + + std::vector generateStaticTransformMsg() + { + // generate defined transformations + return { + generateTransformMsg("base_link", "lidar_top", 5.0, 5.0, 5.0, 0.683, 0.5, 0.183, 0.499), + generateTransformMsg("base_link", "imu_link", 1.0, 1.0, 3.0, 0.278, 0.717, 0.441, 0.453)}; + } + + std::shared_ptr generateTwistMsg( + double linear_x, double angular_z, rclcpp::Time stamp) + { + auto twist_msg = std::make_shared(); + twist_msg->header.stamp = stamp; + twist_msg->header.frame_id = "base_link"; + twist_msg->twist.twist.linear.x = linear_x; + twist_msg->twist.twist.angular.z = angular_z; + return twist_msg; + } + + std::shared_ptr generateImuMsg( + double angular_vel_x, double angular_vel_y, double angular_vel_z, rclcpp::Time stamp) + { + auto imu_msg = std::make_shared(); + imu_msg->header.stamp = stamp; + imu_msg->header.frame_id = "imu_link"; + imu_msg->angular_velocity.x = angular_vel_x; + imu_msg->angular_velocity.y = angular_vel_y; + imu_msg->angular_velocity.z = angular_vel_z; + return imu_msg; + } + + std::vector> generateTwistMsgs( + rclcpp::Time pointcloud_timestamp) + { + std::vector> twist_msgs; + rclcpp::Time twist_stamp = subtractMilliseconds(pointcloud_timestamp, 5); + + for (int i = 0; i < number_of_twist_msgs_; ++i) { + auto twist_msg = generateTwistMsg( + twist_linear_x_ + i * twist_linear_x_increment_, + twist_angular_z_ + i * twist_angular_z_increment_, twist_stamp); + twist_msgs.push_back(twist_msg); + + twist_stamp = addMilliseconds(twist_stamp, twist_msgs_interval_ms_); + } + + return twist_msgs; + } + + std::vector> generateImuMsgs( + rclcpp::Time pointcloud_timestamp) + { + std::vector> imu_msgs; + rclcpp::Time imu_stamp = subtractMilliseconds(pointcloud_timestamp, 10); + + for (int i = 0; i < number_of_imu_msgs_; ++i) { + auto imu_msg = generateImuMsg( + imu_angular_x_ + i * imu_angular_x_increment_, + imu_angular_y_ + i * imu_angular_y_increment_, + imu_angular_z_ + i * imu_angular_z_increment_, imu_stamp); + imu_msgs.push_back(imu_msg); + imu_stamp = addMilliseconds(imu_stamp, imu_msgs_interval_ms_); + } + + return imu_msgs; + } + + sensor_msgs::msg::PointCloud2 generatePointCloudMsg( + bool generate_points, bool is_lidar_frame, rclcpp::Time stamp) + { + sensor_msgs::msg::PointCloud2 pointcloud_msg; + pointcloud_msg.header.stamp = stamp; + pointcloud_msg.header.frame_id = is_lidar_frame ? "lidar_top" : "base_link"; + pointcloud_msg.height = 1; + pointcloud_msg.is_dense = true; + pointcloud_msg.is_bigendian = false; + + if (generate_points) { + std::array points = {{ + Eigen::Vector3f(10.0f, 0.0f, 0.0f), // point 1 + Eigen::Vector3f(0.0f, 10.0f, 0.0f), // point 2 + Eigen::Vector3f(0.0f, 0.0f, 10.0f), // point 3 + Eigen::Vector3f(20.0f, 0.0f, 0.0f), // point 4 + Eigen::Vector3f(0.0f, 20.0f, 0.0f), // point 5 + Eigen::Vector3f(0.0f, 0.0f, 20.0f), // point 6 + Eigen::Vector3f(30.0f, 0.0f, 0.0f), // point 7 + Eigen::Vector3f(0.0f, 30.0f, 0.0f), // point 8 + Eigen::Vector3f(0.0f, 0.0f, 30.0f), // point 9 + Eigen::Vector3f(10.0f, 10.0f, 10.0f) // point 10 + }}; + + // Generate timestamps for the points + std::vector timestamps = generatePointTimestamps(stamp, number_of_points_); + + sensor_msgs::PointCloud2Modifier modifier(pointcloud_msg); + modifier.setPointCloud2Fields( + 4, "x", 1, sensor_msgs::msg::PointField::FLOAT32, "y", 1, + sensor_msgs::msg::PointField::FLOAT32, "z", 1, sensor_msgs::msg::PointField::FLOAT32, + "time_stamp", 1, sensor_msgs::msg::PointField::FLOAT64); + modifier.resize(number_of_points_); + + sensor_msgs::PointCloud2Iterator iter_x(pointcloud_msg, "x"); + sensor_msgs::PointCloud2Iterator iter_y(pointcloud_msg, "y"); + sensor_msgs::PointCloud2Iterator iter_z(pointcloud_msg, "z"); + sensor_msgs::PointCloud2Iterator iter_t(pointcloud_msg, "time_stamp"); + + for (size_t i = 0; i < number_of_points_; ++i) { + *iter_x = points[i].x(); + *iter_y = points[i].y(); + *iter_z = points[i].z(); + *iter_t = timestamps[i]; + ++iter_x; + ++iter_y; + ++iter_z; + ++iter_t; + } + } else { + pointcloud_msg.width = 0; + pointcloud_msg.row_step = 0; + } + + return pointcloud_msg; + } + + std::vector generatePointTimestamps( + rclcpp::Time pointcloud_timestamp, size_t number_of_points) + { + std::vector timestamps; + rclcpp::Time point_stamp = pointcloud_timestamp; + for (size_t i = 0; i < number_of_points; ++i) { + double timestamp = point_stamp.seconds(); + timestamps.push_back(timestamp); + point_stamp = addMilliseconds(point_stamp, points_interval_ms_); + } + + return timestamps; + } + + std::shared_ptr node_; + std::shared_ptr distortion_corrector_2d_; + std::shared_ptr distortion_corrector_3d_; + std::shared_ptr tf_broadcaster_; + + static constexpr float standard_tolerance_{1e-4}; + static constexpr float coarse_tolerance_{5e-3}; + static constexpr int number_of_twist_msgs_{6}; + static constexpr int number_of_imu_msgs_{6}; + static constexpr size_t number_of_points_{10}; + static constexpr int32_t timestamp_seconds_{10}; + static constexpr uint32_t timestamp_nanoseconds_{100000000}; + + static constexpr double twist_linear_x_{10.0}; + static constexpr double twist_angular_z_{0.02}; + static constexpr double twist_linear_x_increment_{2.0}; + static constexpr double twist_angular_z_increment_{0.01}; + + static constexpr double imu_angular_x_{0.01}; + static constexpr double imu_angular_y_{-0.02}; + static constexpr double imu_angular_z_{0.05}; + static constexpr double imu_angular_x_increment_{0.005}; + static constexpr double imu_angular_y_increment_{0.005}; + static constexpr double imu_angular_z_increment_{0.005}; + + static constexpr int points_interval_ms_{10}; + static constexpr int twist_msgs_interval_ms_{24}; + static constexpr int imu_msgs_interval_ms_{27}; + + // for debugging or regenerating the ground truth point cloud + bool debug_{false}; +}; + +TEST_F(DistortionCorrectorTest, TestProcessTwistMessage) +{ + rclcpp::Time timestamp(timestamp_seconds_, timestamp_nanoseconds_, RCL_ROS_TIME); + auto twist_msg = generateTwistMsg(twist_linear_x_, twist_angular_z_, timestamp); + distortion_corrector_2d_->processTwistMessage(twist_msg); + + ASSERT_FALSE(distortion_corrector_2d_->get_twist_queue().empty()); + EXPECT_EQ(distortion_corrector_2d_->get_twist_queue().front().twist.linear.x, twist_linear_x_); + EXPECT_EQ(distortion_corrector_2d_->get_twist_queue().front().twist.angular.z, twist_angular_z_); +} + +TEST_F(DistortionCorrectorTest, TestProcessIMUMessage) +{ + rclcpp::Time timestamp(timestamp_seconds_, timestamp_nanoseconds_, RCL_ROS_TIME); + auto imu_msg = generateImuMsg(imu_angular_x_, imu_angular_y_, imu_angular_z_, timestamp); + distortion_corrector_2d_->processIMUMessage("base_link", imu_msg); + + ASSERT_FALSE(distortion_corrector_2d_->get_angular_velocity_queue().empty()); + EXPECT_NEAR( + distortion_corrector_2d_->get_angular_velocity_queue().front().vector.z, -0.03159, + standard_tolerance_); +} + +TEST_F(DistortionCorrectorTest, TestIsInputValid) +{ + rclcpp::Time timestamp(timestamp_seconds_, timestamp_nanoseconds_, RCL_ROS_TIME); + + // input normal pointcloud without twist + sensor_msgs::msg::PointCloud2 pointcloud = generatePointCloudMsg(true, false, timestamp); + bool result = distortion_corrector_2d_->isInputValid(pointcloud); + EXPECT_FALSE(result); + + // input normal pointcloud with valid twist + auto twist_msg = generateTwistMsg(twist_linear_x_, twist_angular_z_, timestamp); + distortion_corrector_2d_->processTwistMessage(twist_msg); + + pointcloud = generatePointCloudMsg(true, false, timestamp); + result = distortion_corrector_2d_->isInputValid(pointcloud); + EXPECT_TRUE(result); + + // input empty pointcloud + pointcloud = generatePointCloudMsg(false, false, timestamp); + result = distortion_corrector_2d_->isInputValid(pointcloud); + EXPECT_FALSE(result); +} + +TEST_F(DistortionCorrectorTest, TestSetPointCloudTransformWithBaseLink) +{ + distortion_corrector_2d_->setPointCloudTransform("base_link", "base_link"); + EXPECT_TRUE(distortion_corrector_2d_->pointcloud_transform_exists()); + EXPECT_FALSE(distortion_corrector_2d_->pointcloud_transform_needed()); +} + +TEST_F(DistortionCorrectorTest, TestSetPointCloudTransformWithLidarFrame) +{ + distortion_corrector_2d_->setPointCloudTransform("base_link", "lidar_top"); + EXPECT_TRUE(distortion_corrector_2d_->pointcloud_transform_exists()); + EXPECT_TRUE(distortion_corrector_2d_->pointcloud_transform_needed()); +} + +TEST_F(DistortionCorrectorTest, TestSetPointCloudTransformWithMissingFrame) +{ + distortion_corrector_2d_->setPointCloudTransform("base_link", "missing_lidar_frame"); + EXPECT_FALSE(distortion_corrector_2d_->pointcloud_transform_exists()); + EXPECT_FALSE(distortion_corrector_2d_->pointcloud_transform_needed()); +} + +TEST_F(DistortionCorrectorTest, TestUndistortPointCloudWithEmptyTwist) +{ + rclcpp::Time timestamp(timestamp_seconds_, timestamp_nanoseconds_, RCL_ROS_TIME); + // Generate the point cloud message + sensor_msgs::msg::PointCloud2 pointcloud = generatePointCloudMsg(true, false, timestamp); + + // Process empty twist queue + distortion_corrector_2d_->initialize(); + distortion_corrector_2d_->undistortPointCloud(false, pointcloud); + + // Verify the point cloud is not changed + sensor_msgs::PointCloud2ConstIterator iter_x(pointcloud, "x"); + sensor_msgs::PointCloud2ConstIterator iter_y(pointcloud, "y"); + sensor_msgs::PointCloud2ConstIterator iter_z(pointcloud, "z"); + + std::array expected_pointcloud = { + {Eigen::Vector3f(10.0f, 0.0f, 0.0f), Eigen::Vector3f(0.0f, 10.0f, 0.0f), + Eigen::Vector3f(0.0f, 0.0f, 10.0f), Eigen::Vector3f(20.0f, 0.0f, 0.0f), + Eigen::Vector3f(0.0f, 20.0f, 0.0f), Eigen::Vector3f(0.0f, 0.0f, 20.0f), + Eigen::Vector3f(30.0f, 0.0f, 0.0f), Eigen::Vector3f(0.0f, 30.0f, 0.0f), + Eigen::Vector3f(0.0f, 0.0f, 30.0f), Eigen::Vector3f(10.0f, 10.0f, 10.0f)}}; + + size_t i = 0; + std::ostringstream oss; + oss << "Expected pointcloud:\n"; + + for (; iter_x != iter_x.end(); ++iter_x, ++iter_y, ++iter_z, ++i) { + oss << "Point " << i << ": (" << *iter_x << ", " << *iter_y << ", " << *iter_z << ")\n"; + EXPECT_NEAR(*iter_x, expected_pointcloud[i].x(), standard_tolerance_); + EXPECT_NEAR(*iter_y, expected_pointcloud[i].y(), standard_tolerance_); + EXPECT_NEAR(*iter_z, expected_pointcloud[i].z(), standard_tolerance_); + } + + if (debug_) { + RCLCPP_INFO(node_->get_logger(), "%s", oss.str().c_str()); + } +} + +TEST_F(DistortionCorrectorTest, TestUndistortPointCloudWithEmptyPointCloud) +{ + rclcpp::Time timestamp(timestamp_seconds_, timestamp_nanoseconds_, RCL_ROS_TIME); + // Generate and process multiple twist messages + auto twist_msgs = generateTwistMsgs(timestamp); + for (const auto & twist_msg : twist_msgs) { + distortion_corrector_2d_->processTwistMessage(twist_msg); + } + // Generate an empty point cloud message + sensor_msgs::msg::PointCloud2 empty_pointcloud = generatePointCloudMsg(false, false, timestamp); + + // Process empty point cloud + distortion_corrector_2d_->initialize(); + distortion_corrector_2d_->undistortPointCloud(true, empty_pointcloud); + + // Verify the point cloud is still empty + EXPECT_EQ(empty_pointcloud.width, 0); + EXPECT_EQ(empty_pointcloud.row_step, 0); +} + +TEST_F(DistortionCorrectorTest, TestUndistortPointCloud2dWithoutImuInBaseLink) +{ + // Generate the point cloud message + rclcpp::Time timestamp(timestamp_seconds_, timestamp_nanoseconds_, RCL_ROS_TIME); + sensor_msgs::msg::PointCloud2 pointcloud = generatePointCloudMsg(true, false, timestamp); + + // Generate and process multiple twist messages + auto twist_msgs = generateTwistMsgs(timestamp); + for (const auto & twist_msg : twist_msgs) { + distortion_corrector_2d_->processTwistMessage(twist_msg); + } + + // Test using only twist + distortion_corrector_2d_->initialize(); + distortion_corrector_2d_->setPointCloudTransform("base_link", "base_link"); + distortion_corrector_2d_->undistortPointCloud(false, pointcloud); + + sensor_msgs::PointCloud2ConstIterator iter_x(pointcloud, "x"); + sensor_msgs::PointCloud2ConstIterator iter_y(pointcloud, "y"); + sensor_msgs::PointCloud2ConstIterator iter_z(pointcloud, "z"); + + // Expected undistorted point cloud values + std::array expected_pointcloud = { + {Eigen::Vector3f(10.0f, 0.0f, 0.0f), Eigen::Vector3f(0.117124f, 10.0f, 0.0f), + Eigen::Vector3f(0.26f, 0.000135182f, 10.0f), Eigen::Vector3f(20.4f, 0.0213818f, 0.0f), + Eigen::Vector3f(0.50932f, 20.0005f, 0.0f), Eigen::Vector3f(0.699999f, 0.000819721f, 20.0f), + Eigen::Vector3f(30.8599f, 0.076f, 0.0f), Eigen::Vector3f(0.947959f, 30.0016f, 0.0f), + Eigen::Vector3f(1.22f, 0.00244382f, 30.0f), Eigen::Vector3f(11.3568f, 10.0463f, 10.0f)}}; + + // Verify each point in the undistorted point cloud + size_t i = 0; + std::ostringstream oss; + oss << "Expected pointcloud:\n"; + + for (; iter_x != iter_x.end(); ++iter_x, ++iter_y, ++iter_z, ++i) { + oss << "Point " << i << ": (" << *iter_x << ", " << *iter_y << ", " << *iter_z << ")\n"; + EXPECT_NEAR(*iter_x, expected_pointcloud[i].x(), standard_tolerance_); + EXPECT_NEAR(*iter_y, expected_pointcloud[i].y(), standard_tolerance_); + EXPECT_NEAR(*iter_z, expected_pointcloud[i].z(), standard_tolerance_); + } + + if (debug_) { + RCLCPP_INFO(node_->get_logger(), "%s", oss.str().c_str()); + } +} + +TEST_F(DistortionCorrectorTest, TestUndistortPointCloud2dWithImuInBaseLink) +{ + // Generate the point cloud message + rclcpp::Time timestamp(timestamp_seconds_, timestamp_nanoseconds_, RCL_ROS_TIME); + sensor_msgs::msg::PointCloud2 pointcloud = generatePointCloudMsg(true, false, timestamp); + + // Generate and process multiple twist messages + auto twist_msgs = generateTwistMsgs(timestamp); + for (const auto & twist_msg : twist_msgs) { + distortion_corrector_2d_->processTwistMessage(twist_msg); + } + + // Generate and process multiple IMU messages + auto imu_msgs = generateImuMsgs(timestamp); + for (const auto & imu_msg : imu_msgs) { + distortion_corrector_2d_->processIMUMessage("base_link", imu_msg); + } + + distortion_corrector_2d_->initialize(); + distortion_corrector_2d_->setPointCloudTransform("base_link", "base_link"); + distortion_corrector_2d_->undistortPointCloud(true, pointcloud); + + sensor_msgs::PointCloud2ConstIterator iter_x(pointcloud, "x"); + sensor_msgs::PointCloud2ConstIterator iter_y(pointcloud, "y"); + sensor_msgs::PointCloud2ConstIterator iter_z(pointcloud, "z"); + + // Expected undistorted point cloud values + std::array expected_pointcloud = { + {Eigen::Vector3f(10.0f, 0.0f, 0.0f), Eigen::Vector3f(0.122876f, 9.99996f, 0.0f), + Eigen::Vector3f(0.26f, -0.000115049f, 10.0f), Eigen::Vector3f(20.4f, -0.0174931f, 0.0f), + Eigen::Vector3f(0.56301f, 19.9996f, 0.0f), Eigen::Vector3f(0.7f, -0.000627014f, 20.0f), + Eigen::Vector3f(30.86f, -0.052675f, 0.0f), Eigen::Vector3f(1.1004f, 29.9987f, 0.0f), + Eigen::Vector3f(1.22f, -0.00166245f, 30.0f), Eigen::Vector3f(11.4249f, 9.97293f, 10.0f)}}; + + // Verify each point in the undistorted point cloud + size_t i = 0; + std::ostringstream oss; + oss << "Expected pointcloud:\n"; + + for (; iter_x != iter_x.end(); ++iter_x, ++iter_y, ++iter_z, ++i) { + oss << "Point " << i << ": (" << *iter_x << ", " << *iter_y << ", " << *iter_z << ")\n"; + EXPECT_NEAR(*iter_x, expected_pointcloud[i].x(), standard_tolerance_); + EXPECT_NEAR(*iter_y, expected_pointcloud[i].y(), standard_tolerance_); + EXPECT_NEAR(*iter_z, expected_pointcloud[i].z(), standard_tolerance_); + } + + if (debug_) { + RCLCPP_INFO(node_->get_logger(), "%s", oss.str().c_str()); + } +} + +TEST_F(DistortionCorrectorTest, TestUndistortPointCloud2dWithImuInLidarFrame) +{ + // Generate the point cloud message + rclcpp::Time timestamp(timestamp_seconds_, timestamp_nanoseconds_, RCL_ROS_TIME); + sensor_msgs::msg::PointCloud2 pointcloud = generatePointCloudMsg(true, true, timestamp); + + // Generate and process multiple twist messages + auto twist_msgs = generateTwistMsgs(timestamp); + for (const auto & twist_msg : twist_msgs) { + distortion_corrector_2d_->processTwistMessage(twist_msg); + } + + // Generate and process multiple IMU messages + auto imu_msgs = generateImuMsgs(timestamp); + for (const auto & imu_msg : imu_msgs) { + distortion_corrector_2d_->processIMUMessage("base_link", imu_msg); + } + + distortion_corrector_2d_->initialize(); + distortion_corrector_2d_->setPointCloudTransform("base_link", "lidar_top"); + distortion_corrector_2d_->undistortPointCloud(true, pointcloud); + + sensor_msgs::PointCloud2ConstIterator iter_x(pointcloud, "x"); + sensor_msgs::PointCloud2ConstIterator iter_y(pointcloud, "y"); + sensor_msgs::PointCloud2ConstIterator iter_z(pointcloud, "z"); + + // Expected undistorted point cloud values + std::array expected_pointcloud = { + {Eigen::Vector3f(10.0f, -1.77636e-15f, -4.44089e-16f), + Eigen::Vector3f(0.049989f, 10.0608f, 0.0924992f), + Eigen::Vector3f(0.106107f, 0.130237f, 10.1986f), + Eigen::Vector3f(20.1709f, 0.210011f, 0.32034f), + Eigen::Vector3f(0.220674f, 20.2734f, 0.417974f), + Eigen::Vector3f(0.274146f, 0.347043f, 20.5341f), + Eigen::Vector3f(30.3673f, 0.457564f, 0.700818f), + Eigen::Vector3f(0.418014f, 30.5259f, 0.807963f), + Eigen::Vector3f(0.464088f, 0.600081f, 30.9292f), + Eigen::Vector3f(10.5657f, 10.7121f, 11.094f)}}; + + // Verify each point in the undistorted point cloud + size_t i = 0; + std::ostringstream oss; + oss << "Expected pointcloud:\n"; + + for (; iter_x != iter_x.end(); ++iter_x, ++iter_y, ++iter_z, ++i) { + oss << "Point " << i << ": (" << *iter_x << ", " << *iter_y << ", " << *iter_z << ")\n"; + EXPECT_NEAR(*iter_x, expected_pointcloud[i].x(), standard_tolerance_); + EXPECT_NEAR(*iter_y, expected_pointcloud[i].y(), standard_tolerance_); + EXPECT_NEAR(*iter_z, expected_pointcloud[i].z(), standard_tolerance_); + } + + if (debug_) { + RCLCPP_INFO(node_->get_logger(), "%s", oss.str().c_str()); + } +} + +TEST_F(DistortionCorrectorTest, TestUndistortPointCloud3dWithoutImuInBaseLink) +{ + // Generate the point cloud message + rclcpp::Time timestamp(timestamp_seconds_, timestamp_nanoseconds_, RCL_ROS_TIME); + sensor_msgs::msg::PointCloud2 pointcloud = generatePointCloudMsg(true, false, timestamp); + + // Generate and process multiple twist messages + auto twist_msgs = generateTwistMsgs(timestamp); + for (const auto & twist_msg : twist_msgs) { + distortion_corrector_3d_->processTwistMessage(twist_msg); + } + + // Test using only twist + distortion_corrector_3d_->initialize(); + distortion_corrector_3d_->setPointCloudTransform("base_link", "base_link"); + distortion_corrector_3d_->undistortPointCloud(false, pointcloud); + + sensor_msgs::PointCloud2ConstIterator iter_x(pointcloud, "x"); + sensor_msgs::PointCloud2ConstIterator iter_y(pointcloud, "y"); + sensor_msgs::PointCloud2ConstIterator iter_z(pointcloud, "z"); + + // Expected undistorted point cloud values + std::array expected_pointcloud = { + {Eigen::Vector3f(10.0f, 0.0f, 0.0f), Eigen::Vector3f(0.117f, 10.0f, 0.0f), + Eigen::Vector3f(0.26f, 9.27035e-05f, 10.0f), Eigen::Vector3f(20.4f, 0.0222176f, 0.0f), + Eigen::Vector3f(0.51f, 20.0004f, 0.0f), Eigen::Vector3f(0.7f, 0.000706573f, 20.0f), + Eigen::Vector3f(30.8599f, 0.0760946f, 0.0f), Eigen::Vector3f(0.946998f, 30.0015f, 0.0f), + Eigen::Vector3f(1.22f, 0.00234201f, 30.0f), Eigen::Vector3f(11.3569f, 10.046f, 10.0f)}}; + + // Verify each point in the undistorted point cloud + size_t i = 0; + std::ostringstream oss; + oss << "Expected pointcloud:\n"; + + for (; iter_x != iter_x.end(); ++iter_x, ++iter_y, ++iter_z, ++i) { + oss << "Point " << i << ": (" << *iter_x << ", " << *iter_y << ", " << *iter_z << ")\n"; + EXPECT_NEAR(*iter_x, expected_pointcloud[i].x(), standard_tolerance_); + EXPECT_NEAR(*iter_y, expected_pointcloud[i].y(), standard_tolerance_); + EXPECT_NEAR(*iter_z, expected_pointcloud[i].z(), standard_tolerance_); + } + + if (debug_) { + RCLCPP_INFO(node_->get_logger(), "%s", oss.str().c_str()); + } +} + +TEST_F(DistortionCorrectorTest, TestUndistortPointCloud3dWithImuInBaseLink) +{ + // Generate the point cloud message + rclcpp::Time timestamp(timestamp_seconds_, timestamp_nanoseconds_, RCL_ROS_TIME); + sensor_msgs::msg::PointCloud2 pointcloud = generatePointCloudMsg(true, false, timestamp); + + // Generate and process multiple twist messages + auto twist_msgs = generateTwistMsgs(timestamp); + for (const auto & twist_msg : twist_msgs) { + distortion_corrector_3d_->processTwistMessage(twist_msg); + } + + // Generate and process multiple IMU messages + auto imu_msgs = generateImuMsgs(timestamp); + for (const auto & imu_msg : imu_msgs) { + distortion_corrector_3d_->processIMUMessage("base_link", imu_msg); + } + + distortion_corrector_3d_->initialize(); + distortion_corrector_3d_->setPointCloudTransform("base_link", "base_link"); + distortion_corrector_3d_->undistortPointCloud(true, pointcloud); + + sensor_msgs::PointCloud2ConstIterator iter_x(pointcloud, "x"); + sensor_msgs::PointCloud2ConstIterator iter_y(pointcloud, "y"); + sensor_msgs::PointCloud2ConstIterator iter_z(pointcloud, "z"); + + // Expected undistorted point cloud values + std::array expected_pointcloud = { + {Eigen::Vector3f(10.0f, 0.0f, 0.0f), Eigen::Vector3f(0.123015f, 9.99998f, 0.00430552f), + Eigen::Vector3f(0.266103f, -0.00895269f, 9.99992f), + Eigen::Vector3f(20.4f, -0.0176539f, -0.0193392f), + Eigen::Vector3f(0.563265f, 19.9997f, 0.035628f), + Eigen::Vector3f(0.734597f, -0.046068f, 19.9993f), + Eigen::Vector3f(30.8599f, -0.0517931f, -0.0658165f), + Eigen::Vector3f(1.0995f, 29.9989f, 0.0956997f), + Eigen::Vector3f(1.31283f, -0.113544f, 29.9977f), + Eigen::Vector3f(11.461f, 9.93096f, 10.0035f)}}; + + // Verify each point in the undistorted point cloud + size_t i = 0; + std::ostringstream oss; + oss << "Expected pointcloud:\n"; + + for (; iter_x != iter_x.end(); ++iter_x, ++iter_y, ++iter_z, ++i) { + oss << "Point " << i << ": (" << *iter_x << ", " << *iter_y << ", " << *iter_z << ")\n"; + EXPECT_NEAR(*iter_x, expected_pointcloud[i].x(), standard_tolerance_); + EXPECT_NEAR(*iter_y, expected_pointcloud[i].y(), standard_tolerance_); + EXPECT_NEAR(*iter_z, expected_pointcloud[i].z(), standard_tolerance_); + } + + if (debug_) { + RCLCPP_INFO(node_->get_logger(), "%s", oss.str().c_str()); + } +} + +TEST_F(DistortionCorrectorTest, TestUndistortPointCloud3dWithImuInLidarFrame) +{ + // Generate the point cloud message + rclcpp::Time timestamp(timestamp_seconds_, timestamp_nanoseconds_, RCL_ROS_TIME); + sensor_msgs::msg::PointCloud2 pointcloud = generatePointCloudMsg(true, true, timestamp); + + // Generate and process multiple twist messages + auto twist_msgs = generateTwistMsgs(timestamp); + for (const auto & twist_msg : twist_msgs) { + distortion_corrector_3d_->processTwistMessage(twist_msg); + } + + // Generate and process multiple IMU messages + auto imu_msgs = generateImuMsgs(timestamp); + for (const auto & imu_msg : imu_msgs) { + distortion_corrector_3d_->processIMUMessage("base_link", imu_msg); + } + + distortion_corrector_3d_->initialize(); + distortion_corrector_3d_->setPointCloudTransform("base_link", "lidar_top"); + distortion_corrector_3d_->undistortPointCloud(true, pointcloud); + + sensor_msgs::PointCloud2ConstIterator iter_x(pointcloud, "x"); + sensor_msgs::PointCloud2ConstIterator iter_y(pointcloud, "y"); + sensor_msgs::PointCloud2ConstIterator iter_z(pointcloud, "z"); + + // Expected undistorted point cloud values + std::array expected_pointcloud = { + {Eigen::Vector3f(10.0f, 0.0f, 0.0f), Eigen::Vector3f(0.046484f, 10.0622f, 0.098484f), + Eigen::Vector3f(0.107595f, 0.123767f, 10.2026f), + Eigen::Vector3f(20.1667f, 0.22465f, 0.313351f), + Eigen::Vector3f(0.201149f, 20.2784f, 0.464665f), + Eigen::Vector3f(0.290531f, 0.303489f, 20.5452f), + Eigen::Vector3f(30.3598f, 0.494116f, 0.672914f), + Eigen::Vector3f(0.375848f, 30.5336f, 0.933633f), + Eigen::Vector3f(0.510001f, 0.479651f, 30.9493f), + Eigen::Vector3f(10.5629f, 10.6855f, 11.1461f)}}; + + // Verify each point in the undistorted point cloud + size_t i = 0; + std::ostringstream oss; + oss << "Expected pointcloud:\n"; + + for (; iter_x != iter_x.end(); ++iter_x, ++iter_y, ++iter_z, ++i) { + oss << "Point " << i << ": (" << *iter_x << ", " << *iter_y << ", " << *iter_z << ")\n"; + EXPECT_NEAR(*iter_x, expected_pointcloud[i].x(), standard_tolerance_); + EXPECT_NEAR(*iter_y, expected_pointcloud[i].y(), standard_tolerance_); + EXPECT_NEAR(*iter_z, expected_pointcloud[i].z(), standard_tolerance_); + } + + if (debug_) { + RCLCPP_INFO(node_->get_logger(), "%s", oss.str().c_str()); + } +} + +TEST_F(DistortionCorrectorTest, TestUndistortPointCloudWithPureLinearMotion) +{ + rclcpp::Time timestamp(timestamp_seconds_, timestamp_nanoseconds_, RCL_ROS_TIME); + sensor_msgs::msg::PointCloud2 test2d_pointcloud = generatePointCloudMsg(true, false, timestamp); + sensor_msgs::msg::PointCloud2 test3d_pointcloud = generatePointCloudMsg(true, false, timestamp); + + // Generate and process a single twist message with constant linear velocity + auto twist_msg = generateTwistMsg(1.0, 0.0, timestamp); + + distortion_corrector_2d_->processTwistMessage(twist_msg); + distortion_corrector_2d_->initialize(); + distortion_corrector_2d_->setPointCloudTransform("base_link", "base_link"); + distortion_corrector_2d_->undistortPointCloud(false, test2d_pointcloud); + + distortion_corrector_3d_->processTwistMessage(twist_msg); + distortion_corrector_3d_->initialize(); + distortion_corrector_3d_->setPointCloudTransform("base_link", "base_link"); + distortion_corrector_3d_->undistortPointCloud(false, test3d_pointcloud); + + // Generate expected point cloud for testing + sensor_msgs::msg::PointCloud2 expected_pointcloud_msg = + generatePointCloudMsg(true, false, timestamp); + + // Calculate expected point cloud values based on constant linear motion + double velocity = 1.0; // 1 m/s linear velocity + sensor_msgs::PointCloud2Iterator iter_x(expected_pointcloud_msg, "x"); + sensor_msgs::PointCloud2Iterator iter_y(expected_pointcloud_msg, "y"); + sensor_msgs::PointCloud2Iterator iter_z(expected_pointcloud_msg, "z"); + sensor_msgs::PointCloud2Iterator iter_t(expected_pointcloud_msg, "time_stamp"); + + std::vector expected_points; + for (; iter_t != iter_t.end(); ++iter_t, ++iter_x, ++iter_y, ++iter_z) { + double time_offset = *iter_t - timestamp.seconds(); + expected_points.emplace_back( + *iter_x + static_cast(velocity * time_offset), *iter_y, *iter_z); + } + + // Verify each point in the undistorted point cloud + sensor_msgs::PointCloud2Iterator test2d_iter_x(test2d_pointcloud, "x"); + sensor_msgs::PointCloud2Iterator test2d_iter_y(test2d_pointcloud, "y"); + sensor_msgs::PointCloud2Iterator test2d_iter_z(test2d_pointcloud, "z"); + size_t i = 0; + std::ostringstream oss; + oss << "Expected pointcloud:\n"; + + for (; test2d_iter_x != test2d_iter_x.end(); + ++test2d_iter_x, ++test2d_iter_y, ++test2d_iter_z, ++i) { + oss << "Point " << i << ": (" << *test2d_iter_x << ", " << *test2d_iter_y << ", " + << *test2d_iter_z << ")\n"; + EXPECT_FLOAT_EQ(*test2d_iter_x, expected_points[i].x()); + EXPECT_FLOAT_EQ(*test2d_iter_y, expected_points[i].y()); + EXPECT_FLOAT_EQ(*test2d_iter_z, expected_points[i].z()); + } + + if (debug_) { + RCLCPP_INFO(node_->get_logger(), "%s", oss.str().c_str()); + } + + // Verify each point in the undistorted 2d point cloud with undistorted 3d point cloud + test2d_iter_x = sensor_msgs::PointCloud2Iterator(test2d_pointcloud, "x"); + test2d_iter_y = sensor_msgs::PointCloud2Iterator(test2d_pointcloud, "y"); + test2d_iter_z = sensor_msgs::PointCloud2Iterator(test2d_pointcloud, "z"); + sensor_msgs::PointCloud2Iterator test3d_iter_x(test3d_pointcloud, "x"); + sensor_msgs::PointCloud2Iterator test3d_iter_y(test3d_pointcloud, "y"); + sensor_msgs::PointCloud2Iterator test3d_iter_z(test3d_pointcloud, "z"); + + i = 0; + oss.str(""); + oss.clear(); + + oss << "Expected pointcloud:\n"; + for (; test2d_iter_x != test2d_iter_x.end(); ++test2d_iter_x, ++test2d_iter_y, ++test2d_iter_z, + ++test3d_iter_x, ++test3d_iter_y, ++test3d_iter_z, + ++i) { + oss << "Point " << i << " - 2D: (" << *test2d_iter_x << ", " << *test2d_iter_y << ", " + << *test2d_iter_z << ")" + << " vs 3D: (" << *test3d_iter_x << ", " << *test3d_iter_y << ", " << *test3d_iter_z + << ")\n"; + EXPECT_FLOAT_EQ(*test2d_iter_x, *test3d_iter_x); + EXPECT_FLOAT_EQ(*test2d_iter_y, *test3d_iter_y); + EXPECT_FLOAT_EQ(*test2d_iter_z, *test3d_iter_z); + } + if (debug_) { + RCLCPP_INFO(node_->get_logger(), "%s", oss.str().c_str()); + } +} + +TEST_F(DistortionCorrectorTest, TestUndistortPointCloudWithPureRotationalMotion) +{ + rclcpp::Time timestamp(timestamp_seconds_, timestamp_nanoseconds_, RCL_ROS_TIME); + sensor_msgs::msg::PointCloud2 test2d_pointcloud = generatePointCloudMsg(true, false, timestamp); + sensor_msgs::msg::PointCloud2 test3d_pointcloud = generatePointCloudMsg(true, false, timestamp); + + // Generate and process a single twist message with constant angular velocity + auto twist_msg = generateTwistMsg(0.0, 0.1, timestamp); + + distortion_corrector_2d_->processTwistMessage(twist_msg); + distortion_corrector_2d_->initialize(); + distortion_corrector_2d_->setPointCloudTransform("base_link", "base_link"); + distortion_corrector_2d_->undistortPointCloud(false, test2d_pointcloud); + + distortion_corrector_3d_->processTwistMessage(twist_msg); + distortion_corrector_3d_->initialize(); + distortion_corrector_3d_->setPointCloudTransform("base_link", "base_link"); + distortion_corrector_3d_->undistortPointCloud(false, test3d_pointcloud); + + // Generate expected point cloud for testing + sensor_msgs::msg::PointCloud2 expected_pointcloud_msg = + generatePointCloudMsg(true, false, timestamp); + + // Calculate expected point cloud values based on constant rotational motion + double angular_velocity = 0.1; // 0.1 rad/s rotational velocity + sensor_msgs::PointCloud2Iterator iter_x(expected_pointcloud_msg, "x"); + sensor_msgs::PointCloud2Iterator iter_y(expected_pointcloud_msg, "y"); + sensor_msgs::PointCloud2Iterator iter_z(expected_pointcloud_msg, "z"); + sensor_msgs::PointCloud2Iterator iter_t(expected_pointcloud_msg, "time_stamp"); + + std::vector expected_pointcloud; + for (; iter_t != iter_t.end(); ++iter_t, ++iter_x, ++iter_y, ++iter_z) { + double time_offset = *iter_t - timestamp.seconds(); + float angle = angular_velocity * time_offset; + + // Set the quaternion for the current angle + tf2::Quaternion quaternion; + quaternion.setValue( + 0, 0, autoware::universe_utils::sin(angle * 0.5f), + autoware::universe_utils::cos(angle * 0.5f)); + + tf2::Vector3 point(*iter_x, *iter_y, *iter_z); + tf2::Vector3 rotated_point = tf2::quatRotate(quaternion, point); + expected_pointcloud.emplace_back( + static_cast(rotated_point.x()), static_cast(rotated_point.y()), + static_cast(rotated_point.z())); + } + + // Verify each point in the undistorted 2D point cloud + sensor_msgs::PointCloud2Iterator test2d_iter_x(test2d_pointcloud, "x"); + sensor_msgs::PointCloud2Iterator test2d_iter_y(test2d_pointcloud, "y"); + sensor_msgs::PointCloud2Iterator test2d_iter_z(test2d_pointcloud, "z"); + + size_t i = 0; + std::ostringstream oss; + oss << "Expected pointcloud:\n"; + + for (; test2d_iter_x != test2d_iter_x.end(); + ++test2d_iter_x, ++test2d_iter_y, ++test2d_iter_z, ++i) { + oss << "Point " << i << ": (" << *test2d_iter_x << ", " << *test2d_iter_y << ", " + << *test2d_iter_z << ")\n"; + EXPECT_FLOAT_EQ(*test2d_iter_x, expected_pointcloud[i].x()); + EXPECT_FLOAT_EQ(*test2d_iter_y, expected_pointcloud[i].y()); + EXPECT_FLOAT_EQ(*test2d_iter_z, expected_pointcloud[i].z()); + } + + if (debug_) { + RCLCPP_INFO(node_->get_logger(), "%s", oss.str().c_str()); + } + + // Verify each point in the undistorted 2D point cloud with undistorted 3D point cloud + test2d_iter_x = sensor_msgs::PointCloud2Iterator(test2d_pointcloud, "x"); + test2d_iter_y = sensor_msgs::PointCloud2Iterator(test2d_pointcloud, "y"); + test2d_iter_z = sensor_msgs::PointCloud2Iterator(test2d_pointcloud, "z"); + sensor_msgs::PointCloud2Iterator test3d_iter_x(test3d_pointcloud, "x"); + sensor_msgs::PointCloud2Iterator test3d_iter_y(test3d_pointcloud, "y"); + sensor_msgs::PointCloud2Iterator test3d_iter_z(test3d_pointcloud, "z"); + + i = 0; + oss.str(""); + oss.clear(); + + oss << "Expected pointcloud:\n"; + for (; test2d_iter_x != test2d_iter_x.end(); ++test2d_iter_x, ++test2d_iter_y, ++test2d_iter_z, + ++test3d_iter_x, ++test3d_iter_y, ++test3d_iter_z, + ++i) { + oss << "Point " << i << " - 2D: (" << *test2d_iter_x << ", " << *test2d_iter_y << ", " + << *test2d_iter_z << ")" + << " vs 3D: (" << *test3d_iter_x << ", " << *test3d_iter_y << ", " << *test3d_iter_z + << ")\n"; + EXPECT_NEAR(*test2d_iter_x, *test3d_iter_x, coarse_tolerance_); + EXPECT_NEAR(*test2d_iter_y, *test3d_iter_y, coarse_tolerance_); + EXPECT_NEAR(*test2d_iter_z, *test3d_iter_z, coarse_tolerance_); + } + + if (debug_) { + RCLCPP_INFO(node_->get_logger(), "%s", oss.str().c_str()); + } +} + +int main(int argc, char ** argv) +{ + ::testing::InitGoogleTest(&argc, argv); + rclcpp::init(argc, argv); + int ret = RUN_ALL_TESTS(); + rclcpp::shutdown(); + return ret; +} From 54a0c57ed29db912b8edc5d64d38f92d776fe5ac Mon Sep 17 00:00:00 2001 From: Kenzo Lobos Tsunekawa Date: Tue, 9 Jul 2024 22:17:40 +0900 Subject: [PATCH 8/8] feat: migrating pointcloud types (#6996) * feat: changed most of sensing to the new type Signed-off-by: Kenzo Lobos-Tsunekawa * chore: started applying changes to the perception stack Signed-off-by: Kenzo Lobos-Tsunekawa * feat: confirmed operation until centerpoint Signed-off-by: Kenzo Lobos-Tsunekawa * feat: reverted to the original implementation of pointpainting Signed-off-by: Kenzo Lobos-Tsunekawa * chore: forgot to push a header Signed-off-by: Kenzo Lobos-Tsunekawa * feat: also implemented the changes for the subsample filters that were out of scope before Signed-off-by: Kenzo Lobos-Tsunekawa * fix: some point type changes were missing from the latest merge from main Signed-off-by: Kenzo Lobos-Tsunekawa * chore: removed unused code, added comments, and brought back a removed publish Signed-off-by: Kenzo Lobos-Tsunekawa * chore: replaced pointcloud_raw for pointcloud_raw_ex to avoid extra processing time in the drivers Signed-off-by: Kenzo Lobos-Tsunekawa * feat: added memory layout checks Signed-off-by: Kenzo Lobos-Tsunekawa * chore: updated documentation regarding the point types Signed-off-by: Kenzo Lobos-Tsunekawa * chore: added hyperlinks to the point definitions. will be valid only once the PR is merged Signed-off-by: Kenzo Lobos-Tsunekawa * fix: fixed compilation due to moving the utilities files to the base library Signed-off-by: Kenzo Lobos-Tsunekawa * chore: separated the utilities functions due to a dependency issue * chore: forgot that perception also uses the filter class Signed-off-by: Kenzo Lobos-Tsunekawa * feature: adapted the undistortion tests to the new point type Signed-off-by: Kenzo Lobos-Tsunekawa --------- Signed-off-by: Kenzo Lobos-Tsunekawa Co-authored-by: kminoda <44218668+kminoda@users.noreply.github.com> Co-authored-by: badai nguyen <94814556+badai-nguyen@users.noreply.github.com> --- .../include/autoware_point_types/types.hpp | 91 +++++++- .../test/test_point_types.cpp | 9 + .../traffic_light.launch.xml | 2 +- .../src/scan_ground_filter/node.cpp | 4 +- .../src/scan_ground_filter/node.hpp | 1 + .../src/pointpainting_fusion/node.cpp | 6 +- .../src/detector.cpp | 24 +- .../src/lidar_apollo_segmentation_tvm.cpp | 26 ++- .../src/low_intensity_cluster_filter_node.cpp | 6 +- .../pointcloud_preprocessor/CMakeLists.txt | 14 +- .../docs/dual-return-outlier-filter.md | 4 +- .../docs/ring-outlier-filter.md | 11 +- .../concatenate_and_time_sync_nodelet.hpp | 4 +- .../concatenate_pointclouds.hpp | 4 +- .../faster_voxel_grid_downsample_filter.hpp | 4 +- .../ring_outlier_filter_nodelet.hpp | 14 +- .../polygon_remover/polygon_remover.hpp | 2 +- .../time_synchronizer_nodelet.hpp | 4 +- .../utility/{utilities.hpp => geometry.hpp} | 16 +- .../utility/memory.hpp | 40 ++++ .../vector_map_inside_area_filter.hpp | 4 +- .../blockage_diag/blockage_diag_nodelet.cpp | 10 +- .../concatenate_and_time_sync_nodelet.cpp | 63 ++++-- .../concatenate_pointclouds.cpp | 62 +++-- .../distortion_corrector.cpp | 43 +++- .../faster_voxel_grid_downsample_filter.cpp | 27 ++- .../voxel_grid_downsample_filter_nodelet.cpp | 2 +- .../pointcloud_preprocessor/src/filter.cpp | 26 +++ .../dual_return_outlier_filter_nodelet.cpp | 34 +-- .../ring_outlier_filter_nodelet.cpp | 149 ++++++------- .../time_synchronizer_nodelet.cpp | 47 ++-- .../utility/{utilities.cpp => geometry.cpp} | 93 +------- .../src/utility/memory.cpp | 211 ++++++++++++++++++ .../test/test_distortion_corrector_node.cpp | 35 +-- .../test/test_utilities.cpp | 4 +- 35 files changed, 767 insertions(+), 329 deletions(-) rename sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/utility/{utilities.hpp => geometry.hpp} (78%) create mode 100644 sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/utility/memory.hpp rename sensing/pointcloud_preprocessor/src/utility/{utilities.cpp => geometry.cpp} (52%) create mode 100644 sensing/pointcloud_preprocessor/src/utility/memory.cpp diff --git a/common/autoware_point_types/include/autoware_point_types/types.hpp b/common/autoware_point_types/include/autoware_point_types/types.hpp index 71ea80c5a9733..a3b0670280530 100644 --- a/common/autoware_point_types/include/autoware_point_types/types.hpp +++ b/common/autoware_point_types/include/autoware_point_types/types.hpp @@ -54,6 +54,23 @@ enum ReturnType : uint8_t { DUAL_ONLY, }; +struct PointXYZIRC +{ + float x{0.0F}; + float y{0.0F}; + float z{0.0F}; + std::uint8_t intensity{0U}; + std::uint8_t return_type{0U}; + std::uint16_t channel{0U}; + + friend bool operator==(const PointXYZIRC & p1, const PointXYZIRC & p2) noexcept + { + return float_eq(p1.x, p2.x) && float_eq(p1.y, p2.y) && + float_eq(p1.z, p2.z) && p1.intensity == p2.intensity && + p1.return_type == p2.return_type && p1.channel == p2.channel; + } +}; + struct PointXYZIRADRT { float x{0.0F}; @@ -75,25 +92,97 @@ struct PointXYZIRADRT } }; -enum class PointIndex { X, Y, Z, Intensity, Ring, Azimuth, Distance, ReturnType, TimeStamp }; +struct PointXYZIRCAEDT +{ + float x{0.0F}; + float y{0.0F}; + float z{0.0F}; + std::uint8_t intensity{0U}; + std::uint8_t return_type{0U}; + std::uint16_t channel{0U}; + float azimuth{0.0F}; + float elevation{0.0F}; + float distance{0.0F}; + std::uint32_t time_stamp{0U}; + + friend bool operator==(const PointXYZIRCAEDT & p1, const PointXYZIRCAEDT & p2) noexcept + { + return float_eq(p1.x, p2.x) && float_eq(p1.y, p2.y) && + float_eq(p1.z, p2.z) && p1.intensity == p2.intensity && + p1.return_type == p2.return_type && p1.channel == p2.channel && + float_eq(p1.azimuth, p2.azimuth) && float_eq(p1.distance, p2.distance) && + p1.time_stamp == p2.time_stamp; + } +}; + +enum class PointXYZIIndex { X, Y, Z, Intensity }; +enum class PointXYZIRCIndex { X, Y, Z, Intensity, ReturnType, Channel }; +enum class PointXYZIRADRTIndex { + X, + Y, + Z, + Intensity, + Ring, + Azimuth, + Distance, + ReturnType, + TimeStamp +}; +enum class PointXYZIRCAEDTIndex { + X, + Y, + Z, + Intensity, + ReturnType, + Channel, + Azimuth, + Elevation, + Distance, + TimeStamp +}; LIDAR_UTILS__DEFINE_FIELD_GENERATOR_FOR_MEMBER(azimuth); +LIDAR_UTILS__DEFINE_FIELD_GENERATOR_FOR_MEMBER(elevation); LIDAR_UTILS__DEFINE_FIELD_GENERATOR_FOR_MEMBER(distance); LIDAR_UTILS__DEFINE_FIELD_GENERATOR_FOR_MEMBER(return_type); LIDAR_UTILS__DEFINE_FIELD_GENERATOR_FOR_MEMBER(time_stamp); +LIDAR_UTILS__DEFINE_FIELD_GENERATOR_FOR_MEMBER(channel); + +using PointXYZIRCGenerator = std::tuple< + point_cloud_msg_wrapper::field_x_generator, point_cloud_msg_wrapper::field_y_generator, + point_cloud_msg_wrapper::field_z_generator, point_cloud_msg_wrapper::field_intensity_generator, + field_return_type_generator, field_channel_generator>; + using PointXYZIRADRTGenerator = std::tuple< point_cloud_msg_wrapper::field_x_generator, point_cloud_msg_wrapper::field_y_generator, point_cloud_msg_wrapper::field_z_generator, point_cloud_msg_wrapper::field_intensity_generator, point_cloud_msg_wrapper::field_ring_generator, field_azimuth_generator, field_distance_generator, field_return_type_generator, field_time_stamp_generator>; +using PointXYZIRCAEDTGenerator = std::tuple< + point_cloud_msg_wrapper::field_x_generator, point_cloud_msg_wrapper::field_y_generator, + point_cloud_msg_wrapper::field_z_generator, point_cloud_msg_wrapper::field_intensity_generator, + field_return_type_generator, field_channel_generator, field_azimuth_generator, + field_elevation_generator, field_distance_generator, field_time_stamp_generator>; + } // namespace autoware_point_types +POINT_CLOUD_REGISTER_POINT_STRUCT( + autoware_point_types::PointXYZIRC, + (float, x, x)(float, y, y)(float, z, z)(std::uint8_t, intensity, intensity)( + std::uint8_t, return_type, return_type)(std::uint16_t, channel, channel)) + POINT_CLOUD_REGISTER_POINT_STRUCT( autoware_point_types::PointXYZIRADRT, (float, x, x)(float, y, y)(float, z, z)(float, intensity, intensity)(std::uint16_t, ring, ring)( float, azimuth, azimuth)(float, distance, distance)(std::uint8_t, return_type, return_type)( double, time_stamp, time_stamp)) +POINT_CLOUD_REGISTER_POINT_STRUCT( + autoware_point_types::PointXYZIRCAEDT, + (float, x, x)(float, y, y)(float, z, z)(std::uint8_t, intensity, intensity)( + std::uint8_t, return_type, + return_type)(std::uint16_t, channel, channel)(float, azimuth, azimuth)( + float, elevation, elevation)(float, distance, distance)(std::uint32_t, time_stamp, time_stamp)) #endif // AUTOWARE_POINT_TYPES__TYPES_HPP_ diff --git a/common/autoware_point_types/test/test_point_types.cpp b/common/autoware_point_types/test/test_point_types.cpp index 27ab1ef993ab3..16887ac2aa498 100644 --- a/common/autoware_point_types/test/test_point_types.cpp +++ b/common/autoware_point_types/test/test_point_types.cpp @@ -36,6 +36,15 @@ TEST(PointEquality, PointXYZIRADRT) EXPECT_EQ(pt0, pt1); } +TEST(PointEquality, PointXYZIRCAEDT) +{ + using autoware_point_types::PointXYZIRCAEDT; + + PointXYZIRCAEDT pt0{0, 1, 2, 3, 4, 5, 6, 7, 8, 9}; + PointXYZIRCAEDT pt1{0, 1, 2, 3, 4, 5, 6, 7, 8, 9}; + EXPECT_EQ(pt0, pt1); +} + TEST(PointEquality, FloatEq) { // test template diff --git a/launch/tier4_perception_launch/launch/traffic_light_recognition/traffic_light.launch.xml b/launch/tier4_perception_launch/launch/traffic_light_recognition/traffic_light.launch.xml index ed7ff059a6e53..6cc47de76ea96 100644 --- a/launch/tier4_perception_launch/launch/traffic_light_recognition/traffic_light.launch.xml +++ b/launch/tier4_perception_launch/launch/traffic_light_recognition/traffic_light.launch.xml @@ -11,7 +11,7 @@ - + diff --git a/perception/ground_segmentation/src/scan_ground_filter/node.cpp b/perception/ground_segmentation/src/scan_ground_filter/node.cpp index 26db036f79bbe..dd867c3cfba0e 100644 --- a/perception/ground_segmentation/src/scan_ground_filter/node.cpp +++ b/perception/ground_segmentation/src/scan_ground_filter/node.cpp @@ -96,8 +96,9 @@ inline void ScanGroundFilterComponent::set_field_offsets(const PointCloud2ConstP int intensity_index = pcl::getFieldIndex(*input, "intensity"); if (intensity_index != -1) { intensity_offset_ = input->fields[intensity_index].offset; + intensity_type_ = input->fields[intensity_index].datatype; } else { - intensity_offset_ = z_offset_ + sizeof(float); + intensity_offset_ = -1; } offset_initialized_ = true; } @@ -569,6 +570,7 @@ void ScanGroundFilterComponent::extractObjectPoints( PointCloud2 & out_object_cloud) { size_t output_data_size = 0; + for (const auto & i : in_indices.indices) { std::memcpy( &out_object_cloud.data[output_data_size], &in_cloud_ptr->data[i * in_cloud_ptr->point_step], diff --git a/perception/ground_segmentation/src/scan_ground_filter/node.hpp b/perception/ground_segmentation/src/scan_ground_filter/node.hpp index 0e6ed598053ba..6de1d1b7ad09f 100644 --- a/perception/ground_segmentation/src/scan_ground_filter/node.hpp +++ b/perception/ground_segmentation/src/scan_ground_filter/node.hpp @@ -161,6 +161,7 @@ class ScanGroundFilterComponent : public pointcloud_preprocessor::Filter int y_offset_; int z_offset_; int intensity_offset_; + int intensity_type_; bool offset_initialized_; void set_field_offsets(const PointCloud2ConstPtr & input); diff --git a/perception/image_projection_based_fusion/src/pointpainting_fusion/node.cpp b/perception/image_projection_based_fusion/src/pointpainting_fusion/node.cpp index 0f3a50628ccbc..ffeb47d3123a3 100644 --- a/perception/image_projection_based_fusion/src/pointpainting_fusion/node.cpp +++ b/perception/image_projection_based_fusion/src/pointpainting_fusion/node.cpp @@ -289,13 +289,13 @@ void PointPaintingFusionNode::fuseOnSingleImage( // tf2::doTransform(painted_pointcloud_msg, transformed_pointcloud, transform_stamped); const auto x_offset = - painted_pointcloud_msg.fields.at(static_cast(autoware_point_types::PointIndex::X)) + painted_pointcloud_msg.fields.at(static_cast(autoware_point_types::PointXYZIRCIndex::X)) .offset; const auto y_offset = - painted_pointcloud_msg.fields.at(static_cast(autoware_point_types::PointIndex::Y)) + painted_pointcloud_msg.fields.at(static_cast(autoware_point_types::PointXYZIRCIndex::Y)) .offset; const auto z_offset = - painted_pointcloud_msg.fields.at(static_cast(autoware_point_types::PointIndex::Z)) + painted_pointcloud_msg.fields.at(static_cast(autoware_point_types::PointXYZIRCIndex::Z)) .offset; const auto class_offset = painted_pointcloud_msg.fields.at(4).offset; const auto p_step = painted_pointcloud_msg.point_step; diff --git a/perception/lidar_apollo_instance_segmentation/src/detector.cpp b/perception/lidar_apollo_instance_segmentation/src/detector.cpp index b704bab39338a..ba4d7f788b0f5 100644 --- a/perception/lidar_apollo_instance_segmentation/src/detector.cpp +++ b/perception/lidar_apollo_instance_segmentation/src/detector.cpp @@ -16,6 +16,8 @@ #include "lidar_apollo_instance_segmentation/feature_map.hpp" +#include + #include #include #include @@ -126,7 +128,27 @@ bool LidarApolloInstanceSegmentation::detectDynamicObjects( // convert from ros to pcl pcl::PointCloud::Ptr pcl_pointcloud_raw_ptr(new pcl::PointCloud); - pcl::fromROSMsg(transformed_cloud, *pcl_pointcloud_raw_ptr); + // pcl::fromROSMsg(transformed_cloud, *pcl_pointcloud_raw_ptr); + + auto pcl_pointcloud_raw = *pcl_pointcloud_raw_ptr; + pcl_pointcloud_raw.width = transformed_cloud.width; + pcl_pointcloud_raw.height = transformed_cloud.height; + pcl_pointcloud_raw.is_dense = transformed_cloud.is_dense == 1; + pcl_pointcloud_raw.resize(transformed_cloud.width * transformed_cloud.height); + + sensor_msgs::PointCloud2ConstIterator it_x(transformed_cloud, "x"); + sensor_msgs::PointCloud2ConstIterator it_y(transformed_cloud, "y"); + sensor_msgs::PointCloud2ConstIterator it_z(transformed_cloud, "z"); + sensor_msgs::PointCloud2ConstIterator it_intensity(transformed_cloud, "intensity"); + + for (; it_x != it_x.end(); ++it_x, ++it_y, ++it_z, ++it_intensity) { + pcl::PointXYZI point; + point.x = *it_x; + point.y = *it_y; + point.z = *it_z; + point.intensity = static_cast(*it_intensity); + pcl_pointcloud_raw.emplace_back(std::move(point)); + } // generate feature map std::shared_ptr feature_map_ptr = diff --git a/perception/lidar_apollo_segmentation_tvm/src/lidar_apollo_segmentation_tvm.cpp b/perception/lidar_apollo_segmentation_tvm/src/lidar_apollo_segmentation_tvm.cpp index 9d41770eb5331..c83e4836c1dd0 100644 --- a/perception/lidar_apollo_segmentation_tvm/src/lidar_apollo_segmentation_tvm.cpp +++ b/perception/lidar_apollo_segmentation_tvm/src/lidar_apollo_segmentation_tvm.cpp @@ -18,6 +18,8 @@ #include #include +#include + #include #include #include @@ -168,7 +170,29 @@ std::shared_ptr ApolloLidarSegmentation::detec sensor_msgs::msg::PointCloud2 transformed_cloud; ApolloLidarSegmentation::transformCloud(input, transformed_cloud, z_offset_); // convert from ros to pcl - pcl::fromROSMsg(transformed_cloud, *pcl_pointcloud_ptr_); + // pcl::fromROSMsg( + // transformed_cloud, *pcl_pointcloud_ptr_); // Manual conversion is needed since intensity + // comes as an uint8_t + + auto pcl_pointcloud = *pcl_pointcloud_ptr_; + pcl_pointcloud.width = input.width; + pcl_pointcloud.height = input.height; + pcl_pointcloud.is_dense = input.is_dense == 1; + pcl_pointcloud.resize(input.width * input.height); + + sensor_msgs::PointCloud2ConstIterator it_x(input, "x"); + sensor_msgs::PointCloud2ConstIterator it_y(input, "y"); + sensor_msgs::PointCloud2ConstIterator it_z(input, "z"); + sensor_msgs::PointCloud2ConstIterator it_intensity(input, "intensity"); + + for (; it_x != it_x.end(); ++it_x, ++it_y, ++it_z, ++it_intensity) { + pcl::PointXYZI point; + point.x = *it_x; + point.y = *it_y; + point.z = *it_z; + point.intensity = static_cast(*it_intensity); + pcl_pointcloud.emplace_back(std::move(point)); + } // inference pipeline auto output = pipeline->schedule(pcl_pointcloud_ptr_); diff --git a/perception/raindrop_cluster_filter/src/low_intensity_cluster_filter_node.cpp b/perception/raindrop_cluster_filter/src/low_intensity_cluster_filter_node.cpp index 105fc98722a26..62b6b9f3b09b5 100644 --- a/perception/raindrop_cluster_filter/src/low_intensity_cluster_filter_node.cpp +++ b/perception/raindrop_cluster_filter/src/low_intensity_cluster_filter_node.cpp @@ -125,9 +125,9 @@ bool LowIntensityClusterFilter::isValidatedCluster(const sensor_msgs::msg::Point RCLCPP_WARN(get_logger(), "Invalid point cloud data. point_step is less than 16."); return true; } - for (sensor_msgs::PointCloud2ConstIterator iter(cluster, "intensity"); iter != iter.end(); - ++iter) { - mean_intensity += *iter; + for (sensor_msgs::PointCloud2ConstIterator iter(cluster, "intensity"); + iter != iter.end(); ++iter) { + mean_intensity += static_cast(*iter); } const size_t num_points = cluster.width * cluster.height; mean_intensity /= static_cast(num_points); diff --git a/sensing/pointcloud_preprocessor/CMakeLists.txt b/sensing/pointcloud_preprocessor/CMakeLists.txt index 2d0649fe43954..76b9cc91ea586 100644 --- a/sensing/pointcloud_preprocessor/CMakeLists.txt +++ b/sensing/pointcloud_preprocessor/CMakeLists.txt @@ -26,11 +26,13 @@ include_directories( add_library(pointcloud_preprocessor_filter_base SHARED src/filter.cpp + src/utility/memory.cpp ) target_include_directories(pointcloud_preprocessor_filter_base PUBLIC "$" "$" + ${autoware_point_types_INCLUDE_DIRS} ) ament_target_dependencies(pointcloud_preprocessor_filter_base @@ -41,6 +43,7 @@ ament_target_dependencies(pointcloud_preprocessor_filter_base tf2_ros autoware_universe_utils pcl_ros + autoware_point_types ) add_library(faster_voxel_grid_downsample_filter SHARED @@ -59,7 +62,6 @@ ament_target_dependencies(faster_voxel_grid_downsample_filter ) ament_auto_add_library(pointcloud_preprocessor_filter SHARED - src/utility/utilities.cpp src/concatenate_data/concatenate_and_time_sync_nodelet.cpp src/concatenate_data/concatenate_pointclouds.cpp src/time_synchronizer/time_synchronizer_nodelet.cpp @@ -82,6 +84,7 @@ ament_auto_add_library(pointcloud_preprocessor_filter SHARED src/blockage_diag/blockage_diag_nodelet.cpp src/polygon_remover/polygon_remover.cpp src/vector_map_filter/vector_map_inside_area_filter.cpp + src/utility/geometry.cpp ) target_link_libraries(pointcloud_preprocessor_filter @@ -227,16 +230,19 @@ ament_auto_package(INSTALL_TO_SHARE if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) find_package(ament_cmake_gtest) + ament_lint_auto_find_test_dependencies() + ament_add_gtest(test_utilities test/test_utilities.cpp ) - ament_add_gtest(distortion_corrector_node_tests - test/test_distortion_corrector_node.cpp + + ament_add_gtest(test_distortion_corrector_node + test/test_distortion_corrector_node.cpp ) target_link_libraries(test_utilities pointcloud_preprocessor_filter) - target_link_libraries(distortion_corrector_node_tests pointcloud_preprocessor_filter) + target_link_libraries(test_distortion_corrector_node pointcloud_preprocessor_filter) endif() diff --git a/sensing/pointcloud_preprocessor/docs/dual-return-outlier-filter.md b/sensing/pointcloud_preprocessor/docs/dual-return-outlier-filter.md index c5a0b75e9b33b..d5993a803fa87 100644 --- a/sensing/pointcloud_preprocessor/docs/dual-return-outlier-filter.md +++ b/sensing/pointcloud_preprocessor/docs/dual-return-outlier-filter.md @@ -10,7 +10,7 @@ This node can remove rain and fog by considering the light reflected from the ob ![outlier_filter-return_type](./image/outlier_filter-return_type.drawio.svg) -Therefore, in order to use this node, the sensor driver must publish custom data including `return_type`. please refer to [PointXYZIRADRT](../../../common/autoware_point_types/include/autoware_point_types/types.hpp#L57-L76) data structure. +Therefore, in order to use this node, the sensor driver must publish custom data including `return_type`. please refer to [PointXYZIRCAEDT](../../../common/autoware_point_types/include/autoware_point_types/types.hpp#L95-L116) data structure. Another feature of this node is that it publishes visibility as a diagnostic topic. With this function, for example, in heavy rain, the sensing module can notify that the processing performance has reached its limit, which can lead to ensuring the safety of the vehicle. @@ -73,7 +73,7 @@ This implementation inherits `pointcloud_preprocessor::Filter` class, please ref ## Assumptions / Known limits Not recommended for use as it is under development. -Input data must be `PointXYZIRADRT` type data including `return_type`. +Input data must be [PointXYZIRCAEDT](../../../common/autoware_point_types/include/autoware_point_types/types.hpp#L95-L116) type data including `return_type`. ## (Optional) Error detection and handling diff --git a/sensing/pointcloud_preprocessor/docs/ring-outlier-filter.md b/sensing/pointcloud_preprocessor/docs/ring-outlier-filter.md index e87023ef00149..0a179ddf6a8ef 100644 --- a/sensing/pointcloud_preprocessor/docs/ring-outlier-filter.md +++ b/sensing/pointcloud_preprocessor/docs/ring-outlier-filter.md @@ -73,16 +73,7 @@ This implementation inherits `pointcloud_preprocessor::Filter` class, please ref ## Assumptions / Known limits -It is a prerequisite to input a scan point cloud in chronological order. In this repository it is defined as blow structure (please refer to [PointXYZIRADT](https://github.com/tier4/AutowareArchitectureProposal.iv/blob/5d8dff0db51634f0c42d2a3e87ca423fbee84348/sensing/preprocessor/pointcloud/pointcloud_preprocessor/include/pointcloud_preprocessor/outlier_filter/ring_outlier_filter_nodelet.hpp#L53-L62)). - -- X: x -- Y: y -- z: z -- I: intensity -- R: ring -- A :azimuth -- D: distance -- T: time_stamp +This nodes requires that the points of the input point cloud are in chronological order and that individual points follow the memory layout specified by [PointXYZIRCAEDT](../../../common/autoware_point_types/include/autoware_point_types/types.hpp#L95-L116). ## (Optional) Error detection and handling diff --git a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/concatenate_data/concatenate_and_time_sync_nodelet.hpp b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/concatenate_data/concatenate_and_time_sync_nodelet.hpp index 86c4ed5943540..619a0b8520b97 100644 --- a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/concatenate_data/concatenate_and_time_sync_nodelet.hpp +++ b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/concatenate_data/concatenate_and_time_sync_nodelet.hpp @@ -87,7 +87,7 @@ namespace pointcloud_preprocessor { -using autoware_point_types::PointXYZI; +using autoware_point_types::PointXYZIRC; using point_cloud_msg_wrapper::PointCloud2Modifier; /** \brief @b PointCloudConcatenateDataSynchronizerComponent is a special form of data @@ -169,7 +169,7 @@ class PointCloudConcatenateDataSynchronizerComponent : public rclcpp::Node sensor_msgs::msg::PointCloud2::SharedPtr & concat_cloud_ptr); void publish(); - void convertToXYZICloud( + void convertToXYZIRCCloud( const sensor_msgs::msg::PointCloud2::SharedPtr & input_ptr, sensor_msgs::msg::PointCloud2::SharedPtr & output_ptr); void setPeriod(const int64_t new_period); diff --git a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/concatenate_data/concatenate_pointclouds.hpp b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/concatenate_data/concatenate_pointclouds.hpp index ead66d98b8be7..77717c51e6981 100644 --- a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/concatenate_data/concatenate_pointclouds.hpp +++ b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/concatenate_data/concatenate_pointclouds.hpp @@ -86,7 +86,7 @@ namespace pointcloud_preprocessor { -using autoware_point_types::PointXYZI; +using autoware_point_types::PointXYZIRC; using point_cloud_msg_wrapper::PointCloud2Modifier; /** \brief @b PointCloudConcatenationComponent is a special form of data * synchronizer: it listens for a set of input PointCloud messages on the same topic, @@ -160,7 +160,7 @@ class PointCloudConcatenationComponent : public rclcpp::Node void combineClouds(sensor_msgs::msg::PointCloud2::SharedPtr & concat_cloud_ptr); void publish(); - void convertToXYZICloud( + void convertToXYZIRCCloud( const sensor_msgs::msg::PointCloud2::SharedPtr & input_ptr, sensor_msgs::msg::PointCloud2::SharedPtr & output_ptr); void setPeriod(const int64_t new_period); diff --git a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/downsample_filter/faster_voxel_grid_downsample_filter.hpp b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/downsample_filter/faster_voxel_grid_downsample_filter.hpp index d80e62d08330d..379c4c79e555a 100644 --- a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/downsample_filter/faster_voxel_grid_downsample_filter.hpp +++ b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/downsample_filter/faster_voxel_grid_downsample_filter.hpp @@ -34,7 +34,7 @@ class FasterVoxelGridDownsampleFilter public: FasterVoxelGridDownsampleFilter(); void set_voxel_size(float voxel_size_x, float voxel_size_y, float voxel_size_z); - void set_field_offsets(const PointCloud2ConstPtr & input); + void set_field_offsets(const PointCloud2ConstPtr & input, const rclcpp::Logger & logger); void filter( const PointCloud2ConstPtr & input, PointCloud2 & output, const TransformInfo & transform_info, const rclcpp::Logger & logger); @@ -48,7 +48,7 @@ class FasterVoxelGridDownsampleFilter float intensity; uint32_t point_count_; - Centroid() : x(0), y(0), z(0), intensity(-1.0) {} + Centroid() : x(0), y(0), z(0), intensity(0) {} Centroid(float _x, float _y, float _z, float _intensity) : x(_x), y(_y), z(_z), intensity(_intensity) { diff --git a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/outlier_filter/ring_outlier_filter_nodelet.hpp b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/outlier_filter/ring_outlier_filter_nodelet.hpp index 55ba79aac4593..08fbfe73f2744 100644 --- a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/outlier_filter/ring_outlier_filter_nodelet.hpp +++ b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/outlier_filter/ring_outlier_filter_nodelet.hpp @@ -35,12 +35,15 @@ namespace pointcloud_preprocessor { -using autoware_point_types::PointXYZI; using point_cloud_msg_wrapper::PointCloud2Modifier; class RingOutlierFilterComponent : public pointcloud_preprocessor::Filter { protected: + using InputPointIndex = autoware_point_types::PointXYZIRCAEDTIndex; + using InputPointType = autoware_point_types::PointXYZIRCAEDT; + using OutputPointType = autoware_point_types::PointXYZIRC; + virtual void filter( const PointCloud2ConstPtr & input, const IndicesPtr & indices, PointCloud2 & output); @@ -83,8 +86,10 @@ class RingOutlierFilterComponent : public pointcloud_preprocessor::Filter { if (walk_size > num_points_threshold_) return true; - auto first_point = reinterpret_cast(&input->data[data_idx_both_ends.first]); - auto last_point = reinterpret_cast(&input->data[data_idx_both_ends.second]); + auto first_point = + reinterpret_cast(&input->data[data_idx_both_ends.first]); + auto last_point = + reinterpret_cast(&input->data[data_idx_both_ends.second]); const auto x = first_point->x - last_point->x; const auto y = first_point->y - last_point->y; @@ -94,8 +99,7 @@ class RingOutlierFilterComponent : public pointcloud_preprocessor::Filter } void setUpPointCloudFormat( - const PointCloud2ConstPtr & input, PointCloud2 & formatted_points, size_t points_size, - size_t num_fields); + const PointCloud2ConstPtr & input, PointCloud2 & formatted_points, size_t points_size); float calculateVisibilityScore(const PointCloud2 & input); public: diff --git a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/polygon_remover/polygon_remover.hpp b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/polygon_remover/polygon_remover.hpp index 49baed4ed7ed5..4d2e66eea700e 100644 --- a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/polygon_remover/polygon_remover.hpp +++ b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/polygon_remover/polygon_remover.hpp @@ -16,7 +16,7 @@ #define POINTCLOUD_PREPROCESSOR__POLYGON_REMOVER__POLYGON_REMOVER_HPP_ #include "pointcloud_preprocessor/filter.hpp" -#include "pointcloud_preprocessor/utility/utilities.hpp" +#include "pointcloud_preprocessor/utility/geometry.hpp" #include #include diff --git a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/time_synchronizer/time_synchronizer_nodelet.hpp b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/time_synchronizer/time_synchronizer_nodelet.hpp index 4153cee7695f8..2dc98a571ff2c 100644 --- a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/time_synchronizer/time_synchronizer_nodelet.hpp +++ b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/time_synchronizer/time_synchronizer_nodelet.hpp @@ -87,7 +87,7 @@ namespace pointcloud_preprocessor { -using autoware_point_types::PointXYZI; +using autoware_point_types::PointXYZIRC; using point_cloud_msg_wrapper::PointCloud2Modifier; // cspell:ignore Yoshi /** \brief @b PointCloudDataSynchronizerComponent is a special form of data @@ -166,7 +166,7 @@ class PointCloudDataSynchronizerComponent : public rclcpp::Node std::map synchronizeClouds(); void publish(); - void convertToXYZICloud( + void convertToXYZIRCCloud( const sensor_msgs::msg::PointCloud2::SharedPtr & input_ptr, sensor_msgs::msg::PointCloud2::SharedPtr & output_ptr); void setPeriod(const int64_t new_period); diff --git a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/utility/utilities.hpp b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/utility/geometry.hpp similarity index 78% rename from sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/utility/utilities.hpp rename to sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/utility/geometry.hpp index 654057e7bb8d7..0175f88ceb89c 100644 --- a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/utility/utilities.hpp +++ b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/utility/geometry.hpp @@ -1,4 +1,4 @@ -// Copyright 2022 Tier IV, Inc. +// Copyright 2024 TIER IV, Inc. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef POINTCLOUD_PREPROCESSOR__UTILITY__UTILITIES_HPP_ -#define POINTCLOUD_PREPROCESSOR__UTILITY__UTILITIES_HPP_ +#ifndef POINTCLOUD_PREPROCESSOR__UTILITY__GEOMETRY_HPP_ +#define POINTCLOUD_PREPROCESSOR__UTILITY__GEOMETRY_HPP_ #include #include @@ -81,14 +81,6 @@ void remove_polygon_cgal_from_cloud( bool point_within_cgal_polys( const pcl::PointXYZ & point, const std::vector & polyline_polygons); -/** \brief Return whether the input PointCloud2 data has the same layout than PointXYZI. That is to - * say whether you can memcpy from the PointCloud2 data buffer to a PointXYZI */ -bool is_data_layout_compatible_with_PointXYZI(const sensor_msgs::msg::PointCloud2 & input); - -/** \brief Return whether the input PointCloud2 data has the same layout than PointXYZIRADRT. That - * is to say whether you can memcpy from the PointCloud2 data buffer to a PointXYZIRADRT */ -bool is_data_layout_compatible_with_PointXYZIRADRT(const sensor_msgs::msg::PointCloud2 & input); - } // namespace pointcloud_preprocessor::utils -#endif // POINTCLOUD_PREPROCESSOR__UTILITY__UTILITIES_HPP_ +#endif // POINTCLOUD_PREPROCESSOR__UTILITY__GEOMETRY_HPP_ diff --git a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/utility/memory.hpp b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/utility/memory.hpp new file mode 100644 index 0000000000000..ef87a4f31457b --- /dev/null +++ b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/utility/memory.hpp @@ -0,0 +1,40 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef POINTCLOUD_PREPROCESSOR__UTILITY__MEMORY_HPP_ +#define POINTCLOUD_PREPROCESSOR__UTILITY__MEMORY_HPP_ + +#include + +namespace pointcloud_preprocessor::utils +{ +/** \brief Return whether the input PointCloud2 data has the same layout than PointXYZI. That is to + * say whether you can memcpy from the PointCloud2 data buffer to a PointXYZI */ +bool is_data_layout_compatible_with_point_xyzi(const sensor_msgs::msg::PointCloud2 & input); + +/** \brief Return whether the input PointCloud2 data has the same layout than PointXYZIRC. That is + * to say whether you can memcpy from the PointCloud2 data buffer to a PointXYZIRC */ +bool is_data_layout_compatible_with_point_xyzirc(const sensor_msgs::msg::PointCloud2 & input); + +/** \brief Return whether the input PointCloud2 data has the same layout than PointXYZIRADRT. That + * is to say whether you can memcpy from the PointCloud2 data buffer to a PointXYZIRADRT */ +bool is_data_layout_compatible_with_point_xyziradrt(const sensor_msgs::msg::PointCloud2 & input); + +/** \brief Return whether the input PointCloud2 data has the same layout than PointXYZIRCAEDT. That + * is to say whether you can memcpy from the PointCloud2 data buffer to a PointXYZIRCAEDT */ +bool is_data_layout_compatible_with_point_xyzircaedt(const sensor_msgs::msg::PointCloud2 & input); + +} // namespace pointcloud_preprocessor::utils + +#endif // POINTCLOUD_PREPROCESSOR__UTILITY__MEMORY_HPP_ diff --git a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/vector_map_filter/vector_map_inside_area_filter.hpp b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/vector_map_filter/vector_map_inside_area_filter.hpp index 7f48497d7e578..d28a6550123cf 100644 --- a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/vector_map_filter/vector_map_inside_area_filter.hpp +++ b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/vector_map_filter/vector_map_inside_area_filter.hpp @@ -1,4 +1,4 @@ -// Copyright 2022 Tier IV, Inc. +// Copyright 2024 TIER IV, Inc. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -16,7 +16,7 @@ #define POINTCLOUD_PREPROCESSOR__VECTOR_MAP_FILTER__VECTOR_MAP_INSIDE_AREA_FILTER_HPP_ #include "pointcloud_preprocessor/filter.hpp" -#include "pointcloud_preprocessor/utility/utilities.hpp" +#include "pointcloud_preprocessor/utility/geometry.hpp" #include #include diff --git a/sensing/pointcloud_preprocessor/src/blockage_diag/blockage_diag_nodelet.cpp b/sensing/pointcloud_preprocessor/src/blockage_diag/blockage_diag_nodelet.cpp index 773e6db056990..aae50a18f2024 100644 --- a/sensing/pointcloud_preprocessor/src/blockage_diag/blockage_diag_nodelet.cpp +++ b/sensing/pointcloud_preprocessor/src/blockage_diag/blockage_diag_nodelet.cpp @@ -21,7 +21,7 @@ namespace pointcloud_preprocessor { -using autoware_point_types::PointXYZIRADRT; +using autoware_point_types::PointXYZIRCAEDT; using diagnostic_msgs::msg::DiagnosticStatus; BlockageDiagComponent::BlockageDiagComponent(const rclcpp::NodeOptions & options) @@ -175,7 +175,7 @@ void BlockageDiagComponent::filter( } ideal_horizontal_bins = static_cast( (angle_range_deg_[1] + compensate_angle - angle_range_deg_[0]) / horizontal_resolution_); - pcl::PointCloud::Ptr pcl_input(new pcl::PointCloud); + pcl::PointCloud::Ptr pcl_input(new pcl::PointCloud); pcl::fromROSMsg(*input, *pcl_input); cv::Mat full_size_depth_map( cv::Size(ideal_horizontal_bins, vertical_bins), CV_16UC1, cv::Scalar(0)); @@ -196,7 +196,7 @@ void BlockageDiagComponent::filter( sky_blockage_range_deg_[1] = angle_range_deg_[1]; } else { for (const auto p : pcl_input->points) { - double azimuth_deg = p.azimuth / 100.; + double azimuth_deg = p.azimuth * (180.0 / M_PI); if ( ((azimuth_deg > angle_range_deg_[0]) && (azimuth_deg <= angle_range_deg_[1] + compensate_angle)) || @@ -208,9 +208,9 @@ void BlockageDiagComponent::filter( uint16_t depth_intensity = UINT16_MAX * (1.0 - std::min(p.distance / max_distance_range_, 1.0)); if (is_channel_order_top2down_) { - full_size_depth_map.at(p.ring, horizontal_bin_index) = depth_intensity; + full_size_depth_map.at(p.channel, horizontal_bin_index) = depth_intensity; } else { - full_size_depth_map.at(vertical_bins - p.ring - 1, horizontal_bin_index) = + full_size_depth_map.at(vertical_bins - p.channel - 1, horizontal_bin_index) = depth_intensity; } } diff --git a/sensing/pointcloud_preprocessor/src/concatenate_data/concatenate_and_time_sync_nodelet.cpp b/sensing/pointcloud_preprocessor/src/concatenate_data/concatenate_and_time_sync_nodelet.cpp index 8799a70c01736..c550c9dfb4933 100644 --- a/sensing/pointcloud_preprocessor/src/concatenate_data/concatenate_and_time_sync_nodelet.cpp +++ b/sensing/pointcloud_preprocessor/src/concatenate_data/concatenate_and_time_sync_nodelet.cpp @@ -51,6 +51,8 @@ #include "pointcloud_preprocessor/concatenate_data/concatenate_and_time_sync_nodelet.hpp" +#include "pointcloud_preprocessor/utility/memory.hpp" + #include #include @@ -491,40 +493,56 @@ void PointCloudConcatenateDataSynchronizerComponent::publish() } /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// -void PointCloudConcatenateDataSynchronizerComponent::convertToXYZICloud( +void PointCloudConcatenateDataSynchronizerComponent::convertToXYZIRCCloud( const sensor_msgs::msg::PointCloud2::SharedPtr & input_ptr, sensor_msgs::msg::PointCloud2::SharedPtr & output_ptr) { output_ptr->header = input_ptr->header; - PointCloud2Modifier output_modifier{*output_ptr, input_ptr->header.frame_id}; + PointCloud2Modifier output_modifier{ + *output_ptr, input_ptr->header.frame_id}; output_modifier.reserve(input_ptr->width); - bool has_intensity = std::any_of( - input_ptr->fields.begin(), input_ptr->fields.end(), - [](auto & field) { return field.name == "intensity"; }); + bool has_valid_intensity = + std::any_of(input_ptr->fields.begin(), input_ptr->fields.end(), [](auto & field) { + return field.name == "intensity" && field.datatype == sensor_msgs::msg::PointField::UINT8; + }); + + bool has_valid_return_type = + std::any_of(input_ptr->fields.begin(), input_ptr->fields.end(), [](auto & field) { + return field.name == "return_type" && field.datatype == sensor_msgs::msg::PointField::UINT8; + }); + + bool has_valid_channel = + std::any_of(input_ptr->fields.begin(), input_ptr->fields.end(), [](auto & field) { + return field.name == "channel" && field.datatype == sensor_msgs::msg::PointField::UINT16; + }); sensor_msgs::PointCloud2Iterator it_x(*input_ptr, "x"); sensor_msgs::PointCloud2Iterator it_y(*input_ptr, "y"); sensor_msgs::PointCloud2Iterator it_z(*input_ptr, "z"); - if (has_intensity) { - sensor_msgs::PointCloud2Iterator it_i(*input_ptr, "intensity"); - for (; it_x != it_x.end(); ++it_x, ++it_y, ++it_z, ++it_i) { - PointXYZI point; + if (has_valid_intensity && has_valid_return_type && has_valid_channel) { + sensor_msgs::PointCloud2Iterator it_i(*input_ptr, "intensity"); + sensor_msgs::PointCloud2Iterator it_r(*input_ptr, "return_type"); + sensor_msgs::PointCloud2Iterator it_c(*input_ptr, "channel"); + + for (; it_x != it_x.end(); ++it_x, ++it_y, ++it_z, ++it_i, ++it_r, ++it_c) { + PointXYZIRC point; point.x = *it_x; point.y = *it_y; point.z = *it_z; point.intensity = *it_i; + point.return_type = *it_r; + point.channel = *it_c; output_modifier.push_back(std::move(point)); } } else { for (; it_x != it_x.end(); ++it_x, ++it_y, ++it_z) { - PointXYZI point; + PointXYZIRC point; point.x = *it_x; point.y = *it_y; point.z = *it_z; - point.intensity = 0.0f; output_modifier.push_back(std::move(point)); } } @@ -549,15 +567,28 @@ void PointCloudConcatenateDataSynchronizerComponent::setPeriod(const int64_t new void PointCloudConcatenateDataSynchronizerComponent::cloud_callback( const sensor_msgs::msg::PointCloud2::ConstSharedPtr & input_ptr, const std::string & topic_name) { + if (!utils::is_data_layout_compatible_with_point_xyzirc(*input_ptr)) { + RCLCPP_ERROR( + get_logger(), "The pointcloud layout is not compatible with PointXYZIRC. Aborting"); + + if (utils::is_data_layout_compatible_with_point_xyzi(*input_ptr)) { + RCLCPP_ERROR( + get_logger(), + "The pointcloud layout is compatible with PointXYZI. You may be using legacy code/data"); + } + + return; + } + std::lock_guard lock(mutex_); - sensor_msgs::msg::PointCloud2::SharedPtr xyzi_input_ptr(new sensor_msgs::msg::PointCloud2()); + sensor_msgs::msg::PointCloud2::SharedPtr xyzirc_input_ptr(new sensor_msgs::msg::PointCloud2()); auto input = std::make_shared(*input_ptr); if (input->data.empty()) { RCLCPP_WARN_STREAM_THROTTLE( this->get_logger(), *this->get_clock(), 1000, "Empty sensor points!"); } else { - // convert to XYZI pointcloud if pointcloud is not empty - convertToXYZICloud(input, xyzi_input_ptr); + // convert to XYZIRC pointcloud if pointcloud is not empty + convertToXYZIRCCloud(input, xyzirc_input_ptr); } const bool is_already_subscribed_this = (cloud_stdmap_[topic_name] != nullptr); @@ -566,7 +597,7 @@ void PointCloudConcatenateDataSynchronizerComponent::cloud_callback( [](const auto & e) { return e.second != nullptr; }); if (is_already_subscribed_this) { - cloud_stdmap_tmp_[topic_name] = xyzi_input_ptr; + cloud_stdmap_tmp_[topic_name] = xyzirc_input_ptr; if (!is_already_subscribed_tmp) { auto period = std::chrono::duration_cast( @@ -579,7 +610,7 @@ void PointCloudConcatenateDataSynchronizerComponent::cloud_callback( timer_->reset(); } } else { - cloud_stdmap_[topic_name] = xyzi_input_ptr; + cloud_stdmap_[topic_name] = xyzirc_input_ptr; const bool is_subscribed_all = std::all_of( std::begin(cloud_stdmap_), std::end(cloud_stdmap_), diff --git a/sensing/pointcloud_preprocessor/src/concatenate_data/concatenate_pointclouds.cpp b/sensing/pointcloud_preprocessor/src/concatenate_data/concatenate_pointclouds.cpp index 89e058d11bb53..d1f02f5f0bbf1 100644 --- a/sensing/pointcloud_preprocessor/src/concatenate_data/concatenate_pointclouds.cpp +++ b/sensing/pointcloud_preprocessor/src/concatenate_data/concatenate_pointclouds.cpp @@ -14,6 +14,8 @@ #include "pointcloud_preprocessor/concatenate_data/concatenate_pointclouds.hpp" +#include "pointcloud_preprocessor/utility/memory.hpp" + #include #include @@ -325,39 +327,56 @@ void PointCloudConcatenationComponent::publish() } /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// -void PointCloudConcatenationComponent::convertToXYZICloud( +void PointCloudConcatenationComponent::convertToXYZIRCCloud( const sensor_msgs::msg::PointCloud2::SharedPtr & input_ptr, sensor_msgs::msg::PointCloud2::SharedPtr & output_ptr) { output_ptr->header = input_ptr->header; - PointCloud2Modifier output_modifier{*output_ptr, input_ptr->header.frame_id}; + + PointCloud2Modifier output_modifier{ + *output_ptr, input_ptr->header.frame_id}; output_modifier.reserve(input_ptr->width); - bool has_intensity = std::any_of( - input_ptr->fields.begin(), input_ptr->fields.end(), - [](auto & field) { return field.name == "intensity"; }); + bool has_valid_intensity = + std::any_of(input_ptr->fields.begin(), input_ptr->fields.end(), [](auto & field) { + return field.name == "intensity" && field.datatype == sensor_msgs::msg::PointField::UINT8; + }); + + bool has_valid_return_type = + std::any_of(input_ptr->fields.begin(), input_ptr->fields.end(), [](auto & field) { + return field.name == "return_type" && field.datatype == sensor_msgs::msg::PointField::UINT8; + }); + + bool has_valid_channel = + std::any_of(input_ptr->fields.begin(), input_ptr->fields.end(), [](auto & field) { + return field.name == "channel" && field.datatype == sensor_msgs::msg::PointField::UINT16; + }); sensor_msgs::PointCloud2Iterator it_x(*input_ptr, "x"); sensor_msgs::PointCloud2Iterator it_y(*input_ptr, "y"); sensor_msgs::PointCloud2Iterator it_z(*input_ptr, "z"); - if (has_intensity) { - sensor_msgs::PointCloud2Iterator it_i(*input_ptr, "intensity"); - for (; it_x != it_x.end(); ++it_x, ++it_y, ++it_z, ++it_i) { - PointXYZI point; + if (has_valid_intensity && has_valid_return_type && has_valid_channel) { + sensor_msgs::PointCloud2Iterator it_i(*input_ptr, "intensity"); + sensor_msgs::PointCloud2Iterator it_r(*input_ptr, "return_type"); + sensor_msgs::PointCloud2Iterator it_c(*input_ptr, "channel"); + + for (; it_x != it_x.end(); ++it_x, ++it_y, ++it_z, ++it_i, ++it_r, ++it_c) { + PointXYZIRC point; point.x = *it_x; point.y = *it_y; point.z = *it_z; point.intensity = *it_i; + point.return_type = *it_r; + point.channel = *it_c; output_modifier.push_back(std::move(point)); } } else { for (; it_x != it_x.end(); ++it_x, ++it_y, ++it_z) { - PointXYZI point; + PointXYZIRC point; point.x = *it_x; point.y = *it_y; point.z = *it_z; - point.intensity = 0.0f; output_modifier.push_back(std::move(point)); } } @@ -382,10 +401,23 @@ void PointCloudConcatenationComponent::setPeriod(const int64_t new_period) void PointCloudConcatenationComponent::cloud_callback( const sensor_msgs::msg::PointCloud2::ConstSharedPtr & input_ptr, const std::string & topic_name) { + if (!utils::is_data_layout_compatible_with_point_xyzirc(*input_ptr)) { + RCLCPP_ERROR( + get_logger(), "The pointcloud layout is not compatible with PointXYZIRC. Aborting"); + + if (utils::is_data_layout_compatible_with_point_xyzi(*input_ptr)) { + RCLCPP_ERROR( + get_logger(), + "The pointcloud layout is compatible with PointXYZI. You may be using legacy code/data"); + } + + return; + } + std::lock_guard lock(mutex_); auto input = std::make_shared(*input_ptr); - sensor_msgs::msg::PointCloud2::SharedPtr xyzi_input_ptr(new sensor_msgs::msg::PointCloud2()); - convertToXYZICloud(input, xyzi_input_ptr); + sensor_msgs::msg::PointCloud2::SharedPtr xyzirc_input_ptr(new sensor_msgs::msg::PointCloud2()); + convertToXYZIRCCloud(input, xyzirc_input_ptr); const bool is_already_subscribed_this = (cloud_stdmap_[topic_name] != nullptr); const bool is_already_subscribed_tmp = std::any_of( @@ -393,7 +425,7 @@ void PointCloudConcatenationComponent::cloud_callback( [](const auto & e) { return e.second != nullptr; }); if (is_already_subscribed_this) { - cloud_stdmap_tmp_[topic_name] = xyzi_input_ptr; + cloud_stdmap_tmp_[topic_name] = xyzirc_input_ptr; if (!is_already_subscribed_tmp) { auto period = std::chrono::duration_cast( @@ -406,7 +438,7 @@ void PointCloudConcatenationComponent::cloud_callback( timer_->reset(); } } else { - cloud_stdmap_[topic_name] = xyzi_input_ptr; + cloud_stdmap_[topic_name] = xyzirc_input_ptr; const bool is_subscribed_all = std::all_of( std::begin(cloud_stdmap_), std::end(cloud_stdmap_), diff --git a/sensing/pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp b/sensing/pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp index f933d4c6f75d9..4a7152a3cd38a 100644 --- a/sensing/pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp +++ b/sensing/pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp @@ -14,8 +14,9 @@ #include "pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp" -#include "autoware/universe_utils/math/trigonometry.hpp" +#include "pointcloud_preprocessor/utility/memory.hpp" +#include #include namespace pointcloud_preprocessor @@ -181,6 +182,22 @@ bool DistortionCorrector::isInputValid(sensor_msgs::msg::PointCloud2 & pointc "Required field time stamp doesn't exist in the point cloud."); return false; } + + if (!utils::is_data_layout_compatible_with_point_xyzircaedt(pointcloud)) { + RCLCPP_ERROR( + node_->get_logger(), + "The pointcloud layout is not compatible with PointXYZIRCAEDT. Aborting"); + + if (utils::is_data_layout_compatible_with_point_xyziradrt(pointcloud)) { + RCLCPP_ERROR( + node_->get_logger(), + "The pointcloud layout is compatible with PointXYZIRADRT. You may be using legacy " + "code/data"); + } + + return false; + } + return true; } @@ -193,10 +210,12 @@ void DistortionCorrector::undistortPointCloud( sensor_msgs::PointCloud2Iterator it_x(pointcloud, "x"); sensor_msgs::PointCloud2Iterator it_y(pointcloud, "y"); sensor_msgs::PointCloud2Iterator it_z(pointcloud, "z"); - sensor_msgs::PointCloud2ConstIterator it_time_stamp(pointcloud, "time_stamp"); + sensor_msgs::PointCloud2ConstIterator it_time_stamp(pointcloud, "time_stamp"); - double prev_time_stamp_sec{*it_time_stamp}; - const double first_point_time_stamp_sec{*it_time_stamp}; + double prev_time_stamp_sec{ + pointcloud.header.stamp.sec + 1e-9 * (pointcloud.header.stamp.nanosec + *it_time_stamp)}; + const double first_point_time_stamp_sec{ + pointcloud.header.stamp.sec + 1e-9 * (pointcloud.header.stamp.nanosec + *it_time_stamp)}; std::deque::iterator it_twist; std::deque::iterator it_imu; @@ -214,29 +233,33 @@ void DistortionCorrector::undistortPointCloud( bool is_imu_time_stamp_too_late = false; bool is_twist_valid = true; bool is_imu_valid = true; + double global_point_stamp; for (; it_x != it_x.end(); ++it_x, ++it_y, ++it_z, ++it_time_stamp) { is_twist_valid = true; is_imu_valid = true; + global_point_stamp = + pointcloud.header.stamp.sec + 1e-9 * (pointcloud.header.stamp.nanosec + *it_time_stamp); + // Get closest twist information - while (it_twist != std::end(twist_queue_) - 1 && *it_time_stamp > twist_stamp) { + while (it_twist != std::end(twist_queue_) - 1 && global_point_stamp > twist_stamp) { ++it_twist; twist_stamp = rclcpp::Time(it_twist->header.stamp).seconds(); } - if (std::abs(*it_time_stamp - twist_stamp) > 0.1) { + if (std::abs(global_point_stamp - twist_stamp) > 0.1) { is_twist_time_stamp_too_late = true; is_twist_valid = false; } // Get closest IMU information if (use_imu && !angular_velocity_queue_.empty()) { - while (it_imu != std::end(angular_velocity_queue_) - 1 && *it_time_stamp > imu_stamp) { + while (it_imu != std::end(angular_velocity_queue_) - 1 && global_point_stamp > imu_stamp) { ++it_imu; imu_stamp = rclcpp::Time(it_imu->header.stamp).seconds(); } - if (std::abs(*it_time_stamp - imu_stamp) > 0.1) { + if (std::abs(global_point_stamp - imu_stamp) > 0.1) { is_imu_time_stamp_too_late = true; is_imu_valid = false; } @@ -244,12 +267,12 @@ void DistortionCorrector::undistortPointCloud( is_imu_valid = false; } - float time_offset = static_cast(*it_time_stamp - prev_time_stamp_sec); + float time_offset = static_cast(global_point_stamp - prev_time_stamp_sec); // Undistort a single point based on the strategy undistortPoint(it_x, it_y, it_z, it_twist, it_imu, time_offset, is_twist_valid, is_imu_valid); - prev_time_stamp_sec = *it_time_stamp; + prev_time_stamp_sec = global_point_stamp; } warnIfTimestampIsTooLate(is_twist_time_stamp_too_late, is_imu_time_stamp_too_late); diff --git a/sensing/pointcloud_preprocessor/src/downsample_filter/faster_voxel_grid_downsample_filter.cpp b/sensing/pointcloud_preprocessor/src/downsample_filter/faster_voxel_grid_downsample_filter.cpp index 0ca28f5f9a455..b9488d1ed7a73 100644 --- a/sensing/pointcloud_preprocessor/src/downsample_filter/faster_voxel_grid_downsample_filter.cpp +++ b/sensing/pointcloud_preprocessor/src/downsample_filter/faster_voxel_grid_downsample_filter.cpp @@ -29,16 +29,27 @@ void FasterVoxelGridDownsampleFilter::set_voxel_size( Eigen::Array3f::Ones() / Eigen::Array3f(voxel_size_x, voxel_size_y, voxel_size_z); } -void FasterVoxelGridDownsampleFilter::set_field_offsets(const PointCloud2ConstPtr & input) +void FasterVoxelGridDownsampleFilter::set_field_offsets( + const PointCloud2ConstPtr & input, const rclcpp::Logger & logger) { x_offset_ = input->fields[pcl::getFieldIndex(*input, "x")].offset; y_offset_ = input->fields[pcl::getFieldIndex(*input, "y")].offset; z_offset_ = input->fields[pcl::getFieldIndex(*input, "z")].offset; intensity_index_ = pcl::getFieldIndex(*input, "intensity"); + + if ( + intensity_index_ < 0 || input->fields[pcl::getFieldIndex(*input, "intensity")].datatype != + sensor_msgs::msg::PointField::UINT8) { + RCLCPP_ERROR( + logger, + "There is no intensity field in the input point cloud or the intensity field is not of type " + "UINT8."); + } + if (intensity_index_ != -1) { intensity_offset_ = input->fields[intensity_index_].offset; } else { - intensity_offset_ = z_offset_ + sizeof(float); + intensity_offset_ = -1; } offset_initialized_ = true; } @@ -49,7 +60,7 @@ void FasterVoxelGridDownsampleFilter::filter( { // Check if the field offset has been set if (!offset_initialized_) { - set_field_offsets(input); + set_field_offsets(input, logger); } // Compute the minimum and maximum voxel coordinates @@ -85,10 +96,9 @@ Eigen::Vector4f FasterVoxelGridDownsampleFilter::get_point_from_global_offset( const PointCloud2ConstPtr & input, size_t global_offset) { float intensity = 0.0; - if (intensity_index_ == -1) { - intensity = -1.0; - } else { - intensity = *reinterpret_cast(&input->data[global_offset + intensity_offset_]); + if (intensity_index_ >= 0) { + intensity = static_cast( + *reinterpret_cast(&input->data[global_offset + intensity_offset_])); } Eigen::Vector4f point( *reinterpret_cast(&input->data[global_offset + x_offset_]), @@ -179,7 +189,8 @@ void FasterVoxelGridDownsampleFilter::copy_centroids_to_output( *reinterpret_cast(&output.data[output_data_size + x_offset_]) = centroid[0]; *reinterpret_cast(&output.data[output_data_size + y_offset_]) = centroid[1]; *reinterpret_cast(&output.data[output_data_size + z_offset_]) = centroid[2]; - *reinterpret_cast(&output.data[output_data_size + intensity_offset_]) = centroid[3]; + *reinterpret_cast(&output.data[output_data_size + intensity_offset_]) = + static_cast(centroid[3]); output_data_size += output.point_step; } } diff --git a/sensing/pointcloud_preprocessor/src/downsample_filter/voxel_grid_downsample_filter_nodelet.cpp b/sensing/pointcloud_preprocessor/src/downsample_filter/voxel_grid_downsample_filter_nodelet.cpp index f11f37397a142..6e7469ff05f67 100644 --- a/sensing/pointcloud_preprocessor/src/downsample_filter/voxel_grid_downsample_filter_nodelet.cpp +++ b/sensing/pointcloud_preprocessor/src/downsample_filter/voxel_grid_downsample_filter_nodelet.cpp @@ -108,7 +108,7 @@ void VoxelGridDownsampleFilterComponent::faster_filter( std::scoped_lock lock(mutex_); FasterVoxelGridDownsampleFilter faster_voxel_filter; faster_voxel_filter.set_voxel_size(voxel_size_x_, voxel_size_y_, voxel_size_z_); - faster_voxel_filter.set_field_offsets(input); + faster_voxel_filter.set_field_offsets(input, this->get_logger()); faster_voxel_filter.filter(input, output, transform_info, this->get_logger()); } diff --git a/sensing/pointcloud_preprocessor/src/filter.cpp b/sensing/pointcloud_preprocessor/src/filter.cpp index 035be38f4c342..a0b1814995d8b 100644 --- a/sensing/pointcloud_preprocessor/src/filter.cpp +++ b/sensing/pointcloud_preprocessor/src/filter.cpp @@ -51,6 +51,8 @@ #include "pointcloud_preprocessor/filter.hpp" +#include "pointcloud_preprocessor/utility/memory.hpp" + #include #include @@ -411,6 +413,30 @@ bool pointcloud_preprocessor::Filter::convert_output_costly(std::unique_ptrget_logger(), "[input_indices_callback] Invalid input!"); return; diff --git a/sensing/pointcloud_preprocessor/src/outlier_filter/dual_return_outlier_filter_nodelet.cpp b/sensing/pointcloud_preprocessor/src/outlier_filter/dual_return_outlier_filter_nodelet.cpp index ea16b7700e5e3..37374d9ad246c 100644 --- a/sensing/pointcloud_preprocessor/src/outlier_filter/dual_return_outlier_filter_nodelet.cpp +++ b/sensing/pointcloud_preprocessor/src/outlier_filter/dual_return_outlier_filter_nodelet.cpp @@ -28,7 +28,7 @@ namespace pointcloud_preprocessor { -using autoware_point_types::PointXYZIRADRT; +using autoware_point_types::PointXYZIRCAEDT; using autoware_point_types::ReturnType; using diagnostic_msgs::msg::DiagnosticStatus; @@ -110,22 +110,22 @@ void DualReturnOutlierFilterComponent::filter( if (indices) { RCLCPP_WARN(get_logger(), "Indices are not supported and will be ignored"); } - pcl::PointCloud::Ptr pcl_input(new pcl::PointCloud); + pcl::PointCloud::Ptr pcl_input(new pcl::PointCloud); pcl::fromROSMsg(*input, *pcl_input); uint32_t vertical_bins = vertical_bins_; uint32_t horizontal_bins = 36; - float max_azimuth = 36000.0f; + float max_azimuth = 2 * M_PI; float min_azimuth = 0.0f; switch (roi_mode_map_[roi_mode_]) { case 2: { - max_azimuth = max_azimuth_deg_ * 100.0; - min_azimuth = min_azimuth_deg_ * 100.0; + max_azimuth = max_azimuth_deg_ * (M_PI / 180.0); + min_azimuth = min_azimuth_deg_ * (M_PI / 180.0); break; } default: { - max_azimuth = 36000.0f; + max_azimuth = 2 * M_PI; min_azimuth = 0.0f; break; } @@ -134,13 +134,13 @@ void DualReturnOutlierFilterComponent::filter( uint32_t horizontal_resolution = static_cast((max_azimuth - min_azimuth) / horizontal_bins); - pcl::PointCloud::Ptr pcl_output(new pcl::PointCloud); + pcl::PointCloud::Ptr pcl_output(new pcl::PointCloud); pcl_output->points.reserve(pcl_input->points.size()); - std::vector> pcl_input_ring_array; - std::vector> weak_first_pcl_input_ring_array; + std::vector> pcl_input_ring_array; + std::vector> weak_first_pcl_input_ring_array; - pcl::PointCloud::Ptr noise_output(new pcl::PointCloud); + pcl::PointCloud::Ptr noise_output(new pcl::PointCloud); noise_output->points.reserve(pcl_input->points.size()); pcl_input_ring_array.resize( vertical_bins); // TODO(davidw): this is for Pandar 40 only, make dynamic @@ -149,9 +149,9 @@ void DualReturnOutlierFilterComponent::filter( // Split into 36 x 10 degree bins x 40 lines (TODO: change to dynamic) for (const auto & p : pcl_input->points) { if (p.return_type == ReturnType::DUAL_WEAK_FIRST) { - weak_first_pcl_input_ring_array.at(p.ring).push_back(p); + weak_first_pcl_input_ring_array.at(p.channel).push_back(p); } else { - pcl_input_ring_array.at(p.ring).push_back(p); + pcl_input_ring_array.at(p.channel).push_back(p); } } @@ -164,16 +164,16 @@ void DualReturnOutlierFilterComponent::filter( } std::vector deleted_azimuths; std::vector deleted_distances; - pcl::PointCloud temp_segment; + pcl::PointCloud temp_segment; bool keep_next = false; - uint ring_id = weak_first_single_ring.points.front().ring; + uint ring_id = weak_first_single_ring.points.front().channel; for (auto iter = std::begin(weak_first_single_ring) + 1; iter != std::end(weak_first_single_ring) - 1; ++iter) { const float min_dist = std::min(iter->distance, (iter + 1)->distance); const float max_dist = std::max(iter->distance, (iter + 1)->distance); float azimuth_diff = (iter + 1)->azimuth - iter->azimuth; - azimuth_diff = azimuth_diff < 0.f ? azimuth_diff + 36000.f : azimuth_diff; + azimuth_diff = azimuth_diff < 0.f ? azimuth_diff + 2 * M_PI : azimuth_diff; if (max_dist < min_dist * weak_first_distance_ratio_ && azimuth_diff < max_azimuth_diff) { temp_segment.points.push_back(*iter); @@ -283,14 +283,14 @@ void DualReturnOutlierFilterComponent::filter( if (input_ring.size() < 2) { continue; } - pcl::PointCloud temp_segment; + pcl::PointCloud temp_segment; bool keep_next = false; // uint ring_id = input_ring.points.front().ring; for (auto iter = std::begin(input_ring) + 1; iter != std::end(input_ring) - 1; ++iter) { const float min_dist = std::min(iter->distance, (iter + 1)->distance); const float max_dist = std::max(iter->distance, (iter + 1)->distance); float azimuth_diff = (iter + 1)->azimuth - iter->azimuth; - azimuth_diff = azimuth_diff < 0.f ? azimuth_diff + 36000.f : azimuth_diff; + azimuth_diff = azimuth_diff < 0.f ? azimuth_diff + 2 * M_PI : azimuth_diff; if (max_dist < min_dist * general_distance_ratio_ && azimuth_diff < max_azimuth_diff) { temp_segment.points.push_back(*iter); diff --git a/sensing/pointcloud_preprocessor/src/outlier_filter/ring_outlier_filter_nodelet.cpp b/sensing/pointcloud_preprocessor/src/outlier_filter/ring_outlier_filter_nodelet.cpp index a7c9e343a00e1..5e783f1e07418 100644 --- a/sensing/pointcloud_preprocessor/src/outlier_filter/ring_outlier_filter_nodelet.cpp +++ b/sensing/pointcloud_preprocessor/src/outlier_filter/ring_outlier_filter_nodelet.cpp @@ -82,24 +82,22 @@ void RingOutlierFilterComponent::faster_filter( } stop_watch_ptr_->toc("processing_time", true); - output.point_step = sizeof(PointXYZI); + output.point_step = sizeof(OutputPointType); output.data.resize(output.point_step * input->width); size_t output_size = 0; - pcl::PointCloud::Ptr outlier_pcl(new pcl::PointCloud); - - const auto ring_offset = - input->fields.at(static_cast(autoware_point_types::PointIndex::Ring)).offset; - const auto azimuth_offset = - input->fields.at(static_cast(autoware_point_types::PointIndex::Azimuth)).offset; - const auto distance_offset = - input->fields.at(static_cast(autoware_point_types::PointIndex::Distance)).offset; - const auto intensity_offset = - input->fields.at(static_cast(autoware_point_types::PointIndex::Intensity)).offset; - const auto return_type_offset = - input->fields.at(static_cast(autoware_point_types::PointIndex::ReturnType)).offset; - const auto time_stamp_offset = - input->fields.at(static_cast(autoware_point_types::PointIndex::TimeStamp)).offset; + pcl::PointCloud::Ptr outlier_pcl(new pcl::PointCloud); + + const auto input_channel_offset = + input->fields.at(static_cast(InputPointIndex::Channel)).offset; + const auto input_azimuth_offset = + input->fields.at(static_cast(InputPointIndex::Azimuth)).offset; + const auto input_distance_offset = + input->fields.at(static_cast(InputPointIndex::Distance)).offset; + const auto input_intensity_offset = + input->fields.at(static_cast(InputPointIndex::Intensity)).offset; + const auto input_return_type_offset = + input->fields.at(static_cast(InputPointIndex::ReturnType)).offset; std::vector> ring2indices; ring2indices.reserve(max_rings_num_); @@ -110,7 +108,8 @@ void RingOutlierFilterComponent::faster_filter( } for (size_t data_idx = 0; data_idx < input->data.size(); data_idx += input->point_step) { - const uint16_t ring = *reinterpret_cast(&input->data[data_idx + ring_offset]); + const uint16_t ring = + *reinterpret_cast(&input->data[data_idx + input_channel_offset]); ring2indices[ring].push_back(data_idx); } @@ -132,30 +131,30 @@ void RingOutlierFilterComponent::faster_filter( // if(std::abs(iter->distance - (iter+1)->distance) <= std::sqrt(iter->distance) * 0.08) const float & current_azimuth = - *reinterpret_cast(&input->data[current_data_idx + azimuth_offset]); + *reinterpret_cast(&input->data[current_data_idx + input_azimuth_offset]); const float & next_azimuth = - *reinterpret_cast(&input->data[next_data_idx + azimuth_offset]); + *reinterpret_cast(&input->data[next_data_idx + input_azimuth_offset]); float azimuth_diff = next_azimuth - current_azimuth; - azimuth_diff = azimuth_diff < 0.f ? azimuth_diff + 36000.f : azimuth_diff; + azimuth_diff = azimuth_diff < 0.f ? azimuth_diff + 2 * M_PI : azimuth_diff; const float & current_distance = - *reinterpret_cast(&input->data[current_data_idx + distance_offset]); + *reinterpret_cast(&input->data[current_data_idx + input_distance_offset]); const float & next_distance = - *reinterpret_cast(&input->data[next_data_idx + distance_offset]); + *reinterpret_cast(&input->data[next_data_idx + input_distance_offset]); if ( std::max(current_distance, next_distance) < std::min(current_distance, next_distance) * distance_ratio_ && - azimuth_diff < 100.f) { - continue; // Determined to be included in the same walk + azimuth_diff < 1.0 * (180.0 / M_PI)) { // one degree + continue; // Determined to be included in the same walk } if (isCluster( input, std::make_pair(indices[walk_first_idx], indices[walk_last_idx]), walk_last_idx - walk_first_idx + 1)) { for (int i = walk_first_idx; i <= walk_last_idx; i++) { - auto output_ptr = reinterpret_cast(&output.data[output_size]); - auto input_ptr = reinterpret_cast(&input->data[indices[i]]); + auto output_ptr = reinterpret_cast(&output.data[output_size]); + auto input_ptr = reinterpret_cast(&input->data[indices[i]]); if (transform_info.need_transform) { Eigen::Vector4f p(input_ptr->x, input_ptr->y, input_ptr->z, 1); @@ -164,41 +163,38 @@ void RingOutlierFilterComponent::faster_filter( output_ptr->y = p[1]; output_ptr->z = p[2]; } else { - *output_ptr = *input_ptr; + output_ptr->x = input_ptr->x; + output_ptr->y = input_ptr->y; + output_ptr->z = input_ptr->z; } - const float & intensity = - *reinterpret_cast(&input->data[indices[i] + intensity_offset]); + const std::uint8_t & intensity = *reinterpret_cast( + &input->data[indices[i] + input_intensity_offset]); output_ptr->intensity = intensity; + const std::uint8_t & return_type = *reinterpret_cast( + &input->data[indices[i] + input_return_type_offset]); + output_ptr->return_type = return_type; + + const std::uint8_t & channel = *reinterpret_cast( + &input->data[indices[i] + input_channel_offset]); + output_ptr->channel = channel; + output_size += output.point_step; } } else if (publish_outlier_pointcloud_) { for (int i = walk_first_idx; i <= walk_last_idx; i++) { - PointXYZIRADRT outlier_point; auto input_ptr = - reinterpret_cast(&input->data[indices[walk_first_idx]]); + reinterpret_cast(&input->data[indices[walk_first_idx]]); + InputPointType outlier_point = *input_ptr; + if (transform_info.need_transform) { Eigen::Vector4f p(input_ptr->x, input_ptr->y, input_ptr->z, 1); p = transform_info.eigen_transform * p; outlier_point.x = p[0]; outlier_point.y = p[1]; outlier_point.z = p[2]; - } else { - outlier_point = *input_ptr; } - outlier_point.intensity = *reinterpret_cast( - &input->data[indices[walk_first_idx] + intensity_offset]); - outlier_point.ring = *reinterpret_cast( - &input->data[indices[walk_first_idx] + ring_offset]); - outlier_point.azimuth = *reinterpret_cast( - &input->data[indices[walk_first_idx] + azimuth_offset]); - outlier_point.distance = *reinterpret_cast( - &input->data[indices[walk_first_idx] + distance_offset]); - outlier_point.return_type = *reinterpret_cast( - &input->data[indices[walk_first_idx] + return_type_offset]); - outlier_point.time_stamp = *reinterpret_cast( - &input->data[indices[walk_first_idx] + time_stamp_offset]); outlier_pcl->push_back(outlier_point); } } @@ -212,8 +208,8 @@ void RingOutlierFilterComponent::faster_filter( input, std::make_pair(indices[walk_first_idx], indices[walk_last_idx]), walk_last_idx - walk_first_idx + 1)) { for (int i = walk_first_idx; i <= walk_last_idx; i++) { - auto output_ptr = reinterpret_cast(&output.data[output_size]); - auto input_ptr = reinterpret_cast(&input->data[indices[i]]); + auto output_ptr = reinterpret_cast(&output.data[output_size]); + auto input_ptr = reinterpret_cast(&input->data[indices[i]]); if (transform_info.need_transform) { Eigen::Vector4f p(input_ptr->x, input_ptr->y, input_ptr->z, 1); @@ -222,46 +218,42 @@ void RingOutlierFilterComponent::faster_filter( output_ptr->y = p[1]; output_ptr->z = p[2]; } else { - *output_ptr = *input_ptr; + output_ptr->x = input_ptr->x; + output_ptr->y = input_ptr->y; + output_ptr->z = input_ptr->z; } const float & intensity = - *reinterpret_cast(&input->data[indices[i] + intensity_offset]); + *reinterpret_cast(&input->data[indices[i] + input_intensity_offset]); output_ptr->intensity = intensity; + const std::uint8_t & return_type = *reinterpret_cast( + &input->data[indices[i] + input_return_type_offset]); + output_ptr->return_type = return_type; + + const std::uint8_t & channel = + *reinterpret_cast(&input->data[indices[i] + input_channel_offset]); + output_ptr->channel = channel; + output_size += output.point_step; } } else if (publish_outlier_pointcloud_) { for (int i = walk_first_idx; i < walk_last_idx; i++) { - PointXYZIRADRT outlier_point; - - auto input_ptr = reinterpret_cast(&input->data[indices[i]]); + auto input_ptr = reinterpret_cast(&input->data[indices[i]]); + InputPointType outlier_point = *input_ptr; if (transform_info.need_transform) { Eigen::Vector4f p(input_ptr->x, input_ptr->y, input_ptr->z, 1); p = transform_info.eigen_transform * p; outlier_point.x = p[0]; outlier_point.y = p[1]; outlier_point.z = p[2]; - } else { - outlier_point = *input_ptr; } - outlier_point.intensity = - *reinterpret_cast(&input->data[indices[i] + intensity_offset]); - outlier_point.ring = - *reinterpret_cast(&input->data[indices[i] + ring_offset]); - outlier_point.azimuth = - *reinterpret_cast(&input->data[indices[i] + azimuth_offset]); - outlier_point.distance = - *reinterpret_cast(&input->data[indices[i] + distance_offset]); - outlier_point.return_type = - *reinterpret_cast(&input->data[indices[i] + return_type_offset]); - outlier_point.time_stamp = - *reinterpret_cast(&input->data[indices[i] + time_stamp_offset]); + outlier_pcl->push_back(outlier_point); } } } - setUpPointCloudFormat(input, output, output_size, /*num_fields=*/4); + setUpPointCloudFormat(input, output, output_size); if (publish_outlier_pointcloud_) { PointCloud2 outlier; @@ -351,8 +343,7 @@ rcl_interfaces::msg::SetParametersResult RingOutlierFilterComponent::paramCallba } void RingOutlierFilterComponent::setUpPointCloudFormat( - const PointCloud2ConstPtr & input, PointCloud2 & formatted_points, size_t points_size, - size_t num_fields) + const PointCloud2ConstPtr & input, PointCloud2 & formatted_points, size_t points_size) { formatted_points.data.resize(points_size); // Note that `input->header.frame_id` is data before converted when `transform_info.need_transform @@ -366,40 +357,40 @@ void RingOutlierFilterComponent::setUpPointCloudFormat( formatted_points.is_bigendian = input->is_bigendian; formatted_points.is_dense = input->is_dense; - sensor_msgs::PointCloud2Modifier pcd_modifier(formatted_points); - pcd_modifier.setPointCloud2Fields( - num_fields, "x", 1, sensor_msgs::msg::PointField::FLOAT32, "y", 1, - sensor_msgs::msg::PointField::FLOAT32, "z", 1, sensor_msgs::msg::PointField::FLOAT32, - "intensity", 1, sensor_msgs::msg::PointField::FLOAT32); + // This is a hack to get the correct fields in the output point cloud without creating the fields + // manually + sensor_msgs::msg::PointCloud2 msg_aux; + pcl::toROSMsg(pcl::PointCloud(), msg_aux); + formatted_points.fields = msg_aux.fields; } float RingOutlierFilterComponent::calculateVisibilityScore( const sensor_msgs::msg::PointCloud2 & input) { - pcl::PointCloud::Ptr input_cloud(new pcl::PointCloud); + pcl::PointCloud::Ptr input_cloud(new pcl::PointCloud); pcl::fromROSMsg(input, *input_cloud); const uint32_t vertical_bins = vertical_bins_; const uint32_t horizontal_bins = horizontal_bins_; - const float max_azimuth = max_azimuth_deg_ * 100.0; - const float min_azimuth = min_azimuth_deg_ * 100.0; + const float max_azimuth = max_azimuth_deg_ * (M_PI / 180.f); + const float min_azimuth = min_azimuth_deg_ * (M_PI / 180.f); const uint32_t horizontal_resolution = static_cast((max_azimuth - min_azimuth) / horizontal_bins); - std::vector> ring_point_clouds(vertical_bins); + std::vector> ring_point_clouds(vertical_bins); cv::Mat frequency_image(cv::Size(horizontal_bins, vertical_bins), CV_8UC1, cv::Scalar(0)); // Split points into rings for (const auto & point : input_cloud->points) { - ring_point_clouds.at(point.ring).push_back(point); + ring_point_clouds.at(point.channel).push_back(point); } // Calculate frequency for each bin in each ring for (const auto & ring_points : ring_point_clouds) { if (ring_points.empty()) continue; - const uint ring_id = ring_points.front().ring; + const uint ring_id = ring_points.front().channel; std::vector frequency_in_ring(horizontal_bins, 0); for (const auto & point : ring_points.points) { diff --git a/sensing/pointcloud_preprocessor/src/time_synchronizer/time_synchronizer_nodelet.cpp b/sensing/pointcloud_preprocessor/src/time_synchronizer/time_synchronizer_nodelet.cpp index 19920dd1ba5d7..9aba44be1065c 100644 --- a/sensing/pointcloud_preprocessor/src/time_synchronizer/time_synchronizer_nodelet.cpp +++ b/sensing/pointcloud_preprocessor/src/time_synchronizer/time_synchronizer_nodelet.cpp @@ -430,39 +430,56 @@ void PointCloudDataSynchronizerComponent::publish() } /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// -void PointCloudDataSynchronizerComponent::convertToXYZICloud( +void PointCloudDataSynchronizerComponent::convertToXYZIRCCloud( const sensor_msgs::msg::PointCloud2::SharedPtr & input_ptr, sensor_msgs::msg::PointCloud2::SharedPtr & output_ptr) { output_ptr->header = input_ptr->header; - PointCloud2Modifier output_modifier{*output_ptr, input_ptr->header.frame_id}; + + PointCloud2Modifier output_modifier{ + *output_ptr, input_ptr->header.frame_id}; output_modifier.reserve(input_ptr->width); - bool has_intensity = std::any_of( - input_ptr->fields.begin(), input_ptr->fields.end(), - [](auto & field) { return field.name == "intensity"; }); + bool has_valid_intensity = + std::any_of(input_ptr->fields.begin(), input_ptr->fields.end(), [](auto & field) { + return field.name == "intensity" && field.datatype == sensor_msgs::msg::PointField::UINT8; + }); + + bool has_valid_return_type = + std::any_of(input_ptr->fields.begin(), input_ptr->fields.end(), [](auto & field) { + return field.name == "return_type" && field.datatype == sensor_msgs::msg::PointField::UINT8; + }); + + bool has_valid_channel = + std::any_of(input_ptr->fields.begin(), input_ptr->fields.end(), [](auto & field) { + return field.name == "channel" && field.datatype == sensor_msgs::msg::PointField::UINT16; + }); sensor_msgs::PointCloud2Iterator it_x(*input_ptr, "x"); sensor_msgs::PointCloud2Iterator it_y(*input_ptr, "y"); sensor_msgs::PointCloud2Iterator it_z(*input_ptr, "z"); - if (has_intensity) { - sensor_msgs::PointCloud2Iterator it_i(*input_ptr, "intensity"); - for (; it_x != it_x.end(); ++it_x, ++it_y, ++it_z, ++it_i) { - PointXYZI point; + if (has_valid_intensity && has_valid_return_type && has_valid_channel) { + sensor_msgs::PointCloud2Iterator it_i(*input_ptr, "intensity"); + sensor_msgs::PointCloud2Iterator it_r(*input_ptr, "return_type"); + sensor_msgs::PointCloud2Iterator it_c(*input_ptr, "channel"); + + for (; it_x != it_x.end(); ++it_x, ++it_y, ++it_z, ++it_i, ++it_r, ++it_c) { + PointXYZIRC point; point.x = *it_x; point.y = *it_y; point.z = *it_z; point.intensity = *it_i; + point.return_type = *it_r; + point.channel = *it_c; output_modifier.push_back(std::move(point)); } } else { for (; it_x != it_x.end(); ++it_x, ++it_y, ++it_z) { - PointXYZI point; + PointXYZIRC point; point.x = *it_x; point.y = *it_y; point.z = *it_z; - point.intensity = 0.0f; output_modifier.push_back(std::move(point)); } } @@ -489,9 +506,9 @@ void PointCloudDataSynchronizerComponent::cloud_callback( { std::lock_guard lock(mutex_); auto input = std::make_shared(*input_ptr); - sensor_msgs::msg::PointCloud2::SharedPtr xyzi_input_ptr(new sensor_msgs::msg::PointCloud2()); + sensor_msgs::msg::PointCloud2::SharedPtr xyzirc_input_ptr(new sensor_msgs::msg::PointCloud2()); if (input->data.size() > 0) { - convertToXYZICloud(input, xyzi_input_ptr); + convertToXYZIRCCloud(input, xyzirc_input_ptr); } const bool is_already_subscribed_this = (cloud_stdmap_[topic_name] != nullptr); @@ -500,7 +517,7 @@ void PointCloudDataSynchronizerComponent::cloud_callback( [](const auto & e) { return e.second != nullptr; }); if (is_already_subscribed_this) { - cloud_stdmap_tmp_[topic_name] = xyzi_input_ptr; + cloud_stdmap_tmp_[topic_name] = xyzirc_input_ptr; if (!is_already_subscribed_tmp) { auto period = std::chrono::duration_cast( @@ -513,7 +530,7 @@ void PointCloudDataSynchronizerComponent::cloud_callback( timer_->reset(); } } else { - cloud_stdmap_[topic_name] = xyzi_input_ptr; + cloud_stdmap_[topic_name] = xyzirc_input_ptr; const bool is_subscribed_all = std::all_of( std::begin(cloud_stdmap_), std::end(cloud_stdmap_), diff --git a/sensing/pointcloud_preprocessor/src/utility/utilities.cpp b/sensing/pointcloud_preprocessor/src/utility/geometry.cpp similarity index 52% rename from sensing/pointcloud_preprocessor/src/utility/utilities.cpp rename to sensing/pointcloud_preprocessor/src/utility/geometry.cpp index ec84a2467db78..e3013d05d925f 100644 --- a/sensing/pointcloud_preprocessor/src/utility/utilities.cpp +++ b/sensing/pointcloud_preprocessor/src/utility/geometry.cpp @@ -1,4 +1,4 @@ -// Copyright 2022 Tier IV, Inc. +// Copyright 2024 TIER IV, Inc. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -12,9 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "pointcloud_preprocessor/utility/utilities.hpp" - -#include +#include "pointcloud_preprocessor/utility/geometry.hpp" namespace pointcloud_preprocessor::utils { @@ -157,91 +155,4 @@ bool point_within_cgal_polys( return false; } -bool is_data_layout_compatible_with_PointXYZI(const sensor_msgs::msg::PointCloud2 & input) -{ - using autoware_point_types::PointIndex; - using autoware_point_types::PointXYZI; - if (input.fields.size() < 4) { - return false; - } - bool same_layout = true; - const auto & field_x = input.fields.at(static_cast(PointIndex::X)); - same_layout &= field_x.name == "x"; - same_layout &= field_x.offset == offsetof(PointXYZI, x); - same_layout &= field_x.datatype == sensor_msgs::msg::PointField::FLOAT32; - same_layout &= field_x.count == 1; - const auto & field_y = input.fields.at(static_cast(PointIndex::Y)); - same_layout &= field_y.name == "y"; - same_layout &= field_y.offset == offsetof(PointXYZI, y); - same_layout &= field_y.datatype == sensor_msgs::msg::PointField::FLOAT32; - same_layout &= field_y.count == 1; - const auto & field_z = input.fields.at(static_cast(PointIndex::Z)); - same_layout &= field_z.name == "z"; - same_layout &= field_z.offset == offsetof(PointXYZI, z); - same_layout &= field_z.datatype == sensor_msgs::msg::PointField::FLOAT32; - same_layout &= field_z.count == 1; - const auto & field_intensity = input.fields.at(static_cast(PointIndex::Intensity)); - same_layout &= field_intensity.name == "intensity"; - same_layout &= field_intensity.offset == offsetof(PointXYZI, intensity); - same_layout &= field_intensity.datatype == sensor_msgs::msg::PointField::FLOAT32; - same_layout &= field_intensity.count == 1; - return same_layout; -} - -bool is_data_layout_compatible_with_PointXYZIRADRT(const sensor_msgs::msg::PointCloud2 & input) -{ - using autoware_point_types::PointIndex; - using autoware_point_types::PointXYZIRADRT; - if (input.fields.size() < 9) { - return false; - } - bool same_layout = true; - const auto & field_x = input.fields.at(static_cast(PointIndex::X)); - same_layout &= field_x.name == "x"; - same_layout &= field_x.offset == offsetof(PointXYZIRADRT, x); - same_layout &= field_x.datatype == sensor_msgs::msg::PointField::FLOAT32; - same_layout &= field_x.count == 1; - const auto & field_y = input.fields.at(static_cast(PointIndex::Y)); - same_layout &= field_y.name == "y"; - same_layout &= field_y.offset == offsetof(PointXYZIRADRT, y); - same_layout &= field_y.datatype == sensor_msgs::msg::PointField::FLOAT32; - same_layout &= field_y.count == 1; - const auto & field_z = input.fields.at(static_cast(PointIndex::Z)); - same_layout &= field_z.name == "z"; - same_layout &= field_z.offset == offsetof(PointXYZIRADRT, z); - same_layout &= field_z.datatype == sensor_msgs::msg::PointField::FLOAT32; - same_layout &= field_z.count == 1; - const auto & field_intensity = input.fields.at(static_cast(PointIndex::Intensity)); - same_layout &= field_intensity.name == "intensity"; - same_layout &= field_intensity.offset == offsetof(PointXYZIRADRT, intensity); - same_layout &= field_intensity.datatype == sensor_msgs::msg::PointField::FLOAT32; - same_layout &= field_intensity.count == 1; - const auto & field_ring = input.fields.at(static_cast(PointIndex::Ring)); - same_layout &= field_ring.name == "ring"; - same_layout &= field_ring.offset == offsetof(PointXYZIRADRT, ring); - same_layout &= field_ring.datatype == sensor_msgs::msg::PointField::UINT16; - same_layout &= field_ring.count == 1; - const auto & field_azimuth = input.fields.at(static_cast(PointIndex::Azimuth)); - same_layout &= field_azimuth.name == "azimuth"; - same_layout &= field_azimuth.offset == offsetof(PointXYZIRADRT, azimuth); - same_layout &= field_azimuth.datatype == sensor_msgs::msg::PointField::FLOAT32; - same_layout &= field_azimuth.count == 1; - const auto & field_distance = input.fields.at(static_cast(PointIndex::Distance)); - same_layout &= field_distance.name == "distance"; - same_layout &= field_distance.offset == offsetof(PointXYZIRADRT, distance); - same_layout &= field_distance.datatype == sensor_msgs::msg::PointField::FLOAT32; - same_layout &= field_distance.count == 1; - const auto & field_return_type = input.fields.at(static_cast(PointIndex::ReturnType)); - same_layout &= field_return_type.name == "return_type"; - same_layout &= field_return_type.offset == offsetof(PointXYZIRADRT, return_type); - same_layout &= field_return_type.datatype == sensor_msgs::msg::PointField::UINT8; - same_layout &= field_return_type.count == 1; - const auto & field_time_stamp = input.fields.at(static_cast(PointIndex::TimeStamp)); - same_layout &= field_time_stamp.name == "time_stamp"; - same_layout &= field_time_stamp.offset == offsetof(PointXYZIRADRT, time_stamp); - same_layout &= field_time_stamp.datatype == sensor_msgs::msg::PointField::FLOAT64; - same_layout &= field_time_stamp.count == 1; - return same_layout; -} - } // namespace pointcloud_preprocessor::utils diff --git a/sensing/pointcloud_preprocessor/src/utility/memory.cpp b/sensing/pointcloud_preprocessor/src/utility/memory.cpp new file mode 100644 index 0000000000000..138573a2014ff --- /dev/null +++ b/sensing/pointcloud_preprocessor/src/utility/memory.cpp @@ -0,0 +1,211 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "pointcloud_preprocessor/utility/memory.hpp" + +#include + +namespace pointcloud_preprocessor::utils +{ +bool is_data_layout_compatible_with_point_xyzi(const sensor_msgs::msg::PointCloud2 & input) +{ + using PointIndex = autoware_point_types::PointXYZIIndex; + using autoware_point_types::PointXYZI; + if (input.fields.size() < 4) { + return false; + } + bool same_layout = true; + const auto & field_x = input.fields.at(static_cast(PointIndex::X)); + same_layout &= field_x.name == "x"; + same_layout &= field_x.offset == offsetof(PointXYZI, x); + same_layout &= field_x.datatype == sensor_msgs::msg::PointField::FLOAT32; + same_layout &= field_x.count == 1; + const auto & field_y = input.fields.at(static_cast(PointIndex::Y)); + same_layout &= field_y.name == "y"; + same_layout &= field_y.offset == offsetof(PointXYZI, y); + same_layout &= field_y.datatype == sensor_msgs::msg::PointField::FLOAT32; + same_layout &= field_y.count == 1; + const auto & field_z = input.fields.at(static_cast(PointIndex::Z)); + same_layout &= field_z.name == "z"; + same_layout &= field_z.offset == offsetof(PointXYZI, z); + same_layout &= field_z.datatype == sensor_msgs::msg::PointField::FLOAT32; + same_layout &= field_z.count == 1; + const auto & field_intensity = input.fields.at(static_cast(PointIndex::Intensity)); + same_layout &= field_intensity.name == "intensity"; + same_layout &= field_intensity.offset == offsetof(PointXYZI, intensity); + same_layout &= field_intensity.datatype == sensor_msgs::msg::PointField::FLOAT32; + same_layout &= field_intensity.count == 1; + return same_layout; +} + +bool is_data_layout_compatible_with_point_xyzirc(const sensor_msgs::msg::PointCloud2 & input) +{ + using PointIndex = autoware_point_types::PointXYZIRCIndex; + using autoware_point_types::PointXYZIRC; + if (input.fields.size() < 6) { + return false; + } + bool same_layout = true; + const auto & field_x = input.fields.at(static_cast(PointIndex::X)); + same_layout &= field_x.name == "x"; + same_layout &= field_x.offset == offsetof(PointXYZIRC, x); + same_layout &= field_x.datatype == sensor_msgs::msg::PointField::FLOAT32; + same_layout &= field_x.count == 1; + const auto & field_y = input.fields.at(static_cast(PointIndex::Y)); + same_layout &= field_y.name == "y"; + same_layout &= field_y.offset == offsetof(PointXYZIRC, y); + same_layout &= field_y.datatype == sensor_msgs::msg::PointField::FLOAT32; + same_layout &= field_y.count == 1; + const auto & field_z = input.fields.at(static_cast(PointIndex::Z)); + same_layout &= field_z.name == "z"; + same_layout &= field_z.offset == offsetof(PointXYZIRC, z); + same_layout &= field_z.datatype == sensor_msgs::msg::PointField::FLOAT32; + same_layout &= field_z.count == 1; + const auto & field_intensity = input.fields.at(static_cast(PointIndex::Intensity)); + same_layout &= field_intensity.name == "intensity"; + same_layout &= field_intensity.offset == offsetof(PointXYZIRC, intensity); + same_layout &= field_intensity.datatype == sensor_msgs::msg::PointField::UINT8; + same_layout &= field_intensity.count == 1; + const auto & field_return_type = input.fields.at(static_cast(PointIndex::ReturnType)); + same_layout &= field_return_type.name == "return_type"; + same_layout &= field_return_type.offset == offsetof(PointXYZIRC, return_type); + same_layout &= field_return_type.datatype == sensor_msgs::msg::PointField::UINT8; + same_layout &= field_return_type.count == 1; + const auto & field_ring = input.fields.at(static_cast(PointIndex::Channel)); + same_layout &= field_ring.name == "channel"; + same_layout &= field_ring.offset == offsetof(PointXYZIRC, channel); + same_layout &= field_ring.datatype == sensor_msgs::msg::PointField::UINT16; + same_layout &= field_ring.count == 1; + + return same_layout; +} + +bool is_data_layout_compatible_with_point_xyziradrt(const sensor_msgs::msg::PointCloud2 & input) +{ + using PointIndex = autoware_point_types::PointXYZIRADRTIndex; + using autoware_point_types::PointXYZIRADRT; + if (input.fields.size() < 9) { + return false; + } + bool same_layout = true; + const auto & field_x = input.fields.at(static_cast(PointIndex::X)); + same_layout &= field_x.name == "x"; + same_layout &= field_x.offset == offsetof(PointXYZIRADRT, x); + same_layout &= field_x.datatype == sensor_msgs::msg::PointField::FLOAT32; + same_layout &= field_x.count == 1; + const auto & field_y = input.fields.at(static_cast(PointIndex::Y)); + same_layout &= field_y.name == "y"; + same_layout &= field_y.offset == offsetof(PointXYZIRADRT, y); + same_layout &= field_y.datatype == sensor_msgs::msg::PointField::FLOAT32; + same_layout &= field_y.count == 1; + const auto & field_z = input.fields.at(static_cast(PointIndex::Z)); + same_layout &= field_z.name == "z"; + same_layout &= field_z.offset == offsetof(PointXYZIRADRT, z); + same_layout &= field_z.datatype == sensor_msgs::msg::PointField::FLOAT32; + same_layout &= field_z.count == 1; + const auto & field_intensity = input.fields.at(static_cast(PointIndex::Intensity)); + same_layout &= field_intensity.name == "intensity"; + same_layout &= field_intensity.offset == offsetof(PointXYZIRADRT, intensity); + same_layout &= field_intensity.datatype == sensor_msgs::msg::PointField::FLOAT32; + same_layout &= field_intensity.count == 1; + const auto & field_ring = input.fields.at(static_cast(PointIndex::Ring)); + same_layout &= field_ring.name == "ring"; + same_layout &= field_ring.offset == offsetof(PointXYZIRADRT, ring); + same_layout &= field_ring.datatype == sensor_msgs::msg::PointField::UINT16; + same_layout &= field_ring.count == 1; + const auto & field_azimuth = input.fields.at(static_cast(PointIndex::Azimuth)); + same_layout &= field_azimuth.name == "azimuth"; + same_layout &= field_azimuth.offset == offsetof(PointXYZIRADRT, azimuth); + same_layout &= field_azimuth.datatype == sensor_msgs::msg::PointField::FLOAT32; + same_layout &= field_azimuth.count == 1; + const auto & field_distance = input.fields.at(static_cast(PointIndex::Distance)); + same_layout &= field_distance.name == "distance"; + same_layout &= field_distance.offset == offsetof(PointXYZIRADRT, distance); + same_layout &= field_distance.datatype == sensor_msgs::msg::PointField::FLOAT32; + same_layout &= field_distance.count == 1; + const auto & field_return_type = input.fields.at(static_cast(PointIndex::ReturnType)); + same_layout &= field_return_type.name == "return_type"; + same_layout &= field_return_type.offset == offsetof(PointXYZIRADRT, return_type); + same_layout &= field_return_type.datatype == sensor_msgs::msg::PointField::UINT8; + same_layout &= field_return_type.count == 1; + const auto & field_time_stamp = input.fields.at(static_cast(PointIndex::TimeStamp)); + same_layout &= field_time_stamp.name == "time_stamp"; + same_layout &= field_time_stamp.offset == offsetof(PointXYZIRADRT, time_stamp); + same_layout &= field_time_stamp.datatype == sensor_msgs::msg::PointField::FLOAT64; + same_layout &= field_time_stamp.count == 1; + return same_layout; +} + +bool is_data_layout_compatible_with_point_xyzircaedt(const sensor_msgs::msg::PointCloud2 & input) +{ + using PointIndex = autoware_point_types::PointXYZIRCAEDTIndex; + using autoware_point_types::PointXYZIRCAEDT; + if (input.fields.size() != 10) { + return false; + } + bool same_layout = true; + const auto & field_x = input.fields.at(static_cast(PointIndex::X)); + same_layout &= field_x.name == "x"; + same_layout &= field_x.offset == offsetof(PointXYZIRCAEDT, x); + same_layout &= field_x.datatype == sensor_msgs::msg::PointField::FLOAT32; + same_layout &= field_x.count == 1; + const auto & field_y = input.fields.at(static_cast(PointIndex::Y)); + same_layout &= field_y.name == "y"; + same_layout &= field_y.offset == offsetof(PointXYZIRCAEDT, y); + same_layout &= field_y.datatype == sensor_msgs::msg::PointField::FLOAT32; + same_layout &= field_y.count == 1; + const auto & field_z = input.fields.at(static_cast(PointIndex::Z)); + same_layout &= field_z.name == "z"; + same_layout &= field_z.offset == offsetof(PointXYZIRCAEDT, z); + same_layout &= field_z.datatype == sensor_msgs::msg::PointField::FLOAT32; + same_layout &= field_z.count == 1; + const auto & field_intensity = input.fields.at(static_cast(PointIndex::Intensity)); + same_layout &= field_intensity.name == "intensity"; + same_layout &= field_intensity.offset == offsetof(PointXYZIRCAEDT, intensity); + same_layout &= field_intensity.datatype == sensor_msgs::msg::PointField::UINT8; + same_layout &= field_intensity.count == 1; + const auto & field_return_type = input.fields.at(static_cast(PointIndex::ReturnType)); + same_layout &= field_return_type.name == "return_type"; + same_layout &= field_return_type.offset == offsetof(PointXYZIRCAEDT, return_type); + same_layout &= field_return_type.datatype == sensor_msgs::msg::PointField::UINT8; + same_layout &= field_return_type.count == 1; + const auto & field_ring = input.fields.at(static_cast(PointIndex::Channel)); + same_layout &= field_ring.name == "channel"; + same_layout &= field_ring.offset == offsetof(PointXYZIRCAEDT, channel); + same_layout &= field_ring.datatype == sensor_msgs::msg::PointField::UINT16; + same_layout &= field_ring.count == 1; + const auto & field_azimuth = input.fields.at(static_cast(PointIndex::Azimuth)); + same_layout &= field_azimuth.name == "azimuth"; + same_layout &= field_azimuth.offset == offsetof(PointXYZIRCAEDT, azimuth); + same_layout &= field_azimuth.datatype == sensor_msgs::msg::PointField::FLOAT32; + same_layout &= field_azimuth.count == 1; + const auto & field_elevation = input.fields.at(static_cast(PointIndex::Elevation)); + same_layout &= field_elevation.name == "elevation"; + same_layout &= field_elevation.offset == offsetof(PointXYZIRCAEDT, elevation); + same_layout &= field_elevation.datatype == sensor_msgs::msg::PointField::FLOAT32; + same_layout &= field_elevation.count == 1; + const auto & field_distance = input.fields.at(static_cast(PointIndex::Distance)); + same_layout &= field_distance.name == "distance"; + same_layout &= field_distance.offset == offsetof(PointXYZIRCAEDT, distance); + same_layout &= field_distance.datatype == sensor_msgs::msg::PointField::FLOAT32; + same_layout &= field_distance.count == 1; + const auto & field_time_stamp = input.fields.at(static_cast(PointIndex::TimeStamp)); + same_layout &= field_time_stamp.name == "time_stamp"; + same_layout &= field_time_stamp.offset == offsetof(PointXYZIRCAEDT, time_stamp); + same_layout &= field_time_stamp.datatype == sensor_msgs::msg::PointField::UINT32; + same_layout &= field_time_stamp.count == 1; + return same_layout; +} + +} // namespace pointcloud_preprocessor::utils diff --git a/sensing/pointcloud_preprocessor/test/test_distortion_corrector_node.cpp b/sensing/pointcloud_preprocessor/test/test_distortion_corrector_node.cpp index 7997aaf43c202..3b500bfb3e982 100644 --- a/sensing/pointcloud_preprocessor/test/test_distortion_corrector_node.cpp +++ b/sensing/pointcloud_preprocessor/test/test_distortion_corrector_node.cpp @@ -58,7 +58,7 @@ class DistortionCorrectorTest : public ::testing::Test // Spin the node for a while to ensure transforms are published auto start = std::chrono::steady_clock::now(); - auto timeout = std::chrono::seconds(1); + auto timeout = std::chrono::milliseconds(100); while (std::chrono::steady_clock::now() - start < timeout) { rclcpp::spin_some(node_); std::this_thread::sleep_for(std::chrono::milliseconds(10)); @@ -194,19 +194,24 @@ class DistortionCorrectorTest : public ::testing::Test }}; // Generate timestamps for the points - std::vector timestamps = generatePointTimestamps(stamp, number_of_points_); + std::vector timestamps = generatePointTimestamps(stamp, number_of_points_); sensor_msgs::PointCloud2Modifier modifier(pointcloud_msg); modifier.setPointCloud2Fields( - 4, "x", 1, sensor_msgs::msg::PointField::FLOAT32, "y", 1, + 10, "x", 1, sensor_msgs::msg::PointField::FLOAT32, "y", 1, sensor_msgs::msg::PointField::FLOAT32, "z", 1, sensor_msgs::msg::PointField::FLOAT32, - "time_stamp", 1, sensor_msgs::msg::PointField::FLOAT64); + "intensity", 1, sensor_msgs::msg::PointField::UINT8, "return_type", 1, + sensor_msgs::msg::PointField::UINT8, "channel", 1, sensor_msgs::msg::PointField::UINT16, + "azimuth", 1, sensor_msgs::msg::PointField::FLOAT32, "elevation", 1, + sensor_msgs::msg::PointField::FLOAT32, "distance", 1, sensor_msgs::msg::PointField::FLOAT32, + "time_stamp", 1, sensor_msgs::msg::PointField::UINT32); + modifier.resize(number_of_points_); sensor_msgs::PointCloud2Iterator iter_x(pointcloud_msg, "x"); sensor_msgs::PointCloud2Iterator iter_y(pointcloud_msg, "y"); sensor_msgs::PointCloud2Iterator iter_z(pointcloud_msg, "z"); - sensor_msgs::PointCloud2Iterator iter_t(pointcloud_msg, "time_stamp"); + sensor_msgs::PointCloud2Iterator iter_t(pointcloud_msg, "time_stamp"); for (size_t i = 0; i < number_of_points_; ++i) { *iter_x = points[i].x(); @@ -226,15 +231,15 @@ class DistortionCorrectorTest : public ::testing::Test return pointcloud_msg; } - std::vector generatePointTimestamps( + std::vector generatePointTimestamps( rclcpp::Time pointcloud_timestamp, size_t number_of_points) { - std::vector timestamps; - rclcpp::Time point_stamp = pointcloud_timestamp; + std::vector timestamps; + rclcpp::Time global_point_stamp = pointcloud_timestamp; for (size_t i = 0; i < number_of_points; ++i) { - double timestamp = point_stamp.seconds(); - timestamps.push_back(timestamp); - point_stamp = addMilliseconds(point_stamp, points_interval_ms_); + std::uint32_t relative_timestamp = (global_point_stamp - pointcloud_timestamp).nanoseconds(); + timestamps.push_back(relative_timestamp); + global_point_stamp = addMilliseconds(global_point_stamp, points_interval_ms_); } return timestamps; @@ -735,11 +740,11 @@ TEST_F(DistortionCorrectorTest, TestUndistortPointCloudWithPureLinearMotion) sensor_msgs::PointCloud2Iterator iter_x(expected_pointcloud_msg, "x"); sensor_msgs::PointCloud2Iterator iter_y(expected_pointcloud_msg, "y"); sensor_msgs::PointCloud2Iterator iter_z(expected_pointcloud_msg, "z"); - sensor_msgs::PointCloud2Iterator iter_t(expected_pointcloud_msg, "time_stamp"); + sensor_msgs::PointCloud2Iterator iter_t(expected_pointcloud_msg, "time_stamp"); std::vector expected_points; for (; iter_t != iter_t.end(); ++iter_t, ++iter_x, ++iter_y, ++iter_z) { - double time_offset = *iter_t - timestamp.seconds(); + double time_offset = static_cast(*iter_t) / 1e9; expected_points.emplace_back( *iter_x + static_cast(velocity * time_offset), *iter_y, *iter_z); } @@ -822,11 +827,11 @@ TEST_F(DistortionCorrectorTest, TestUndistortPointCloudWithPureRotationalMotion) sensor_msgs::PointCloud2Iterator iter_x(expected_pointcloud_msg, "x"); sensor_msgs::PointCloud2Iterator iter_y(expected_pointcloud_msg, "y"); sensor_msgs::PointCloud2Iterator iter_z(expected_pointcloud_msg, "z"); - sensor_msgs::PointCloud2Iterator iter_t(expected_pointcloud_msg, "time_stamp"); + sensor_msgs::PointCloud2Iterator iter_t(expected_pointcloud_msg, "time_stamp"); std::vector expected_pointcloud; for (; iter_t != iter_t.end(); ++iter_t, ++iter_x, ++iter_y, ++iter_z) { - double time_offset = *iter_t - timestamp.seconds(); + double time_offset = static_cast(*iter_t) / 1e9; float angle = angular_velocity * time_offset; // Set the quaternion for the current angle diff --git a/sensing/pointcloud_preprocessor/test/test_utilities.cpp b/sensing/pointcloud_preprocessor/test/test_utilities.cpp index 68c86dfbd0505..e3075b4b11e79 100644 --- a/sensing/pointcloud_preprocessor/test/test_utilities.cpp +++ b/sensing/pointcloud_preprocessor/test/test_utilities.cpp @@ -1,4 +1,4 @@ -// Copyright 2024 Tier IV, Inc. +// Copyright 2024 TIER IV, Inc. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "pointcloud_preprocessor/utility/utilities.hpp" +#include "pointcloud_preprocessor/utility/geometry.hpp" #include