From 6d0247e078d2e944c482b78a95eac9025f5863c2 Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Tue, 9 Jul 2024 11:24:09 +0900 Subject: [PATCH 01/70] refactor(euclidean_cluster)!: fix namespace and directory structure (#7887) * refactor: update include paths for euclidean_cluster code Signed-off-by: Taekjin LEE refactor: fix namespace Signed-off-by: Taekjin LEE chore: update include paths for euclidean_cluster code Signed-off-by: Taekjin LEE * style(pre-commit): autofix --------- Signed-off-by: Taekjin LEE Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- .../src/debugger/debugger.hpp | 3 -- .../src/detection_by_tracker_node.cpp | 4 +-- .../src/detection_by_tracker_node.hpp | 8 +++--- perception/euclidean_cluster/CMakeLists.txt | 28 +++++++++---------- .../euclidean_cluster/euclidean_cluster.hpp | 8 +++--- .../euclidean_cluster_interface.hpp | 4 +-- .../euclidean_cluster/utils.hpp | 4 +-- .../voxel_grid_based_euclidean_cluster.hpp | 8 +++--- .../launch/euclidean_cluster.launch.py | 4 +-- ...xel_grid_based_euclidean_cluster.launch.py | 4 +-- .../lib/euclidean_cluster.cpp | 6 ++-- perception/euclidean_cluster/lib/utils.cpp | 6 ++-- .../voxel_grid_based_euclidean_cluster.cpp | 6 ++-- .../src/euclidean_cluster_node.cpp | 8 +++--- .../src/euclidean_cluster_node.hpp | 7 +++-- ...oxel_grid_based_euclidean_cluster_node.cpp | 8 +++--- ...oxel_grid_based_euclidean_cluster_node.hpp | 6 ++-- .../src/roi_pointcloud_fusion/node.cpp | 4 +-- 18 files changed, 62 insertions(+), 64 deletions(-) rename perception/euclidean_cluster/include/{ => autoware}/euclidean_cluster/euclidean_cluster.hpp (87%) rename perception/euclidean_cluster/include/{ => autoware}/euclidean_cluster/euclidean_cluster_interface.hpp (95%) rename perception/euclidean_cluster/include/{ => autoware}/euclidean_cluster/utils.hpp (94%) rename perception/euclidean_cluster/include/{ => autoware}/euclidean_cluster/voxel_grid_based_euclidean_cluster.hpp (90%) diff --git a/perception/detection_by_tracker/src/debugger/debugger.hpp b/perception/detection_by_tracker/src/debugger/debugger.hpp index 56f37fb10043d..bda42aa5f0b55 100644 --- a/perception/detection_by_tracker/src/debugger/debugger.hpp +++ b/perception/detection_by_tracker/src/debugger/debugger.hpp @@ -17,9 +17,6 @@ #include "autoware/universe_utils/ros/debug_publisher.hpp" #include "autoware/universe_utils/system/stop_watch.hpp" -#include "euclidean_cluster/euclidean_cluster.hpp" -#include "euclidean_cluster/utils.hpp" -#include "euclidean_cluster/voxel_grid_based_euclidean_cluster.hpp" #include diff --git a/perception/detection_by_tracker/src/detection_by_tracker_node.cpp b/perception/detection_by_tracker/src/detection_by_tracker_node.cpp index c22d015aa11a4..4c65500d96aaa 100644 --- a/perception/detection_by_tracker/src/detection_by_tracker_node.cpp +++ b/perception/detection_by_tracker/src/detection_by_tracker_node.cpp @@ -115,7 +115,7 @@ DetectionByTracker::DetectionByTracker(const rclcpp::NodeOptions & node_options) setMaxSearchRange(); shape_estimator_ = std::make_shared(true, true); - cluster_ = std::make_shared( + cluster_ = std::make_shared( false, 10, 10000, 0.7, 0.3, 0); debugger_ = std::make_shared(this); published_time_publisher_ = @@ -277,7 +277,7 @@ float DetectionByTracker::optimizeUnderSegmentedObject( const auto & label = target_object.classification.front().label; // initialize clustering parameters - euclidean_cluster::VoxelGridBasedEuclideanCluster cluster( + autoware::euclidean_cluster::VoxelGridBasedEuclideanCluster cluster( false, 4, 10000, initial_cluster_range, initial_voxel_size, 0); // convert to pcl diff --git a/perception/detection_by_tracker/src/detection_by_tracker_node.hpp b/perception/detection_by_tracker/src/detection_by_tracker_node.hpp index 095708fb4b51f..9c974d72cfdca 100644 --- a/perception/detection_by_tracker/src/detection_by_tracker_node.hpp +++ b/perception/detection_by_tracker/src/detection_by_tracker_node.hpp @@ -15,12 +15,12 @@ #ifndef DETECTION_BY_TRACKER_NODE_HPP_ #define DETECTION_BY_TRACKER_NODE_HPP_ +#include "autoware/euclidean_cluster/euclidean_cluster.hpp" +#include "autoware/euclidean_cluster/utils.hpp" +#include "autoware/euclidean_cluster/voxel_grid_based_euclidean_cluster.hpp" #include "autoware/shape_estimation/shape_estimator.hpp" #include "autoware/universe_utils/ros/published_time_publisher.hpp" #include "debugger/debugger.hpp" -#include "euclidean_cluster/euclidean_cluster.hpp" -#include "euclidean_cluster/utils.hpp" -#include "euclidean_cluster/voxel_grid_based_euclidean_cluster.hpp" #include "tracker/tracker_handler.hpp" #include "utils/utils.hpp" @@ -69,7 +69,7 @@ class DetectionByTracker : public rclcpp::Node TrackerHandler tracker_handler_; std::shared_ptr shape_estimator_; - std::shared_ptr cluster_; + std::shared_ptr cluster_; std::shared_ptr debugger_; std::map max_search_distance_for_merger_; std::map max_search_distance_for_divider_; diff --git a/perception/euclidean_cluster/CMakeLists.txt b/perception/euclidean_cluster/CMakeLists.txt index fe1fa9fda5fca..dacbcf460a14a 100644 --- a/perception/euclidean_cluster/CMakeLists.txt +++ b/perception/euclidean_cluster/CMakeLists.txt @@ -13,45 +13,45 @@ include_directories( ${PCL_INCLUDE_DIRS} ) -ament_auto_add_library(cluster_lib SHARED - lib/utils.cpp +ament_auto_add_library(${PROJECT_NAME}_lib SHARED lib/euclidean_cluster.cpp lib/voxel_grid_based_euclidean_cluster.cpp + lib/utils.cpp ) -target_link_libraries(cluster_lib +target_link_libraries(${PROJECT_NAME}_lib ${PCL_LIBRARIES} ) -target_include_directories(cluster_lib +target_include_directories(${PROJECT_NAME}_lib PUBLIC $ $ ) -ament_auto_add_library(euclidean_cluster_node_core SHARED +ament_auto_add_library(${PROJECT_NAME}_node_core SHARED src/euclidean_cluster_node.cpp ) -target_link_libraries(euclidean_cluster_node_core +target_link_libraries(${PROJECT_NAME}_node_core ${PCL_LIBRARIES} - cluster_lib + ${PROJECT_NAME}_lib ) -rclcpp_components_register_node(euclidean_cluster_node_core - PLUGIN "euclidean_cluster::EuclideanClusterNode" +rclcpp_components_register_node(${PROJECT_NAME}_node_core + PLUGIN "autoware::euclidean_cluster::EuclideanClusterNode" EXECUTABLE euclidean_cluster_node ) -ament_auto_add_library(voxel_grid_based_euclidean_cluster_node_core SHARED +ament_auto_add_library(${PROJECT_NAME}_voxel_grid_node_core SHARED src/voxel_grid_based_euclidean_cluster_node.cpp ) -target_link_libraries(voxel_grid_based_euclidean_cluster_node_core +target_link_libraries(${PROJECT_NAME}_voxel_grid_node_core ${PCL_LIBRARIES} - cluster_lib + ${PROJECT_NAME}_lib ) -rclcpp_components_register_node(voxel_grid_based_euclidean_cluster_node_core - PLUGIN "euclidean_cluster::VoxelGridBasedEuclideanClusterNode" +rclcpp_components_register_node(${PROJECT_NAME}_voxel_grid_node_core + PLUGIN "autoware::euclidean_cluster::VoxelGridBasedEuclideanClusterNode" EXECUTABLE voxel_grid_based_euclidean_cluster_node ) diff --git a/perception/euclidean_cluster/include/euclidean_cluster/euclidean_cluster.hpp b/perception/euclidean_cluster/include/autoware/euclidean_cluster/euclidean_cluster.hpp similarity index 87% rename from perception/euclidean_cluster/include/euclidean_cluster/euclidean_cluster.hpp rename to perception/euclidean_cluster/include/autoware/euclidean_cluster/euclidean_cluster.hpp index aec3f56936828..8c4aad537cc43 100644 --- a/perception/euclidean_cluster/include/euclidean_cluster/euclidean_cluster.hpp +++ b/perception/euclidean_cluster/include/autoware/euclidean_cluster/euclidean_cluster.hpp @@ -14,14 +14,14 @@ #pragma once -#include "euclidean_cluster/euclidean_cluster_interface.hpp" -#include "euclidean_cluster/utils.hpp" +#include "autoware/euclidean_cluster/euclidean_cluster_interface.hpp" +#include "autoware/euclidean_cluster/utils.hpp" #include #include -namespace euclidean_cluster +namespace autoware::euclidean_cluster { class EuclideanCluster : public EuclideanClusterInterface { @@ -42,4 +42,4 @@ class EuclideanCluster : public EuclideanClusterInterface float tolerance_; }; -} // namespace euclidean_cluster +} // namespace autoware::euclidean_cluster diff --git a/perception/euclidean_cluster/include/euclidean_cluster/euclidean_cluster_interface.hpp b/perception/euclidean_cluster/include/autoware/euclidean_cluster/euclidean_cluster_interface.hpp similarity index 95% rename from perception/euclidean_cluster/include/euclidean_cluster/euclidean_cluster_interface.hpp rename to perception/euclidean_cluster/include/autoware/euclidean_cluster/euclidean_cluster_interface.hpp index 65b907f747666..461d75d4931db 100644 --- a/perception/euclidean_cluster/include/euclidean_cluster/euclidean_cluster_interface.hpp +++ b/perception/euclidean_cluster/include/autoware/euclidean_cluster/euclidean_cluster_interface.hpp @@ -24,7 +24,7 @@ #include -namespace euclidean_cluster +namespace autoware::euclidean_cluster { class EuclideanClusterInterface { @@ -54,4 +54,4 @@ class EuclideanClusterInterface int max_cluster_size_; }; -} // namespace euclidean_cluster +} // namespace autoware::euclidean_cluster diff --git a/perception/euclidean_cluster/include/euclidean_cluster/utils.hpp b/perception/euclidean_cluster/include/autoware/euclidean_cluster/utils.hpp similarity index 94% rename from perception/euclidean_cluster/include/euclidean_cluster/utils.hpp rename to perception/euclidean_cluster/include/autoware/euclidean_cluster/utils.hpp index 50d2306d48f72..a52c0840c4d41 100644 --- a/perception/euclidean_cluster/include/euclidean_cluster/utils.hpp +++ b/perception/euclidean_cluster/include/autoware/euclidean_cluster/utils.hpp @@ -24,7 +24,7 @@ #include -namespace euclidean_cluster +namespace autoware::euclidean_cluster { geometry_msgs::msg::Point getCentroid(const sensor_msgs::msg::PointCloud2 & pointcloud); void convertPointCloudClusters2Msg( @@ -37,4 +37,4 @@ void convertPointCloudClusters2Msg( void convertObjectMsg2SensorMsg( const tier4_perception_msgs::msg::DetectedObjectsWithFeature & input, sensor_msgs::msg::PointCloud2 & output); -} // namespace euclidean_cluster +} // namespace autoware::euclidean_cluster diff --git a/perception/euclidean_cluster/include/euclidean_cluster/voxel_grid_based_euclidean_cluster.hpp b/perception/euclidean_cluster/include/autoware/euclidean_cluster/voxel_grid_based_euclidean_cluster.hpp similarity index 90% rename from perception/euclidean_cluster/include/euclidean_cluster/voxel_grid_based_euclidean_cluster.hpp rename to perception/euclidean_cluster/include/autoware/euclidean_cluster/voxel_grid_based_euclidean_cluster.hpp index 375cc2d19a01f..5c51bb85ce177 100644 --- a/perception/euclidean_cluster/include/euclidean_cluster/voxel_grid_based_euclidean_cluster.hpp +++ b/perception/euclidean_cluster/include/autoware/euclidean_cluster/voxel_grid_based_euclidean_cluster.hpp @@ -14,15 +14,15 @@ #pragma once -#include "euclidean_cluster/euclidean_cluster_interface.hpp" -#include "euclidean_cluster/utils.hpp" +#include "autoware/euclidean_cluster/euclidean_cluster_interface.hpp" +#include "autoware/euclidean_cluster/utils.hpp" #include #include #include -namespace euclidean_cluster +namespace autoware::euclidean_cluster { class VoxelGridBasedEuclideanCluster : public EuclideanClusterInterface { @@ -52,4 +52,4 @@ class VoxelGridBasedEuclideanCluster : public EuclideanClusterInterface int min_points_number_per_voxel_; }; -} // namespace euclidean_cluster +} // namespace autoware::euclidean_cluster diff --git a/perception/euclidean_cluster/launch/euclidean_cluster.launch.py b/perception/euclidean_cluster/launch/euclidean_cluster.launch.py index b8d4f61a9cf28..053438a4352b0 100644 --- a/perception/euclidean_cluster/launch/euclidean_cluster.launch.py +++ b/perception/euclidean_cluster/launch/euclidean_cluster.launch.py @@ -49,7 +49,7 @@ def load_composable_node_param(param_path): use_low_height_euclidean_component = ComposableNode( package=pkg, - plugin="euclidean_cluster::EuclideanClusterNode", + plugin="autoware::euclidean_cluster::EuclideanClusterNode", name=AnonName("euclidean_cluster"), remappings=[ ("input", "low_height/pointcloud"), @@ -60,7 +60,7 @@ def load_composable_node_param(param_path): disuse_low_height_euclidean_component = ComposableNode( package=pkg, - plugin="euclidean_cluster::EuclideanClusterNode", + plugin="autoware::euclidean_cluster::EuclideanClusterNode", name=AnonName("euclidean_cluster"), remappings=[ ("input", LaunchConfiguration("input_pointcloud")), diff --git a/perception/euclidean_cluster/launch/voxel_grid_based_euclidean_cluster.launch.py b/perception/euclidean_cluster/launch/voxel_grid_based_euclidean_cluster.launch.py index 607e1bf30ccaa..0b502b1c43d67 100644 --- a/perception/euclidean_cluster/launch/voxel_grid_based_euclidean_cluster.launch.py +++ b/perception/euclidean_cluster/launch/voxel_grid_based_euclidean_cluster.launch.py @@ -49,7 +49,7 @@ def load_composable_node_param(param_path): use_low_height_euclidean_component = ComposableNode( package=pkg, namespace=ns, - plugin="euclidean_cluster::VoxelGridBasedEuclideanClusterNode", + plugin="autoware::euclidean_cluster::VoxelGridBasedEuclideanClusterNode", name="euclidean_cluster", remappings=[ ("input", "low_height/pointcloud"), @@ -61,7 +61,7 @@ def load_composable_node_param(param_path): disuse_low_height_euclidean_component = ComposableNode( package=pkg, namespace=ns, - plugin="euclidean_cluster::VoxelGridBasedEuclideanClusterNode", + plugin="autoware::euclidean_cluster::VoxelGridBasedEuclideanClusterNode", name="euclidean_cluster", remappings=[ ("input", LaunchConfiguration("input_pointcloud")), diff --git a/perception/euclidean_cluster/lib/euclidean_cluster.cpp b/perception/euclidean_cluster/lib/euclidean_cluster.cpp index 5ba1b99403280..0481b7e1b742d 100644 --- a/perception/euclidean_cluster/lib/euclidean_cluster.cpp +++ b/perception/euclidean_cluster/lib/euclidean_cluster.cpp @@ -12,12 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "euclidean_cluster/euclidean_cluster.hpp" +#include "autoware/euclidean_cluster/euclidean_cluster.hpp" #include #include -namespace euclidean_cluster +namespace autoware::euclidean_cluster { EuclideanCluster::EuclideanCluster() { @@ -93,4 +93,4 @@ bool EuclideanCluster::cluster( return true; } -} // namespace euclidean_cluster +} // namespace autoware::euclidean_cluster diff --git a/perception/euclidean_cluster/lib/utils.cpp b/perception/euclidean_cluster/lib/utils.cpp index 6d119ef3d01aa..624c838ef0647 100644 --- a/perception/euclidean_cluster/lib/utils.cpp +++ b/perception/euclidean_cluster/lib/utils.cpp @@ -11,7 +11,7 @@ // 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 "euclidean_cluster/utils.hpp" +#include "autoware/euclidean_cluster/utils.hpp" #include #include @@ -19,7 +19,7 @@ #include #include -namespace euclidean_cluster +namespace autoware::euclidean_cluster { geometry_msgs::msg::Point getCentroid(const sensor_msgs::msg::PointCloud2 & pointcloud) { @@ -128,4 +128,4 @@ void convertObjectMsg2SensorMsg( output.height = 1; output.is_dense = false; } -} // namespace euclidean_cluster +} // namespace autoware::euclidean_cluster diff --git a/perception/euclidean_cluster/lib/voxel_grid_based_euclidean_cluster.cpp b/perception/euclidean_cluster/lib/voxel_grid_based_euclidean_cluster.cpp index 126f877afddb0..096de3dc1b296 100644 --- a/perception/euclidean_cluster/lib/voxel_grid_based_euclidean_cluster.cpp +++ b/perception/euclidean_cluster/lib/voxel_grid_based_euclidean_cluster.cpp @@ -12,14 +12,14 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "euclidean_cluster/voxel_grid_based_euclidean_cluster.hpp" +#include "autoware/euclidean_cluster/voxel_grid_based_euclidean_cluster.hpp" #include #include #include -namespace euclidean_cluster +namespace autoware::euclidean_cluster { VoxelGridBasedEuclideanCluster::VoxelGridBasedEuclideanCluster() { @@ -166,4 +166,4 @@ bool VoxelGridBasedEuclideanCluster::cluster( return true; } -} // namespace euclidean_cluster +} // namespace autoware::euclidean_cluster diff --git a/perception/euclidean_cluster/src/euclidean_cluster_node.cpp b/perception/euclidean_cluster/src/euclidean_cluster_node.cpp index f986b0334935f..b0b9448dfc0c4 100644 --- a/perception/euclidean_cluster/src/euclidean_cluster_node.cpp +++ b/perception/euclidean_cluster/src/euclidean_cluster_node.cpp @@ -14,11 +14,11 @@ #include "euclidean_cluster_node.hpp" -#include "euclidean_cluster/utils.hpp" +#include "autoware/euclidean_cluster/utils.hpp" #include -namespace euclidean_cluster +namespace autoware::euclidean_cluster { EuclideanClusterNode::EuclideanClusterNode(const rclcpp::NodeOptions & options) : Node("euclidean_cluster_node", options) @@ -86,8 +86,8 @@ void EuclideanClusterNode::onPointCloud( } } -} // namespace euclidean_cluster +} // namespace autoware::euclidean_cluster #include -RCLCPP_COMPONENTS_REGISTER_NODE(euclidean_cluster::EuclideanClusterNode) +RCLCPP_COMPONENTS_REGISTER_NODE(autoware::euclidean_cluster::EuclideanClusterNode) diff --git a/perception/euclidean_cluster/src/euclidean_cluster_node.hpp b/perception/euclidean_cluster/src/euclidean_cluster_node.hpp index 92f10696d17dc..5ab48abb4f042 100644 --- a/perception/euclidean_cluster/src/euclidean_cluster_node.hpp +++ b/perception/euclidean_cluster/src/euclidean_cluster_node.hpp @@ -14,7 +14,7 @@ #pragma once -#include "euclidean_cluster/euclidean_cluster.hpp" +#include "autoware/euclidean_cluster/euclidean_cluster.hpp" #include #include @@ -26,7 +26,8 @@ #include #include -namespace euclidean_cluster + +namespace autoware::euclidean_cluster { class EuclideanClusterNode : public rclcpp::Node { @@ -45,4 +46,4 @@ class EuclideanClusterNode : public rclcpp::Node std::unique_ptr debug_publisher_; }; -} // namespace euclidean_cluster +} // namespace autoware::euclidean_cluster diff --git a/perception/euclidean_cluster/src/voxel_grid_based_euclidean_cluster_node.cpp b/perception/euclidean_cluster/src/voxel_grid_based_euclidean_cluster_node.cpp index e9425095a3b0d..e54c55e4873ee 100644 --- a/perception/euclidean_cluster/src/voxel_grid_based_euclidean_cluster_node.cpp +++ b/perception/euclidean_cluster/src/voxel_grid_based_euclidean_cluster_node.cpp @@ -14,11 +14,11 @@ #include "voxel_grid_based_euclidean_cluster_node.hpp" -#include "euclidean_cluster/utils.hpp" +#include "autoware/euclidean_cluster/utils.hpp" #include -namespace euclidean_cluster +namespace autoware::euclidean_cluster { VoxelGridBasedEuclideanClusterNode::VoxelGridBasedEuclideanClusterNode( const rclcpp::NodeOptions & options) @@ -89,8 +89,8 @@ void VoxelGridBasedEuclideanClusterNode::onPointCloud( } } -} // namespace euclidean_cluster +} // namespace autoware::euclidean_cluster #include -RCLCPP_COMPONENTS_REGISTER_NODE(euclidean_cluster::VoxelGridBasedEuclideanClusterNode) +RCLCPP_COMPONENTS_REGISTER_NODE(autoware::euclidean_cluster::VoxelGridBasedEuclideanClusterNode) diff --git a/perception/euclidean_cluster/src/voxel_grid_based_euclidean_cluster_node.hpp b/perception/euclidean_cluster/src/voxel_grid_based_euclidean_cluster_node.hpp index b0c5d0e5a7fbf..b48e30b37de04 100644 --- a/perception/euclidean_cluster/src/voxel_grid_based_euclidean_cluster_node.hpp +++ b/perception/euclidean_cluster/src/voxel_grid_based_euclidean_cluster_node.hpp @@ -14,7 +14,7 @@ #pragma once -#include "euclidean_cluster/voxel_grid_based_euclidean_cluster.hpp" +#include "autoware/euclidean_cluster/voxel_grid_based_euclidean_cluster.hpp" #include #include @@ -26,7 +26,7 @@ #include -namespace euclidean_cluster +namespace autoware::euclidean_cluster { class VoxelGridBasedEuclideanClusterNode : public rclcpp::Node { @@ -45,4 +45,4 @@ class VoxelGridBasedEuclideanClusterNode : public rclcpp::Node std::unique_ptr debug_publisher_; }; -} // namespace euclidean_cluster +} // namespace autoware::euclidean_cluster diff --git a/perception/image_projection_based_fusion/src/roi_pointcloud_fusion/node.cpp b/perception/image_projection_based_fusion/src/roi_pointcloud_fusion/node.cpp index 7f19402d9e565..904e66cb53298 100644 --- a/perception/image_projection_based_fusion/src/roi_pointcloud_fusion/node.cpp +++ b/perception/image_projection_based_fusion/src/roi_pointcloud_fusion/node.cpp @@ -25,7 +25,7 @@ #include #endif -#include "euclidean_cluster/utils.hpp" +#include "autoware/euclidean_cluster/utils.hpp" namespace image_projection_based_fusion { @@ -68,7 +68,7 @@ void RoiPointCloudFusionNode::postprocess( // publish debug cluster if (cluster_debug_pub_->get_subscription_count() > 0) { sensor_msgs::msg::PointCloud2 debug_cluster_msg; - euclidean_cluster::convertObjectMsg2SensorMsg(output_msg, debug_cluster_msg); + autoware::euclidean_cluster::convertObjectMsg2SensorMsg(output_msg, debug_cluster_msg); cluster_debug_pub_->publish(debug_cluster_msg); } } From 03877b072f7d58f5c42fc06a33d2b330941acd3b Mon Sep 17 00:00:00 2001 From: Satoshi Tanaka <16330533+scepter914@users.noreply.github.com> Date: Tue, 9 Jul 2024 13:42:29 +0900 Subject: [PATCH 02/70] chore(radar_object_tracker): delete maintainer (#7898) Signed-off-by: scepter914 --- perception/radar_object_tracker/package.xml | 1 - 1 file changed, 1 deletion(-) diff --git a/perception/radar_object_tracker/package.xml b/perception/radar_object_tracker/package.xml index 79ae0385d4b5e..f104e2d7ea456 100644 --- a/perception/radar_object_tracker/package.xml +++ b/perception/radar_object_tracker/package.xml @@ -6,7 +6,6 @@ Do tracking radar object Yoshi Ri Yukihiro Saito - Satoshi Tanaka Taekjin Lee Apache License 2.0 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 03/70] 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 04/70] 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 05/70] 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 06/70] 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 07/70] 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 08/70] 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 09/70] 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 10/70] 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 From 951d4ccb92f26018ed494d4886424da5d10a7d61 Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Tue, 9 Jul 2024 22:43:00 +0900 Subject: [PATCH 11/70] feat(map_loader, route_handler)!: add format_version validation (#7074) Signed-off-by: Mamoru Sobue Co-authored-by: Yamato Ando --- .../config/lanelet2_map_loader.param.yaml | 1 + .../map_loader/lanelet2_map_loader_node.hpp | 4 +++ .../schema/lanelet2_map_loader.schema.json | 5 +++ .../lanelet2_map_loader_node.cpp | 32 +++++++++++++++++++ .../test/lanelet2_map_loader_launch.test.py | 1 + .../src/route_handler.cpp | 11 +++++++ 6 files changed, 54 insertions(+) diff --git a/map/map_loader/config/lanelet2_map_loader.param.yaml b/map/map_loader/config/lanelet2_map_loader.param.yaml index 48152605ec0a2..48d392a1b8e66 100755 --- a/map/map_loader/config/lanelet2_map_loader.param.yaml +++ b/map/map_loader/config/lanelet2_map_loader.param.yaml @@ -1,5 +1,6 @@ /**: ros__parameters: + allow_unsupported_version: true # flag to load unsupported format_version anyway. If true, just prints warning. center_line_resolution: 5.0 # [m] use_waypoints: true # "centerline" in the Lanelet2 map will be used as a "waypoints" tag. lanelet2_map_path: $(var lanelet2_map_path) # The lanelet2 map path diff --git a/map/map_loader/include/map_loader/lanelet2_map_loader_node.hpp b/map/map_loader/include/map_loader/lanelet2_map_loader_node.hpp index 8bfdbc5d5560c..e38d65201ee56 100644 --- a/map/map_loader/include/map_loader/lanelet2_map_loader_node.hpp +++ b/map/map_loader/include/map_loader/lanelet2_map_loader_node.hpp @@ -15,6 +15,7 @@ #ifndef MAP_LOADER__LANELET2_MAP_LOADER_NODE_HPP_ #define MAP_LOADER__LANELET2_MAP_LOADER_NODE_HPP_ +#include #include #include #include @@ -29,6 +30,9 @@ class Lanelet2MapLoaderNode : public rclcpp::Node { +public: + static constexpr lanelet::autoware::Version version = lanelet::autoware::version; + public: explicit Lanelet2MapLoaderNode(const rclcpp::NodeOptions & options); diff --git a/map/map_loader/schema/lanelet2_map_loader.schema.json b/map/map_loader/schema/lanelet2_map_loader.schema.json index aae295f847ab2..a55050e4ed570 100644 --- a/map/map_loader/schema/lanelet2_map_loader.schema.json +++ b/map/map_loader/schema/lanelet2_map_loader.schema.json @@ -6,6 +6,11 @@ "lanelet2_map_loader": { "type": "object", "properties": { + "allow_unsupported_version": { + "type": "boolean", + "description": "Flag to load unsupported format_version anyway. If true, just prints warning.", + "default": "true" + }, "center_line_resolution": { "type": "number", "description": "Resolution of the Lanelet center line [m]", diff --git a/map/map_loader/src/lanelet2_map_loader/lanelet2_map_loader_node.cpp b/map/map_loader/src/lanelet2_map_loader/lanelet2_map_loader_node.cpp index e2b2a052619e4..437c9e44cb7c1 100644 --- a/map/map_loader/src/lanelet2_map_loader/lanelet2_map_loader_node.cpp +++ b/map/map_loader/src/lanelet2_map_loader/lanelet2_map_loader_node.cpp @@ -49,6 +49,7 @@ #include #include +#include #include using autoware_map_msgs::msg::LaneletMapBin; @@ -64,6 +65,7 @@ Lanelet2MapLoaderNode::Lanelet2MapLoaderNode(const rclcpp::NodeOptions & options sub_map_projector_info_, [this](const MapProjectorInfo::Message::ConstSharedPtr msg) { on_map_projector_info(msg); }); + declare_parameter("allow_unsupported_version"); declare_parameter("lanelet2_map_path"); declare_parameter("center_line_resolution"); declare_parameter("use_waypoints"); @@ -72,6 +74,7 @@ Lanelet2MapLoaderNode::Lanelet2MapLoaderNode(const rclcpp::NodeOptions & options void Lanelet2MapLoaderNode::on_map_projector_info( const MapProjectorInfo::Message::ConstSharedPtr msg) { + const auto allow_unsupported_version = get_parameter("allow_unsupported_version").as_bool(); const auto lanelet2_filename = get_parameter("lanelet2_map_path").as_string(); const auto center_line_resolution = get_parameter("center_line_resolution").as_double(); const auto use_waypoints = get_parameter("use_waypoints").as_bool(); @@ -83,6 +86,35 @@ void Lanelet2MapLoaderNode::on_map_projector_info( return; } + std::string format_version{"null"}, map_version{""}; + lanelet::io_handlers::AutowareOsmParser::parseVersions( + lanelet2_filename, &format_version, &map_version); + if (format_version == "null" || format_version.empty() || !isdigit(format_version[0])) { + RCLCPP_WARN( + get_logger(), + "%s has no format_version(null) or non semver-style format_version(%s) information", + lanelet2_filename.c_str(), format_version.c_str()); + if (!allow_unsupported_version) { + throw std::invalid_argument( + "allow_unsupported_version is false, so stop loading lanelet map"); + } + } else if (const auto map_major_ver_opt = lanelet::io_handlers::parseMajorVersion(format_version); + map_major_ver_opt.has_value()) { + const auto map_major_ver = map_major_ver_opt.value(); + if (map_major_ver > static_cast(lanelet::autoware::version)) { + RCLCPP_WARN( + get_logger(), + "format_version(%ld) of the provided map(%s) is larger than the supported version(%ld)", + map_major_ver, lanelet2_filename.c_str(), + static_cast(lanelet::autoware::version)); + if (!allow_unsupported_version) { + throw std::invalid_argument( + "allow_unsupported_version is false, so stop loading lanelet map"); + } + } + } + RCLCPP_INFO(get_logger(), "Loaded map format_version: %s", format_version.c_str()); + // overwrite centerline if (use_waypoints) { lanelet::utils::overwriteLaneletsCenterlineWithWaypoints(map, center_line_resolution, false); diff --git a/map/map_loader/test/lanelet2_map_loader_launch.test.py b/map/map_loader/test/lanelet2_map_loader_launch.test.py index 9f9a59f565e3f..0a29a74e90b06 100644 --- a/map/map_loader/test/lanelet2_map_loader_launch.test.py +++ b/map/map_loader/test/lanelet2_map_loader_launch.test.py @@ -39,6 +39,7 @@ def generate_test_description(): "lanelet2_map_path": lanelet2_map_path, "center_line_resolution": 5.0, "use_waypoints": True, + "allow_unsupported_version": True, } ], ) diff --git a/planning/autoware_route_handler/src/route_handler.cpp b/planning/autoware_route_handler/src/route_handler.cpp index 0540fb1758802..9811f982f8e1b 100644 --- a/planning/autoware_route_handler/src/route_handler.cpp +++ b/planning/autoware_route_handler/src/route_handler.cpp @@ -15,6 +15,7 @@ #include "autoware/route_handler/route_handler.hpp" #include +#include #include #include #include @@ -186,6 +187,16 @@ void RouteHandler::setMap(const LaneletMapBin & map_msg) lanelet_map_ptr_ = std::make_shared(); lanelet::utils::conversion::fromBinMsg( map_msg, lanelet_map_ptr_, &traffic_rules_ptr_, &routing_graph_ptr_); + const auto map_major_version_opt = + lanelet::io_handlers::parseMajorVersion(map_msg.version_map_format); + if (!map_major_version_opt) { + RCLCPP_WARN( + logger_, "setMap() for invalid version map: %s", map_msg.version_map_format.c_str()); + } else if (map_major_version_opt.value() > static_cast(lanelet::autoware::version)) { + RCLCPP_WARN( + logger_, "setMap() for a map(version %s) newer than lanelet2_extension support version(%d)", + map_msg.version_map_format.c_str(), static_cast(lanelet::autoware::version)); + } const auto traffic_rules = lanelet::traffic_rules::TrafficRulesFactory::create( lanelet::Locations::Germany, lanelet::Participants::Vehicle); From 12b31ab091d375ee9a068ec80977b4fc0c4c8353 Mon Sep 17 00:00:00 2001 From: Shunsuke Miura <37187849+miursh@users.noreply.github.com> Date: Tue, 9 Jul 2024 23:27:18 +0900 Subject: [PATCH 12/70] chore: add missing dependency (#7919) add raindrop_cluster_filter dependency --- launch/tier4_perception_launch/package.xml | 1 + 1 file changed, 1 insertion(+) diff --git a/launch/tier4_perception_launch/package.xml b/launch/tier4_perception_launch/package.xml index 5c15a937304ad..592ef7cf01f86 100644 --- a/launch/tier4_perception_launch/package.xml +++ b/launch/tier4_perception_launch/package.xml @@ -38,6 +38,7 @@ radar_fusion_to_detected_object radar_object_clustering radar_object_tracker + raindrop_cluster_filter shape_estimation topic_tools tracking_object_merger From 050fa298170e0e1282ff36654dc0ec80020f843f Mon Sep 17 00:00:00 2001 From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Date: Wed, 10 Jul 2024 08:13:26 +0900 Subject: [PATCH 13/70] fix(static_obstacle_avoidance): stop position is unstable (#7880) fix(static_obstacle_avoidance): fix stop position Signed-off-by: satoshi-ota --- .../src/scene.cpp | 6 +- .../debug.hpp | 2 + .../scene.hpp | 6 + .../utils.hpp | 11 +- .../src/debug.cpp | 17 +++ .../src/scene.cpp | 46 ++++--- .../src/utils.cpp | 117 +++++++++--------- 7 files changed, 119 insertions(+), 86 deletions(-) diff --git a/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/scene.cpp b/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/scene.cpp index 303d7449e40dd..501554b0a2b02 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/scene.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/scene.cpp @@ -113,10 +113,8 @@ void AvoidanceByLaneChange::updateSpecialData() : Direction::RIGHT; } - utils::static_obstacle_avoidance::updateRegisteredObject( - registered_objects_, avoidance_data_.target_objects, p); - utils::static_obstacle_avoidance::compensateDetectionLost( - registered_objects_, avoidance_data_.target_objects, avoidance_data_.other_objects); + utils::static_obstacle_avoidance::compensateLostTargetObjects( + registered_objects_, avoidance_data_, clock_.now(), planner_data_, p); std::sort( avoidance_data_.target_objects.begin(), avoidance_data_.target_objects.end(), diff --git a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/debug.hpp b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/debug.hpp index 316501fbbd37f..63985e574f3b2 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/debug.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/debug.hpp @@ -47,6 +47,8 @@ MarkerArray createTargetObjectsMarkerArray(const ObjectDataArray & objects, cons MarkerArray createOtherObjectsMarkerArray( const ObjectDataArray & objects, const ObjectInfo & info, const bool verbose); +MarkerArray createStopTargetObjectMarkerArray(const AvoidancePlanningData & data); + MarkerArray createDebugMarkerArray( const AvoidancePlanningData & data, const PathShifter & shifter, const DebugData & debug, const std::shared_ptr & parameters); diff --git a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/scene.hpp b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/scene.hpp index 635edb7c84f40..34d06a46d9ac8 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/scene.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/scene.hpp @@ -266,6 +266,12 @@ class StaticObstacleAvoidanceModule : public SceneModuleInterface */ void fillAvoidanceTargetObjects(AvoidancePlanningData & data, DebugData & debug) const; + /** + * @brief fill additional data which are necessary to plan avoidance path/velocity. + * @param avoidance target objects. + */ + void fillAvoidanceTargetData(ObjectDataArray & objects) const; + /** * @brief fill candidate shift lines. * @param avoidance data. diff --git a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/utils.hpp b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/utils.hpp index 3797fe0d55147..a3b1b7e10b885 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/utils.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/utils.hpp @@ -118,15 +118,12 @@ void fillObjectStoppableJudge( ObjectData & object_data, const ObjectDataArray & registered_objects, const double feasible_stop_distance, const std::shared_ptr & parameters); -void updateRegisteredObject( - ObjectDataArray & registered_objects, const ObjectDataArray & now_objects, - const std::shared_ptr & parameters); - void updateClipObject(ObjectDataArray & clip_objects, AvoidancePlanningData & data); -void compensateDetectionLost( - const ObjectDataArray & registered_objects, ObjectDataArray & now_objects, - ObjectDataArray & other_objects); +void compensateLostTargetObjects( + ObjectDataArray & stored_objects, AvoidancePlanningData & data, const rclcpp::Time & now, + const std::shared_ptr & planner_data, + const std::shared_ptr & parameters); void filterTargetObjects( ObjectDataArray & objects, AvoidancePlanningData & data, const double forward_detection_range, diff --git a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/debug.cpp b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/debug.cpp index eb0d903f2e398..e1f8bd4ad2734 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/debug.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/debug.cpp @@ -466,6 +466,23 @@ MarkerArray createOtherObjectsMarkerArray( return msg; } +MarkerArray createStopTargetObjectMarkerArray(const AvoidancePlanningData & data) +{ + MarkerArray msg; + + if (!data.stop_target_object.has_value()) { + return msg; + } + + appendMarkerArray( + createObjectsCubeMarkerArray( + {data.stop_target_object.value()}, "stop_target", createMarkerScale(3.4, 1.9, 1.9), + createMarkerColor(1.0, 0.0, 0.42, 0.5)), + &msg); + + return msg; +} + MarkerArray createDrivableBounds( const AvoidancePlanningData & data, std::string && ns, const float & r, const float & g, const float & b) diff --git a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/scene.cpp b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/scene.cpp index b0e7d68c75d44..641ae9547b712 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/scene.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/scene.cpp @@ -286,16 +286,22 @@ void StaticObstacleAvoidanceModule::fillFundamentalData( data.to_start_point = utils::static_obstacle_avoidance::calcDistanceToAvoidStartLine( data.current_lanelets, data.reference_path_rough, planner_data_, parameters_); - // target objects for avoidance + // filter only for the latest detected objects. fillAvoidanceTargetObjects(data, debug); - // lost object compensation - utils::static_obstacle_avoidance::updateRegisteredObject( - registered_objects_, data.target_objects, parameters_); - utils::static_obstacle_avoidance::compensateDetectionLost( - registered_objects_, data.target_objects, data.other_objects); + // compensate lost object which was avoidance target. if the time hasn't passed more than + // threshold since perception module lost the target yet, this module keeps it as avoidance + // target. + utils::static_obstacle_avoidance::compensateLostTargetObjects( + registered_objects_, data, clock_->now(), planner_data_, parameters_); + + // once an object filtered for boundary clipping, this module keeps the information until the end + // of execution. utils::static_obstacle_avoidance::updateClipObject(clip_objects_, data); + // calculate various data for each target objects. + fillAvoidanceTargetData(data.target_objects); + // sort object order by longitudinal distance std::sort(data.target_objects.begin(), data.target_objects.end(), [](auto a, auto b) { return a.longitudinal < b.longitudinal; @@ -308,8 +314,6 @@ void StaticObstacleAvoidanceModule::fillFundamentalData( void StaticObstacleAvoidanceModule::fillAvoidanceTargetObjects( AvoidancePlanningData & data, DebugData & debug) const { - using utils::static_obstacle_avoidance::fillAvoidanceNecessity; - using utils::static_obstacle_avoidance::fillObjectStoppableJudge; using utils::static_obstacle_avoidance::filterTargetObjects; using utils::static_obstacle_avoidance::separateObjectsByPath; using utils::static_obstacle_avoidance::updateRoadShoulderDistance; @@ -345,15 +349,6 @@ void StaticObstacleAvoidanceModule::fillAvoidanceTargetObjects( filterTargetObjects(objects, data, forward_detection_range, planner_data_, parameters_); updateRoadShoulderDistance(data, planner_data_, parameters_); - // Calculate the distance needed to safely decelerate the ego vehicle to a stop line. - const auto & vehicle_width = planner_data_->parameters.vehicle_width; - const auto feasible_stop_distance = helper_->getFeasibleDecelDistance(0.0, false); - std::for_each(data.target_objects.begin(), data.target_objects.end(), [&, this](auto & o) { - fillAvoidanceNecessity(o, registered_objects_, vehicle_width, parameters_); - o.to_stop_line = calcDistanceToStopLine(o); - fillObjectStoppableJudge(o, registered_objects_, feasible_stop_distance, parameters_); - }); - // debug { std::vector debug_info_array; @@ -371,6 +366,21 @@ void StaticObstacleAvoidanceModule::fillAvoidanceTargetObjects( } } +void StaticObstacleAvoidanceModule::fillAvoidanceTargetData(ObjectDataArray & objects) const +{ + using utils::static_obstacle_avoidance::fillAvoidanceNecessity; + using utils::static_obstacle_avoidance::fillObjectStoppableJudge; + + // Calculate the distance needed to safely decelerate the ego vehicle to a stop line. + const auto & vehicle_width = planner_data_->parameters.vehicle_width; + const auto feasible_stop_distance = helper_->getFeasibleDecelDistance(0.0, false); + std::for_each(objects.begin(), objects.end(), [&, this](auto & o) { + fillAvoidanceNecessity(o, registered_objects_, vehicle_width, parameters_); + o.to_stop_line = calcDistanceToStopLine(o); + fillObjectStoppableJudge(o, registered_objects_, feasible_stop_distance, parameters_); + }); +} + ObjectData StaticObstacleAvoidanceModule::createObjectData( const AvoidancePlanningData & data, const PredictedObject & object) const { @@ -1376,11 +1386,13 @@ void StaticObstacleAvoidanceModule::updateRTCData() void StaticObstacleAvoidanceModule::updateInfoMarker(const AvoidancePlanningData & data) const { + using utils::static_obstacle_avoidance::createStopTargetObjectMarkerArray; using utils::static_obstacle_avoidance::createTargetObjectsMarkerArray; info_marker_.markers.clear(); appendMarkerArray( createTargetObjectsMarkerArray(data.target_objects, "target_objects"), &info_marker_); + appendMarkerArray(createStopTargetObjectMarkerArray(data), &info_marker_); } void StaticObstacleAvoidanceModule::updateDebugMarker( diff --git a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/utils.cpp b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/utils.cpp index 30d1aad16374b..ddc544553f30e 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/utils.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/utils.cpp @@ -1645,31 +1645,39 @@ void fillObjectStoppableJudge( object_data.is_stoppable = same_id_obj->is_stoppable; } -void updateRegisteredObject( - ObjectDataArray & registered_objects, const ObjectDataArray & now_objects, +void compensateLostTargetObjects( + ObjectDataArray & stored_objects, AvoidancePlanningData & data, const rclcpp::Time & now, + const std::shared_ptr & planner_data, const std::shared_ptr & parameters) { - const auto updateIfDetectedNow = [&now_objects](auto & registered_object) { - const auto & n = now_objects; - const auto r_id = registered_object.object.object_id; - const auto same_id_obj = std::find_if( - n.begin(), n.end(), [&r_id](const auto & o) { return o.object.object_id == r_id; }); + const auto include = [](const auto & objects, const auto & search_id) { + return std::all_of(objects.begin(), objects.end(), [&search_id](const auto & o) { + return o.object.object_id != search_id; + }); + }; + + // STEP.1 UPDATE STORED OBJECTS. + const auto match = [&data](auto & object) { + const auto & search_id = object.object.object_id; + const auto same_id_object = std::find_if( + data.target_objects.begin(), data.target_objects.end(), + [&search_id](const auto & o) { return o.object.object_id == search_id; }); // same id object is detected. update registered. - if (same_id_obj != n.end()) { - registered_object = *same_id_obj; + if (same_id_object != data.target_objects.end()) { + object = *same_id_object; return true; } - constexpr auto POS_THR = 1.5; - const auto r_pos = registered_object.getPose(); - const auto similar_pos_obj = std::find_if(n.begin(), n.end(), [&](const auto & o) { - return calcDistance2d(r_pos, o.getPose()) < POS_THR; - }); + const auto similar_pos_obj = std::find_if( + data.target_objects.begin(), data.target_objects.end(), [&object](const auto & o) { + constexpr auto POS_THR = 1.5; + return calcDistance2d(object.getPose(), o.getPose()) < POS_THR; + }); // same id object is not detected, but object is found around registered. update registered. - if (similar_pos_obj != n.end()) { - registered_object = *similar_pos_obj; + if (similar_pos_obj != data.target_objects.end()) { + object = *similar_pos_obj; return true; } @@ -1677,61 +1685,54 @@ void updateRegisteredObject( return false; }; - const rclcpp::Time now = rclcpp::Clock(RCL_ROS_TIME).now(); - - // -- check registered_objects, remove if lost_count exceeds limit. -- - for (int i = static_cast(registered_objects.size()) - 1; i >= 0; --i) { - auto & r = registered_objects.at(i); - - // registered object is not detected this time. lost count up. - if (!updateIfDetectedNow(r)) { - r.lost_time = (now - r.last_seen).seconds(); - } else { - r.last_seen = now; - r.lost_time = 0.0; - } + // STEP1-1: REMOVE EXPIRED OBJECTS. + const auto itr = std::remove_if( + stored_objects.begin(), stored_objects.end(), [&now, &match, ¶meters](auto & o) { + if (!match(o)) { + o.lost_time = (now - o.last_seen).seconds(); + } else { + o.last_seen = now; + o.lost_time = 0.0; + } - // lost count exceeds threshold. remove object from register. - if (r.lost_time > parameters->object_last_seen_threshold) { - registered_objects.erase(registered_objects.begin() + i); - } - } + return o.lost_time > parameters->object_last_seen_threshold; + }); - const auto isAlreadyRegistered = [&](const auto & n_id) { - const auto & r = registered_objects; - return std::any_of( - r.begin(), r.end(), [&n_id](const auto & o) { return o.object.object_id == n_id; }); - }; + stored_objects.erase(itr, stored_objects.end()); - // -- check now_objects, add it if it has new object id -- - for (const auto & now_obj : now_objects) { - if (!isAlreadyRegistered(now_obj.object.object_id)) { - registered_objects.push_back(now_obj); + // STEP1-2: UPDATE STORED OBJECTS IF THERE ARE NEW OBJECTS. + for (const auto & current_object : data.target_objects) { + if (!include(stored_objects, current_object.object.object_id)) { + stored_objects.push_back(current_object); } } -} -void compensateDetectionLost( - const ObjectDataArray & registered_objects, ObjectDataArray & now_objects, - ObjectDataArray & other_objects) -{ - const auto isDetectedNow = [&](const auto & r_id) { - const auto & n = now_objects; + // STEP2: COMPENSATE CURRENT TARGET OBJECTS + const auto is_detected = [&](const auto & object_id) { return std::any_of( - n.begin(), n.end(), [&r_id](const auto & o) { return o.object.object_id == r_id; }); + data.target_objects.begin(), data.target_objects.end(), + [&object_id](const auto & o) { return o.object.object_id == object_id; }); }; - const auto isIgnoreObject = [&](const auto & r_id) { - const auto & n = other_objects; + const auto is_ignored = [&](const auto & object_id) { return std::any_of( - n.begin(), n.end(), [&r_id](const auto & o) { return o.object.object_id == r_id; }); + data.other_objects.begin(), data.other_objects.end(), + [&object_id](const auto & o) { return o.object.object_id == object_id; }); }; - for (const auto & registered : registered_objects) { - if ( - !isDetectedNow(registered.object.object_id) && !isIgnoreObject(registered.object.object_id)) { - now_objects.push_back(registered); + for (auto & stored_object : stored_objects) { + if (is_detected(stored_object.object.object_id)) { + continue; + } + if (is_ignored(stored_object.object.object_id)) { + continue; } + + const auto & ego_pos = planner_data->self_odometry->pose.pose.position; + fillLongitudinalAndLengthByClosestEnvelopeFootprint( + data.reference_path_rough, ego_pos, stored_object); + + data.target_objects.push_back(stored_object); } } From 5e5ad0bc01470eb44db781e642f272ebfc30721f Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Wed, 10 Jul 2024 09:39:43 +0900 Subject: [PATCH 14/70] feat(blind_spot): consider road_shoulder if exist (#7925) Signed-off-by: Mamoru Sobue --- .../src/scene.cpp | 31 +++++++++++++++---- 1 file changed, 25 insertions(+), 6 deletions(-) diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/src/scene.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/src/scene.cpp index 1880adb045b42..d8725f016fda2 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/src/scene.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/src/scene.cpp @@ -621,14 +621,14 @@ lanelet::ConstLanelets BlindSpotModule::generateBlindSpotLanelets( for (const auto i : lane_ids) { const auto lane = lanelet_map_ptr->laneletLayer.get(i); const auto ego_half_lanelet = generateHalfLanelet(lane); - const auto adj = + const auto assoc_adj = turn_direction_ == TurnDirection::LEFT ? (routing_graph_ptr->adjacentLeft(lane)) : (turn_direction_ == TurnDirection::RIGHT ? (routing_graph_ptr->adjacentRight(lane)) : boost::none); const std::optional opposite_adj = [&]() -> std::optional { - if (!!adj) { + if (!!assoc_adj) { return std::nullopt; } if (turn_direction_ == TurnDirection::LEFT) { @@ -653,10 +653,27 @@ lanelet::ConstLanelets BlindSpotModule::generateBlindSpotLanelets( } }(); - if (!adj && !opposite_adj) { - blind_spot_lanelets.push_back(ego_half_lanelet); - } else if (!!adj) { - const auto adj_half_lanelet = generateExtendedAdjacentLanelet(adj.value(), turn_direction_); + const auto assoc_shoulder = [&]() -> std::optional { + if (turn_direction_ == TurnDirection::LEFT) { + return planner_data_->route_handler_->getLeftShoulderLanelet(lane); + } else if (turn_direction_ == TurnDirection::RIGHT) { + return planner_data_->route_handler_->getRightShoulderLanelet(lane); + } + return std::nullopt; + }(); + if (assoc_shoulder) { + const auto lefts = (turn_direction_ == TurnDirection::LEFT) + ? assoc_shoulder.value().leftBound() + : ego_half_lanelet.leftBound(); + const auto rights = (turn_direction_ == TurnDirection::LEFT) + ? ego_half_lanelet.rightBound() + : assoc_shoulder.value().rightBound(); + blind_spot_lanelets.push_back( + lanelet::ConstLanelet(lanelet::InvalId, removeConst(lefts), removeConst(rights))); + + } else if (!!assoc_adj) { + const auto adj_half_lanelet = + generateExtendedAdjacentLanelet(assoc_adj.value(), turn_direction_); const auto lefts = (turn_direction_ == TurnDirection::LEFT) ? adj_half_lanelet.leftBound() : ego_half_lanelet.leftBound(); const auto rights = (turn_direction_ == TurnDirection::RIGHT) ? adj_half_lanelet.rightBound() @@ -672,6 +689,8 @@ lanelet::ConstLanelets BlindSpotModule::generateBlindSpotLanelets( : adj_half_lanelet.rightBound(); blind_spot_lanelets.push_back( lanelet::ConstLanelet(lanelet::InvalId, removeConst(lefts), removeConst(rights))); + } else { + blind_spot_lanelets.push_back(ego_half_lanelet); } } return blind_spot_lanelets; From 6dc805e4f1b411ce0d5b37a07bf3607342b1c9cf Mon Sep 17 00:00:00 2001 From: danielsanchezaran Date: Wed, 10 Jul 2024 10:29:13 +0900 Subject: [PATCH 15/70] feat(planning_evaluator,control_evaluator, evaluator utils): add diagnostics subscriber to planning eval (#7849) * add utils and diagnostics subscription to planning_evaluator Signed-off-by: Daniel Sanchez * add diagnostics eval Signed-off-by: Daniel Sanchez * fix input diag in launch Signed-off-by: kosuke55 --------- Signed-off-by: Daniel Sanchez Signed-off-by: kosuke55 Co-authored-by: kosuke55 --- .../control_evaluator_node.hpp | 7 -- .../autoware_control_evaluator/package.xml | 1 + .../src/control_evaluator_node.cpp | 50 +------------- .../autoware_evaluator_utils/CMakeLists.txt | 13 ++++ evaluator/autoware_evaluator_utils/README.md | 5 ++ .../evaluator_utils/evaluator_utils.hpp | 45 +++++++++++++ .../autoware_evaluator_utils/package.xml | 24 +++++++ .../src/evaluator_utils.cpp | 66 +++++++++++++++++++ .../planning_evaluator_node.hpp | 20 +++++- .../launch/planning_evaluator.launch.xml | 2 + .../autoware_planning_evaluator/package.xml | 1 + .../src/planning_evaluator_node.cpp | 46 +++++++++++++ 12 files changed, 225 insertions(+), 55 deletions(-) create mode 100644 evaluator/autoware_evaluator_utils/CMakeLists.txt create mode 100644 evaluator/autoware_evaluator_utils/README.md create mode 100644 evaluator/autoware_evaluator_utils/include/autoware/evaluator_utils/evaluator_utils.hpp create mode 100644 evaluator/autoware_evaluator_utils/package.xml create mode 100644 evaluator/autoware_evaluator_utils/src/evaluator_utils.cpp diff --git a/evaluator/autoware_control_evaluator/include/autoware/control_evaluator/control_evaluator_node.hpp b/evaluator/autoware_control_evaluator/include/autoware/control_evaluator/control_evaluator_node.hpp index da60c820c45b1..614f1d66b9e0d 100644 --- a/evaluator/autoware_control_evaluator/include/autoware/control_evaluator/control_evaluator_node.hpp +++ b/evaluator/autoware_control_evaluator/include/autoware/control_evaluator/control_evaluator_node.hpp @@ -52,13 +52,6 @@ class ControlEvaluatorNode : public rclcpp::Node { public: explicit ControlEvaluatorNode(const rclcpp::NodeOptions & node_options); - void removeOldDiagnostics(const rclcpp::Time & stamp); - void removeDiagnosticsByName(const std::string & name); - void addDiagnostic(const DiagnosticStatus & diag, const rclcpp::Time & stamp); - void updateDiagnosticQueue( - const DiagnosticArray & input_diagnostics, const std::string & function, - const rclcpp::Time & stamp); - DiagnosticStatus generateLateralDeviationDiagnosticStatus( const Trajectory & traj, const Point & ego_point); DiagnosticStatus generateYawDeviationDiagnosticStatus( diff --git a/evaluator/autoware_control_evaluator/package.xml b/evaluator/autoware_control_evaluator/package.xml index 21f48b1c64485..d7300e6a3bfb4 100644 --- a/evaluator/autoware_control_evaluator/package.xml +++ b/evaluator/autoware_control_evaluator/package.xml @@ -14,6 +14,7 @@ ament_cmake_auto autoware_cmake + autoware_evaluator_utils autoware_motion_utils autoware_planning_msgs autoware_route_handler diff --git a/evaluator/autoware_control_evaluator/src/control_evaluator_node.cpp b/evaluator/autoware_control_evaluator/src/control_evaluator_node.cpp index 5862522d582f7..47706ed56cd7a 100644 --- a/evaluator/autoware_control_evaluator/src/control_evaluator_node.cpp +++ b/evaluator/autoware_control_evaluator/src/control_evaluator_node.cpp @@ -14,6 +14,8 @@ #include "autoware/control_evaluator/control_evaluator_node.hpp" +#include "autoware/evaluator_utils/evaluator_utils.hpp" + #include #include @@ -64,57 +66,11 @@ void ControlEvaluatorNode::getRouteData() } } -void ControlEvaluatorNode::removeOldDiagnostics(const rclcpp::Time & stamp) -{ - constexpr double KEEP_TIME = 1.0; - diag_queue_.erase( - std::remove_if( - diag_queue_.begin(), diag_queue_.end(), - [stamp](const std::pair & p) { - return (stamp - p.second).seconds() > KEEP_TIME; - }), - diag_queue_.end()); -} - -void ControlEvaluatorNode::removeDiagnosticsByName(const std::string & name) -{ - diag_queue_.erase( - std::remove_if( - diag_queue_.begin(), diag_queue_.end(), - [&name](const std::pair & p) { - return p.first.name.find(name) != std::string::npos; - }), - diag_queue_.end()); -} - -void ControlEvaluatorNode::addDiagnostic( - const diagnostic_msgs::msg::DiagnosticStatus & diag, const rclcpp::Time & stamp) -{ - diag_queue_.push_back(std::make_pair(diag, stamp)); -} - -void ControlEvaluatorNode::updateDiagnosticQueue( - const DiagnosticArray & input_diagnostics, const std::string & function, - const rclcpp::Time & stamp) -{ - const auto it = std::find_if( - input_diagnostics.status.begin(), input_diagnostics.status.end(), - [&function](const diagnostic_msgs::msg::DiagnosticStatus & diag) { - return diag.name.find(function) != std::string::npos; - }); - if (it != input_diagnostics.status.end()) { - removeDiagnosticsByName(it->name); - addDiagnostic(*it, input_diagnostics.header.stamp); - } - - removeOldDiagnostics(stamp); -} - void ControlEvaluatorNode::onDiagnostics(const DiagnosticArray::ConstSharedPtr diag_msg) { // add target diagnostics to the queue and remove old ones for (const auto & function : target_functions_) { - updateDiagnosticQueue(*diag_msg, function, now()); + autoware::evaluator_utils::updateDiagnosticQueue(*diag_msg, function, now(), diag_queue_); } } diff --git a/evaluator/autoware_evaluator_utils/CMakeLists.txt b/evaluator/autoware_evaluator_utils/CMakeLists.txt new file mode 100644 index 0000000000000..d75ed43a4e3cb --- /dev/null +++ b/evaluator/autoware_evaluator_utils/CMakeLists.txt @@ -0,0 +1,13 @@ +cmake_minimum_required(VERSION 3.14) +project(autoware_evaluator_utils) + +find_package(autoware_cmake REQUIRED) +autoware_package() + +ament_auto_add_library(evaluator_utils SHARED + src/evaluator_utils.cpp +) + +ament_auto_package( + INSTALL_TO_SHARE +) diff --git a/evaluator/autoware_evaluator_utils/README.md b/evaluator/autoware_evaluator_utils/README.md new file mode 100644 index 0000000000000..b0db86f86b5c0 --- /dev/null +++ b/evaluator/autoware_evaluator_utils/README.md @@ -0,0 +1,5 @@ +# Evaluator Utils + +## Purpose + +This package provides utils functions for other evaluator packages diff --git a/evaluator/autoware_evaluator_utils/include/autoware/evaluator_utils/evaluator_utils.hpp b/evaluator/autoware_evaluator_utils/include/autoware/evaluator_utils/evaluator_utils.hpp new file mode 100644 index 0000000000000..3b91c9d7605e0 --- /dev/null +++ b/evaluator/autoware_evaluator_utils/include/autoware/evaluator_utils/evaluator_utils.hpp @@ -0,0 +1,45 @@ +// 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 AUTOWARE__EVALUATOR_UTILS__EVALUATOR_UTILS_HPP_ +#define AUTOWARE__EVALUATOR_UTILS__EVALUATOR_UTILS_HPP_ + +#include + +#include + +#include +#include +#include +#include +#include + +namespace autoware::evaluator_utils +{ + +using diagnostic_msgs::msg::DiagnosticArray; +using diagnostic_msgs::msg::DiagnosticStatus; +using DiagnosticQueue = std::deque>; + +void removeOldDiagnostics(const rclcpp::Time & stamp, DiagnosticQueue & diag_queue); +void removeDiagnosticsByName(const std::string & name, DiagnosticQueue & diag_queue); +void addDiagnostic( + const DiagnosticStatus & diag, const rclcpp::Time & stamp, DiagnosticQueue & diag_queue); +void updateDiagnosticQueue( + const DiagnosticArray & input_diagnostics, const std::string & function, + const rclcpp::Time & stamp, DiagnosticQueue & diag_queue); + +} // namespace autoware::evaluator_utils + +#endif // AUTOWARE__EVALUATOR_UTILS__EVALUATOR_UTILS_HPP_ diff --git a/evaluator/autoware_evaluator_utils/package.xml b/evaluator/autoware_evaluator_utils/package.xml new file mode 100644 index 0000000000000..b8566e33a32a3 --- /dev/null +++ b/evaluator/autoware_evaluator_utils/package.xml @@ -0,0 +1,24 @@ + + + + autoware_evaluator_utils + 0.1.0 + ROS 2 node for evaluating control + Daniel SANCHEZ + Takayuki MUROOKA + Apache License 2.0 + + Daniel SANCHEZ + Takayuki MUROOKA + + ament_cmake_auto + autoware_cmake + + diagnostic_msgs + rclcpp + rclcpp_components + + + ament_cmake + + diff --git a/evaluator/autoware_evaluator_utils/src/evaluator_utils.cpp b/evaluator/autoware_evaluator_utils/src/evaluator_utils.cpp new file mode 100644 index 0000000000000..293db21f50e7f --- /dev/null +++ b/evaluator/autoware_evaluator_utils/src/evaluator_utils.cpp @@ -0,0 +1,66 @@ +// 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 "autoware/evaluator_utils/evaluator_utils.hpp" + +#include + +namespace autoware::evaluator_utils +{ +void removeOldDiagnostics(const rclcpp::Time & stamp, DiagnosticQueue & diag_queue) +{ + constexpr double KEEP_TIME = 1.0; + diag_queue.erase( + std::remove_if( + diag_queue.begin(), diag_queue.end(), + [stamp](const std::pair & p) { + return (stamp - p.second).seconds() > KEEP_TIME; + }), + diag_queue.end()); +} + +void removeDiagnosticsByName(const std::string & name, DiagnosticQueue & diag_queue) +{ + diag_queue.erase( + std::remove_if( + diag_queue.begin(), diag_queue.end(), + [&name](const std::pair & p) { + return p.first.name.find(name) != std::string::npos; + }), + diag_queue.end()); +} + +void addDiagnostic( + const DiagnosticStatus & diag, const rclcpp::Time & stamp, DiagnosticQueue & diag_queue) +{ + diag_queue.push_back(std::make_pair(diag, stamp)); +} + +void updateDiagnosticQueue( + const DiagnosticArray & input_diagnostics, const std::string & function, + const rclcpp::Time & stamp, DiagnosticQueue & diag_queue) +{ + const auto it = std::find_if( + input_diagnostics.status.begin(), input_diagnostics.status.end(), + [&function](const DiagnosticStatus & diag) { + return diag.name.find(function) != std::string::npos; + }); + if (it != input_diagnostics.status.end()) { + removeDiagnosticsByName(it->name, diag_queue); + addDiagnostic(*it, input_diagnostics.header.stamp, diag_queue); + } + + removeOldDiagnostics(stamp, diag_queue); +} +} // namespace autoware::evaluator_utils diff --git a/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/planning_evaluator_node.hpp b/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/planning_evaluator_node.hpp index 91dc0386ae6c6..6925cb15e8fad 100644 --- a/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/planning_evaluator_node.hpp +++ b/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/planning_evaluator_node.hpp @@ -32,13 +32,14 @@ #include "geometry_msgs/msg/accel_with_covariance_stamped.hpp" #include "nav_msgs/msg/odometry.hpp" #include +#include #include #include #include #include +#include #include - namespace planning_diagnostics { using autoware_perception_msgs::msg::PredictedObjects; @@ -93,12 +94,22 @@ class PlanningEvaluatorNode : public rclcpp::Node const PoseWithUuidStamped::ConstSharedPtr modified_goal_msg, const Odometry::ConstSharedPtr ego_state_ptr); + /** + * @brief obtain diagnostics information + */ + void onDiagnostics(const DiagnosticArray::ConstSharedPtr diag_msg); + /** * @brief publish the given metric statistic */ DiagnosticStatus generateDiagnosticStatus( const Metric & metric, const Stat & metric_stat) const; + /** + * @brief publish current ego lane info + */ + DiagnosticStatus generateDiagnosticEvaluationStatus(const DiagnosticStatus & diag); + /** * @brief publish current ego lane info */ @@ -127,6 +138,10 @@ class PlanningEvaluatorNode : public rclcpp::Node * @brief fetch topic data */ void fetchData(); + // The diagnostics cycle is faster than timer, and each node publishes diagnostic separately. + // takeData() in onTimer() with a polling subscriber will miss a topic, so save all topics with + // onDiagnostics(). + rclcpp::Subscription::SharedPtr planning_diag_sub_; // ROS autoware::universe_utils::InterProcessPollingSubscriber traj_sub_{ @@ -164,6 +179,9 @@ class PlanningEvaluatorNode : public rclcpp::Node std::array>, static_cast(Metric::SIZE)> metric_stats_; rclcpp::TimerBase::SharedPtr timer_; + // queue for diagnostics and time stamp + std::deque> diag_queue_; + const std::vector target_functions_ = {"obstacle_cruise_planner"}; std::optional prev_acc_stamped_{std::nullopt}; }; } // namespace planning_diagnostics diff --git a/evaluator/autoware_planning_evaluator/launch/planning_evaluator.launch.xml b/evaluator/autoware_planning_evaluator/launch/planning_evaluator.launch.xml index 70301ebe6275f..c9517404b63a9 100644 --- a/evaluator/autoware_planning_evaluator/launch/planning_evaluator.launch.xml +++ b/evaluator/autoware_planning_evaluator/launch/planning_evaluator.launch.xml @@ -7,6 +7,7 @@ + @@ -15,6 +16,7 @@ + diff --git a/evaluator/autoware_planning_evaluator/package.xml b/evaluator/autoware_planning_evaluator/package.xml index 3c3d4bd8bb595..eb3161e0afb35 100644 --- a/evaluator/autoware_planning_evaluator/package.xml +++ b/evaluator/autoware_planning_evaluator/package.xml @@ -13,6 +13,7 @@ ament_cmake_auto autoware_cmake + autoware_evaluator_utils autoware_motion_utils autoware_perception_msgs autoware_planning_msgs diff --git a/evaluator/autoware_planning_evaluator/src/planning_evaluator_node.cpp b/evaluator/autoware_planning_evaluator/src/planning_evaluator_node.cpp index 9b65dbbc0b89e..ae3416b9cba8f 100644 --- a/evaluator/autoware_planning_evaluator/src/planning_evaluator_node.cpp +++ b/evaluator/autoware_planning_evaluator/src/planning_evaluator_node.cpp @@ -14,6 +14,8 @@ #include "autoware/planning_evaluator/planning_evaluator_node.hpp" +#include "autoware/evaluator_utils/evaluator_utils.hpp" + #include #include @@ -21,6 +23,7 @@ #include "boost/lexical_cast.hpp" +#include #include #include #include @@ -55,6 +58,9 @@ PlanningEvaluatorNode::PlanningEvaluatorNode(const rclcpp::NodeOptions & node_op output_file_str_ = declare_parameter("output_file"); ego_frame_str_ = declare_parameter("ego_frame"); + planning_diag_sub_ = create_subscription( + "~/input/diagnostics", 1, std::bind(&PlanningEvaluatorNode::onDiagnostics, this, _1)); + // List of metrics to calculate and publish metrics_pub_ = create_publisher("~/metrics", 1); for (const std::string & selected_metric : @@ -97,6 +103,29 @@ PlanningEvaluatorNode::~PlanningEvaluatorNode() } } +void PlanningEvaluatorNode::onDiagnostics(const DiagnosticArray::ConstSharedPtr diag_msg) +{ + // add target diagnostics to the queue and remove old ones + for (const auto & function : target_functions_) { + autoware::evaluator_utils::updateDiagnosticQueue(*diag_msg, function, now(), diag_queue_); + } +} + +DiagnosticStatus PlanningEvaluatorNode::generateDiagnosticEvaluationStatus( + const DiagnosticStatus & diag) +{ + DiagnosticStatus status; + status.name = diag.name; + + const auto it = std::find_if(diag.values.begin(), diag.values.end(), [](const auto & key_value) { + return key_value.key.find("decision") != std::string::npos; + }); + const bool found = it != diag.values.end(); + status.level = (found) ? status.OK : status.ERROR; + status.values.push_back((found) ? *it : diagnostic_msgs::msg::KeyValue{}); + return status; +} + void PlanningEvaluatorNode::getRouteData() { // route @@ -232,6 +261,23 @@ void PlanningEvaluatorNode::onTimer() const auto modified_goal_msg = modified_goal_sub_.takeData(); onModifiedGoal(modified_goal_msg, ego_state_ptr); } + + { + // generate decision diagnostics from input diagnostics + for (const auto & function : target_functions_) { + const auto it = std::find_if( + diag_queue_.begin(), diag_queue_.end(), + [&function](const std::pair & p) { + return p.first.name.find(function) != std::string::npos; + }); + if (it == diag_queue_.end()) { + continue; + } + // generate each decision diagnostics + metrics_msg_.status.push_back(generateDiagnosticEvaluationStatus(it->first)); + } + } + if (!metrics_msg_.status.empty()) { metrics_pub_->publish(metrics_msg_); } From 9c559e6ac5f6c24d51931f0bdf225de929ebf664 Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Wed, 10 Jul 2024 10:50:22 +0900 Subject: [PATCH 16/70] refactor(image_diagnostics): fix namespace and directory structure (#7915) refactor: update include paths and namespace for image_diagnostics Signed-off-by: Taekjin LEE --- sensing/image_diagnostics/CMakeLists.txt | 8 ++++---- .../image_diagnostics/src/image_diagnostics_node.cpp | 10 ++++++---- .../image_diagnostics_node.hpp | 11 ++++++----- 3 files changed, 16 insertions(+), 13 deletions(-) rename sensing/image_diagnostics/{include/image_diagnostics => src}/image_diagnostics_node.hpp (93%) diff --git a/sensing/image_diagnostics/CMakeLists.txt b/sensing/image_diagnostics/CMakeLists.txt index 800cd3f67d902..186d9340dc08a 100644 --- a/sensing/image_diagnostics/CMakeLists.txt +++ b/sensing/image_diagnostics/CMakeLists.txt @@ -28,14 +28,14 @@ include_directories( ) -ament_auto_add_library(image_diagnostics SHARED +ament_auto_add_library(${PROJECT_NAME} SHARED src/image_diagnostics_node.cpp ) -target_link_libraries(image_diagnostics +target_link_libraries(${PROJECT_NAME} ${OpenCV_LIBRARIES} ) -rclcpp_components_register_node(image_diagnostics - PLUGIN "image_diagnostics::ImageDiagNode" +rclcpp_components_register_node(${PROJECT_NAME} + PLUGIN "autoware::image_diagnostics::ImageDiagNode" EXECUTABLE image_diagnostics_node ) diff --git a/sensing/image_diagnostics/src/image_diagnostics_node.cpp b/sensing/image_diagnostics/src/image_diagnostics_node.cpp index 3db9ab94f50fa..49204f5cbc756 100644 --- a/sensing/image_diagnostics/src/image_diagnostics_node.cpp +++ b/sensing/image_diagnostics/src/image_diagnostics_node.cpp @@ -12,11 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "image_diagnostics/image_diagnostics_node.hpp" +#include "image_diagnostics_node.hpp" #include -namespace image_diagnostics +#include + +namespace autoware::image_diagnostics { using image_diagnostics::Image_State; ImageDiagNode::ImageDiagNode(const rclcpp::NodeOptions & node_options) @@ -271,7 +273,7 @@ void ImageDiagNode::shiftImage(cv::Mat & img) tmp.copyTo(left_bottom); } -} // namespace image_diagnostics +} // namespace autoware::image_diagnostics #include -RCLCPP_COMPONENTS_REGISTER_NODE(image_diagnostics::ImageDiagNode) +RCLCPP_COMPONENTS_REGISTER_NODE(autoware::image_diagnostics::ImageDiagNode) diff --git a/sensing/image_diagnostics/include/image_diagnostics/image_diagnostics_node.hpp b/sensing/image_diagnostics/src/image_diagnostics_node.hpp similarity index 93% rename from sensing/image_diagnostics/include/image_diagnostics/image_diagnostics_node.hpp rename to sensing/image_diagnostics/src/image_diagnostics_node.hpp index d6d150220555f..325624062b90b 100644 --- a/sensing/image_diagnostics/include/image_diagnostics/image_diagnostics_node.hpp +++ b/sensing/image_diagnostics/src/image_diagnostics_node.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef IMAGE_DIAGNOSTICS__IMAGE_DIAGNOSTICS_NODE_HPP_ -#define IMAGE_DIAGNOSTICS__IMAGE_DIAGNOSTICS_NODE_HPP_ +#ifndef IMAGE_DIAGNOSTICS_NODE_HPP_ +#define IMAGE_DIAGNOSTICS_NODE_HPP_ #include #include @@ -34,7 +34,8 @@ #include #include -namespace image_diagnostics + +namespace autoware::image_diagnostics { using diagnostic_updater::DiagnosticStatusWrapper; using diagnostic_updater::Updater; @@ -98,6 +99,6 @@ class ImageDiagNode : public rclcpp::Node rclcpp::Publisher::SharedPtr image_state_pub_; }; -} // namespace image_diagnostics +} // namespace autoware::image_diagnostics -#endif // IMAGE_DIAGNOSTICS__IMAGE_DIAGNOSTICS_NODE_HPP_ +#endif // IMAGE_DIAGNOSTICS_NODE_HPP_ From b43cffb3be7059640ed30b51e17e834a1ad3227d Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Wed, 10 Jul 2024 10:53:32 +0900 Subject: [PATCH 17/70] fix(controller): revival of dry steering (#7903) * Revert "fix(autoware_mpc_lateral_controller): delete the zero speed constraint (#7673)" This reverts commit 69258bd92cb8a0ff8320df9b2302db72975e027f. * dry steering Signed-off-by: Takayuki Murooka * add comments Signed-off-by: Takayuki Murooka * add minor fix and modify unit test for dry steering Signed-off-by: Takayuki Murooka --------- Signed-off-by: Takayuki Murooka --- .../autoware/mpc_lateral_controller/mpc.hpp | 5 +- .../src/mpc.cpp | 20 ++++-- .../pid_longitudinal_controller.hpp | 2 + .../src/pid_longitudinal_controller.cpp | 63 +++++++++++-------- .../test/test_controller_node.cpp | 27 ++++++-- 5 files changed, 79 insertions(+), 38 deletions(-) diff --git a/control/autoware_mpc_lateral_controller/include/autoware/mpc_lateral_controller/mpc.hpp b/control/autoware_mpc_lateral_controller/include/autoware/mpc_lateral_controller/mpc.hpp index 902790f260e5e..058eb45bfaaff 100644 --- a/control/autoware_mpc_lateral_controller/include/autoware/mpc_lateral_controller/mpc.hpp +++ b/control/autoware_mpc_lateral_controller/include/autoware/mpc_lateral_controller/mpc.hpp @@ -271,7 +271,7 @@ class MPC */ std::pair executeOptimization( const MPCMatrix & mpc_matrix, const VectorXd & x0, const double prediction_dt, - const MPCTrajectory & trajectory); + const MPCTrajectory & trajectory, const double current_velocity); /** * @brief Resample the trajectory with the MPC resampling time. @@ -386,7 +386,8 @@ class MPC * @param reference_trajectory The reference trajectory. * @param current_velocity current velocity of ego. */ - VectorXd calcSteerRateLimitOnTrajectory(const MPCTrajectory & trajectory) const; + VectorXd calcSteerRateLimitOnTrajectory( + const MPCTrajectory & trajectory, const double current_velocity) const; //!< @brief logging with warn and return false template diff --git a/control/autoware_mpc_lateral_controller/src/mpc.cpp b/control/autoware_mpc_lateral_controller/src/mpc.cpp index ea97e9e6d5f39..dfc4d82541bf8 100644 --- a/control/autoware_mpc_lateral_controller/src/mpc.cpp +++ b/control/autoware_mpc_lateral_controller/src/mpc.cpp @@ -76,8 +76,9 @@ bool MPC::calculateMPC( const auto mpc_matrix = generateMPCMatrix(mpc_resampled_ref_trajectory, prediction_dt); // solve Optimization problem - const auto [success_opt, Uex] = - executeOptimization(mpc_matrix, x0_delayed, prediction_dt, mpc_resampled_ref_trajectory); + const auto [success_opt, Uex] = executeOptimization( + mpc_matrix, x0_delayed, prediction_dt, mpc_resampled_ref_trajectory, + current_kinematics.twist.twist.linear.x); if (!success_opt) { return fail_warn_throttle("optimization failed. Stop MPC."); } @@ -543,7 +544,8 @@ MPCMatrix MPC::generateMPCMatrix( * [ -au_lim * dt ] < [uN-uN-1] < [ au_lim * dt ] (*N... DIM_U) */ std::pair MPC::executeOptimization( - const MPCMatrix & m, const VectorXd & x0, const double prediction_dt, const MPCTrajectory & traj) + const MPCMatrix & m, const VectorXd & x0, const double prediction_dt, const MPCTrajectory & traj, + const double current_velocity) { VectorXd Uex; @@ -576,7 +578,7 @@ std::pair MPC::executeOptimization( VectorXd ub = VectorXd::Constant(DIM_U_N, m_steer_lim); // max steering angle // steering angle rate limit - VectorXd steer_rate_limits = calcSteerRateLimitOnTrajectory(traj); + VectorXd steer_rate_limits = calcSteerRateLimitOnTrajectory(traj, current_velocity); VectorXd ubA = steer_rate_limits * prediction_dt; VectorXd lbA = -steer_rate_limits * prediction_dt; ubA(0) = m_raw_steer_cmd_prev + steer_rate_limits(0) * m_ctrl_period; @@ -728,7 +730,8 @@ double MPC::calcDesiredSteeringRate( return steer_rate; } -VectorXd MPC::calcSteerRateLimitOnTrajectory(const MPCTrajectory & trajectory) const +VectorXd MPC::calcSteerRateLimitOnTrajectory( + const MPCTrajectory & trajectory, const double current_velocity) const { const auto interp = [&](const auto & steer_rate_limit_map, const auto & current) { std::vector reference, limits; @@ -762,6 +765,13 @@ VectorXd MPC::calcSteerRateLimitOnTrajectory(const MPCTrajectory & trajectory) c return reference.back(); }; + // When the vehicle is stopped, a large steer rate limit is used for the dry steering. + constexpr double steer_rate_lim = 100.0; + const bool is_vehicle_stopped = std::fabs(current_velocity) < 0.01; + if (is_vehicle_stopped) { + return steer_rate_lim * VectorXd::Ones(m_param.prediction_horizon); + } + // calculate steering rate limit VectorXd steer_rate_limits = VectorXd::Zero(m_param.prediction_horizon); for (int i = 0; i < m_param.prediction_horizon; ++i) { diff --git a/control/autoware_pid_longitudinal_controller/include/autoware/pid_longitudinal_controller/pid_longitudinal_controller.hpp b/control/autoware_pid_longitudinal_controller/include/autoware/pid_longitudinal_controller/pid_longitudinal_controller.hpp index 5805ef7db9f86..0fd85b5822924 100644 --- a/control/autoware_pid_longitudinal_controller/include/autoware/pid_longitudinal_controller/pid_longitudinal_controller.hpp +++ b/control/autoware_pid_longitudinal_controller/include/autoware/pid_longitudinal_controller/pid_longitudinal_controller.hpp @@ -234,6 +234,8 @@ class PidLongitudinalController : public trajectory_follower::LongitudinalContro // debug values DebugValues m_debug_values; + std::optional m_prev_keep_stopped_condition{std::nullopt}; + std::shared_ptr m_last_running_time{std::make_shared(clock_->now())}; // Diagnostic diff --git a/control/autoware_pid_longitudinal_controller/src/pid_longitudinal_controller.cpp b/control/autoware_pid_longitudinal_controller/src/pid_longitudinal_controller.cpp index 93496c85c741b..a0118092413a9 100644 --- a/control/autoware_pid_longitudinal_controller/src/pid_longitudinal_controller.cpp +++ b/control/autoware_pid_longitudinal_controller/src/pid_longitudinal_controller.cpp @@ -599,21 +599,6 @@ void PidLongitudinalController::updateControlState(const ControlData & control_d // NOTE: the same velocity threshold as autoware::motion_utils::searchZeroVelocity static constexpr double vel_epsilon = 1e-3; - // Let vehicle start after the steering is converged for control accuracy - const bool keep_stopped_condition = std::fabs(current_vel) < vel_epsilon && - m_enable_keep_stopped_until_steer_convergence && - !lateral_sync_data_.is_steer_converged; - if (keep_stopped_condition) { - auto marker = createDefaultMarker( - "map", clock_->now(), "stop_reason", 0, Marker::TEXT_VIEW_FACING, - createMarkerScale(0.0, 0.0, 1.0), createMarkerColor(1.0, 1.0, 1.0, 0.999)); - marker.pose = autoware::universe_utils::calcOffsetPose( - m_current_kinematic_state.pose.pose, m_wheel_base + m_front_overhang, - m_vehicle_width / 2 + 2.0, 1.5); - marker.text = "steering not\nconverged"; - m_pub_stop_reason_marker->publish(marker); - } - const bool stopping_condition = stop_dist < p.stopping_state_stop_dist; const bool is_stopped = std::abs(current_vel) < p.stopped_state_entry_vel && @@ -672,15 +657,18 @@ void PidLongitudinalController::updateControlState(const ControlData & control_d m_under_control_starting_time = is_under_control ? std::make_shared(clock_->now()) : nullptr; } + + if (m_control_state != ControlState::STOPPED) { + m_prev_keep_stopped_condition = std::nullopt; + } + // transit state // in DRIVE state if (m_control_state == ControlState::DRIVE) { if (emergency_condition) { return changeState(ControlState::EMERGENCY); } - if (!is_under_control && stopped_condition && keep_stopped_condition) { - // NOTE: When the ego is stopped on manual driving, since the driving state may transit to - // autonomous, keep_stopped_condition should be checked. + if (!is_under_control && stopped_condition) { return changeState(ControlState::STOPPED); } @@ -723,19 +711,42 @@ void PidLongitudinalController::updateControlState(const ControlData & control_d // in STOPPED state if (m_control_state == ControlState::STOPPED) { - // -- debug print -- + // debug print if (has_nonzero_target_vel && !departure_condition_from_stopped) { debug_msg_once("target speed > 0, but departure condition is not met. Keep STOPPED."); } - if (has_nonzero_target_vel && keep_stopped_condition) { - debug_msg_once("target speed > 0, but keep stop condition is met. Keep STOPPED."); - } - // --------------- - if (keep_stopped_condition) { - return changeState(ControlState::STOPPED); - } if (departure_condition_from_stopped) { + // Let vehicle start after the steering is converged for dry steering + const bool current_keep_stopped_condition = + std::fabs(current_vel) < vel_epsilon && !lateral_sync_data_.is_steer_converged; + // NOTE: Dry steering is considered unnecessary when the steering is converged twice in a + // row. This is because lateral_sync_data_.is_steer_converged is not the current but + // the previous value due to the order controllers' run and sync functions. + const bool keep_stopped_condition = + !m_prev_keep_stopped_condition || + (current_keep_stopped_condition || *m_prev_keep_stopped_condition); + m_prev_keep_stopped_condition = current_keep_stopped_condition; + if (m_enable_keep_stopped_until_steer_convergence && keep_stopped_condition) { + // debug print + if (has_nonzero_target_vel) { + debug_msg_once("target speed > 0, but keep stop condition is met. Keep STOPPED."); + } + + // publish debug marker + auto marker = createDefaultMarker( + "map", clock_->now(), "stop_reason", 0, Marker::TEXT_VIEW_FACING, + createMarkerScale(0.0, 0.0, 1.0), createMarkerColor(1.0, 1.0, 1.0, 0.999)); + marker.pose = autoware::universe_utils::calcOffsetPose( + m_current_kinematic_state.pose.pose, m_wheel_base + m_front_overhang, + m_vehicle_width / 2 + 2.0, 1.5); + marker.text = "steering not\nconverged"; + m_pub_stop_reason_marker->publish(marker); + + // keep STOPPED + return; + } + m_pid_vel.reset(); m_lpf_vel_error->reset(0.0); // prevent the car from taking a long time to start to move diff --git a/control/autoware_trajectory_follower_node/test/test_controller_node.cpp b/control/autoware_trajectory_follower_node/test/test_controller_node.cpp index e0dc5ede906a9..48ffab52e2b15 100644 --- a/control/autoware_trajectory_follower_node/test/test_controller_node.cpp +++ b/control/autoware_trajectory_follower_node/test/test_controller_node.cpp @@ -589,11 +589,28 @@ TEST_F(FakeNodeFixture, longitudinal_check_steer_converged) traj.points.push_back(make_traj_point(0.0, 0.0, 1.0f)); traj.points.push_back(make_traj_point(50.0, 0.0, 1.0f)); traj.points.push_back(make_traj_point(100.0, 0.0, 1.0f)); - tester.traj_pub->publish(traj); - test_utils::waitForMessage(tester.node, this, tester.received_control_command); + { // Check if the ego can keep stopped when the steering is not converged. + tester.traj_pub->publish(traj); + test_utils::waitForMessage(tester.node, this, tester.received_control_command); - ASSERT_TRUE(tester.received_control_command); - // Keep stopped state when the lateral control is not converged. - EXPECT_DOUBLE_EQ(tester.cmd_msg->longitudinal.velocity, 0.0f); + ASSERT_TRUE(tester.received_control_command); + // Keep stopped state when the lateral control is not converged. + EXPECT_DOUBLE_EQ(tester.cmd_msg->longitudinal.velocity, 0.0f); + } + + { // Check if the ego can keep stopped after the following sequence + // 1. not converged -> 2. converged -> 3. not converged + tester.publish_steer_angle(0.0); + tester.traj_pub->publish(traj); + test_utils::waitForMessage(tester.node, this, tester.received_control_command); + + tester.publish_steer_angle(steering_tire_angle); + tester.traj_pub->publish(traj); + test_utils::waitForMessage(tester.node, this, tester.received_control_command); + + ASSERT_TRUE(tester.received_control_command); + // Keep stopped state when the lateral control is not converged. + EXPECT_DOUBLE_EQ(tester.cmd_msg->longitudinal.velocity, 0.0f); + } } From a39cfb7e0722631546028ed54de48b5d5105ae20 Mon Sep 17 00:00:00 2001 From: Yukinari Hisaki <42021302+yhisaki@users.noreply.github.com> Date: Wed, 10 Jul 2024 13:11:17 +0900 Subject: [PATCH 18/70] fix(autoware_behavior_velocity_no_drivable_lane_module): fix bug of uninitialization (#7928) Signed-off-by: Y.Hisaki --- .../src/scene.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_drivable_lane_module/src/scene.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_drivable_lane_module/src/scene.cpp index 42d896fec8ab3..465fd770049e9 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_drivable_lane_module/src/scene.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_drivable_lane_module/src/scene.cpp @@ -149,7 +149,7 @@ void NoDrivableLaneModule::handle_approaching_state(PathWithLaneId * path, StopR const auto & op_target_point_idx = autoware::motion_utils::insertTargetPoint(target_segment_idx, target_point, path->points, 5e-2); - size_t target_point_idx; + size_t target_point_idx = 0; if (op_target_point_idx) { target_point_idx = op_target_point_idx.value(); } From 6b9f164b123e2f6a6fedf7330e507d4b68e45a09 Mon Sep 17 00:00:00 2001 From: Kenzo Lobos Tsunekawa Date: Wed, 10 Jul 2024 13:36:31 +0900 Subject: [PATCH 19/70] fix: added temporary retrocompatibility to old perception data (#7929) Signed-off-by: Kenzo Lobos-Tsunekawa --- sensing/pointcloud_preprocessor/src/filter.cpp | 11 +++++++++-- 1 file changed, 9 insertions(+), 2 deletions(-) diff --git a/sensing/pointcloud_preprocessor/src/filter.cpp b/sensing/pointcloud_preprocessor/src/filter.cpp index a0b1814995d8b..435cecec8b6f2 100644 --- a/sensing/pointcloud_preprocessor/src/filter.cpp +++ b/sensing/pointcloud_preprocessor/src/filter.cpp @@ -413,12 +413,19 @@ bool pointcloud_preprocessor::Filter::convert_output_costly(std::unique_ptr Date: Wed, 10 Jul 2024 13:42:44 +0900 Subject: [PATCH 20/70] feat(lidar_transfusion): intensity as uint8 and tests (#7745) * feat(lidar_transfusion): intensity as uint8 and tests Signed-off-by: Amadeusz Szymko * style(pre-commit): autofix * fix(lidar_transfusion): headers include Signed-off-by: Amadeusz Szymko * feat(lidar_transfusion): use autoware_point_types for validation & refactor Signed-off-by: Amadeusz Szymko * style(lidar_transfusion): remove deprecated function Signed-off-by: Amadeusz Szymko * test(lidar_transfusion): point_step validation is not required Signed-off-by: Amadeusz Szymko * docs(lidar_transfusion): update assumptions / known limits Signed-off-by: Amadeusz Szymko * refactor(lidar_transfusion): redundant points structure Signed-off-by: Amadeusz Szymko * docs(lidar_transfusion): update assumptions / known limits Signed-off-by: Amadeusz Szymko * test(lidar_transfusion): temporary disable CUDA tests Signed-off-by: Amadeusz Szymko --------- Signed-off-by: Amadeusz Szymko Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> Co-authored-by: Kenzo Lobos Tsunekawa --- perception/lidar_transfusion/CMakeLists.txt | 52 +++ perception/lidar_transfusion/README.md | 18 ++ .../preprocess/pointcloud_densification.hpp | 2 +- .../preprocess/preprocess_kernel.hpp | 6 +- .../preprocess/voxel_generator.hpp | 8 +- .../include/lidar_transfusion/ros_utils.hpp | 68 ++++ .../include/lidar_transfusion/utils.hpp | 46 --- .../preprocess/pointcloud_densification.cpp | 9 +- .../lib/preprocess/preprocess_kernel.cu | 61 ++-- .../lib/preprocess/voxel_generator.cpp | 23 +- perception/lidar_transfusion/package.xml | 1 + .../test/test_detection_class_remapper.cpp | 92 ++++++ .../lidar_transfusion/test/test_nms.cpp | 121 +++++++ .../test/test_postprocess_kernel.cpp | 299 ++++++++++++++++++ .../test/test_postprocess_kernel.hpp | 48 +++ .../test/test_preprocess_kernel.cpp | 265 ++++++++++++++++ .../test/test_preprocess_kernel.hpp | 50 +++ .../lidar_transfusion/test/test_ros_utils.cpp | 104 ++++++ .../test/test_voxel_generator.cpp | 284 +++++++++++++++++ .../test/test_voxel_generator.hpp | 65 ++++ 20 files changed, 1517 insertions(+), 105 deletions(-) create mode 100644 perception/lidar_transfusion/test/test_detection_class_remapper.cpp create mode 100644 perception/lidar_transfusion/test/test_nms.cpp create mode 100644 perception/lidar_transfusion/test/test_postprocess_kernel.cpp create mode 100644 perception/lidar_transfusion/test/test_postprocess_kernel.hpp create mode 100644 perception/lidar_transfusion/test/test_preprocess_kernel.cpp create mode 100644 perception/lidar_transfusion/test/test_preprocess_kernel.hpp create mode 100644 perception/lidar_transfusion/test/test_ros_utils.cpp create mode 100644 perception/lidar_transfusion/test/test_voxel_generator.cpp create mode 100644 perception/lidar_transfusion/test/test_voxel_generator.hpp diff --git a/perception/lidar_transfusion/CMakeLists.txt b/perception/lidar_transfusion/CMakeLists.txt index c0e2d85f27e62..677a835e5b04e 100644 --- a/perception/lidar_transfusion/CMakeLists.txt +++ b/perception/lidar_transfusion/CMakeLists.txt @@ -137,6 +137,58 @@ if(TRT_AVAIL AND CUDA_AVAIL AND CUDNN_AVAIL) if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) ament_lint_auto_find_test_dependencies() + + find_package(ament_cmake_gtest REQUIRED) + ament_auto_add_gtest(test_detection_class_remapper + test/test_detection_class_remapper.cpp + ) + ament_auto_add_gtest(test_ros_utils + test/test_ros_utils.cpp + ) + ament_auto_add_gtest(test_nms + test/test_nms.cpp + ) + + # Temporary disabled, tracked by: + # https://github.com/autowarefoundation/autoware.universe/issues/7724 + # ament_auto_add_gtest(test_voxel_generator + # test/test_voxel_generator.cpp + # ) + + # # preprocess kernel test + # add_executable(test_preprocess_kernel + # test/test_preprocess_kernel.cpp + # ) + # target_include_directories(test_preprocess_kernel PUBLIC + # ${test_preprocess_kernel_SOURCE_DIR} + # ) + # target_link_libraries(test_preprocess_kernel + # transfusion_cuda_lib + # gtest + # gtest_main + # ) + # ament_add_test(test_preprocess_kernel + # GENERATE_RESULT_FOR_RETURN_CODE_ZERO + # COMMAND "$" + # ) + + # # postprocess kernel test + # add_executable(test_postprocess_kernel + # test/test_postprocess_kernel.cpp + # ) + # target_include_directories(test_postprocess_kernel PUBLIC + # ${test_postprocess_kernel_SOURCE_DIR} + # ) + # target_link_libraries(test_postprocess_kernel + # transfusion_cuda_lib + # gtest + # gtest_main + # ) + # ament_add_test(test_postprocess_kernel + # GENERATE_RESULT_FOR_RETURN_CODE_ZERO + # COMMAND "$" + # ) + endif() ament_auto_package( diff --git a/perception/lidar_transfusion/README.md b/perception/lidar_transfusion/README.md index 7974714720c32..e30403141e8ad 100644 --- a/perception/lidar_transfusion/README.md +++ b/perception/lidar_transfusion/README.md @@ -59,6 +59,24 @@ ros2 launch lidar_transfusion lidar_transfusion.launch.xml log_level:=debug ## Assumptions / Known limits +This library operates on raw cloud data (bytes). It is assumed that the input pointcloud message has following format: + +```python +[ + sensor_msgs.msg.PointField(name='x', offset=0, datatype=7, count=1), + sensor_msgs.msg.PointField(name='y', offset=4, datatype=7, count=1), + sensor_msgs.msg.PointField(name='z', offset=8, datatype=7, count=1), + sensor_msgs.msg.PointField(name='intensity', offset=12, datatype=2, count=1) +] +``` + +This input may consist of other fields as well - shown format is required minimum. +For debug purposes, you can validate your pointcloud topic using simple command: + +```bash +ros2 topic echo --field fields +``` + ## Trained Models You can download the onnx format of trained models by clicking on the links below. diff --git a/perception/lidar_transfusion/include/lidar_transfusion/preprocess/pointcloud_densification.hpp b/perception/lidar_transfusion/include/lidar_transfusion/preprocess/pointcloud_densification.hpp index 25095f4dc9c0b..6ac0a6544389f 100644 --- a/perception/lidar_transfusion/include/lidar_transfusion/preprocess/pointcloud_densification.hpp +++ b/perception/lidar_transfusion/include/lidar_transfusion/preprocess/pointcloud_densification.hpp @@ -51,7 +51,7 @@ class DensificationParam struct PointCloudWithTransform { - cuda::unique_ptr points_d{nullptr}; + cuda::unique_ptr data_d{nullptr}; std_msgs::msg::Header header; size_t num_points{0}; Eigen::Affine3f affine_past2world; diff --git a/perception/lidar_transfusion/include/lidar_transfusion/preprocess/preprocess_kernel.hpp b/perception/lidar_transfusion/include/lidar_transfusion/preprocess/preprocess_kernel.hpp index c571990d84b51..592f09c2d288a 100644 --- a/perception/lidar_transfusion/include/lidar_transfusion/preprocess/preprocess_kernel.hpp +++ b/perception/lidar_transfusion/include/lidar_transfusion/preprocess/preprocess_kernel.hpp @@ -56,12 +56,8 @@ class PreprocessCuda unsigned int * mask, float * voxels, unsigned int * pillar_num, float * voxel_features, unsigned int * voxel_num, unsigned int * voxel_idxs); - // cudaError_t generateVoxelsInput_launch( - // uint8_t * cloud_data, CloudInfo & cloud_info, unsigned int points_agg, unsigned int - // points_size, float time_lag, float * affine_transform, float * points); - cudaError_t generateSweepPoints_launch( - const float * input_points, size_t points_size, int input_point_step, float time_lag, + const uint8_t * input_data, size_t points_size, int input_point_step, float time_lag, const float * transform, float * output_points); private: diff --git a/perception/lidar_transfusion/include/lidar_transfusion/preprocess/voxel_generator.hpp b/perception/lidar_transfusion/include/lidar_transfusion/preprocess/voxel_generator.hpp index 3e3660e238473..f0d253ee28755 100644 --- a/perception/lidar_transfusion/include/lidar_transfusion/preprocess/voxel_generator.hpp +++ b/perception/lidar_transfusion/include/lidar_transfusion/preprocess/voxel_generator.hpp @@ -18,8 +18,8 @@ #include "lidar_transfusion/cuda_utils.hpp" #include "lidar_transfusion/preprocess/pointcloud_densification.hpp" #include "lidar_transfusion/preprocess/preprocess_kernel.hpp" +#include "lidar_transfusion/ros_utils.hpp" #include "lidar_transfusion/transfusion_config.hpp" -#include "lidar_transfusion/utils.hpp" #ifdef ROS_DISTRO_GALACTIC #include @@ -27,6 +27,8 @@ #include #endif +#include + #include #include @@ -36,8 +38,8 @@ namespace lidar_transfusion { -constexpr size_t AFF_MAT_SIZE = 16; // 4x4 matrix -constexpr size_t MAX_CLOUD_STEP_SIZE = 35; // PointXYZIRCADT +constexpr size_t AFF_MAT_SIZE = 16; // 4x4 matrix +constexpr size_t MAX_CLOUD_STEP_SIZE = sizeof(autoware_point_types::PointXYZIRCAEDT); class VoxelGenerator { diff --git a/perception/lidar_transfusion/include/lidar_transfusion/ros_utils.hpp b/perception/lidar_transfusion/include/lidar_transfusion/ros_utils.hpp index f013a02404a29..cbfc4e87b1610 100644 --- a/perception/lidar_transfusion/include/lidar_transfusion/ros_utils.hpp +++ b/perception/lidar_transfusion/include/lidar_transfusion/ros_utils.hpp @@ -17,16 +17,84 @@ #include "lidar_transfusion/utils.hpp" +#include + #include #include #include #include +#include +#include #include #include +#define CHECK_OFFSET(offset, structure, field) \ + static_assert( \ + offsetof(structure, field) == offset, \ + "Offset of " #field " in " #structure " does not match expected offset.") +#define CHECK_TYPE(type, structure, field) \ + static_assert( \ + std::is_same::value, \ + "Type of " #field " in " #structure " does not match expected type.") +#define CHECK_FIELD(offset, type, structure, field) \ + CHECK_OFFSET(offset, structure, field); \ + CHECK_TYPE(type, structure, field) + namespace lidar_transfusion { +using sensor_msgs::msg::PointField; + +CHECK_FIELD(0, float, autoware_point_types::PointXYZIRCAEDT, x); +CHECK_FIELD(4, float, autoware_point_types::PointXYZIRCAEDT, y); +CHECK_FIELD(8, float, autoware_point_types::PointXYZIRCAEDT, z); +CHECK_FIELD(12, uint8_t, autoware_point_types::PointXYZIRCAEDT, intensity); + +struct CloudInfo +{ + uint32_t x_offset{0}; + uint32_t y_offset{sizeof(float)}; + uint32_t z_offset{sizeof(float) * 2}; + uint32_t intensity_offset{sizeof(float) * 3}; + uint8_t x_datatype{PointField::FLOAT32}; + uint8_t y_datatype{PointField::FLOAT32}; + uint8_t z_datatype{PointField::FLOAT32}; + uint8_t intensity_datatype{PointField::UINT8}; + uint8_t x_count{1}; + uint8_t y_count{1}; + uint8_t z_count{1}; + uint8_t intensity_count{1}; + uint32_t point_step{sizeof(autoware_point_types::PointXYZIRCAEDT)}; + bool is_bigendian{false}; + + bool operator!=(const CloudInfo & rhs) const + { + return x_offset != rhs.x_offset || y_offset != rhs.y_offset || z_offset != rhs.z_offset || + intensity_offset != rhs.intensity_offset || x_datatype != rhs.x_datatype || + y_datatype != rhs.y_datatype || z_datatype != rhs.z_datatype || + intensity_datatype != rhs.intensity_datatype || x_count != rhs.x_count || + y_count != rhs.y_count || z_count != rhs.z_count || + intensity_count != rhs.intensity_count || is_bigendian != rhs.is_bigendian; + } + + friend std::ostream & operator<<(std::ostream & os, const CloudInfo & info) + { + os << "x_offset: " << static_cast(info.x_offset) << std::endl; + os << "y_offset: " << static_cast(info.y_offset) << std::endl; + os << "z_offset: " << static_cast(info.z_offset) << std::endl; + os << "intensity_offset: " << static_cast(info.intensity_offset) << std::endl; + os << "x_datatype: " << static_cast(info.x_datatype) << std::endl; + os << "y_datatype: " << static_cast(info.y_datatype) << std::endl; + os << "z_datatype: " << static_cast(info.z_datatype) << std::endl; + os << "intensity_datatype: " << static_cast(info.intensity_datatype) << std::endl; + os << "x_count: " << static_cast(info.x_count) << std::endl; + os << "y_count: " << static_cast(info.y_count) << std::endl; + os << "z_count: " << static_cast(info.z_count) << std::endl; + os << "intensity_count: " << static_cast(info.intensity_count) << std::endl; + os << "is_bigendian: " << static_cast(info.is_bigendian) << std::endl; + return os; + } +}; void box3DToDetectedObject( const Box3D & box3d, const std::vector & class_names, diff --git a/perception/lidar_transfusion/include/lidar_transfusion/utils.hpp b/perception/lidar_transfusion/include/lidar_transfusion/utils.hpp index e73fe7f055953..cc40e55851738 100644 --- a/perception/lidar_transfusion/include/lidar_transfusion/utils.hpp +++ b/perception/lidar_transfusion/include/lidar_transfusion/utils.hpp @@ -36,52 +36,6 @@ struct Box3D float yaw; }; -struct CloudInfo -{ - uint32_t x_offset; - uint32_t y_offset; - uint32_t z_offset; - uint32_t intensity_offset; - uint8_t x_datatype; - uint8_t y_datatype; - uint8_t z_datatype; - uint8_t intensity_datatype; - uint8_t x_count; - uint8_t y_count; - uint8_t z_count; - uint8_t intensity_count; - uint32_t point_step; - bool is_bigendian; - - CloudInfo() - : x_offset(0), - y_offset(4), - z_offset(8), - intensity_offset(12), - x_datatype(7), - y_datatype(7), - z_datatype(7), - intensity_datatype(7), - x_count(1), - y_count(1), - z_count(1), - intensity_count(1), - point_step(16), - is_bigendian(false) - { - } - - bool operator!=(const CloudInfo & rhs) const - { - return x_offset != rhs.x_offset || y_offset != rhs.y_offset || z_offset != rhs.z_offset || - intensity_offset != rhs.intensity_offset || x_datatype != rhs.x_datatype || - y_datatype != rhs.y_datatype || z_datatype != rhs.z_datatype || - intensity_datatype != rhs.intensity_datatype || x_count != rhs.x_count || - y_count != rhs.y_count || z_count != rhs.z_count || - intensity_count != rhs.intensity_count || is_bigendian != rhs.is_bigendian; - } -}; - enum NetworkIO { voxels = 0, num_points, coors, cls_score, dir_pred, bbox_pred, ENUM_SIZE }; // cspell: ignore divup diff --git a/perception/lidar_transfusion/lib/preprocess/pointcloud_densification.cpp b/perception/lidar_transfusion/lib/preprocess/pointcloud_densification.cpp index 774b3a05d71c1..c13423f2d24d8 100644 --- a/perception/lidar_transfusion/lib/preprocess/pointcloud_densification.cpp +++ b/perception/lidar_transfusion/lib/preprocess/pointcloud_densification.cpp @@ -93,16 +93,15 @@ void PointCloudDensification::enqueue( affine_world2current_ = affine_world2current; current_timestamp_ = rclcpp::Time(msg.header.stamp).seconds(); - assert(sizeof(uint8_t) * msg.width * msg.height * msg.point_step % sizeof(float) == 1); - auto points_d = cuda::make_unique( - sizeof(uint8_t) * msg.width * msg.height * msg.point_step / sizeof(float)); + auto data_d = cuda::make_unique( + sizeof(uint8_t) * msg.width * msg.height * msg.point_step / sizeof(uint8_t)); CHECK_CUDA_ERROR(cudaMemcpyAsync( - points_d.get(), msg.data.data(), sizeof(uint8_t) * msg.width * msg.height * msg.point_step, + data_d.get(), msg.data.data(), sizeof(uint8_t) * msg.width * msg.height * msg.point_step, cudaMemcpyHostToDevice, stream_)); PointCloudWithTransform pointcloud = { - std::move(points_d), msg.header, msg.width * msg.height, affine_world2current.inverse()}; + std::move(data_d), msg.header, msg.width * msg.height, affine_world2current.inverse()}; pointcloud_cache_.push_front(std::move(pointcloud)); } diff --git a/perception/lidar_transfusion/lib/preprocess/preprocess_kernel.cu b/perception/lidar_transfusion/lib/preprocess/preprocess_kernel.cu index b8f9ae998fd4f..48bb4eb9332a8 100644 --- a/perception/lidar_transfusion/lib/preprocess/preprocess_kernel.cu +++ b/perception/lidar_transfusion/lib/preprocess/preprocess_kernel.cu @@ -31,6 +31,8 @@ #include "lidar_transfusion/cuda_utils.hpp" #include "lidar_transfusion/preprocess/preprocess_kernel.hpp" +#include + namespace lidar_transfusion { @@ -99,9 +101,12 @@ __global__ void generateVoxels_random_kernel( cudaError_t PreprocessCuda::generateVoxels_random_launch( float * points, unsigned int points_size, unsigned int * mask, float * voxels) { - int threadNum = config_.threads_for_voxel_; - dim3 blocks((points_size + threadNum - 1) / threadNum); - dim3 threads(threadNum); + if (points_size == 0) { + return cudaGetLastError(); + } + dim3 blocks(divup(points_size, config_.threads_for_voxel_)); + dim3 threads(config_.threads_for_voxel_); + generateVoxels_random_kernel<<>>( points, points_size, config_.min_x_range_, config_.max_x_range_, config_.min_y_range_, config_.max_y_range_, config_.min_z_range_, config_.max_z_range_, config_.voxel_x_size_, @@ -165,40 +170,48 @@ cudaError_t PreprocessCuda::generateBaseFeatures_launch( } __global__ void generateSweepPoints_kernel( - const float * input_points, size_t points_size, int input_point_step, float time_lag, + const uint8_t * input_data, size_t points_size, int input_point_step, float time_lag, const float * transform_array, int num_features, float * output_points) { int point_idx = blockIdx.x * blockDim.x + threadIdx.x; if (point_idx >= points_size) return; - const float input_x = input_points[point_idx * input_point_step + 0]; - const float input_y = input_points[point_idx * input_point_step + 1]; - const float input_z = input_points[point_idx * input_point_step + 2]; - const float intensity = input_points[point_idx * input_point_step + 3]; - - output_points[point_idx * num_features] = transform_array[0] * input_x + - transform_array[4] * input_y + - transform_array[8] * input_z + transform_array[12]; - output_points[point_idx * num_features + 1] = transform_array[1] * input_x + - transform_array[5] * input_y + - transform_array[9] * input_z + transform_array[13]; - output_points[point_idx * num_features + 2] = transform_array[2] * input_x + - transform_array[6] * input_y + - transform_array[10] * input_z + transform_array[14]; - output_points[point_idx * num_features + 3] = intensity; + union { + uint32_t raw{0}; + float value; + } input_x, input_y, input_z; + +#pragma unroll + for (int i = 0; i < 4; i++) { // 4 bytes for float32 + input_x.raw |= input_data[point_idx * input_point_step + i] << i * 8; + input_y.raw |= input_data[point_idx * input_point_step + i + 4] << i * 8; + input_z.raw |= input_data[point_idx * input_point_step + i + 8] << i * 8; + } + + float input_intensity = static_cast(input_data[point_idx * input_point_step + 12]); + + output_points[point_idx * num_features] = + transform_array[0] * input_x.value + transform_array[4] * input_y.value + + transform_array[8] * input_z.value + transform_array[12]; + output_points[point_idx * num_features + 1] = + transform_array[1] * input_x.value + transform_array[5] * input_y.value + + transform_array[9] * input_z.value + transform_array[13]; + output_points[point_idx * num_features + 2] = + transform_array[2] * input_x.value + transform_array[6] * input_y.value + + transform_array[10] * input_z.value + transform_array[14]; + output_points[point_idx * num_features + 3] = input_intensity; output_points[point_idx * num_features + 4] = time_lag; } cudaError_t PreprocessCuda::generateSweepPoints_launch( - const float * input_points, size_t points_size, int input_point_step, float time_lag, + const uint8_t * input_data, size_t points_size, int input_point_step, float time_lag, const float * transform_array, float * output_points) { - int threadNum = config_.threads_for_voxel_; - dim3 blocks((points_size + threadNum - 1) / threadNum); - dim3 threads(threadNum); + dim3 blocks(divup(points_size, config_.threads_for_voxel_)); + dim3 threads(config_.threads_for_voxel_); generateSweepPoints_kernel<<>>( - input_points, points_size, input_point_step, time_lag, transform_array, + input_data, points_size, input_point_step, time_lag, transform_array, config_.num_point_feature_size_, output_points); cudaError_t err = cudaGetLastError(); diff --git a/perception/lidar_transfusion/lib/preprocess/voxel_generator.cpp b/perception/lidar_transfusion/lib/preprocess/voxel_generator.cpp index 7072e8491af66..cb8bac984aef3 100644 --- a/perception/lidar_transfusion/lib/preprocess/voxel_generator.cpp +++ b/perception/lidar_transfusion/lib/preprocess/voxel_generator.cpp @@ -23,25 +23,6 @@ namespace lidar_transfusion { -std::ostream & operator<<(std::ostream & os, const CloudInfo & info) -{ - os << "x_offset: " << static_cast(info.x_offset) << std::endl; - os << "y_offset: " << static_cast(info.y_offset) << std::endl; - os << "z_offset: " << static_cast(info.z_offset) << std::endl; - os << "intensity_offset: " << static_cast(info.intensity_offset) << std::endl; - os << "x_datatype: " << static_cast(info.x_datatype) << std::endl; - os << "y_datatype: " << static_cast(info.y_datatype) << std::endl; - os << "z_datatype: " << static_cast(info.z_datatype) << std::endl; - os << "intensity_datatype: " << static_cast(info.intensity_datatype) << std::endl; - os << "x_count: " << static_cast(info.x_count) << std::endl; - os << "y_count: " << static_cast(info.y_count) << std::endl; - os << "z_count: " << static_cast(info.z_count) << std::endl; - os << "intensity_count: " << static_cast(info.intensity_count) << std::endl; - os << "point_step: " << static_cast(info.point_step) << std::endl; - os << "is_bigendian: " << static_cast(info.is_bigendian) << std::endl; - return os; -} - VoxelGenerator::VoxelGenerator( const DensificationParam & densification_param, const TransfusionConfig & config, cudaStream_t & stream) @@ -106,8 +87,8 @@ std::size_t VoxelGenerator::generateSweepPoints( CHECK_CUDA_ERROR(cudaStreamSynchronize(stream_)); pre_ptr_->generateSweepPoints_launch( - pc_cache_iter->points_d.get(), sweep_num_points, cloud_info_.point_step / sizeof(float), - time_lag, affine_past2current_d_.get(), points_d.get() + shift); + pc_cache_iter->data_d.get(), sweep_num_points, cloud_info_.point_step, time_lag, + affine_past2current_d_.get(), points_d.get() + shift); point_counter += sweep_num_points; } diff --git a/perception/lidar_transfusion/package.xml b/perception/lidar_transfusion/package.xml index ffbe091b998e5..3b495025b1c34 100644 --- a/perception/lidar_transfusion/package.xml +++ b/perception/lidar_transfusion/package.xml @@ -13,6 +13,7 @@ autoware_cmake autoware_perception_msgs + autoware_point_types autoware_universe_utils launch_ros object_recognition_utils diff --git a/perception/lidar_transfusion/test/test_detection_class_remapper.cpp b/perception/lidar_transfusion/test/test_detection_class_remapper.cpp new file mode 100644 index 0000000000000..6e7f896e44d2c --- /dev/null +++ b/perception/lidar_transfusion/test/test_detection_class_remapper.cpp @@ -0,0 +1,92 @@ +// 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 + +#include + +TEST(DetectionClassRemapperTest, MapClasses) +{ + lidar_transfusion::DetectionClassRemapper remapper; + + // Set up the parameters for the remapper + // Labels: CAR, TRUCK, TRAILER + std::vector allow_remapping_by_area_matrix = { + 0, 0, 0, // CAR cannot be remapped + 0, 0, 1, // TRUCK can be remapped to TRAILER + 0, 1, 0 // TRAILER can be remapped to TRUCK + }; + std::vector min_area_matrix = {0.0, 0.0, 0.0, 0.0, 0.0, 10.0, 0.0, 0.0, 0.0}; + std::vector max_area_matrix = {0.0, 0.0, 0.0, 0.0, 0.0, 999.0, 0.0, 10.0, 0.0}; + + remapper.setParameters(allow_remapping_by_area_matrix, min_area_matrix, max_area_matrix); + + // Create a DetectedObjects message with some objects + autoware_perception_msgs::msg::DetectedObjects msg; + + // CAR with area 4.0, which is out of the range for remapping + autoware_perception_msgs::msg::DetectedObject obj1; + autoware_perception_msgs::msg::ObjectClassification obj1_classification; + obj1.shape.dimensions.x = 2.0; + obj1.shape.dimensions.y = 2.0; + obj1_classification.label = 0; + obj1_classification.probability = 1.0; + obj1.classification = {obj1_classification}; + msg.objects.push_back(obj1); + + // TRUCK with area 16.0, which is in the range for remapping to TRAILER + autoware_perception_msgs::msg::DetectedObject obj2; + autoware_perception_msgs::msg::ObjectClassification obj2_classification; + obj2.shape.dimensions.x = 8.0; + obj2.shape.dimensions.y = 2.0; + obj2_classification.label = 1; + obj2_classification.probability = 1.0; + obj2.classification = {obj2_classification}; + msg.objects.push_back(obj2); + + // TRAILER with area 9.0, which is in the range for remapping to TRUCK + autoware_perception_msgs::msg::DetectedObject obj3; + autoware_perception_msgs::msg::ObjectClassification obj3_classification; + obj3.shape.dimensions.x = 3.0; + obj3.shape.dimensions.y = 3.0; + obj3_classification.label = 2; + obj3_classification.probability = 1.0; + obj3.classification = {obj3_classification}; + msg.objects.push_back(obj3); + + // TRAILER with area 12.0, which is out of the range for remapping + autoware_perception_msgs::msg::DetectedObject obj4; + autoware_perception_msgs::msg::ObjectClassification obj4_classification; + obj4.shape.dimensions.x = 4.0; + obj4.shape.dimensions.y = 3.0; + obj4_classification.label = 2; + obj4_classification.probability = 1.0; + obj4.classification = {obj4_classification}; + msg.objects.push_back(obj4); + + // Call the mapClasses function + remapper.mapClasses(msg); + + // Check the remapped labels + EXPECT_EQ(msg.objects[0].classification[0].label, 0); + EXPECT_EQ(msg.objects[1].classification[0].label, 2); + EXPECT_EQ(msg.objects[2].classification[0].label, 1); + EXPECT_EQ(msg.objects[3].classification[0].label, 2); +} + +int main(int argc, char ** argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/perception/lidar_transfusion/test/test_nms.cpp b/perception/lidar_transfusion/test/test_nms.cpp new file mode 100644 index 0000000000000..b6f0ec8c31684 --- /dev/null +++ b/perception/lidar_transfusion/test/test_nms.cpp @@ -0,0 +1,121 @@ +// 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 "lidar_transfusion/postprocess/non_maximum_suppression.hpp" + +#include + +TEST(NonMaximumSuppressionTest, Apply) +{ + lidar_transfusion::NonMaximumSuppression nms; + lidar_transfusion::NMSParams params; + params.search_distance_2d_ = 1.0; + params.iou_threshold_ = 0.2; + params.nms_type_ = lidar_transfusion::NMS_TYPE::IoU_BEV; + params.target_class_names_ = {"CAR"}; + nms.setParameters(params); + + std::vector input_objects(4); + + // Object 1 + autoware_perception_msgs::msg::ObjectClassification obj1_classification; + obj1_classification.label = 0; // Assuming "car" has label 0 + obj1_classification.probability = 1.0; + input_objects[0].classification = {obj1_classification}; // Assuming "car" has label 0 + input_objects[0].kinematics.pose_with_covariance.pose.position.x = 0.0; + input_objects[0].kinematics.pose_with_covariance.pose.position.y = 0.0; + input_objects[0].kinematics.pose_with_covariance.pose.orientation.x = 0.0; + input_objects[0].kinematics.pose_with_covariance.pose.orientation.y = 0.0; + input_objects[0].kinematics.pose_with_covariance.pose.orientation.z = 0.0; + input_objects[0].kinematics.pose_with_covariance.pose.orientation.w = 1.0; + input_objects[0].shape.type = autoware_perception_msgs::msg::Shape::BOUNDING_BOX; + input_objects[0].shape.dimensions.x = 4.0; + input_objects[0].shape.dimensions.y = 2.0; + + // Object 2 (overlaps with Object 1) + autoware_perception_msgs::msg::ObjectClassification obj2_classification; + obj2_classification.label = 0; // Assuming "car" has label 0 + obj2_classification.probability = 1.0; + input_objects[1].classification = {obj2_classification}; // Assuming "car" has label 0 + input_objects[1].kinematics.pose_with_covariance.pose.position.x = 0.5; + input_objects[1].kinematics.pose_with_covariance.pose.position.y = 0.5; + input_objects[1].kinematics.pose_with_covariance.pose.orientation.x = 0.0; + input_objects[1].kinematics.pose_with_covariance.pose.orientation.y = 0.0; + input_objects[1].kinematics.pose_with_covariance.pose.orientation.z = 0.0; + input_objects[1].kinematics.pose_with_covariance.pose.orientation.w = 1.0; + input_objects[1].shape.type = autoware_perception_msgs::msg::Shape::BOUNDING_BOX; + input_objects[1].shape.dimensions.x = 4.0; + input_objects[1].shape.dimensions.y = 2.0; + + // Object 3 + autoware_perception_msgs::msg::ObjectClassification obj3_classification; + obj3_classification.label = 0; // Assuming "car" has label 0 + obj3_classification.probability = 1.0; + input_objects[2].classification = {obj3_classification}; // Assuming "car" has label 0 + input_objects[2].kinematics.pose_with_covariance.pose.position.x = 5.0; + input_objects[2].kinematics.pose_with_covariance.pose.position.y = 5.0; + input_objects[2].kinematics.pose_with_covariance.pose.orientation.x = 0.0; + input_objects[2].kinematics.pose_with_covariance.pose.orientation.y = 0.0; + input_objects[2].kinematics.pose_with_covariance.pose.orientation.z = 0.0; + input_objects[2].kinematics.pose_with_covariance.pose.orientation.w = 1.0; + input_objects[2].shape.type = autoware_perception_msgs::msg::Shape::BOUNDING_BOX; + input_objects[2].shape.dimensions.x = 4.0; + input_objects[2].shape.dimensions.y = 2.0; + + // Object 4 (different class) + autoware_perception_msgs::msg::ObjectClassification obj4_classification; + obj4_classification.label = 1; // Assuming "pedestrian" has label 1 + obj4_classification.probability = 1.0; + input_objects[3].classification = {obj4_classification}; // Assuming "pedestrian" has label 1 + input_objects[3].kinematics.pose_with_covariance.pose.position.x = 0.0; + input_objects[3].kinematics.pose_with_covariance.pose.position.y = 0.0; + input_objects[3].kinematics.pose_with_covariance.pose.orientation.x = 0.0; + input_objects[3].kinematics.pose_with_covariance.pose.orientation.y = 0.0; + input_objects[3].kinematics.pose_with_covariance.pose.orientation.z = 0.0; + input_objects[3].kinematics.pose_with_covariance.pose.orientation.w = 1.0; + input_objects[3].shape.type = autoware_perception_msgs::msg::Shape::BOUNDING_BOX; + input_objects[3].shape.dimensions.x = 0.5; + input_objects[3].shape.dimensions.y = 0.5; + + std::vector output_objects = nms.apply(input_objects); + + // Assert the expected number of output objects + EXPECT_EQ(output_objects.size(), 3); + + // Assert that input_objects[2] is included in the output_objects + bool is_input_object_2_included = false; + for (const auto & output_object : output_objects) { + if (output_object == input_objects[2]) { + is_input_object_2_included = true; + break; + } + } + EXPECT_TRUE(is_input_object_2_included); + + // Assert that input_objects[3] is included in the output_objects + bool is_input_object_3_included = false; + for (const auto & output_object : output_objects) { + if (output_object == input_objects[3]) { + is_input_object_3_included = true; + break; + } + } + EXPECT_TRUE(is_input_object_3_included); +} + +int main(int argc, char ** argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/perception/lidar_transfusion/test/test_postprocess_kernel.cpp b/perception/lidar_transfusion/test/test_postprocess_kernel.cpp new file mode 100644 index 0000000000000..1e01db5a8b3d7 --- /dev/null +++ b/perception/lidar_transfusion/test/test_postprocess_kernel.cpp @@ -0,0 +1,299 @@ +// 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 "test_postprocess_kernel.hpp" + +#include + +#include +#include + +namespace lidar_transfusion +{ + +void PostprocessKernelTest::SetUp() +{ + cudaStreamCreate(&stream_); + + auto voxels_num = std::vector{1, 3, 5}; + auto point_cloud_range = std::vector{-76.8, -76.8, -3.0, 76.8, 76.8, 5.0}; + auto voxel_size = std::vector{0.3, 0.3, 8.0}; + auto score_threshold = 0.2f; + auto circle_nms_dist_threshold = 0.5f; + auto yaw_norm_thresholds = std::vector{0.5, 0.5, 0.5}; + config_ptr_ = std::make_unique( + voxels_num, point_cloud_range, voxel_size, circle_nms_dist_threshold, yaw_norm_thresholds, + score_threshold); + post_ptr_ = std::make_unique(*config_ptr_, stream_); + + cls_size_ = config_ptr_->num_proposals_ * config_ptr_->num_classes_; + box_size_ = config_ptr_->num_proposals_ * config_ptr_->num_box_values_; + dir_cls_size_ = config_ptr_->num_proposals_ * 2; // x, y + + cls_output_d_ = cuda::make_unique(cls_size_); + box_output_d_ = cuda::make_unique(box_size_); + dir_cls_output_d_ = cuda::make_unique(dir_cls_size_); + + cuda::clear_async(cls_output_d_.get(), cls_size_, stream_); + cuda::clear_async(box_output_d_.get(), box_size_, stream_); + cuda::clear_async(dir_cls_output_d_.get(), dir_cls_size_, stream_); + CHECK_CUDA_ERROR(cudaStreamSynchronize(stream_)); +} + +void PostprocessKernelTest::TearDown() +{ +} + +TEST_F(PostprocessKernelTest, EmptyTensorTest) +{ + std::vector det_boxes3d; + + CHECK_CUDA_ERROR(post_ptr_->generateDetectedBoxes3D_launch( + cls_output_d_.get(), box_output_d_.get(), dir_cls_output_d_.get(), det_boxes3d, stream_)); + + EXPECT_EQ(0, det_boxes3d.size()); +} + +TEST_F(PostprocessKernelTest, SingleDetectionTest) +{ + cuda::clear_async(cls_output_d_.get(), cls_size_, stream_); + cuda::clear_async(box_output_d_.get(), box_size_, stream_); + cuda::clear_async(dir_cls_output_d_.get(), dir_cls_size_, stream_); + CHECK_CUDA_ERROR(cudaStreamSynchronize(stream_)); + + std::vector det_boxes3d; + unsigned int det_idx = 42; + unsigned int cls_idx = 2; + + constexpr float detection_score = 1.f; + constexpr float detection_x = 50.f; + constexpr float detection_y = -38.4f; + constexpr float detection_z = 1.0; + const float detection_raw_x = (detection_x - config_ptr_->min_x_range_) / + (config_ptr_->num_point_values_ * config_ptr_->voxel_x_size_); + const float detection_raw_y = (detection_y - config_ptr_->min_y_range_) / + (config_ptr_->num_point_values_ * config_ptr_->voxel_y_size_); + const float detection_raw_z = detection_z; + + constexpr float detection_w = 3.2f; + constexpr float detection_l = 9.3f; + constexpr float detection_h = 1.7f; + constexpr float detection_log_w = std::log(detection_w); + constexpr float detection_log_l = std::log(detection_l); + constexpr float detection_log_h = std::log(detection_h); + + constexpr float detection_yaw = M_PI_4; + constexpr float detection_yaw_sin = std::sin(detection_yaw); + constexpr float detection_yaw_cos = std::cos(detection_yaw); + + // Set the values in the cuda tensor + cudaMemcpy( + &cls_output_d_[cls_idx * config_ptr_->num_proposals_ + det_idx], &detection_score, + 1 * sizeof(float), cudaMemcpyHostToDevice); + cudaMemcpy( + &dir_cls_output_d_[det_idx], &detection_yaw_sin, 1 * sizeof(float), cudaMemcpyHostToDevice); + cudaMemcpy( + &dir_cls_output_d_[det_idx + config_ptr_->num_proposals_], &detection_yaw_cos, + 1 * sizeof(float), cudaMemcpyHostToDevice); + cudaMemcpy(&box_output_d_[det_idx], &detection_raw_x, 1 * sizeof(float), cudaMemcpyHostToDevice); + cudaMemcpy( + &box_output_d_[det_idx + config_ptr_->num_proposals_], &detection_raw_y, 1 * sizeof(float), + cudaMemcpyHostToDevice); + cudaMemcpy( + &box_output_d_[det_idx + 2 * config_ptr_->num_proposals_], &detection_raw_z, 1 * sizeof(float), + cudaMemcpyHostToDevice); + cudaMemcpy( + &box_output_d_[det_idx + 3 * config_ptr_->num_proposals_], &detection_log_w, 1 * sizeof(float), + cudaMemcpyHostToDevice); + cudaMemcpy( + &box_output_d_[det_idx + 4 * config_ptr_->num_proposals_], &detection_log_l, 1 * sizeof(float), + cudaMemcpyHostToDevice); + cudaMemcpy( + &box_output_d_[det_idx + 5 * config_ptr_->num_proposals_], &detection_log_h, 1 * sizeof(float), + cudaMemcpyHostToDevice); + + auto code1 = cudaGetLastError(); + ASSERT_EQ(cudaSuccess, code1); + + // Extract the boxes + auto code2 = post_ptr_->generateDetectedBoxes3D_launch( + cls_output_d_.get(), box_output_d_.get(), dir_cls_output_d_.get(), det_boxes3d, stream_); + CHECK_CUDA_ERROR(cudaStreamSynchronize(stream_)); + + ASSERT_EQ(cudaSuccess, code2); + ASSERT_EQ(1, det_boxes3d.size()); + + const auto det_box3d = det_boxes3d[0]; + EXPECT_EQ(cls_idx, det_box3d.label); + EXPECT_EQ(detection_score, det_box3d.score); + EXPECT_EQ(detection_x, det_box3d.x); + EXPECT_EQ(detection_y, det_box3d.y); + EXPECT_EQ(detection_z, det_box3d.z); + EXPECT_NEAR(detection_w, det_box3d.width, 1e-6); + EXPECT_NEAR(detection_l, det_box3d.length, 1e-6); + EXPECT_NEAR(detection_h, det_box3d.height, 1e-6); + EXPECT_EQ(detection_yaw, det_box3d.yaw); +} + +TEST_F(PostprocessKernelTest, InvalidYawTest) +{ + cuda::clear_async(cls_output_d_.get(), cls_size_, stream_); + cuda::clear_async(box_output_d_.get(), box_size_, stream_); + cuda::clear_async(dir_cls_output_d_.get(), dir_cls_size_, stream_); + CHECK_CUDA_ERROR(cudaStreamSynchronize(stream_)); + + std::vector det_boxes3d; + unsigned int det_idx = 42; + unsigned int cls_idx = 2; + + constexpr float detection_score = 1.f; + constexpr float detection_x = 50.f; + constexpr float detection_y = -38.4f; + constexpr float detection_z = 1.0; + const float detection_raw_x = (detection_x - config_ptr_->min_x_range_) / + (config_ptr_->num_point_values_ * config_ptr_->voxel_x_size_); + const float detection_raw_y = (detection_y - config_ptr_->min_y_range_) / + (config_ptr_->num_point_values_ * config_ptr_->voxel_y_size_); + const float detection_raw_z = detection_z; + + constexpr float detection_w = 3.2f; + constexpr float detection_l = 9.3f; + constexpr float detection_h = 1.7f; + constexpr float detection_log_w = std::log(detection_w); + constexpr float detection_log_l = std::log(detection_l); + constexpr float detection_log_h = std::log(detection_h); + + constexpr float detection_yaw_sin = 0.f; + constexpr float detection_yaw_cos = 0.2f; + + // Set the values in the cuda tensor + cudaMemcpy( + &cls_output_d_[cls_idx * config_ptr_->num_proposals_ + det_idx], &detection_score, + 1 * sizeof(float), cudaMemcpyHostToDevice); + cudaMemcpy( + &dir_cls_output_d_[det_idx], &detection_yaw_sin, 1 * sizeof(float), cudaMemcpyHostToDevice); + cudaMemcpy( + &dir_cls_output_d_[det_idx + config_ptr_->num_proposals_], &detection_yaw_cos, + 1 * sizeof(float), cudaMemcpyHostToDevice); + cudaMemcpy(&box_output_d_[det_idx], &detection_raw_x, 1 * sizeof(float), cudaMemcpyHostToDevice); + cudaMemcpy( + &box_output_d_[det_idx + config_ptr_->num_proposals_], &detection_raw_y, 1 * sizeof(float), + cudaMemcpyHostToDevice); + cudaMemcpy( + &box_output_d_[det_idx + 2 * config_ptr_->num_proposals_], &detection_raw_z, 1 * sizeof(float), + cudaMemcpyHostToDevice); + cudaMemcpy( + &box_output_d_[det_idx + 3 * config_ptr_->num_proposals_], &detection_log_w, 1 * sizeof(float), + cudaMemcpyHostToDevice); + cudaMemcpy( + &box_output_d_[det_idx + 4 * config_ptr_->num_proposals_], &detection_log_l, 1 * sizeof(float), + cudaMemcpyHostToDevice); + cudaMemcpy( + &box_output_d_[det_idx + 5 * config_ptr_->num_proposals_], &detection_log_h, 1 * sizeof(float), + cudaMemcpyHostToDevice); + + auto code1 = cudaGetLastError(); + ASSERT_EQ(cudaSuccess, code1); + + // Extract the boxes + auto code2 = post_ptr_->generateDetectedBoxes3D_launch( + cls_output_d_.get(), box_output_d_.get(), dir_cls_output_d_.get(), det_boxes3d, stream_); + CHECK_CUDA_ERROR(cudaStreamSynchronize(stream_)); + + ASSERT_EQ(cudaSuccess, code2); + EXPECT_EQ(0, det_boxes3d.size()); +} + +TEST_F(PostprocessKernelTest, CircleNMSTest) +{ + cuda::clear_async(cls_output_d_.get(), cls_size_, stream_); + cuda::clear_async(box_output_d_.get(), box_size_, stream_); + cuda::clear_async(dir_cls_output_d_.get(), dir_cls_size_, stream_); + CHECK_CUDA_ERROR(cudaStreamSynchronize(stream_)); + + std::vector det_boxes3d; + unsigned int det_idx = 42; + unsigned int det_num = 2; + unsigned int cls_idx = 2; + + constexpr float detection_score = 1.f; + constexpr float detection_x = 50.f; + constexpr float detection_y = -38.4f; + constexpr float detection_z = 1.0; + const float detection_raw_x = (detection_x - config_ptr_->min_x_range_) / + (config_ptr_->num_point_values_ * config_ptr_->voxel_x_size_); + const float detection_raw_y = (detection_y - config_ptr_->min_y_range_) / + (config_ptr_->num_point_values_ * config_ptr_->voxel_y_size_); + const float detection_raw_z = detection_z; + + constexpr float detection_w = 3.2f; + constexpr float detection_l = 9.3f; + constexpr float detection_h = 1.7f; + constexpr float detection_log_w = std::log(detection_w); + constexpr float detection_log_l = std::log(detection_l); + constexpr float detection_log_h = std::log(detection_h); + + constexpr float detection_yaw = M_PI_4; + constexpr float detection_yaw_sin = std::sin(detection_yaw); + constexpr float detection_yaw_cos = std::cos(detection_yaw); + + // Set the values in the cuda tensor for 2 detections + for (std::size_t i = 0; i < det_num; ++i) { + cudaMemcpy( + &cls_output_d_[cls_idx * config_ptr_->num_proposals_ + det_idx + i], &detection_score, + 1 * sizeof(float), cudaMemcpyHostToDevice); + cudaMemcpy( + &dir_cls_output_d_[det_idx + i], &detection_yaw_sin, 1 * sizeof(float), + cudaMemcpyHostToDevice); + cudaMemcpy( + &dir_cls_output_d_[det_idx + i + config_ptr_->num_proposals_], &detection_yaw_cos, + 1 * sizeof(float), cudaMemcpyHostToDevice); + cudaMemcpy( + &box_output_d_[det_idx + i], &detection_raw_x, 1 * sizeof(float), cudaMemcpyHostToDevice); + cudaMemcpy( + &box_output_d_[det_idx + i + config_ptr_->num_proposals_], &detection_raw_y, + 1 * sizeof(float), cudaMemcpyHostToDevice); + cudaMemcpy( + &box_output_d_[det_idx + i + 2 * config_ptr_->num_proposals_], &detection_raw_z, + 1 * sizeof(float), cudaMemcpyHostToDevice); + cudaMemcpy( + &box_output_d_[det_idx + i + 3 * config_ptr_->num_proposals_], &detection_log_w, + 1 * sizeof(float), cudaMemcpyHostToDevice); + cudaMemcpy( + &box_output_d_[det_idx + i + 4 * config_ptr_->num_proposals_], &detection_log_l, + 1 * sizeof(float), cudaMemcpyHostToDevice); + cudaMemcpy( + &box_output_d_[det_idx + i + 5 * config_ptr_->num_proposals_], &detection_log_h, + 1 * sizeof(float), cudaMemcpyHostToDevice); + } + + auto code1 = cudaGetLastError(); + ASSERT_EQ(cudaSuccess, code1); + + // Extract the boxes + auto code2 = post_ptr_->generateDetectedBoxes3D_launch( + cls_output_d_.get(), box_output_d_.get(), dir_cls_output_d_.get(), det_boxes3d, stream_); + CHECK_CUDA_ERROR(cudaStreamSynchronize(stream_)); + + ASSERT_EQ(cudaSuccess, code2); + EXPECT_EQ(1, det_boxes3d.size()); +} + +} // namespace lidar_transfusion + +int main(int argc, char ** argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/perception/lidar_transfusion/test/test_postprocess_kernel.hpp b/perception/lidar_transfusion/test/test_postprocess_kernel.hpp new file mode 100644 index 0000000000000..60fa7882ecc11 --- /dev/null +++ b/perception/lidar_transfusion/test/test_postprocess_kernel.hpp @@ -0,0 +1,48 @@ +// 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 TEST_POSTPROCESS_KERNEL_HPP_ +#define TEST_POSTPROCESS_KERNEL_HPP_ + +#include +#include + +#include + +#include + +namespace lidar_transfusion +{ + +class PostprocessKernelTest : public testing::Test +{ +public: + void SetUp() override; + void TearDown() override; + + std::unique_ptr post_ptr_{nullptr}; + std::unique_ptr config_ptr_{}; + cudaStream_t stream_{}; + + unsigned int cls_size_{0}; + unsigned int box_size_{0}; + unsigned int dir_cls_size_{0}; + cuda::unique_ptr cls_output_d_{nullptr}; + cuda::unique_ptr box_output_d_{nullptr}; + cuda::unique_ptr dir_cls_output_d_{nullptr}; +}; + +} // namespace lidar_transfusion + +#endif // TEST_POSTPROCESS_KERNEL_HPP_ diff --git a/perception/lidar_transfusion/test/test_preprocess_kernel.cpp b/perception/lidar_transfusion/test/test_preprocess_kernel.cpp new file mode 100644 index 0000000000000..c1c2a824f0f53 --- /dev/null +++ b/perception/lidar_transfusion/test/test_preprocess_kernel.cpp @@ -0,0 +1,265 @@ +// 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 "test_preprocess_kernel.hpp" + +#include "lidar_transfusion/cuda_utils.hpp" + +#include + +#include +#include +#include + +#include +#include + +namespace lidar_transfusion +{ + +void PreprocessKernelTest::SetUp() +{ + cudaStreamCreate(&stream_); + + auto voxels_num = std::vector{1, 3, 5}; + auto point_cloud_range = std::vector{-76.8, -76.8, -3.0, 76.8, 76.8, 5.0}; + auto voxel_size = std::vector{0.3, 0.3, 8.0}; + auto score_threshold = 0.2f; + auto circle_nms_dist_threshold = 0.5f; + auto yaw_norm_thresholds = std::vector{0.5, 0.5, 0.5}; + config_ptr_ = std::make_unique( + voxels_num, point_cloud_range, voxel_size, circle_nms_dist_threshold, yaw_norm_thresholds, + score_threshold); + pre_ptr_ = std::make_unique(*config_ptr_, stream_); + + voxel_features_size_ = config_ptr_->max_voxels_ * config_ptr_->max_num_points_per_pillar_ * + config_ptr_->num_point_feature_size_; + voxel_num_size_ = config_ptr_->max_voxels_; + voxel_idxs_size_ = config_ptr_->max_voxels_ * config_ptr_->num_point_values_; + + params_input_d_ = cuda::make_unique(); + voxel_features_d_ = cuda::make_unique(voxel_features_size_); + voxel_num_d_ = cuda::make_unique(voxel_num_size_); + voxel_idxs_d_ = cuda::make_unique(voxel_idxs_size_); + points_d_ = + cuda::make_unique(config_ptr_->cloud_capacity_ * config_ptr_->num_point_feature_size_); + + cuda::clear_async(voxel_features_d_.get(), voxel_features_size_, stream_); + cuda::clear_async(voxel_num_d_.get(), voxel_num_size_, stream_); + cuda::clear_async(voxel_idxs_d_.get(), voxel_idxs_size_, stream_); + cuda::clear_async(params_input_d_.get(), 1, stream_); + cuda::clear_async( + points_d_.get(), config_ptr_->cloud_capacity_ * config_ptr_->num_point_feature_size_, stream_); + CHECK_CUDA_ERROR(cudaStreamSynchronize(stream_)); +} + +void PreprocessKernelTest::TearDown() +{ +} + +TEST_F(PreprocessKernelTest, EmptyVoxelTest) +{ + std::vector points{}; + std::size_t count = 0; + + pre_ptr_->generateVoxels( + points_d_.get(), count, params_input_d_.get(), voxel_features_d_.get(), voxel_num_d_.get(), + voxel_idxs_d_.get()); + unsigned int params_input; + auto code1 = cudaStreamSynchronize(stream_); + + auto code2 = cudaMemcpyAsync( + ¶ms_input, params_input_d_.get(), sizeof(unsigned int), cudaMemcpyDeviceToHost, stream_); + CHECK_CUDA_ERROR(cudaStreamSynchronize(stream_)); + + auto code3 = cudaMemcpy( + points_d_.get(), points.data(), count * config_ptr_->num_point_feature_size_ * sizeof(float), + cudaMemcpyHostToDevice); + + ASSERT_EQ(cudaSuccess, code1); + ASSERT_EQ(cudaSuccess, code2); + ASSERT_EQ(cudaSuccess, code3); + + EXPECT_EQ(0, params_input); +} + +TEST_F(PreprocessKernelTest, BasicTest) +{ + cuda::clear_async(voxel_features_d_.get(), voxel_features_size_, stream_); + cuda::clear_async(voxel_num_d_.get(), voxel_num_size_, stream_); + cuda::clear_async(voxel_idxs_d_.get(), voxel_idxs_size_, stream_); + cuda::clear_async(params_input_d_.get(), 1, stream_); + cuda::clear_async( + points_d_.get(), config_ptr_->cloud_capacity_ * config_ptr_->num_point_feature_size_, stream_); + cudaStreamSynchronize(stream_); + + std::vector point1{25.f, -61.1f, 1.8f, 42.0, 0.1f}; + std::vector point2{-31.6f, 1.1f, 2.8f, 77.0, 0.1f}; + std::vector point3{42.6f, 15.1f, -0.1f, 3.0, 0.1f}; + + std::vector points; + points.reserve(point1.size() + point2.size() + point3.size()); + points.insert(points.end(), point1.begin(), point1.end()); + points.insert(points.end(), point2.begin(), point2.end()); + points.insert(points.end(), point3.begin(), point3.end()); + std::size_t count = 3; + + CHECK_CUDA_ERROR(cudaMemcpy( + points_d_.get(), points.data(), count * config_ptr_->num_point_feature_size_ * sizeof(float), + cudaMemcpyHostToDevice)); + + // Compute the total amount of voxels filled + pre_ptr_->generateVoxels( + points_d_.get(), count, params_input_d_.get(), voxel_features_d_.get(), voxel_num_d_.get(), + voxel_idxs_d_.get()); + + unsigned int params_input; + CHECK_CUDA_ERROR( + cudaMemcpy(¶ms_input, params_input_d_.get(), sizeof(unsigned int), cudaMemcpyDeviceToHost)); + CHECK_CUDA_ERROR(cudaMemcpy( + points_d_.get(), points.data(), count * config_ptr_->num_point_feature_size_ * sizeof(float), + cudaMemcpyHostToDevice)); + + ASSERT_EQ(count, params_input); + + // Check if voxels were filled + unsigned int voxel_num; + + for (std::size_t i = 0; i < count; ++i) { + CHECK_CUDA_ERROR( + cudaMemcpy(&voxel_num, voxel_num_d_.get() + i, sizeof(unsigned int), cudaMemcpyDeviceToHost)); + EXPECT_EQ(1, voxel_num); + } + + // Check grid indices + thrust::host_vector voxel_idxs(config_ptr_->num_point_values_ * count, 0); + unsigned int voxel_idx_gt; + unsigned int voxel_idy_gt; + CHECK_CUDA_ERROR(cudaMemcpy( + voxel_idxs.data(), voxel_idxs_d_.get(), + config_ptr_->num_point_values_ * count * sizeof(unsigned int), cudaMemcpyDeviceToHost)); + + for (std::size_t i = 0; i < count; ++i) { + voxel_idx_gt = std::floor( + (points[config_ptr_->num_point_feature_size_ * i + 0] - config_ptr_->min_x_range_) / + config_ptr_->voxel_x_size_); + voxel_idy_gt = std::floor( + (points[config_ptr_->num_point_feature_size_ * i + 1] - config_ptr_->min_y_range_) / + config_ptr_->voxel_y_size_); + + EXPECT_EQ( + voxel_idx_gt, + voxel_idxs[config_ptr_->num_point_values_ * i + 3]); // {0, 0, voxel_idy, VOXEL_IDX} + EXPECT_EQ( + voxel_idy_gt, + voxel_idxs[config_ptr_->num_point_values_ * i + 2]); // {0, 0, VOXEL_IDY, voxel_idx} + } + + // Check voxel features + thrust::host_vector voxel_features( + count * config_ptr_->max_num_points_per_pillar_ * config_ptr_->num_point_feature_size_, 0.f); + CHECK_CUDA_ERROR(cudaMemcpy( + voxel_features.data(), voxel_features_d_.get(), + count * config_ptr_->max_num_points_per_pillar_ * config_ptr_->num_point_feature_size_ * + sizeof(float), + cudaMemcpyDeviceToHost)); + + for (std::size_t i = 0; i < count; ++i) { + for (std::size_t j = 0; j < config_ptr_->num_point_feature_size_; ++j) { + EXPECT_EQ( + points[config_ptr_->num_point_feature_size_ * i + j], + voxel_features + [i * config_ptr_->max_num_points_per_pillar_ * config_ptr_->num_point_feature_size_ + j]); + } + } +} + +TEST_F(PreprocessKernelTest, OutOfRangeTest) +{ + cuda::clear_async(voxel_features_d_.get(), voxel_features_size_, stream_); + cuda::clear_async(voxel_num_d_.get(), voxel_num_size_, stream_); + cuda::clear_async(voxel_idxs_d_.get(), voxel_idxs_size_, stream_); + cuda::clear_async(params_input_d_.get(), 1, stream_); + cuda::clear_async( + points_d_.get(), config_ptr_->cloud_capacity_ * config_ptr_->num_point_feature_size_, stream_); + cudaStreamSynchronize(stream_); + + std::vector points{25.f, config_ptr_->max_y_range_ + 0.1f, 2.1f, 55.f, 0.1f}; + std::size_t count = 0; + + CHECK_CUDA_ERROR(cudaMemcpy( + points_d_.get(), points.data(), count * config_ptr_->num_point_feature_size_ * sizeof(float), + cudaMemcpyHostToDevice)); + + pre_ptr_->generateVoxels( + points_d_.get(), count, params_input_d_.get(), voxel_features_d_.get(), voxel_num_d_.get(), + voxel_idxs_d_.get()); + + unsigned int params_input; + CHECK_CUDA_ERROR( + cudaMemcpy(¶ms_input, params_input_d_.get(), sizeof(unsigned int), cudaMemcpyDeviceToHost)); + CHECK_CUDA_ERROR(cudaMemcpy( + points_d_.get(), points.data(), count * config_ptr_->num_point_feature_size_ * sizeof(float), + cudaMemcpyHostToDevice)); + + EXPECT_EQ(count, params_input); +} + +TEST_F(PreprocessKernelTest, VoxelOverflowTest) +{ + cuda::clear_async(voxel_features_d_.get(), voxel_features_size_, stream_); + cuda::clear_async(voxel_num_d_.get(), voxel_num_size_, stream_); + cuda::clear_async(voxel_idxs_d_.get(), voxel_idxs_size_, stream_); + cuda::clear_async(params_input_d_.get(), 1, stream_); + cuda::clear_async( + points_d_.get(), config_ptr_->cloud_capacity_ * config_ptr_->num_point_feature_size_, stream_); + cudaStreamSynchronize(stream_); + + std::vector point{config_ptr_->min_x_range_, -61.1f, 1.8f, 42.f, 0.1f}; + std::size_t count = config_ptr_->max_voxels_ + 1; + std::vector points{}; + points.reserve(count * config_ptr_->num_point_feature_size_); + + for (std::size_t i = 0; i < count; ++i) { + points.insert(points.end(), point.begin(), point.end()); + point[0] += config_ptr_->voxel_x_size_; + } + + CHECK_CUDA_ERROR(cudaMemcpy( + points_d_.get(), points.data(), count * config_ptr_->num_point_feature_size_ * sizeof(float), + cudaMemcpyHostToDevice)); + + pre_ptr_->generateVoxels( + points_d_.get(), count, params_input_d_.get(), voxel_features_d_.get(), voxel_num_d_.get(), + voxel_idxs_d_.get()); + + ASSERT_EQ(cudaSuccess, cudaGetLastError()); + + unsigned int params_input; + CHECK_CUDA_ERROR( + cudaMemcpy(¶ms_input, params_input_d_.get(), sizeof(unsigned int), cudaMemcpyDeviceToHost)); + CHECK_CUDA_ERROR(cudaMemcpy( + points_d_.get(), points.data(), count * config_ptr_->num_point_feature_size_ * sizeof(float), + cudaMemcpyHostToDevice)); + + EXPECT_EQ(config_ptr_->max_voxels_, params_input); +} + +} // namespace lidar_transfusion + +int main(int argc, char ** argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/perception/lidar_transfusion/test/test_preprocess_kernel.hpp b/perception/lidar_transfusion/test/test_preprocess_kernel.hpp new file mode 100644 index 0000000000000..024fae5eae4b1 --- /dev/null +++ b/perception/lidar_transfusion/test/test_preprocess_kernel.hpp @@ -0,0 +1,50 @@ +// 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 TEST_PREPROCESS_KERNEL_HPP_ +#define TEST_PREPROCESS_KERNEL_HPP_ + +#include +#include + +#include + +#include + +namespace lidar_transfusion +{ + +class PreprocessKernelTest : public testing::Test +{ +public: + void SetUp() override; + void TearDown() override; + + std::unique_ptr pre_ptr_{nullptr}; + std::unique_ptr config_ptr_{}; + cudaStream_t stream_{}; + + unsigned int voxel_features_size_{0}; + unsigned int voxel_num_size_{0}; + unsigned int voxel_idxs_size_{0}; + cuda::unique_ptr points_d_{nullptr}; + cuda::unique_ptr params_input_d_{nullptr}; + cuda::unique_ptr voxel_features_d_{nullptr}; + cuda::unique_ptr voxel_num_d_{nullptr}; + cuda::unique_ptr voxel_idxs_d_{nullptr}; +}; + +} // namespace lidar_transfusion + +#endif // TEST_PREPROCESS_KERNEL_HPP_ diff --git a/perception/lidar_transfusion/test/test_ros_utils.cpp b/perception/lidar_transfusion/test/test_ros_utils.cpp new file mode 100644 index 0000000000000..15541e6353b42 --- /dev/null +++ b/perception/lidar_transfusion/test/test_ros_utils.cpp @@ -0,0 +1,104 @@ +// 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 "lidar_transfusion/ros_utils.hpp" + +#include + +TEST(TestSuite, box3DToDetectedObject) +{ + std::vector class_names = {"CAR", "TRUCK", "BUS", "TRAILER", + "BICYCLE", "MOTORCYCLE", "PEDESTRIAN"}; + + // Test case 1: Test with valid label + { + lidar_transfusion::Box3D box3d; + box3d.label = 0; // CAR + box3d.score = 0.8f; + box3d.x = 1.0; + box3d.y = 2.0; + box3d.z = 3.0; + box3d.width = 2.0; + box3d.height = 1.5; + box3d.length = 4.0; + box3d.yaw = 0.5; + + autoware_perception_msgs::msg::DetectedObject obj; + lidar_transfusion::box3DToDetectedObject(box3d, class_names, obj); + + EXPECT_FLOAT_EQ(obj.existence_probability, 0.8f); + EXPECT_EQ( + obj.classification[0].label, autoware_perception_msgs::msg::ObjectClassification::CAR); + EXPECT_FLOAT_EQ(obj.kinematics.pose_with_covariance.pose.position.x, 1.0); + EXPECT_FLOAT_EQ(obj.kinematics.pose_with_covariance.pose.position.y, 2.0); + EXPECT_FLOAT_EQ(obj.kinematics.pose_with_covariance.pose.position.z, 3.0); + EXPECT_FLOAT_EQ(obj.shape.dimensions.x, 4.0); + EXPECT_FLOAT_EQ(obj.shape.dimensions.y, 2.0); + EXPECT_FLOAT_EQ(obj.shape.dimensions.z, 1.5); + EXPECT_FALSE(obj.kinematics.has_position_covariance); + EXPECT_FALSE(obj.kinematics.has_twist); + EXPECT_FALSE(obj.kinematics.has_twist_covariance); + } + + // Test case 2: Test with invalid label + { + lidar_transfusion::Box3D box3d; + box3d.score = 0.5f; + box3d.label = 10; // Invalid + + autoware_perception_msgs::msg::DetectedObject obj; + lidar_transfusion::box3DToDetectedObject(box3d, class_names, obj); + + EXPECT_FLOAT_EQ(obj.existence_probability, 0.5f); + EXPECT_EQ( + obj.classification[0].label, autoware_perception_msgs::msg::ObjectClassification::UNKNOWN); + EXPECT_FALSE(obj.kinematics.has_position_covariance); + EXPECT_FALSE(obj.kinematics.has_twist); + EXPECT_FALSE(obj.kinematics.has_twist_covariance); + } +} + +TEST(TestSuite, getSemanticType) +{ + EXPECT_EQ( + lidar_transfusion::getSemanticType("CAR"), + autoware_perception_msgs::msg::ObjectClassification::CAR); + EXPECT_EQ( + lidar_transfusion::getSemanticType("TRUCK"), + autoware_perception_msgs::msg::ObjectClassification::TRUCK); + EXPECT_EQ( + lidar_transfusion::getSemanticType("BUS"), + autoware_perception_msgs::msg::ObjectClassification::BUS); + EXPECT_EQ( + lidar_transfusion::getSemanticType("TRAILER"), + autoware_perception_msgs::msg::ObjectClassification::TRAILER); + EXPECT_EQ( + lidar_transfusion::getSemanticType("MOTORCYCLE"), + autoware_perception_msgs::msg::ObjectClassification::MOTORCYCLE); + EXPECT_EQ( + lidar_transfusion::getSemanticType("BICYCLE"), + autoware_perception_msgs::msg::ObjectClassification::BICYCLE); + EXPECT_EQ( + lidar_transfusion::getSemanticType("PEDESTRIAN"), + autoware_perception_msgs::msg::ObjectClassification::PEDESTRIAN); + EXPECT_EQ( + lidar_transfusion::getSemanticType("UNKNOWN"), + autoware_perception_msgs::msg::ObjectClassification::UNKNOWN); +} + +int main(int argc, char ** argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/perception/lidar_transfusion/test/test_voxel_generator.cpp b/perception/lidar_transfusion/test/test_voxel_generator.cpp new file mode 100644 index 0000000000000..2673a341b3721 --- /dev/null +++ b/perception/lidar_transfusion/test/test_voxel_generator.cpp @@ -0,0 +1,284 @@ +// 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 "test_voxel_generator.hpp" + +#include "gtest/gtest.h" +#include "lidar_transfusion/transfusion_config.hpp" +#include "lidar_transfusion/utils.hpp" + +#include + +#include "sensor_msgs/point_cloud2_iterator.hpp" + +namespace lidar_transfusion +{ + +void VoxelGeneratorTest::SetUp() +{ + // Setup things that should occur before every test instance should go here + node_ = std::make_shared("voxel_generator_test_node"); + + world_frame_ = "map"; + lidar_frame_ = "lidar"; + delta_pointcloud_x_ = 1.0; + points_per_pointcloud_ = 300; + + voxels_num_ = std::vector{5000, 30000, 60000}; + point_cloud_range_ = std::vector{-76.8, -76.8, -3.0, 76.8, 76.8, 5.0}; + voxel_size_ = std::vector{0.3, 0.3, 8.0}; + score_threshold_ = 0.2f; + circle_nms_dist_threshold_ = 0.5f; + yaw_norm_thresholds_ = std::vector{0.5, 0.5, 0.5}; + config_ptr_ = std::make_unique( + voxels_num_, point_cloud_range_, voxel_size_, circle_nms_dist_threshold_, yaw_norm_thresholds_, + score_threshold_); + + cloud1_ = std::make_unique(); + cloud2_ = std::make_unique(); + + // Set up the fields + point_cloud_msg_wrapper::PointCloud2Modifier< + autoware_point_types::PointXYZIRCAEDT, autoware_point_types::PointXYZIRCAEDTGenerator> + modifier{*cloud1_, lidar_frame_}; + + // Resize the cloud to hold points_per_pointcloud_ points + modifier.resize(points_per_pointcloud_); + + // Create an iterator for desired fields + sensor_msgs::PointCloud2Iterator iter_x(*cloud1_, "x"); + sensor_msgs::PointCloud2Iterator iter_y(*cloud1_, "y"); + sensor_msgs::PointCloud2Iterator iter_z(*cloud1_, "z"); + sensor_msgs::PointCloud2Iterator iter_i(*cloud1_, "intensity"); + + // Populate the point cloud + for (size_t i = 0; i < modifier.size(); ++i, ++iter_x, ++iter_y, ++iter_z) { + *iter_x = static_cast(i); + *iter_y = static_cast(i); + *iter_z = static_cast(i); + } + for (size_t i = 0; i < modifier.size(); ++i, ++iter_i) { + *iter_i = static_cast(i % 256); + } + + *cloud2_ = *cloud1_; + + // Set the stamps for the point clouds. They usually come every 100ms + cloud1_->header.stamp.sec = 1; + cloud1_->header.stamp.nanosec = 100'000'000; + cloud2_->header.stamp.sec = 1; + cloud2_->header.stamp.nanosec = 200'000'000; + + tf2_buffer_ = std::make_unique(node_->get_clock()); + tf2_buffer_->setUsingDedicatedThread(true); + + // The vehicle moves 1m/s in the x direction + const double world_origin_x = 6'371'000.0; + const double world_origin_y = 1'371'000.0; + + transform1_.header.stamp = cloud1_->header.stamp; + transform1_.header.frame_id = world_frame_; + transform1_.child_frame_id = lidar_frame_; + transform1_.transform.translation.x = world_origin_x; + transform1_.transform.translation.y = world_origin_y; + transform1_.transform.translation.z = 1.8; + transform1_.transform.rotation.x = 0.0; + transform1_.transform.rotation.y = 0.0; + transform1_.transform.rotation.z = 0.0; + transform1_.transform.rotation.w = 1.0; + + transform2_ = transform1_; + transform2_.header.stamp = cloud2_->header.stamp; + transform2_.transform.translation.x = world_origin_x + delta_pointcloud_x_; + + cudaStreamCreate(&stream_); +} + +void VoxelGeneratorTest::TearDown() +{ +} + +TEST_F(VoxelGeneratorTest, CloudInfo) +{ + CloudInfo cloud_info{}; + EXPECT_EQ(cloud1_->is_bigendian, cloud_info.is_bigendian); + EXPECT_EQ(cloud1_->fields[0].name, "x"); + EXPECT_EQ(cloud1_->fields[0].datatype, cloud_info.x_datatype); + EXPECT_EQ(cloud1_->fields[0].count, cloud_info.x_count); + EXPECT_EQ(cloud1_->fields[0].offset, cloud_info.x_offset); + EXPECT_EQ(cloud1_->fields[1].name, "y"); + EXPECT_EQ(cloud1_->fields[1].datatype, cloud_info.y_datatype); + EXPECT_EQ(cloud1_->fields[1].count, cloud_info.y_count); + EXPECT_EQ(cloud1_->fields[1].offset, cloud_info.y_offset); + EXPECT_EQ(cloud1_->fields[2].name, "z"); + EXPECT_EQ(cloud1_->fields[2].datatype, cloud_info.z_datatype); + EXPECT_EQ(cloud1_->fields[2].count, cloud_info.z_count); + EXPECT_EQ(cloud1_->fields[2].offset, cloud_info.z_offset); + EXPECT_EQ(cloud1_->fields[3].name, "intensity"); + EXPECT_EQ(cloud1_->fields[3].datatype, cloud_info.intensity_datatype); + EXPECT_EQ(cloud1_->fields[3].count, cloud_info.intensity_count); + EXPECT_EQ(cloud1_->fields[3].offset, cloud_info.intensity_offset); + EXPECT_EQ(cloud1_->width, points_per_pointcloud_); + EXPECT_EQ(cloud1_->height, 1); +} + +TEST_F(VoxelGeneratorTest, SingleFrame) +{ + const unsigned int num_past_frames = 0; // only current frame + DensificationParam param(world_frame_, num_past_frames); + VoxelGenerator voxel_generator(param, *config_ptr_, stream_); + std::vector points; + points.resize(config_ptr_->cloud_capacity_ * config_ptr_->num_point_feature_size_); + std::fill(points.begin(), points.end(), std::nan("")); + + auto points_d = + cuda::make_unique(config_ptr_->cloud_capacity_ * config_ptr_->num_point_feature_size_); + cudaMemcpy( + points_d.get(), points.data(), + points_per_pointcloud_ * config_ptr_->num_point_feature_size_ * sizeof(float), + cudaMemcpyHostToDevice); + + bool status1 = voxel_generator.enqueuePointCloud(*cloud1_, *tf2_buffer_); + std::size_t generated_points_num = voxel_generator.generateSweepPoints(*cloud1_, points_d); + + cudaMemcpy( + points.data(), points_d.get(), + points_per_pointcloud_ * config_ptr_->num_point_feature_size_ * sizeof(float), + cudaMemcpyDeviceToHost); + + EXPECT_TRUE(status1); + EXPECT_EQ(generated_points_num, points_per_pointcloud_); + + // Check valid points + for (std::size_t i = 0; i < points_per_pointcloud_; ++i) { + // There are no tf conversions + EXPECT_EQ(static_cast(i), points[i * config_ptr_->num_point_feature_size_ + 0]); + EXPECT_EQ(static_cast(i), points[i * config_ptr_->num_point_feature_size_ + 1]); + EXPECT_EQ(static_cast(i), points[i * config_ptr_->num_point_feature_size_ + 2]); + EXPECT_EQ(static_cast(i % 256), points[i * config_ptr_->num_point_feature_size_ + 3]); + EXPECT_EQ(static_cast(0), points[i * config_ptr_->num_point_feature_size_ + 4]); + } + + // Check invalid points + for (std::size_t i = points_per_pointcloud_ * config_ptr_->num_point_feature_size_; + i < points_per_pointcloud_ * config_ptr_->num_point_feature_size_ * 2; ++i) { + EXPECT_TRUE(std::isnan(points[i])); + } +} + +TEST_F(VoxelGeneratorTest, TwoFramesNoTf) +{ + const unsigned int num_past_frames = 1; + DensificationParam param(world_frame_, num_past_frames); + VoxelGenerator voxel_generator(param, *config_ptr_, stream_); + std::vector points; + points.resize(config_ptr_->cloud_capacity_ * config_ptr_->num_point_feature_size_); + std::fill(points.begin(), points.end(), std::nan("")); + + auto points_d = + cuda::make_unique(config_ptr_->cloud_capacity_ * config_ptr_->num_point_feature_size_); + cudaMemcpy( + points_d.get(), points.data(), + points_per_pointcloud_ * config_ptr_->num_point_feature_size_ * sizeof(float), + cudaMemcpyHostToDevice); + + bool status1 = voxel_generator.enqueuePointCloud(*cloud1_, *tf2_buffer_); + bool status2 = voxel_generator.enqueuePointCloud(*cloud2_, *tf2_buffer_); + std::size_t generated_points_num = voxel_generator.generateSweepPoints(*cloud1_, points_d); + + cudaMemcpy( + points.data(), points_d.get(), + points_per_pointcloud_ * config_ptr_->num_point_feature_size_ * sizeof(float), + cudaMemcpyDeviceToHost); + + EXPECT_FALSE(status1); + EXPECT_FALSE(status2); + EXPECT_EQ(0, generated_points_num); +} + +TEST_F(VoxelGeneratorTest, TwoFrames) +{ + const unsigned int num_past_frames = 1; + DensificationParam param(world_frame_, num_past_frames); + VoxelGenerator voxel_generator(param, *config_ptr_, stream_); + std::vector points; + points.resize(config_ptr_->cloud_capacity_ * config_ptr_->num_point_feature_size_); + std::fill(points.begin(), points.end(), std::nan("")); + + auto points_d = + cuda::make_unique(config_ptr_->cloud_capacity_ * config_ptr_->num_point_feature_size_); + cudaMemcpy( + points_d.get(), points.data(), + 2 * points_per_pointcloud_ * config_ptr_->num_point_feature_size_ * sizeof(float), + cudaMemcpyHostToDevice); + + tf2_buffer_->setTransform(transform1_, "authority1"); + tf2_buffer_->setTransform(transform2_, "authority1"); + + bool status1 = voxel_generator.enqueuePointCloud(*cloud1_, *tf2_buffer_); + bool status2 = voxel_generator.enqueuePointCloud(*cloud2_, *tf2_buffer_); + std::size_t generated_points_num = voxel_generator.generateSweepPoints(*cloud1_, points_d); + + cudaMemcpy( + points.data(), points_d.get(), + 2 * points_per_pointcloud_ * config_ptr_->num_point_feature_size_ * sizeof(float), + cudaMemcpyDeviceToHost); + + EXPECT_TRUE(status1); + EXPECT_TRUE(status2); + EXPECT_EQ(2 * points_per_pointcloud_, generated_points_num); + + // Check valid points for the latest pointcloud + for (std::size_t i = 0; i < points_per_pointcloud_; ++i) { + // There are no tf conversions + EXPECT_EQ(static_cast(i), points[i * config_ptr_->num_point_feature_size_ + 0]); + EXPECT_EQ(static_cast(i), points[i * config_ptr_->num_point_feature_size_ + 1]); + EXPECT_EQ(static_cast(i), points[i * config_ptr_->num_point_feature_size_ + 2]); + EXPECT_EQ(static_cast(i % 256), points[i * config_ptr_->num_point_feature_size_ + 3]); + EXPECT_EQ(static_cast(0), points[i * config_ptr_->num_point_feature_size_ + 4]); + } + + // Check valid points for the oldest pointcloud + for (std::size_t i = 0; i < points_per_pointcloud_; ++i) { + // There are tf conversions, so results are not numerically the same + EXPECT_NEAR( + static_cast(i) - delta_pointcloud_x_, + points[(points_per_pointcloud_ + i) * config_ptr_->num_point_feature_size_ + 0], 1e-6); + EXPECT_NEAR( + static_cast(i), + points[(points_per_pointcloud_ + i) * config_ptr_->num_point_feature_size_ + 1], 1e-6); + EXPECT_NEAR( + static_cast(i), + points[(points_per_pointcloud_ + i) * config_ptr_->num_point_feature_size_ + 2], 1e-6); + EXPECT_NEAR( + static_cast(i % 256), + points[(points_per_pointcloud_ + i) * config_ptr_->num_point_feature_size_ + 3], 1e-6); + EXPECT_NEAR( + 0.1, points[(points_per_pointcloud_ + i) * config_ptr_->num_point_feature_size_ + 4], 1e-6); + } + + // Check invalid points + for (std::size_t i = 2 * points_per_pointcloud_ * config_ptr_->num_point_feature_size_; + i < 3 * points_per_pointcloud_ * config_ptr_->num_point_feature_size_; ++i) { + EXPECT_TRUE(std::isnan(points[i])); + } +} +} // namespace lidar_transfusion + +int main(int argc, char ** argv) +{ + testing::InitGoogleTest(&argc, argv); + rclcpp::init(argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/perception/lidar_transfusion/test/test_voxel_generator.hpp b/perception/lidar_transfusion/test/test_voxel_generator.hpp new file mode 100644 index 0000000000000..eccbe6412d183 --- /dev/null +++ b/perception/lidar_transfusion/test/test_voxel_generator.hpp @@ -0,0 +1,65 @@ +// 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 TEST_VOXEL_GENERATOR_HPP_ +#define TEST_VOXEL_GENERATOR_HPP_ + +#include +#include + +#include + +#include + +#include +#include +#include + +namespace lidar_transfusion +{ + +class VoxelGeneratorTest : public testing::Test +{ +public: + void SetUp() override; + void TearDown() override; + + // These need to be public so that they can be accessed in the test cases + rclcpp::Node::SharedPtr node_{}; + + std::unique_ptr cloud1_{}, cloud2_{}; + geometry_msgs::msg::TransformStamped transform1_{}, transform2_{}; + + std::unique_ptr densification_param_ptr_{}; + std::unique_ptr config_ptr_{}; + + std::unique_ptr tf2_buffer_{}; + + std::string world_frame_{}; + std::string lidar_frame_{}; + double delta_pointcloud_x_{}; + std::size_t points_per_pointcloud_{}; + + std::vector voxels_num_{}; + std::vector point_cloud_range_{}; + std::vector voxel_size_{}; + float circle_nms_dist_threshold_{}; + std::vector yaw_norm_thresholds_{}; + float score_threshold_{}; + + cudaStream_t stream_{}; +}; +} // namespace lidar_transfusion + +#endif // TEST_VOXEL_GENERATOR_HPP_ From 73febbd6ee43dea05a15be3ef7a38f46aecbb224 Mon Sep 17 00:00:00 2001 From: kobayu858 <129580202+kobayu858@users.noreply.github.com> Date: Wed, 10 Jul 2024 14:24:40 +0900 Subject: [PATCH 21/70] fix(autoware_universe_utils): fix constParameterReference (#7882) * fix: constParameterReference Signed-off-by: kobayu858 * fix: constParameterReference Signed-off-by: kobayu858 --------- Signed-off-by: kobayu858 --- .../include/autoware/universe_utils/geometry/geometry.hpp | 2 +- common/autoware_universe_utils/src/geometry/geometry.cpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/common/autoware_universe_utils/include/autoware/universe_utils/geometry/geometry.hpp b/common/autoware_universe_utils/include/autoware/universe_utils/geometry/geometry.hpp index 87c06dfd9cf08..e6dd57c9d3fed 100644 --- a/common/autoware_universe_utils/include/autoware/universe_utils/geometry/geometry.hpp +++ b/common/autoware_universe_utils/include/autoware/universe_utils/geometry/geometry.hpp @@ -413,7 +413,7 @@ geometry_msgs::msg::Pose transformPose( const geometry_msgs::msg::Pose & pose, const geometry_msgs::msg::TransformStamped & transform); geometry_msgs::msg::Pose transformPose( - const geometry_msgs::msg::Pose & pose, geometry_msgs::msg::Transform & transform); + const geometry_msgs::msg::Pose & pose, const geometry_msgs::msg::Transform & transform); geometry_msgs::msg::Pose transformPose( const geometry_msgs::msg::Pose & pose, const geometry_msgs::msg::Pose & pose_transform); diff --git a/common/autoware_universe_utils/src/geometry/geometry.cpp b/common/autoware_universe_utils/src/geometry/geometry.cpp index 1f4fd318e227b..b0f5756fc2d94 100644 --- a/common/autoware_universe_utils/src/geometry/geometry.cpp +++ b/common/autoware_universe_utils/src/geometry/geometry.cpp @@ -225,7 +225,7 @@ geometry_msgs::msg::Pose transformPose( } geometry_msgs::msg::Pose transformPose( - const geometry_msgs::msg::Pose & pose, geometry_msgs::msg::Transform & transform) + const geometry_msgs::msg::Pose & pose, const geometry_msgs::msg::Transform & transform) { geometry_msgs::msg::TransformStamped transform_stamped; transform_stamped.transform = transform; From ea0276acd134a75bc0227aeeb96266c26db7100e Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Wed, 10 Jul 2024 14:44:19 +0900 Subject: [PATCH 22/70] refactor(compare_map_segmentation)!: fix namespace and directory structure (#7910) * feat: update namespace and directory structure for compare_map_segmentation code Signed-off-by: Taekjin LEE * refactor: update directory structure Signed-off-by: Taekjin LEE * fix: add missing include Signed-off-by: Taekjin LEE * style(pre-commit): autofix --------- Signed-off-by: Taekjin LEE Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- .../filter/pointcloud_map_filter.launch.py | 2 +- .../ground_segmentation.launch.py | 2 +- .../behavior_planning.launch.xml | 7 +++- .../compare_map_segmentation/CMakeLists.txt | 42 +++++++++---------- .../node.cpp} | 9 ++-- .../compare_elevation_map_filter/node.hpp} | 10 ++--- .../node.cpp} | 12 ++++-- .../node.hpp} | 12 +++--- .../node.cpp} | 11 +++-- .../node.hpp} | 12 +++--- .../node.cpp} | 12 ++++-- .../voxel_based_compare_map_filter/node.hpp} | 12 +++--- .../node.cpp} | 11 +++-- .../node.hpp} | 12 +++--- .../voxel_grid_map_loader.cpp | 6 ++- .../voxel_grid_map_loader.hpp | 16 ++++--- 16 files changed, 108 insertions(+), 80 deletions(-) rename perception/compare_map_segmentation/src/{compare_elevation_map_filter_node.cpp => compare_elevation_map_filter/node.cpp} (93%) rename perception/compare_map_segmentation/{include/compare_map_segmentation/compare_elevation_map_filter_node.hpp => src/compare_elevation_map_filter/node.hpp} (86%) rename perception/compare_map_segmentation/src/{distance_based_compare_map_filter_nodelet.cpp => distance_based_compare_map_filter/node.cpp} (94%) rename perception/compare_map_segmentation/{include/compare_map_segmentation/distance_based_compare_map_filter_nodelet.hpp => src/distance_based_compare_map_filter/node.hpp} (91%) rename perception/compare_map_segmentation/src/{voxel_based_approximate_compare_map_filter_nodelet.cpp => voxel_based_approximate_compare_map_filter/node.cpp} (94%) rename perception/compare_map_segmentation/{include/compare_map_segmentation/voxel_based_approximate_compare_map_filter_nodelet.hpp => src/voxel_based_approximate_compare_map_filter/node.hpp} (85%) rename perception/compare_map_segmentation/src/{voxel_based_compare_map_filter_nodelet.cpp => voxel_based_compare_map_filter/node.cpp} (92%) rename perception/compare_map_segmentation/{include/compare_map_segmentation/voxel_based_compare_map_filter_nodelet.hpp => src/voxel_based_compare_map_filter/node.hpp} (78%) rename perception/compare_map_segmentation/src/{voxel_distance_based_compare_map_filter_nodelet.cpp => voxel_distance_based_compare_map_filter/node.cpp} (95%) rename perception/compare_map_segmentation/{include/compare_map_segmentation/voxel_distance_based_compare_map_filter_nodelet.hpp => src/voxel_distance_based_compare_map_filter/node.hpp} (93%) rename perception/compare_map_segmentation/src/{ => voxel_grid_map_loader}/voxel_grid_map_loader.cpp (99%) rename perception/compare_map_segmentation/{include/compare_map_segmentation => src/voxel_grid_map_loader}/voxel_grid_map_loader.hpp (95%) diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/filter/pointcloud_map_filter.launch.py b/launch/tier4_perception_launch/launch/object_recognition/detection/filter/pointcloud_map_filter.launch.py index 5fa71cd1aa0e9..6c0e5f20b8957 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/detection/filter/pointcloud_map_filter.launch.py +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/filter/pointcloud_map_filter.launch.py @@ -106,7 +106,7 @@ def create_compare_map_pipeline(self): components.append( ComposableNode( package="compare_map_segmentation", - plugin="compare_map_segmentation::VoxelBasedCompareMapFilterComponent", + plugin="autoware::compare_map_segmentation::VoxelBasedCompareMapFilterComponent", name="voxel_based_compare_map_filter", remappings=[ ("input", down_sample_topic), diff --git a/launch/tier4_perception_launch/launch/obstacle_segmentation/ground_segmentation/ground_segmentation.launch.py b/launch/tier4_perception_launch/launch/obstacle_segmentation/ground_segmentation/ground_segmentation.launch.py index ba0b058d8c00b..fd38b7bfd6ab3 100644 --- a/launch/tier4_perception_launch/launch/obstacle_segmentation/ground_segmentation/ground_segmentation.launch.py +++ b/launch/tier4_perception_launch/launch/obstacle_segmentation/ground_segmentation/ground_segmentation.launch.py @@ -391,7 +391,7 @@ def create_single_frame_outlier_filter_components(input_topic, output_topic, con components.append( ComposableNode( package="compare_map_segmentation", - plugin="compare_map_segmentation::CompareElevationMapFilterComponent", + plugin="autoware::compare_map_segmentation::CompareElevationMapFilterComponent", name="compare_elevation_map_filter", namespace="elevation_map", remappings=[ diff --git a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml index 3deff32d6e297..3c58c8d85b5be 100644 --- a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml +++ b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml @@ -260,7 +260,12 @@ - + diff --git a/perception/compare_map_segmentation/CMakeLists.txt b/perception/compare_map_segmentation/CMakeLists.txt index a76b0fc56e2a0..0a33711e946ef 100644 --- a/perception/compare_map_segmentation/CMakeLists.txt +++ b/perception/compare_map_segmentation/CMakeLists.txt @@ -21,23 +21,23 @@ include_directories( ${GRID_MAP_INCLUDE_DIR} ) -add_library(compare_map_segmentation SHARED - src/distance_based_compare_map_filter_nodelet.cpp - src/voxel_based_approximate_compare_map_filter_nodelet.cpp - src/voxel_based_compare_map_filter_nodelet.cpp - src/voxel_distance_based_compare_map_filter_nodelet.cpp - src/compare_elevation_map_filter_node.cpp - src/voxel_grid_map_loader.cpp +add_library(${PROJECT_NAME} SHARED + src/distance_based_compare_map_filter/node.cpp + src/voxel_based_approximate_compare_map_filter/node.cpp + src/voxel_based_compare_map_filter/node.cpp + src/voxel_distance_based_compare_map_filter/node.cpp + src/compare_elevation_map_filter/node.cpp + src/voxel_grid_map_loader/voxel_grid_map_loader.cpp ) -target_link_libraries(compare_map_segmentation +target_link_libraries(${PROJECT_NAME} pointcloud_preprocessor::pointcloud_preprocessor_filter_base ${Boost_LIBRARIES} ${OpenCV_LIBRARIES} ${PCL_LIBRARIES} ) -ament_target_dependencies(compare_map_segmentation +ament_target_dependencies(${PROJECT_NAME} grid_map_pcl grid_map_ros pcl_conversions @@ -50,7 +50,7 @@ ament_target_dependencies(compare_map_segmentation ) if(OPENMP_FOUND) - set_target_properties(compare_map_segmentation PROPERTIES + set_target_properties(${PROJECT_NAME} PROPERTIES COMPILE_FLAGS ${OpenMP_CXX_FLAGS} LINK_FLAGS ${OpenMP_CXX_FLAGS} ) @@ -58,32 +58,32 @@ endif() # ========== Compare Map Filter ========== # -- Distance Based Compare Map Filter -- -rclcpp_components_register_node(compare_map_segmentation - PLUGIN "compare_map_segmentation::DistanceBasedCompareMapFilterComponent" +rclcpp_components_register_node(${PROJECT_NAME} + PLUGIN "autoware::compare_map_segmentation::DistanceBasedCompareMapFilterComponent" EXECUTABLE distance_based_compare_map_filter_node) # -- Voxel Based Approximate Compare Map Filter -- -rclcpp_components_register_node(compare_map_segmentation - PLUGIN "compare_map_segmentation::VoxelBasedApproximateCompareMapFilterComponent" +rclcpp_components_register_node(${PROJECT_NAME} + PLUGIN "autoware::compare_map_segmentation::VoxelBasedApproximateCompareMapFilterComponent" EXECUTABLE voxel_based_approximate_compare_map_filter_node) # -- Voxel Based Compare Map Filter -- -rclcpp_components_register_node(compare_map_segmentation - PLUGIN "compare_map_segmentation::VoxelBasedCompareMapFilterComponent" +rclcpp_components_register_node(${PROJECT_NAME} + PLUGIN "autoware::compare_map_segmentation::VoxelBasedCompareMapFilterComponent" EXECUTABLE voxel_based_compare_map_filter_node) # -- Voxel Distance Based Compare Map Filter -- -rclcpp_components_register_node(compare_map_segmentation - PLUGIN "compare_map_segmentation::VoxelDistanceBasedCompareMapFilterComponent" +rclcpp_components_register_node(${PROJECT_NAME} + PLUGIN "autoware::compare_map_segmentation::VoxelDistanceBasedCompareMapFilterComponent" EXECUTABLE voxel_distance_based_compare_map_filter_node) # -- Compare Elevation Map Filter -- -rclcpp_components_register_node(compare_map_segmentation - PLUGIN "compare_map_segmentation::CompareElevationMapFilterComponent" +rclcpp_components_register_node(${PROJECT_NAME} + PLUGIN "autoware::compare_map_segmentation::CompareElevationMapFilterComponent" EXECUTABLE compare_elevation_map_filter_node) install( - TARGETS compare_map_segmentation + TARGETS ${PROJECT_NAME} ARCHIVE DESTINATION lib LIBRARY DESTINATION lib RUNTIME DESTINATION bin diff --git a/perception/compare_map_segmentation/src/compare_elevation_map_filter_node.cpp b/perception/compare_map_segmentation/src/compare_elevation_map_filter/node.cpp similarity index 93% rename from perception/compare_map_segmentation/src/compare_elevation_map_filter_node.cpp rename to perception/compare_map_segmentation/src/compare_elevation_map_filter/node.cpp index 1eadeeb3bec05..e5a19245bc5b1 100644 --- a/perception/compare_map_segmentation/src/compare_elevation_map_filter_node.cpp +++ b/perception/compare_map_segmentation/src/compare_elevation_map_filter/node.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "compare_map_segmentation/compare_elevation_map_filter_node.hpp" +#include "node.hpp" #include #include @@ -33,7 +33,7 @@ #include #include -namespace compare_map_segmentation +namespace autoware::compare_map_segmentation { CompareElevationMapFilterComponent::CompareElevationMapFilterComponent( const rclcpp::NodeOptions & options) @@ -94,7 +94,8 @@ void CompareElevationMapFilterComponent::filter( output.header.stamp = input->header.stamp; output.header.frame_id = output_frame; } -} // namespace compare_map_segmentation +} // namespace autoware::compare_map_segmentation #include -RCLCPP_COMPONENTS_REGISTER_NODE(compare_map_segmentation::CompareElevationMapFilterComponent) +RCLCPP_COMPONENTS_REGISTER_NODE( + autoware::compare_map_segmentation::CompareElevationMapFilterComponent) diff --git a/perception/compare_map_segmentation/include/compare_map_segmentation/compare_elevation_map_filter_node.hpp b/perception/compare_map_segmentation/src/compare_elevation_map_filter/node.hpp similarity index 86% rename from perception/compare_map_segmentation/include/compare_map_segmentation/compare_elevation_map_filter_node.hpp rename to perception/compare_map_segmentation/src/compare_elevation_map_filter/node.hpp index 63aad08fe7611..ba2328d862951 100644 --- a/perception/compare_map_segmentation/include/compare_map_segmentation/compare_elevation_map_filter_node.hpp +++ b/perception/compare_map_segmentation/src/compare_elevation_map_filter/node.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef COMPARE_MAP_SEGMENTATION__COMPARE_ELEVATION_MAP_FILTER_NODE_HPP_ -#define COMPARE_MAP_SEGMENTATION__COMPARE_ELEVATION_MAP_FILTER_NODE_HPP_ +#ifndef COMPARE_ELEVATION_MAP_FILTER__NODE_HPP_ +#define COMPARE_ELEVATION_MAP_FILTER__NODE_HPP_ #include "pointcloud_preprocessor/filter.hpp" @@ -29,7 +29,7 @@ #include #include #include -namespace compare_map_segmentation +namespace autoware::compare_map_segmentation { class CompareElevationMapFilterComponent : public pointcloud_preprocessor::Filter { @@ -56,6 +56,6 @@ class CompareElevationMapFilterComponent : public pointcloud_preprocessor::Filte PCL_MAKE_ALIGNED_OPERATOR_NEW explicit CompareElevationMapFilterComponent(const rclcpp::NodeOptions & options); }; -} // namespace compare_map_segmentation +} // namespace autoware::compare_map_segmentation -#endif // COMPARE_MAP_SEGMENTATION__COMPARE_ELEVATION_MAP_FILTER_NODE_HPP_ +#endif // COMPARE_ELEVATION_MAP_FILTER__NODE_HPP_ diff --git a/perception/compare_map_segmentation/src/distance_based_compare_map_filter_nodelet.cpp b/perception/compare_map_segmentation/src/distance_based_compare_map_filter/node.cpp similarity index 94% rename from perception/compare_map_segmentation/src/distance_based_compare_map_filter_nodelet.cpp rename to perception/compare_map_segmentation/src/distance_based_compare_map_filter/node.cpp index 2962ba1e28a75..53183cd7dc3f0 100644 --- a/perception/compare_map_segmentation/src/distance_based_compare_map_filter_nodelet.cpp +++ b/perception/compare_map_segmentation/src/distance_based_compare_map_filter/node.cpp @@ -12,7 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "compare_map_segmentation/distance_based_compare_map_filter_nodelet.hpp" +#include "node.hpp" + +#include "autoware/universe_utils/ros/debug_publisher.hpp" +#include "autoware/universe_utils/system/stop_watch.hpp" #include #include @@ -20,7 +23,7 @@ #include -namespace compare_map_segmentation +namespace autoware::compare_map_segmentation { void DistanceBasedStaticMapLoader::onMapCallback( @@ -176,7 +179,8 @@ void DistanceBasedCompareMapFilterComponent::filter( } } -} // namespace compare_map_segmentation +} // namespace autoware::compare_map_segmentation #include -RCLCPP_COMPONENTS_REGISTER_NODE(compare_map_segmentation::DistanceBasedCompareMapFilterComponent) +RCLCPP_COMPONENTS_REGISTER_NODE( + autoware::compare_map_segmentation::DistanceBasedCompareMapFilterComponent) diff --git a/perception/compare_map_segmentation/include/compare_map_segmentation/distance_based_compare_map_filter_nodelet.hpp b/perception/compare_map_segmentation/src/distance_based_compare_map_filter/node.hpp similarity index 91% rename from perception/compare_map_segmentation/include/compare_map_segmentation/distance_based_compare_map_filter_nodelet.hpp rename to perception/compare_map_segmentation/src/distance_based_compare_map_filter/node.hpp index 301dc75839f8e..115a73cf0d263 100644 --- a/perception/compare_map_segmentation/include/compare_map_segmentation/distance_based_compare_map_filter_nodelet.hpp +++ b/perception/compare_map_segmentation/src/distance_based_compare_map_filter/node.hpp @@ -12,11 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef COMPARE_MAP_SEGMENTATION__DISTANCE_BASED_COMPARE_MAP_FILTER_NODELET_HPP_ -#define COMPARE_MAP_SEGMENTATION__DISTANCE_BASED_COMPARE_MAP_FILTER_NODELET_HPP_ +#ifndef DISTANCE_BASED_COMPARE_MAP_FILTER__NODE_HPP_ +#define DISTANCE_BASED_COMPARE_MAP_FILTER__NODE_HPP_ +#include "../voxel_grid_map_loader/voxel_grid_map_loader.hpp" #include "pointcloud_preprocessor/filter.hpp" -#include "voxel_grid_map_loader.hpp" #include // for pcl::isFinite #include @@ -26,7 +26,7 @@ #include #include -namespace compare_map_segmentation +namespace autoware::compare_map_segmentation { typedef typename pcl::Filter::PointCloud PointCloud; @@ -114,8 +114,8 @@ class DistanceBasedCompareMapFilterComponent : public pointcloud_preprocessor::F PCL_MAKE_ALIGNED_OPERATOR_NEW explicit DistanceBasedCompareMapFilterComponent(const rclcpp::NodeOptions & options); }; -} // namespace compare_map_segmentation +} // namespace autoware::compare_map_segmentation // clang-format off -#endif // COMPARE_MAP_SEGMENTATION__DISTANCE_BASED_COMPARE_MAP_FILTER_NODELET_HPP_ // NOLINT +#endif // DISTANCE_BASED_COMPARE_MAP_FILTER__NODE_HPP_ // NOLINT // clang-format on diff --git a/perception/compare_map_segmentation/src/voxel_based_approximate_compare_map_filter_nodelet.cpp b/perception/compare_map_segmentation/src/voxel_based_approximate_compare_map_filter/node.cpp similarity index 94% rename from perception/compare_map_segmentation/src/voxel_based_approximate_compare_map_filter_nodelet.cpp rename to perception/compare_map_segmentation/src/voxel_based_approximate_compare_map_filter/node.cpp index e14ae7d55a1d4..8bc22f8c31184 100644 --- a/perception/compare_map_segmentation/src/voxel_based_approximate_compare_map_filter_nodelet.cpp +++ b/perception/compare_map_segmentation/src/voxel_based_approximate_compare_map_filter/node.cpp @@ -12,7 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "compare_map_segmentation/voxel_based_approximate_compare_map_filter_nodelet.hpp" +#include "node.hpp" + +#include "autoware/universe_utils/ros/debug_publisher.hpp" +#include "autoware/universe_utils/system/stop_watch.hpp" #include #include @@ -20,7 +23,7 @@ #include -namespace compare_map_segmentation +namespace autoware::compare_map_segmentation { bool VoxelBasedApproximateStaticMapLoader::is_close_to_map( @@ -145,8 +148,8 @@ void VoxelBasedApproximateCompareMapFilterComponent::filter( } } -} // namespace compare_map_segmentation +} // namespace autoware::compare_map_segmentation #include RCLCPP_COMPONENTS_REGISTER_NODE( - compare_map_segmentation::VoxelBasedApproximateCompareMapFilterComponent) + autoware::compare_map_segmentation::VoxelBasedApproximateCompareMapFilterComponent) diff --git a/perception/compare_map_segmentation/include/compare_map_segmentation/voxel_based_approximate_compare_map_filter_nodelet.hpp b/perception/compare_map_segmentation/src/voxel_based_approximate_compare_map_filter/node.hpp similarity index 85% rename from perception/compare_map_segmentation/include/compare_map_segmentation/voxel_based_approximate_compare_map_filter_nodelet.hpp rename to perception/compare_map_segmentation/src/voxel_based_approximate_compare_map_filter/node.hpp index 3a5e183767620..26b0204210209 100644 --- a/perception/compare_map_segmentation/include/compare_map_segmentation/voxel_based_approximate_compare_map_filter_nodelet.hpp +++ b/perception/compare_map_segmentation/src/voxel_based_approximate_compare_map_filter/node.hpp @@ -12,11 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef COMPARE_MAP_SEGMENTATION__VOXEL_BASED_APPROXIMATE_COMPARE_MAP_FILTER_NODELET_HPP_ // NOLINT -#define COMPARE_MAP_SEGMENTATION__VOXEL_BASED_APPROXIMATE_COMPARE_MAP_FILTER_NODELET_HPP_ // NOLINT +#ifndef VOXEL_BASED_APPROXIMATE_COMPARE_MAP_FILTER__NODE_HPP_ // NOLINT +#define VOXEL_BASED_APPROXIMATE_COMPARE_MAP_FILTER__NODE_HPP_ // NOLINT +#include "../voxel_grid_map_loader/voxel_grid_map_loader.hpp" #include "pointcloud_preprocessor/filter.hpp" -#include "voxel_grid_map_loader.hpp" #include #include @@ -25,7 +25,7 @@ #include #include -namespace compare_map_segmentation +namespace autoware::compare_map_segmentation { class VoxelBasedApproximateStaticMapLoader : public VoxelGridStaticMapLoader @@ -70,8 +70,8 @@ class VoxelBasedApproximateCompareMapFilterComponent : public pointcloud_preproc PCL_MAKE_ALIGNED_OPERATOR_NEW explicit VoxelBasedApproximateCompareMapFilterComponent(const rclcpp::NodeOptions & options); }; -} // namespace compare_map_segmentation +} // namespace autoware::compare_map_segmentation // clang-format off -#endif // COMPARE_MAP_SEGMENTATION__VOXEL_BASED_APPROXIMATE_COMPARE_MAP_FILTER_NODELET_HPP_ // NOLINT +#endif // VOXEL_BASED_APPROXIMATE_COMPARE_MAP_FILTER__NODE_HPP_ // NOLINT // clang-format on diff --git a/perception/compare_map_segmentation/src/voxel_based_compare_map_filter_nodelet.cpp b/perception/compare_map_segmentation/src/voxel_based_compare_map_filter/node.cpp similarity index 92% rename from perception/compare_map_segmentation/src/voxel_based_compare_map_filter_nodelet.cpp rename to perception/compare_map_segmentation/src/voxel_based_compare_map_filter/node.cpp index 449ae76181bc6..1861a710cdebd 100644 --- a/perception/compare_map_segmentation/src/voxel_based_compare_map_filter_nodelet.cpp +++ b/perception/compare_map_segmentation/src/voxel_based_compare_map_filter/node.cpp @@ -12,7 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "compare_map_segmentation/voxel_based_compare_map_filter_nodelet.hpp" +#include "node.hpp" + +#include "autoware/universe_utils/ros/debug_publisher.hpp" +#include "autoware/universe_utils/system/stop_watch.hpp" #include #include @@ -21,7 +24,7 @@ #include #include -namespace compare_map_segmentation +namespace autoware::compare_map_segmentation { using pointcloud_preprocessor::get_param; @@ -106,7 +109,8 @@ void VoxelBasedCompareMapFilterComponent::filter( } } -} // namespace compare_map_segmentation +} // namespace autoware::compare_map_segmentation #include -RCLCPP_COMPONENTS_REGISTER_NODE(compare_map_segmentation::VoxelBasedCompareMapFilterComponent) +RCLCPP_COMPONENTS_REGISTER_NODE( + autoware::compare_map_segmentation::VoxelBasedCompareMapFilterComponent) diff --git a/perception/compare_map_segmentation/include/compare_map_segmentation/voxel_based_compare_map_filter_nodelet.hpp b/perception/compare_map_segmentation/src/voxel_based_compare_map_filter/node.hpp similarity index 78% rename from perception/compare_map_segmentation/include/compare_map_segmentation/voxel_based_compare_map_filter_nodelet.hpp rename to perception/compare_map_segmentation/src/voxel_based_compare_map_filter/node.hpp index 692193d13b04f..6e03e5d7a2e09 100644 --- a/perception/compare_map_segmentation/include/compare_map_segmentation/voxel_based_compare_map_filter_nodelet.hpp +++ b/perception/compare_map_segmentation/src/voxel_based_compare_map_filter/node.hpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef COMPARE_MAP_SEGMENTATION__VOXEL_BASED_COMPARE_MAP_FILTER_NODELET_HPP_ -#define COMPARE_MAP_SEGMENTATION__VOXEL_BASED_COMPARE_MAP_FILTER_NODELET_HPP_ +#ifndef VOXEL_BASED_COMPARE_MAP_FILTER__NODE_HPP_ +#define VOXEL_BASED_COMPARE_MAP_FILTER__NODE_HPP_ -#include "compare_map_segmentation/voxel_grid_map_loader.hpp" +#include "../voxel_grid_map_loader/voxel_grid_map_loader.hpp" #include "pointcloud_preprocessor/filter.hpp" #include @@ -24,7 +24,7 @@ #include #include -namespace compare_map_segmentation +namespace autoware::compare_map_segmentation { class VoxelBasedCompareMapFilterComponent : public pointcloud_preprocessor::Filter { @@ -45,6 +45,6 @@ class VoxelBasedCompareMapFilterComponent : public pointcloud_preprocessor::Filt PCL_MAKE_ALIGNED_OPERATOR_NEW explicit VoxelBasedCompareMapFilterComponent(const rclcpp::NodeOptions & options); }; -} // namespace compare_map_segmentation +} // namespace autoware::compare_map_segmentation -#endif // COMPARE_MAP_SEGMENTATION__VOXEL_BASED_COMPARE_MAP_FILTER_NODELET_HPP_ +#endif // VOXEL_BASED_COMPARE_MAP_FILTER__NODE_HPP_ diff --git a/perception/compare_map_segmentation/src/voxel_distance_based_compare_map_filter_nodelet.cpp b/perception/compare_map_segmentation/src/voxel_distance_based_compare_map_filter/node.cpp similarity index 95% rename from perception/compare_map_segmentation/src/voxel_distance_based_compare_map_filter_nodelet.cpp rename to perception/compare_map_segmentation/src/voxel_distance_based_compare_map_filter/node.cpp index 0a660bfffd7fb..7fdeb5d8ea163 100644 --- a/perception/compare_map_segmentation/src/voxel_distance_based_compare_map_filter_nodelet.cpp +++ b/perception/compare_map_segmentation/src/voxel_distance_based_compare_map_filter/node.cpp @@ -12,7 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "compare_map_segmentation/voxel_distance_based_compare_map_filter_nodelet.hpp" +#include "node.hpp" + +#include "autoware/universe_utils/ros/debug_publisher.hpp" +#include "autoware/universe_utils/system/stop_watch.hpp" #include #include @@ -20,7 +23,7 @@ #include -namespace compare_map_segmentation +namespace autoware::compare_map_segmentation { void VoxelDistanceBasedStaticMapLoader::onMapCallback( @@ -174,8 +177,8 @@ void VoxelDistanceBasedCompareMapFilterComponent::filter( "debug/processing_time_ms", processing_time_ms); } } -} // namespace compare_map_segmentation +} // namespace autoware::compare_map_segmentation #include RCLCPP_COMPONENTS_REGISTER_NODE( - compare_map_segmentation::VoxelDistanceBasedCompareMapFilterComponent) + autoware::compare_map_segmentation::VoxelDistanceBasedCompareMapFilterComponent) diff --git a/perception/compare_map_segmentation/include/compare_map_segmentation/voxel_distance_based_compare_map_filter_nodelet.hpp b/perception/compare_map_segmentation/src/voxel_distance_based_compare_map_filter/node.hpp similarity index 93% rename from perception/compare_map_segmentation/include/compare_map_segmentation/voxel_distance_based_compare_map_filter_nodelet.hpp rename to perception/compare_map_segmentation/src/voxel_distance_based_compare_map_filter/node.hpp index 0cfc8a64ab2dd..e0656359f3df2 100644 --- a/perception/compare_map_segmentation/include/compare_map_segmentation/voxel_distance_based_compare_map_filter_nodelet.hpp +++ b/perception/compare_map_segmentation/src/voxel_distance_based_compare_map_filter/node.hpp @@ -12,11 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef COMPARE_MAP_SEGMENTATION__VOXEL_DISTANCE_BASED_COMPARE_MAP_FILTER_NODELET_HPP_ // NOLINT -#define COMPARE_MAP_SEGMENTATION__VOXEL_DISTANCE_BASED_COMPARE_MAP_FILTER_NODELET_HPP_ // NOLINT +#ifndef VOXEL_DISTANCE_BASED_COMPARE_MAP_FILTER__NODE_HPP_ // NOLINT +#define VOXEL_DISTANCE_BASED_COMPARE_MAP_FILTER__NODE_HPP_ // NOLINT +#include "../voxel_grid_map_loader/voxel_grid_map_loader.hpp" #include "pointcloud_preprocessor/filter.hpp" -#include "voxel_grid_map_loader.hpp" #include #include @@ -27,7 +27,7 @@ #include #include -namespace compare_map_segmentation +namespace autoware::compare_map_segmentation { typedef typename pcl::Filter::PointCloud PointCloud; @@ -139,8 +139,8 @@ class VoxelDistanceBasedCompareMapFilterComponent : public pointcloud_preprocess PCL_MAKE_ALIGNED_OPERATOR_NEW explicit VoxelDistanceBasedCompareMapFilterComponent(const rclcpp::NodeOptions & options); }; -} // namespace compare_map_segmentation +} // namespace autoware::compare_map_segmentation // clang-format off -#endif // COMPARE_MAP_SEGMENTATION__VOXEL_DISTANCE_BASED_COMPARE_MAP_FILTER_NODELET_HPP_ // NOLINT +#endif // VOXEL_DISTANCE_BASED_COMPARE_MAP_FILTER__NODE_HPP_ // NOLINT // clang-format on diff --git a/perception/compare_map_segmentation/src/voxel_grid_map_loader.cpp b/perception/compare_map_segmentation/src/voxel_grid_map_loader/voxel_grid_map_loader.cpp similarity index 99% rename from perception/compare_map_segmentation/src/voxel_grid_map_loader.cpp rename to perception/compare_map_segmentation/src/voxel_grid_map_loader/voxel_grid_map_loader.cpp index ef3727019c0f4..b011799e5b912 100644 --- a/perception/compare_map_segmentation/src/voxel_grid_map_loader.cpp +++ b/perception/compare_map_segmentation/src/voxel_grid_map_loader/voxel_grid_map_loader.cpp @@ -12,8 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "compare_map_segmentation/voxel_grid_map_loader.hpp" +#include "voxel_grid_map_loader.hpp" +namespace autoware::compare_map_segmentation +{ VoxelGridMapLoader::VoxelGridMapLoader( rclcpp::Node * node, double leaf_size, double downsize_ratio_z_axis, std::string * tf_map_input_frame, std::mutex * mutex) @@ -462,3 +464,5 @@ void VoxelGridDynamicMapLoader::request_update_map(const geometry_msgs::msg::Poi } } } + +} // namespace autoware::compare_map_segmentation diff --git a/perception/compare_map_segmentation/include/compare_map_segmentation/voxel_grid_map_loader.hpp b/perception/compare_map_segmentation/src/voxel_grid_map_loader/voxel_grid_map_loader.hpp similarity index 95% rename from perception/compare_map_segmentation/include/compare_map_segmentation/voxel_grid_map_loader.hpp rename to perception/compare_map_segmentation/src/voxel_grid_map_loader/voxel_grid_map_loader.hpp index 21cb19edcd0ec..bc54519a95e47 100644 --- a/perception/compare_map_segmentation/include/compare_map_segmentation/voxel_grid_map_loader.hpp +++ b/perception/compare_map_segmentation/src/voxel_grid_map_loader/voxel_grid_map_loader.hpp @@ -12,12 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef COMPARE_MAP_SEGMENTATION__VOXEL_GRID_MAP_LOADER_HPP_ -#define COMPARE_MAP_SEGMENTATION__VOXEL_GRID_MAP_LOADER_HPP_ +#ifndef VOXEL_GRID_MAP_LOADER__VOXEL_GRID_MAP_LOADER_HPP_ +#define VOXEL_GRID_MAP_LOADER__VOXEL_GRID_MAP_LOADER_HPP_ #include -#include +#include "autoware_map_msgs/srv/get_differential_point_cloud_map.hpp" #include #include #include @@ -33,6 +33,8 @@ #include #include +namespace autoware::compare_map_segmentation +{ template double distance3D(const T p1, const U p2) { @@ -138,7 +140,7 @@ class VoxelGridStaticMapLoader : public VoxelGridMapLoader rclcpp::Node * node, double leaf_size, double downsize_ratio_z_axis, std::string * tf_map_input_frame, std::mutex * mutex); virtual void onMapCallback(const sensor_msgs::msg::PointCloud2::ConstSharedPtr map); - virtual bool is_close_to_map(const pcl::PointXYZ & point, const double distance_threshold); + bool is_close_to_map(const pcl::PointXYZ & point, const double distance_threshold) override; }; class VoxelGridDynamicMapLoader : public VoxelGridMapLoader @@ -199,7 +201,7 @@ class VoxelGridDynamicMapLoader : public VoxelGridMapLoader void timer_callback(); bool should_update_map() const; void request_update_map(const geometry_msgs::msg::Point & position); - virtual bool is_close_to_map(const pcl::PointXYZ & point, const double distance_threshold); + bool is_close_to_map(const pcl::PointXYZ & point, const double distance_threshold) override; /** \brief Check if point close to map pointcloud in the */ bool is_close_to_next_map_grid( const pcl::PointXYZ & point, const int current_map_grid_index, const double distance_threshold); @@ -319,4 +321,6 @@ class VoxelGridDynamicMapLoader : public VoxelGridMapLoader } }; -#endif // COMPARE_MAP_SEGMENTATION__VOXEL_GRID_MAP_LOADER_HPP_ +} // namespace autoware::compare_map_segmentation + +#endif // VOXEL_GRID_MAP_LOADER__VOXEL_GRID_MAP_LOADER_HPP_ From cfbf0400fb5c0166b77a4e0ac526718417631af9 Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Wed, 10 Jul 2024 14:48:41 +0900 Subject: [PATCH 23/70] refactor(radar_threshold_filter): fix namespace and directory structure (#7913) Signed-off-by: Taekjin LEE --- sensing/radar_threshold_filter/CMakeLists.txt | 14 +++++++------- .../radar_threshold_filter_node.cpp | 11 +++++------ .../radar_threshold_filter_node.hpp | 10 +++++----- .../test_radar_threshold_filter.cpp | 4 ++-- 4 files changed, 19 insertions(+), 20 deletions(-) rename sensing/radar_threshold_filter/src/{radar_threshold_filter_node => }/radar_threshold_filter_node.cpp (95%) rename sensing/radar_threshold_filter/{include/radar_threshold_filter => src}/radar_threshold_filter_node.hpp (87%) diff --git a/sensing/radar_threshold_filter/CMakeLists.txt b/sensing/radar_threshold_filter/CMakeLists.txt index 3c4ae8d40d2c1..136b41cb3b37b 100644 --- a/sensing/radar_threshold_filter/CMakeLists.txt +++ b/sensing/radar_threshold_filter/CMakeLists.txt @@ -6,12 +6,12 @@ find_package(autoware_cmake REQUIRED) autoware_package() ## Targets -ament_auto_add_library(radar_threshold_filter_node_component SHARED - src/radar_threshold_filter_node/radar_threshold_filter_node.cpp +ament_auto_add_library(${PROJECT_NAME} SHARED + src/radar_threshold_filter_node.cpp ) -rclcpp_components_register_node(radar_threshold_filter_node_component - PLUGIN "radar_threshold_filter::RadarThresholdFilterNode" +rclcpp_components_register_node(${PROJECT_NAME} + PLUGIN "autoware::radar_threshold_filter::RadarThresholdFilterNode" EXECUTABLE radar_threshold_filter_node ) @@ -24,10 +24,10 @@ if(BUILD_TESTING) ament_lint_auto_find_test_dependencies() file(GLOB_RECURSE test_files test/**/*.cpp) - ament_add_ros_isolated_gtest(radar_threshold_filter ${test_files}) + ament_add_ros_isolated_gtest(${PROJECT_NAME}_test ${test_files}) - target_link_libraries(radar_threshold_filter - radar_threshold_filter_node_component + target_link_libraries(${PROJECT_NAME}_test + ${PROJECT_NAME} ) endif() diff --git a/sensing/radar_threshold_filter/src/radar_threshold_filter_node/radar_threshold_filter_node.cpp b/sensing/radar_threshold_filter/src/radar_threshold_filter_node.cpp similarity index 95% rename from sensing/radar_threshold_filter/src/radar_threshold_filter_node/radar_threshold_filter_node.cpp rename to sensing/radar_threshold_filter/src/radar_threshold_filter_node.cpp index 3f3837b678992..7a260766c2c7a 100644 --- a/sensing/radar_threshold_filter/src/radar_threshold_filter_node/radar_threshold_filter_node.cpp +++ b/sensing/radar_threshold_filter/src/radar_threshold_filter_node.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "radar_threshold_filter/radar_threshold_filter_node.hpp" +#include "radar_threshold_filter_node.hpp" #include @@ -20,8 +20,6 @@ #include #include -using std::placeholders::_1; - namespace { template @@ -51,10 +49,11 @@ bool isWithin(double value, double max, double min) } } // namespace -namespace radar_threshold_filter +namespace autoware::radar_threshold_filter { using radar_msgs::msg::RadarReturn; using radar_msgs::msg::RadarScan; +using std::placeholders::_1; RadarThresholdFilterNode::RadarThresholdFilterNode(const rclcpp::NodeOptions & node_options) : Node("radar_threshold_filter", node_options) @@ -156,7 +155,7 @@ bool RadarThresholdFilterNode::isWithinThreshold(const RadarReturn & radar_retur return true; } -} // namespace radar_threshold_filter +} // namespace autoware::radar_threshold_filter #include "rclcpp_components/register_node_macro.hpp" -RCLCPP_COMPONENTS_REGISTER_NODE(radar_threshold_filter::RadarThresholdFilterNode) +RCLCPP_COMPONENTS_REGISTER_NODE(autoware::radar_threshold_filter::RadarThresholdFilterNode) diff --git a/sensing/radar_threshold_filter/include/radar_threshold_filter/radar_threshold_filter_node.hpp b/sensing/radar_threshold_filter/src/radar_threshold_filter_node.hpp similarity index 87% rename from sensing/radar_threshold_filter/include/radar_threshold_filter/radar_threshold_filter_node.hpp rename to sensing/radar_threshold_filter/src/radar_threshold_filter_node.hpp index ac38261dc34f1..a8a25bd972112 100644 --- a/sensing/radar_threshold_filter/include/radar_threshold_filter/radar_threshold_filter_node.hpp +++ b/sensing/radar_threshold_filter/src/radar_threshold_filter_node.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef RADAR_THRESHOLD_FILTER__RADAR_THRESHOLD_FILTER_NODE_HPP_ -#define RADAR_THRESHOLD_FILTER__RADAR_THRESHOLD_FILTER_NODE_HPP_ +#ifndef RADAR_THRESHOLD_FILTER_NODE_HPP_ +#define RADAR_THRESHOLD_FILTER_NODE_HPP_ #include @@ -24,7 +24,7 @@ #include #include -namespace radar_threshold_filter +namespace autoware::radar_threshold_filter { using radar_msgs::msg::RadarReturn; using radar_msgs::msg::RadarScan; @@ -73,6 +73,6 @@ class RadarThresholdFilterNode : public rclcpp::Node bool isWithinThreshold(const RadarReturn & radar_return); }; -} // namespace radar_threshold_filter +} // namespace autoware::radar_threshold_filter -#endif // RADAR_THRESHOLD_FILTER__RADAR_THRESHOLD_FILTER_NODE_HPP_ +#endif // RADAR_THRESHOLD_FILTER_NODE_HPP_ diff --git a/sensing/radar_threshold_filter/test/radar_threshold_filter/test_radar_threshold_filter.cpp b/sensing/radar_threshold_filter/test/radar_threshold_filter/test_radar_threshold_filter.cpp index 71a03f7d97cad..dd9063a380d69 100644 --- a/sensing/radar_threshold_filter/test/radar_threshold_filter/test_radar_threshold_filter.cpp +++ b/sensing/radar_threshold_filter/test/radar_threshold_filter/test_radar_threshold_filter.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "radar_threshold_filter/radar_threshold_filter_node.hpp" +#include "../../src/radar_threshold_filter_node.hpp" #include @@ -21,8 +21,8 @@ TEST(RadarThresholdFilter, isWithinThreshold) { rclcpp::init(0, nullptr); + using autoware::radar_threshold_filter::RadarThresholdFilterNode; using radar_msgs::msg::RadarReturn; - using radar_threshold_filter::RadarThresholdFilterNode; const double amplitude_min = -10.0; const double amplitude_max = 100.0; From 651d75bb950c3f673812fed62967b829c72675b1 Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Wed, 10 Jul 2024 14:48:53 +0900 Subject: [PATCH 24/70] refactor(radar_scan_to_pointcloud2): fix namespace and directory structure (#7912) * refactor(radar_scan_to_pointcloud2): fix namespace and directory structure Signed-off-by: Taekjin LEE * refactor(radar_scan_to_pointcloud2): add PCL dependency and update CMakeLists.txt Signed-off-by: Taekjin LEE --------- Signed-off-by: Taekjin LEE --- .../radar_scan_to_pointcloud2/CMakeLists.txt | 17 +++++++++++++---- sensing/radar_scan_to_pointcloud2/package.xml | 2 +- .../radar_scan_to_pointcloud2_node.cpp | 8 ++++---- .../radar_scan_to_pointcloud2_node.hpp | 10 +++++----- 4 files changed, 23 insertions(+), 14 deletions(-) rename sensing/radar_scan_to_pointcloud2/src/{radar_scan_to_pointcloud2_node => }/radar_scan_to_pointcloud2_node.cpp (95%) rename sensing/radar_scan_to_pointcloud2/{include/radar_scan_to_pointcloud2 => src}/radar_scan_to_pointcloud2_node.hpp (86%) diff --git a/sensing/radar_scan_to_pointcloud2/CMakeLists.txt b/sensing/radar_scan_to_pointcloud2/CMakeLists.txt index 6ec1759fc7d4d..44b668b49c87a 100644 --- a/sensing/radar_scan_to_pointcloud2/CMakeLists.txt +++ b/sensing/radar_scan_to_pointcloud2/CMakeLists.txt @@ -5,13 +5,22 @@ project(radar_scan_to_pointcloud2) find_package(autoware_cmake REQUIRED) autoware_package() +find_package(PCL REQUIRED COMPONENTS common) + # Targets -ament_auto_add_library(radar_scan_to_pointcloud2_node_component SHARED - src/radar_scan_to_pointcloud2_node/radar_scan_to_pointcloud2_node.cpp +ament_auto_add_library(${PROJECT_NAME} SHARED + src/radar_scan_to_pointcloud2_node.cpp +) + +target_include_directories(${PROJECT_NAME} + SYSTEM PUBLIC ${PCL_INCLUDE_DIRS} +) +target_link_libraries(${PROJECT_NAME} + ${PCL_LIBRARIES} ) -rclcpp_components_register_node(radar_scan_to_pointcloud2_node_component - PLUGIN "radar_scan_to_pointcloud2::RadarScanToPointcloud2Node" +rclcpp_components_register_node(${PROJECT_NAME} + PLUGIN "autoware::radar_scan_to_pointcloud2::RadarScanToPointcloud2Node" EXECUTABLE radar_scan_to_pointcloud2_node ) diff --git a/sensing/radar_scan_to_pointcloud2/package.xml b/sensing/radar_scan_to_pointcloud2/package.xml index a9d4fe7ebda8f..c8c84ee3c8a4a 100644 --- a/sensing/radar_scan_to_pointcloud2/package.xml +++ b/sensing/radar_scan_to_pointcloud2/package.xml @@ -14,8 +14,8 @@ ament_cmake_auto autoware_cmake + libpcl-common pcl_conversions - pcl_ros radar_msgs rclcpp rclcpp_components diff --git a/sensing/radar_scan_to_pointcloud2/src/radar_scan_to_pointcloud2_node/radar_scan_to_pointcloud2_node.cpp b/sensing/radar_scan_to_pointcloud2/src/radar_scan_to_pointcloud2_node.cpp similarity index 95% rename from sensing/radar_scan_to_pointcloud2/src/radar_scan_to_pointcloud2_node/radar_scan_to_pointcloud2_node.cpp rename to sensing/radar_scan_to_pointcloud2/src/radar_scan_to_pointcloud2_node.cpp index 8874b744bced6..30f267eebc205 100644 --- a/sensing/radar_scan_to_pointcloud2/src/radar_scan_to_pointcloud2_node/radar_scan_to_pointcloud2_node.cpp +++ b/sensing/radar_scan_to_pointcloud2/src/radar_scan_to_pointcloud2_node.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "radar_scan_to_pointcloud2/radar_scan_to_pointcloud2_node.hpp" +#include "radar_scan_to_pointcloud2_node.hpp" #include #include @@ -87,7 +87,7 @@ sensor_msgs::msg::PointCloud2 toDopplerPointcloud2(const radar_msgs::msg::RadarS } } // namespace -namespace radar_scan_to_pointcloud2 +namespace autoware::radar_scan_to_pointcloud2 { using radar_msgs::msg::RadarReturn; using radar_msgs::msg::RadarScan; @@ -153,7 +153,7 @@ void RadarScanToPointcloud2Node::onData(const RadarScan::ConstSharedPtr radar_ms } } -} // namespace radar_scan_to_pointcloud2 +} // namespace autoware::radar_scan_to_pointcloud2 #include "rclcpp_components/register_node_macro.hpp" -RCLCPP_COMPONENTS_REGISTER_NODE(radar_scan_to_pointcloud2::RadarScanToPointcloud2Node) +RCLCPP_COMPONENTS_REGISTER_NODE(autoware::radar_scan_to_pointcloud2::RadarScanToPointcloud2Node) diff --git a/sensing/radar_scan_to_pointcloud2/include/radar_scan_to_pointcloud2/radar_scan_to_pointcloud2_node.hpp b/sensing/radar_scan_to_pointcloud2/src/radar_scan_to_pointcloud2_node.hpp similarity index 86% rename from sensing/radar_scan_to_pointcloud2/include/radar_scan_to_pointcloud2/radar_scan_to_pointcloud2_node.hpp rename to sensing/radar_scan_to_pointcloud2/src/radar_scan_to_pointcloud2_node.hpp index bb331609f606d..f758d149b184b 100644 --- a/sensing/radar_scan_to_pointcloud2/include/radar_scan_to_pointcloud2/radar_scan_to_pointcloud2_node.hpp +++ b/sensing/radar_scan_to_pointcloud2/src/radar_scan_to_pointcloud2_node.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef RADAR_SCAN_TO_POINTCLOUD2__RADAR_SCAN_TO_POINTCLOUD2_NODE_HPP_ -#define RADAR_SCAN_TO_POINTCLOUD2__RADAR_SCAN_TO_POINTCLOUD2_NODE_HPP_ +#ifndef RADAR_SCAN_TO_POINTCLOUD2_NODE_HPP_ +#define RADAR_SCAN_TO_POINTCLOUD2_NODE_HPP_ #include "rclcpp/rclcpp.hpp" @@ -24,7 +24,7 @@ #include #include -namespace radar_scan_to_pointcloud2 +namespace autoware::radar_scan_to_pointcloud2 { using radar_msgs::msg::RadarReturn; using radar_msgs::msg::RadarScan; @@ -65,6 +65,6 @@ class RadarScanToPointcloud2Node : public rclcpp::Node NodeParam node_param_{}; }; -} // namespace radar_scan_to_pointcloud2 +} // namespace autoware::radar_scan_to_pointcloud2 -#endif // RADAR_SCAN_TO_POINTCLOUD2__RADAR_SCAN_TO_POINTCLOUD2_NODE_HPP_ +#endif // RADAR_SCAN_TO_POINTCLOUD2_NODE_HPP_ From 0db8f67b4332626e35169339b68fe57a71f62648 Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Wed, 10 Jul 2024 14:49:08 +0900 Subject: [PATCH 25/70] refactor(radar_tracks_noise_filter)!: fix namespace and directory structure (#7911) feat(radar_tracks_noise_filter): fix namespace and directory structure Signed-off-by: Taekjin LEE --- sensing/radar_tracks_noise_filter/CMakeLists.txt | 14 +++++++------- .../radar_tracks_noise_filter_node.cpp | 9 +++++---- .../radar_tracks_noise_filter_node.hpp | 10 +++++----- .../test_radar_tracks_noise_filter_is_noise.cpp | 9 +++++---- 4 files changed, 22 insertions(+), 20 deletions(-) rename sensing/radar_tracks_noise_filter/src/{radar_tracks_noise_filter_node => }/radar_tracks_noise_filter_node.cpp (93%) rename sensing/radar_tracks_noise_filter/{include/radar_tracks_noise_filter => src}/radar_tracks_noise_filter_node.hpp (84%) diff --git a/sensing/radar_tracks_noise_filter/CMakeLists.txt b/sensing/radar_tracks_noise_filter/CMakeLists.txt index a257821a90198..436c3472c8734 100644 --- a/sensing/radar_tracks_noise_filter/CMakeLists.txt +++ b/sensing/radar_tracks_noise_filter/CMakeLists.txt @@ -6,12 +6,12 @@ find_package(autoware_cmake REQUIRED) autoware_package() # Targets -ament_auto_add_library(radar_tracks_noise_filter_node_component SHARED - src/radar_tracks_noise_filter_node/radar_tracks_noise_filter_node.cpp +ament_auto_add_library(${PROJECT_NAME} SHARED + src/radar_tracks_noise_filter_node.cpp ) -rclcpp_components_register_node(radar_tracks_noise_filter_node_component - PLUGIN "radar_tracks_noise_filter::RadarTrackCrossingNoiseFilterNode" +rclcpp_components_register_node(${PROJECT_NAME} + PLUGIN "autoware::radar_tracks_noise_filter::RadarTrackCrossingNoiseFilterNode" EXECUTABLE radar_tracks_noise_filter_node ) @@ -22,10 +22,10 @@ if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) ament_lint_auto_find_test_dependencies() file(GLOB_RECURSE test_files test/**/*.cpp) - ament_add_ros_isolated_gtest(radar_tracks_noise_filter ${test_files}) + ament_add_ros_isolated_gtest(${PROJECT_NAME}_test ${test_files}) - target_link_libraries(radar_tracks_noise_filter - radar_tracks_noise_filter_node_component + target_link_libraries(${PROJECT_NAME}_test + ${PROJECT_NAME} ) endif() diff --git a/sensing/radar_tracks_noise_filter/src/radar_tracks_noise_filter_node/radar_tracks_noise_filter_node.cpp b/sensing/radar_tracks_noise_filter/src/radar_tracks_noise_filter_node.cpp similarity index 93% rename from sensing/radar_tracks_noise_filter/src/radar_tracks_noise_filter_node/radar_tracks_noise_filter_node.cpp rename to sensing/radar_tracks_noise_filter/src/radar_tracks_noise_filter_node.cpp index 82a0a21ff6fdc..c71a95c4f59f0 100644 --- a/sensing/radar_tracks_noise_filter/src/radar_tracks_noise_filter_node/radar_tracks_noise_filter_node.cpp +++ b/sensing/radar_tracks_noise_filter/src/radar_tracks_noise_filter_node.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "radar_tracks_noise_filter/radar_tracks_noise_filter_node.hpp" +#include "radar_tracks_noise_filter_node.hpp" #include #include @@ -38,7 +38,7 @@ bool update_param( } } // namespace -namespace radar_tracks_noise_filter +namespace autoware::radar_tracks_noise_filter { using radar_msgs::msg::RadarTrack; using radar_msgs::msg::RadarTracks; @@ -116,7 +116,8 @@ bool RadarTrackCrossingNoiseFilterNode::isNoise(const RadarTrack & radar_track) } } -} // namespace radar_tracks_noise_filter +} // namespace autoware::radar_tracks_noise_filter #include "rclcpp_components/register_node_macro.hpp" -RCLCPP_COMPONENTS_REGISTER_NODE(radar_tracks_noise_filter::RadarTrackCrossingNoiseFilterNode) +RCLCPP_COMPONENTS_REGISTER_NODE( + autoware::radar_tracks_noise_filter::RadarTrackCrossingNoiseFilterNode) diff --git a/sensing/radar_tracks_noise_filter/include/radar_tracks_noise_filter/radar_tracks_noise_filter_node.hpp b/sensing/radar_tracks_noise_filter/src/radar_tracks_noise_filter_node.hpp similarity index 84% rename from sensing/radar_tracks_noise_filter/include/radar_tracks_noise_filter/radar_tracks_noise_filter_node.hpp rename to sensing/radar_tracks_noise_filter/src/radar_tracks_noise_filter_node.hpp index 2ff4025cc64bc..9262c34167363 100644 --- a/sensing/radar_tracks_noise_filter/include/radar_tracks_noise_filter/radar_tracks_noise_filter_node.hpp +++ b/sensing/radar_tracks_noise_filter/src/radar_tracks_noise_filter_node.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef RADAR_TRACKS_NOISE_FILTER__RADAR_TRACKS_NOISE_FILTER_NODE_HPP_ -#define RADAR_TRACKS_NOISE_FILTER__RADAR_TRACKS_NOISE_FILTER_NODE_HPP_ +#ifndef RADAR_TRACKS_NOISE_FILTER_NODE_HPP_ +#define RADAR_TRACKS_NOISE_FILTER_NODE_HPP_ #include "rclcpp/rclcpp.hpp" @@ -23,7 +23,7 @@ #include #include -namespace radar_tracks_noise_filter +namespace autoware::radar_tracks_noise_filter { using radar_msgs::msg::RadarTrack; using radar_msgs::msg::RadarTracks; @@ -62,6 +62,6 @@ class RadarTrackCrossingNoiseFilterNode : public rclcpp::Node bool isNoise(const RadarTrack & radar_track); }; -} // namespace radar_tracks_noise_filter +} // namespace autoware::radar_tracks_noise_filter -#endif // RADAR_TRACKS_NOISE_FILTER__RADAR_TRACKS_NOISE_FILTER_NODE_HPP_ +#endif // RADAR_TRACKS_NOISE_FILTER_NODE_HPP_ diff --git a/sensing/radar_tracks_noise_filter/test/radar_tracks_noise_filter/test_radar_tracks_noise_filter_is_noise.cpp b/sensing/radar_tracks_noise_filter/test/radar_tracks_noise_filter/test_radar_tracks_noise_filter_is_noise.cpp index aa08260dee2e7..ea2dfc28b5a4c 100644 --- a/sensing/radar_tracks_noise_filter/test/radar_tracks_noise_filter/test_radar_tracks_noise_filter_is_noise.cpp +++ b/sensing/radar_tracks_noise_filter/test/radar_tracks_noise_filter/test_radar_tracks_noise_filter_is_noise.cpp @@ -12,13 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "radar_tracks_noise_filter/radar_tracks_noise_filter_node.hpp" +#include "../../src/radar_tracks_noise_filter_node.hpp" #include #include -std::shared_ptr get_node( +std::shared_ptr get_node( float velocity_y_threshold) { rclcpp::NodeOptions node_options; @@ -28,7 +28,8 @@ std::shared_ptr ge }); auto node = - std::make_shared(node_options); + std::make_shared( + node_options); return node; } @@ -45,8 +46,8 @@ radar_msgs::msg::RadarTrack getRadarTrack(float velocity_x, float velocity_y) TEST(RadarTracksNoiseFilter, isNoise) { + using autoware::radar_tracks_noise_filter::RadarTrackCrossingNoiseFilterNode; using radar_msgs::msg::RadarTrack; - using radar_tracks_noise_filter::RadarTrackCrossingNoiseFilterNode; rclcpp::init(0, nullptr); { float velocity_node_threshold = 0.0; From f2fa7150b8abfffd7031af5cf6c9b6a75de93a43 Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Wed, 10 Jul 2024 14:57:05 +0900 Subject: [PATCH 26/70] refactor(radar_static_pointcloud_filter): fix namespace and directory structure (#7914) refactor: update include paths and namespace for radar_static_pointcloud_filter --- sensing/radar_static_pointcloud_filter/CMakeLists.txt | 8 ++++---- sensing/radar_static_pointcloud_filter/package.xml | 3 --- .../radar_static_pointcloud_filter_node.cpp | 9 +++++---- .../radar_static_pointcloud_filter_node.hpp | 10 +++++----- 4 files changed, 14 insertions(+), 16 deletions(-) rename sensing/radar_static_pointcloud_filter/src/{radar_static_pointcloud_filter_node => }/radar_static_pointcloud_filter_node.cpp (95%) rename sensing/radar_static_pointcloud_filter/{include/radar_static_pointcloud_filter => src}/radar_static_pointcloud_filter_node.hpp (87%) diff --git a/sensing/radar_static_pointcloud_filter/CMakeLists.txt b/sensing/radar_static_pointcloud_filter/CMakeLists.txt index ecd33d3166b19..a796a0fa9734d 100644 --- a/sensing/radar_static_pointcloud_filter/CMakeLists.txt +++ b/sensing/radar_static_pointcloud_filter/CMakeLists.txt @@ -6,12 +6,12 @@ find_package(autoware_cmake REQUIRED) autoware_package() # Targets -ament_auto_add_library(radar_static_pointcloud_filter_node_component SHARED - src/radar_static_pointcloud_filter_node/radar_static_pointcloud_filter_node.cpp +ament_auto_add_library(${PROJECT_NAME} SHARED + src/radar_static_pointcloud_filter_node.cpp ) -rclcpp_components_register_node(radar_static_pointcloud_filter_node_component - PLUGIN "radar_static_pointcloud_filter::RadarStaticPointcloudFilterNode" +rclcpp_components_register_node(${PROJECT_NAME} + PLUGIN "autoware::radar_static_pointcloud_filter::RadarStaticPointcloudFilterNode" EXECUTABLE radar_static_pointcloud_filter_node ) diff --git a/sensing/radar_static_pointcloud_filter/package.xml b/sensing/radar_static_pointcloud_filter/package.xml index b1063562b1708..45c706791535c 100644 --- a/sensing/radar_static_pointcloud_filter/package.xml +++ b/sensing/radar_static_pointcloud_filter/package.xml @@ -20,9 +20,6 @@ radar_msgs rclcpp rclcpp_components - tf2 - tf2_geometry_msgs - tf2_ros ament_clang_format ament_lint_auto diff --git a/sensing/radar_static_pointcloud_filter/src/radar_static_pointcloud_filter_node/radar_static_pointcloud_filter_node.cpp b/sensing/radar_static_pointcloud_filter/src/radar_static_pointcloud_filter_node.cpp similarity index 95% rename from sensing/radar_static_pointcloud_filter/src/radar_static_pointcloud_filter_node/radar_static_pointcloud_filter_node.cpp rename to sensing/radar_static_pointcloud_filter/src/radar_static_pointcloud_filter_node.cpp index 985f30c8e34b5..74c099b9bf32b 100644 --- a/sensing/radar_static_pointcloud_filter/src/radar_static_pointcloud_filter_node/radar_static_pointcloud_filter_node.cpp +++ b/sensing/radar_static_pointcloud_filter/src/radar_static_pointcloud_filter_node.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "radar_static_pointcloud_filter/radar_static_pointcloud_filter_node.hpp" +#include "radar_static_pointcloud_filter_node.hpp" #include #include @@ -75,7 +75,7 @@ geometry_msgs::msg::Vector3 compensateEgoVehicleTwist( } } // namespace -namespace radar_static_pointcloud_filter +namespace autoware::radar_static_pointcloud_filter { using nav_msgs::msg::Odometry; using radar_msgs::msg::RadarReturn; @@ -167,7 +167,8 @@ bool RadarStaticPointcloudFilterNode::isStaticPointcloud( (compensated_velocity.x < node_param_.doppler_velocity_sd); } -} // namespace radar_static_pointcloud_filter +} // namespace autoware::radar_static_pointcloud_filter #include "rclcpp_components/register_node_macro.hpp" -RCLCPP_COMPONENTS_REGISTER_NODE(radar_static_pointcloud_filter::RadarStaticPointcloudFilterNode) +RCLCPP_COMPONENTS_REGISTER_NODE( + autoware::radar_static_pointcloud_filter::RadarStaticPointcloudFilterNode) diff --git a/sensing/radar_static_pointcloud_filter/include/radar_static_pointcloud_filter/radar_static_pointcloud_filter_node.hpp b/sensing/radar_static_pointcloud_filter/src/radar_static_pointcloud_filter_node.hpp similarity index 87% rename from sensing/radar_static_pointcloud_filter/include/radar_static_pointcloud_filter/radar_static_pointcloud_filter_node.hpp rename to sensing/radar_static_pointcloud_filter/src/radar_static_pointcloud_filter_node.hpp index 72a78f35251f7..eff8437eb51d2 100644 --- a/sensing/radar_static_pointcloud_filter/include/radar_static_pointcloud_filter/radar_static_pointcloud_filter_node.hpp +++ b/sensing/radar_static_pointcloud_filter/src/radar_static_pointcloud_filter_node.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef RADAR_STATIC_POINTCLOUD_FILTER__RADAR_STATIC_POINTCLOUD_FILTER_NODE_HPP_ -#define RADAR_STATIC_POINTCLOUD_FILTER__RADAR_STATIC_POINTCLOUD_FILTER_NODE_HPP_ +#ifndef RADAR_STATIC_POINTCLOUD_FILTER_NODE_HPP_ +#define RADAR_STATIC_POINTCLOUD_FILTER_NODE_HPP_ #include "autoware/universe_utils/ros/transform_listener.hpp" @@ -31,7 +31,7 @@ #include #include -namespace radar_static_pointcloud_filter +namespace autoware::radar_static_pointcloud_filter { using nav_msgs::msg::Odometry; using radar_msgs::msg::RadarReturn; @@ -77,6 +77,6 @@ class RadarStaticPointcloudFilterNode : public rclcpp::Node const RadarReturn & radar_return, const Odometry::ConstSharedPtr & odom_msg, geometry_msgs::msg::TransformStamped::ConstSharedPtr transform); }; -} // namespace radar_static_pointcloud_filter +} // namespace autoware::radar_static_pointcloud_filter -#endif // RADAR_STATIC_POINTCLOUD_FILTER__RADAR_STATIC_POINTCLOUD_FILTER_NODE_HPP_ +#endif // RADAR_STATIC_POINTCLOUD_FILTER_NODE_HPP_ From 038a71c86503cf1a22f32081ec8f5e46cf36c633 Mon Sep 17 00:00:00 2001 From: Satoshi Tanaka <16330533+scepter914@users.noreply.github.com> Date: Wed, 10 Jul 2024 15:41:17 +0900 Subject: [PATCH 27/70] feat(lidar_transfusion): update TransFusion-L model (#7890) * add num_proposals Signed-off-by: scepter914 * fix config Signed-off-by: scepter914 * update README Signed-off-by: scepter914 --------- Signed-off-by: scepter914 --- perception/lidar_transfusion/README.md | 4 ++-- .../lidar_transfusion/config/transfusion.param.yaml | 7 ++++--- .../lidar_transfusion/transfusion_config.hpp | 13 ++++++++++--- .../schema/transfusion.schema.json | 6 ++++++ .../src/lidar_transfusion_node.cpp | 5 +++-- 5 files changed, 25 insertions(+), 10 deletions(-) diff --git a/perception/lidar_transfusion/README.md b/perception/lidar_transfusion/README.md index e30403141e8ad..6745cc1f7d219 100644 --- a/perception/lidar_transfusion/README.md +++ b/perception/lidar_transfusion/README.md @@ -81,9 +81,9 @@ ros2 topic echo --field fields You can download the onnx format of trained models by clicking on the links below. -- TransFusion: [transfusion.onnx](https://awf.ml.dev.web.auto/perception/models/transfusion/v1/transfusion.onnx) +- TransFusion: [transfusion.onnx](https://awf.ml.dev.web.auto/perception/models/transfusion/t4xx1_90m/v2/transfusion.onnx) -The model was trained in TIER IV's internal database (~11k lidar frames) for 20 epochs. +The model was trained in TIER IV's internal database (~11k lidar frames) for 50 epochs. ### Changelog diff --git a/perception/lidar_transfusion/config/transfusion.param.yaml b/perception/lidar_transfusion/config/transfusion.param.yaml index feabe359caf1f..2c6680fe50af1 100644 --- a/perception/lidar_transfusion/config/transfusion.param.yaml +++ b/perception/lidar_transfusion/config/transfusion.param.yaml @@ -4,8 +4,9 @@ class_names: ["CAR", "TRUCK", "BUS", "BICYCLE", "PEDESTRIAN"] trt_precision: fp16 voxels_num: [5000, 30000, 60000] # [min, opt, max] - point_cloud_range: [-76.8, -76.8, -3.0, 76.8, 76.8, 5.0] # [x_min, y_min, z_min, x_max, y_max, z_max] - voxel_size: [0.3, 0.3, 8.0] # [x, y, z] + point_cloud_range: [-92.16, -92.16, -3.0, 92.16, 92.16, 7.0] # [x_min, y_min, z_min, x_max, y_max, z_max] + voxel_size: [0.24, 0.24, 10.0] # [x, y, z] + num_proposals: 500 onnx_path: "$(var model_path)/transfusion.onnx" engine_path: "$(var model_path)/transfusion.engine" # pre-process params @@ -17,4 +18,4 @@ iou_nms_search_distance_2d: 10.0 iou_nms_threshold: 0.1 yaw_norm_thresholds: [0.3, 0.3, 0.3, 0.3, 0.0] # refers to the class_names - score_threshold: 0.2 + score_threshold: 0.1 diff --git a/perception/lidar_transfusion/include/lidar_transfusion/transfusion_config.hpp b/perception/lidar_transfusion/include/lidar_transfusion/transfusion_config.hpp index 31976de56a9da..0ad3ab2231f50 100644 --- a/perception/lidar_transfusion/include/lidar_transfusion/transfusion_config.hpp +++ b/perception/lidar_transfusion/include/lidar_transfusion/transfusion_config.hpp @@ -26,8 +26,9 @@ class TransfusionConfig public: TransfusionConfig( const std::vector & voxels_num, const std::vector & point_cloud_range, - const std::vector & voxel_size, const float circle_nms_dist_threshold, - const std::vector & yaw_norm_thresholds, const float score_threshold) + const std::vector & voxel_size, const int num_proposals, + const float circle_nms_dist_threshold, const std::vector & yaw_norm_thresholds, + const float score_threshold) { if (voxels_num.size() == 3) { max_voxels_ = voxels_num[2]; @@ -61,6 +62,9 @@ class TransfusionConfig voxel_y_size_ = static_cast(voxel_size[1]); voxel_z_size_ = static_cast(voxel_size[2]); } + if (num_proposals > 0) { + num_proposals_ = num_proposals; + } if (score_threshold > 0.0) { score_threshold_ = score_threshold; } @@ -76,6 +80,9 @@ class TransfusionConfig grid_x_size_ = static_cast((max_x_range_ - min_x_range_) / voxel_x_size_); grid_y_size_ = static_cast((max_y_range_ - min_y_range_) / voxel_y_size_); grid_z_size_ = static_cast((max_z_range_ - min_z_range_) / voxel_z_size_); + + feature_x_size_ = grid_x_size_ / out_size_factor_; + feature_y_size_ = grid_y_size_ / out_size_factor_; } ///// INPUT PARAMETERS ///// @@ -107,7 +114,7 @@ class TransfusionConfig const std::size_t out_size_factor_{4}; const std::size_t max_num_points_per_pillar_{points_per_voxel_}; const std::size_t num_point_values_{4}; - const std::size_t num_proposals_{200}; + std::size_t num_proposals_{200}; // the number of feature maps for pillar scatter const std::size_t num_feature_scatter_{pillar_feature_size_}; // the score threshold for classification diff --git a/perception/lidar_transfusion/schema/transfusion.schema.json b/perception/lidar_transfusion/schema/transfusion.schema.json index 41d8d887236a8..7debc0edda6fb 100644 --- a/perception/lidar_transfusion/schema/transfusion.schema.json +++ b/perception/lidar_transfusion/schema/transfusion.schema.json @@ -61,6 +61,12 @@ "default": "$(var model_path)/transfusion.engine", "pattern": "\\.engine$" }, + "num_proposals": { + "type": "integer", + "description": "Number of proposals at TransHead.", + "default": 500, + "minimum": 1 + }, "down_sample_factor": { "type": "integer", "description": "A scale factor of downsampling points", diff --git a/perception/lidar_transfusion/src/lidar_transfusion_node.cpp b/perception/lidar_transfusion/src/lidar_transfusion_node.cpp index e3ea6b3780de8..7f5833e60d6d0 100644 --- a/perception/lidar_transfusion/src/lidar_transfusion_node.cpp +++ b/perception/lidar_transfusion/src/lidar_transfusion_node.cpp @@ -31,6 +31,7 @@ LidarTransfusionNode::LidarTransfusionNode(const rclcpp::NodeOptions & options) const auto point_cloud_range = this->declare_parameter>("point_cloud_range", descriptor); const auto voxel_size = this->declare_parameter>("voxel_size", descriptor); + const int num_proposals = (this->declare_parameter("num_proposals", descriptor)); const std::string onnx_path = this->declare_parameter("onnx_path", descriptor); const std::string engine_path = this->declare_parameter("engine_path", descriptor); @@ -73,8 +74,8 @@ LidarTransfusionNode::LidarTransfusionNode(const rclcpp::NodeOptions & options) DensificationParam densification_param( densification_world_frame_id, densification_num_past_frames); TransfusionConfig config( - voxels_num, point_cloud_range, voxel_size, circle_nms_dist_threshold, yaw_norm_thresholds, - score_threshold); + voxels_num, point_cloud_range, voxel_size, num_proposals, circle_nms_dist_threshold, + yaw_norm_thresholds, score_threshold); const auto allow_remapping_by_area_matrix = this->declare_parameter>("allow_remapping_by_area_matrix", descriptor); From defce4484a8b22d3a43708bb02b07ecd7a2022eb Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Wed, 10 Jul 2024 16:08:05 +0900 Subject: [PATCH 28/70] refactor(radar)!: add package name prefix of autoware_ (#7892) * refactor: rename radar_object_tracker Signed-off-by: Taekjin LEE * refactor: rename package from radar_object_tracker to autoware_radar_object_tracker Signed-off-by: Taekjin LEE * refactor: rename package from radar_object_clustering to autoware_radar_object_clustering Signed-off-by: Taekjin LEE * refactor: rename package from radar_fusion_to_detected_object to autoware_radar_fusion_to_detected_object Signed-off-by: Taekjin LEE * refactor: rename radar_crossing_objects_noise_filter to autoware_radar_crossing_objects_noise_filter Signed-off-by: Taekjin LEE * refactor: rename object_velocity_splitter to autoware_object_velocity_splitter Signed-off-by: Taekjin LEE * refactor: rename object_range_splitter to autoware_object_range_splitter Signed-off-by: Taekjin LEE * refactor: update readme Signed-off-by: Taekjin LEE --------- Signed-off-by: Taekjin LEE --- .github/CODEOWNERS | 12 ++++++------ .../detection/detection.launch.xml | 2 +- .../detection/filter/radar_filter.launch.xml | 10 +++++----- .../merger/camera_lidar_radar_merger.launch.xml | 2 +- .../object_recognition/tracking/tracking.launch.xml | 2 +- launch/tier4_perception_launch/package.xml | 12 ++++++------ .../CMakeLists.txt | 2 +- .../README.md | 4 ++-- .../config/object_range_splitter.param.yaml | 0 .../launch/object_range_splitter.launch.xml | 4 ++-- .../package.xml | 4 ++-- .../schema/object_range_splitter.schema.json | 0 .../src/object_range_splitter_node.cpp | 0 .../src/object_range_splitter_node.hpp | 0 .../CMakeLists.txt | 2 +- .../README.md | 2 +- .../config/object_velocity_splitter.param.yaml | 0 .../launch/object_velocity_splitter.launch.xml | 4 ++-- .../package.xml | 4 ++-- .../src/object_velocity_splitter_node.cpp | 0 .../src/object_velocity_splitter_node.hpp | 0 .../CMakeLists.txt | 2 +- .../README.md | 2 +- .../radar_crossing_objects_noise_filter.param.yaml | 0 .../radar_crossing_objects_noise_filter.drawio.svg | 0 .../docs/turning_around.png | Bin .../docs/vertical_velocity_objects.png | Bin .../radar_crossing_objects_noise_filter.launch.xml | 4 ++-- .../package.xml | 4 ++-- .../radar_crossing_objects_noise_filter_node.cpp | 0 .../radar_crossing_objects_noise_filter_node.hpp | 0 .../test/test_radar_crossing_objects_filter.cpp | 0 .../test_radar_crossing_objects_filter_is_noise.cpp | 0 .../CMakeLists.txt | 2 +- .../README.md | 6 +++--- ...adar_object_fusion_to_detected_object.param.yaml | 0 .../docs/algorithm.md | 0 .../radar_fusion_to_detected_object_1.drawio.svg | 0 .../radar_fusion_to_detected_object_2.drawio.svg | 0 .../radar_fusion_to_detected_object_3.drawio.svg | 0 .../radar_fusion_to_detected_object_4.drawio.svg | 0 .../radar_fusion_to_detected_object_5.drawio.svg | 0 .../radar_fusion_to_detected_object_6.drawio.svg | 0 ...adar_object_fusion_to_detected_object.launch.xml | 4 ++-- .../package.xml | 4 ++-- ...dar_object_fusion_to_detected_object.schema.json | 0 .../src/include/node.hpp | 0 .../src/include/radar_fusion_to_detected_object.hpp | 0 .../src/node.cpp | 0 .../src/radar_fusion_to_detected_object.cpp | 0 .../CMakeLists.txt | 2 +- .../README.md | 2 +- .../config/radar_object_clustering.param.yaml | 0 .../docs/clustering.drawio.svg | 0 .../docs/radar_clustering.drawio.svg | 0 .../launch/radar_object_clustering.launch.xml | 4 ++-- .../package.xml | 4 ++-- .../src/radar_object_clustering_node.cpp | 0 .../src/radar_object_clustering_node.hpp | 0 .../CMakeLists.txt | 2 +- .../README.md | 2 +- .../config/data_association_matrix.param.yaml | 0 .../config/default_tracker.param.yaml | 0 .../config/radar_object_tracker.param.yaml | 0 .../config/simulation_tracker.param.yaml | 0 .../tracking/constant_turn_rate_motion_tracker.yaml | 0 .../config/tracking/linear_motion_tracker.yaml | 0 .../image/noise_filtering.drawio.svg | 0 .../association/data_association.hpp | 0 .../association/solver/gnn_solver.hpp | 0 .../association/solver/gnn_solver_interface.hpp | 0 .../association/solver/mu_ssp.hpp | 0 .../association/solver/ssp.hpp | 0 .../model/constant_turn_rate_motion_tracker.hpp | 0 .../tracker/model/linear_motion_tracker.hpp | 0 .../tracker/model/tracker_base.hpp | 0 .../tracker/tracker.hpp | 0 .../utils/radar_object_tracker_utils.hpp | 0 .../autoware_radar_object_tracker/utils/utils.hpp | 0 .../launch/radar_object_tracker.launch.xml | 8 ++++---- .../models.md | 0 .../package.xml | 2 +- .../src/association/data_association.cpp | 0 .../mu_ssp/mu_successive_shortest_path_wrapper.cpp | 0 .../association/ssp/successive_shortest_path.cpp | 0 .../src/radar_object_tracker_node.cpp | 0 .../src/radar_object_tracker_node.hpp | 0 .../model/constant_turn_rate_motion_tracker.cpp | 0 .../src/tracker/model/linear_motion_tracker.cpp | 0 .../src/tracker/model/tracker_base.cpp | 0 .../src/utils/radar_object_tracker_utils.cpp | 0 .../src/utils/utils.cpp | 0 .../test/test_radar_object_tracker_utils.cpp | 0 perception/simple_object_merger/README.md | 2 +- 94 files changed, 61 insertions(+), 61 deletions(-) rename perception/{object_range_splitter => autoware_object_range_splitter}/CMakeLists.txt (90%) rename perception/{object_range_splitter => autoware_object_range_splitter}/README.md (94%) rename perception/{object_range_splitter => autoware_object_range_splitter}/config/object_range_splitter.param.yaml (100%) rename perception/{object_range_splitter => autoware_object_range_splitter}/launch/object_range_splitter.launch.xml (66%) rename perception/{object_range_splitter => autoware_object_range_splitter}/package.xml (86%) rename perception/{object_range_splitter => autoware_object_range_splitter}/schema/object_range_splitter.schema.json (100%) rename perception/{object_range_splitter => autoware_object_range_splitter}/src/object_range_splitter_node.cpp (100%) rename perception/{object_range_splitter => autoware_object_range_splitter}/src/object_range_splitter_node.hpp (100%) rename perception/{object_velocity_splitter => autoware_object_velocity_splitter}/CMakeLists.txt (93%) rename perception/{object_velocity_splitter => autoware_object_velocity_splitter}/README.md (95%) rename perception/{object_velocity_splitter => autoware_object_velocity_splitter}/config/object_velocity_splitter.param.yaml (100%) rename perception/{object_velocity_splitter => autoware_object_velocity_splitter}/launch/object_velocity_splitter.launch.xml (68%) rename perception/{object_velocity_splitter => autoware_object_velocity_splitter}/package.xml (89%) rename perception/{object_velocity_splitter => autoware_object_velocity_splitter}/src/object_velocity_splitter_node.cpp (100%) rename perception/{object_velocity_splitter => autoware_object_velocity_splitter}/src/object_velocity_splitter_node.hpp (100%) rename perception/{radar_crossing_objects_noise_filter => autoware_radar_crossing_objects_noise_filter}/CMakeLists.txt (94%) rename perception/{radar_crossing_objects_noise_filter => autoware_radar_crossing_objects_noise_filter}/README.md (98%) rename perception/{radar_crossing_objects_noise_filter => autoware_radar_crossing_objects_noise_filter}/config/radar_crossing_objects_noise_filter.param.yaml (100%) rename perception/{radar_crossing_objects_noise_filter => autoware_radar_crossing_objects_noise_filter}/docs/radar_crossing_objects_noise_filter.drawio.svg (100%) rename perception/{radar_crossing_objects_noise_filter => autoware_radar_crossing_objects_noise_filter}/docs/turning_around.png (100%) rename perception/{radar_crossing_objects_noise_filter => autoware_radar_crossing_objects_noise_filter}/docs/vertical_velocity_objects.png (100%) rename perception/{radar_crossing_objects_noise_filter => autoware_radar_crossing_objects_noise_filter}/launch/radar_crossing_objects_noise_filter.launch.xml (63%) rename perception/{radar_crossing_objects_noise_filter => autoware_radar_crossing_objects_noise_filter}/package.xml (88%) rename perception/{radar_crossing_objects_noise_filter => autoware_radar_crossing_objects_noise_filter}/src/radar_crossing_objects_noise_filter_node.cpp (100%) rename perception/{radar_crossing_objects_noise_filter => autoware_radar_crossing_objects_noise_filter}/src/radar_crossing_objects_noise_filter_node.hpp (100%) rename perception/{radar_crossing_objects_noise_filter => autoware_radar_crossing_objects_noise_filter}/test/test_radar_crossing_objects_filter.cpp (100%) rename perception/{radar_crossing_objects_noise_filter => autoware_radar_crossing_objects_noise_filter}/test/test_radar_crossing_objects_noise_filter/test_radar_crossing_objects_filter_is_noise.cpp (100%) rename perception/{radar_fusion_to_detected_object => autoware_radar_fusion_to_detected_object}/CMakeLists.txt (93%) rename perception/{radar_fusion_to_detected_object => autoware_radar_fusion_to_detected_object}/README.md (96%) rename perception/{radar_fusion_to_detected_object => autoware_radar_fusion_to_detected_object}/config/radar_object_fusion_to_detected_object.param.yaml (100%) rename perception/{radar_fusion_to_detected_object => autoware_radar_fusion_to_detected_object}/docs/algorithm.md (100%) rename perception/{radar_fusion_to_detected_object => autoware_radar_fusion_to_detected_object}/docs/radar_fusion_to_detected_object_1.drawio.svg (100%) rename perception/{radar_fusion_to_detected_object => autoware_radar_fusion_to_detected_object}/docs/radar_fusion_to_detected_object_2.drawio.svg (100%) rename perception/{radar_fusion_to_detected_object => autoware_radar_fusion_to_detected_object}/docs/radar_fusion_to_detected_object_3.drawio.svg (100%) rename perception/{radar_fusion_to_detected_object => autoware_radar_fusion_to_detected_object}/docs/radar_fusion_to_detected_object_4.drawio.svg (100%) rename perception/{radar_fusion_to_detected_object => autoware_radar_fusion_to_detected_object}/docs/radar_fusion_to_detected_object_5.drawio.svg (100%) rename perception/{radar_fusion_to_detected_object => autoware_radar_fusion_to_detected_object}/docs/radar_fusion_to_detected_object_6.drawio.svg (100%) rename perception/{radar_fusion_to_detected_object => autoware_radar_fusion_to_detected_object}/launch/radar_object_fusion_to_detected_object.launch.xml (60%) rename perception/{radar_fusion_to_detected_object => autoware_radar_fusion_to_detected_object}/package.xml (89%) rename perception/{radar_fusion_to_detected_object => autoware_radar_fusion_to_detected_object}/schema/radar_object_fusion_to_detected_object.schema.json (100%) rename perception/{radar_fusion_to_detected_object => autoware_radar_fusion_to_detected_object}/src/include/node.hpp (100%) rename perception/{radar_fusion_to_detected_object => autoware_radar_fusion_to_detected_object}/src/include/radar_fusion_to_detected_object.hpp (100%) rename perception/{radar_fusion_to_detected_object => autoware_radar_fusion_to_detected_object}/src/node.cpp (100%) rename perception/{radar_fusion_to_detected_object => autoware_radar_fusion_to_detected_object}/src/radar_fusion_to_detected_object.cpp (100%) rename perception/{radar_object_clustering => autoware_radar_object_clustering}/CMakeLists.txt (93%) rename perception/{radar_object_clustering => autoware_radar_object_clustering}/README.md (99%) rename perception/{radar_object_clustering => autoware_radar_object_clustering}/config/radar_object_clustering.param.yaml (100%) rename perception/{radar_object_clustering => autoware_radar_object_clustering}/docs/clustering.drawio.svg (100%) rename perception/{radar_object_clustering => autoware_radar_object_clustering}/docs/radar_clustering.drawio.svg (100%) rename perception/{radar_object_clustering => autoware_radar_object_clustering}/launch/radar_object_clustering.launch.xml (55%) rename perception/{radar_object_clustering => autoware_radar_object_clustering}/package.xml (91%) rename perception/{radar_object_clustering => autoware_radar_object_clustering}/src/radar_object_clustering_node.cpp (100%) rename perception/{radar_object_clustering => autoware_radar_object_clustering}/src/radar_object_clustering_node.hpp (100%) rename perception/{radar_object_tracker => autoware_radar_object_tracker}/CMakeLists.txt (97%) rename perception/{radar_object_tracker => autoware_radar_object_tracker}/README.md (99%) rename perception/{radar_object_tracker => autoware_radar_object_tracker}/config/data_association_matrix.param.yaml (100%) rename perception/{radar_object_tracker => autoware_radar_object_tracker}/config/default_tracker.param.yaml (100%) rename perception/{radar_object_tracker => autoware_radar_object_tracker}/config/radar_object_tracker.param.yaml (100%) rename perception/{radar_object_tracker => autoware_radar_object_tracker}/config/simulation_tracker.param.yaml (100%) rename perception/{radar_object_tracker => autoware_radar_object_tracker}/config/tracking/constant_turn_rate_motion_tracker.yaml (100%) rename perception/{radar_object_tracker => autoware_radar_object_tracker}/config/tracking/linear_motion_tracker.yaml (100%) rename perception/{radar_object_tracker => autoware_radar_object_tracker}/image/noise_filtering.drawio.svg (100%) rename perception/{radar_object_tracker => autoware_radar_object_tracker}/include/autoware_radar_object_tracker/association/data_association.hpp (100%) rename perception/{radar_object_tracker => autoware_radar_object_tracker}/include/autoware_radar_object_tracker/association/solver/gnn_solver.hpp (100%) rename perception/{radar_object_tracker => autoware_radar_object_tracker}/include/autoware_radar_object_tracker/association/solver/gnn_solver_interface.hpp (100%) rename perception/{radar_object_tracker => autoware_radar_object_tracker}/include/autoware_radar_object_tracker/association/solver/mu_ssp.hpp (100%) rename perception/{radar_object_tracker => autoware_radar_object_tracker}/include/autoware_radar_object_tracker/association/solver/ssp.hpp (100%) rename perception/{radar_object_tracker => autoware_radar_object_tracker}/include/autoware_radar_object_tracker/tracker/model/constant_turn_rate_motion_tracker.hpp (100%) rename perception/{radar_object_tracker => autoware_radar_object_tracker}/include/autoware_radar_object_tracker/tracker/model/linear_motion_tracker.hpp (100%) rename perception/{radar_object_tracker => autoware_radar_object_tracker}/include/autoware_radar_object_tracker/tracker/model/tracker_base.hpp (100%) rename perception/{radar_object_tracker => autoware_radar_object_tracker}/include/autoware_radar_object_tracker/tracker/tracker.hpp (100%) rename perception/{radar_object_tracker => autoware_radar_object_tracker}/include/autoware_radar_object_tracker/utils/radar_object_tracker_utils.hpp (100%) rename perception/{radar_object_tracker => autoware_radar_object_tracker}/include/autoware_radar_object_tracker/utils/utils.hpp (100%) rename perception/{radar_object_tracker => autoware_radar_object_tracker}/launch/radar_object_tracker.launch.xml (63%) rename perception/{radar_object_tracker => autoware_radar_object_tracker}/models.md (100%) rename perception/{radar_object_tracker => autoware_radar_object_tracker}/package.xml (96%) rename perception/{radar_object_tracker => autoware_radar_object_tracker}/src/association/data_association.cpp (100%) rename perception/{radar_object_tracker => autoware_radar_object_tracker}/src/association/mu_ssp/mu_successive_shortest_path_wrapper.cpp (100%) rename perception/{radar_object_tracker => autoware_radar_object_tracker}/src/association/ssp/successive_shortest_path.cpp (100%) rename perception/{radar_object_tracker => autoware_radar_object_tracker}/src/radar_object_tracker_node.cpp (100%) rename perception/{radar_object_tracker => autoware_radar_object_tracker}/src/radar_object_tracker_node.hpp (100%) rename perception/{radar_object_tracker => autoware_radar_object_tracker}/src/tracker/model/constant_turn_rate_motion_tracker.cpp (100%) rename perception/{radar_object_tracker => autoware_radar_object_tracker}/src/tracker/model/linear_motion_tracker.cpp (100%) rename perception/{radar_object_tracker => autoware_radar_object_tracker}/src/tracker/model/tracker_base.cpp (100%) rename perception/{radar_object_tracker => autoware_radar_object_tracker}/src/utils/radar_object_tracker_utils.cpp (100%) rename perception/{radar_object_tracker => autoware_radar_object_tracker}/src/utils/utils.cpp (100%) rename perception/{radar_object_tracker => autoware_radar_object_tracker}/test/test_radar_object_tracker_utils.cpp (100%) diff --git a/.github/CODEOWNERS b/.github/CODEOWNERS index ed496d7b7ba74..48e4a9921bb9b 100644 --- a/.github/CODEOWNERS +++ b/.github/CODEOWNERS @@ -122,14 +122,14 @@ perception/lidar_centerpoint/** kenzo.lobos@tier4.jp koji.minoda@tier4.jp perception/lidar_transfusion/** amadeusz.szymko.2@tier4.jp kenzo.lobos@tier4.jp satoshi.tanaka@tier4.jp perception/multi_object_tracker/** taekjin.lee@tier4.jp yoshi.ri@tier4.jp yukihiro.saito@tier4.jp perception/object_merger/** taekjin.lee@tier4.jp yoshi.ri@tier4.jp yukihiro.saito@tier4.jp -perception/object_range_splitter/** yoshi.ri@tier4.jp yukihiro.saito@tier4.jp -perception/object_velocity_splitter/** satoshi.tanaka@tier4.jp shunsuke.miura@tier4.jp yoshi.ri@tier4.jp +perception/autoware_object_range_splitter/** yoshi.ri@tier4.jp yukihiro.saito@tier4.jp +perception/autoware_object_velocity_splitter/** satoshi.tanaka@tier4.jp shunsuke.miura@tier4.jp yoshi.ri@tier4.jp perception/occupancy_grid_map_outlier_filter/** abrahammonrroy@yahoo.com yoshi.ri@tier4.jp yukihiro.saito@tier4.jp perception/probabilistic_occupancy_grid_map/** mamoru.sobue@tier4.jp yoshi.ri@tier4.jp yukihiro.saito@tier4.jp -perception/radar_crossing_objects_noise_filter/** satoshi.tanaka@tier4.jp shunsuke.miura@tier4.jp taekjin.lee@tier4.jp yoshi.ri@tier4.jp -perception/radar_fusion_to_detected_object/** satoshi.tanaka@tier4.jp shunsuke.miura@tier4.jp taekjin.lee@tier4.jp yoshi.ri@tier4.jp -perception/radar_object_clustering/** satoshi.tanaka@tier4.jp shunsuke.miura@tier4.jp taekjin.lee@tier4.jp yoshi.ri@tier4.jp -perception/radar_object_tracker/** satoshi.tanaka@tier4.jp taekjin.lee@tier4.jp yoshi.ri@tier4.jp yukihiro.saito@tier4.jp +perception/autoware_radar_crossing_objects_noise_filter/** satoshi.tanaka@tier4.jp shunsuke.miura@tier4.jp taekjin.lee@tier4.jp yoshi.ri@tier4.jp +perception/autoware_radar_fusion_to_detected_object/** satoshi.tanaka@tier4.jp shunsuke.miura@tier4.jp taekjin.lee@tier4.jp yoshi.ri@tier4.jp +perception/autoware_radar_object_clustering/** satoshi.tanaka@tier4.jp shunsuke.miura@tier4.jp taekjin.lee@tier4.jp yoshi.ri@tier4.jp +perception/autoware_radar_object_tracker/** satoshi.tanaka@tier4.jp taekjin.lee@tier4.jp yoshi.ri@tier4.jp yukihiro.saito@tier4.jp perception/radar_tracks_msgs_converter/** satoshi.tanaka@tier4.jp shunsuke.miura@tier4.jp taekjin.lee@tier4.jp yoshi.ri@tier4.jp perception/raindrop_cluster_filter/** dai.nguyen@tier4.jp yoshi.ri@tier4.jp yukihiro.saito@tier4.jp perception/shape_estimation/** yoshi.ri@tier4.jp yukihiro.saito@tier4.jp diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/detection.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/detection.launch.xml index b3280fee47364..8f64b624540e7 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/detection/detection.launch.xml +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/detection.launch.xml @@ -359,7 +359,7 @@ - + diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/filter/radar_filter.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/filter/radar_filter.launch.xml index fe7ab70e9331e..151e3f28739d9 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/detection/filter/radar_filter.launch.xml +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/filter/radar_filter.launch.xml @@ -5,28 +5,28 @@ - + - + - + - + @@ -39,7 +39,7 @@ - + diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/merger/camera_lidar_radar_merger.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/merger/camera_lidar_radar_merger.launch.xml index 6ea745a8bae2c..5bd2c36955b55 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/detection/merger/camera_lidar_radar_merger.launch.xml +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/merger/camera_lidar_radar_merger.launch.xml @@ -89,7 +89,7 @@ - + diff --git a/launch/tier4_perception_launch/launch/object_recognition/tracking/tracking.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/tracking/tracking.launch.xml index 1975ad2d15eb9..580436411a895 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/tracking/tracking.launch.xml +++ b/launch/tier4_perception_launch/launch/object_recognition/tracking/tracking.launch.xml @@ -46,7 +46,7 @@ - + diff --git a/launch/tier4_perception_launch/package.xml b/launch/tier4_perception_launch/package.xml index 592ef7cf01f86..349472ed5f8d3 100644 --- a/launch/tier4_perception_launch/package.xml +++ b/launch/tier4_perception_launch/package.xml @@ -15,6 +15,12 @@ autoware_crosswalk_traffic_light_estimator autoware_map_based_prediction + autoware_object_range_splitter + autoware_object_velocity_splitter + autoware_radar_crossing_objects_noise_filter + autoware_radar_fusion_to_detected_object + autoware_radar_object_clustering + autoware_radar_object_tracker cluster_merger compare_map_segmentation detected_object_feature_remover @@ -28,16 +34,10 @@ lidar_apollo_instance_segmentation multi_object_tracker object_merger - object_range_splitter - object_velocity_splitter occupancy_grid_map_outlier_filter pointcloud_preprocessor pointcloud_to_laserscan probabilistic_occupancy_grid_map - radar_crossing_objects_noise_filter - radar_fusion_to_detected_object - radar_object_clustering - radar_object_tracker raindrop_cluster_filter shape_estimation topic_tools diff --git a/perception/object_range_splitter/CMakeLists.txt b/perception/autoware_object_range_splitter/CMakeLists.txt similarity index 90% rename from perception/object_range_splitter/CMakeLists.txt rename to perception/autoware_object_range_splitter/CMakeLists.txt index 1da71a31b26d0..2c2d8e4108578 100644 --- a/perception/object_range_splitter/CMakeLists.txt +++ b/perception/autoware_object_range_splitter/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.14) -project(object_range_splitter) +project(autoware_object_range_splitter) find_package(autoware_cmake REQUIRED) autoware_package() diff --git a/perception/object_range_splitter/README.md b/perception/autoware_object_range_splitter/README.md similarity index 94% rename from perception/object_range_splitter/README.md rename to perception/autoware_object_range_splitter/README.md index 39375c3139b49..3cdfe36528094 100644 --- a/perception/object_range_splitter/README.md +++ b/perception/autoware_object_range_splitter/README.md @@ -1,8 +1,8 @@ -# object_range_splitter +# `autoware_object_range_splitter` ## Purpose -object_range_splitter is a package to divide detected objects into two messages by the distance from the origin. +autoware_object_range_splitter is a package to divide detected objects into two messages by the distance from the origin. ## Inner-workings / Algorithms diff --git a/perception/object_range_splitter/config/object_range_splitter.param.yaml b/perception/autoware_object_range_splitter/config/object_range_splitter.param.yaml similarity index 100% rename from perception/object_range_splitter/config/object_range_splitter.param.yaml rename to perception/autoware_object_range_splitter/config/object_range_splitter.param.yaml diff --git a/perception/object_range_splitter/launch/object_range_splitter.launch.xml b/perception/autoware_object_range_splitter/launch/object_range_splitter.launch.xml similarity index 66% rename from perception/object_range_splitter/launch/object_range_splitter.launch.xml rename to perception/autoware_object_range_splitter/launch/object_range_splitter.launch.xml index 3f2f3c6ba24c6..77ac04681ac7f 100644 --- a/perception/object_range_splitter/launch/object_range_splitter.launch.xml +++ b/perception/autoware_object_range_splitter/launch/object_range_splitter.launch.xml @@ -3,9 +3,9 @@ - + - + diff --git a/perception/object_range_splitter/package.xml b/perception/autoware_object_range_splitter/package.xml similarity index 86% rename from perception/object_range_splitter/package.xml rename to perception/autoware_object_range_splitter/package.xml index 7fc9245ee894f..1f678f72b7942 100644 --- a/perception/object_range_splitter/package.xml +++ b/perception/autoware_object_range_splitter/package.xml @@ -1,9 +1,9 @@ - object_range_splitter + autoware_object_range_splitter 0.1.0 - The object_range_splitter package + The autoware_object_range_splitter package Yukihiro Saito Yoshi Ri Apache License 2.0 diff --git a/perception/object_range_splitter/schema/object_range_splitter.schema.json b/perception/autoware_object_range_splitter/schema/object_range_splitter.schema.json similarity index 100% rename from perception/object_range_splitter/schema/object_range_splitter.schema.json rename to perception/autoware_object_range_splitter/schema/object_range_splitter.schema.json diff --git a/perception/object_range_splitter/src/object_range_splitter_node.cpp b/perception/autoware_object_range_splitter/src/object_range_splitter_node.cpp similarity index 100% rename from perception/object_range_splitter/src/object_range_splitter_node.cpp rename to perception/autoware_object_range_splitter/src/object_range_splitter_node.cpp diff --git a/perception/object_range_splitter/src/object_range_splitter_node.hpp b/perception/autoware_object_range_splitter/src/object_range_splitter_node.hpp similarity index 100% rename from perception/object_range_splitter/src/object_range_splitter_node.hpp rename to perception/autoware_object_range_splitter/src/object_range_splitter_node.hpp diff --git a/perception/object_velocity_splitter/CMakeLists.txt b/perception/autoware_object_velocity_splitter/CMakeLists.txt similarity index 93% rename from perception/object_velocity_splitter/CMakeLists.txt rename to perception/autoware_object_velocity_splitter/CMakeLists.txt index afad278e383d8..ec6b57083e058 100644 --- a/perception/object_velocity_splitter/CMakeLists.txt +++ b/perception/autoware_object_velocity_splitter/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.5) -project(object_velocity_splitter) +project(autoware_object_velocity_splitter) # Dependencies find_package(autoware_cmake REQUIRED) diff --git a/perception/object_velocity_splitter/README.md b/perception/autoware_object_velocity_splitter/README.md similarity index 95% rename from perception/object_velocity_splitter/README.md rename to perception/autoware_object_velocity_splitter/README.md index 659ae5845f46a..ac57d5d7f6353 100644 --- a/perception/object_velocity_splitter/README.md +++ b/perception/autoware_object_velocity_splitter/README.md @@ -1,4 +1,4 @@ -# object_velocity_splitter +# autoware_object_velocity_splitter This package contains a object filter module for [autoware_perception_msgs/msg/DetectedObject](https://github.com/autowarefoundation/autoware_msgs/tree/main/autoware_perception_msgs/msg/DetectedObject.idl). This package can split DetectedObjects into two messages by object's speed. diff --git a/perception/object_velocity_splitter/config/object_velocity_splitter.param.yaml b/perception/autoware_object_velocity_splitter/config/object_velocity_splitter.param.yaml similarity index 100% rename from perception/object_velocity_splitter/config/object_velocity_splitter.param.yaml rename to perception/autoware_object_velocity_splitter/config/object_velocity_splitter.param.yaml diff --git a/perception/object_velocity_splitter/launch/object_velocity_splitter.launch.xml b/perception/autoware_object_velocity_splitter/launch/object_velocity_splitter.launch.xml similarity index 68% rename from perception/object_velocity_splitter/launch/object_velocity_splitter.launch.xml rename to perception/autoware_object_velocity_splitter/launch/object_velocity_splitter.launch.xml index 8ab654d4907a4..f3a359d0ca246 100644 --- a/perception/object_velocity_splitter/launch/object_velocity_splitter.launch.xml +++ b/perception/autoware_object_velocity_splitter/launch/object_velocity_splitter.launch.xml @@ -5,10 +5,10 @@ - + - + diff --git a/perception/object_velocity_splitter/package.xml b/perception/autoware_object_velocity_splitter/package.xml similarity index 89% rename from perception/object_velocity_splitter/package.xml rename to perception/autoware_object_velocity_splitter/package.xml index 32fd5b2ff6337..eb233150a44d9 100644 --- a/perception/object_velocity_splitter/package.xml +++ b/perception/autoware_object_velocity_splitter/package.xml @@ -1,9 +1,9 @@ - object_velocity_splitter + autoware_object_velocity_splitter 0.1.0 - object_velocity_splitter + autoware_object_velocity_splitter Sathshi Tanaka Shunsuke Miura Yoshi Ri diff --git a/perception/object_velocity_splitter/src/object_velocity_splitter_node.cpp b/perception/autoware_object_velocity_splitter/src/object_velocity_splitter_node.cpp similarity index 100% rename from perception/object_velocity_splitter/src/object_velocity_splitter_node.cpp rename to perception/autoware_object_velocity_splitter/src/object_velocity_splitter_node.cpp diff --git a/perception/object_velocity_splitter/src/object_velocity_splitter_node.hpp b/perception/autoware_object_velocity_splitter/src/object_velocity_splitter_node.hpp similarity index 100% rename from perception/object_velocity_splitter/src/object_velocity_splitter_node.hpp rename to perception/autoware_object_velocity_splitter/src/object_velocity_splitter_node.hpp diff --git a/perception/radar_crossing_objects_noise_filter/CMakeLists.txt b/perception/autoware_radar_crossing_objects_noise_filter/CMakeLists.txt similarity index 94% rename from perception/radar_crossing_objects_noise_filter/CMakeLists.txt rename to perception/autoware_radar_crossing_objects_noise_filter/CMakeLists.txt index 6414739413d5c..aebd29f72b360 100644 --- a/perception/radar_crossing_objects_noise_filter/CMakeLists.txt +++ b/perception/autoware_radar_crossing_objects_noise_filter/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.5) -project(radar_crossing_objects_noise_filter) +project(autoware_radar_crossing_objects_noise_filter) # Dependencies find_package(autoware_cmake REQUIRED) diff --git a/perception/radar_crossing_objects_noise_filter/README.md b/perception/autoware_radar_crossing_objects_noise_filter/README.md similarity index 98% rename from perception/radar_crossing_objects_noise_filter/README.md rename to perception/autoware_radar_crossing_objects_noise_filter/README.md index 96a624a1d4872..846da0a0c11d1 100644 --- a/perception/radar_crossing_objects_noise_filter/README.md +++ b/perception/autoware_radar_crossing_objects_noise_filter/README.md @@ -1,4 +1,4 @@ -# radar_crossing_objects_noise_filter +# autoware_radar_crossing_objects_noise_filter This package contains a radar noise filter module for [autoware_perception_msgs/msg/DetectedObject](https://github.com/autowarefoundation/autoware_msgs/tree/main/autoware_perception_msgs/msg/DetectedObject.idl). This package can filter the noise objects which cross to the ego vehicle. diff --git a/perception/radar_crossing_objects_noise_filter/config/radar_crossing_objects_noise_filter.param.yaml b/perception/autoware_radar_crossing_objects_noise_filter/config/radar_crossing_objects_noise_filter.param.yaml similarity index 100% rename from perception/radar_crossing_objects_noise_filter/config/radar_crossing_objects_noise_filter.param.yaml rename to perception/autoware_radar_crossing_objects_noise_filter/config/radar_crossing_objects_noise_filter.param.yaml diff --git a/perception/radar_crossing_objects_noise_filter/docs/radar_crossing_objects_noise_filter.drawio.svg b/perception/autoware_radar_crossing_objects_noise_filter/docs/radar_crossing_objects_noise_filter.drawio.svg similarity index 100% rename from perception/radar_crossing_objects_noise_filter/docs/radar_crossing_objects_noise_filter.drawio.svg rename to perception/autoware_radar_crossing_objects_noise_filter/docs/radar_crossing_objects_noise_filter.drawio.svg diff --git a/perception/radar_crossing_objects_noise_filter/docs/turning_around.png b/perception/autoware_radar_crossing_objects_noise_filter/docs/turning_around.png similarity index 100% rename from perception/radar_crossing_objects_noise_filter/docs/turning_around.png rename to perception/autoware_radar_crossing_objects_noise_filter/docs/turning_around.png diff --git a/perception/radar_crossing_objects_noise_filter/docs/vertical_velocity_objects.png b/perception/autoware_radar_crossing_objects_noise_filter/docs/vertical_velocity_objects.png similarity index 100% rename from perception/radar_crossing_objects_noise_filter/docs/vertical_velocity_objects.png rename to perception/autoware_radar_crossing_objects_noise_filter/docs/vertical_velocity_objects.png diff --git a/perception/radar_crossing_objects_noise_filter/launch/radar_crossing_objects_noise_filter.launch.xml b/perception/autoware_radar_crossing_objects_noise_filter/launch/radar_crossing_objects_noise_filter.launch.xml similarity index 63% rename from perception/radar_crossing_objects_noise_filter/launch/radar_crossing_objects_noise_filter.launch.xml rename to perception/autoware_radar_crossing_objects_noise_filter/launch/radar_crossing_objects_noise_filter.launch.xml index 25856e12c3ba5..54cafd5086642 100644 --- a/perception/radar_crossing_objects_noise_filter/launch/radar_crossing_objects_noise_filter.launch.xml +++ b/perception/autoware_radar_crossing_objects_noise_filter/launch/radar_crossing_objects_noise_filter.launch.xml @@ -5,10 +5,10 @@ - + - + diff --git a/perception/radar_crossing_objects_noise_filter/package.xml b/perception/autoware_radar_crossing_objects_noise_filter/package.xml similarity index 88% rename from perception/radar_crossing_objects_noise_filter/package.xml rename to perception/autoware_radar_crossing_objects_noise_filter/package.xml index d0245dfe62a40..6476d66eef4f8 100644 --- a/perception/radar_crossing_objects_noise_filter/package.xml +++ b/perception/autoware_radar_crossing_objects_noise_filter/package.xml @@ -1,9 +1,9 @@ - radar_crossing_objects_noise_filter + autoware_radar_crossing_objects_noise_filter 0.1.0 - radar_crossing_objects_noise_filter + autoware_radar_crossing_objects_noise_filter Sathshi Tanaka Shunsuke Miura Yoshi Ri diff --git a/perception/radar_crossing_objects_noise_filter/src/radar_crossing_objects_noise_filter_node.cpp b/perception/autoware_radar_crossing_objects_noise_filter/src/radar_crossing_objects_noise_filter_node.cpp similarity index 100% rename from perception/radar_crossing_objects_noise_filter/src/radar_crossing_objects_noise_filter_node.cpp rename to perception/autoware_radar_crossing_objects_noise_filter/src/radar_crossing_objects_noise_filter_node.cpp diff --git a/perception/radar_crossing_objects_noise_filter/src/radar_crossing_objects_noise_filter_node.hpp b/perception/autoware_radar_crossing_objects_noise_filter/src/radar_crossing_objects_noise_filter_node.hpp similarity index 100% rename from perception/radar_crossing_objects_noise_filter/src/radar_crossing_objects_noise_filter_node.hpp rename to perception/autoware_radar_crossing_objects_noise_filter/src/radar_crossing_objects_noise_filter_node.hpp diff --git a/perception/radar_crossing_objects_noise_filter/test/test_radar_crossing_objects_filter.cpp b/perception/autoware_radar_crossing_objects_noise_filter/test/test_radar_crossing_objects_filter.cpp similarity index 100% rename from perception/radar_crossing_objects_noise_filter/test/test_radar_crossing_objects_filter.cpp rename to perception/autoware_radar_crossing_objects_noise_filter/test/test_radar_crossing_objects_filter.cpp diff --git a/perception/radar_crossing_objects_noise_filter/test/test_radar_crossing_objects_noise_filter/test_radar_crossing_objects_filter_is_noise.cpp b/perception/autoware_radar_crossing_objects_noise_filter/test/test_radar_crossing_objects_noise_filter/test_radar_crossing_objects_filter_is_noise.cpp similarity index 100% rename from perception/radar_crossing_objects_noise_filter/test/test_radar_crossing_objects_noise_filter/test_radar_crossing_objects_filter_is_noise.cpp rename to perception/autoware_radar_crossing_objects_noise_filter/test/test_radar_crossing_objects_noise_filter/test_radar_crossing_objects_filter_is_noise.cpp diff --git a/perception/radar_fusion_to_detected_object/CMakeLists.txt b/perception/autoware_radar_fusion_to_detected_object/CMakeLists.txt similarity index 93% rename from perception/radar_fusion_to_detected_object/CMakeLists.txt rename to perception/autoware_radar_fusion_to_detected_object/CMakeLists.txt index 76c27d3958841..aab25d6f54133 100644 --- a/perception/radar_fusion_to_detected_object/CMakeLists.txt +++ b/perception/autoware_radar_fusion_to_detected_object/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.5) -project(radar_fusion_to_detected_object) +project(autoware_radar_fusion_to_detected_object) # Dependencies find_package(autoware_cmake REQUIRED) diff --git a/perception/radar_fusion_to_detected_object/README.md b/perception/autoware_radar_fusion_to_detected_object/README.md similarity index 96% rename from perception/radar_fusion_to_detected_object/README.md rename to perception/autoware_radar_fusion_to_detected_object/README.md index 831dbd24e4360..c57b710b09619 100644 --- a/perception/radar_fusion_to_detected_object/README.md +++ b/perception/autoware_radar_fusion_to_detected_object/README.md @@ -1,4 +1,4 @@ -# radar_fusion_to_detected_object +# `autoware_radar_fusion_to_detected_object` This package contains a sensor fusion module for radar-detected objects and 3D detected objects. @@ -94,7 +94,7 @@ If the probability of an output object is lower than this parameter, and the out This parameter is the flag to use probability compensation. If this parameter is true, compensate probability of objects to threshold probability. -## Interface for radar_object_fusion_to_detected_object +## Interface for `autoware_radar_object_fusion_to_detected_object` Sensor fusion with radar objects and a detected object. @@ -105,7 +105,7 @@ Sensor fusion with radar objects and a detected object. ### How to launch ```sh -ros2 launch radar_fusion_to_detected_object radar_object_to_detected_object.launch.xml +ros2 launch autoware_radar_fusion_to_detected_object radar_object_to_detected_object.launch.xml ``` ### Input diff --git a/perception/radar_fusion_to_detected_object/config/radar_object_fusion_to_detected_object.param.yaml b/perception/autoware_radar_fusion_to_detected_object/config/radar_object_fusion_to_detected_object.param.yaml similarity index 100% rename from perception/radar_fusion_to_detected_object/config/radar_object_fusion_to_detected_object.param.yaml rename to perception/autoware_radar_fusion_to_detected_object/config/radar_object_fusion_to_detected_object.param.yaml diff --git a/perception/radar_fusion_to_detected_object/docs/algorithm.md b/perception/autoware_radar_fusion_to_detected_object/docs/algorithm.md similarity index 100% rename from perception/radar_fusion_to_detected_object/docs/algorithm.md rename to perception/autoware_radar_fusion_to_detected_object/docs/algorithm.md diff --git a/perception/radar_fusion_to_detected_object/docs/radar_fusion_to_detected_object_1.drawio.svg b/perception/autoware_radar_fusion_to_detected_object/docs/radar_fusion_to_detected_object_1.drawio.svg similarity index 100% rename from perception/radar_fusion_to_detected_object/docs/radar_fusion_to_detected_object_1.drawio.svg rename to perception/autoware_radar_fusion_to_detected_object/docs/radar_fusion_to_detected_object_1.drawio.svg diff --git a/perception/radar_fusion_to_detected_object/docs/radar_fusion_to_detected_object_2.drawio.svg b/perception/autoware_radar_fusion_to_detected_object/docs/radar_fusion_to_detected_object_2.drawio.svg similarity index 100% rename from perception/radar_fusion_to_detected_object/docs/radar_fusion_to_detected_object_2.drawio.svg rename to perception/autoware_radar_fusion_to_detected_object/docs/radar_fusion_to_detected_object_2.drawio.svg diff --git a/perception/radar_fusion_to_detected_object/docs/radar_fusion_to_detected_object_3.drawio.svg b/perception/autoware_radar_fusion_to_detected_object/docs/radar_fusion_to_detected_object_3.drawio.svg similarity index 100% rename from perception/radar_fusion_to_detected_object/docs/radar_fusion_to_detected_object_3.drawio.svg rename to perception/autoware_radar_fusion_to_detected_object/docs/radar_fusion_to_detected_object_3.drawio.svg diff --git a/perception/radar_fusion_to_detected_object/docs/radar_fusion_to_detected_object_4.drawio.svg b/perception/autoware_radar_fusion_to_detected_object/docs/radar_fusion_to_detected_object_4.drawio.svg similarity index 100% rename from perception/radar_fusion_to_detected_object/docs/radar_fusion_to_detected_object_4.drawio.svg rename to perception/autoware_radar_fusion_to_detected_object/docs/radar_fusion_to_detected_object_4.drawio.svg diff --git a/perception/radar_fusion_to_detected_object/docs/radar_fusion_to_detected_object_5.drawio.svg b/perception/autoware_radar_fusion_to_detected_object/docs/radar_fusion_to_detected_object_5.drawio.svg similarity index 100% rename from perception/radar_fusion_to_detected_object/docs/radar_fusion_to_detected_object_5.drawio.svg rename to perception/autoware_radar_fusion_to_detected_object/docs/radar_fusion_to_detected_object_5.drawio.svg diff --git a/perception/radar_fusion_to_detected_object/docs/radar_fusion_to_detected_object_6.drawio.svg b/perception/autoware_radar_fusion_to_detected_object/docs/radar_fusion_to_detected_object_6.drawio.svg similarity index 100% rename from perception/radar_fusion_to_detected_object/docs/radar_fusion_to_detected_object_6.drawio.svg rename to perception/autoware_radar_fusion_to_detected_object/docs/radar_fusion_to_detected_object_6.drawio.svg diff --git a/perception/radar_fusion_to_detected_object/launch/radar_object_fusion_to_detected_object.launch.xml b/perception/autoware_radar_fusion_to_detected_object/launch/radar_object_fusion_to_detected_object.launch.xml similarity index 60% rename from perception/radar_fusion_to_detected_object/launch/radar_object_fusion_to_detected_object.launch.xml rename to perception/autoware_radar_fusion_to_detected_object/launch/radar_object_fusion_to_detected_object.launch.xml index e5505ad87d9ec..d074c66464f72 100644 --- a/perception/radar_fusion_to_detected_object/launch/radar_object_fusion_to_detected_object.launch.xml +++ b/perception/autoware_radar_fusion_to_detected_object/launch/radar_object_fusion_to_detected_object.launch.xml @@ -5,10 +5,10 @@ - + - + diff --git a/perception/radar_fusion_to_detected_object/package.xml b/perception/autoware_radar_fusion_to_detected_object/package.xml similarity index 89% rename from perception/radar_fusion_to_detected_object/package.xml rename to perception/autoware_radar_fusion_to_detected_object/package.xml index 2094ff7770555..94a0c55574f60 100644 --- a/perception/radar_fusion_to_detected_object/package.xml +++ b/perception/autoware_radar_fusion_to_detected_object/package.xml @@ -1,9 +1,9 @@ - radar_fusion_to_detected_object + autoware_radar_fusion_to_detected_object 0.0.0 - radar_fusion_to_detected_object + autoware_radar_fusion_to_detected_object Satoshi Tanaka Shunsuke Miura Yoshi Ri diff --git a/perception/radar_fusion_to_detected_object/schema/radar_object_fusion_to_detected_object.schema.json b/perception/autoware_radar_fusion_to_detected_object/schema/radar_object_fusion_to_detected_object.schema.json similarity index 100% rename from perception/radar_fusion_to_detected_object/schema/radar_object_fusion_to_detected_object.schema.json rename to perception/autoware_radar_fusion_to_detected_object/schema/radar_object_fusion_to_detected_object.schema.json diff --git a/perception/radar_fusion_to_detected_object/src/include/node.hpp b/perception/autoware_radar_fusion_to_detected_object/src/include/node.hpp similarity index 100% rename from perception/radar_fusion_to_detected_object/src/include/node.hpp rename to perception/autoware_radar_fusion_to_detected_object/src/include/node.hpp diff --git a/perception/radar_fusion_to_detected_object/src/include/radar_fusion_to_detected_object.hpp b/perception/autoware_radar_fusion_to_detected_object/src/include/radar_fusion_to_detected_object.hpp similarity index 100% rename from perception/radar_fusion_to_detected_object/src/include/radar_fusion_to_detected_object.hpp rename to perception/autoware_radar_fusion_to_detected_object/src/include/radar_fusion_to_detected_object.hpp diff --git a/perception/radar_fusion_to_detected_object/src/node.cpp b/perception/autoware_radar_fusion_to_detected_object/src/node.cpp similarity index 100% rename from perception/radar_fusion_to_detected_object/src/node.cpp rename to perception/autoware_radar_fusion_to_detected_object/src/node.cpp diff --git a/perception/radar_fusion_to_detected_object/src/radar_fusion_to_detected_object.cpp b/perception/autoware_radar_fusion_to_detected_object/src/radar_fusion_to_detected_object.cpp similarity index 100% rename from perception/radar_fusion_to_detected_object/src/radar_fusion_to_detected_object.cpp rename to perception/autoware_radar_fusion_to_detected_object/src/radar_fusion_to_detected_object.cpp diff --git a/perception/radar_object_clustering/CMakeLists.txt b/perception/autoware_radar_object_clustering/CMakeLists.txt similarity index 93% rename from perception/radar_object_clustering/CMakeLists.txt rename to perception/autoware_radar_object_clustering/CMakeLists.txt index 9a54bbb0dae43..43f6a3f73ebc0 100644 --- a/perception/radar_object_clustering/CMakeLists.txt +++ b/perception/autoware_radar_object_clustering/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.5) -project(radar_object_clustering) +project(autoware_radar_object_clustering) # Dependencies find_package(autoware_cmake REQUIRED) diff --git a/perception/radar_object_clustering/README.md b/perception/autoware_radar_object_clustering/README.md similarity index 99% rename from perception/radar_object_clustering/README.md rename to perception/autoware_radar_object_clustering/README.md index 8f936ce61a1f3..5fbb4df81a115 100644 --- a/perception/radar_object_clustering/README.md +++ b/perception/autoware_radar_object_clustering/README.md @@ -1,4 +1,4 @@ -# radar_object_clustering +# `autoware_radar_object_clustering` This package contains a radar object clustering for [autoware_perception_msgs/msg/DetectedObject](https://github.com/autowarefoundation/autoware_msgs/tree/main/autoware_perception_msgs/msg/DetectedObject.idl) input. diff --git a/perception/radar_object_clustering/config/radar_object_clustering.param.yaml b/perception/autoware_radar_object_clustering/config/radar_object_clustering.param.yaml similarity index 100% rename from perception/radar_object_clustering/config/radar_object_clustering.param.yaml rename to perception/autoware_radar_object_clustering/config/radar_object_clustering.param.yaml diff --git a/perception/radar_object_clustering/docs/clustering.drawio.svg b/perception/autoware_radar_object_clustering/docs/clustering.drawio.svg similarity index 100% rename from perception/radar_object_clustering/docs/clustering.drawio.svg rename to perception/autoware_radar_object_clustering/docs/clustering.drawio.svg diff --git a/perception/radar_object_clustering/docs/radar_clustering.drawio.svg b/perception/autoware_radar_object_clustering/docs/radar_clustering.drawio.svg similarity index 100% rename from perception/radar_object_clustering/docs/radar_clustering.drawio.svg rename to perception/autoware_radar_object_clustering/docs/radar_clustering.drawio.svg diff --git a/perception/radar_object_clustering/launch/radar_object_clustering.launch.xml b/perception/autoware_radar_object_clustering/launch/radar_object_clustering.launch.xml similarity index 55% rename from perception/radar_object_clustering/launch/radar_object_clustering.launch.xml rename to perception/autoware_radar_object_clustering/launch/radar_object_clustering.launch.xml index 1759c17a5ab6b..a701b9f45b480 100644 --- a/perception/radar_object_clustering/launch/radar_object_clustering.launch.xml +++ b/perception/autoware_radar_object_clustering/launch/radar_object_clustering.launch.xml @@ -1,10 +1,10 @@ - + - + diff --git a/perception/radar_object_clustering/package.xml b/perception/autoware_radar_object_clustering/package.xml similarity index 91% rename from perception/radar_object_clustering/package.xml rename to perception/autoware_radar_object_clustering/package.xml index c7666d2c5701e..d9c3464e02b18 100644 --- a/perception/radar_object_clustering/package.xml +++ b/perception/autoware_radar_object_clustering/package.xml @@ -1,9 +1,9 @@ - radar_object_clustering + autoware_radar_object_clustering 0.1.0 - radar_object_clustering + autoware_radar_object_clustering Sathshi Tanaka Shunsuke Miura Yoshi Ri diff --git a/perception/radar_object_clustering/src/radar_object_clustering_node.cpp b/perception/autoware_radar_object_clustering/src/radar_object_clustering_node.cpp similarity index 100% rename from perception/radar_object_clustering/src/radar_object_clustering_node.cpp rename to perception/autoware_radar_object_clustering/src/radar_object_clustering_node.cpp diff --git a/perception/radar_object_clustering/src/radar_object_clustering_node.hpp b/perception/autoware_radar_object_clustering/src/radar_object_clustering_node.hpp similarity index 100% rename from perception/radar_object_clustering/src/radar_object_clustering_node.hpp rename to perception/autoware_radar_object_clustering/src/radar_object_clustering_node.hpp diff --git a/perception/radar_object_tracker/CMakeLists.txt b/perception/autoware_radar_object_tracker/CMakeLists.txt similarity index 97% rename from perception/radar_object_tracker/CMakeLists.txt rename to perception/autoware_radar_object_tracker/CMakeLists.txt index b732ef6cc863c..614dbfc6e1a30 100644 --- a/perception/radar_object_tracker/CMakeLists.txt +++ b/perception/autoware_radar_object_tracker/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.8) -project(radar_object_tracker) +project(autoware_radar_object_tracker) find_package(autoware_cmake REQUIRED) autoware_package() diff --git a/perception/radar_object_tracker/README.md b/perception/autoware_radar_object_tracker/README.md similarity index 99% rename from perception/radar_object_tracker/README.md rename to perception/autoware_radar_object_tracker/README.md index 056a27d8c4ed0..f9ca194de2491 100644 --- a/perception/radar_object_tracker/README.md +++ b/perception/autoware_radar_object_tracker/README.md @@ -1,4 +1,4 @@ -# Radar Object Tracker +# `autoware_radar_object_tracker` ## Purpose diff --git a/perception/radar_object_tracker/config/data_association_matrix.param.yaml b/perception/autoware_radar_object_tracker/config/data_association_matrix.param.yaml similarity index 100% rename from perception/radar_object_tracker/config/data_association_matrix.param.yaml rename to perception/autoware_radar_object_tracker/config/data_association_matrix.param.yaml diff --git a/perception/radar_object_tracker/config/default_tracker.param.yaml b/perception/autoware_radar_object_tracker/config/default_tracker.param.yaml similarity index 100% rename from perception/radar_object_tracker/config/default_tracker.param.yaml rename to perception/autoware_radar_object_tracker/config/default_tracker.param.yaml diff --git a/perception/radar_object_tracker/config/radar_object_tracker.param.yaml b/perception/autoware_radar_object_tracker/config/radar_object_tracker.param.yaml similarity index 100% rename from perception/radar_object_tracker/config/radar_object_tracker.param.yaml rename to perception/autoware_radar_object_tracker/config/radar_object_tracker.param.yaml diff --git a/perception/radar_object_tracker/config/simulation_tracker.param.yaml b/perception/autoware_radar_object_tracker/config/simulation_tracker.param.yaml similarity index 100% rename from perception/radar_object_tracker/config/simulation_tracker.param.yaml rename to perception/autoware_radar_object_tracker/config/simulation_tracker.param.yaml diff --git a/perception/radar_object_tracker/config/tracking/constant_turn_rate_motion_tracker.yaml b/perception/autoware_radar_object_tracker/config/tracking/constant_turn_rate_motion_tracker.yaml similarity index 100% rename from perception/radar_object_tracker/config/tracking/constant_turn_rate_motion_tracker.yaml rename to perception/autoware_radar_object_tracker/config/tracking/constant_turn_rate_motion_tracker.yaml diff --git a/perception/radar_object_tracker/config/tracking/linear_motion_tracker.yaml b/perception/autoware_radar_object_tracker/config/tracking/linear_motion_tracker.yaml similarity index 100% rename from perception/radar_object_tracker/config/tracking/linear_motion_tracker.yaml rename to perception/autoware_radar_object_tracker/config/tracking/linear_motion_tracker.yaml diff --git a/perception/radar_object_tracker/image/noise_filtering.drawio.svg b/perception/autoware_radar_object_tracker/image/noise_filtering.drawio.svg similarity index 100% rename from perception/radar_object_tracker/image/noise_filtering.drawio.svg rename to perception/autoware_radar_object_tracker/image/noise_filtering.drawio.svg diff --git a/perception/radar_object_tracker/include/autoware_radar_object_tracker/association/data_association.hpp b/perception/autoware_radar_object_tracker/include/autoware_radar_object_tracker/association/data_association.hpp similarity index 100% rename from perception/radar_object_tracker/include/autoware_radar_object_tracker/association/data_association.hpp rename to perception/autoware_radar_object_tracker/include/autoware_radar_object_tracker/association/data_association.hpp diff --git a/perception/radar_object_tracker/include/autoware_radar_object_tracker/association/solver/gnn_solver.hpp b/perception/autoware_radar_object_tracker/include/autoware_radar_object_tracker/association/solver/gnn_solver.hpp similarity index 100% rename from perception/radar_object_tracker/include/autoware_radar_object_tracker/association/solver/gnn_solver.hpp rename to perception/autoware_radar_object_tracker/include/autoware_radar_object_tracker/association/solver/gnn_solver.hpp diff --git a/perception/radar_object_tracker/include/autoware_radar_object_tracker/association/solver/gnn_solver_interface.hpp b/perception/autoware_radar_object_tracker/include/autoware_radar_object_tracker/association/solver/gnn_solver_interface.hpp similarity index 100% rename from perception/radar_object_tracker/include/autoware_radar_object_tracker/association/solver/gnn_solver_interface.hpp rename to perception/autoware_radar_object_tracker/include/autoware_radar_object_tracker/association/solver/gnn_solver_interface.hpp diff --git a/perception/radar_object_tracker/include/autoware_radar_object_tracker/association/solver/mu_ssp.hpp b/perception/autoware_radar_object_tracker/include/autoware_radar_object_tracker/association/solver/mu_ssp.hpp similarity index 100% rename from perception/radar_object_tracker/include/autoware_radar_object_tracker/association/solver/mu_ssp.hpp rename to perception/autoware_radar_object_tracker/include/autoware_radar_object_tracker/association/solver/mu_ssp.hpp diff --git a/perception/radar_object_tracker/include/autoware_radar_object_tracker/association/solver/ssp.hpp b/perception/autoware_radar_object_tracker/include/autoware_radar_object_tracker/association/solver/ssp.hpp similarity index 100% rename from perception/radar_object_tracker/include/autoware_radar_object_tracker/association/solver/ssp.hpp rename to perception/autoware_radar_object_tracker/include/autoware_radar_object_tracker/association/solver/ssp.hpp diff --git a/perception/radar_object_tracker/include/autoware_radar_object_tracker/tracker/model/constant_turn_rate_motion_tracker.hpp b/perception/autoware_radar_object_tracker/include/autoware_radar_object_tracker/tracker/model/constant_turn_rate_motion_tracker.hpp similarity index 100% rename from perception/radar_object_tracker/include/autoware_radar_object_tracker/tracker/model/constant_turn_rate_motion_tracker.hpp rename to perception/autoware_radar_object_tracker/include/autoware_radar_object_tracker/tracker/model/constant_turn_rate_motion_tracker.hpp diff --git a/perception/radar_object_tracker/include/autoware_radar_object_tracker/tracker/model/linear_motion_tracker.hpp b/perception/autoware_radar_object_tracker/include/autoware_radar_object_tracker/tracker/model/linear_motion_tracker.hpp similarity index 100% rename from perception/radar_object_tracker/include/autoware_radar_object_tracker/tracker/model/linear_motion_tracker.hpp rename to perception/autoware_radar_object_tracker/include/autoware_radar_object_tracker/tracker/model/linear_motion_tracker.hpp diff --git a/perception/radar_object_tracker/include/autoware_radar_object_tracker/tracker/model/tracker_base.hpp b/perception/autoware_radar_object_tracker/include/autoware_radar_object_tracker/tracker/model/tracker_base.hpp similarity index 100% rename from perception/radar_object_tracker/include/autoware_radar_object_tracker/tracker/model/tracker_base.hpp rename to perception/autoware_radar_object_tracker/include/autoware_radar_object_tracker/tracker/model/tracker_base.hpp diff --git a/perception/radar_object_tracker/include/autoware_radar_object_tracker/tracker/tracker.hpp b/perception/autoware_radar_object_tracker/include/autoware_radar_object_tracker/tracker/tracker.hpp similarity index 100% rename from perception/radar_object_tracker/include/autoware_radar_object_tracker/tracker/tracker.hpp rename to perception/autoware_radar_object_tracker/include/autoware_radar_object_tracker/tracker/tracker.hpp diff --git a/perception/radar_object_tracker/include/autoware_radar_object_tracker/utils/radar_object_tracker_utils.hpp b/perception/autoware_radar_object_tracker/include/autoware_radar_object_tracker/utils/radar_object_tracker_utils.hpp similarity index 100% rename from perception/radar_object_tracker/include/autoware_radar_object_tracker/utils/radar_object_tracker_utils.hpp rename to perception/autoware_radar_object_tracker/include/autoware_radar_object_tracker/utils/radar_object_tracker_utils.hpp diff --git a/perception/radar_object_tracker/include/autoware_radar_object_tracker/utils/utils.hpp b/perception/autoware_radar_object_tracker/include/autoware_radar_object_tracker/utils/utils.hpp similarity index 100% rename from perception/radar_object_tracker/include/autoware_radar_object_tracker/utils/utils.hpp rename to perception/autoware_radar_object_tracker/include/autoware_radar_object_tracker/utils/utils.hpp diff --git a/perception/radar_object_tracker/launch/radar_object_tracker.launch.xml b/perception/autoware_radar_object_tracker/launch/radar_object_tracker.launch.xml similarity index 63% rename from perception/radar_object_tracker/launch/radar_object_tracker.launch.xml rename to perception/autoware_radar_object_tracker/launch/radar_object_tracker.launch.xml index 313ef4b0f9fcd..16e96e527af94 100644 --- a/perception/radar_object_tracker/launch/radar_object_tracker.launch.xml +++ b/perception/autoware_radar_object_tracker/launch/radar_object_tracker.launch.xml @@ -2,12 +2,12 @@ - - - + + + - + diff --git a/perception/radar_object_tracker/models.md b/perception/autoware_radar_object_tracker/models.md similarity index 100% rename from perception/radar_object_tracker/models.md rename to perception/autoware_radar_object_tracker/models.md diff --git a/perception/radar_object_tracker/package.xml b/perception/autoware_radar_object_tracker/package.xml similarity index 96% rename from perception/radar_object_tracker/package.xml rename to perception/autoware_radar_object_tracker/package.xml index f104e2d7ea456..50dd70e6cec67 100644 --- a/perception/radar_object_tracker/package.xml +++ b/perception/autoware_radar_object_tracker/package.xml @@ -1,7 +1,7 @@ - radar_object_tracker + autoware_radar_object_tracker 0.0.0 Do tracking radar object Yoshi Ri diff --git a/perception/radar_object_tracker/src/association/data_association.cpp b/perception/autoware_radar_object_tracker/src/association/data_association.cpp similarity index 100% rename from perception/radar_object_tracker/src/association/data_association.cpp rename to perception/autoware_radar_object_tracker/src/association/data_association.cpp diff --git a/perception/radar_object_tracker/src/association/mu_ssp/mu_successive_shortest_path_wrapper.cpp b/perception/autoware_radar_object_tracker/src/association/mu_ssp/mu_successive_shortest_path_wrapper.cpp similarity index 100% rename from perception/radar_object_tracker/src/association/mu_ssp/mu_successive_shortest_path_wrapper.cpp rename to perception/autoware_radar_object_tracker/src/association/mu_ssp/mu_successive_shortest_path_wrapper.cpp diff --git a/perception/radar_object_tracker/src/association/ssp/successive_shortest_path.cpp b/perception/autoware_radar_object_tracker/src/association/ssp/successive_shortest_path.cpp similarity index 100% rename from perception/radar_object_tracker/src/association/ssp/successive_shortest_path.cpp rename to perception/autoware_radar_object_tracker/src/association/ssp/successive_shortest_path.cpp diff --git a/perception/radar_object_tracker/src/radar_object_tracker_node.cpp b/perception/autoware_radar_object_tracker/src/radar_object_tracker_node.cpp similarity index 100% rename from perception/radar_object_tracker/src/radar_object_tracker_node.cpp rename to perception/autoware_radar_object_tracker/src/radar_object_tracker_node.cpp diff --git a/perception/radar_object_tracker/src/radar_object_tracker_node.hpp b/perception/autoware_radar_object_tracker/src/radar_object_tracker_node.hpp similarity index 100% rename from perception/radar_object_tracker/src/radar_object_tracker_node.hpp rename to perception/autoware_radar_object_tracker/src/radar_object_tracker_node.hpp diff --git a/perception/radar_object_tracker/src/tracker/model/constant_turn_rate_motion_tracker.cpp b/perception/autoware_radar_object_tracker/src/tracker/model/constant_turn_rate_motion_tracker.cpp similarity index 100% rename from perception/radar_object_tracker/src/tracker/model/constant_turn_rate_motion_tracker.cpp rename to perception/autoware_radar_object_tracker/src/tracker/model/constant_turn_rate_motion_tracker.cpp diff --git a/perception/radar_object_tracker/src/tracker/model/linear_motion_tracker.cpp b/perception/autoware_radar_object_tracker/src/tracker/model/linear_motion_tracker.cpp similarity index 100% rename from perception/radar_object_tracker/src/tracker/model/linear_motion_tracker.cpp rename to perception/autoware_radar_object_tracker/src/tracker/model/linear_motion_tracker.cpp diff --git a/perception/radar_object_tracker/src/tracker/model/tracker_base.cpp b/perception/autoware_radar_object_tracker/src/tracker/model/tracker_base.cpp similarity index 100% rename from perception/radar_object_tracker/src/tracker/model/tracker_base.cpp rename to perception/autoware_radar_object_tracker/src/tracker/model/tracker_base.cpp diff --git a/perception/radar_object_tracker/src/utils/radar_object_tracker_utils.cpp b/perception/autoware_radar_object_tracker/src/utils/radar_object_tracker_utils.cpp similarity index 100% rename from perception/radar_object_tracker/src/utils/radar_object_tracker_utils.cpp rename to perception/autoware_radar_object_tracker/src/utils/radar_object_tracker_utils.cpp diff --git a/perception/radar_object_tracker/src/utils/utils.cpp b/perception/autoware_radar_object_tracker/src/utils/utils.cpp similarity index 100% rename from perception/radar_object_tracker/src/utils/utils.cpp rename to perception/autoware_radar_object_tracker/src/utils/utils.cpp diff --git a/perception/radar_object_tracker/test/test_radar_object_tracker_utils.cpp b/perception/autoware_radar_object_tracker/test/test_radar_object_tracker_utils.cpp similarity index 100% rename from perception/radar_object_tracker/test/test_radar_object_tracker_utils.cpp rename to perception/autoware_radar_object_tracker/test/test_radar_object_tracker_utils.cpp diff --git a/perception/simple_object_merger/README.md b/perception/simple_object_merger/README.md index 782012ff1eaa1..f69b31f70969e 100644 --- a/perception/simple_object_merger/README.md +++ b/perception/simple_object_merger/README.md @@ -25,7 +25,7 @@ Merged objects will not be published until all topic data is received when initi - Post-processing -Because this package does not have matching processing, there are overlapping objects depending on the input objects. So output objects can be used only when post-processing is used. For now, [clustering processing](https://github.com/autowarefoundation/autoware.universe/tree/main/perception/radar_object_clustering) can be used as post-processing. +Because this package does not have matching processing, there are overlapping objects depending on the input objects. So output objects can be used only when post-processing is used. For now, [clustering processing](https://github.com/autowarefoundation/autoware.universe/tree/main/perception/autoware_radar_object_clustering) can be used as post-processing. ## Interface From b1d7fcd1c3833c981e1eb5ce9a4c1aa9cbf5932c Mon Sep 17 00:00:00 2001 From: kobayu858 <129580202+kobayu858@users.noreply.github.com> Date: Wed, 10 Jul 2024 16:15:36 +0900 Subject: [PATCH 29/70] fix(tensorrt_common): fix shadowVariable (#7906) * fix:shadowVariable Signed-off-by: kobayu858 * fix:shadowVariable Signed-off-by: kobayu858 --------- Signed-off-by: kobayu858 --- .../tensorrt_common/src/tensorrt_common.cpp | 20 +++++++++---------- 1 file changed, 10 insertions(+), 10 deletions(-) diff --git a/common/tensorrt_common/src/tensorrt_common.cpp b/common/tensorrt_common/src/tensorrt_common.cpp index 2b218cd3e49f2..a22372f74a286 100644 --- a/common/tensorrt_common/src/tensorrt_common.cpp +++ b/common/tensorrt_common/src/tensorrt_common.cpp @@ -427,18 +427,18 @@ bool TrtCommon::buildEngineFromOnnx( layer->setPrecision(nvinfer1::DataType::kHALF); std::cout << "Set kHALF in " << name << std::endl; } - for (int i = num - 1; i >= 0; i--) { - nvinfer1::ILayer * layer = network->getLayer(i); - auto layer_type = layer->getType(); - std::string name = layer->getName(); - if (layer_type == nvinfer1::LayerType::kCONVOLUTION) { - layer->setPrecision(nvinfer1::DataType::kHALF); - std::cout << "Set kHALF in " << name << std::endl; + for (int j = num - 1; j >= 0; j--) { + nvinfer1::ILayer * inner_layer = network->getLayer(j); + auto inner_layer_type = inner_layer->getType(); + std::string inner_name = inner_layer->getName(); + if (inner_layer_type == nvinfer1::LayerType::kCONVOLUTION) { + inner_layer->setPrecision(nvinfer1::DataType::kHALF); + std::cout << "Set kHALF in " << inner_name << std::endl; break; } - if (layer_type == nvinfer1::LayerType::kMATRIX_MULTIPLY) { - layer->setPrecision(nvinfer1::DataType::kHALF); - std::cout << "Set kHALF in " << name << std::endl; + if (inner_layer_type == nvinfer1::LayerType::kMATRIX_MULTIPLY) { + inner_layer->setPrecision(nvinfer1::DataType::kHALF); + std::cout << "Set kHALF in " << inner_name << std::endl; break; } } From 08deb7840c28a319e7be44a34f3ef479af14b59c Mon Sep 17 00:00:00 2001 From: Masato Saeki <78376491+MasatoSaeki@users.noreply.github.com> Date: Wed, 10 Jul 2024 16:26:24 +0900 Subject: [PATCH 30/70] feat(tensorrt_yolox): change CMakeLists to be able to build cuda function without runtime gpu (#7800) change CMakeLists like lidar_centerpoint Signed-off-by: MasatoSaeki --- perception/tensorrt_yolox/CMakeLists.txt | 68 +++++++++++++++++++++--- 1 file changed, 62 insertions(+), 6 deletions(-) diff --git a/perception/tensorrt_yolox/CMakeLists.txt b/perception/tensorrt_yolox/CMakeLists.txt index a5498a845e62e..cac574aff8623 100644 --- a/perception/tensorrt_yolox/CMakeLists.txt +++ b/perception/tensorrt_yolox/CMakeLists.txt @@ -11,13 +11,69 @@ find_package(autoware_cmake REQUIRED) autoware_package() find_package(OpenCV REQUIRED) -include(CheckLanguage) -check_language(CUDA) -if(CMAKE_CUDA_COMPILER) - enable_language(CUDA) + +option(CUDA_VERBOSE "Verbose output of CUDA modules" OFF) + +# set flags for CUDA availability +option(CUDA_AVAIL "CUDA available" OFF) +find_package(CUDA) +if(CUDA_FOUND) + find_library(CUBLAS_LIBRARIES cublas HINTS + ${CUDA_TOOLKIT_ROOT_DIR}/lib64 + ${CUDA_TOOLKIT_ROOT_DIR}/lib + ) + if(CUDA_VERBOSE) + message("CUDA is available!") + message("CUDA Libs: ${CUDA_LIBRARIES}") + message("CUDA Headers: ${CUDA_INCLUDE_DIRS}") + endif() + # Note: cublas_device was depreciated in CUDA version 9.2 + # https://forums.developer.nvidia.com/t/where-can-i-find-libcublas-device-so-or-libcublas-device-a/67251/4 + # In LibTorch, CUDA_cublas_device_LIBRARY is used. + unset(CUDA_cublas_device_LIBRARY CACHE) + set(CUDA_AVAIL ON) +else() + message("CUDA NOT FOUND") + set(CUDA_AVAIL OFF) +endif() + +# set flags for TensorRT availability +option(TRT_AVAIL "TensorRT available" OFF) +# try to find the tensorRT modules +find_library(NVINFER nvinfer) +find_library(NVONNXPARSER nvonnxparser) +if(NVINFER AND NVONNXPARSER) + if(CUDA_VERBOSE) + message("TensorRT is available!") + message("NVINFER: ${NVINFER}") + message("NVONNXPARSER: ${NVONNXPARSER}") + endif() + set(TRT_AVAIL ON) else() - message(WARNING "CUDA is not found. preprocess acceleration using CUDA will not be available.") + message("TensorRT is NOT Available") + set(TRT_AVAIL OFF) endif() + +# set flags for CUDNN availability +option(CUDNN_AVAIL "CUDNN available" OFF) +# try to find the CUDNN module +find_library(CUDNN_LIBRARY +NAMES libcudnn.so${__cudnn_ver_suffix} libcudnn${__cudnn_ver_suffix}.dylib ${__cudnn_lib_win_name} +PATHS $ENV{LD_LIBRARY_PATH} ${__libpath_cudart} ${CUDNN_ROOT_DIR} ${PC_CUDNN_LIBRARY_DIRS} ${CMAKE_INSTALL_PREFIX} +PATH_SUFFIXES lib lib64 bin +DOC "CUDNN library." +) +if(CUDNN_LIBRARY) + if(CUDA_VERBOSE) + message(STATUS "CUDNN is available!") + message(STATUS "CUDNN_LIBRARY: ${CUDNN_LIBRARY}") + endif() + set(CUDNN_AVAIL ON) +else() + message("CUDNN is NOT Available") + set(CUDNN_AVAIL OFF) +endif() + find_package(OpenMP) if(OpenMP_FOUND) set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${OpenMP_C_FLAGS}") @@ -34,7 +90,7 @@ ament_target_dependencies(${PROJECT_NAME} OpenCV ) -if(CMAKE_CUDA_COMPILER) +if(TRT_AVAIL AND CUDA_AVAIL AND CUDNN_AVAIL) # Officially, add_library supports .cu file compilation. # However, as of cmake 3.22.1, it seems to fail compilation because compiler flags for # C++ are directly passed to nvcc (they are originally space separated From f65af78d7515c44f1b1b09cd0c0777852e6894ac Mon Sep 17 00:00:00 2001 From: kminoda <44218668+kminoda@users.noreply.github.com> Date: Wed, 10 Jul 2024 18:08:20 +0900 Subject: [PATCH 31/70] chore(localization, map): remove maintainer (#7940) Signed-off-by: kminoda --- launch/tier4_localization_launch/package.xml | 1 - launch/tier4_map_launch/package.xml | 1 - localization/autoware_stop_filter/package.xml | 1 - localization/ekf_localizer/package.xml | 1 - localization/geo_pose_projector/package.xml | 1 - localization/localization_error_monitor/package.xml | 1 - localization/ndt_scan_matcher/package.xml | 1 - localization/twist2accel/package.xml | 1 - localization/yabloc/yabloc_monitor/package.xml | 1 - map/map_loader/package.xml | 1 - map/map_projection_loader/package.xml | 1 - sensing/gnss_poser/package.xml | 1 - sensing/imu_corrector/package.xml | 1 - 13 files changed, 13 deletions(-) diff --git a/launch/tier4_localization_launch/package.xml b/launch/tier4_localization_launch/package.xml index 150744bbae4a7..b6f3ad4c27eea 100644 --- a/launch/tier4_localization_launch/package.xml +++ b/launch/tier4_localization_launch/package.xml @@ -5,7 +5,6 @@ 0.1.0 The tier4_localization_launch package Yamato Ando - Koji Minoda Kento Yabuuchi Ryu Yamamoto NGUYEN Viet Anh diff --git a/launch/tier4_map_launch/package.xml b/launch/tier4_map_launch/package.xml index 05643e354cfce..bca5a853d69f7 100644 --- a/launch/tier4_map_launch/package.xml +++ b/launch/tier4_map_launch/package.xml @@ -5,7 +5,6 @@ 0.1.0 The tier4_map_launch package Ryu Yamamoto - Koji Minoda Kento Yabuuchi Yamato Ando NGUYEN Viet Anh diff --git a/localization/autoware_stop_filter/package.xml b/localization/autoware_stop_filter/package.xml index dfdbd5f182983..d020f390e5bbf 100644 --- a/localization/autoware_stop_filter/package.xml +++ b/localization/autoware_stop_filter/package.xml @@ -4,7 +4,6 @@ autoware_stop_filter 0.0.0 The stop filter package - Koji Minoda Yamato Ando Masahiro Sakamoto Kento Yabuuchi diff --git a/localization/ekf_localizer/package.xml b/localization/ekf_localizer/package.xml index 9c89867025632..b53538de11f15 100644 --- a/localization/ekf_localizer/package.xml +++ b/localization/ekf_localizer/package.xml @@ -5,7 +5,6 @@ 0.1.0 The ekf_localizer package Takamasa Horibe - Koji Minoda Yamato Ando Takeshi Ishita Masahiro Sakamoto diff --git a/localization/geo_pose_projector/package.xml b/localization/geo_pose_projector/package.xml index 36b2aec8384ac..3ffa7cb1c7111 100644 --- a/localization/geo_pose_projector/package.xml +++ b/localization/geo_pose_projector/package.xml @@ -5,7 +5,6 @@ 0.1.0 The geo_pose_projector package Yamato Ando - Koji Minoda Masahiro Sakamoto Kento Yabuuchi NGUYEN Viet Anh diff --git a/localization/localization_error_monitor/package.xml b/localization/localization_error_monitor/package.xml index b8a466f95dfab..02602cbebff37 100644 --- a/localization/localization_error_monitor/package.xml +++ b/localization/localization_error_monitor/package.xml @@ -5,7 +5,6 @@ 0.1.0 ros node for monitoring localization error Yamato Ando - Koji Minoda Masahiro Sakamoto Kento Yabuuchi NGUYEN Viet Anh diff --git a/localization/ndt_scan_matcher/package.xml b/localization/ndt_scan_matcher/package.xml index c69836d77c825..932ee55874cea 100644 --- a/localization/ndt_scan_matcher/package.xml +++ b/localization/ndt_scan_matcher/package.xml @@ -6,7 +6,6 @@ The ndt_scan_matcher package Yamato Ando Kento Yabuuchi - Koji Minoda Masahiro Sakamoto NGUYEN Viet Anh Taiki Yamada diff --git a/localization/twist2accel/package.xml b/localization/twist2accel/package.xml index 0dbce08f309ac..158d8a8a8d283 100644 --- a/localization/twist2accel/package.xml +++ b/localization/twist2accel/package.xml @@ -4,7 +4,6 @@ twist2accel 0.0.0 The acceleration estimation package - Koji Minoda Yamato Ando Masahiro Sakamoto Kento Yabuuchi diff --git a/localization/yabloc/yabloc_monitor/package.xml b/localization/yabloc/yabloc_monitor/package.xml index a42a734dbab31..f3e6cfcdfbe5a 100644 --- a/localization/yabloc/yabloc_monitor/package.xml +++ b/localization/yabloc/yabloc_monitor/package.xml @@ -5,7 +5,6 @@ 0.1.0 YabLoc monitor package Kento Yabuuchi - Koji Minoda Masahiro Sakamoto Yamato Ando NGUYEN Viet Anh diff --git a/map/map_loader/package.xml b/map/map_loader/package.xml index 7c4d9eaf99556..6cceff98a8f9a 100644 --- a/map/map_loader/package.xml +++ b/map/map_loader/package.xml @@ -6,7 +6,6 @@ The map_loader package Yamato Ando Ryu Yamamoto - Koji Minoda Masahiro Sakamoto Kento Yabuuchi NGUYEN Viet Anh diff --git a/map/map_projection_loader/package.xml b/map/map_projection_loader/package.xml index b77ef18ac2293..c36c22c29a0fd 100644 --- a/map/map_projection_loader/package.xml +++ b/map/map_projection_loader/package.xml @@ -4,7 +4,6 @@ map_projection_loader 0.1.0 map_projection_loader package as a ROS 2 node - Koji Minoda Yamato Ando Masahiro Sakamoto Kento Yabuuchi diff --git a/sensing/gnss_poser/package.xml b/sensing/gnss_poser/package.xml index 2de081b6b009f..eef6d690b6e72 100644 --- a/sensing/gnss_poser/package.xml +++ b/sensing/gnss_poser/package.xml @@ -5,7 +5,6 @@ 1.0.0 The ROS 2 gnss_poser package Yamato Ando - Koji Minoda Masahiro Sakamoto Kento Yabuuchi NGUYEN Viet Anh diff --git a/sensing/imu_corrector/package.xml b/sensing/imu_corrector/package.xml index cbc28e2ff0eeb..f5c719d2b4f7e 100644 --- a/sensing/imu_corrector/package.xml +++ b/sensing/imu_corrector/package.xml @@ -6,7 +6,6 @@ The ROS 2 imu_corrector package Yamato Ando Taiki Yamada - Koji Minoda Apache License 2.0 Yamato Ando From aaf465f7cad0d80056add548594d6ef6d0571388 Mon Sep 17 00:00:00 2001 From: kobayu858 <129580202+kobayu858@users.noreply.github.com> Date: Wed, 10 Jul 2024 19:15:11 +0900 Subject: [PATCH 32/70] fix(object_merger): fix shadowVariable (#7941) fix:shadowVariable Signed-off-by: kobayu858 --- .../src/association/solver/successive_shortest_path.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/perception/object_merger/src/association/solver/successive_shortest_path.cpp b/perception/object_merger/src/association/solver/successive_shortest_path.cpp index 782aa1ef4ac31..e384f12d60051 100644 --- a/perception/object_merger/src/association/solver/successive_shortest_path.cpp +++ b/perception/object_merger/src/association/solver/successive_shortest_path.cpp @@ -328,12 +328,12 @@ void SSP::maximizeLinearAssignment( #ifndef NDEBUG // Check if the potentials are feasible potentials - for (int v = 0; v < n_nodes; ++v) { - for (auto it_incident_edge = adjacency_list.at(v).cbegin(); - it_incident_edge != adjacency_list.at(v).cend(); ++it_incident_edge) { + for (int w = 0; w < n_nodes; ++w) { + for (auto it_incident_edge = adjacency_list.at(w).cbegin(); + it_incident_edge != adjacency_list.at(w).cend(); ++it_incident_edge) { if (it_incident_edge->capacity > 0) { double reduced_cost = - it_incident_edge->cost + potentials.at(v) - potentials.at(it_incident_edge->dst); + it_incident_edge->cost + potentials.at(w) - potentials.at(it_incident_edge->dst); assert(reduced_cost >= 0); } } From 74e70b49d6481e56c2acf6cdc82edca1da105aa1 Mon Sep 17 00:00:00 2001 From: kobayu858 <129580202+kobayu858@users.noreply.github.com> Date: Wed, 10 Jul 2024 19:15:34 +0900 Subject: [PATCH 33/70] fix(multi_object_tracker): fix shadowVariable (#7939) fix:shadowVariable Signed-off-by: kobayu858 --- .../src/multi_object_tracker_node.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/perception/multi_object_tracker/src/multi_object_tracker_node.cpp b/perception/multi_object_tracker/src/multi_object_tracker_node.cpp index 198d9e2238566..45955ac983dde 100644 --- a/perception/multi_object_tracker/src/multi_object_tracker_node.cpp +++ b/perception/multi_object_tracker/src/multi_object_tracker_node.cpp @@ -350,10 +350,10 @@ void MultiObjectTracker::publish(const rclcpp::Time & time) const debugger_->endPublishTime(this->now(), time); if (debugger_->shouldPublishTentativeObjects()) { - TrackedObjects tentative_objects_msg; - tentative_objects_msg.header.frame_id = world_frame_id_; - processor_->getTentativeObjects(time, tentative_objects_msg); - debugger_->publishTentativeObjects(tentative_objects_msg); + TrackedObjects tentative_output_msg; + tentative_output_msg.header.frame_id = world_frame_id_; + processor_->getTentativeObjects(time, tentative_output_msg); + debugger_->publishTentativeObjects(tentative_output_msg); } debugger_->publishObjectsMarkers(); } From 2e76976ebf03cc13dc2a9f76dc17114ecc4bf98b Mon Sep 17 00:00:00 2001 From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Date: Wed, 10 Jul 2024 19:18:30 +0900 Subject: [PATCH 34/70] fix(static_obstacle_avoidance): don't automatically avoid ambiguous vehicle (#7851) * fix(static_obstacle_avoidance): don't automatically avoid ambiguous vehicle Signed-off-by: satoshi-ota * chore(schema): update schema Signed-off-by: satoshi-ota --------- Signed-off-by: satoshi-ota --- .../src/manager.cpp | 8 ++-- .../static_obstacle_avoidance.param.yaml | 10 +++- .../data_structs.hpp | 5 +- .../helper.hpp | 47 +++++++++++++++++-- .../parameter_helper.hpp | 8 ++-- .../static_obstacle_avoidance.schema.json | 31 ++++++++---- .../src/manager.cpp | 5 +- .../src/utils.cpp | 23 +++------ 8 files changed, 95 insertions(+), 42 deletions(-) diff --git a/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/manager.cpp b/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/manager.cpp index 02b90186d9b2f..8096d2944ee2b 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/manager.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/manager.cpp @@ -137,9 +137,11 @@ void AvoidanceByLaneChangeModuleManager::init(rclcpp::Node * node) { const std::string ns = "avoidance.target_filtering.avoidance_for_ambiguous_vehicle."; - p.enable_avoidance_for_ambiguous_vehicle = getOrDeclareParameter(*node, ns + "enable"); - p.closest_distance_to_wait_and_see_for_ambiguous_vehicle = - getOrDeclareParameter(*node, ns + "closest_distance_to_wait_and_see"); + p.policy_ambiguous_vehicle = getOrDeclareParameter(*node, ns + "policy"); + p.wait_and_see_target_behaviors = + getOrDeclareParameter>(*node, ns + "wait_and_see.target_behaviors"); + p.wait_and_see_th_closest_distance = + getOrDeclareParameter(*node, ns + "wait_and_see.th_closest_distance"); p.time_threshold_for_ambiguous_vehicle = getOrDeclareParameter(*node, ns + "condition.th_stopped_time"); p.distance_threshold_for_ambiguous_vehicle = diff --git a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/config/static_obstacle_avoidance.param.yaml b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/config/static_obstacle_avoidance.param.yaml index 3087ccc93934b..6bd5e0faf1938 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/config/static_obstacle_avoidance.param.yaml +++ b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/config/static_obstacle_avoidance.param.yaml @@ -138,8 +138,11 @@ # params for avoidance of vehicle type objects that are ambiguous as to whether they are parked. avoidance_for_ambiguous_vehicle: - enable: true # [-] - closest_distance_to_wait_and_see: 10.0 # [m] + # policy for ego behavior for ambiguous vehicle. + # "auto" : generate candidate path. if RTC is running as AUTO mode, the ego avoids it automatically. + # "manual" : generate candidate path and wait operator approval even if RTC is running as AUTO mode. + # "ignore" : never avoid it. + policy: "auto" # [-] condition: th_stopped_time: 3.0 # [s] th_moving_distance: 1.0 # [m] @@ -149,6 +152,9 @@ crosswalk: front_distance: 30.0 # [m] behind_distance: 30.0 # [m] + wait_and_see: + target_behaviors: ["MERGING", "DEVIATING"] # [-] + th_closest_distance: 10.0 # [m] # params for filtering objects that are in intersection intersection: diff --git a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/data_structs.hpp b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/data_structs.hpp index 49599004e0952..079928a58bd9f 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/data_structs.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/data_structs.hpp @@ -108,7 +108,7 @@ struct AvoidanceParameters bool enable_cancel_maneuver{false}; // enable avoidance for all parking vehicle - bool enable_avoidance_for_ambiguous_vehicle{false}; + std::string policy_ambiguous_vehicle{"ignore"}; // enable yield maneuver. bool enable_yield_maneuver{false}; @@ -192,7 +192,8 @@ struct AvoidanceParameters double object_check_min_road_shoulder_width{0.0}; // force avoidance - double closest_distance_to_wait_and_see_for_ambiguous_vehicle{0.0}; + std::vector wait_and_see_target_behaviors{"NONE", "MERGING", "DEVIATING"}; + double wait_and_see_th_closest_distance{0.0}; double time_threshold_for_ambiguous_vehicle{0.0}; double distance_threshold_for_ambiguous_vehicle{0.0}; diff --git a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/helper.hpp b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/helper.hpp index 8c46affbc64e3..bfeb942c82be3 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/helper.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/helper.hpp @@ -23,6 +23,7 @@ #include #include #include +#include #include #include @@ -325,10 +326,27 @@ class AvoidanceHelper const auto object = objects.front(); + // if the object is NOT ambiguous, this module doesn't wait operator approval if RTC is running + // as AUTO mode. if (!object.is_ambiguous) { return true; } + // check only front objects. + if (object.longitudinal < 0.0) { + return true; + } + + // if the policy is "manual", this module generates candidate path and waits approval. + if (parameters_->policy_ambiguous_vehicle == "manual") { + return false; + } + + // don't delay avoidance start position if it's not MERGING or DEVIATING vehicle. + if (!isWaitAndSeeTarget(object)) { + return true; + } + if (!object.avoid_margin.has_value()) { return true; } @@ -341,9 +359,32 @@ class AvoidanceHelper const auto constant_distance = getFrontConstantDistance(object); const auto avoidance_distance = getMinAvoidanceDistance(desire_shift_length); - return object.longitudinal < - prepare_distance + constant_distance + avoidance_distance + - parameters_->closest_distance_to_wait_and_see_for_ambiguous_vehicle; + return object.longitudinal < prepare_distance + constant_distance + avoidance_distance + + parameters_->wait_and_see_th_closest_distance; + } + + bool isWaitAndSeeTarget(const ObjectData & object) const + { + const auto & behaviors = parameters_->wait_and_see_target_behaviors; + if (object.behavior == ObjectData::Behavior::MERGING) { + return std::any_of(behaviors.begin(), behaviors.end(), [](const std::string & behavior) { + return behavior == "MERGING"; + }); + } + + if (object.behavior == ObjectData::Behavior::DEVIATING) { + return std::any_of(behaviors.begin(), behaviors.end(), [](const std::string & behavior) { + return behavior == "DEVIATING"; + }); + } + + if (object.behavior == ObjectData::Behavior::NONE) { + return std::any_of(behaviors.begin(), behaviors.end(), [](const std::string & behavior) { + return behavior == "NONE"; + }); + } + + return false; } static bool isAbsolutelyNotAvoidable(const ObjectData & object) diff --git a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/parameter_helper.hpp b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/parameter_helper.hpp index 84cf7c4e33d26..0cea2a4e633c3 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/parameter_helper.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/parameter_helper.hpp @@ -137,9 +137,11 @@ AvoidanceParameters getParameter(rclcpp::Node * node) { const std::string ns = "avoidance.target_filtering.avoidance_for_ambiguous_vehicle."; - p.enable_avoidance_for_ambiguous_vehicle = getOrDeclareParameter(*node, ns + "enable"); - p.closest_distance_to_wait_and_see_for_ambiguous_vehicle = - getOrDeclareParameter(*node, ns + "closest_distance_to_wait_and_see"); + p.policy_ambiguous_vehicle = getOrDeclareParameter(*node, ns + "policy"); + p.wait_and_see_target_behaviors = + getOrDeclareParameter>(*node, ns + "wait_and_see.target_behaviors"); + p.wait_and_see_th_closest_distance = + getOrDeclareParameter(*node, ns + "wait_and_see.th_closest_distance"); p.time_threshold_for_ambiguous_vehicle = getOrDeclareParameter(*node, ns + "condition.th_stopped_time"); p.distance_threshold_for_ambiguous_vehicle = diff --git a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/schema/static_obstacle_avoidance.schema.json b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/schema/static_obstacle_avoidance.schema.json index db3215fa8d238..246b96ec5440e 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/schema/static_obstacle_avoidance.schema.json +++ b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/schema/static_obstacle_avoidance.schema.json @@ -716,9 +716,10 @@ "avoidance_for_ambiguous_vehicle": { "type": "object", "properties": { - "enable": { - "type": "boolean", - "description": "Enable avoidance maneuver for ambiguous vehicles.", + "policy": { + "type": "string", + "enum": ["auto", "manual", "ignore"], + "description": "Ego behavior policy for ambiguous vehicle.", "default": "true" }, "closest_distance_to_wait_and_see": { @@ -778,14 +779,26 @@ }, "required": ["traffic_light", "crosswalk"], "additionalProperties": false + }, + "wait_and_see": { + "type": "object", + "properties": { + "target_behaviors": { + "type": "array", + "default": ["MERGING", "DEVIATING"], + "description": "This module doesn't avoid these behaviors vehicle until it gets closer than threshold." + }, + "th_closest_distance": { + "type": "number", + "default": 10.0, + "description": "Threshold to check whether the ego gets close enough the ambiguous vehicle." + } + }, + "required": ["target_behaviors", "th_closest_distance"], + "additionalProperties": false } }, - "required": [ - "enable", - "closest_distance_to_wait_and_see", - "condition", - "ignore_area" - ], + "required": ["policy", "condition", "ignore_area", "wait_and_see"], "additionalProperties": false }, "intersection": { diff --git a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/manager.cpp b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/manager.cpp index 2d02f33e19870..c5f338e91acbd 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/manager.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/manager.cpp @@ -132,10 +132,9 @@ void StaticObstacleAvoidanceModuleManager::updateModuleParams( { const std::string ns = "avoidance.avoidance.lateral.avoidance_for_ambiguous_vehicle."; - updateParam(parameters, ns + "enable", p->enable_avoidance_for_ambiguous_vehicle); + updateParam(parameters, ns + "policy", p->policy_ambiguous_vehicle); updateParam( - parameters, ns + "closest_distance_to_wait_and_see", - p->closest_distance_to_wait_and_see_for_ambiguous_vehicle); + parameters, ns + "wait_and_see.th_closest_distance", p->wait_and_see_th_closest_distance); updateParam( parameters, ns + "condition.th_stopped_time", p->time_threshold_for_ambiguous_vehicle); updateParam( diff --git a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/utils.cpp b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/utils.cpp index ddc544553f30e..75eb6ab0fb62e 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/utils.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/utils.cpp @@ -880,7 +880,7 @@ bool isSatisfiedWithVehicleCondition( // from here, filtering for ambiguous vehicle. - if (!parameters->enable_avoidance_for_ambiguous_vehicle) { + if (parameters->policy_ambiguous_vehicle == "ignore") { object.info = ObjectInfo::AMBIGUOUS_STOPPED_VEHICLE; return false; } @@ -889,6 +889,7 @@ bool isSatisfiedWithVehicleCondition( object.stop_time > parameters->time_threshold_for_ambiguous_vehicle; if (!stop_time_longer_than_threshold) { object.info = ObjectInfo::AMBIGUOUS_STOPPED_VEHICLE; + object.is_ambiguous = false; return false; } @@ -897,6 +898,7 @@ bool isSatisfiedWithVehicleCondition( parameters->distance_threshold_for_ambiguous_vehicle; if (is_moving_distance_longer_than_threshold) { object.info = ObjectInfo::AMBIGUOUS_STOPPED_VEHICLE; + object.is_ambiguous = false; return false; } @@ -907,22 +909,9 @@ bool isSatisfiedWithVehicleCondition( return true; } } else { - if (object.behavior == ObjectData::Behavior::MERGING) { - object.info = ObjectInfo::AMBIGUOUS_STOPPED_VEHICLE; - object.is_ambiguous = true; - return true; - } - - if (object.behavior == ObjectData::Behavior::DEVIATING) { - object.info = ObjectInfo::AMBIGUOUS_STOPPED_VEHICLE; - object.is_ambiguous = true; - return true; - } - - if (object.behavior == ObjectData::Behavior::NONE) { - object.is_ambiguous = false; - return true; - } + object.info = ObjectInfo::AMBIGUOUS_STOPPED_VEHICLE; + object.is_ambiguous = true; + return true; } object.info = ObjectInfo::IS_NOT_PARKING_OBJECT; From d45a7e595ea26b5b4e94f9e18fdf3e4af1cea0ee Mon Sep 17 00:00:00 2001 From: kobayu858 <129580202+kobayu858@users.noreply.github.com> Date: Wed, 10 Jul 2024 20:31:22 +0900 Subject: [PATCH 35/70] fix(lidar_apollo_segmentation_tvm): fix shadowVariable (#7938) fix:shadowVariable Signed-off-by: kobayu858 --- .../src/lidar_apollo_segmentation_tvm.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) 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 c83e4836c1dd0..32211bf84efec 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 @@ -202,10 +202,10 @@ std::shared_ptr ApolloLidarSegmentation::detec output->header = input.header; // move down pointcloud z_offset in z axis - for (std::size_t i = 0; i < output->feature_objects.size(); i++) { - sensor_msgs::msg::PointCloud2 transformed_cloud; - transformCloud(output->feature_objects.at(i).feature.cluster, transformed_cloud, -z_offset_); - output->feature_objects.at(i).feature.cluster = transformed_cloud; + for (auto & feature_object : output->feature_objects) { + sensor_msgs::msg::PointCloud2 updated_cloud; + transformCloud(feature_object.feature.cluster, updated_cloud, -z_offset_); + feature_object.feature.cluster = updated_cloud; } return output; From 4cec64501cec5f60b7838eed6fb6c47f17573187 Mon Sep 17 00:00:00 2001 From: badai nguyen <94814556+badai-nguyen@users.noreply.github.com> Date: Thu, 11 Jul 2024 00:11:18 +0900 Subject: [PATCH 36/70] test(euclidean_cluster): add unit tests (#7735) * chore(euclidean_cluster): add unit tests Signed-off-by: badai-nguyen * fix: pre-commit Signed-off-by: badai-nguyen * fix: namespace update Signed-off-by: badai-nguyen --------- Signed-off-by: badai-nguyen --- perception/euclidean_cluster/CMakeLists.txt | 8 + perception/euclidean_cluster/package.xml | 3 + ...est_voxel_grid_based_euclidean_cluster.cpp | 173 ++++++++++++++++++ 3 files changed, 184 insertions(+) create mode 100644 perception/euclidean_cluster/test/test_voxel_grid_based_euclidean_cluster.cpp diff --git a/perception/euclidean_cluster/CMakeLists.txt b/perception/euclidean_cluster/CMakeLists.txt index dacbcf460a14a..d27390ae7707a 100644 --- a/perception/euclidean_cluster/CMakeLists.txt +++ b/perception/euclidean_cluster/CMakeLists.txt @@ -55,7 +55,15 @@ rclcpp_components_register_node(${PROJECT_NAME}_voxel_grid_node_core EXECUTABLE voxel_grid_based_euclidean_cluster_node ) +if(BUILD_TESTING) + find_package(ament_cmake_gtest REQUIRED) + ament_auto_add_gtest(test_voxel_grid_based_euclidean_cluster_fusion + test/test_voxel_grid_based_euclidean_cluster.cpp + ) +endif() + ament_auto_package(INSTALL_TO_SHARE launch config + test ) diff --git a/perception/euclidean_cluster/package.xml b/perception/euclidean_cluster/package.xml index 2644786ead061..45b5d05031c72 100644 --- a/perception/euclidean_cluster/package.xml +++ b/perception/euclidean_cluster/package.xml @@ -12,6 +12,7 @@ autoware_cmake autoware_perception_msgs + autoware_point_types autoware_universe_utils geometry_msgs libpcl-all-dev @@ -21,6 +22,8 @@ sensor_msgs tier4_perception_msgs + ament_cmake_gtest + ament_lint_auto autoware_lint_common diff --git a/perception/euclidean_cluster/test/test_voxel_grid_based_euclidean_cluster.cpp b/perception/euclidean_cluster/test/test_voxel_grid_based_euclidean_cluster.cpp new file mode 100644 index 0000000000000..7d2edbe0de0d7 --- /dev/null +++ b/perception/euclidean_cluster/test/test_voxel_grid_based_euclidean_cluster.cpp @@ -0,0 +1,173 @@ +// 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 "autoware/euclidean_cluster/voxel_grid_based_euclidean_cluster.hpp" + +#include +#include + +#include +#include +#include + +#include + +using autoware_point_types::PointXYZI; +void setPointCloud2Fields(sensor_msgs::msg::PointCloud2 & pointcloud) +{ + pointcloud.fields.resize(4); + pointcloud.fields[0].name = "x"; + pointcloud.fields[1].name = "y"; + pointcloud.fields[2].name = "z"; + pointcloud.fields[3].name = "intensity"; + pointcloud.fields[0].offset = 0; + pointcloud.fields[1].offset = 4; + pointcloud.fields[2].offset = 8; + pointcloud.fields[3].offset = 12; + pointcloud.fields[0].datatype = sensor_msgs::msg::PointField::FLOAT32; + pointcloud.fields[1].datatype = sensor_msgs::msg::PointField::FLOAT32; + pointcloud.fields[2].datatype = sensor_msgs::msg::PointField::FLOAT32; + pointcloud.fields[3].datatype = sensor_msgs::msg::PointField::FLOAT32; + pointcloud.fields[0].count = 1; + pointcloud.fields[1].count = 1; + pointcloud.fields[2].count = 1; + pointcloud.fields[3].count = 1; + pointcloud.height = 1; + pointcloud.point_step = 16; + pointcloud.is_bigendian = false; + pointcloud.is_dense = true; + pointcloud.header.frame_id = "dummy_frame_id"; + pointcloud.header.stamp.sec = 0; + pointcloud.header.stamp.nanosec = 0; +} + +sensor_msgs::msg::PointCloud2 generateClusterWithinVoxel(const int nb_points) +{ + sensor_msgs::msg::PointCloud2 pointcloud; + setPointCloud2Fields(pointcloud); + pointcloud.data.resize(nb_points * pointcloud.point_step); + + // generate one cluster with specified number of points within 1 voxel + for (int i = 0; i < nb_points; ++i) { + PointXYZI point; + point.x = std::experimental::randint(0, 30) / 100.0; // point.x within 0.0 to 0.3 + point.y = std::experimental::randint(0, 30) / 100.0; // point.y within 0.0 to 0.3 + point.z = std::experimental::randint(0, 30) / 1.0; + point.intensity = 0.0; + memcpy(&pointcloud.data[i * pointcloud.point_step], &point, pointcloud.point_step); + } + pointcloud.width = nb_points; + pointcloud.row_step = pointcloud.point_step * nb_points; + return pointcloud; +} + +// Test case 1: Test case when the input pointcloud has only one cluster with points number equal to +// max_cluster_size +TEST(VoxelGridBasedEuclideanClusterTest, testcase1) +{ + int nb_generated_points = 100; + sensor_msgs::msg::PointCloud2 pointcloud = generateClusterWithinVoxel(nb_generated_points); + + const sensor_msgs::msg::PointCloud2::ConstSharedPtr pointcloud_msg = + std::make_shared(pointcloud); + tier4_perception_msgs::msg::DetectedObjectsWithFeature output; + std::shared_ptr cluster_; + float tolerance = 0.7; + float voxel_leaf_size = 0.3; + int min_points_number_per_voxel = 1; + int min_cluster_size = 1; + int max_cluster_size = 100; + bool use_height = false; + cluster_ = std::make_shared( + use_height, min_cluster_size, max_cluster_size, tolerance, voxel_leaf_size, + min_points_number_per_voxel); + if (cluster_->cluster(pointcloud_msg, output)) { + std::cout << "cluster success" << std::endl; + } else { + std::cout << "cluster failed" << std::endl; + } + std::cout << "number of output clusters " << output.feature_objects.size() << std::endl; + std::cout << "number points of first cluster: " << output.feature_objects[0].feature.cluster.width + << std::endl; + // the output clusters should has only one cluster with nb_generated_points points + EXPECT_EQ(output.feature_objects.size(), 1); + EXPECT_EQ(output.feature_objects[0].feature.cluster.width, nb_generated_points); +} + +// Test case 2: Test case when the input pointcloud has only one cluster with points number less +// than min_cluster_size +TEST(VoxelGridBasedEuclideanClusterTest, testcase2) +{ + int nb_generated_points = 1; + + sensor_msgs::msg::PointCloud2 pointcloud = generateClusterWithinVoxel(nb_generated_points); + + const sensor_msgs::msg::PointCloud2::ConstSharedPtr pointcloud_msg = + std::make_shared(pointcloud); + tier4_perception_msgs::msg::DetectedObjectsWithFeature output; + std::shared_ptr cluster_; + float tolerance = 0.7; + float voxel_leaf_size = 0.3; + int min_points_number_per_voxel = 1; + int min_cluster_size = 2; + int max_cluster_size = 100; + bool use_height = false; + cluster_ = std::make_shared( + use_height, min_cluster_size, max_cluster_size, tolerance, voxel_leaf_size, + min_points_number_per_voxel); + if (cluster_->cluster(pointcloud_msg, output)) { + std::cout << "cluster success" << std::endl; + } else { + std::cout << "cluster failed" << std::endl; + } + std::cout << "number of output clusters " << output.feature_objects.size() << std::endl; + // the output clusters should be empty + EXPECT_EQ(output.feature_objects.size(), 0); +} + +// Test case 3: Test case when the input pointcloud has two clusters with points number greater to +// max_cluster_size +TEST(VoxelGridBasedEuclideanClusterTest, testcase3) +{ + int nb_generated_points = 100; + sensor_msgs::msg::PointCloud2 pointcloud = generateClusterWithinVoxel(nb_generated_points); + + const sensor_msgs::msg::PointCloud2::ConstSharedPtr pointcloud_msg = + std::make_shared(pointcloud); + tier4_perception_msgs::msg::DetectedObjectsWithFeature output; + std::shared_ptr cluster_; + float tolerance = 0.7; + float voxel_leaf_size = 0.3; + int min_points_number_per_voxel = 1; + int min_cluster_size = 1; + int max_cluster_size = 99; // max_cluster_size is less than nb_generated_points + bool use_height = false; + cluster_ = std::make_shared( + use_height, min_cluster_size, max_cluster_size, tolerance, voxel_leaf_size, + min_points_number_per_voxel); + if (cluster_->cluster(pointcloud_msg, output)) { + std::cout << "cluster success" << std::endl; + } else { + std::cout << "cluster failed" << std::endl; + } + std::cout << "number of output clusters " << output.feature_objects.size() << std::endl; + // the output clusters should be emtpy + EXPECT_EQ(output.feature_objects.size(), 0); +} + +int main(int argc, char ** argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} From 1a8f6f327d7e3606c284c95e124f0f129980659c Mon Sep 17 00:00:00 2001 From: kobayu858 <129580202+kobayu858@users.noreply.github.com> Date: Thu, 11 Jul 2024 09:48:50 +0900 Subject: [PATCH 37/70] fix(autoware_radar_object_tracker): fix shadowVariable (#7945) fix:shadowVariable Signed-off-by: kobayu858 --- .../src/association/ssp/successive_shortest_path.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/perception/autoware_radar_object_tracker/src/association/ssp/successive_shortest_path.cpp b/perception/autoware_radar_object_tracker/src/association/ssp/successive_shortest_path.cpp index 7877fe5451d8d..d18e4f140e2ed 100644 --- a/perception/autoware_radar_object_tracker/src/association/ssp/successive_shortest_path.cpp +++ b/perception/autoware_radar_object_tracker/src/association/ssp/successive_shortest_path.cpp @@ -328,12 +328,12 @@ void SSP::maximizeLinearAssignment( #ifndef NDEBUG // Check if the potentials are feasible potentials - for (int v = 0; v < n_nodes; ++v) { - for (auto it_incident_edge = adjacency_list.at(v).cbegin(); - it_incident_edge != adjacency_list.at(v).cend(); ++it_incident_edge) { + for (int w = 0; w < n_nodes; ++w) { + for (auto it_incident_edge = adjacency_list.at(w).cbegin(); + it_incident_edge != adjacency_list.at(w).cend(); ++it_incident_edge) { if (it_incident_edge->capacity > 0) { double reduced_cost = - it_incident_edge->cost + potentials.at(v) - potentials.at(it_incident_edge->dst); + it_incident_edge->cost + potentials.at(w) - potentials.at(it_incident_edge->dst); assert(reduced_cost >= 0); } } From 0f79b65f56385f0dc6cfc3281e2dae87f8cfa1dc Mon Sep 17 00:00:00 2001 From: kobayu858 <129580202+kobayu858@users.noreply.github.com> Date: Thu, 11 Jul 2024 09:49:30 +0900 Subject: [PATCH 38/70] fix(tensorrt_yolox): fix shadowVariable (#7947) fix:shadowVariable Signed-off-by: kobayu858 --- perception/tensorrt_yolox/src/tensorrt_yolox.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/perception/tensorrt_yolox/src/tensorrt_yolox.cpp b/perception/tensorrt_yolox/src/tensorrt_yolox.cpp index 94c795cfc2403..bb6975d7a1f21 100644 --- a/perception/tensorrt_yolox/src/tensorrt_yolox.cpp +++ b/perception/tensorrt_yolox/src/tensorrt_yolox.cpp @@ -130,12 +130,12 @@ std::vector get_seg_colormap(const std::string & filen cmap.name = name; colormapString.erase(0, npos + 1); while (!colormapString.empty()) { - size_t npos = colormapString.find_first_of(','); - if (npos != std::string::npos) { - substr = colormapString.substr(0, npos); + size_t inner_npos = colormapString.find_first_of(','); + if (inner_npos != std::string::npos) { + substr = colormapString.substr(0, inner_npos); unsigned char c = (unsigned char)std::stoi(trim(substr)); cmap.color.push_back(c); - colormapString.erase(0, npos + 1); + colormapString.erase(0, inner_npos + 1); } else { unsigned char c = (unsigned char)std::stoi(trim(colormapString)); cmap.color.push_back(c); From f388b549c50b6c8dd61866a0ccb31c0e824e2bf5 Mon Sep 17 00:00:00 2001 From: kobayu858 <129580202+kobayu858@users.noreply.github.com> Date: Thu, 11 Jul 2024 09:50:16 +0900 Subject: [PATCH 39/70] fix(tracking_object_merger): fix shadowVariable (#7948) fix:shadowVariable Signed-off-by: kobayu858 --- .../src/association/solver/successive_shortest_path.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/perception/tracking_object_merger/src/association/solver/successive_shortest_path.cpp b/perception/tracking_object_merger/src/association/solver/successive_shortest_path.cpp index 7be715b24bd5f..7d792c157d356 100644 --- a/perception/tracking_object_merger/src/association/solver/successive_shortest_path.cpp +++ b/perception/tracking_object_merger/src/association/solver/successive_shortest_path.cpp @@ -328,12 +328,12 @@ void SSP::maximizeLinearAssignment( #ifndef NDEBUG // Check if the potentials are feasible potentials - for (int v = 0; v < n_nodes; ++v) { - for (auto it_incident_edge = adjacency_list.at(v).cbegin(); - it_incident_edge != adjacency_list.at(v).cend(); ++it_incident_edge) { + for (int w = 0; w < n_nodes; ++w) { + for (auto it_incident_edge = adjacency_list.at(w).cbegin(); + it_incident_edge != adjacency_list.at(w).cend(); ++it_incident_edge) { if (it_incident_edge->capacity > 0) { double reduced_cost = - it_incident_edge->cost + potentials.at(v) - potentials.at(it_incident_edge->dst); + it_incident_edge->cost + potentials.at(w) - potentials.at(it_incident_edge->dst); assert(reduced_cost >= 0); } } From 1c7e46288e5116c2b67e6a7ff2e93a35ea3b9969 Mon Sep 17 00:00:00 2001 From: Amadeusz Szymko Date: Thu, 11 Jul 2024 11:09:24 +0900 Subject: [PATCH 40/70] fix(lidar_transfusion): install lib (#7953) Signed-off-by: Amadeusz Szymko --- perception/lidar_transfusion/CMakeLists.txt | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/perception/lidar_transfusion/CMakeLists.txt b/perception/lidar_transfusion/CMakeLists.txt index 677a835e5b04e..15c89beb15d3e 100644 --- a/perception/lidar_transfusion/CMakeLists.txt +++ b/perception/lidar_transfusion/CMakeLists.txt @@ -134,6 +134,11 @@ if(TRT_AVAIL AND CUDA_AVAIL AND CUDNN_AVAIL) EXECUTABLE lidar_transfusion_node ) + install( + TARGETS transfusion_cuda_lib + DESTINATION lib + ) + if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) ament_lint_auto_find_test_dependencies() From 4b26093ee6ffae324a06dbe9d7c39b3b4389d2f2 Mon Sep 17 00:00:00 2001 From: SHtokuda <165623782+shtokuda@users.noreply.github.com> Date: Thu, 11 Jul 2024 11:28:18 +0900 Subject: [PATCH 41/70] fix(autoware_external_cmd_converter): fix check_topic_state (#7921) * fix(autoware_external_cmd_converter): fix check_topic_state Signed-off-by: shtokuda * style(pre-commit): autofix * Update vehicle/autoware_external_cmd_converter/src/node.cpp Co-authored-by: Shumpei Wakabayashi <42209144+shmpwk@users.noreply.github.com> * style(pre-commit): autofix --------- Signed-off-by: shtokuda Co-authored-by: shtokuda Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> Co-authored-by: Shumpei Wakabayashi <42209144+shmpwk@users.noreply.github.com> --- .../autoware_external_cmd_converter/src/node.cpp | 13 +++++++++++-- 1 file changed, 11 insertions(+), 2 deletions(-) diff --git a/vehicle/autoware_external_cmd_converter/src/node.cpp b/vehicle/autoware_external_cmd_converter/src/node.cpp index cbff8ab5ef2ce..22fc1c319a4e3 100644 --- a/vehicle/autoware_external_cmd_converter/src/node.cpp +++ b/vehicle/autoware_external_cmd_converter/src/node.cpp @@ -179,6 +179,9 @@ void ExternalCmdConverterNode::check_topic_status( { using diagnostic_msgs::msg::DiagnosticStatus; DiagnosticStatus status; + + current_gate_mode_ = gate_mode_sub_.takeData(); + if (!check_emergency_stop_topic_timeout()) { status.level = DiagnosticStatus::ERROR; status.message = "emergency stop topic is timeout"; @@ -195,6 +198,14 @@ void ExternalCmdConverterNode::check_topic_status( bool ExternalCmdConverterNode::check_emergency_stop_topic_timeout() { + if (!current_gate_mode_) { + return true; + } + + if (current_gate_mode_->data == tier4_control_msgs::msg::GateMode::AUTO) { + latest_emergency_stop_heartbeat_received_time_ = nullptr; + } + if (!latest_emergency_stop_heartbeat_received_time_) { return wait_for_first_topic_; } @@ -205,8 +216,6 @@ bool ExternalCmdConverterNode::check_emergency_stop_topic_timeout() bool ExternalCmdConverterNode::check_remote_topic_rate() { - current_gate_mode_ = gate_mode_sub_.takeData(); - if (!current_gate_mode_) { return true; } From 53bd126a803feb77f003644f436b6f2ae042ab83 Mon Sep 17 00:00:00 2001 From: Mitsuhiro Sakamoto <50359861+mitukou1109@users.noreply.github.com> Date: Thu, 11 Jul 2024 13:08:28 +0900 Subject: [PATCH 42/70] feat(obstacle_cruise_planner): prevent chattering when using point cloud (#7861) * prevent chattering of stop planning Signed-off-by: mitukou1109 * Update planning/autoware_obstacle_cruise_planner/src/node.cpp Co-authored-by: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> * fix stop position oscillation Signed-off-by: mitukou1109 --------- Signed-off-by: mitukou1109 Co-authored-by: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> --- .../autoware/obstacle_cruise_planner/node.hpp | 13 +++-- .../src/node.cpp | 47 +++++++++++++++---- 2 files changed, 46 insertions(+), 14 deletions(-) diff --git a/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/node.hpp b/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/node.hpp index b96605f57b943..d4094ff08c3f0 100644 --- a/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/node.hpp +++ b/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/node.hpp @@ -192,10 +192,13 @@ class ObstacleCruisePlannerNode : public rclcpp::Node // planner std::unique_ptr planner_ptr_{nullptr}; - // previous obstacles - std::vector prev_stop_obstacles_; - std::vector prev_cruise_obstacles_; - std::vector prev_slow_down_obstacles_; + // previous PredictedObject-based obstacles + std::vector prev_stop_object_obstacles_; + std::vector prev_cruise_object_obstacles_; + std::vector prev_slow_down_object_obstacles_; + + // PointCloud-based stop obstacle history + std::vector stop_pc_obstacle_history_; // behavior determination parameter struct BehaviorDeterminationParam @@ -303,7 +306,7 @@ class ObstacleCruisePlannerNode : public rclcpp::Node bool enable_slow_down_planning_{false}; bool use_pointcloud_{false}; - std::vector prev_closest_stop_obstacles_{}; + std::vector prev_closest_stop_object_obstacles_{}; std::unique_ptr logger_configure_; diff --git a/planning/autoware_obstacle_cruise_planner/src/node.cpp b/planning/autoware_obstacle_cruise_planner/src/node.cpp index ec0b70e4e1b2d..2734ee8866c6e 100644 --- a/planning/autoware_obstacle_cruise_planner/src/node.cpp +++ b/planning/autoware_obstacle_cruise_planner/src/node.cpp @@ -597,7 +597,7 @@ void ObstacleCruisePlannerNode::onTrajectory(const Trajectory::ConstSharedPtr ms concatenate(cruise_obstacles, cruise_object_obstacles); concatenate(slow_down_obstacles, slow_down_object_obstacles); } - if (pointcloud_ptr && !pointcloud_ptr->data.empty()) { + if (pointcloud_ptr) { const auto target_obstacles = convertToObstacles(ego_odom, *pointcloud_ptr, traj_points, msg->header); @@ -822,7 +822,7 @@ std::vector ObstacleCruisePlannerNode::convertToObstacles( transform_stamped = std::nullopt; } - if (transform_stamped) { + if (!pointcloud.data.empty() && transform_stamped) { // 1. transform pointcloud PointCloud::Ptr pointcloud_ptr(new PointCloud); pcl::fromROSMsg(pointcloud, *pointcloud_ptr); @@ -1033,9 +1033,9 @@ ObstacleCruisePlannerNode::determineEgoBehaviorAgainstPredictedObjectObstacles( checkConsistency(objects.header.stamp, objects, stop_obstacles); // update previous obstacles - prev_stop_obstacles_ = stop_obstacles; - prev_cruise_obstacles_ = cruise_obstacles; - prev_slow_down_obstacles_ = slow_down_obstacles; + prev_stop_object_obstacles_ = stop_obstacles; + prev_cruise_object_obstacles_ = cruise_obstacles; + prev_slow_down_object_obstacles_ = slow_down_obstacles; const double calculation_time = stop_watch_.toc(__func__); RCLCPP_INFO_EXPRESSION( @@ -1052,6 +1052,8 @@ ObstacleCruisePlannerNode::determineEgoBehaviorAgainstPointCloudObstacles( { stop_watch_.tic(__func__); + const auto & p = behavior_determination_param_; + // calculated decimated trajectory points and trajectory polygon const auto decimated_traj_points = decimateTrajectoryPoints(odometry, traj_points); const auto decimated_traj_polys = @@ -1078,6 +1080,32 @@ ObstacleCruisePlannerNode::determineEgoBehaviorAgainstPointCloudObstacles( } } + std::vector past_stop_obstacles; + for (auto itr = stop_pc_obstacle_history_.begin(); itr != stop_pc_obstacle_history_.end();) { + const double elapsed_time = (rclcpp::Time(odometry.header.stamp) - itr->stamp).seconds(); + if (elapsed_time >= p.stop_obstacle_hold_time_threshold) { + itr = stop_pc_obstacle_history_.erase(itr); + continue; + } + + const auto lat_dist_from_obstacle_to_traj = + autoware::motion_utils::calcLateralOffset(traj_points, itr->collision_point); + const auto min_lat_dist_to_traj_poly = + std::abs(lat_dist_from_obstacle_to_traj) - vehicle_info_.vehicle_width_m; + + if (min_lat_dist_to_traj_poly < p.max_lat_margin_for_stop_against_unknown) { + auto stop_obstacle = *itr; + stop_obstacle.dist_to_collide_on_decimated_traj = autoware::motion_utils::calcSignedArcLength( + decimated_traj_points, 0, stop_obstacle.collision_point); + past_stop_obstacles.push_back(stop_obstacle); + } + + ++itr; + } + + concatenate(stop_pc_obstacle_history_, stop_obstacles); + concatenate(stop_obstacles, past_stop_obstacles); + const double calculation_time = stop_watch_.toc(__func__); RCLCPP_INFO_EXPRESSION( rclcpp::get_logger("ObstacleCruisePlanner"), enable_calculation_time_info_, " %s := %f [ms]", @@ -1308,7 +1336,7 @@ ObstacleCruisePlannerNode::createCollisionPointsForInsideCruiseObstacle( // const bool is_prev_obstacle_stop = getObstacleFromUuid(prev_stop_obstacles_, // obstacle.uuid).has_value(); const bool is_prev_obstacle_cruise = - getObstacleFromUuid(prev_cruise_obstacles_, obstacle.uuid).has_value(); + getObstacleFromUuid(prev_cruise_object_obstacles_, obstacle.uuid).has_value(); if (is_prev_obstacle_cruise) { if (obstacle_tangent_vel < p.obstacle_velocity_threshold_from_cruise_to_stop) { @@ -1555,7 +1583,7 @@ std::optional ObstacleCruisePlannerNode::createSlowDownObstacl slow_down_condition_counter_.addCurrentUuid(obstacle.uuid); const bool is_prev_obstacle_slow_down = - getObstacleFromUuid(prev_slow_down_obstacles_, obstacle.uuid).has_value(); + getObstacleFromUuid(prev_slow_down_object_obstacles_, obstacle.uuid).has_value(); if (!enable_slow_down_planning_ || !isSlowDownObstacle(obstacle.classification.label)) { return std::nullopt; @@ -1694,7 +1722,7 @@ void ObstacleCruisePlannerNode::checkConsistency( const rclcpp::Time & current_time, const PredictedObjects & predicted_objects, std::vector & stop_obstacles) { - for (const auto & prev_closest_stop_obstacle : prev_closest_stop_obstacles_) { + for (const auto & prev_closest_stop_obstacle : prev_closest_stop_object_obstacles_) { const auto predicted_object_itr = std::find_if( predicted_objects.objects.begin(), predicted_objects.objects.end(), [&prev_closest_stop_obstacle](const PredictedObject & po) { @@ -1724,7 +1752,8 @@ void ObstacleCruisePlannerNode::checkConsistency( } } - prev_closest_stop_obstacles_ = obstacle_cruise_utils::getClosestStopObstacles(stop_obstacles); + prev_closest_stop_object_obstacles_ = + obstacle_cruise_utils::getClosestStopObstacles(stop_obstacles); } bool ObstacleCruisePlannerNode::isObstacleCrossing( From 82aea7ba727e7307b29e8f8ae4c0f609488089a9 Mon Sep 17 00:00:00 2001 From: Dmitrii Koldaev <39071246+dkoldaev@users.noreply.github.com> Date: Thu, 11 Jul 2024 14:13:54 +0900 Subject: [PATCH 43/70] feat(duplicated_node_checker): add duplicate nodes to ignore (#7959) * feat: add duplicate nodes to ignore Signed-off-by: Dmitrii Koldaev * remove talker Signed-off-by: Dmitrii Koldaev * newline Signed-off-by: Dmitrii Koldaev * commments Signed-off-by: Dmitrii Koldaev * pre-commit and sign Signed-off-by: Dmitrii Koldaev * rviz->rviz2 Signed-off-by: Dmitrii Koldaev --------- Signed-off-by: Dmitrii Koldaev Co-authored-by: Dmitrii Koldaev --- .../config/duplicated_node_checker.param.yaml | 2 ++ .../duplicated_node_checker/duplicated_node_checker_core.hpp | 4 ++++ .../src/duplicated_node_checker_core.cpp | 5 +++++ 3 files changed, 11 insertions(+) diff --git a/system/duplicated_node_checker/config/duplicated_node_checker.param.yaml b/system/duplicated_node_checker/config/duplicated_node_checker.param.yaml index 96dce7eb360e4..412c59e66e2c9 100644 --- a/system/duplicated_node_checker/config/duplicated_node_checker.param.yaml +++ b/system/duplicated_node_checker/config/duplicated_node_checker.param.yaml @@ -2,3 +2,5 @@ ros__parameters: update_rate: 10.0 add_duplicated_node_names_to_msg: true # if true, duplicated node names are added to msg + nodes_to_ignore: # List of nodes to ignore when checking for duplicates + - /rviz2 diff --git a/system/duplicated_node_checker/include/duplicated_node_checker/duplicated_node_checker_core.hpp b/system/duplicated_node_checker/include/duplicated_node_checker/duplicated_node_checker_core.hpp index 9d806754f3c20..66e125de666b7 100644 --- a/system/duplicated_node_checker/include/duplicated_node_checker/duplicated_node_checker_core.hpp +++ b/system/duplicated_node_checker/include/duplicated_node_checker/duplicated_node_checker_core.hpp @@ -33,6 +33,9 @@ class DuplicatedNodeChecker : public rclcpp::Node std::unordered_set unique_names; std::vector identical_names; for (auto name : input_name_lists) { + if (nodes_to_ignore_.count(name) != 0) { + continue; + } if (unique_names.find(name) != unique_names.end()) { identical_names.push_back(name); } else { @@ -49,6 +52,7 @@ class DuplicatedNodeChecker : public rclcpp::Node diagnostic_updater::Updater updater_{this}; rclcpp::TimerBase::SharedPtr timer_; bool add_duplicated_node_names_to_msg_; + std::unordered_set nodes_to_ignore_; }; } // namespace duplicated_node_checker diff --git a/system/duplicated_node_checker/src/duplicated_node_checker_core.cpp b/system/duplicated_node_checker/src/duplicated_node_checker_core.cpp index 384c8fdc46c5f..c9bc07cffde72 100644 --- a/system/duplicated_node_checker/src/duplicated_node_checker_core.cpp +++ b/system/duplicated_node_checker/src/duplicated_node_checker_core.cpp @@ -29,6 +29,11 @@ DuplicatedNodeChecker::DuplicatedNodeChecker(const rclcpp::NodeOptions & node_op { double update_rate = declare_parameter("update_rate"); add_duplicated_node_names_to_msg_ = declare_parameter("add_duplicated_node_names_to_msg"); + + std::vector nodes_to_ignore_vector = + declare_parameter>("nodes_to_ignore"); + nodes_to_ignore_.insert(nodes_to_ignore_vector.begin(), nodes_to_ignore_vector.end()); + updater_.setHardwareID("duplicated_node_checker"); updater_.add("duplicated_node_checker", this, &DuplicatedNodeChecker::produceDiagnostics); From bb2fdcdbfa9b31aeafc572cc25d9dc325a561ea6 Mon Sep 17 00:00:00 2001 From: Maxime CLEMENT <78338830+maxime-clem@users.noreply.github.com> Date: Thu, 11 Jul 2024 15:24:06 +0900 Subject: [PATCH 44/70] perf(dynamic_obstacle_stop): construct rtree nodes in place (#7753) Signed-off-by: Maxime CLEMENT --- .../src/footprint.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/footprint.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/footprint.cpp index 73189e732b312..8f59160a56f2a 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/footprint.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/footprint.cpp @@ -79,7 +79,7 @@ void make_ego_footprint_rtree(EgoData & ego_data, const PlannerParam & params) for (auto i = 0UL; i < ego_data.trajectory_footprints.size(); ++i) { const auto box = boost::geometry::return_envelope( ego_data.trajectory_footprints[i]); - rtree_nodes.push_back(std::make_pair(box, i)); + rtree_nodes.emplace_back(box, i); } ego_data.rtree = Rtree(rtree_nodes); } From 5e8899d4572713939ae9449168a34de87bd709f3 Mon Sep 17 00:00:00 2001 From: Ryuta Kambe Date: Thu, 11 Jul 2024 15:46:57 +0900 Subject: [PATCH 45/70] fix(ci): ignore only-hpp-package in cppcheck-differential (#7961) * fix(ci): ignore only-hpp-package in cppcheck-differential Signed-off-by: veqcc * test Signed-off-by: veqcc * test Signed-off-by: veqcc --------- Signed-off-by: veqcc --- .github/workflows/cppcheck-differential.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/cppcheck-differential.yaml b/.github/workflows/cppcheck-differential.yaml index 63f15cb9da319..dc4fed1084bc0 100644 --- a/.github/workflows/cppcheck-differential.yaml +++ b/.github/workflows/cppcheck-differential.yaml @@ -58,7 +58,7 @@ jobs: run: | filtered_paths="" for dir in ${{ steps.get-full-paths.outputs.full-paths }}; do - if [ -d "$dir" ] && find "$dir" -name "*.cpp" -o -name "*.hpp" | grep -q .; then + if [ -d "$dir" ] && find "$dir" -name "*.cpp" | grep -q .; then filtered_paths="$filtered_paths $dir" fi done From 3e6b5c0fedaad3958800a13c158a05754cc2b184 Mon Sep 17 00:00:00 2001 From: Satoshi Tanaka <16330533+scepter914@users.noreply.github.com> Date: Thu, 11 Jul 2024 16:13:29 +0900 Subject: [PATCH 46/70] chore(perception_utils): delete maintainer (#7899) Signed-off-by: scepter914 --- common/perception_utils/package.xml | 1 - 1 file changed, 1 deletion(-) diff --git a/common/perception_utils/package.xml b/common/perception_utils/package.xml index 9d5fbf40a4e89..a6bfbc42090fe 100644 --- a/common/perception_utils/package.xml +++ b/common/perception_utils/package.xml @@ -4,7 +4,6 @@ perception_utils 0.1.0 The perception_utils package - Satoshi Tanaka Shunsuke Miura Yoshi Ri Apache License 2.0 From 833deea97ed0ae518c9bfd50c6eba54fdac0d8d1 Mon Sep 17 00:00:00 2001 From: Koichi98 <45482193+Koichi98@users.noreply.github.com> Date: Thu, 11 Jul 2024 17:06:12 +0900 Subject: [PATCH 47/70] fix(tensorrt_yolox): fix cstyleCast to reinterpret_cast (#7873) * fix(tensorrt_yolox): fix cstyleCast to reinterpret_cast Signed-off-by: Koichi Imai * style(pre-commit): autofix * fix(tensorrt_yolox): fix cstyleCast to reinterpret_cast Signed-off-by: Koichi Imai * style(pre-commit): autofix --------- Signed-off-by: Koichi Imai Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- perception/tensorrt_yolox/src/tensorrt_yolox.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/perception/tensorrt_yolox/src/tensorrt_yolox.cpp b/perception/tensorrt_yolox/src/tensorrt_yolox.cpp index bb6975d7a1f21..25f762e2a4ec8 100644 --- a/perception/tensorrt_yolox/src/tensorrt_yolox.cpp +++ b/perception/tensorrt_yolox/src/tensorrt_yolox.cpp @@ -1205,8 +1205,8 @@ cv::Mat TrtYoloX::getMaskImageGpu(float * d_prob, nvinfer1::Dims dims, int out_w cv::Mat mask = cv::Mat::zeros(out_h, out_w, CV_8UC1); int index = b * out_w * out_h; argmax_gpu( - (unsigned char *)argmax_buf_d_.get() + index, d_prob, out_w, out_h, width, height, classes, 1, - *stream_); + reinterpret_cast(argmax_buf_d_.get()) + index, d_prob, out_w, out_h, width, + height, classes, 1, *stream_); CHECK_CUDA_ERROR(cudaMemcpyAsync( argmax_buf_h_.get(), argmax_buf_d_.get(), sizeof(unsigned char) * 1 * out_w * out_h, cudaMemcpyDeviceToHost, *stream_)); From 19e194553accc4f659a043ed0a994614e7d668e5 Mon Sep 17 00:00:00 2001 From: kobayu858 <129580202+kobayu858@users.noreply.github.com> Date: Thu, 11 Jul 2024 17:14:26 +0900 Subject: [PATCH 48/70] fix(autoware_behavior_path_planner_common): fix shadowVariable (#7965) fix:shadowVariable Signed-off-by: kobayu858 --- .../autoware_behavior_path_planner_common/src/utils/utils.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/utils.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/utils.cpp index b7955da9a44f1..aa16f1bb23b79 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/utils.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/utils.cpp @@ -1268,8 +1268,8 @@ lanelet::ConstLanelets getCurrentLanesFromPath( const auto & p = planner_data->parameters; std::set lane_ids; - for (const auto & p : path.points) { - for (const auto & id : p.lane_ids) { + for (const auto & point : path.points) { + for (const auto & id : point.lane_ids) { lane_ids.insert(id); } } From 0479f047ef071ba861ea49dd36be1a13c4a78869 Mon Sep 17 00:00:00 2001 From: kobayu858 <129580202+kobayu858@users.noreply.github.com> Date: Thu, 11 Jul 2024 17:15:20 +0900 Subject: [PATCH 49/70] fix(autoware_behavior_path_sampling_planner_module): fix shadowVariable (#7966) * fix:shadowVariable Signed-off-by: kobayu858 * refactor:clang format Signed-off-by: kobayu858 * fix:shadowVariable Signed-off-by: kobayu858 * refactor:CodeSence Signed-off-by: kobayu858 --------- Signed-off-by: kobayu858 --- .../src/manager.cpp | 2 -- .../src/sampling_planner_module.cpp | 4 ++-- 2 files changed, 2 insertions(+), 4 deletions(-) diff --git a/planning/behavior_path_planner/autoware_behavior_path_sampling_planner_module/src/manager.cpp b/planning/behavior_path_planner/autoware_behavior_path_sampling_planner_module/src/manager.cpp index af8a538f894cc..b2b86433002cc 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_sampling_planner_module/src/manager.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_sampling_planner_module/src/manager.cpp @@ -81,8 +81,6 @@ void SamplingPlannerModuleManager::updateModuleParams( auto & p = parameters_; - [[maybe_unused]] const std::string ns = name_ + "."; - { std::string ns{"constraints.hard"}; updateParam(parameters, ns + ".max_curvature", p->max_curvature); diff --git a/planning/behavior_path_planner/autoware_behavior_path_sampling_planner_module/src/sampling_planner_module.cpp b/planning/behavior_path_planner/autoware_behavior_path_sampling_planner_module/src/sampling_planner_module.cpp index 526a761e6957a..3990a2fbf5475 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_sampling_planner_module/src/sampling_planner_module.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_sampling_planner_module/src/sampling_planner_module.cpp @@ -553,8 +553,8 @@ BehaviorModuleOutput SamplingPlannerModule::plan() prepareSamplingParameters(future_state, reference_spline, *internal_params_); auto extension_frenet_paths = autoware::frenet_planner::generatePaths( reference_spline, frenet_reuse_state, extension_sampling_parameters); - for (auto & p : extension_frenet_paths) { - if (!p.points.empty()) frenet_paths.push_back(reused_path.extend(p)); + for (auto & path : extension_frenet_paths) { + if (!path.points.empty()) frenet_paths.push_back(reused_path.extend(path)); } } } else { From 24aa4997c14bb680be85d3220f79784687943073 Mon Sep 17 00:00:00 2001 From: kobayu858 <129580202+kobayu858@users.noreply.github.com> Date: Thu, 11 Jul 2024 17:39:49 +0900 Subject: [PATCH 50/70] fix(autoware_behavior_path_lane_change_module): fix shadowVariable (#7964) fix:shadowVariable Signed-off-by: kobayu858 --- .../src/scene.cpp | 32 +++++++++---------- .../src/utils/utils.cpp | 8 ++--- 2 files changed, 20 insertions(+), 20 deletions(-) diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp index f3e05eca1e92d..8cf67f8cfc66f 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp @@ -1445,7 +1445,7 @@ bool NormalLaneChange::getLaneChangePaths( RCLCPP_DEBUG(logger_, " - sampling num for lat_acc: %lu", sample_lat_acc.size()); for (const auto & lateral_acc : sample_lat_acc) { - const auto debug_print = [&](const auto & s) { + const auto debug_print_lat = [&](const auto & s) { RCLCPP_DEBUG_STREAM( logger_, " - " << s << " : prep_time = " << prepare_duration << ", lon_acc = " << sampled_longitudinal_acc << ", lat_acc = " << lateral_acc); @@ -1467,7 +1467,7 @@ bool NormalLaneChange::getLaneChangePaths( prepare_segment, current_velocity, terminal_lane_changing_velocity); if (lane_changing_length + prepare_length > dist_to_end_of_current_lanes) { - debug_print("Reject: length of lane changing path is longer than length to goal!!"); + debug_print_lat("Reject: length of lane changing path is longer than length to goal!!"); continue; } @@ -1486,7 +1486,7 @@ bool NormalLaneChange::getLaneChangePaths( s_start + lane_changing_length + finish_judge_buffer + backward_buffer + next_lane_change_buffer > s_goal) { - debug_print("Reject: length of lane changing path is longer than length to goal!!"); + debug_print_lat("Reject: length of lane changing path is longer than length to goal!!"); continue; } } @@ -1496,7 +1496,7 @@ bool NormalLaneChange::getLaneChangePaths( initial_lane_changing_velocity, next_lane_change_buffer); if (target_segment.points.empty()) { - debug_print("Reject: target segment is empty!! something wrong..."); + debug_print_lat("Reject: target segment is empty!! something wrong..."); continue; } @@ -1527,7 +1527,7 @@ bool NormalLaneChange::getLaneChangePaths( lane_change_info.terminal_lane_changing_velocity = terminal_lane_changing_velocity; if (!is_valid_start_point) { - debug_print( + debug_print_lat( "Reject: lane changing points are not inside of the target preferred lanes or its " "neighbors"); continue; @@ -1541,7 +1541,7 @@ bool NormalLaneChange::getLaneChangePaths( next_lane_change_buffer); if (target_lane_reference_path.points.empty()) { - debug_print("Reject: target_lane_reference_path is empty!!"); + debug_print_lat("Reject: target_lane_reference_path is empty!!"); continue; } @@ -1553,12 +1553,12 @@ bool NormalLaneChange::getLaneChangePaths( sorted_lane_ids); if (!candidate_path) { - debug_print("Reject: failed to generate candidate path!!"); + debug_print_lat("Reject: failed to generate candidate path!!"); continue; } if (!hasEnoughLength(*candidate_path, current_lanes, target_lanes, direction)) { - debug_print("Reject: invalid candidate path!!"); + debug_print_lat("Reject: invalid candidate path!!"); continue; } @@ -1566,7 +1566,7 @@ bool NormalLaneChange::getLaneChangePaths( lane_change_parameters_->regulate_on_crosswalk && !hasEnoughLengthToCrosswalk(*candidate_path, current_lanes)) { if (getStopTime() < lane_change_parameters_->stop_time_threshold) { - debug_print("Reject: including crosswalk!!"); + debug_print_lat("Reject: including crosswalk!!"); continue; } RCLCPP_INFO_THROTTLE( @@ -1577,7 +1577,7 @@ bool NormalLaneChange::getLaneChangePaths( lane_change_parameters_->regulate_on_intersection && !hasEnoughLengthToIntersection(*candidate_path, current_lanes)) { if (getStopTime() < lane_change_parameters_->stop_time_threshold) { - debug_print("Reject: including intersection!!"); + debug_print_lat("Reject: including intersection!!"); continue; } RCLCPP_WARN_STREAM( @@ -1587,14 +1587,14 @@ bool NormalLaneChange::getLaneChangePaths( if ( lane_change_parameters_->regulate_on_traffic_light && !hasEnoughLengthToTrafficLight(*candidate_path, current_lanes)) { - debug_print("Reject: regulate on traffic light!!"); + debug_print_lat("Reject: regulate on traffic light!!"); continue; } if (utils::traffic_light::isStoppedAtRedTrafficLightWithinDistance( lane_change_info.current_lanes, candidate_path.value().path, planner_data_, lane_change_info.length.sum())) { - debug_print("Ego is stopping near traffic light. Do not allow lane change"); + debug_print_lat("Ego is stopping near traffic light. Do not allow lane change"); continue; } candidate_paths->push_back(*candidate_path); @@ -1604,14 +1604,14 @@ bool NormalLaneChange::getLaneChangePaths( route_handler, *candidate_path, filtered_objects.target_lane, lane_change_buffer, is_goal_in_route, *lane_change_parameters_, lane_change_debug_.collision_check_objects)) { - debug_print( + debug_print_lat( "Reject: parking vehicle exists in the target lane, and the ego is not in stuck. Skip " "lane change."); return false; } if (!check_safety) { - debug_print("ACCEPT!!!: it is valid (and safety check is skipped)."); + debug_print_lat("ACCEPT!!!: it is valid (and safety check is skipped)."); return false; } @@ -1619,11 +1619,11 @@ bool NormalLaneChange::getLaneChangePaths( *candidate_path, target_objects, rss_params, lane_change_debug_.collision_check_objects); if (is_safe) { - debug_print("ACCEPT!!!: it is valid and safe!"); + debug_print_lat("ACCEPT!!!: it is valid and safe!"); return true; } - debug_print("Reject: sampled path is not safe."); + debug_print_lat("Reject: sampled path is not safe."); } } } diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/utils.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/utils.cpp index ad789975cbcd7..ead047f839f47 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/utils.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/utils.cpp @@ -888,7 +888,7 @@ bool isParkedObject( const auto most_left_lanelet_candidates = route_handler.getLaneletMapPtr()->laneletLayer.findUsages(most_left_road_lanelet.leftBound()); lanelet::ConstLanelet most_left_lanelet = most_left_road_lanelet; - const lanelet::Attribute sub_type = + const lanelet::Attribute most_left_sub_type = most_left_lanelet.attribute(lanelet::AttributeName::Subtype); for (const auto & ll : most_left_lanelet_candidates) { @@ -898,7 +898,7 @@ bool isParkedObject( } } bound = most_left_lanelet.leftBound2d().basicLineString(); - if (sub_type.value() != "road_shoulder") { + if (most_left_sub_type.value() != "road_shoulder") { center_to_bound_buffer = object_check_min_road_shoulder_width; } } else { @@ -909,7 +909,7 @@ bool isParkedObject( most_right_road_lanelet.rightBound()); lanelet::ConstLanelet most_right_lanelet = most_right_road_lanelet; - const lanelet::Attribute sub_type = + const lanelet::Attribute most_right_sub_type = most_right_lanelet.attribute(lanelet::AttributeName::Subtype); for (const auto & ll : most_right_lanelet_candidates) { @@ -919,7 +919,7 @@ bool isParkedObject( } } bound = most_right_lanelet.rightBound2d().basicLineString(); - if (sub_type.value() != "road_shoulder") { + if (most_right_sub_type.value() != "road_shoulder") { center_to_bound_buffer = object_check_min_road_shoulder_width; } } From 9aedc684b5b3c88000052703c7bc0d4a0a9a7bab Mon Sep 17 00:00:00 2001 From: kobayu858 <129580202+kobayu858@users.noreply.github.com> Date: Thu, 11 Jul 2024 17:53:30 +0900 Subject: [PATCH 51/70] fix(probabilistic_occupancy_grid_map): fix shadowVariable (#7942) * fix:shadowVariable Signed-off-by: kobayu858 * refactor: use range-based for loop Signed-off-by: kobayu858 * style: rename variable Signed-off-by: kobayu858 * style: rename variable Signed-off-by: kobayu858 --------- Signed-off-by: kobayu858 --- .../lib/costmap_2d/occupancy_grid_map_fixed.cpp | 7 +++---- .../lib/costmap_2d/occupancy_grid_map_projective.cpp | 7 +++---- 2 files changed, 6 insertions(+), 8 deletions(-) diff --git a/perception/probabilistic_occupancy_grid_map/lib/costmap_2d/occupancy_grid_map_fixed.cpp b/perception/probabilistic_occupancy_grid_map/lib/costmap_2d/occupancy_grid_map_fixed.cpp index f25009d47bdcc..dffbda18d0c8a 100644 --- a/perception/probabilistic_occupancy_grid_map/lib/costmap_2d/occupancy_grid_map_fixed.cpp +++ b/perception/probabilistic_occupancy_grid_map/lib/costmap_2d/occupancy_grid_map_fixed.cpp @@ -201,11 +201,10 @@ void OccupancyGridMapFixedBlindSpot::updateWithPointCloud( } // Third step: Overwrite occupied cell - for (size_t bin_index = 0; bin_index < obstacle_pointcloud_angle_bins.size(); ++bin_index) { - auto & obstacle_pointcloud_angle_bin = obstacle_pointcloud_angle_bins.at(bin_index); + for (auto & obstacle_pointcloud_angle_bin : obstacle_pointcloud_angle_bins) { for (size_t dist_index = 0; dist_index < obstacle_pointcloud_angle_bin.size(); ++dist_index) { - const auto & source = obstacle_pointcloud_angle_bin.at(dist_index); - setCellValue(source.wx, source.wy, cost_value::LETHAL_OBSTACLE); + const auto & obstacle_point = obstacle_pointcloud_angle_bin.at(dist_index); + setCellValue(obstacle_point.wx, obstacle_point.wy, cost_value::LETHAL_OBSTACLE); if (dist_index + 1 == obstacle_pointcloud_angle_bin.size()) { continue; diff --git a/perception/probabilistic_occupancy_grid_map/lib/costmap_2d/occupancy_grid_map_projective.cpp b/perception/probabilistic_occupancy_grid_map/lib/costmap_2d/occupancy_grid_map_projective.cpp index a2400fb3c81b2..39224bc7e3a69 100644 --- a/perception/probabilistic_occupancy_grid_map/lib/costmap_2d/occupancy_grid_map_projective.cpp +++ b/perception/probabilistic_occupancy_grid_map/lib/costmap_2d/occupancy_grid_map_projective.cpp @@ -261,11 +261,10 @@ void OccupancyGridMapProjectiveBlindSpot::updateWithPointCloud( if (pub_debug_grid_) converter.addLayerFromCostmap2D(*this, "added_unknown", debug_grid_); // Third step: Overwrite occupied cell - for (size_t bin_index = 0; bin_index < obstacle_pointcloud_angle_bins.size(); ++bin_index) { - auto & obstacle_pointcloud_angle_bin = obstacle_pointcloud_angle_bins.at(bin_index); + for (auto & obstacle_pointcloud_angle_bin : obstacle_pointcloud_angle_bins) { for (size_t dist_index = 0; dist_index < obstacle_pointcloud_angle_bin.size(); ++dist_index) { - const auto & source = obstacle_pointcloud_angle_bin.at(dist_index); - setCellValue(source.wx, source.wy, cost_value::LETHAL_OBSTACLE); + const auto & obstacle_point = obstacle_pointcloud_angle_bin.at(dist_index); + setCellValue(obstacle_point.wx, obstacle_point.wy, cost_value::LETHAL_OBSTACLE); if (dist_index + 1 == obstacle_pointcloud_angle_bin.size()) { continue; From 9f41bf13d9e8a73d2bfd6278cc249e2306348c1e Mon Sep 17 00:00:00 2001 From: danielsanchezaran Date: Thu, 11 Jul 2024 18:38:06 +0900 Subject: [PATCH 52/70] feat(cruise_planner,planning_evaluator): add cruise and slow down diags (#7960) * add cruise and slow down diags to cruise planner Signed-off-by: Daniel Sanchez * add cruise types Signed-off-by: Daniel Sanchez * adjust planning eval Signed-off-by: Daniel Sanchez --------- Signed-off-by: Daniel Sanchez --- .../planning_evaluator_node.hpp | 4 +- .../src/planning_evaluator_node.cpp | 16 +- .../common_structs.hpp | 5 + .../planner_interface.hpp | 8 + .../src/node.cpp | 1 + .../optimization_based_planner.cpp | 2 + .../pid_based_planner/pid_based_planner.cpp | 1 + .../src/planner_interface.cpp | 171 ++++++++++-------- 8 files changed, 130 insertions(+), 78 deletions(-) diff --git a/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/planning_evaluator_node.hpp b/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/planning_evaluator_node.hpp index 6925cb15e8fad..e15427c59ce06 100644 --- a/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/planning_evaluator_node.hpp +++ b/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/planning_evaluator_node.hpp @@ -181,7 +181,9 @@ class PlanningEvaluatorNode : public rclcpp::Node rclcpp::TimerBase::SharedPtr timer_; // queue for diagnostics and time stamp std::deque> diag_queue_; - const std::vector target_functions_ = {"obstacle_cruise_planner"}; + const std::vector target_functions_ = { + "obstacle_cruise_planner_stop", "obstacle_cruise_planner_slow_down", + "obstacle_cruise_planner_cruise"}; std::optional prev_acc_stamped_{std::nullopt}; }; } // namespace planning_diagnostics diff --git a/evaluator/autoware_planning_evaluator/src/planning_evaluator_node.cpp b/evaluator/autoware_planning_evaluator/src/planning_evaluator_node.cpp index ae3416b9cba8f..e02367407f4ae 100644 --- a/evaluator/autoware_planning_evaluator/src/planning_evaluator_node.cpp +++ b/evaluator/autoware_planning_evaluator/src/planning_evaluator_node.cpp @@ -122,7 +122,21 @@ DiagnosticStatus PlanningEvaluatorNode::generateDiagnosticEvaluationStatus( }); const bool found = it != diag.values.end(); status.level = (found) ? status.OK : status.ERROR; - status.values.push_back((found) ? *it : diagnostic_msgs::msg::KeyValue{}); + + auto get_key_value = [&]() { + if (!found) { + return diagnostic_msgs::msg::KeyValue{}; + } + if (it->value.find("none") != std::string::npos) { + return *it; + } + diagnostic_msgs::msg::KeyValue key_value; + key_value.key = "decision"; + key_value.value = "deceleration"; + return key_value; + }; + + status.values.push_back(get_key_value()); return status; } diff --git a/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/common_structs.hpp b/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/common_structs.hpp index fc8a0a96ade42..f2a475c494aba 100644 --- a/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/common_structs.hpp +++ b/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/common_structs.hpp @@ -25,6 +25,8 @@ #include +#include + #include #include #include @@ -286,6 +288,9 @@ struct DebugData MarkerArray cruise_wall_marker; MarkerArray slow_down_wall_marker; std::vector detection_polygons; + std::optional stop_reason_diag{std::nullopt}; + std::optional slow_down_reason_diag{std::nullopt}; + std::optional cruise_reason_diag{std::nullopt}; }; struct EgoNearestParam diff --git a/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/planner_interface.hpp b/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/planner_interface.hpp index 0b54542cbea85..099d80306bb4e 100644 --- a/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/planner_interface.hpp +++ b/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/planner_interface.hpp @@ -95,6 +95,14 @@ class PlannerInterface const std::vector & slow_down_obstacles, std::optional & vel_limit); + DiagnosticStatus makeEmptyDiagnostic(const std::string & reason); + DiagnosticStatus makeDiagnostic( + const std::string & reason, const std::optional & planner_data = std::nullopt, + const std::optional & stop_pose = std::nullopt, + const std::optional & stop_obstacle = std::nullopt); + void publishDiagnostics(const rclcpp::Time & current_time); + void clearDiagnostics(); + void onParam(const std::vector & parameters) { updateCommonParam(parameters); diff --git a/planning/autoware_obstacle_cruise_planner/src/node.cpp b/planning/autoware_obstacle_cruise_planner/src/node.cpp index 2734ee8866c6e..210577d92ef93 100644 --- a/planning/autoware_obstacle_cruise_planner/src/node.cpp +++ b/planning/autoware_obstacle_cruise_planner/src/node.cpp @@ -636,6 +636,7 @@ void ObstacleCruisePlannerNode::onTrajectory(const Trajectory::ConstSharedPtr ms // 8. Publish debug data published_time_publisher_->publish_if_subscribed(trajectory_pub_, output_traj.header.stamp); + planner_ptr_->publishDiagnostics(now()); publishDebugMarker(); publishDebugInfo(); diff --git a/planning/autoware_obstacle_cruise_planner/src/optimization_based_planner/optimization_based_planner.cpp b/planning/autoware_obstacle_cruise_planner/src/optimization_based_planner/optimization_based_planner.cpp index b3a0fc014c410..899cc66cb4331 100644 --- a/planning/autoware_obstacle_cruise_planner/src/optimization_based_planner/optimization_based_planner.cpp +++ b/planning/autoware_obstacle_cruise_planner/src/optimization_based_planner/optimization_based_planner.cpp @@ -199,6 +199,7 @@ std::vector OptimizationBasedPlanner::generateCruiseTrajectory( output.at(i).longitudinal_velocity_mps = 0.0; } prev_output_ = output; + debug_data_ptr_->cruise_reason_diag = makeDiagnostic("cruise", planner_data); return output; } else if (opt_position.size() == 1) { RCLCPP_DEBUG( @@ -255,6 +256,7 @@ std::vector OptimizationBasedPlanner::generateCruiseTrajectory( // Insert Closest Stop Point autoware::motion_utils::insertStopPoint(0, closest_stop_dist, output); + debug_data_ptr_->cruise_reason_diag = makeDiagnostic("cruise", planner_data); prev_output_ = output; return output; } diff --git a/planning/autoware_obstacle_cruise_planner/src/pid_based_planner/pid_based_planner.cpp b/planning/autoware_obstacle_cruise_planner/src/pid_based_planner/pid_based_planner.cpp index d0736c6158a7a..a021e87e6199e 100644 --- a/planning/autoware_obstacle_cruise_planner/src/pid_based_planner/pid_based_planner.cpp +++ b/planning/autoware_obstacle_cruise_planner/src/pid_based_planner/pid_based_planner.cpp @@ -323,6 +323,7 @@ std::vector PIDBasedPlanner::planCruise( // cruise obstacle debug_data_ptr_->obstacles_to_cruise.push_back(cruise_obstacle_info->obstacle); + debug_data_ptr_->cruise_reason_diag = makeDiagnostic("cruise", planner_data); } // do cruise planning diff --git a/planning/autoware_obstacle_cruise_planner/src/planner_interface.cpp b/planning/autoware_obstacle_cruise_planner/src/planner_interface.cpp index c531c4fefac95..0b9a9eaa9d58c 100644 --- a/planning/autoware_obstacle_cruise_planner/src/planner_interface.cpp +++ b/planning/autoware_obstacle_cruise_planner/src/planner_interface.cpp @@ -25,6 +25,7 @@ #include #include +#include #include namespace @@ -106,78 +107,6 @@ VelocityFactorArray makeVelocityFactorArray( return velocity_factor_array; } -DiagnosticArray makeEmptyStopReasonDiagnosticArray(const rclcpp::Time & current_time) -{ - // Create status - DiagnosticStatus status; - status.level = status.OK; - status.name = "obstacle_cruise_planner"; - diagnostic_msgs::msg::KeyValue key_value; - { - // Decision - key_value.key = "decision"; - key_value.value = "none"; - status.values.push_back(key_value); - } - // create array - DiagnosticArray diagnostics; - diagnostics.header.stamp = current_time; - diagnostics.header.frame_id = "map"; - diagnostics.status.push_back(status); - return diagnostics; -} - -DiagnosticArray makeStopReasonDiagnosticArray( - const PlannerData & planner_data, const geometry_msgs::msg::Pose & stop_pose, - const StopObstacle & stop_obstacle) -{ - // Create status - DiagnosticStatus status; - status.level = status.OK; - status.name = "obstacle_cruise_planner"; - diagnostic_msgs::msg::KeyValue key_value; - { - // Decision - key_value.key = "decision"; - key_value.value = "stop"; - status.values.push_back(key_value); - } - - { // Stop info - key_value.key = "stop_position"; - const auto & p = stop_pose.position; - key_value.value = - "{" + std::to_string(p.x) + ", " + std::to_string(p.y) + ", " + std::to_string(p.z) + "}"; - status.values.push_back(key_value); - key_value.key = "stop_orientation"; - const auto & o = stop_pose.orientation; - key_value.value = "{" + std::to_string(o.w) + ", " + std::to_string(o.x) + ", " + - std::to_string(o.y) + ", " + std::to_string(o.z) + "}"; - status.values.push_back(key_value); - const auto dist_to_stop_pose = autoware::motion_utils::calcSignedArcLength( - planner_data.traj_points, planner_data.ego_pose.position, stop_pose.position); - key_value.key = "distance_to_stop_pose"; - key_value.value = std::to_string(dist_to_stop_pose); - status.values.push_back(key_value); - } - - { - // Obstacle info - const auto & p = stop_obstacle.collision_point; - key_value.key = "collision_point"; - key_value.value = - "{" + std::to_string(p.x) + ", " + std::to_string(p.y) + ", " + std::to_string(p.z) + "}"; - status.values.push_back(key_value); - } - - // create array - DiagnosticArray diagnostics; - diagnostics.header.stamp = planner_data.current_time; - diagnostics.header.frame_id = "map"; - diagnostics.status.push_back(status); - return diagnostics; -} - double calcMinimumDistanceToStop( const double initial_vel, const double max_acc, const double min_acc) { @@ -319,7 +248,6 @@ std::vector PlannerInterface::generateStopTrajectory( if (stop_obstacles.empty()) { stop_reasons_pub_->publish(makeEmptyStopReasonArray(planner_data.current_time)); velocity_factors_pub_->publish(makeVelocityFactorArray(planner_data.current_time)); - diagnostics_pub_->publish(makeEmptyStopReasonDiagnosticArray(planner_data.current_time)); // delete marker const auto markers = autoware::motion_utils::createDeletedStopVirtualWallMarker(planner_data.current_time, 0); @@ -474,11 +402,11 @@ std::vector PlannerInterface::generateStopTrajectory( const auto stop_pose = output_traj_points.at(*zero_vel_idx).pose; const auto stop_reasons_msg = makeStopReasonArray(planner_data, stop_pose, *determined_stop_obstacle); - const auto stop_reason_diagnostic_array = - makeStopReasonDiagnosticArray(planner_data, stop_pose, *determined_stop_obstacle); stop_reasons_pub_->publish(stop_reasons_msg); velocity_factors_pub_->publish(makeVelocityFactorArray(planner_data.current_time, stop_pose)); - diagnostics_pub_->publish(stop_reason_diagnostic_array); + // Store stop reason debug data + debug_data_ptr_->stop_reason_diag = + makeDiagnostic("stop", planner_data, stop_pose, *determined_stop_obstacle); // Publish if ego vehicle will over run against the stop point with a limit acceleration const bool will_over_run = determined_zero_vel_dist.value() > @@ -747,7 +675,11 @@ std::vector PlannerInterface::generateSlowDownTrajectory( markers, &debug_data_ptr_->slow_down_debug_wall_marker); } + // Add debug data debug_data_ptr_->obstacles_to_slow_down.push_back(obstacle); + if (!debug_data_ptr_->slow_down_reason_diag.has_value()) { + debug_data_ptr_->slow_down_reason_diag = makeDiagnostic("slow_down", planner_data); + } // update prev_slow_down_output_ new_prev_slow_down_output.push_back(SlowDownOutput{ @@ -904,3 +836,90 @@ PlannerInterface::calculateDistanceToSlowDownWithConstraints( return std::make_tuple( filtered_dist_to_slow_down_start, filtered_dist_to_slow_down_end, feasible_slow_down_vel); } + +DiagnosticStatus PlannerInterface::makeEmptyDiagnostic(const std::string & reason) +{ + // Create status + DiagnosticStatus status; + status.level = status.OK; + status.name = "obstacle_cruise_planner_" + reason; + diagnostic_msgs::msg::KeyValue key_value; + { + // Decision + key_value.key = "decision"; + key_value.value = "none"; + status.values.push_back(key_value); + } + + return status; +} + +DiagnosticStatus PlannerInterface::makeDiagnostic( + const std::string & reason, const std::optional & planner_data, + const std::optional & stop_pose, + const std::optional & stop_obstacle) +{ + // Create status + DiagnosticStatus status; + status.level = status.OK; + status.name = "obstacle_cruise_planner_" + reason; + diagnostic_msgs::msg::KeyValue key_value; + { + // Decision + key_value.key = "decision"; + key_value.value = reason; + status.values.push_back(key_value); + } + + if (stop_pose.has_value() && planner_data.has_value()) { // Stop info + key_value.key = "stop_position"; + const auto & p = stop_pose.value().position; + key_value.value = + "{" + std::to_string(p.x) + ", " + std::to_string(p.y) + ", " + std::to_string(p.z) + "}"; + status.values.push_back(key_value); + key_value.key = "stop_orientation"; + const auto & o = stop_pose.value().orientation; + key_value.value = "{" + std::to_string(o.w) + ", " + std::to_string(o.x) + ", " + + std::to_string(o.y) + ", " + std::to_string(o.z) + "}"; + status.values.push_back(key_value); + const auto dist_to_stop_pose = autoware::motion_utils::calcSignedArcLength( + planner_data.value().traj_points, planner_data.value().ego_pose.position, + stop_pose.value().position); + key_value.key = "distance_to_stop_pose"; + key_value.value = std::to_string(dist_to_stop_pose); + status.values.push_back(key_value); + } + + if (stop_obstacle.has_value()) { + // Obstacle info + const auto & p = stop_obstacle.value().collision_point; + key_value.key = "collision_point"; + key_value.value = + "{" + std::to_string(p.x) + ", " + std::to_string(p.y) + ", " + std::to_string(p.z) + "}"; + status.values.push_back(key_value); + } + + return status; +} + +void PlannerInterface::publishDiagnostics(const rclcpp::Time & current_time) +{ + // create array + DiagnosticArray diagnostics; + diagnostics.header.stamp = current_time; + diagnostics.header.frame_id = "map"; + const auto & d = debug_data_ptr_; + diagnostics.status = { + (d->stop_reason_diag) ? d->stop_reason_diag.value() : makeEmptyDiagnostic("stop"), + (d->slow_down_reason_diag) ? *(d->slow_down_reason_diag) : makeEmptyDiagnostic("slow_down"), + (d->cruise_reason_diag) ? d->cruise_reason_diag.value() : makeEmptyDiagnostic("cruise")}; + diagnostics_pub_->publish(diagnostics); + clearDiagnostics(); +} + +void PlannerInterface::clearDiagnostics() +{ + debug_data_ptr_->stop_reason_diag = std::nullopt; + debug_data_ptr_->slow_down_reason_diag = std::nullopt; + debug_data_ptr_->cruise_reason_diag = std::nullopt; +} From b038006967e336f7415bc3f9bafc0439140fb971 Mon Sep 17 00:00:00 2001 From: Yuntianyi Chen Date: Thu, 11 Jul 2024 21:57:50 +0800 Subject: [PATCH 53/70] refactor(dummy_infrastructure): rework parameters (#5275) Signed-off-by: Yuntianyi Chen --- system/dummy_infrastructure/CMakeLists.txt | 4 +- system/dummy_infrastructure/README.md | 3 +- .../config/dummy_infrastructure.param.yaml | 10 +-- .../launch/dummy_infrastructure.launch.xml | 1 + .../dummy_infrastructure_node.schema.json | 65 +++++++++++++++++++ .../dummy_infrastructure_node.cpp | 12 ++-- 6 files changed, 81 insertions(+), 14 deletions(-) create mode 100644 system/dummy_infrastructure/schema/dummy_infrastructure_node.schema.json diff --git a/system/dummy_infrastructure/CMakeLists.txt b/system/dummy_infrastructure/CMakeLists.txt index cfe326d7bb53b..5d3d39d02365d 100644 --- a/system/dummy_infrastructure/CMakeLists.txt +++ b/system/dummy_infrastructure/CMakeLists.txt @@ -15,6 +15,6 @@ rclcpp_components_register_node(dummy_infrastructure_node_component ament_auto_package( INSTALL_TO_SHARE - config - launch + launch + config ) diff --git a/system/dummy_infrastructure/README.md b/system/dummy_infrastructure/README.md index 2f3196189a1b3..d032bfa1a7e3d 100644 --- a/system/dummy_infrastructure/README.md +++ b/system/dummy_infrastructure/README.md @@ -29,8 +29,9 @@ ros2 run rqt_reconfigure rqt_reconfigure | Name | Type | Default Value | Explanation | | ------------------- | ------ | ------------- | ------------------------------------------------- | -| `update_rate` | int | `10` | Timer callback period [Hz] | +| `update_rate` | double | `10.0` | Timer callback period [Hz] | | `use_first_command` | bool | `true` | Consider instrument id or not | +| `use_command_state` | bool | `false` | Consider command state or not | | `instrument_id` | string | `` | Used as command id | | `approval` | bool | `false` | set approval filed to ros param | | `is_finalized` | bool | `false` | Stop at stop_line if finalization isn't completed | diff --git a/system/dummy_infrastructure/config/dummy_infrastructure.param.yaml b/system/dummy_infrastructure/config/dummy_infrastructure.param.yaml index 50dc36193776b..83976798d30b2 100644 --- a/system/dummy_infrastructure/config/dummy_infrastructure.param.yaml +++ b/system/dummy_infrastructure/config/dummy_infrastructure.param.yaml @@ -1,8 +1,8 @@ /**: ros__parameters: update_rate_hz: 10.0 - use_first_command: false - use_command_state: true - instrument_id: '0' - approval: true - is_finalized: true + use_first_command: true + use_command_state: false + instrument_id: "" + approval: false + is_finalized: false diff --git a/system/dummy_infrastructure/launch/dummy_infrastructure.launch.xml b/system/dummy_infrastructure/launch/dummy_infrastructure.launch.xml index 751575e343fa6..bc45abdebec70 100644 --- a/system/dummy_infrastructure/launch/dummy_infrastructure.launch.xml +++ b/system/dummy_infrastructure/launch/dummy_infrastructure.launch.xml @@ -1,3 +1,4 @@ + diff --git a/system/dummy_infrastructure/schema/dummy_infrastructure_node.schema.json b/system/dummy_infrastructure/schema/dummy_infrastructure_node.schema.json new file mode 100644 index 0000000000000..697ec01d31a64 --- /dev/null +++ b/system/dummy_infrastructure/schema/dummy_infrastructure_node.schema.json @@ -0,0 +1,65 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameters for Dummy Infrastructure Node", + "type": "object", + "definitions": { + "dummy_infrastructure": { + "type": "object", + "properties": { + "update_rate_hz": { + "type": "number", + "default": "10.0", + "description": "Timer callback period [Hz]" + }, + "use_first_command": { + "type": "boolean", + "default": "true", + "description": "Consider instrument id or not" + }, + "use_command_state": { + "type": "boolean", + "default": "false", + "description": "Consider command state or not" + }, + "instrument_id": { + "type": "string", + "default": "", + "description": "Used as command id" + }, + "approval": { + "type": "boolean", + "default": "false", + "description": "set approval filed to ros param" + }, + "is_finalized": { + "type": "boolean", + "default": "false", + "description": "Stop at stop_line if finalization isn't completed" + } + }, + "required": [ + "update_rate_hz", + "use_first_command", + "use_command_state", + "instrument_id", + "approval", + "is_finalized" + ], + "additionalProperties": false + } + }, + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "$ref": "#/definitions/dummy_infrastructure" + } + }, + "required": ["ros__parameters"], + "additionalProperties": false + } + }, + "required": ["/**"], + "additionalProperties": false +} diff --git a/system/dummy_infrastructure/src/dummy_infrastructure_node/dummy_infrastructure_node.cpp b/system/dummy_infrastructure/src/dummy_infrastructure_node/dummy_infrastructure_node.cpp index f24ce4587de0b..485d85c926268 100644 --- a/system/dummy_infrastructure/src/dummy_infrastructure_node/dummy_infrastructure_node.cpp +++ b/system/dummy_infrastructure/src/dummy_infrastructure_node/dummy_infrastructure_node.cpp @@ -89,12 +89,12 @@ DummyInfrastructureNode::DummyInfrastructureNode(const rclcpp::NodeOptions & nod this->add_on_set_parameters_callback(std::bind(&DummyInfrastructureNode::onSetParam, this, _1)); // Parameter - node_param_.update_rate_hz = declare_parameter("update_rate_hz", 10.0); - node_param_.use_first_command = declare_parameter("use_first_command", true); - node_param_.use_command_state = declare_parameter("use_command_state", false); - node_param_.instrument_id = declare_parameter("instrument_id", ""); - node_param_.approval = declare_parameter("approval", false); - node_param_.is_finalized = declare_parameter("is_finalized", false); + node_param_.update_rate_hz = declare_parameter("update_rate_hz"); + node_param_.use_first_command = declare_parameter("use_first_command"); + node_param_.use_command_state = declare_parameter("use_command_state"); + node_param_.instrument_id = declare_parameter("instrument_id"); + node_param_.approval = declare_parameter("approval"); + node_param_.is_finalized = declare_parameter("is_finalized"); // Subscriber sub_command_array_ = create_subscription( From fec446ae1ca08dce587d19bbae1957a25c058a33 Mon Sep 17 00:00:00 2001 From: Koichi98 <45482193+Koichi98@users.noreply.github.com> Date: Fri, 12 Jul 2024 10:17:30 +0900 Subject: [PATCH 54/70] chore(cppcheck): delete cstyleCast from .cppcheck_suppressions (#7978) * chore(cppcheck): delete cstyleCast from .cppcheck_suppressions Signed-off-by: Koichi Imai * chore(cppcheck): clean .cppcheck_suppressions Signed-off-by: Koichi Imai --------- Signed-off-by: Koichi Imai --- .cppcheck_suppressions | 2 -- 1 file changed, 2 deletions(-) diff --git a/.cppcheck_suppressions b/.cppcheck_suppressions index 7c921dac7db4c..0b484ad372448 100644 --- a/.cppcheck_suppressions +++ b/.cppcheck_suppressions @@ -3,8 +3,6 @@ checkersReport constParameterReference constVariableReference -// cspell: ignore cstyle -cstyleCast funcArgNamesDifferent functionConst functionStatic From b1eeb68374c67ba0292bbfd5a3a466cf7209da05 Mon Sep 17 00:00:00 2001 From: kobayu858 <129580202+kobayu858@users.noreply.github.com> Date: Fri, 12 Jul 2024 11:48:02 +0900 Subject: [PATCH 55/70] fix(autoware_behavior_velocity_intersection_module): fix shadowVariable (#7976) Signed-off-by: kobayu858 --- .../src/scene_intersection.cpp | 1 - .../src/scene_intersection_prepare_data.cpp | 8 ++++---- 2 files changed, 4 insertions(+), 5 deletions(-) diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection.cpp index ac157fb3ab6dc..9f6935e6d7e7b 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection.cpp @@ -392,7 +392,6 @@ DecisionResult IntersectionModule::modifyPathVelocityDetail( if (!has_traffic_light_) { if (fromEgoDist(occlusion_wo_tl_pass_judge_line_idx) < 0) { if (has_collision) { - const auto closest_idx = intersection_stoplines.closest_idx; const std::string evasive_diag = generateEgoRiskEvasiveDiagnosis( *path, closest_idx, time_distance_array, too_late_detect_objects, misjudge_objects); return OverPassJudge{ diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection_prepare_data.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection_prepare_data.cpp index a4c4e1b41688f..92a8fc57079df 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection_prepare_data.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection_prepare_data.cpp @@ -689,8 +689,8 @@ IntersectionLanelets IntersectionModule::generateObjectiveLanelets( routing_graph_ptr, ll, length, ego_lanelets); for (const auto & ls : lanelet_sequences) { for (const auto & l : ls) { - const auto & inserted = detection_ids.insert(l.id()); - if (inserted.second) detection_and_preceding_lanelets.push_back(l); + const auto & inner_inserted = detection_ids.insert(l.id()); + if (inner_inserted.second) detection_and_preceding_lanelets.push_back(l); } } } @@ -710,8 +710,8 @@ IntersectionLanelets IntersectionModule::generateObjectiveLanelets( routing_graph_ptr, ll, length, ego_lanelets); for (const auto & ls : lanelet_sequences) { for (const auto & l : ls) { - const auto & inserted = detection_ids.insert(l.id()); - if (inserted.second) occlusion_detection_and_preceding_lanelets.push_back(l); + const auto & inner_inserted = detection_ids.insert(l.id()); + if (inner_inserted.second) occlusion_detection_and_preceding_lanelets.push_back(l); } } } From 18e264821bc5944d80c83ce83168333b8e9c156a Mon Sep 17 00:00:00 2001 From: kobayu858 <129580202+kobayu858@users.noreply.github.com> Date: Fri, 12 Jul 2024 13:19:53 +0900 Subject: [PATCH 56/70] fix(autoware_motion_velocity_obstacle_velocity_limiter_module): fix shadowVariable (#7977) * fix:shadowVariable Signed-off-by: kobayu858 * fix:shadowVariable Signed-off-by: kobayu858 --------- Signed-off-by: kobayu858 --- .../src/trajectory_preprocessing.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/trajectory_preprocessing.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/trajectory_preprocessing.cpp index f4b1304d67e5e..f56b50635c96e 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/trajectory_preprocessing.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/trajectory_preprocessing.cpp @@ -71,7 +71,7 @@ void calculateSteeringAngles(TrajectoryPoints & trajectory, const double wheel_b auto prev_point = trajectory.front(); auto prev_heading = tf2::getYaw(prev_point.pose.orientation); for (auto i = 1ul; i < trajectory.size(); ++i) { - const auto & prev_point = trajectory[i - 1]; + prev_point = trajectory[i - 1]; auto & point = trajectory[i]; const auto dt = autoware::universe_utils::calcDistance2d(prev_point, point) / prev_point.longitudinal_velocity_mps; From 8c20fcc669db260a0ed9d9b16455c2a3bbc24082 Mon Sep 17 00:00:00 2001 From: kobayu858 <129580202+kobayu858@users.noreply.github.com> Date: Fri, 12 Jul 2024 13:23:35 +0900 Subject: [PATCH 57/70] fix(system_monitor): fix shadowVariable (#7981) fix:shadowVariable Signed-off-by: kobayu858 --- system/system_monitor/src/hdd_monitor/hdd_monitor.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/system/system_monitor/src/hdd_monitor/hdd_monitor.cpp b/system/system_monitor/src/hdd_monitor/hdd_monitor.cpp index 85a88d2430bce..50ac4363da609 100644 --- a/system/system_monitor/src/hdd_monitor/hdd_monitor.cpp +++ b/system/system_monitor/src/hdd_monitor/hdd_monitor.cpp @@ -684,8 +684,8 @@ void HddMonitor::updateHddInfoList() // Restore HDD information list try { std::istringstream iss(buf); - boost::archive::text_iarchive oa(iss); - oa >> hdd_info_list_; + boost::archive::text_iarchive ia(iss); + ia >> hdd_info_list_; } catch (const std::exception & e) { connect_diag_.summary(DiagStatus::ERROR, "recv error"); connect_diag_.add("recv", e.what()); From dc864d3062af8ae2023e5586a8ee96e550df6aa4 Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Fri, 12 Jul 2024 13:36:08 +0900 Subject: [PATCH 58/70] feat(processing_time_checker): add a new package (#7957) * feat(processing_time_checker): add a new package Signed-off-by: Takayuki Murooka * fix Signed-off-by: Takayuki Murooka * fix Signed-off-by: Takayuki Murooka * update README and schema.json Signed-off-by: Takayuki Murooka * fix Signed-off-by: Takayuki Murooka * fix Signed-off-by: Takayuki Murooka * fix Signed-off-by: Takayuki Murooka --------- Signed-off-by: Takayuki Murooka --- .../launch/simulator.launch.xml | 2 +- .../launch/system.launch.xml | 8 ++ .../CMakeLists.txt | 20 +++ .../README.md | 36 ++++++ .../config/processing_time_checker.param.yaml | 37 ++++++ .../launch/processing_time_checker.launch.xml | 7 + .../package.xml | 26 ++++ .../processing_time_checker.schema.json | 38 ++++++ .../src/processing_time_checker.cpp | 122 ++++++++++++++++++ .../src/processing_time_checker.hpp | 53 ++++++++ 10 files changed, 348 insertions(+), 1 deletion(-) create mode 100644 system/autoware_processing_time_checker/CMakeLists.txt create mode 100644 system/autoware_processing_time_checker/README.md create mode 100644 system/autoware_processing_time_checker/config/processing_time_checker.param.yaml create mode 100644 system/autoware_processing_time_checker/launch/processing_time_checker.launch.xml create mode 100644 system/autoware_processing_time_checker/package.xml create mode 100644 system/autoware_processing_time_checker/schema/processing_time_checker.schema.json create mode 100644 system/autoware_processing_time_checker/src/processing_time_checker.cpp create mode 100644 system/autoware_processing_time_checker/src/processing_time_checker.hpp diff --git a/launch/tier4_simulator_launch/launch/simulator.launch.xml b/launch/tier4_simulator_launch/launch/simulator.launch.xml index 7dc33e0e82ea9..bf43fde85e0a2 100644 --- a/launch/tier4_simulator_launch/launch/simulator.launch.xml +++ b/launch/tier4_simulator_launch/launch/simulator.launch.xml @@ -148,7 +148,7 @@ - + diff --git a/launch/tier4_system_launch/launch/system.launch.xml b/launch/tier4_system_launch/launch/system.launch.xml index 55c4403b65923..d11d00c43d55b 100644 --- a/launch/tier4_system_launch/launch/system.launch.xml +++ b/launch/tier4_system_launch/launch/system.launch.xml @@ -4,6 +4,7 @@ + @@ -64,6 +65,13 @@ + + + + + + + diff --git a/system/autoware_processing_time_checker/CMakeLists.txt b/system/autoware_processing_time_checker/CMakeLists.txt new file mode 100644 index 0000000000000..d9b3acb9abacf --- /dev/null +++ b/system/autoware_processing_time_checker/CMakeLists.txt @@ -0,0 +1,20 @@ +cmake_minimum_required(VERSION 3.14) +project(autoware_processing_time_checker) + +find_package(autoware_cmake REQUIRED) +autoware_package() +find_package(rclcpp REQUIRED) + +ament_auto_add_library(${PROJECT_NAME} SHARED + src/processing_time_checker.cpp +) + +rclcpp_components_register_node(${PROJECT_NAME} + PLUGIN "autoware::processing_time_checker::ProcessingTimeChecker" + EXECUTABLE processing_time_checker_node +) + +ament_auto_package(INSTALL_TO_SHARE + launch + config +) diff --git a/system/autoware_processing_time_checker/README.md b/system/autoware_processing_time_checker/README.md new file mode 100644 index 0000000000000..db2dd8d041e61 --- /dev/null +++ b/system/autoware_processing_time_checker/README.md @@ -0,0 +1,36 @@ +# Processing Time Checker + +## Purpose + +This node checks whether the processing time of each module is valid or not, and send a diagnostic. +NOTE: Currently, there is no validation feature, and "OK" is always assigned in the diagnostic. + +### Standalone Startup + +```bash +ros2 launch autoware_processing_time_checker processing_time_checker.launch.xml +``` + +## Inner-workings / Algorithms + +## Inputs / Outputs + +### Input + +| Name | Type | Description | +| ------------------------- | --------------------------------- | ------------------------------ | +| `/.../processing_time_ms` | `tier4_debug_msgs/Float64Stamped` | processing time of each module | + +### Output + +| Name | Type | Description | +| ----------------------------------------- | --------------------------------- | ---------------------------------- | +| `/system/processing_time_checker/metrics` | `diagnostic_msgs/DiagnosticArray` | processing time of all the modules | + +## Parameters + +{{ json_to_markdown("system/autoware_processing_time_checker/schema/processing_time_checker.schema.json") }} + +## Assumptions / Known limits + +TBD. diff --git a/system/autoware_processing_time_checker/config/processing_time_checker.param.yaml b/system/autoware_processing_time_checker/config/processing_time_checker.param.yaml new file mode 100644 index 0000000000000..84e7d79079dae --- /dev/null +++ b/system/autoware_processing_time_checker/config/processing_time_checker.param.yaml @@ -0,0 +1,37 @@ +/**: + ros__parameters: + update_rate: 10.0 + processing_time_topic_name_list: + - /control/trajectory_follower/controller_node_exe/lateral/debug/processing_time_ms + - /control/trajectory_follower/controller_node_exe/longitudinal/debug/processing_time_ms + - /control/trajectory_follower/lane_departure_checker_node/debug/processing_time_ms + - /perception/object_recognition/prediction/map_based_prediction/debug/processing_time_ms + - /perception/object_recognition/tracking/multi_object_tracker/debug/processing_time_ms + - /planning/planning_validator/debug/processing_time_ms + - /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/debug/avoidance_by_lane_change/processing_time_ms + - /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/debug/dynamic_obstacle_avoidance/processing_time_ms + - /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/debug/goal_planner/processing_time_ms + - /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/debug/lane_change_left/processing_time_ms + - /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/debug/lane_change_right/processing_time_ms + - /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/debug/side_shift/processing_time_ms + - /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/debug/start_planner/processing_time_ms + - /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/debug/static_obstacle_avoidance/processing_time_ms + - /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/debug/total_time/processing_time_ms + - /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/blind_spot/processing_time_ms + - /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/crosswalk/processing_time_ms + - /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/detection_area/processing_time_ms + - /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/intersection/processing_time_ms + - /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/merge_from_private/processing_time_ms + - /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/no_stopping_area/processing_time_ms + - /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/run_out/processing_time_ms + - /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/stop_line/processing_time_ms + - /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/walkway/processing_time_ms + - /planning/scenario_planning/lane_driving/motion_planning/elastic_band_smoother/debug/processing_time_ms + - /planning/scenario_planning/lane_driving/motion_planning/motion_velocity_planner/debug/dynamic_obstacle_stop/processing_time_ms + - /planning/scenario_planning/lane_driving/motion_planning/motion_velocity_planner/debug/obstacle_velocity_limiter/processing_time_ms + - /planning/scenario_planning/lane_driving/motion_planning/motion_velocity_planner/debug/out_of_lane/processing_time_ms + - /planning/scenario_planning/lane_driving/motion_planning/motion_velocity_planner/debug/processing_time_ms + - /planning/scenario_planning/lane_driving/motion_planning/obstacle_cruise_planner/debug/processing_time_ms + - /planning/scenario_planning/lane_driving/motion_planning/path_optimizer/debug/processing_time_ms + - /planning/scenario_planning/velocity_smoother/debug/processing_time_ms + - /simulation/shape_estimation/debug/processing_time_ms diff --git a/system/autoware_processing_time_checker/launch/processing_time_checker.launch.xml b/system/autoware_processing_time_checker/launch/processing_time_checker.launch.xml new file mode 100644 index 0000000000000..1e9eb6a3d03e1 --- /dev/null +++ b/system/autoware_processing_time_checker/launch/processing_time_checker.launch.xml @@ -0,0 +1,7 @@ + + + + + + + diff --git a/system/autoware_processing_time_checker/package.xml b/system/autoware_processing_time_checker/package.xml new file mode 100644 index 0000000000000..52f6fe6c2c27d --- /dev/null +++ b/system/autoware_processing_time_checker/package.xml @@ -0,0 +1,26 @@ + + + + autoware_processing_time_checker + 1.0.0 + A package to find out nodes with common names + Takayuki Murooka + Kosuke Takeuchi + + Apache 2.0 + + ament_cmake + autoware_cmake + + diagnostic_updater + rclcpp + rclcpp_components + tier4_debug_msgs + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/system/autoware_processing_time_checker/schema/processing_time_checker.schema.json b/system/autoware_processing_time_checker/schema/processing_time_checker.schema.json new file mode 100644 index 0000000000000..152cedb60f8ab --- /dev/null +++ b/system/autoware_processing_time_checker/schema/processing_time_checker.schema.json @@ -0,0 +1,38 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameters for Processing Time Checker", + "type": "object", + "definitions": { + "autoware_processing_time_checker": { + "type": "object", + "properties": { + "update_rate": { + "type": "number", + "default": 10, + "exclusiveMinimum": 2, + "description": "The scanning and update frequency of the checker." + }, + "processing_time_topic_name_list": { + "type": "array", + "items": { + "type": "string" + }, + "description": "The topic name list of the processing time." + } + }, + "required": ["update_rate", "processing_time_topic_name_list"] + } + }, + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "$ref": "#/definitions/autoware_processing_time_checker" + } + }, + "required": ["ros__parameters"] + } + }, + "required": ["/**"] +} diff --git a/system/autoware_processing_time_checker/src/processing_time_checker.cpp b/system/autoware_processing_time_checker/src/processing_time_checker.cpp new file mode 100644 index 0000000000000..fec7a16134915 --- /dev/null +++ b/system/autoware_processing_time_checker/src/processing_time_checker.cpp @@ -0,0 +1,122 @@ +// 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 "processing_time_checker.hpp" + +#include + +#include +#include +#include + +namespace autoware::processing_time_checker +{ + +namespace +{ +std::string remove_last_name(const std::string & str) +{ + return str.substr(0, str.find_last_of("/")); +} + +std::string get_last_name(const std::string & str) +{ + return str.substr(str.find_last_of("/") + 1); +} +} // namespace + +ProcessingTimeChecker::ProcessingTimeChecker(const rclcpp::NodeOptions & node_options) +: Node("processing_time_checker", node_options) +{ + const double update_rate = declare_parameter("update_rate"); + const auto processing_time_topic_name_list = + declare_parameter>("processing_time_topic_name_list"); + + for (const auto & processing_time_topic_name : processing_time_topic_name_list) { + std::optional module_name{std::nullopt}; + + // extract module name from topic name + auto tmp_topic_name = processing_time_topic_name; + for (size_t i = 0; i < 4; ++i) { // 4 is enouh for the search depth + tmp_topic_name = remove_last_name(tmp_topic_name); + const auto module_name_candidate = get_last_name(tmp_topic_name); + // clang-format off + if ( + module_name_candidate != "processing_time_ms" && module_name_candidate != "debug" && + module_name_candidate != "total_time") + { + module_name = module_name_candidate; + break; + } + // clang-format on + } + + // register module name + if (module_name) { + module_name_map_.insert_or_assign(processing_time_topic_name, *module_name); + } else { + throw std::invalid_argument("The format of the processing time topic name is not correct."); + } + } + + // create subscribers + for (const auto & processing_time_topic_name : processing_time_topic_name_list) { + const auto & module_name = module_name_map_.at(processing_time_topic_name); + + // clang-format off + processing_time_subscribers_.push_back( + create_subscription( + processing_time_topic_name, 1, + [this, &module_name]([[maybe_unused]] const Float64Stamped & msg) { + processing_time_map_.insert_or_assign(module_name, msg.data); + })); + // clang-format on + } + + diag_pub_ = create_publisher("~/metrics", 1); + + const auto period_ns = rclcpp::Rate(update_rate).period(); + timer_ = rclcpp::create_timer( + this, get_clock(), period_ns, std::bind(&ProcessingTimeChecker::on_timer, this)); +} + +void ProcessingTimeChecker::on_timer() +{ + // create diagnostic status + DiagnosticStatus status; + status.level = status.OK; + status.name = "processing_time"; + for (const auto & processing_time_iterator : processing_time_map_) { + const auto processing_time_topic_name = processing_time_iterator.first; + const double processing_time = processing_time_iterator.second; + + // generate diagnostic status + diagnostic_msgs::msg::KeyValue key_value; + key_value.key = processing_time_topic_name; + key_value.value = std::to_string(processing_time); + status.values.push_back(key_value); + } + + // create diagnostic array + DiagnosticArray diag_msg; + diag_msg.header.stamp = now(); + diag_msg.status.push_back(status); + + // publish + diag_pub_->publish(diag_msg); +} +} // namespace autoware::processing_time_checker + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(autoware::processing_time_checker::ProcessingTimeChecker) diff --git a/system/autoware_processing_time_checker/src/processing_time_checker.hpp b/system/autoware_processing_time_checker/src/processing_time_checker.hpp new file mode 100644 index 0000000000000..eba6f2c0642e6 --- /dev/null +++ b/system/autoware_processing_time_checker/src/processing_time_checker.hpp @@ -0,0 +1,53 @@ +// 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 PROCESSING_TIME_CHECKER_HPP_ +#define PROCESSING_TIME_CHECKER_HPP_ + +#include + +#include "diagnostic_msgs/msg/diagnostic_array.hpp" +#include "tier4_debug_msgs/msg/float64_stamped.hpp" + +#include +#include +#include + +namespace autoware::processing_time_checker +{ +using diagnostic_msgs::msg::DiagnosticArray; +using diagnostic_msgs::msg::DiagnosticStatus; +using tier4_debug_msgs::msg::Float64Stamped; + +class ProcessingTimeChecker : public rclcpp::Node +{ +public: + explicit ProcessingTimeChecker(const rclcpp::NodeOptions & node_options); + +private: + void on_timer(); + + rclcpp::TimerBase::SharedPtr timer_; + + rclcpp::Publisher::SharedPtr diag_pub_; + std::vector::SharedPtr> processing_time_subscribers_; + + // topic name - module name + std::unordered_map module_name_map_{}; + // module name - processing time + std::unordered_map processing_time_map_{}; +}; +} // namespace autoware::processing_time_checker + +#endif // PROCESSING_TIME_CHECKER_HPP_ From 65c039d31fa81a21e2ad1498e0160add2de44868 Mon Sep 17 00:00:00 2001 From: SakodaShintaro Date: Fri, 12 Jul 2024 14:01:33 +0900 Subject: [PATCH 59/70] docs(localization_error_monitor): fix input description of localization_error_monitor (#7993) Fixed input of localization_error_monitor Signed-off-by: Shintaro Sakoda --- localization/localization_error_monitor/README.md | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/localization/localization_error_monitor/README.md b/localization/localization_error_monitor/README.md index 7912ad843ef19..8dc82f09f3d07 100644 --- a/localization/localization_error_monitor/README.md +++ b/localization/localization_error_monitor/README.md @@ -16,9 +16,9 @@ The package monitors the following two values: ### Input -| Name | Type | Description | -| --------------------- | ----------------------------------------------- | ------------------- | -| `input/pose_with_cov` | `geometry_msgs::msg::PoseWithCovarianceStamped` | localization result | +| Name | Type | Description | +| ------------ | ------------------------- | ------------------- | +| `input/odom` | `nav_msgs::msg::Odometry` | localization result | ### Output From 9c94b7adf2a4bf9914bfc19047b6542fd0dd4362 Mon Sep 17 00:00:00 2001 From: kobayu858 <129580202+kobayu858@users.noreply.github.com> Date: Fri, 12 Jul 2024 16:15:12 +0900 Subject: [PATCH 60/70] fix(system_error_monitor): fix shadowVariable (#7979) * fix:shadowVariable Signed-off-by: kobayu858 * fix:shadowVariable Signed-off-by: kobayu858 * fix:syntax error Signed-off-by: kobayu858 * refactor:clang format Signed-off-by: kobayu858 --------- Signed-off-by: kobayu858 --- .../system_error_monitor/src/system_error_monitor_core.cpp | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) diff --git a/system/system_error_monitor/src/system_error_monitor_core.cpp b/system/system_error_monitor/src/system_error_monitor_core.cpp index 1daa4624ec934..ef892bd01ff50 100644 --- a/system/system_error_monitor/src/system_error_monitor_core.cpp +++ b/system/system_error_monitor/src/system_error_monitor_core.cpp @@ -270,13 +270,12 @@ AutowareErrorMonitor::AutowareErrorMonitor(const rclcpp::NodeOptions & options) void AutowareErrorMonitor::loadRequiredModules(const std::string & key) { - const auto param_key = std::string("required_modules.") + key; - const uint64_t depth = 3; - const auto param_names = this->list_parameters({param_key}, depth).names; + const auto param_names = + this->list_parameters({std::string("required_modules.") + key}, depth).names; if (param_names.empty()) { - throw std::runtime_error(fmt::format("no parameter found: {}", param_key)); + throw std::runtime_error(fmt::format("no parameter found: required_modules.{}", key)); } // Load module names from parameter key From 7de0ec0e024d867b564fdc1df08bd7efdcd13ac4 Mon Sep 17 00:00:00 2001 From: Yukinari Hisaki <42021302+yhisaki@users.noreply.github.com> Date: Fri, 12 Jul 2024 16:26:25 +0900 Subject: [PATCH 61/70] chore(autoware_universe_utils): update document (#7907) * update readme Signed-off-by: Y.Hisaki * refactoring Signed-off-by: Y.Hisaki * remove string reporter Signed-off-by: Y.Hisaki * fix readme.md Signed-off-by: Y.Hisaki * change node name of example Signed-off-by: Y.Hisaki * update readme Signed-off-by: Y.Hisaki --------- Signed-off-by: Y.Hisaki --- common/autoware_universe_utils/README.md | 223 ++++++------------ .../examples/example_scoped_time_track.cpp | 61 ----- .../examples/example_time_keeper.cpp | 78 +++--- .../universe_utils/system/time_keeper.hpp | 16 +- .../src/system/time_keeper.cpp | 14 +- 5 files changed, 127 insertions(+), 265 deletions(-) delete mode 100644 common/autoware_universe_utils/examples/example_scoped_time_track.cpp diff --git a/common/autoware_universe_utils/README.md b/common/autoware_universe_utils/README.md index 2d24f84423299..cd5e366c520fc 100644 --- a/common/autoware_universe_utils/README.md +++ b/common/autoware_universe_utils/README.md @@ -49,56 +49,80 @@ explicit TimeKeeper(Reporters... reporters); - Ends tracking the processing time of a function. - `func_name`: Name of the function to end tracking. +##### Note + +- It's possible to start and end time measurements using `start_track` and `end_track` as shown below: + + ```cpp + time_keeper.start_track("example_function"); + // Your function code here + time_keeper.end_track("example_function"); + ``` + +- For safety and to ensure proper tracking, it is recommended to use `ScopedTimeTrack`. + ##### Example ```cpp -#include "autoware/universe_utils/system/time_keeper.hpp" - #include +#include + +#include #include #include +#include + +class ExampleNode : public rclcpp::Node +{ +public: + ExampleNode() : Node("time_keeper_example") + { + publisher_ = + create_publisher("processing_time", 1); + + time_keeper_ = std::make_shared(publisher_, &std::cerr); + // You can also add a reporter later by add_reporter. + // time_keeper_->add_reporter(publisher_); + // time_keeper_->add_reporter(&std::cerr); + + timer_ = create_wall_timer(std::chrono::seconds(1), std::bind(&ExampleNode::func_a, this)); + } + +private: + std::shared_ptr time_keeper_; + rclcpp::Publisher::SharedPtr publisher_; + rclcpp::Publisher::SharedPtr publisher_str_; + rclcpp::TimerBase::SharedPtr timer_; + + void func_a() + { + // Start constructing ProcessingTimeTree (because func_a is the root function) + autoware::universe_utils::ScopedTimeTrack st("func_a", *time_keeper_); + std::this_thread::sleep_for(std::chrono::milliseconds(1)); + func_b(); + // End constructing ProcessingTimeTree. After this, the tree will be reported (publishing + // message and outputting to std::cerr) + } + + void func_b() + { + autoware::universe_utils::ScopedTimeTrack st("func_b", *time_keeper_); + std::this_thread::sleep_for(std::chrono::milliseconds(2)); + func_c(); + } + + void func_c() + { + autoware::universe_utils::ScopedTimeTrack st("func_c", *time_keeper_); + std::this_thread::sleep_for(std::chrono::milliseconds(3)); + } +}; int main(int argc, char ** argv) { rclcpp::init(argc, argv); - auto node = std::make_shared("time_keeper_example"); - - auto time_keeper = std::make_shared(); - - time_keeper->add_reporter(&std::cout); - - auto publisher = - node->create_publisher("processing_time", 10); - - time_keeper->add_reporter(publisher); - - auto publisher_str = node->create_publisher("processing_time_str", 10); - - time_keeper->add_reporter(publisher_str); - - auto funcA = [&time_keeper]() { - time_keeper->start_track("funcA"); - std::this_thread::sleep_for(std::chrono::seconds(1)); - time_keeper->end_track("funcA"); - }; - - auto funcB = [&time_keeper, &funcA]() { - time_keeper->start_track("funcB"); - std::this_thread::sleep_for(std::chrono::seconds(2)); - funcA(); - time_keeper->end_track("funcB"); - }; - - auto funcC = [&time_keeper, &funcB]() { - time_keeper->start_track("funcC"); - std::this_thread::sleep_for(std::chrono::seconds(3)); - funcB(); - time_keeper->end_track("funcC"); - }; - - funcC(); - + auto node = std::make_shared(); rclcpp::spin(node); rclcpp::shutdown(); return 0; @@ -109,38 +133,28 @@ int main(int argc, char ** argv) ```text ========================== - funcC (6000.7ms) - └── funcB (3000.44ms) - └── funcA (1000.19ms) + func_a (6.382ms) + └── func_b (5.243ms) + └── func_c (3.146ms) ``` - Output (`ros2 topic echo /processing_time`) ```text + --- nodes: - id: 1 - name: funcC - processing_time: 6000.659 + name: func_a + processing_time: 6.397 parent_id: 0 - id: 2 - name: funcB - processing_time: 3000.415 + name: func_b + processing_time: 5.263 parent_id: 1 - id: 3 - name: funcA - processing_time: 1000.181 + name: func_c + processing_time: 3.129 parent_id: 2 - --- - ``` - -- Output (`ros2 topic echo /processing_time_str --field data`) - - ```text - funcC (6000.67ms) - └── funcB (3000.42ms) - └── funcA (1000.19ms) - - --- ``` #### `autoware::universe_utils::ScopedTimeTrack` @@ -165,94 +179,3 @@ ScopedTimeTrack(const std::string & func_name, TimeKeeper & time_keeper); ``` - Destroys the `ScopedTimeTrack` object, ending the tracking of the function. - -##### Example - -```cpp -#include "autoware/universe_utils/system/time_keeper.hpp" - -#include - -#include -#include - -int main(int argc, char ** argv) -{ - rclcpp::init(argc, argv); - auto node = std::make_shared("scoped_time_track_example"); - - auto time_keeper = std::make_shared(); - - time_keeper->add_reporter(&std::cout); - - auto publisher = - node->create_publisher("processing_time", 10); - - time_keeper->add_reporter(publisher); - - auto publisher_str = node->create_publisher("processing_time_str", 10); - - time_keeper->add_reporter(publisher_str); - - auto funcA = [&time_keeper]() { - autoware::universe_utils::ScopedTimeTrack st("funcA", *time_keeper); - std::this_thread::sleep_for(std::chrono::seconds(1)); - }; - - auto funcB = [&time_keeper, &funcA]() { - autoware::universe_utils::ScopedTimeTrack st("funcB", *time_keeper); - std::this_thread::sleep_for(std::chrono::seconds(2)); - funcA(); - }; - - auto funcC = [&time_keeper, &funcB]() { - autoware::universe_utils::ScopedTimeTrack st("funcC", *time_keeper); - std::this_thread::sleep_for(std::chrono::seconds(3)); - funcB(); - }; - - funcC(); - - rclcpp::spin(node); - rclcpp::shutdown(); - return 0; -} -``` - -- Output (console) - - ```text - ========================== - funcC (6000.7ms) - └── funcB (3000.44ms) - └── funcA (1000.19ms) - ``` - -- Output (`ros2 topic echo /processing_time`) - - ```text - nodes: - - id: 1 - name: funcC - processing_time: 6000.659 - parent_id: 0 - - id: 2 - name: funcB - processing_time: 3000.415 - parent_id: 1 - - id: 3 - name: funcA - processing_time: 1000.181 - parent_id: 2 - --- - ``` - -- Output (`ros2 topic echo /processing_time_str --field data`) - - ```text - funcC (6000.67ms) - └── funcB (3000.42ms) - └── funcA (1000.19ms) - - --- - ``` diff --git a/common/autoware_universe_utils/examples/example_scoped_time_track.cpp b/common/autoware_universe_utils/examples/example_scoped_time_track.cpp deleted file mode 100644 index 010cc58aba8ae..0000000000000 --- a/common/autoware_universe_utils/examples/example_scoped_time_track.cpp +++ /dev/null @@ -1,61 +0,0 @@ -// 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 "autoware/universe_utils/system/time_keeper.hpp" - -#include - -#include -#include - -int main(int argc, char ** argv) -{ - rclcpp::init(argc, argv); - auto node = std::make_shared("scoped_time_track_example"); - - auto time_keeper = std::make_shared(); - - time_keeper->add_reporter(&std::cout); - - auto publisher = - node->create_publisher("processing_time", 10); - - time_keeper->add_reporter(publisher); - - auto publisher_str = node->create_publisher("processing_time_str", 10); - - time_keeper->add_reporter(publisher_str); - - auto funcA = [&time_keeper]() { - autoware::universe_utils::ScopedTimeTrack st("funcA", *time_keeper); - std::this_thread::sleep_for(std::chrono::seconds(1)); - }; - - auto funcB = [&time_keeper, &funcA]() { - autoware::universe_utils::ScopedTimeTrack st("funcB", *time_keeper); - std::this_thread::sleep_for(std::chrono::seconds(2)); - funcA(); - }; - - auto funcC = [&time_keeper, &funcB]() { - autoware::universe_utils::ScopedTimeTrack st("funcC", *time_keeper); - std::this_thread::sleep_for(std::chrono::seconds(3)); - funcB(); - }; - - funcC(); - - rclcpp::spin(node); - rclcpp::shutdown(); - return 0; -} diff --git a/common/autoware_universe_utils/examples/example_time_keeper.cpp b/common/autoware_universe_utils/examples/example_time_keeper.cpp index 3f6037e68daac..c50aa1e1a8342 100644 --- a/common/autoware_universe_utils/examples/example_time_keeper.cpp +++ b/common/autoware_universe_utils/examples/example_time_keeper.cpp @@ -15,49 +15,63 @@ #include +#include + +#include #include #include +#include -int main(int argc, char ** argv) +class ExampleNode : public rclcpp::Node { - rclcpp::init(argc, argv); - auto node = std::make_shared("time_keeper_example"); - - auto time_keeper = std::make_shared(); - - time_keeper->add_reporter(&std::cout); +public: + ExampleNode() : Node("time_keeper_example") + { + publisher_ = + create_publisher("processing_time", 1); - auto publisher = - node->create_publisher("processing_time", 10); + time_keeper_ = std::make_shared(publisher_, &std::cerr); + // You can also add a reporter later by add_reporter. + // time_keeper_->add_reporter(publisher_); + // time_keeper_->add_reporter(&std::cerr); - time_keeper->add_reporter(publisher); + timer_ = create_wall_timer(std::chrono::seconds(1), std::bind(&ExampleNode::func_a, this)); + } - auto publisher_str = node->create_publisher("processing_time_str", 10); +private: + std::shared_ptr time_keeper_; + rclcpp::Publisher::SharedPtr publisher_; + rclcpp::Publisher::SharedPtr publisher_str_; + rclcpp::TimerBase::SharedPtr timer_; - time_keeper->add_reporter(publisher_str); + void func_a() + { + // Start constructing ProcessingTimeTree (because func_a is the root function) + autoware::universe_utils::ScopedTimeTrack st("func_a", *time_keeper_); + std::this_thread::sleep_for(std::chrono::milliseconds(1)); + func_b(); + // End constructing ProcessingTimeTree. After this, the tree will be reported (publishing + // message and outputting to std::cerr) + } - auto funcA = [&time_keeper]() { - time_keeper->start_track("funcA"); - std::this_thread::sleep_for(std::chrono::seconds(1)); - time_keeper->end_track("funcA"); - }; + void func_b() + { + autoware::universe_utils::ScopedTimeTrack st("func_b", *time_keeper_); + std::this_thread::sleep_for(std::chrono::milliseconds(2)); + func_c(); + } - auto funcB = [&time_keeper, &funcA]() { - time_keeper->start_track("funcB"); - std::this_thread::sleep_for(std::chrono::seconds(2)); - funcA(); - time_keeper->end_track("funcB"); - }; - - auto funcC = [&time_keeper, &funcB]() { - time_keeper->start_track("funcC"); - std::this_thread::sleep_for(std::chrono::seconds(3)); - funcB(); - time_keeper->end_track("funcC"); - }; - - funcC(); + void func_c() + { + autoware::universe_utils::ScopedTimeTrack st("func_c", *time_keeper_); + std::this_thread::sleep_for(std::chrono::milliseconds(3)); + } +}; +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + auto node = std::make_shared(); rclcpp::spin(node); rclcpp::shutdown(); return 0; diff --git a/common/autoware_universe_utils/include/autoware/universe_utils/system/time_keeper.hpp b/common/autoware_universe_utils/include/autoware/universe_utils/system/time_keeper.hpp index 96070f0f30b77..c635138416aa5 100644 --- a/common/autoware_universe_utils/include/autoware/universe_utils/system/time_keeper.hpp +++ b/common/autoware_universe_utils/include/autoware/universe_utils/system/time_keeper.hpp @@ -17,14 +17,11 @@ #include "autoware/universe_utils/system/stop_watch.hpp" #include -#include #include #include #include -#include - #include #include #include @@ -134,14 +131,6 @@ class TimeKeeper */ void add_reporter(rclcpp::Publisher::SharedPtr publisher); - /** - * @brief Add a reporter to publish processing times to an rclcpp publisher with - * std_msgs::msg::String - * - * @param publisher Shared pointer to the rclcpp publisher - */ - void add_reporter(rclcpp::Publisher::SharedPtr publisher); - /** * @brief Start tracking the processing time of a function * @@ -187,6 +176,11 @@ class ScopedTimeTrack */ ScopedTimeTrack(const std::string & func_name, TimeKeeper & time_keeper); + ScopedTimeTrack(const ScopedTimeTrack &) = delete; + ScopedTimeTrack & operator=(const ScopedTimeTrack &) = delete; + ScopedTimeTrack(ScopedTimeTrack &&) = delete; + ScopedTimeTrack & operator=(ScopedTimeTrack &&) = delete; + /** * @brief Destroy the ScopedTimeTrack object, ending the tracking of the function */ diff --git a/common/autoware_universe_utils/src/system/time_keeper.cpp b/common/autoware_universe_utils/src/system/time_keeper.cpp index 58bed6677227c..429f063dfc62e 100644 --- a/common/autoware_universe_utils/src/system/time_keeper.cpp +++ b/common/autoware_universe_utils/src/system/time_keeper.cpp @@ -14,7 +14,8 @@ #include "autoware/universe_utils/system/time_keeper.hpp" -#include +#include + #include namespace autoware::universe_utils @@ -67,7 +68,7 @@ tier4_debug_msgs::msg::ProcessingTimeTree ProcessingTimeNode::to_msg() const tier4_debug_msgs::msg::ProcessingTimeNode time_node_msg; time_node_msg.name = node.name_; time_node_msg.processing_time = node.processing_time_; - time_node_msg.id = tree_msg.nodes.size() + 1; + time_node_msg.id = static_cast(tree_msg.nodes.size() + 1); time_node_msg.parent_id = parent_id; tree_msg.nodes.emplace_back(time_node_msg); @@ -112,15 +113,6 @@ void TimeKeeper::add_reporter(rclcpp::Publisher::SharedPtr }); } -void TimeKeeper::add_reporter(rclcpp::Publisher::SharedPtr publisher) -{ - reporters_.emplace_back([publisher](const std::shared_ptr & node) { - std_msgs::msg::String msg; - msg.data = node->to_string(); - publisher->publish(msg); - }); -} - void TimeKeeper::start_track(const std::string & func_name) { if (current_time_node_ == nullptr) { From 19c1d0379942ba3a50907f381c38531ce6a494c3 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?M=2E=20Fatih=20C=C4=B1r=C4=B1t?= Date: Fri, 12 Jul 2024 12:14:50 +0300 Subject: [PATCH 62/70] ci(build-and-test-differential): limit ccache size for cuda builds (#7980) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: M. Fatih Cırıt --- .github/workflows/build-and-test-differential.yaml | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/.github/workflows/build-and-test-differential.yaml b/.github/workflows/build-and-test-differential.yaml index a95a070d19bd4..287769d0708ac 100644 --- a/.github/workflows/build-and-test-differential.yaml +++ b/.github/workflows/build-and-test-differential.yaml @@ -70,6 +70,14 @@ jobs: restore-keys: | ccache-main-${{ runner.arch }}-${{ matrix.rosdistro }}- + # Limit ccache size only for CUDA builds to avoid running out of disk space + - name: Limit ccache size + if: ${{ matrix.container-suffix == '-cuda' }} + run: | + rm -f "${CCACHE_DIR}/ccache.conf" + echo -e "# Set maximum cache size\nmax_size = 1MB" >> "${CCACHE_DIR}/ccache.conf" + shell: bash + - name: Show ccache stats before build run: du -sh ${CCACHE_DIR} && ccache -s shell: bash From 511e5c7ebcb513bb7d1b0f0aed4ee2dc53a16023 Mon Sep 17 00:00:00 2001 From: kobayu858 <129580202+kobayu858@users.noreply.github.com> Date: Fri, 12 Jul 2024 18:46:52 +0900 Subject: [PATCH 63/70] fix(autoware_stop_filter): fix funcArgNamesDifferent (#8008) fix:funcArgNamesDifferent Signed-off-by: kobayu858 --- localization/autoware_stop_filter/src/stop_filter.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/localization/autoware_stop_filter/src/stop_filter.hpp b/localization/autoware_stop_filter/src/stop_filter.hpp index ce6e67f0b7265..b6d56b5f77059 100644 --- a/localization/autoware_stop_filter/src/stop_filter.hpp +++ b/localization/autoware_stop_filter/src/stop_filter.hpp @@ -39,7 +39,7 @@ namespace autoware::stop_filter class StopFilter : public rclcpp::Node { public: - explicit StopFilter(const rclcpp::NodeOptions & options); + explicit StopFilter(const rclcpp::NodeOptions & node_options); private: rclcpp::Publisher::SharedPtr pub_odom_; //!< @brief odom publisher From f1dff450b0d4c0951f31afe8c7b69ab0a23963e6 Mon Sep 17 00:00:00 2001 From: kobayu858 <129580202+kobayu858@users.noreply.github.com> Date: Fri, 12 Jul 2024 19:16:12 +0900 Subject: [PATCH 64/70] fix(detection_by_tracker): fix funcArgNamesDifferent (#8010) fix:funcArgNamesDifferent Signed-off-by: kobayu858 --- .../detection_by_tracker/src/detection_by_tracker_node.hpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/perception/detection_by_tracker/src/detection_by_tracker_node.hpp b/perception/detection_by_tracker/src/detection_by_tracker_node.hpp index 9c974d72cfdca..f0ed51dd1938e 100644 --- a/perception/detection_by_tracker/src/detection_by_tracker_node.hpp +++ b/perception/detection_by_tracker/src/detection_by_tracker_node.hpp @@ -85,7 +85,7 @@ class DetectionByTracker : public rclcpp::Node void divideUnderSegmentedObjects( const autoware_perception_msgs::msg::DetectedObjects & tracked_objects, - const tier4_perception_msgs::msg::DetectedObjectsWithFeature & in_objects, + const tier4_perception_msgs::msg::DetectedObjectsWithFeature & in_cluster_objects, autoware_perception_msgs::msg::DetectedObjects & out_no_found_tracked_objects, tier4_perception_msgs::msg::DetectedObjectsWithFeature & out_objects); @@ -96,7 +96,7 @@ class DetectionByTracker : public rclcpp::Node void mergeOverSegmentedObjects( const autoware_perception_msgs::msg::DetectedObjects & tracked_objects, - const tier4_perception_msgs::msg::DetectedObjectsWithFeature & in_objects, + const tier4_perception_msgs::msg::DetectedObjectsWithFeature & in_cluster_objects, autoware_perception_msgs::msg::DetectedObjects & out_no_found_tracked_objects, tier4_perception_msgs::msg::DetectedObjectsWithFeature & out_objects); }; From 771f71d21d2c0fdcea7fb84ec7c6551c025c65de Mon Sep 17 00:00:00 2001 From: Yukinari Hisaki <42021302+yhisaki@users.noreply.github.com> Date: Fri, 12 Jul 2024 20:06:30 +0900 Subject: [PATCH 65/70] feat(autoware_behavior_path_planner_common,autoware_behavior_path_lane_change_module): add time_keeper to bpp (#8004) * feat(autoware_behavior_path_planner_common,autoware_behavior_path_lane_change_module): add time_keeper to bpp Signed-off-by: Y.Hisaki * update Signed-off-by: Y.Hisaki --------- Signed-off-by: Y.Hisaki --- .../interface.hpp | 2 ++ .../utils/base_class.hpp | 11 ++++++++++- .../src/interface.cpp | 6 ++++++ .../src/scene.cpp | 17 +++++++++++++++++ .../behavior_path_planner/planner_manager.hpp | 2 ++ .../interface/scene_module_interface.hpp | 8 +++++++- .../scene_module_manager_interface.hpp | 5 +++++ 7 files changed, 49 insertions(+), 2 deletions(-) diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/interface.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/interface.hpp index 9c014b77a7e82..bd309dd35a260 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/interface.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/interface.hpp @@ -23,6 +23,7 @@ #include "autoware/behavior_path_planner_common/turn_signal_decider.hpp" #include "autoware/behavior_path_planner_common/utils/path_shifter/path_shifter.hpp" +#include #include #include @@ -108,6 +109,7 @@ class LaneChangeInterface : public SceneModuleInterface const double start_distance, const double finish_distance, const bool safe, const uint8_t & state) { + universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); for (const auto & [module_name, ptr] : rtc_interface_ptr_map_) { if (ptr) { ptr->updateCooperateStatus( diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/base_class.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/base_class.hpp index 9df19c4de6834..cbd3a81827948 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/base_class.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/base_class.hpp @@ -23,6 +23,7 @@ #include "autoware/behavior_path_planner_common/utils/path_shifter/path_shifter.hpp" #include "autoware/universe_utils/system/stop_watch.hpp" +#include #include #include @@ -53,7 +54,8 @@ class LaneChangeBase : lane_change_parameters_{std::move(parameters)}, common_data_ptr_{std::make_shared()}, direction_{direction}, - type_{type} + type_{type}, + time_keeper_(std::make_shared()) { } @@ -167,6 +169,11 @@ class LaneChangeBase common_data_ptr_->direction = direction_; } + void setTimeKeeper(const std::shared_ptr & time_keeper) + { + time_keeper_ = time_keeper; + } + void toNormalState() { current_lane_change_state_ = LaneChangeStates::Normal; } void toStopState() { current_lane_change_state_ = LaneChangeStates::Stop; } @@ -252,6 +259,8 @@ class LaneChangeBase rclcpp::Logger logger_ = utils::lane_change::getLogger(getModuleTypeStr()); mutable rclcpp::Clock clock_{RCL_ROS_TIME}; + + mutable std::shared_ptr time_keeper_; }; } // namespace autoware::behavior_path_planner #endif // AUTOWARE__BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__BASE_CLASS_HPP_ diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/interface.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/interface.cpp index 0a5b31f9e32d2..b55b41828081e 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/interface.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/interface.cpp @@ -21,6 +21,7 @@ #include "autoware/behavior_path_planner_common/marker_utils/utils.hpp" #include +#include #include #include @@ -43,6 +44,7 @@ LaneChangeInterface::LaneChangeInterface( module_type_{std::move(module_type)}, prev_approved_path_{std::make_unique()} { + module_type_->setTimeKeeper(getTimeKeeper()); steering_factor_interface_ptr_ = std::make_unique(&node, name); logger_ = utils::lane_change::getLogger(module_type_->getModuleTypeStr()); } @@ -71,6 +73,7 @@ bool LaneChangeInterface::isExecutionReady() const void LaneChangeInterface::updateData() { + universe_utils::ScopedTimeTrack st(__func__, *getTimeKeeper()); module_type_->setPreviousModuleOutput(getPreviousModuleOutput()); module_type_->updateSpecialData(); @@ -94,6 +97,7 @@ void LaneChangeInterface::postProcess() BehaviorModuleOutput LaneChangeInterface::plan() { + universe_utils::ScopedTimeTrack st(__func__, *getTimeKeeper()); resetPathCandidate(); resetPathReference(); @@ -165,6 +169,7 @@ BehaviorModuleOutput LaneChangeInterface::planWaitingApproval() CandidateOutput LaneChangeInterface::planCandidate() const { + universe_utils::ScopedTimeTrack st(__func__, *getTimeKeeper()); const auto selected_path = module_type_->getLaneChangePath(); if (selected_path.path.points.empty()) { @@ -340,6 +345,7 @@ MarkerArray LaneChangeInterface::getModuleVirtualWall() void LaneChangeInterface::updateSteeringFactorPtr(const BehaviorModuleOutput & output) { + universe_utils::ScopedTimeTrack st(__func__, *getTimeKeeper()); const auto steering_factor_direction = std::invoke([&]() { if (module_type_->getDirection() == Direction::LEFT) { return SteeringFactor::LEFT; diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp index 8cf67f8cfc66f..8636ff7d9dae7 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp @@ -23,6 +23,7 @@ #include "autoware/behavior_path_planner_common/utils/utils.hpp" #include +#include #include #include @@ -55,6 +56,7 @@ NormalLaneChange::NormalLaneChange( void NormalLaneChange::updateLaneChangeStatus() { + universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); updateStopTime(); const auto [found_valid_path, found_safe_path] = getSafePath(status_.lane_change_path); @@ -263,6 +265,7 @@ BehaviorModuleOutput NormalLaneChange::getTerminalLaneChangePath() const BehaviorModuleOutput NormalLaneChange::generateOutput() { + universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); if (!status_.is_valid_path) { RCLCPP_DEBUG(logger_, "No valid path found. Returning previous module's path as output."); return prev_module_output_; @@ -308,6 +311,7 @@ BehaviorModuleOutput NormalLaneChange::generateOutput() void NormalLaneChange::extendOutputDrivableArea(BehaviorModuleOutput & output) const { + universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); const auto & dp = planner_data_->drivable_area_expansion_parameters; const auto drivable_lanes = utils::lane_change::generateDrivableLanes( @@ -327,6 +331,7 @@ void NormalLaneChange::extendOutputDrivableArea(BehaviorModuleOutput & output) c void NormalLaneChange::insertStopPoint( const lanelet::ConstLanelets & lanelets, PathWithLaneId & path) { + universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); if (lanelets.empty()) { return; } @@ -468,6 +473,7 @@ void NormalLaneChange::insertStopPoint( PathWithLaneId NormalLaneChange::getReferencePath() const { + universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); lanelet::ConstLanelet closest_lanelet; if (!lanelet::utils::query::getClosestLanelet( status_.lane_change_path.info.target_lanes, getEgoPose(), &closest_lanelet)) { @@ -482,6 +488,7 @@ PathWithLaneId NormalLaneChange::getReferencePath() const std::optional NormalLaneChange::extendPath() { + universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); const auto path = status_.lane_change_path.path; const auto lc_start_point = status_.lane_change_path.info.lane_changing_start.position; @@ -560,6 +567,7 @@ void NormalLaneChange::resetParameters() TurnSignalInfo NormalLaneChange::updateOutputTurnSignal() const { + universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); const auto & pose = getEgoPose(); const auto & current_lanes = status_.current_lanes; const auto & shift_line = status_.lane_change_path.info.shift_line; @@ -584,6 +592,7 @@ lanelet::ConstLanelets NormalLaneChange::getCurrentLanes() const lanelet::ConstLanelets NormalLaneChange::getLaneChangeLanes( const lanelet::ConstLanelets & current_lanes, Direction direction) const { + universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); if (current_lanes.empty()) { return {}; } @@ -1832,6 +1841,7 @@ PathSafetyStatus NormalLaneChange::evaluateApprovedPathWithUnsafeHysteresis( bool NormalLaneChange::isValidPath(const PathWithLaneId & path) const { + universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); const auto & route_handler = planner_data_->route_handler; const auto & dp = planner_data_->drivable_area_expansion_parameters; @@ -1869,6 +1879,7 @@ bool NormalLaneChange::isValidPath(const PathWithLaneId & path) const bool NormalLaneChange::isRequiredStop(const bool is_object_coming_from_rear) { + universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); const auto threshold = lane_change_parameters_->backward_length_buffer_for_end_of_lane; if ( isNearEndOfCurrentLanes(status_.current_lanes, status_.target_lanes, threshold) && @@ -1882,6 +1893,7 @@ bool NormalLaneChange::isRequiredStop(const bool is_object_coming_from_rear) bool NormalLaneChange::calcAbortPath() { + universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); const auto & route_handler = getRouteHandler(); const auto & common_param = getCommonParam(); const auto current_velocity = @@ -2025,6 +2037,7 @@ PathSafetyStatus NormalLaneChange::isLaneChangePathSafe( const utils::path_safety_checker::RSSparams & rss_params, CollisionCheckDebugMap & debug_data) const { + universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); PathSafetyStatus path_safety_status; if (collision_check_objects.empty()) { @@ -2108,6 +2121,7 @@ PathSafetyStatus NormalLaneChange::isLaneChangePathSafe( bool NormalLaneChange::isVehicleStuck( const lanelet::ConstLanelets & current_lanes, const double obstacle_check_distance) const { + universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); // Ego is still moving, not in stuck if (std::abs(getEgoVelocity()) > lane_change_parameters_->stop_velocity_threshold) { RCLCPP_DEBUG(logger_, "Ego is still moving, not in stuck"); @@ -2163,6 +2177,7 @@ bool NormalLaneChange::isVehicleStuck( double NormalLaneChange::get_max_velocity_for_safety_check() const { + universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); const auto external_velocity_limit_ptr = planner_data_->external_limit_max_velocity; if (external_velocity_limit_ptr) { return std::min( @@ -2174,6 +2189,7 @@ double NormalLaneChange::get_max_velocity_for_safety_check() const bool NormalLaneChange::isVehicleStuck(const lanelet::ConstLanelets & current_lanes) const { + universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); if (current_lanes.empty()) { lane_change_debug_.is_stuck = false; return false; // can not check @@ -2208,6 +2224,7 @@ void NormalLaneChange::setStopPose(const Pose & stop_pose) void NormalLaneChange::updateStopTime() { + universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); const auto current_vel = getEgoVelocity(); if (std::abs(current_vel) > lane_change_parameters_->stop_velocity_threshold) { diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner/include/autoware/behavior_path_planner/planner_manager.hpp b/planning/behavior_path_planner/autoware_behavior_path_planner/include/autoware/behavior_path_planner/planner_manager.hpp index a76284b70b7d8..3a9aa9cafbcca 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner/include/autoware/behavior_path_planner/planner_manager.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner/include/autoware/behavior_path_planner/planner_manager.hpp @@ -21,6 +21,7 @@ #include "autoware/universe_utils/ros/debug_publisher.hpp" #include "autoware/universe_utils/system/stop_watch.hpp" +#include #include #include @@ -277,6 +278,7 @@ class PlannerManager const SceneModulePtr & module_ptr, const std::shared_ptr & planner_data, const BehaviorModuleOutput & previous_module_output) const { + universe_utils::ScopedTimeTrack st(module_ptr->name() + "=>run", *module_ptr->getTimeKeeper()); stop_watch_.tic(module_ptr->name()); module_ptr->setData(planner_data); diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/interface/scene_module_interface.hpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/interface/scene_module_interface.hpp index d11c3181526d2..95eb82e0b4c23 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/interface/scene_module_interface.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/interface/scene_module_interface.hpp @@ -31,6 +31,7 @@ #include #include #include +#include #include #include @@ -97,7 +98,8 @@ class SceneModuleInterface objects_of_interest_marker_interface_ptr_map_( std::move(objects_of_interest_marker_interface_ptr_map)), steering_factor_interface_ptr_( - std::make_unique(&node, utils::convertToSnakeCase(name))) + std::make_unique(&node, utils::convertToSnakeCase(name))), + time_keeper_(std::make_shared()) { for (const auto & [module_name, ptr] : rtc_interface_ptr_map_) { uuid_map_.emplace(module_name, generateUUID()); @@ -244,6 +246,8 @@ class SceneModuleInterface previous_module_output_ = previous_module_output; } + std::shared_ptr getTimeKeeper() const { return time_keeper_; } + /** * @brief set planner data */ @@ -641,6 +645,8 @@ class SceneModuleInterface mutable MarkerArray debug_marker_; mutable MarkerArray drivable_lanes_marker_; + + mutable std::shared_ptr time_keeper_; }; } // namespace autoware::behavior_path_planner diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/interface/scene_module_manager_interface.hpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/interface/scene_module_manager_interface.hpp index fab9a6ba45113..c9adf4d60c21f 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/interface/scene_module_manager_interface.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/interface/scene_module_manager_interface.hpp @@ -85,6 +85,7 @@ class SceneModuleManagerInterface observer.lock()->setData(planner_data_); observer.lock()->setPreviousModuleOutput(previous_module_output); + observer.lock()->getTimeKeeper()->add_reporter(this->pub_processing_time_); observer.lock()->onEntry(); observers_.push_back(observer); @@ -318,6 +319,8 @@ class SceneModuleManagerInterface pub_debug_marker_ = node->create_publisher("~/debug/" + name_, 20); pub_virtual_wall_ = node->create_publisher("~/virtual_wall/" + name_, 20); pub_drivable_lanes_ = node->create_publisher("~/drivable_lanes/" + name_, 20); + pub_processing_time_ = node->create_publisher( + "~/processing_time/" + name_, 20); } // misc @@ -338,6 +341,8 @@ class SceneModuleManagerInterface rclcpp::Publisher::SharedPtr pub_drivable_lanes_; + rclcpp::Publisher::SharedPtr pub_processing_time_; + std::string name_; std::shared_ptr planner_data_; From d571fc4cad6061d95809fb8b4d242c5383d53070 Mon Sep 17 00:00:00 2001 From: Zulfaqar Azmi <93502286+zulfaqar-azmi-t4@users.noreply.github.com> Date: Fri, 12 Jul 2024 22:32:34 +0900 Subject: [PATCH 66/70] refactor(lane_change): update lanes and its polygons only when it's updated (#7989) * refactor(lane_change): compute lanes and polygon only when updated Signed-off-by: Zulfaqar Azmi * Revert accidental changesd This reverts commit cbfd9ae8a88b2d6c3b27b35c9a08bb824ecd5011. Signed-off-by: Zulfaqar Azmi * fix spell check Signed-off-by: Zulfaqar Azmi * Make a common getter for current lanes Signed-off-by: Zulfaqar Azmi * add target lanes getter Signed-off-by: Zulfaqar Azmi * some minor function refactoring Signed-off-by: Zulfaqar Azmi --------- Signed-off-by: Zulfaqar Azmi --- .../src/scene.cpp | 4 +- .../scene.hpp | 13 +- .../utils/base_class.hpp | 19 +- .../utils/data_structs.hpp | 40 ++-- .../utils/path.hpp | 24 +- .../utils/utils.hpp | 22 +- .../src/interface.cpp | 3 +- .../src/scene.cpp | 217 ++++++++++-------- .../src/utils/utils.cpp | 76 ++++-- 9 files changed, 246 insertions(+), 172 deletions(-) diff --git a/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/scene.cpp b/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/scene.cpp index 501554b0a2b02..1a2e4439d4b7d 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/scene.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/scene.cpp @@ -134,7 +134,7 @@ AvoidancePlanningData AvoidanceByLaneChange::calcAvoidancePlanningData( const auto resample_interval = avoidance_parameters_->resample_interval_for_planning; data.reference_path = utils::resamplePathWithSpline(data.reference_path_rough, resample_interval); - data.current_lanelets = getCurrentLanes(); + data.current_lanelets = get_current_lanes(); fillAvoidanceTargetObjects(data, debug); @@ -274,7 +274,7 @@ double AvoidanceByLaneChange::calcMinAvoidanceLength(const ObjectData & nearest_ double AvoidanceByLaneChange::calcMinimumLaneChangeLength() const { - const auto current_lanes = getCurrentLanes(); + const auto current_lanes = get_current_lanes(); if (current_lanes.empty()) { RCLCPP_DEBUG(logger_, "no empty lanes"); return std::numeric_limits::infinity(); diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/scene.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/scene.hpp index be62492b7c2cc..dad96d5f7371a 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/scene.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/scene.hpp @@ -51,6 +51,8 @@ class NormalLaneChange : public LaneChangeBase NormalLaneChange & operator=(NormalLaneChange &&) = delete; ~NormalLaneChange() override = default; + void update_lanes(const bool is_approved) final; + void updateLaneChangeStatus() override; std::pair getSafePath(LaneChangePath & safe_path) const override; @@ -105,8 +107,6 @@ class NormalLaneChange : public LaneChangeBase TurnSignalInfo get_current_turn_signal_info() override; protected: - lanelet::ConstLanelets getCurrentLanes() const override; - lanelet::ConstLanelets getLaneChangeLanes( const lanelet::ConstLanelets & current_lanes, Direction direction) const override; @@ -124,9 +124,7 @@ class NormalLaneChange : public LaneChangeBase const LaneChangeLanesFilteredObjects & predicted_objects, const lanelet::ConstLanelets & current_lanes) const; - LaneChangeLanesFilteredObjects filterObjects( - const lanelet::ConstLanelets & current_lanes, - const lanelet::ConstLanelets & target_lanes) const; + LaneChangeLanesFilteredObjects filterObjects() const; void filterOncomingObjects(PredictedObjects & objects) const; @@ -203,6 +201,11 @@ class NormalLaneChange : public LaneChangeBase double getStopTime() const { return stop_time_; } + const lanelet::ConstLanelets & get_target_lanes() const + { + return common_data_ptr_->lanes_ptr->target; + } + double stop_time_{0.0}; }; } // namespace autoware::behavior_path_planner diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/base_class.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/base_class.hpp index cbd3a81827948..c1cc14d98a7c7 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/base_class.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/base_class.hpp @@ -65,6 +65,8 @@ class LaneChangeBase LaneChangeBase & operator=(LaneChangeBase &&) = delete; virtual ~LaneChangeBase() = default; + virtual void update_lanes(const bool is_approved) = 0; + virtual void updateLaneChangeStatus() = 0; virtual std::pair getSafePath(LaneChangePath & safe_path) const = 0; @@ -139,6 +141,11 @@ class LaneChangeBase const Twist & getEgoTwist() const { return planner_data_->self_odometry->twist.twist; } + const lanelet::ConstLanelets & get_current_lanes() const + { + return common_data_ptr_->lanes_ptr->current; + } + const BehaviorPathPlannerParameters & getCommonParam() const { return planner_data_->parameters; } LaneChangeParameters getLaneChangeParam() const { return *lane_change_parameters_; } @@ -163,9 +170,19 @@ class LaneChangeBase common_data_ptr_->bpp_param_ptr = std::make_shared(data->parameters); } + + if (!common_data_ptr_->lanes_ptr) { + common_data_ptr_->lanes_ptr = std::make_shared(); + } + + if (!common_data_ptr_->lanes_polygon_ptr) { + common_data_ptr_->lanes_polygon_ptr = std::make_shared(); + } + common_data_ptr_->self_odometry_ptr = data->self_odometry; common_data_ptr_->route_handler_ptr = data->route_handler; common_data_ptr_->lc_param_ptr = lane_change_parameters_; + common_data_ptr_->lc_type = type_; common_data_ptr_->direction = direction_; } @@ -211,8 +228,6 @@ class LaneChangeBase virtual TurnSignalInfo get_current_turn_signal_info() = 0; protected: - virtual lanelet::ConstLanelets getCurrentLanes() const = 0; - virtual int getNumToPreferredLane(const lanelet::ConstLanelet & lane) const = 0; virtual PathWithLaneId getPrepareSegment( diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/data_structs.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/data_structs.hpp index 2fdf7c6b550a3..1bb4dfdeb59dc 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/data_structs.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/data_structs.hpp @@ -183,6 +183,13 @@ struct PhaseInfo } }; +struct Lanes +{ + lanelet::ConstLanelets current; + lanelet::ConstLanelets target; + std::vector preceding_target; +}; + struct Info { PhaseInfo longitudinal_acceleration{0.0, 0.0}; @@ -190,9 +197,6 @@ struct Info PhaseInfo duration{0.0, 0.0}; PhaseInfo length{0.0, 0.0}; - lanelet::ConstLanelets current_lanes{}; - lanelet::ConstLanelets target_lanes{}; - Pose lane_changing_start{}; Pose lane_changing_end{}; @@ -225,23 +229,26 @@ struct LanesPolygon { std::optional current; std::optional target; - std::vector target_backward; + std::optional expanded_target; + lanelet::BasicPolygon2d target_neighbor; + std::vector preceding_target; }; -struct Lanes -{ - lanelet::ConstLanelets current; - lanelet::ConstLanelets target; - std::vector preceding_target; -}; +using RouteHandlerPtr = std::shared_ptr; +using BppParamPtr = std::shared_ptr; +using LCParamPtr = std::shared_ptr; +using LanesPtr = std::shared_ptr; +using LanesPolygonPtr = std::shared_ptr; struct CommonData { - std::shared_ptr route_handler_ptr; + RouteHandlerPtr route_handler_ptr; Odometry::ConstSharedPtr self_odometry_ptr; - std::shared_ptr bpp_param_ptr; - std::shared_ptr lc_param_ptr; - Lanes lanes; + BppParamPtr bpp_param_ptr; + LCParamPtr lc_param_ptr; + LanesPtr lanes_ptr; + LanesPolygonPtr lanes_polygon_ptr; + ModuleType lc_type; Direction direction; [[nodiscard]] Pose get_ego_pose() const { return self_odometry_ptr->pose.pose; } @@ -259,12 +266,7 @@ struct CommonData return std::hypot(x, y); } }; - -using RouteHandlerPtr = std::shared_ptr; -using BppParamPtr = std::shared_ptr; -using LCParamPtr = std::shared_ptr; using CommonDataPtr = std::shared_ptr; -using LanesPtr = std::shared_ptr; } // namespace autoware::behavior_path_planner::lane_change namespace autoware::behavior_path_planner diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/path.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/path.hpp index 97b5c621deea5..5623f03a22eb3 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/path.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/path.hpp @@ -23,31 +23,31 @@ #include -namespace autoware::behavior_path_planner +namespace autoware::behavior_path_planner::lane_change { using autoware::behavior_path_planner::TurnSignalInfo; using tier4_planning_msgs::msg::PathWithLaneId; -struct LaneChangePath +struct Path { PathWithLaneId path{}; ShiftedPath shifted_path{}; - LaneChangeInfo info; - bool is_safe{false}; + Info info{}; }; -using LaneChangePaths = std::vector; -struct LaneChangeStatus +struct Status { - PathWithLaneId lane_follow_path{}; - LaneChangePath lane_change_path{}; - lanelet::ConstLanelets current_lanes{}; - lanelet::ConstLanelets target_lanes{}; - std::vector lane_follow_lane_ids{}; - std::vector lane_change_lane_ids{}; + Path lane_change_path{}; bool is_safe{false}; bool is_valid_path{false}; double start_distance{0.0}; }; +} // namespace autoware::behavior_path_planner::lane_change + +namespace autoware::behavior_path_planner +{ +using LaneChangePath = lane_change::Path; +using LaneChangePaths = std::vector; +using LaneChangeStatus = lane_change::Status; } // namespace autoware::behavior_path_planner #endif // AUTOWARE__BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__PATH_HPP_ diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/utils.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/utils.hpp index e25b67bdb73d8..17eaceb055fc1 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/utils.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/utils.hpp @@ -48,6 +48,7 @@ using autoware::universe_utils::Polygon2d; using autoware_perception_msgs::msg::PredictedObject; using autoware_perception_msgs::msg::PredictedObjects; using autoware_perception_msgs::msg::PredictedPath; +using behavior_path_planner::lane_change::CommonDataPtr; using behavior_path_planner::lane_change::LanesPolygon; using behavior_path_planner::lane_change::PathSafetyStatus; using geometry_msgs::msg::Point; @@ -114,8 +115,9 @@ bool isPathInLanelets( const lanelet::ConstLanelets & target_lanes); std::optional constructCandidatePath( - const LaneChangeInfo & lane_change_info, const PathWithLaneId & prepare_segment, - const PathWithLaneId & target_segment, const PathWithLaneId & target_lane_reference_path, + const CommonDataPtr & common_data_ptr, const LaneChangeInfo & lane_change_info, + const PathWithLaneId & prepare_segment, const PathWithLaneId & target_segment, + const PathWithLaneId & target_lane_reference_path, const std::vector> & sorted_lane_ids); ShiftLine getLaneChangingShiftLine( @@ -177,10 +179,9 @@ bool isParkedObject( const double ratio_threshold); bool passParkedObject( - const RouteHandler & route_handler, const LaneChangePath & lane_change_path, + const CommonDataPtr & common_data_ptr, const LaneChangePath & lane_change_path, const std::vector & objects, const double minimum_lane_change_length, - const bool is_goal_in_route, const LaneChangeParameters & lane_change_parameters, - CollisionCheckDebugMap & object_debug); + const bool is_goal_in_route, CollisionCheckDebugMap & object_debug); std::optional getLeadingStaticObjectIdx( const RouteHandler & route_handler, const LaneChangePath & lane_change_path, @@ -195,7 +196,8 @@ ExtendedPredictedObject transform( const LaneChangeParameters & lane_change_parameters, const bool check_at_prepare_phase); bool isCollidedPolygonsInLanelet( - const std::vector & collided_polygons, const lanelet::ConstLanelets & lanes); + const std::vector & collided_polygons, + const std::optional & lanes_polygon); /** * @brief Generates expanded lanelets based on the given direction and offsets. @@ -295,9 +297,11 @@ double calcPhaseLength( const double velocity, const double maximum_velocity, const double acceleration, const double time); -LanesPolygon createLanesPolygon( - const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes, - const std::vector & target_backward_lanes); +LanesPolygon create_lanes_polygon(const CommonDataPtr & common_data_ptr); + +bool is_same_lane_with_prev_iteration( + const CommonDataPtr & common_data_ptr, const lanelet::ConstLanelets & current_lanes, + const lanelet::ConstLanelets & target_lanes); } // namespace autoware::behavior_path_planner::utils::lane_change namespace autoware::behavior_path_planner::utils::lane_change::debug diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/interface.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/interface.cpp index b55b41828081e..4823cb0bfec22 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/interface.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/interface.cpp @@ -75,6 +75,7 @@ void LaneChangeInterface::updateData() { universe_utils::ScopedTimeTrack st(__func__, *getTimeKeeper()); module_type_->setPreviousModuleOutput(getPreviousModuleOutput()); + module_type_->update_lanes(getCurrentStatus() == ModuleStatus::RUNNING); module_type_->updateSpecialData(); if (isWaitingApproval() || module_type_->isAbortState()) { @@ -136,7 +137,7 @@ BehaviorModuleOutput LaneChangeInterface::planWaitingApproval() *prev_approved_path_ = getPreviousModuleOutput().path; BehaviorModuleOutput out = getPreviousModuleOutput(); - module_type_->insertStopPoint(module_type_->getLaneChangeStatus().current_lanes, out.path); + module_type_->insertStopPoint(module_type_->get_current_lanes(), out.path); out.turn_signal_info = module_type_->get_current_turn_signal_info(); const auto & lane_change_debug = module_type_->getDebugData(); diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp index 8636ff7d9dae7..4dfba64615504 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp @@ -41,7 +41,7 @@ namespace autoware::behavior_path_planner { using autoware::motion_utils::calcSignedArcLength; using utils::lane_change::calcMinimumLaneChangeLength; -using utils::lane_change::createLanesPolygon; +using utils::lane_change::create_lanes_polygon; using utils::path_safety_checker::isPolygonOverlapLanelet; using utils::traffic_light::getDistanceToNextTrafficLight; @@ -54,6 +54,42 @@ NormalLaneChange::NormalLaneChange( stop_watch_.tic("stop_time"); } +void NormalLaneChange::update_lanes(const bool is_approved) +{ + if (is_approved) { + return; + } + + const auto current_lanes = + utils::getCurrentLanesFromPath(prev_module_output_.path, planner_data_); + + if (current_lanes.empty()) { + return; + } + + const auto target_lanes = getLaneChangeLanes(current_lanes, direction_); + if (target_lanes.empty()) { + return; + } + + const auto is_same_lanes_with_prev_iteration = + utils::lane_change::is_same_lane_with_prev_iteration( + common_data_ptr_, current_lanes, target_lanes); + + if (is_same_lanes_with_prev_iteration) { + return; + } + + common_data_ptr_->lanes_ptr->current = current_lanes; + common_data_ptr_->lanes_ptr->target = target_lanes; + + common_data_ptr_->lanes_ptr->preceding_target = utils::getPrecedingLanelets( + *common_data_ptr_->route_handler_ptr, get_target_lanes(), common_data_ptr_->get_ego_pose(), + common_data_ptr_->lc_param_ptr->backward_lane_length); + + *common_data_ptr_->lanes_polygon_ptr = create_lanes_polygon(common_data_ptr_); +} + void NormalLaneChange::updateLaneChangeStatus() { universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); @@ -61,34 +97,23 @@ void NormalLaneChange::updateLaneChangeStatus() const auto [found_valid_path, found_safe_path] = getSafePath(status_.lane_change_path); // Update status - status_.current_lanes = status_.lane_change_path.info.current_lanes; - status_.target_lanes = status_.lane_change_path.info.target_lanes; status_.is_valid_path = found_valid_path; status_.is_safe = found_safe_path; - status_.lane_follow_lane_ids = utils::getIds(status_.current_lanes); - status_.lane_change_lane_ids = utils::getIds(status_.target_lanes); - const auto arclength_start = - lanelet::utils::getArcCoordinates(status_.target_lanes, getEgoPose()); + const auto arclength_start = lanelet::utils::getArcCoordinates(get_target_lanes(), getEgoPose()); status_.start_distance = arclength_start.length; status_.lane_change_path.path.header = getRouteHeader(); } std::pair NormalLaneChange::getSafePath(LaneChangePath & safe_path) const { - const auto current_lanes = getCurrentLanes(); - - if (current_lanes.empty()) { - return {false, false}; - } + const auto & current_lanes = get_current_lanes(); + const auto & target_lanes = get_target_lanes(); - const auto target_lanes = getLaneChangeLanes(current_lanes, direction_); - - if (target_lanes.empty()) { + if (current_lanes.empty() || target_lanes.empty()) { return {false, false}; } - // find candidate paths LaneChangePaths valid_paths{}; const bool is_stuck = isVehicleStuck(current_lanes); bool found_safe_path = getLaneChangePaths( @@ -104,8 +129,6 @@ std::pair NormalLaneChange::getSafePath(LaneChangePath & safe_path) lane_change_debug_.valid_paths = valid_paths; if (valid_paths.empty()) { - safe_path.info.current_lanes = current_lanes; - safe_path.info.target_lanes = target_lanes; return {false, false}; } @@ -121,21 +144,22 @@ std::pair NormalLaneChange::getSafePath(LaneChangePath & safe_path) bool NormalLaneChange::isLaneChangeRequired() { - status_.current_lanes = getCurrentLanes(); + const auto current_lanes = + utils::getCurrentLanesFromPath(prev_module_output_.path, planner_data_); - if (status_.current_lanes.empty()) { + if (current_lanes.empty()) { return false; } - status_.target_lanes = getLaneChangeLanes(status_.current_lanes, direction_); + const auto target_lanes = getLaneChangeLanes(current_lanes, direction_); - return !status_.target_lanes.empty(); + return !target_lanes.empty(); } bool NormalLaneChange::isStoppedAtRedTrafficLight() const { return utils::traffic_light::isStoppedAtRedTrafficLightWithinDistance( - status_.current_lanes, status_.lane_change_path.path, planner_data_, + get_current_lanes(), status_.lane_change_path.path, planner_data_, status_.lane_change_path.info.length.sum()); } @@ -143,7 +167,7 @@ TurnSignalInfo NormalLaneChange::get_current_turn_signal_info() { const auto original_turn_signal_info = prev_module_output_.turn_signal_info; - const auto & current_lanes = getLaneChangeStatus().current_lanes; + const auto & current_lanes = get_current_lanes(); const auto is_valid = getLaneChangeStatus().is_valid_path; const auto & lane_change_path = getLaneChangeStatus().lane_change_path; const auto & lane_change_param = getLaneChangeParam(); @@ -236,7 +260,7 @@ BehaviorModuleOutput NormalLaneChange::getTerminalLaneChangePath() const return output; } - const auto current_lanes = getCurrentLanes(); + const auto & current_lanes = get_current_lanes(); if (current_lanes.empty()) { RCLCPP_DEBUG(logger_, "Current lanes not found. Returning previous module's path as output."); return prev_module_output_; @@ -274,7 +298,7 @@ BehaviorModuleOutput NormalLaneChange::generateOutput() auto output = prev_module_output_; if (isAbortState() && abort_path_) { output.path = abort_path_->path; - insertStopPoint(status_.current_lanes, output.path); + insertStopPoint(get_current_lanes(), output.path); } else { output.path = getLaneChangePath().path; @@ -294,7 +318,7 @@ BehaviorModuleOutput NormalLaneChange::generateOutput() const auto stop_point = utils::insertStopPoint(stop_dist + current_dist, output.path); setStopPose(stop_point.point.pose); } else { - insertStopPoint(status_.target_lanes, output.path); + insertStopPoint(get_target_lanes(), output.path); } } @@ -315,7 +339,7 @@ void NormalLaneChange::extendOutputDrivableArea(BehaviorModuleOutput & output) c const auto & dp = planner_data_->drivable_area_expansion_parameters; const auto drivable_lanes = utils::lane_change::generateDrivableLanes( - *getRouteHandler(), status_.current_lanes, status_.target_lanes); + *getRouteHandler(), get_current_lanes(), get_target_lanes()); const auto shorten_lanes = utils::cutOverlappedLanes(output.path, drivable_lanes); const auto expanded_lanes = utils::expandLanelets( shorten_lanes, dp.drivable_area_left_bound_offset, dp.drivable_area_right_bound_offset, @@ -361,15 +385,14 @@ void NormalLaneChange::insertStopPoint( } const double stop_point_buffer = lane_change_parameters_->backward_length_buffer_for_end_of_lane; - const auto target_objects = filterObjects(status_.current_lanes, status_.target_lanes); + const auto target_objects = filterObjects(); double stopping_distance = distance_to_terminal - lane_change_buffer - stop_point_buffer; const auto is_valid_start_point = std::invoke([&]() -> bool { auto lc_start_point = lanelet::utils::conversion::toLaneletPoint( status_.lane_change_path.info.lane_changing_start.position); - const auto target_neighbor_preferred_lane_poly_2d = - utils::lane_change::getTargetNeighborLanesPolygon( - *route_handler, status_.current_lanes, type_); + const auto & target_neighbor_preferred_lane_poly_2d = + common_data_ptr_->lanes_polygon_ptr->target_neighbor; return boost::geometry::covered_by( lanelet::traits::to2D(lc_start_point), target_neighbor_preferred_lane_poly_2d); }); @@ -449,7 +472,7 @@ void NormalLaneChange::insertStopPoint( // target_objects includes objects out of target lanes, so filter them out if (!boost::geometry::intersects( autoware::universe_utils::toPolygon2d(o.initial_pose.pose, o.shape).outer(), - lanelet::utils::combineLaneletsShape(status_.target_lanes) + lanelet::utils::combineLaneletsShape(get_target_lanes()) .polygon2d() .basicPolygon())) { return false; @@ -476,7 +499,7 @@ PathWithLaneId NormalLaneChange::getReferencePath() const universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); lanelet::ConstLanelet closest_lanelet; if (!lanelet::utils::query::getClosestLanelet( - status_.lane_change_path.info.target_lanes, getEgoPose(), &closest_lanelet)) { + get_target_lanes(), getEgoPose(), &closest_lanelet)) { return prev_module_output_.reference_path; } const auto reference_path = utils::getCenterLinePathFromLanelet(closest_lanelet, planner_data_); @@ -498,7 +521,7 @@ std::optional NormalLaneChange::extendPath() return std::nullopt; } - auto & target_lanes = status_.target_lanes; + auto & target_lanes = common_data_ptr_->lanes_ptr->target; const auto target_lane_length = lanelet::utils::getLaneletLength2d(target_lanes); const auto dist_in_target = lanelet::utils::getArcCoordinates(target_lanes, getEgoPose()); @@ -569,7 +592,7 @@ TurnSignalInfo NormalLaneChange::updateOutputTurnSignal() const { universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); const auto & pose = getEgoPose(); - const auto & current_lanes = status_.current_lanes; + const auto & current_lanes = get_current_lanes(); const auto & shift_line = status_.lane_change_path.info.shift_line; const auto & shift_path = status_.lane_change_path.shifted_path; const auto current_shift_length = lanelet::utils::getArcCoordinates(current_lanes, pose).distance; @@ -584,11 +607,6 @@ TurnSignalInfo NormalLaneChange::updateOutputTurnSignal() const return new_signal; } -lanelet::ConstLanelets NormalLaneChange::getCurrentLanes() const -{ - return utils::getCurrentLanesFromPath(prev_module_output_.path, planner_data_); -} - lanelet::ConstLanelets NormalLaneChange::getLaneChangeLanes( const lanelet::ConstLanelets & current_lanes, Direction direction) const { @@ -666,8 +684,9 @@ bool NormalLaneChange::hasFinishedLaneChange() const { const auto & current_pose = getEgoPose(); const auto & lane_change_end = status_.lane_change_path.info.shift_line.end; - const double dist_to_lane_change_end = utils::getSignedDistance( - current_pose, lane_change_end, status_.lane_change_path.info.target_lanes); + const auto & target_lanes = get_target_lanes(); + const double dist_to_lane_change_end = + utils::getSignedDistance(current_pose, lane_change_end, get_target_lanes()); double finish_judge_buffer = lane_change_parameters_->lane_change_finish_judge_buffer; // If ego velocity is low, relax finish judge buffer @@ -685,7 +704,7 @@ bool NormalLaneChange::hasFinishedLaneChange() const return false; } - const auto arc_length = lanelet::utils::getArcCoordinates(status_.target_lanes, current_pose); + const auto arc_length = lanelet::utils::getArcCoordinates(target_lanes, current_pose); const auto reach_target_lane = std::abs(arc_length.distance) < lane_change_parameters_->finish_judge_lateral_threshold; @@ -702,7 +721,7 @@ bool NormalLaneChange::isAbleToReturnCurrentLane() const } if (!utils::isEgoWithinOriginalLane( - status_.current_lanes, getEgoPose(), planner_data_->parameters, + get_current_lanes(), getEgoPose(), planner_data_->parameters, lane_change_parameters_->cancel.overhang_tolerance)) { lane_change_debug_.is_able_to_return_to_current_lane = false; return false; @@ -723,7 +742,7 @@ bool NormalLaneChange::isAbleToReturnCurrentLane() const if (dist > estimated_travel_dist) { const auto & estimated_pose = status_.lane_change_path.path.points.at(idx + 1).point.pose; auto is_ego_within_original_lane = utils::isEgoWithinOriginalLane( - status_.current_lanes, estimated_pose, planner_data_->parameters, + get_current_lanes(), estimated_pose, planner_data_->parameters, lane_change_parameters_->cancel.overhang_tolerance); lane_change_debug_.is_able_to_return_to_current_lane = is_ego_within_original_lane; return is_ego_within_original_lane; @@ -762,7 +781,7 @@ bool NormalLaneChange::isAbleToStopSafely() const if (dist > stop_dist) { const auto & estimated_pose = status_.lane_change_path.path.points.at(idx + 1).point.pose; return utils::isEgoWithinOriginalLane( - status_.current_lanes, estimated_pose, planner_data_->parameters); + get_current_lanes(), estimated_pose, planner_data_->parameters); } } return true; @@ -962,8 +981,7 @@ ExtendedPredictedObjects NormalLaneChange::getTargetObjects( return target_objects; } -LaneChangeLanesFilteredObjects NormalLaneChange::filterObjects( - const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes) const +LaneChangeLanesFilteredObjects NormalLaneChange::filterObjects() const { const auto & current_pose = getEgoPose(); const auto & route_handler = getRouteHandler(); @@ -982,6 +1000,12 @@ LaneChangeLanesFilteredObjects NormalLaneChange::filterObjects( return {}; } + const auto & current_lanes = get_current_lanes(); + + if (current_lanes.empty()) { + return {}; + } + filterAheadTerminalObjects(objects, current_lanes); if (objects.objects.empty()) { @@ -992,6 +1016,12 @@ LaneChangeLanesFilteredObjects NormalLaneChange::filterObjects( std::vector current_lane_objects; std::vector other_lane_objects; + const auto & target_lanes = get_target_lanes(); + + if (target_lanes.empty()) { + return {}; + } + filterObjectsByLanelets( objects, current_lanes, target_lanes, current_lane_objects, target_lane_objects, other_lane_objects); @@ -1132,9 +1162,7 @@ void NormalLaneChange::filterObjectsByLanelets( }; // get backward lanes - const auto backward_length = lane_change_parameters_->backward_lane_length; - const auto target_backward_lanes = - utils::getPrecedingLanelets(*route_handler, target_lanes, current_pose, backward_length); + const auto & target_backward_lanes = common_data_ptr_->lanes_ptr->preceding_target; { lane_change_debug_.current_lanes = current_lanes; @@ -1151,12 +1179,7 @@ void NormalLaneChange::filterObjectsByLanelets( }); } - const auto expanded_target_lanes = utils::lane_change::generateExpandedLanelets( - target_lanes, direction_, lane_change_parameters_->lane_expansion_left_offset, - lane_change_parameters_->lane_expansion_right_offset); - - const auto lanes_polygon = - createLanesPolygon(current_lanes, expanded_target_lanes, target_backward_lanes); + const auto & lanes_polygon = *common_data_ptr_->lanes_polygon_ptr; const auto dist_ego_to_current_lanes_center = lanelet::utils::getLateralDistanceToClosestLanelet(current_lanes, current_pose); @@ -1176,7 +1199,7 @@ void NormalLaneChange::filterObjectsByLanelets( return std::abs(lateral) > (common_parameters.vehicle_width / 2); }); - if (check_optional_polygon(object, lanes_polygon.target) && is_lateral_far) { + if (check_optional_polygon(object, lanes_polygon.expanded_target) && is_lateral_far) { target_lane_objects.push_back(object); continue; } @@ -1186,7 +1209,7 @@ void NormalLaneChange::filterObjectsByLanelets( return isPolygonOverlapLanelet(object, target_backward_polygon); }; return std::any_of( - lanes_polygon.target_backward.begin(), lanes_polygon.target_backward.end(), + lanes_polygon.preceding_target.begin(), lanes_polygon.preceding_target.end(), check_backward_polygon); }); @@ -1376,14 +1399,14 @@ bool NormalLaneChange::getLaneChangePaths( const auto sorted_lane_ids = utils::lane_change::getSortedLaneIds(route_handler, getEgoPose(), current_lanes, target_lanes); - const auto target_neighbor_preferred_lane_poly_2d = - utils::lane_change::getTargetNeighborLanesPolygon(route_handler, current_lanes, type_); + const auto & target_neighbor_preferred_lane_poly_2d = + common_data_ptr_->lanes_polygon_ptr->target_neighbor; if (target_neighbor_preferred_lane_poly_2d.empty()) { RCLCPP_WARN(logger_, "target_neighbor_preferred_lane_poly_2d is empty. Not expected."); return false; } - const auto filtered_objects = filterObjects(current_lanes, target_lanes); + const auto filtered_objects = filterObjects(); const auto target_objects = getTargetObjects(filtered_objects, current_lanes); const auto prepare_durations = calcPrepareDuration(current_lanes, target_lanes); @@ -1513,9 +1536,7 @@ bool NormalLaneChange::getLaneChangePaths( prepare_segment.points.back().point.pose.position.x, prepare_segment.points.back().point.pose.position.y); - const auto target_lane_polygon = lanelet::utils::getPolygonFromArcLength( - target_lanes, 0, std::numeric_limits::max()); - const auto target_lane_poly_2d = lanelet::utils::to2D(target_lane_polygon).basicPolygon(); + const auto target_lane_poly_2d = common_data_ptr_->lanes_polygon_ptr->target.value(); const auto is_valid_start_point = boost::geometry::covered_by(lc_start_point, target_neighbor_preferred_lane_poly_2d) || @@ -1528,8 +1549,6 @@ bool NormalLaneChange::getLaneChangePaths( lane_change_info.velocity = LaneChangePhaseInfo{prepare_velocity, initial_lane_changing_velocity}; lane_change_info.length = LaneChangePhaseInfo{prepare_length, lane_changing_length}; - lane_change_info.current_lanes = current_lanes; - lane_change_info.target_lanes = target_lanes; lane_change_info.lane_changing_start = prepare_segment.points.back().point.pose; lane_change_info.lane_changing_end = target_segment.points.front().point.pose; lane_change_info.lateral_acceleration = lateral_acc; @@ -1558,8 +1577,8 @@ bool NormalLaneChange::getLaneChangePaths( prepare_segment, target_segment, target_lane_reference_path, shift_length); const auto candidate_path = utils::lane_change::constructCandidatePath( - lane_change_info, prepare_segment, target_segment, target_lane_reference_path, - sorted_lane_ids); + common_data_ptr_, lane_change_info, prepare_segment, target_segment, + target_lane_reference_path, sorted_lane_ids); if (!candidate_path) { debug_print_lat("Reject: failed to generate candidate path!!"); @@ -1601,7 +1620,7 @@ bool NormalLaneChange::getLaneChangePaths( } if (utils::traffic_light::isStoppedAtRedTrafficLightWithinDistance( - lane_change_info.current_lanes, candidate_path.value().path, planner_data_, + get_current_lanes(), candidate_path.value().path, planner_data_, lane_change_info.length.sum())) { debug_print_lat("Ego is stopping near traffic light. Do not allow lane change"); continue; @@ -1609,10 +1628,10 @@ bool NormalLaneChange::getLaneChangePaths( candidate_paths->push_back(*candidate_path); if ( - !is_stuck && utils::lane_change::passParkedObject( - route_handler, *candidate_path, filtered_objects.target_lane, - lane_change_buffer, is_goal_in_route, *lane_change_parameters_, - lane_change_debug_.collision_check_objects)) { + !is_stuck && + utils::lane_change::passParkedObject( + common_data_ptr_, *candidate_path, filtered_objects.target_lane, lane_change_buffer, + is_goal_in_route, lane_change_debug_.collision_check_objects)) { debug_print_lat( "Reject: parking vehicle exists in the target lane, and the ego is not in stuck. Skip " "lane change."); @@ -1670,7 +1689,7 @@ std::optional NormalLaneChange::calcTerminalLaneChangePath( utils::lane_change::getSortedLaneIds(route_handler, getEgoPose(), current_lanes, target_lanes); const auto target_neighbor_preferred_lane_poly_2d = - utils::lane_change::getTargetNeighborLanesPolygon(route_handler, current_lanes, type_); + common_data_ptr_->lanes_polygon_ptr->target_neighbor; if (target_neighbor_preferred_lane_poly_2d.empty()) { RCLCPP_WARN(logger_, "target_neighbor_preferred_lane_poly_2d is empty. Not expected."); return {}; @@ -1725,9 +1744,7 @@ std::optional NormalLaneChange::calcTerminalLaneChangePath( const lanelet::BasicPoint2d lc_start_point( lane_changing_start_pose->position.x, lane_changing_start_pose->position.y); - const auto target_lane_polygon = - lanelet::utils::getPolygonFromArcLength(target_lanes, 0, std::numeric_limits::max()); - const auto target_lane_poly_2d = lanelet::utils::to2D(target_lane_polygon).basicPolygon(); + const auto & target_lane_poly_2d = common_data_ptr_->lanes_polygon_ptr->target.value(); const auto is_valid_start_point = boost::geometry::covered_by(lc_start_point, target_neighbor_preferred_lane_poly_2d) || @@ -1739,8 +1756,6 @@ std::optional NormalLaneChange::calcTerminalLaneChangePath( lane_change_info.velocity = LaneChangePhaseInfo{minimum_lane_changing_velocity, minimum_lane_changing_velocity}; lane_change_info.length = LaneChangePhaseInfo{0.0, lane_change_buffer}; - lane_change_info.current_lanes = current_lanes; - lane_change_info.target_lanes = target_lanes; lane_change_info.lane_changing_start = lane_changing_start_pose.value(); lane_change_info.lane_changing_end = target_segment.points.front().point.pose; lane_change_info.lateral_acceleration = max_lateral_acc; @@ -1780,8 +1795,8 @@ std::optional NormalLaneChange::calcTerminalLaneChangePath( reference_segment.points.back().point.longitudinal_velocity_mps = minimum_lane_changing_velocity; const auto terminal_lane_change_path = utils::lane_change::constructCandidatePath( - lane_change_info, reference_segment, target_segment, target_lane_reference_path, - sorted_lane_ids); + common_data_ptr_, lane_change_info, reference_segment, target_segment, + target_lane_reference_path, sorted_lane_ids); return terminal_lane_change_path; } @@ -1789,10 +1804,14 @@ std::optional NormalLaneChange::calcTerminalLaneChangePath( PathSafetyStatus NormalLaneChange::isApprovedPathSafe() const { const auto & path = status_.lane_change_path; - const auto & current_lanes = status_.current_lanes; - const auto & target_lanes = status_.target_lanes; + const auto & current_lanes = get_current_lanes(); + const auto & target_lanes = get_target_lanes(); + + if (current_lanes.empty() || target_lanes.empty()) { + return {true, true}; + } - const auto filtered_objects = filterObjects(current_lanes, target_lanes); + const auto filtered_objects = filterObjects(); const auto target_objects = getTargetObjects(filtered_objects, current_lanes); CollisionCheckDebugMap debug_data; @@ -1847,8 +1866,8 @@ bool NormalLaneChange::isValidPath(const PathWithLaneId & path) const // check lane departure const auto drivable_lanes = utils::lane_change::generateDrivableLanes( - *route_handler, utils::extendLanes(route_handler, status_.current_lanes), - utils::extendLanes(route_handler, status_.target_lanes)); + *route_handler, utils::extendLanes(route_handler, get_current_lanes()), + utils::extendLanes(route_handler, get_target_lanes())); const auto expanded_lanes = utils::expandLanelets( drivable_lanes, dp.drivable_area_left_bound_offset, dp.drivable_area_right_bound_offset, dp.drivable_area_types_to_skip); @@ -1882,7 +1901,7 @@ bool NormalLaneChange::isRequiredStop(const bool is_object_coming_from_rear) universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); const auto threshold = lane_change_parameters_->backward_length_buffer_for_end_of_lane; if ( - isNearEndOfCurrentLanes(status_.current_lanes, status_.target_lanes, threshold) && + isNearEndOfCurrentLanes(get_current_lanes(), get_target_lanes(), threshold) && isAbleToStopSafely() && is_object_coming_from_rear) { current_lane_change_state_ = LaneChangeStates::Stop; return true; @@ -1900,16 +1919,16 @@ bool NormalLaneChange::calcAbortPath() std::max(lane_change_parameters_->minimum_lane_changing_velocity, getEgoVelocity()); const auto current_pose = getEgoPose(); const auto & selected_path = status_.lane_change_path; - const auto reference_lanelets = selected_path.info.current_lanes; const auto ego_nearest_dist_threshold = common_param.ego_nearest_dist_threshold; const auto ego_nearest_yaw_threshold = common_param.ego_nearest_yaw_threshold; const auto direction = getDirection(); const auto minimum_lane_change_length = utils::lane_change::calcMinimumLaneChangeLength( - route_handler, selected_path.info.current_lanes.back(), *lane_change_parameters_, direction); + route_handler, get_current_lanes().back(), *lane_change_parameters_, direction); const auto & lane_changing_path = selected_path.path; + const auto & reference_lanelets = get_current_lanes(); const auto lane_changing_end_pose_idx = std::invoke([&]() { constexpr double s_start = 0.0; const double s_end = std::max( @@ -1998,7 +2017,7 @@ bool NormalLaneChange::calcAbortPath() auto reference_lane_segment = prev_module_output_.path; { // const auto terminal_path = - // calcTerminalLaneChangePath(reference_lanelets, selected_path.info.target_lanes); + // calcTerminalLaneChangePath(reference_lanelets, get_target_lanes()); // if (terminal_path) { // reference_lane_segment = terminal_path->path; // } @@ -2063,12 +2082,8 @@ PathSafetyStatus NormalLaneChange::isLaneChangePathSafe( const auto debug_predicted_path = utils::path_safety_checker::convertToPredictedPath(ego_predicted_path, time_resolution); - const auto current_lanes = getCurrentLanes(); - - const auto expanded_target_lanes = utils::lane_change::generateExpandedLanelets( - lane_change_path.info.target_lanes, direction_, - lane_change_parameters_->lane_expansion_left_offset, - lane_change_parameters_->lane_expansion_right_offset); + const auto & current_lanes_polygon = common_data_ptr_->lanes_polygon_ptr->current; + const auto & expanded_target_polygon = common_data_ptr_->lanes_polygon_ptr->expanded_target; constexpr double collision_check_yaw_diff_threshold{M_PI}; @@ -2089,10 +2104,10 @@ PathSafetyStatus NormalLaneChange::isLaneChangePathSafe( continue; } - const auto collision_in_current_lanes = utils::lane_change::isCollidedPolygonsInLanelet( - collided_polygons, lane_change_path.info.current_lanes); + const auto collision_in_current_lanes = + utils::lane_change::isCollidedPolygonsInLanelet(collided_polygons, current_lanes_polygon); const auto collision_in_target_lanes = - utils::lane_change::isCollidedPolygonsInLanelet(collided_polygons, expanded_target_lanes); + utils::lane_change::isCollidedPolygonsInLanelet(collided_polygons, expanded_target_polygon); if (!collision_in_current_lanes && !collision_in_target_lanes) { utils::path_safety_checker::updateCollisionCheckDebugMap( diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/utils.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/utils.cpp index ead047f839f47..5f13aa3192113 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/utils.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/utils.cpp @@ -330,13 +330,12 @@ bool isPathInLanelets( } std::optional constructCandidatePath( - const LaneChangeInfo & lane_change_info, const PathWithLaneId & prepare_segment, - const PathWithLaneId & target_segment, const PathWithLaneId & target_lane_reference_path, + const CommonDataPtr & common_data_ptr, const LaneChangeInfo & lane_change_info, + const PathWithLaneId & prepare_segment, const PathWithLaneId & target_segment, + const PathWithLaneId & target_lane_reference_path, const std::vector> & sorted_lane_ids) { const auto & shift_line = lane_change_info.shift_line; - const auto & original_lanes = lane_change_info.current_lanes; - const auto & target_lanes = lane_change_info.target_lanes; const auto terminal_lane_changing_velocity = lane_change_info.terminal_lane_changing_velocity; const auto longitudinal_acceleration = lane_change_info.longitudinal_acceleration; const auto lane_change_velocity = lane_change_info.velocity; @@ -393,9 +392,11 @@ std::optional constructCandidatePath( const bool enable_path_check_in_lanelet = false; // check candidate path is in lanelet + const auto & current_lanes = common_data_ptr->lanes_ptr->current; + const auto & target_lanes = common_data_ptr->lanes_ptr->target; if ( enable_path_check_in_lanelet && - !isPathInLanelets(shifted_path.path, original_lanes, target_lanes)) { + !isPathInLanelets(shifted_path.path, current_lanes, target_lanes)) { return std::nullopt; } @@ -968,17 +969,18 @@ bool isParkedObject( } bool passParkedObject( - const RouteHandler & route_handler, const LaneChangePath & lane_change_path, + const CommonDataPtr & common_data_ptr, const LaneChangePath & lane_change_path, const std::vector & objects, const double minimum_lane_change_length, - const bool is_goal_in_route, const LaneChangeParameters & lane_change_parameters, - CollisionCheckDebugMap & object_debug) + const bool is_goal_in_route, CollisionCheckDebugMap & object_debug) { + const auto route_handler = *common_data_ptr->route_handler_ptr; + const auto lane_change_parameters = *common_data_ptr->lc_param_ptr; const auto & object_check_min_road_shoulder_width = lane_change_parameters.object_check_min_road_shoulder_width; const auto & object_shiftable_ratio_threshold = lane_change_parameters.object_shiftable_ratio_threshold; const auto & path = lane_change_path.path; - const auto & current_lanes = lane_change_path.info.current_lanes; + const auto & current_lanes = common_data_ptr->lanes_ptr->current; const auto current_lane_path = route_handler.getCenterLinePath(current_lanes, 0.0, std::numeric_limits::max()); @@ -1135,10 +1137,9 @@ ExtendedPredictedObject transform( } bool isCollidedPolygonsInLanelet( - const std::vector & collided_polygons, const lanelet::ConstLanelets & lanes) + const std::vector & collided_polygons, + const std::optional & lanes_polygon) { - const auto lanes_polygon = createPolygon(lanes, 0.0, std::numeric_limits::max()); - const auto is_in_lanes = [&](const auto & collided_polygon) { return lanes_polygon && boost::geometry::intersects(lanes_polygon.value(), collided_polygon); }; @@ -1207,27 +1208,60 @@ double calcPhaseLength( return std::min(length_with_acceleration, length_with_max_velocity); } -LanesPolygon createLanesPolygon( - const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes, - const std::vector & target_backward_lanes) +LanesPolygon create_lanes_polygon(const CommonDataPtr & common_data_ptr) { + const auto & lanes = common_data_ptr->lanes_ptr; LanesPolygon lanes_polygon; lanes_polygon.current = - utils::lane_change::createPolygon(current_lanes, 0.0, std::numeric_limits::max()); + utils::lane_change::createPolygon(lanes->current, 0.0, std::numeric_limits::max()); lanes_polygon.target = - utils::lane_change::createPolygon(target_lanes, 0.0, std::numeric_limits::max()); + utils::lane_change::createPolygon(lanes->target, 0.0, std::numeric_limits::max()); + + const auto & lc_param_ptr = common_data_ptr->lc_param_ptr; + const auto expanded_target_lanes = utils::lane_change::generateExpandedLanelets( + lanes->target, common_data_ptr->direction, lc_param_ptr->lane_expansion_left_offset, + lc_param_ptr->lane_expansion_right_offset); + lanes_polygon.expanded_target = utils::lane_change::createPolygon( + expanded_target_lanes, 0.0, std::numeric_limits::max()); + + const auto & route_handler = *common_data_ptr->route_handler_ptr; + lanes_polygon.target_neighbor = + getTargetNeighborLanesPolygon(route_handler, lanes->current, common_data_ptr->lc_type); - for (const auto & target_backward_lane : target_backward_lanes) { - auto lane_polygon = utils::lane_change::createPolygon( - target_backward_lane, 0.0, std::numeric_limits::max()); + lanes_polygon.preceding_target.reserve(lanes->preceding_target.size()); + for (const auto & preceding_lane : lanes->preceding_target) { + auto lane_polygon = + utils::lane_change::createPolygon(preceding_lane, 0.0, std::numeric_limits::max()); if (lane_polygon) { - lanes_polygon.target_backward.push_back(*lane_polygon); + lanes_polygon.preceding_target.push_back(*lane_polygon); } } return lanes_polygon; } + +bool is_same_lane_with_prev_iteration( + const CommonDataPtr & common_data_ptr, const lanelet::ConstLanelets & current_lanes, + const lanelet::ConstLanelets & target_lanes) +{ + if (current_lanes.empty() || target_lanes.empty()) { + return false; + } + const auto & prev_current_lanes = common_data_ptr->lanes_ptr->current; + const auto & prev_target_lanes = common_data_ptr->lanes_ptr->target; + if (prev_current_lanes.empty() || prev_target_lanes.empty()) { + return false; + } + + if ( + (prev_current_lanes.front().id() != current_lanes.front().id()) || + (prev_current_lanes.back().id() != prev_current_lanes.back().id())) { + return false; + } + return (prev_target_lanes.front().id() == target_lanes.front().id()) && + (prev_target_lanes.back().id() == prev_target_lanes.back().id()); +} } // namespace autoware::behavior_path_planner::utils::lane_change namespace autoware::behavior_path_planner::utils::lane_change::debug From 0a3e6d8eabe639103fcec3ca57e6ab768a8e6ca3 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Mehmet=20Emin=20BA=C5=9EO=C4=9ELU?= Date: Fri, 12 Jul 2024 19:48:41 +0300 Subject: [PATCH 67/70] feat(system): create a package to monitor component containers (#7094) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Mehmet Emin BAÅžOÄžLU --- .../autoware_component_monitor/CMakeLists.txt | 31 +++ system/autoware_component_monitor/README.md | 84 +++++++++ .../config/component_monitor.param.yaml | 3 + .../launch/component_monitor.launch.xml | 9 + system/autoware_component_monitor/package.xml | 25 +++ .../schema/component_monitor.schema.json | 30 +++ .../src/component_monitor_node.cpp | 177 ++++++++++++++++++ .../src/component_monitor_node.hpp | 102 ++++++++++ .../src/unit_conversions.hpp | 68 +++++++ .../test/test_unit_conversions.cpp | 57 ++++++ 10 files changed, 586 insertions(+) create mode 100644 system/autoware_component_monitor/CMakeLists.txt create mode 100644 system/autoware_component_monitor/README.md create mode 100644 system/autoware_component_monitor/config/component_monitor.param.yaml create mode 100644 system/autoware_component_monitor/launch/component_monitor.launch.xml create mode 100644 system/autoware_component_monitor/package.xml create mode 100644 system/autoware_component_monitor/schema/component_monitor.schema.json create mode 100644 system/autoware_component_monitor/src/component_monitor_node.cpp create mode 100644 system/autoware_component_monitor/src/component_monitor_node.hpp create mode 100644 system/autoware_component_monitor/src/unit_conversions.hpp create mode 100644 system/autoware_component_monitor/test/test_unit_conversions.cpp diff --git a/system/autoware_component_monitor/CMakeLists.txt b/system/autoware_component_monitor/CMakeLists.txt new file mode 100644 index 0000000000000..674b079a90563 --- /dev/null +++ b/system/autoware_component_monitor/CMakeLists.txt @@ -0,0 +1,31 @@ +cmake_minimum_required(VERSION 3.8) +project(autoware_component_monitor) + +find_package(autoware_cmake REQUIRED) +autoware_package() + +find_package(Boost REQUIRED COMPONENTS + filesystem +) + +ament_auto_add_library(${PROJECT_NAME} SHARED + src/component_monitor_node.cpp +) +target_link_libraries(${PROJECT_NAME} ${Boost_LIBRARIES}) + +rclcpp_components_register_node(${PROJECT_NAME} + PLUGIN "autoware::component_monitor::ComponentMonitor" + EXECUTABLE ${PROJECT_NAME}_node +) + +if(BUILD_TESTING) + ament_add_ros_isolated_gtest(test_unit_conversions test/test_unit_conversions.cpp) + target_link_libraries(test_unit_conversions ${PROJECT_NAME}) + target_include_directories(test_unit_conversions PRIVATE src) +endif() + +ament_auto_package( + INSTALL_TO_SHARE + config + launch +) diff --git a/system/autoware_component_monitor/README.md b/system/autoware_component_monitor/README.md new file mode 100644 index 0000000000000..c255c420c048e --- /dev/null +++ b/system/autoware_component_monitor/README.md @@ -0,0 +1,84 @@ +# autoware_component_monitor + +The `autoware_component_monitor` package allows monitoring system usage of component containers. +The composable node inside the package is attached to a component container, and it publishes CPU and memory usage of +the container. + +## Inputs / Outputs + +### Input + +None. + +### Output + +| Name | Type | Description | +| -------------------------- | -------------------------------------------------- | ---------------------- | +| `~/component_system_usage` | `autoware_internal_msgs::msg::ResourceUsageReport` | CPU, Memory usage etc. | + +## Parameters + +### Core Parameters + +{{ json_to_markdown("system/autoware_component_monitor/schema/component_monitor.schema.json") }} + +## How to use + +Add it as a composable node in your launch file: + +```xml + + + + + ... + + + + + + + + ... + + +``` + +### Quick testing + +You can test the package by running the following command: + +```bash +ros2 component load autoware_component_monitor autoware::component_monitor::ComponentMonitor -p publish_rate:=10.0 --node-namespace + +# Example usage +ros2 component load /pointcloud_container autoware_component_monitor autoware::component_monitor::ComponentMonitor -p publish_rate:=10.0 --node-namespace /pointcloud_container +``` + +## How it works + +The package uses the `top` command under the hood. +`top -b -n 1 -E k -p PID` command is run at 10 Hz to get the system usage of the process. + +- `-b` activates the batch mode. By default, `top` doesn't exit and prints to stdout periodically. Batch mode allows + exiting the program. +- `-n` number of times should `top` prints the system usage in batch mode. +- `-p` specifies the PID of the process to monitor. +- `-E k` changes the memory unit in the summary section to KiB. + +Here is a sample output: + +```text +top - 13:57:26 up 3:14, 1 user, load average: 1,09, 1,10, 1,04 +Tasks: 1 total, 0 running, 1 sleeping, 0 stopped, 0 zombie +%Cpu(s): 0,0 us, 0,8 sy, 0,0 ni, 99,2 id, 0,0 wa, 0,0 hi, 0,0 si, 0,0 st +KiB Mem : 65532208 total, 35117428 free, 17669824 used, 12744956 buff/cache +KiB Swap: 39062524 total, 39062524 free, 0 used. 45520816 avail Mem + + PID USER PR NI VIRT RES SHR S %CPU %MEM TIME+ COMMAND + 3352 meb 20 0 2905940 1,2g 39292 S 0,0 2,0 23:24.01 awesome +``` + +We get 5th, 8th fields from the last line, which are RES, %CPU respectively. diff --git a/system/autoware_component_monitor/config/component_monitor.param.yaml b/system/autoware_component_monitor/config/component_monitor.param.yaml new file mode 100644 index 0000000000000..62cf278921460 --- /dev/null +++ b/system/autoware_component_monitor/config/component_monitor.param.yaml @@ -0,0 +1,3 @@ +/**: + ros__parameters: + publish_rate: 5.0 # Hz diff --git a/system/autoware_component_monitor/launch/component_monitor.launch.xml b/system/autoware_component_monitor/launch/component_monitor.launch.xml new file mode 100644 index 0000000000000..1b2a77eddab93 --- /dev/null +++ b/system/autoware_component_monitor/launch/component_monitor.launch.xml @@ -0,0 +1,9 @@ + + + + + + + + + diff --git a/system/autoware_component_monitor/package.xml b/system/autoware_component_monitor/package.xml new file mode 100644 index 0000000000000..640e1f2dc2517 --- /dev/null +++ b/system/autoware_component_monitor/package.xml @@ -0,0 +1,25 @@ + + + + autoware_component_monitor + 0.0.0 + A ROS 2 package to monitor system usage of component containers. + Mehmet Emin BaÅŸoÄŸlu + Apache-2.0 + + ament_cmake_auto + autoware_cmake + + autoware_internal_msgs + libboost-filesystem-dev + rclcpp + rclcpp_components + + ament_cmake_ros + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/system/autoware_component_monitor/schema/component_monitor.schema.json b/system/autoware_component_monitor/schema/component_monitor.schema.json new file mode 100644 index 0000000000000..f0edf5add718e --- /dev/null +++ b/system/autoware_component_monitor/schema/component_monitor.schema.json @@ -0,0 +1,30 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameters for the Component Monitor node", + "type": "object", + "definitions": { + "component_monitor": { + "type": "object", + "properties": { + "publish_rate": { + "type": "number", + "default": "5.0", + "description": "Publish rate in Hz" + } + }, + "required": ["publish_rate"] + } + }, + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "$ref": "#/definitions/component_monitor" + } + }, + "required": ["ros__parameters"] + } + }, + "required": ["/**"] +} diff --git a/system/autoware_component_monitor/src/component_monitor_node.cpp b/system/autoware_component_monitor/src/component_monitor_node.cpp new file mode 100644 index 0000000000000..3c5d6b6667725 --- /dev/null +++ b/system/autoware_component_monitor/src/component_monitor_node.cpp @@ -0,0 +1,177 @@ +// Copyright 2024 The Autoware Foundation +// +// 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 "component_monitor_node.hpp" + +#include "unit_conversions.hpp" + +#include + +#include + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace autoware::component_monitor +{ +ComponentMonitor::ComponentMonitor(const rclcpp::NodeOptions & node_options) +: Node("component_monitor", node_options), publish_rate_(declare_parameter("publish_rate")) +{ + usage_pub_ = + create_publisher("~/component_system_usage", rclcpp::SensorDataQoS()); + + // Make sure top ins installed and is in path + const auto path_top = boost::process::search_path("top"); + if (path_top.empty()) { + RCLCPP_ERROR_STREAM(get_logger(), "Couldn't find 'top' in path."); + rclcpp::shutdown(); + } + + // Get the PID of the current process + int pid = getpid(); + + environment_ = boost::this_process::environment(); + environment_["LC_NUMERIC"] = "en_US.UTF-8"; + + on_timer_tick_wrapped_ = std::bind(&ComponentMonitor::on_timer_tick, this, pid); + + timer_ = rclcpp::create_timer( + this, get_clock(), rclcpp::Rate(publish_rate_).period(), on_timer_tick_wrapped_); +} + +void ComponentMonitor::on_timer_tick(const int pid) const +{ + if (usage_pub_->get_subscription_count() == 0) return; + + try { + auto usage_msg = pid_to_report(pid); + usage_msg.header.stamp = this->now(); + usage_msg.pid = pid; + usage_pub_->publish(usage_msg); + } catch (std::exception & e) { + RCLCPP_ERROR(get_logger(), "%s", e.what()); + } catch (...) { + RCLCPP_ERROR(get_logger(), "An unknown error occurred."); + } +} + +ComponentMonitor::ResourceUsageReport ComponentMonitor::pid_to_report(const pid_t & pid) const +{ + const auto std_out = run_system_command("top -b -n 1 -E k -p " + std::to_string(pid)); + + const auto fields = parse_lines_into_words(std_out); + + ResourceUsageReport report; + report.cpu_cores_utilized = std::stof(fields.back().at(8)) / 100.0f; + report.total_memory_bytes = unit_conversions::kib_to_bytes(std::stoul(fields.at(3).at(3))); + report.free_memory_bytes = unit_conversions::kib_to_bytes(std::stoul(fields.at(3).at(5))); + report.process_memory_bytes = parse_memory_res(fields.back().at(5)); + + return report; +} + +std::stringstream ComponentMonitor::run_system_command(const std::string & cmd) const +{ + int out_fd[2]; + if (pipe2(out_fd, O_CLOEXEC) != 0) { + RCLCPP_ERROR_STREAM(get_logger(), "Error setting flags on out_fd"); + } + boost::process::pipe out_pipe{out_fd[0], out_fd[1]}; + boost::process::ipstream is_out{std::move(out_pipe)}; + + int err_fd[2]; + if (pipe2(err_fd, O_CLOEXEC) != 0) { + RCLCPP_ERROR_STREAM(get_logger(), "Error setting flags on err_fd"); + } + boost::process::pipe err_pipe{err_fd[0], err_fd[1]}; + boost::process::ipstream is_err{std::move(err_pipe)}; + + boost::process::child c( + cmd, environment_, boost::process::std_out > is_out, boost::process::std_err > is_err); + c.wait(); + + if (c.exit_code() != 0) { + std::ostringstream os; + is_err >> os.rdbuf(); + RCLCPP_ERROR_STREAM(get_logger(), "Error running command: " << os.str()); + } + + std::stringstream sstream; + sstream << is_out.rdbuf(); + return sstream; +} + +ComponentMonitor::VecVecStr ComponentMonitor::parse_lines_into_words( + const std::stringstream & std_out) +{ + VecVecStr fields; + std::string line; + std::istringstream input{std_out.str()}; + + while (std::getline(input, line)) { + std::istringstream iss_line{line}; + std::string word; + std::vector words; + + while (iss_line >> word) { + words.push_back(word); + } + + fields.push_back(words); + } + + return fields; +} + +std::uint64_t ComponentMonitor::parse_memory_res(const std::string & mem_res) +{ + // example 1: 12.3g + // example 2: 123 (without suffix, just bytes) + static const std::unordered_map> unit_map{ + {'k', unit_conversions::kib_to_bytes}, {'m', unit_conversions::mib_to_bytes}, + {'g', unit_conversions::gib_to_bytes}, {'t', unit_conversions::tib_to_bytes}, + {'p', unit_conversions::pib_to_bytes}, {'e', unit_conversions::eib_to_bytes}}; + + if (std::isdigit(mem_res.back())) { + return std::stoull(mem_res); // Handle plain bytes without any suffix + } + + // Extract the numeric part and the unit suffix + double value = std::stod(mem_res.substr(0, mem_res.size() - 1)); + char suffix = mem_res.back(); + + // Find the appropriate function from the map + auto it = unit_map.find(suffix); + if (it != unit_map.end()) { + const auto & conversion_function = it->second; + return conversion_function(value); + } + + // Throw an exception or handle the error as needed if the suffix is not recognized + throw std::runtime_error("Unsupported unit suffix: " + std::string(1, suffix)); +} + +} // namespace autoware::component_monitor + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(autoware::component_monitor::ComponentMonitor) diff --git a/system/autoware_component_monitor/src/component_monitor_node.hpp b/system/autoware_component_monitor/src/component_monitor_node.hpp new file mode 100644 index 0000000000000..70a486eb8209b --- /dev/null +++ b/system/autoware_component_monitor/src/component_monitor_node.hpp @@ -0,0 +1,102 @@ +// Copyright 2024 The Autoware Foundation +// +// 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 COMPONENT_MONITOR_NODE_HPP_ +#define COMPONENT_MONITOR_NODE_HPP_ + +#include + +#include + +#include + +#include +#include +#include +#include + +namespace autoware::component_monitor +{ +class ComponentMonitor : public rclcpp::Node +{ +public: + explicit ComponentMonitor(const rclcpp::NodeOptions & node_options); + +private: + using ResourceUsageReport = autoware_internal_msgs::msg::ResourceUsageReport; + using VecVecStr = std::vector>; + + const double publish_rate_; + + std::function on_timer_tick_wrapped_; + + rclcpp::Publisher::SharedPtr usage_pub_; + rclcpp::TimerBase::SharedPtr timer_; + + boost::process::native_environment environment_; + + void on_timer_tick(int pid) const; + + /** + * @brief Get system usage of the component. + * + * @details The output of top -b -n 1 -E k -p PID` is like below: + * + * top - 13:57:26 up 3:14, 1 user, load average: 1,09, 1,10, 1,04 + * Tasks: 1 total, 0 running, 1 sleeping, 0 stopped, 0 zombie + * %Cpu(s): 0,0 us, 0,8 sy, 0,0 ni, 99,2 id, 0,0 wa, 0,0 hi, 0,0 si, 0,0 st + * KiB Mem : 65532208 total, 35117428 free, 17669824 used, 12744956 buff/cache + * KiB Swap: 39062524 total, 39062524 free, 0 used. 45520816 avail Mem + * + * PID USER PR NI VIRT RES SHR S %CPU %MEM TIME+ COMMAND + * 3352 meb 20 0 2905940 1,2g 39292 S 0,0 2,0 23:24.01 awesome + * + * We get 5th and 8th fields, which are RES, %CPU, respectively. + */ + ResourceUsageReport pid_to_report(const pid_t & pid) const; + + /** + * @brief Run a terminal command and return the standard output. + * + * @param cmd The terminal command to run + * @return The standard output of the command + */ + std::stringstream run_system_command(const std::string & cmd) const; + + /** + * @brief Parses text from a stringstream into separated words by line. + * + * @param std_out Reference to the input stringstream. + * @return Nested vector with each inner vector containing words from one line. + */ + static VecVecStr parse_lines_into_words(const std::stringstream & std_out); + + /** + * @brief Parses a memory resource string and converts it to bytes. + * + * This function handles memory size strings with suffixes to denote + * the units (e.g., "k" for kibibytes, "m" for mebibytes, etc.). + * If the string has no suffix, it is interpreted as plain bytes. + * + * @param mem_res A string representing the memory resource with a unit suffix or just bytes. + * @return uint64_t The memory size in bytes. + * + * @exception std::runtime_error Thrown if the suffix is not recognized. + */ + static std::uint64_t parse_memory_res(const std::string & mem_res); +}; + +} // namespace autoware::component_monitor + +#endif // COMPONENT_MONITOR_NODE_HPP_ diff --git a/system/autoware_component_monitor/src/unit_conversions.hpp b/system/autoware_component_monitor/src/unit_conversions.hpp new file mode 100644 index 0000000000000..c8f3fa02da519 --- /dev/null +++ b/system/autoware_component_monitor/src/unit_conversions.hpp @@ -0,0 +1,68 @@ +// Copyright 2024 The Autoware Foundation +// +// 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 UNIT_CONVERSIONS_HPP_ +#define UNIT_CONVERSIONS_HPP_ + +#include +#include + +namespace autoware::component_monitor::unit_conversions +{ +template +std::uint64_t kib_to_bytes(T kibibytes) +{ + static_assert(std::is_arithmetic::value, "Template parameter must be a numeric type"); + return static_cast(kibibytes * 1024); +} + +template +std::uint64_t mib_to_bytes(T mebibytes) +{ + static_assert(std::is_arithmetic::value, "Template parameter must be a numeric type"); + return static_cast(mebibytes * 1024 * 1024); +} + +template +std::uint64_t gib_to_bytes(T gibibytes) +{ + static_assert(std::is_arithmetic::value, "Template parameter must be a numeric type"); + return static_cast(gibibytes * 1024ULL * 1024ULL * 1024ULL); +} + +template +std::uint64_t tib_to_bytes(T tebibytes) +{ + static_assert(std::is_arithmetic::value, "Template parameter must be a numeric type"); + return static_cast(tebibytes * 1024ULL * 1024ULL * 1024ULL * 1024ULL); +} + +template +std::uint64_t pib_to_bytes(T pebibytes) +{ + static_assert(std::is_arithmetic::value, "Template parameter must be a numeric type"); + return static_cast(pebibytes * 1024ULL * 1024ULL * 1024ULL * 1024ULL * 1024ULL); +} + +template +std::uint64_t eib_to_bytes(T exbibytes) +{ + static_assert(std::is_arithmetic::value, "Template parameter must be a numeric type"); + return static_cast( + exbibytes * 1024ULL * 1024ULL * 1024ULL * 1024ULL * 1024ULL * 1024ULL); +} + +} // namespace autoware::component_monitor::unit_conversions + +#endif // UNIT_CONVERSIONS_HPP_ diff --git a/system/autoware_component_monitor/test/test_unit_conversions.cpp b/system/autoware_component_monitor/test/test_unit_conversions.cpp new file mode 100644 index 0000000000000..e8104ff31b80e --- /dev/null +++ b/system/autoware_component_monitor/test/test_unit_conversions.cpp @@ -0,0 +1,57 @@ +// Copyright 2024 The Autoware Foundation +// +// 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 "unit_conversions.hpp" + +#include + +namespace autoware::component_monitor::unit_conversions +{ +TEST(UnitConversions, kib_to_bytes) +{ + EXPECT_EQ(kib_to_bytes(1), 1024U); + EXPECT_EQ(kib_to_bytes(0), 0U); + EXPECT_EQ(kib_to_bytes(10), 10240U); +} +TEST(UnitConversions, mib_to_bytes) +{ + EXPECT_EQ(mib_to_bytes(1), 1048576U); + EXPECT_EQ(mib_to_bytes(0), 0U); + EXPECT_EQ(mib_to_bytes(10), 10485760U); +} +TEST(UnitConversions, gib_to_bytes) +{ + EXPECT_EQ(gib_to_bytes(1), 1073741824U); + EXPECT_EQ(gib_to_bytes(0), 0U); + EXPECT_EQ(gib_to_bytes(10), 10737418240U); +} +TEST(UnitConversions, tib_to_bytes) +{ + EXPECT_EQ(tib_to_bytes(1), 1099511627776U); + EXPECT_EQ(tib_to_bytes(0), 0U); + EXPECT_EQ(tib_to_bytes(10), 10995116277760U); +} +TEST(UnitConversions, pib_to_bytes) +{ + EXPECT_EQ(pib_to_bytes(1), 1125899906842624U); + EXPECT_EQ(pib_to_bytes(0), 0U); + EXPECT_EQ(pib_to_bytes(10), 11258999068426240U); +} +TEST(UnitConversions, eib_to_bytes) +{ + EXPECT_EQ(eib_to_bytes(1), 1152921504606846976U); + EXPECT_EQ(eib_to_bytes(0), 0U); + EXPECT_EQ(eib_to_bytes(10), 11529215046068469760U); +} +} // namespace autoware::component_monitor::unit_conversions From 2251ae5d578c2453f6847f23996544cca115a47a Mon Sep 17 00:00:00 2001 From: "awf-autoware-bot[bot]" <94889083+awf-autoware-bot[bot]@users.noreply.github.com> Date: Fri, 12 Jul 2024 17:03:09 +0000 Subject: [PATCH 68/70] chore: update CODEOWNERS (#7926) Signed-off-by: github-actions[bot] <41898282+github-actions[bot]@users.noreply.github.com> Co-authored-by: github-actions --- .github/CODEOWNERS | 45 +++++++++++++++++++++++---------------------- 1 file changed, 23 insertions(+), 22 deletions(-) diff --git a/.github/CODEOWNERS b/.github/CODEOWNERS index 48e4a9921bb9b..a59a5d9ac224e 100644 --- a/.github/CODEOWNERS +++ b/.github/CODEOWNERS @@ -25,7 +25,7 @@ common/kalman_filter/** koji.minoda@tier4.jp takeshi.ishita@tier4.jp yukihiro.sa common/object_recognition_utils/** satoshi.tanaka@tier4.jp shunsuke.miura@tier4.jp takayuki.murooka@tier4.jp yoshi.ri@tier4.jp common/osqp_interface/** fumiya.watanabe@tier4.jp maxime.clement@tier4.jp satoshi.ota@tier4.jp takayuki.murooka@tier4.jp common/path_distance_calculator/** isamu.takagi@tier4.jp -common/perception_utils/** satoshi.tanaka@tier4.jp shunsuke.miura@tier4.jp yoshi.ri@tier4.jp +common/perception_utils/** shunsuke.miura@tier4.jp yoshi.ri@tier4.jp common/polar_grid/** yukihiro.saito@tier4.jp common/qp_interface/** fumiya.watanabe@tier4.jp maxime.clement@tier4.jp satoshi.ota@tier4.jp takayuki.murooka@tier4.jp common/signal_processing/** ali.boyali@tier4.jp fumiya.watanabe@tier4.jp kyoichi.sugahara@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp @@ -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 kyoichi.sugahara@tier4.jp +control/autoware_autonomous_emergency_braking/** daniel.sanchez@tier4.jp kyoichi.sugahara@tier4.jp mamoru.sobue@tier4.jp takamasa.horibe@tier4.jp tomoya.kimura@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 @@ -63,6 +63,7 @@ control/control_performance_analysis/** berkay@leodrive.ai fumiya.watanabe@tier4 control/obstacle_collision_checker/** 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/predicted_path_checker/** berkay@leodrive.ai evaluator/autoware_control_evaluator/** daniel.sanchez@tier4.jp takayuki.murooka@tier4.jp +evaluator/autoware_evaluator_utils/** daniel.sanchez@tier4.jp takayuki.murooka@tier4.jp evaluator/autoware_planning_evaluator/** kyoichi.sugahara@tier4.jp maxime.clement@tier4.jp evaluator/diagnostic_converter/** kyoichi.sugahara@tier4.jp maxime.clement@tier4.jp takamasa.horibe@tier4.jp evaluator/kinematic_evaluator/** dominik.jargot@robotec.ai fumiya.watanabe@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp @@ -70,9 +71,9 @@ evaluator/localization_evaluator/** dominik.jargot@robotec.ai koji.minoda@tier4. evaluator/perception_online_evaluator/** fumiya.watanabe@tier4.jp kosuke.takeuchi@tier4.jp kotaro.uetake@tier4.jp kyoichi.sugahara@tier4.jp shunsuke.miura@tier4.jp yoshi.ri@tier4.jp launch/tier4_autoware_api_launch/** isamu.takagi@tier4.jp ryohsuke.mitsudome@tier4.jp launch/tier4_control_launch/** takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp -launch/tier4_localization_launch/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp koji.minoda@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp -launch/tier4_map_launch/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp koji.minoda@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp -launch/tier4_perception_launch/** shunsuke.miura@tier4.jp yoshi.ri@tier4.jp yukihiro.saito@tier4.jp +launch/tier4_localization_launch/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp +launch/tier4_map_launch/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp +launch/tier4_perception_launch/** shunsuke.miura@tier4.jp taekjin.lee@tier4.jp yoshi.ri@tier4.jp yukihiro.saito@tier4.jp launch/tier4_planning_launch/** fumiya.watanabe@tier4.jp kosuke.takeuchi@tier4.jp kyoichi.sugahara@tier4.jp makoto.kurihara@tier4.jp mamoru.sobue@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp taiki.tanaka@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp zhe.shen@tier4.jp zulfaqar.azmi@tier4.jp launch/tier4_sensing_launch/** yukihiro.saito@tier4.jp launch/tier4_simulator_launch/** keisuke.shima@tier4.jp taiki.tanaka@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp @@ -81,30 +82,36 @@ launch/tier4_vehicle_launch/** yukihiro.saito@tier4.jp localization/autoware_landmark_based_localizer/autoware_ar_tag_based_localizer/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp localization/autoware_landmark_based_localizer/autoware_landmark_manager/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp localization/autoware_pose_covariance_modifier/** melike@leodrive.ai -localization/autoware_stop_filter/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp koji.minoda@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp -localization/ekf_localizer/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp koji.minoda@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp takamasa.horibe@tier4.jp takeshi.ishita@tier4.jp yamato.ando@tier4.jp -localization/geo_pose_projector/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp koji.minoda@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp +localization/autoware_stop_filter/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp +localization/ekf_localizer/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp takamasa.horibe@tier4.jp takeshi.ishita@tier4.jp yamato.ando@tier4.jp +localization/geo_pose_projector/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp localization/gyro_odometer/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp -localization/localization_error_monitor/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp koji.minoda@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp +localization/localization_error_monitor/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp localization/localization_util/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp -localization/ndt_scan_matcher/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp koji.minoda@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp +localization/ndt_scan_matcher/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp localization/pose2twist/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp localization/pose_estimator_arbiter/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp localization/pose_initializer/** anh.nguyen.2@tier4.jp isamu.takagi@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp localization/pose_instability_detector/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp -localization/twist2accel/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp koji.minoda@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp +localization/twist2accel/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp localization/yabloc/yabloc_common/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp localization/yabloc/yabloc_image_processing/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp -localization/yabloc/yabloc_monitor/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp koji.minoda@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp +localization/yabloc/yabloc_monitor/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp localization/yabloc/yabloc_particle_filter/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp localization/yabloc/yabloc_pose_initializer/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp map/map_height_fitter/** anh.nguyen.2@tier4.jp isamu.takagi@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp -map/map_loader/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp koji.minoda@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp -map/map_projection_loader/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp koji.minoda@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp +map/map_loader/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp +map/map_projection_loader/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp map/map_tf_generator/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp map/util/lanelet2_map_preprocessor/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp perception/autoware_crosswalk_traffic_light_estimator/** satoshi.ota@tier4.jp shunsuke.miura@tier4.jp tao.zhong@tier4.jp perception/autoware_map_based_prediction/** kotaro.uetake@tier4.jp kyoichi.sugahara@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp yoshi.ri@tier4.jp +perception/autoware_object_range_splitter/** yoshi.ri@tier4.jp yukihiro.saito@tier4.jp +perception/autoware_object_velocity_splitter/** satoshi.tanaka@tier4.jp shunsuke.miura@tier4.jp yoshi.ri@tier4.jp +perception/autoware_radar_crossing_objects_noise_filter/** satoshi.tanaka@tier4.jp shunsuke.miura@tier4.jp taekjin.lee@tier4.jp yoshi.ri@tier4.jp +perception/autoware_radar_fusion_to_detected_object/** satoshi.tanaka@tier4.jp shunsuke.miura@tier4.jp taekjin.lee@tier4.jp yoshi.ri@tier4.jp +perception/autoware_radar_object_clustering/** satoshi.tanaka@tier4.jp shunsuke.miura@tier4.jp taekjin.lee@tier4.jp yoshi.ri@tier4.jp +perception/autoware_radar_object_tracker/** taekjin.lee@tier4.jp yoshi.ri@tier4.jp yukihiro.saito@tier4.jp perception/bytetrack/** manato.hirabayashi@tier4.jp yoshi.ri@tier4.jp perception/cluster_merger/** dai.nguyen@tier4.jp shunsuke.miura@tier4.jp yukihiro.saito@tier4.jp perception/compare_map_segmentation/** abrahammonrroy@yahoo.com dai.nguyen@tier4.jp yukihiro.saito@tier4.jp @@ -122,14 +129,8 @@ perception/lidar_centerpoint/** kenzo.lobos@tier4.jp koji.minoda@tier4.jp perception/lidar_transfusion/** amadeusz.szymko.2@tier4.jp kenzo.lobos@tier4.jp satoshi.tanaka@tier4.jp perception/multi_object_tracker/** taekjin.lee@tier4.jp yoshi.ri@tier4.jp yukihiro.saito@tier4.jp perception/object_merger/** taekjin.lee@tier4.jp yoshi.ri@tier4.jp yukihiro.saito@tier4.jp -perception/autoware_object_range_splitter/** yoshi.ri@tier4.jp yukihiro.saito@tier4.jp -perception/autoware_object_velocity_splitter/** satoshi.tanaka@tier4.jp shunsuke.miura@tier4.jp yoshi.ri@tier4.jp perception/occupancy_grid_map_outlier_filter/** abrahammonrroy@yahoo.com yoshi.ri@tier4.jp yukihiro.saito@tier4.jp perception/probabilistic_occupancy_grid_map/** mamoru.sobue@tier4.jp yoshi.ri@tier4.jp yukihiro.saito@tier4.jp -perception/autoware_radar_crossing_objects_noise_filter/** satoshi.tanaka@tier4.jp shunsuke.miura@tier4.jp taekjin.lee@tier4.jp yoshi.ri@tier4.jp -perception/autoware_radar_fusion_to_detected_object/** satoshi.tanaka@tier4.jp shunsuke.miura@tier4.jp taekjin.lee@tier4.jp yoshi.ri@tier4.jp -perception/autoware_radar_object_clustering/** satoshi.tanaka@tier4.jp shunsuke.miura@tier4.jp taekjin.lee@tier4.jp yoshi.ri@tier4.jp -perception/autoware_radar_object_tracker/** satoshi.tanaka@tier4.jp taekjin.lee@tier4.jp yoshi.ri@tier4.jp yukihiro.saito@tier4.jp perception/radar_tracks_msgs_converter/** satoshi.tanaka@tier4.jp shunsuke.miura@tier4.jp taekjin.lee@tier4.jp yoshi.ri@tier4.jp perception/raindrop_cluster_filter/** dai.nguyen@tier4.jp yoshi.ri@tier4.jp yukihiro.saito@tier4.jp perception/shape_estimation/** yoshi.ri@tier4.jp yukihiro.saito@tier4.jp @@ -200,10 +201,10 @@ planning/sampling_based_planner/autoware_bezier_sampler/** maxime.clement@tier4. planning/sampling_based_planner/autoware_frenet_planner/** maxime.clement@tier4.jp planning/sampling_based_planner/autoware_path_sampler/** maxime.clement@tier4.jp planning/sampling_based_planner/autoware_sampler_common/** maxime.clement@tier4.jp -sensing/gnss_poser/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp koji.minoda@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp +sensing/gnss_poser/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp sensing/image_diagnostics/** dai.nguyen@tier4.jp yoshi.ri@tier4.jp sensing/image_transport_decompressor/** kenzo.lobos@tier4.jp yukihiro.saito@tier4.jp -sensing/imu_corrector/** koji.minoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp +sensing/imu_corrector/** taiki.yamada@tier4.jp yamato.ando@tier4.jp sensing/livox/autoware_livox_tag_filter/** kenzo.lobos@tier4.jp ryohsuke.mitsudome@tier4.jp sensing/pointcloud_preprocessor/** abrahammonrroy@yahoo.com dai.nguyen@tier4.jp david.wong@tier4.jp kenzo.lobos@tier4.jp kyoichi.sugahara@tier4.jp melike@leodrive.ai shunsuke.miura@tier4.jp yihsiang.fang@tier4.jp yoshi.ri@tier4.jp yukihiro.saito@tier4.jp sensing/radar_scan_to_pointcloud2/** satoshi.tanaka@tier4.jp shunsuke.miura@tier4.jp taekjin.lee@tier4.jp yoshi.ri@tier4.jp From 0d504f79392e6d05be6caefc106f95173365fd73 Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Sat, 13 Jul 2024 14:27:11 +0900 Subject: [PATCH 69/70] feat(velocity_smoother): add time_keeper (#8026) Signed-off-by: Takayuki Murooka --- .../autoware/velocity_smoother/node.hpp | 5 +++ .../analytical_jerk_constrained_smoother.hpp | 6 +++- .../smoother/jerk_filtered_smoother.hpp | 5 ++- .../smoother/l2_pseudo_jerk_smoother.hpp | 5 ++- .../smoother/linf_pseudo_jerk_smoother.hpp | 5 ++- .../smoother/smoother_base.hpp | 6 +++- .../autoware_velocity_smoother/src/node.cpp | 35 ++++++++++++++++--- .../analytical_jerk_constrained_smoother.cpp | 5 +-- .../src/smoother/jerk_filtered_smoother.cpp | 20 ++++++++++- .../src/smoother/l2_pseudo_jerk_smoother.cpp | 4 ++- .../smoother/linf_pseudo_jerk_smoother.cpp | 4 ++- .../src/smoother/smoother_base.cpp | 8 ++++- 12 files changed, 93 insertions(+), 15 deletions(-) diff --git a/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/node.hpp b/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/node.hpp index 0dd18615be5ef..26918cce24549 100644 --- a/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/node.hpp +++ b/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/node.hpp @@ -23,6 +23,7 @@ #include "autoware/universe_utils/ros/polling_subscriber.hpp" #include "autoware/universe_utils/ros/self_pose_listener.hpp" #include "autoware/universe_utils/system/stop_watch.hpp" +#include "autoware/universe_utils/system/time_keeper.hpp" #include "autoware/velocity_smoother/resample.hpp" #include "autoware/velocity_smoother/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.hpp" #include "autoware/velocity_smoother/smoother/jerk_filtered_smoother.hpp" @@ -260,6 +261,8 @@ class VelocitySmootherNode : public rclcpp::Node rclcpp::Publisher::SharedPtr debug_closest_jerk_; rclcpp::Publisher::SharedPtr debug_calculation_time_; rclcpp::Publisher::SharedPtr debug_closest_max_velocity_; + rclcpp::Publisher::SharedPtr + debug_processing_time_detail_; // For Jerk Filtered Algorithm Debug rclcpp::Publisher::SharedPtr pub_forward_filtered_trajectory_; @@ -275,6 +278,8 @@ class VelocitySmootherNode : public rclcpp::Node std::unique_ptr logger_configure_; std::unique_ptr published_time_publisher_; + + mutable std::shared_ptr time_keeper_{nullptr}; }; } // namespace autoware::velocity_smoother diff --git a/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.hpp b/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.hpp index 42d5a520c9e44..bbc3828bb1b15 100644 --- a/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.hpp +++ b/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.hpp @@ -16,6 +16,7 @@ #define AUTOWARE__VELOCITY_SMOOTHER__SMOOTHER__ANALYTICAL_JERK_CONSTRAINED_SMOOTHER__ANALYTICAL_JERK_CONSTRAINED_SMOOTHER_HPP_ // NOLINT #include "autoware/motion_utils/trajectory/trajectory.hpp" +#include "autoware/universe_utils/system/time_keeper.hpp" #include "autoware/velocity_smoother/smoother/analytical_jerk_constrained_smoother/velocity_planning_utils.hpp" #include "autoware/velocity_smoother/smoother/smoother_base.hpp" #include "rclcpp/rclcpp.hpp" @@ -24,6 +25,7 @@ #include "autoware_planning_msgs/msg/trajectory_point.hpp" #include "geometry_msgs/msg/pose.hpp" +#include #include #include #include @@ -65,7 +67,9 @@ class AnalyticalJerkConstrainedSmoother : public SmootherBase } backward; }; - explicit AnalyticalJerkConstrainedSmoother(rclcpp::Node & node); + explicit AnalyticalJerkConstrainedSmoother( + rclcpp::Node & node, const std::shared_ptr time_keeper = + std::make_shared()); bool apply( const double initial_vel, const double initial_acc, const TrajectoryPoints & input, diff --git a/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/smoother/jerk_filtered_smoother.hpp b/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/smoother/jerk_filtered_smoother.hpp index 14d0d8ab7ff84..06a6f2da7f30a 100644 --- a/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/smoother/jerk_filtered_smoother.hpp +++ b/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/smoother/jerk_filtered_smoother.hpp @@ -17,6 +17,7 @@ #include "autoware/motion_utils/trajectory/trajectory.hpp" #include "autoware/universe_utils/geometry/geometry.hpp" +#include "autoware/universe_utils/system/time_keeper.hpp" #include "autoware/velocity_smoother/smoother/smoother_base.hpp" #include "osqp_interface/osqp_interface.hpp" @@ -24,6 +25,7 @@ #include "boost/optional.hpp" +#include #include namespace autoware::velocity_smoother @@ -40,7 +42,8 @@ class JerkFilteredSmoother : public SmootherBase double jerk_filter_ds; }; - explicit JerkFilteredSmoother(rclcpp::Node & node); + explicit JerkFilteredSmoother( + rclcpp::Node & node, const std::shared_ptr time_keeper); bool apply( const double initial_vel, const double initial_acc, const TrajectoryPoints & input, diff --git a/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/smoother/l2_pseudo_jerk_smoother.hpp b/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/smoother/l2_pseudo_jerk_smoother.hpp index 1e2918d8e313b..a2a07ec6909aa 100644 --- a/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/smoother/l2_pseudo_jerk_smoother.hpp +++ b/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/smoother/l2_pseudo_jerk_smoother.hpp @@ -17,6 +17,7 @@ #include "autoware/motion_utils/trajectory/trajectory.hpp" #include "autoware/universe_utils/geometry/geometry.hpp" +#include "autoware/universe_utils/system/time_keeper.hpp" #include "autoware/velocity_smoother/smoother/smoother_base.hpp" #include "osqp_interface/osqp_interface.hpp" @@ -24,6 +25,7 @@ #include "boost/optional.hpp" +#include #include namespace autoware::velocity_smoother @@ -38,7 +40,8 @@ class L2PseudoJerkSmoother : public SmootherBase double over_a_weight; }; - explicit L2PseudoJerkSmoother(rclcpp::Node & node); + explicit L2PseudoJerkSmoother( + rclcpp::Node & node, const std::shared_ptr time_keeper); bool apply( const double initial_vel, const double initial_acc, const TrajectoryPoints & input, diff --git a/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/smoother/linf_pseudo_jerk_smoother.hpp b/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/smoother/linf_pseudo_jerk_smoother.hpp index b88cd26eb4467..7c46a53431608 100644 --- a/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/smoother/linf_pseudo_jerk_smoother.hpp +++ b/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/smoother/linf_pseudo_jerk_smoother.hpp @@ -17,6 +17,7 @@ #include "autoware/motion_utils/trajectory/trajectory.hpp" #include "autoware/universe_utils/geometry/geometry.hpp" +#include "autoware/universe_utils/system/time_keeper.hpp" #include "autoware/velocity_smoother/smoother/smoother_base.hpp" #include "osqp_interface/osqp_interface.hpp" @@ -24,6 +25,7 @@ #include "boost/optional.hpp" +#include #include namespace autoware::velocity_smoother @@ -38,7 +40,8 @@ class LinfPseudoJerkSmoother : public SmootherBase double over_a_weight; }; - explicit LinfPseudoJerkSmoother(rclcpp::Node & node); + explicit LinfPseudoJerkSmoother( + rclcpp::Node & node, const std::shared_ptr time_keeper); bool apply( const double initial_vel, const double initial_acc, const TrajectoryPoints & input, diff --git a/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/smoother/smoother_base.hpp b/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/smoother/smoother_base.hpp index be7baf694d361..6671eaa3eabe7 100644 --- a/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/smoother/smoother_base.hpp +++ b/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/smoother/smoother_base.hpp @@ -15,12 +15,14 @@ #ifndef AUTOWARE__VELOCITY_SMOOTHER__SMOOTHER__SMOOTHER_BASE_HPP_ #define AUTOWARE__VELOCITY_SMOOTHER__SMOOTHER__SMOOTHER_BASE_HPP_ +#include "autoware/universe_utils/system/time_keeper.hpp" #include "autoware/velocity_smoother/resample.hpp" #include "rclcpp/rclcpp.hpp" #include "autoware_planning_msgs/msg/trajectory_point.hpp" #include +#include #include namespace autoware::velocity_smoother @@ -54,7 +56,8 @@ class SmootherBase resampling::ResampleParam resample_param; }; - explicit SmootherBase(rclcpp::Node & node); + explicit SmootherBase( + rclcpp::Node & node, const std::shared_ptr time_keeper); virtual ~SmootherBase() = default; virtual bool apply( const double initial_vel, const double initial_acc, const TrajectoryPoints & input, @@ -85,6 +88,7 @@ class SmootherBase protected: BaseParam base_param_; + mutable std::shared_ptr time_keeper_{nullptr}; }; } // namespace autoware::velocity_smoother diff --git a/planning/autoware_velocity_smoother/src/node.cpp b/planning/autoware_velocity_smoother/src/node.cpp index b881b68798cdc..ca7526e9adf3b 100644 --- a/planning/autoware_velocity_smoother/src/node.cpp +++ b/planning/autoware_velocity_smoother/src/node.cpp @@ -46,6 +46,13 @@ VelocitySmootherNode::VelocitySmootherNode(const rclcpp::NodeOptions & node_opti initCommonParam(); over_stop_velocity_warn_thr_ = declare_parameter("over_stop_velocity_warn_thr"); + // create time_keeper and its publisher + // NOTE: This has to be called before setupSmoother to pass the time_keeper to the smoother. + debug_processing_time_detail_ = create_publisher( + "~/debug/processing_time_detail_ms", 1); + time_keeper_ = + std::make_shared(debug_processing_time_detail_); + // create smoother setupSmoother(wheelbase_); @@ -99,7 +106,7 @@ void VelocitySmootherNode::setupSmoother(const double wheelbase) { switch (node_param_.algorithm_type) { case AlgorithmType::JERK_FILTERED: { - smoother_ = std::make_shared(*this); + smoother_ = std::make_shared(*this, time_keeper_); // Set Publisher for jerk filtered algorithm pub_forward_filtered_trajectory_ = @@ -113,15 +120,15 @@ void VelocitySmootherNode::setupSmoother(const double wheelbase) break; } case AlgorithmType::L2: { - smoother_ = std::make_shared(*this); + smoother_ = std::make_shared(*this, time_keeper_); break; } case AlgorithmType::LINF: { - smoother_ = std::make_shared(*this); + smoother_ = std::make_shared(*this, time_keeper_); break; } case AlgorithmType::ANALYTICAL: { - smoother_ = std::make_shared(*this); + smoother_ = std::make_shared(*this, time_keeper_); break; } default: @@ -309,6 +316,8 @@ void VelocitySmootherNode::publishTrajectory(const TrajectoryPoints & trajectory void VelocitySmootherNode::calcExternalVelocityLimit() { + autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + if (!external_velocity_limit_ptr_) { return; } @@ -414,6 +423,8 @@ bool VelocitySmootherNode::checkData() const void VelocitySmootherNode::onCurrentTrajectory(const Trajectory::ConstSharedPtr msg) { + autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + RCLCPP_DEBUG(get_logger(), "========================= run start ========================="); stop_watch_.tic(); @@ -502,6 +513,8 @@ void VelocitySmootherNode::onCurrentTrajectory(const Trajectory::ConstSharedPtr void VelocitySmootherNode::updateDataForExternalVelocityLimit() { + autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + if (prev_output_.empty()) { return; } @@ -519,6 +532,8 @@ void VelocitySmootherNode::updateDataForExternalVelocityLimit() TrajectoryPoints VelocitySmootherNode::calcTrajectoryVelocity( const TrajectoryPoints & traj_input) const { + autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + TrajectoryPoints output{}; // velocity is optimized by qp solver // Extract trajectory around self-position with desired forward-backward length @@ -566,6 +581,8 @@ bool VelocitySmootherNode::smoothVelocity( const TrajectoryPoints & input, const size_t input_closest, TrajectoryPoints & traj_smoothed) const { + autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + if (input.empty()) { return false; // cannot apply smoothing } @@ -672,6 +689,8 @@ bool VelocitySmootherNode::smoothVelocity( void VelocitySmootherNode::insertBehindVelocity( const size_t output_closest, const InitializeType type, TrajectoryPoints & output) const { + autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + const bool keep_closest_vel_for_behind = (type == InitializeType::EGO_VELOCITY || type == InitializeType::LARGE_DEVIATION_REPLAN || type == InitializeType::ENGAGING); @@ -734,6 +753,8 @@ void VelocitySmootherNode::publishStopDistance(const TrajectoryPoints & trajecto std::pair VelocitySmootherNode::calcInitialMotion( const TrajectoryPoints & input_traj, const size_t input_closest) const { + autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + const double vehicle_speed = std::fabs(current_odometry_ptr_->twist.twist.linear.x); const double vehicle_acceleration = current_acceleration_ptr_->accel.accel.linear.x; const double target_vel = std::fabs(input_traj.at(input_closest).longitudinal_velocity_mps); @@ -817,6 +838,8 @@ std::pair VelocitySmootherNode::ca void VelocitySmootherNode::overwriteStopPoint( const TrajectoryPoints & input, TrajectoryPoints & output) const { + autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + const auto stop_idx = autoware::motion_utils::searchZeroVelocityIndex(input); if (!stop_idx) { return; @@ -863,6 +886,8 @@ void VelocitySmootherNode::overwriteStopPoint( void VelocitySmootherNode::applyExternalVelocityLimit(TrajectoryPoints & traj) const { + autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + if (traj.size() < 1) { return; } @@ -902,6 +927,8 @@ void VelocitySmootherNode::applyExternalVelocityLimit(TrajectoryPoints & traj) c void VelocitySmootherNode::applyStopApproachingVelocity(TrajectoryPoints & traj) const { + autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + const auto stop_idx = autoware::motion_utils::searchZeroVelocityIndex(traj); if (!stop_idx) { return; // no stop point. diff --git a/planning/autoware_velocity_smoother/src/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.cpp b/planning/autoware_velocity_smoother/src/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.cpp index 7f263fdea5e36..3906222454d35 100644 --- a/planning/autoware_velocity_smoother/src/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.cpp +++ b/planning/autoware_velocity_smoother/src/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.cpp @@ -66,8 +66,9 @@ bool applyMaxVelocity( namespace autoware::velocity_smoother { -AnalyticalJerkConstrainedSmoother::AnalyticalJerkConstrainedSmoother(rclcpp::Node & node) -: SmootherBase(node) +AnalyticalJerkConstrainedSmoother::AnalyticalJerkConstrainedSmoother( + rclcpp::Node & node, const std::shared_ptr time_keeper) +: SmootherBase(node, time_keeper) { auto & p = smoother_param_; p.resample.ds_resample = node.declare_parameter("resample.ds_resample"); diff --git a/planning/autoware_velocity_smoother/src/smoother/jerk_filtered_smoother.cpp b/planning/autoware_velocity_smoother/src/smoother/jerk_filtered_smoother.cpp index cab8e49a3b45d..d458c688d060c 100644 --- a/planning/autoware_velocity_smoother/src/smoother/jerk_filtered_smoother.cpp +++ b/planning/autoware_velocity_smoother/src/smoother/jerk_filtered_smoother.cpp @@ -29,7 +29,9 @@ namespace autoware::velocity_smoother { -JerkFilteredSmoother::JerkFilteredSmoother(rclcpp::Node & node) : SmootherBase(node) +JerkFilteredSmoother::JerkFilteredSmoother( + rclcpp::Node & node, const std::shared_ptr time_keeper) +: SmootherBase(node, time_keeper) { auto & p = smoother_param_; p.jerk_weight = node.declare_parameter("jerk_weight"); @@ -59,6 +61,8 @@ bool JerkFilteredSmoother::apply( const double v0, const double a0, const TrajectoryPoints & input, TrajectoryPoints & output, std::vector & debug_trajectories) { + autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + output = input; if (input.empty()) { @@ -102,6 +106,8 @@ bool JerkFilteredSmoother::apply( const auto initial_traj_pose = filtered.front().pose; const auto resample = [&](const auto & trajectory) { + autoware::universe_utils::ScopedTimeTrack st("resample", *time_keeper_); + return resampling::resampleTrajectory( trajectory, v0, initial_traj_pose, std::numeric_limits::max(), std::numeric_limits::max(), base_param_.resample_param); @@ -152,6 +158,7 @@ bool JerkFilteredSmoother::apply( v_max_arr.at(i) = opt_resampled_trajectory.at(i).longitudinal_velocity_mps; } + time_keeper_->start_track("initOptimization"); /* * x = [ * b[0], b[1], ..., b[N], : 0~N @@ -290,9 +297,12 @@ bool JerkFilteredSmoother::apply( lower_bound[constr_idx] = a0; ++constr_idx; } + time_keeper_->end_track("initOptimization"); // execute optimization + time_keeper_->start_track("optimize"); const auto result = qp_solver_.optimize(P, A, q, lower_bound, upper_bound); + time_keeper_->end_track("optimize"); const std::vector optval = std::get<0>(result); const int status_val = std::get<3>(result); if (status_val != 1) { @@ -356,6 +366,8 @@ TrajectoryPoints JerkFilteredSmoother::forwardJerkFilter( const double v0, const double a0, const double a_max, const double a_start, const double j_max, const TrajectoryPoints & input) const { + autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + auto applyLimits = [&input, &a_start](double & v, double & a, size_t i) { double v_lim = input.at(i).longitudinal_velocity_mps; static constexpr double ep = 1.0e-5; @@ -408,6 +420,8 @@ TrajectoryPoints JerkFilteredSmoother::backwardJerkFilter( const double v0, const double a0, const double a_min, const double a_stop, const double j_min, const TrajectoryPoints & input) const { + autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + auto input_rev = input; std::reverse(input_rev.begin(), input_rev.end()); auto filtered = forwardJerkFilter( @@ -423,6 +437,8 @@ TrajectoryPoints JerkFilteredSmoother::mergeFilteredTrajectory( const double v0, const double a0, const double a_min, const double j_min, const TrajectoryPoints & forward_filtered, const TrajectoryPoints & backward_filtered) const { + autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + TrajectoryPoints merged; merged = forward_filtered; @@ -475,6 +491,8 @@ TrajectoryPoints JerkFilteredSmoother::resampleTrajectory( const geometry_msgs::msg::Pose & current_pose, const double nearest_dist_threshold, const double nearest_yaw_threshold) const { + autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + return resampling::resampleTrajectory( input, current_pose, nearest_dist_threshold, nearest_yaw_threshold, base_param_.resample_param, smoother_param_.jerk_filter_ds); diff --git a/planning/autoware_velocity_smoother/src/smoother/l2_pseudo_jerk_smoother.cpp b/planning/autoware_velocity_smoother/src/smoother/l2_pseudo_jerk_smoother.cpp index b489c994b0495..f379b217187c9 100644 --- a/planning/autoware_velocity_smoother/src/smoother/l2_pseudo_jerk_smoother.cpp +++ b/planning/autoware_velocity_smoother/src/smoother/l2_pseudo_jerk_smoother.cpp @@ -25,7 +25,9 @@ namespace autoware::velocity_smoother { -L2PseudoJerkSmoother::L2PseudoJerkSmoother(rclcpp::Node & node) : SmootherBase(node) +L2PseudoJerkSmoother::L2PseudoJerkSmoother( + rclcpp::Node & node, const std::shared_ptr time_keeper) +: SmootherBase(node, time_keeper) { auto & p = smoother_param_; p.pseudo_jerk_weight = node.declare_parameter("pseudo_jerk_weight"); diff --git a/planning/autoware_velocity_smoother/src/smoother/linf_pseudo_jerk_smoother.cpp b/planning/autoware_velocity_smoother/src/smoother/linf_pseudo_jerk_smoother.cpp index cb8451dba8302..2ab3d6dd80dfc 100644 --- a/planning/autoware_velocity_smoother/src/smoother/linf_pseudo_jerk_smoother.cpp +++ b/planning/autoware_velocity_smoother/src/smoother/linf_pseudo_jerk_smoother.cpp @@ -25,7 +25,9 @@ namespace autoware::velocity_smoother { -LinfPseudoJerkSmoother::LinfPseudoJerkSmoother(rclcpp::Node & node) : SmootherBase(node) +LinfPseudoJerkSmoother::LinfPseudoJerkSmoother( + rclcpp::Node & node, const std::shared_ptr time_keeper) +: SmootherBase(node, time_keeper) { auto & p = smoother_param_; p.pseudo_jerk_weight = node.declare_parameter("pseudo_jerk_weight"); diff --git a/planning/autoware_velocity_smoother/src/smoother/smoother_base.cpp b/planning/autoware_velocity_smoother/src/smoother/smoother_base.cpp index 700cf45b7eb9d..46faf10fe4a62 100644 --- a/planning/autoware_velocity_smoother/src/smoother/smoother_base.cpp +++ b/planning/autoware_velocity_smoother/src/smoother/smoother_base.cpp @@ -60,7 +60,9 @@ TrajectoryPoints applyPreProcess( } } // namespace -SmootherBase::SmootherBase(rclcpp::Node & node) +SmootherBase::SmootherBase( + rclcpp::Node & node, const std::shared_ptr time_keeper) +: time_keeper_(time_keeper) { auto & p = base_param_; p.max_accel = node.declare_parameter("normal.max_acc"); @@ -130,6 +132,8 @@ TrajectoryPoints SmootherBase::applyLateralAccelerationFilter( [[maybe_unused]] const double a0, [[maybe_unused]] const bool enable_smooth_limit, const bool use_resampling, const double input_points_interval) const { + autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + if (input.size() < 3) { return input; // cannot calculate lateral acc. do nothing. } @@ -198,6 +202,8 @@ TrajectoryPoints SmootherBase::applySteeringRateLimit( const TrajectoryPoints & input, const bool use_resampling, const double input_points_interval) const { + autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + if (input.size() < 3) { return input; // cannot calculate the desired velocity. do nothing. } From 0eae67c50eaa41066c0b5e518a540828350f7f0f Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Sat, 13 Jul 2024 14:56:46 +0900 Subject: [PATCH 70/70] refactor(qp_interface): clean up the code (#8029) * refactor qp_interface Signed-off-by: Takayuki Murooka * variable names: m_XXX -> XXX_ Signed-off-by: Takayuki Murooka * update Signed-off-by: Takayuki Murooka * update Signed-off-by: Takayuki Murooka --------- Signed-off-by: Takayuki Murooka --- .../qp_interface/osqp_csc_matrix_conv.hpp | 10 +- .../include/qp_interface/osqp_interface.hpp | 42 ++-- .../include/qp_interface/proxqp_interface.hpp | 18 +- .../include/qp_interface/qp_interface.hpp | 18 +- .../qp_interface/src/osqp_csc_matrix_conv.cpp | 10 +- common/qp_interface/src/osqp_interface.cpp | 236 +++++++++--------- common/qp_interface/src/proxqp_interface.cpp | 84 +++++-- common/qp_interface/src/qp_interface.cpp | 8 +- .../test/test_csc_matrix_conv.cpp | 210 ++++++++-------- .../qp_interface/test/test_osqp_interface.cpp | 64 ++--- .../test/test_proxqp_interface.cpp | 21 +- 11 files changed, 377 insertions(+), 344 deletions(-) diff --git a/common/qp_interface/include/qp_interface/osqp_csc_matrix_conv.hpp b/common/qp_interface/include/qp_interface/osqp_csc_matrix_conv.hpp index 74ec4c1282f46..8ec7280cfc100 100644 --- a/common/qp_interface/include/qp_interface/osqp_csc_matrix_conv.hpp +++ b/common/qp_interface/include/qp_interface/osqp_csc_matrix_conv.hpp @@ -21,17 +21,17 @@ #include -namespace qp +namespace autoware::common { /// \brief Compressed-Column-Sparse Matrix struct CSC_Matrix { /// Vector of non-zero values. Ex: [4,1,1,2] - std::vector m_vals; + std::vector vals_; /// Vector of row index corresponding to values. Ex: [0, 1, 0, 1] (Eigen: 'inner') - std::vector m_row_idxs; + std::vector row_idxs_; /// Vector of 'val' indices where each column starts. Ex: [0, 2, 4] (Eigen: 'outer') - std::vector m_col_idxs; + std::vector col_idxs_; }; /// \brief Calculate CSC matrix from Eigen matrix @@ -41,6 +41,6 @@ CSC_Matrix calCSCMatrixTrapezoidal(const Eigen::MatrixXd & mat); /// \brief Print the given CSC matrix to the standard output void printCSCMatrix(const CSC_Matrix & csc_mat); -} // namespace qp +} // namespace autoware::common #endif // QP_INTERFACE__OSQP_CSC_MATRIX_CONV_HPP_ diff --git a/common/qp_interface/include/qp_interface/osqp_interface.hpp b/common/qp_interface/include/qp_interface/osqp_interface.hpp index ef2c3bd17c89e..14c03a91d8fa1 100644 --- a/common/qp_interface/include/qp_interface/osqp_interface.hpp +++ b/common/qp_interface/include/qp_interface/osqp_interface.hpp @@ -24,9 +24,9 @@ #include #include -namespace qp +namespace autoware::common { -constexpr c_float INF = 1e30; +constexpr c_float OSQP_INF = 1e30; class OSQPInterface : public QPInterface { @@ -34,7 +34,9 @@ class OSQPInterface : public QPInterface /// \brief Constructor without problem formulation OSQPInterface( const bool enable_warm_start = false, - const c_float eps_abs = std::numeric_limits::epsilon(), const bool polish = true); + const c_float eps_abs = std::numeric_limits::epsilon(), + const c_float eps_rel = std::numeric_limits::epsilon(), const bool polish = true, + const bool verbose = false); /// \brief Constructor with problem setup /// \param P: (n,n) matrix defining relations between parameters. /// \param A: (m,n) matrix defining parameter constraints relative to the lower and upper bound. @@ -60,8 +62,10 @@ class OSQPInterface : public QPInterface CSC_Matrix P, CSC_Matrix A, const std::vector & q, const std::vector & l, const std::vector & u); - int getIteration() const override; - int getStatus() const override; + int getIterationNumber() const override; + bool isSolved() const override; + std::string getStatus() const override; + int getPolishStatus() const; std::vector getDualSolution() const; @@ -96,20 +100,18 @@ class OSQPInterface : public QPInterface void updateCheckTermination(const int check_termination); /// \brief Get the number of iteration taken to solve the problem - inline int64_t getTakenIter() const { return static_cast(m_latest_work_info.iter); } + inline int64_t getTakenIter() const { return static_cast(latest_work_info_.iter); } /// \brief Get the status message for the latest problem solved inline std::string getStatusMessage() const { - return static_cast(m_latest_work_info.status); + return static_cast(latest_work_info_.status); } /// \brief Get the runtime of the latest problem solved - inline double getRunTime() const { return m_latest_work_info.run_time; } + inline double getRunTime() const { return latest_work_info_.run_time; } /// \brief Get the objective value the latest problem solved - inline double getObjVal() const { return m_latest_work_info.obj_val; } + inline double getObjVal() const { return latest_work_info_.obj_val; } /// \brief Returns flag asserting interface condition (Healthy condition: 0). - inline int64_t getExitFlag() const { return m_exitflag; } - - void logUnsolvedStatus(const std::string & prefix_message = "") const; + inline int64_t getExitFlag() const { return exitflag_; } // Setter functions for warm start bool setWarmStart( @@ -118,17 +120,17 @@ class OSQPInterface : public QPInterface bool setDualVariables(const std::vector & dual_variables); private: - std::unique_ptr> m_work; - std::unique_ptr m_settings; - std::unique_ptr m_data; + std::unique_ptr> work_; + std::unique_ptr settings_; + std::unique_ptr data_; // store last work info since work is cleaned up at every execution to prevent memory leak. - OSQPInfo m_latest_work_info; + OSQPInfo latest_work_info_; // Number of parameters to optimize - int64_t m_param_n; + int64_t param_n_; // Flag to check if the current work exists - bool m_work_initialized = false; + bool work__initialized = false; // Exitflag - int64_t m_exitflag; + int64_t exitflag_; void initializeProblemImpl( const Eigen::MatrixXd & P, const Eigen::MatrixXd & A, const std::vector & q, @@ -140,6 +142,6 @@ class OSQPInterface : public QPInterface std::vector optimizeImpl() override; }; -} // namespace qp +} // namespace autoware::common #endif // QP_INTERFACE__OSQP_INTERFACE_HPP_ diff --git a/common/qp_interface/include/qp_interface/proxqp_interface.hpp b/common/qp_interface/include/qp_interface/proxqp_interface.hpp index a940262d5f6da..f75fb3da5954c 100644 --- a/common/qp_interface/include/qp_interface/proxqp_interface.hpp +++ b/common/qp_interface/include/qp_interface/proxqp_interface.hpp @@ -22,27 +22,29 @@ #include #include +#include #include -namespace qp +namespace autoware::common { class ProxQPInterface : public QPInterface { public: explicit ProxQPInterface( - const bool enable_warm_start = false, - const double eps_abs = std::numeric_limits::epsilon()); + const bool enable_warm_start, const double eps_abs, const double eps_rel, + const bool verbose = false); - int getIteration() const override; - int getStatus() const override; + int getIterationNumber() const override; + bool isSolved() const override; + std::string getStatus() const override; void updateEpsAbs(const double eps_abs) override; void updateEpsRel(const double eps_rel) override; void updateVerbose(const bool verbose) override; private: - proxsuite::proxqp::Settings m_settings; - std::shared_ptr> m_qp_ptr; + proxsuite::proxqp::Settings settings_{}; + std::shared_ptr> qp_ptr_{nullptr}; void initializeProblemImpl( const Eigen::MatrixXd & P, const Eigen::MatrixXd & A, const std::vector & q, @@ -50,6 +52,6 @@ class ProxQPInterface : public QPInterface std::vector optimizeImpl() override; }; -} // namespace qp +} // namespace autoware::common #endif // QP_INTERFACE__PROXQP_INTERFACE_HPP_ diff --git a/common/qp_interface/include/qp_interface/qp_interface.hpp b/common/qp_interface/include/qp_interface/qp_interface.hpp index 026c0fe413b71..85e2d103cde7a 100644 --- a/common/qp_interface/include/qp_interface/qp_interface.hpp +++ b/common/qp_interface/include/qp_interface/qp_interface.hpp @@ -18,28 +18,30 @@ #include #include +#include #include -namespace qp +namespace autoware::common { class QPInterface { public: - explicit QPInterface(const bool enable_warm_start) : m_enable_warm_start(enable_warm_start) {} + explicit QPInterface(const bool enable_warm_start) : enable_warm_start_(enable_warm_start) {} std::vector optimize( const Eigen::MatrixXd & P, const Eigen::MatrixXd & A, const std::vector & q, const std::vector & l, const std::vector & u); - virtual int getIteration() const = 0; - virtual int getStatus() const = 0; + virtual bool isSolved() const = 0; + virtual int getIterationNumber() const = 0; + virtual std::string getStatus() const = 0; virtual void updateEpsAbs([[maybe_unused]] const double eps_abs) = 0; virtual void updateEpsRel([[maybe_unused]] const double eps_rel) = 0; virtual void updateVerbose([[maybe_unused]] const bool verbose) {} protected: - bool m_enable_warm_start; + bool enable_warm_start_{false}; void initializeProblem( const Eigen::MatrixXd & P, const Eigen::MatrixXd & A, const std::vector & q, @@ -51,9 +53,9 @@ class QPInterface virtual std::vector optimizeImpl() = 0; - std::optional m_variables_num; - std::optional m_constraints_num; + std::optional variables_num_{std::nullopt}; + std::optional constraints_num_{std::nullopt}; }; -} // namespace qp +} // namespace autoware::common #endif // QP_INTERFACE__QP_INTERFACE_HPP_ diff --git a/common/qp_interface/src/osqp_csc_matrix_conv.cpp b/common/qp_interface/src/osqp_csc_matrix_conv.cpp index 0faf7586463fd..77a481f442000 100644 --- a/common/qp_interface/src/osqp_csc_matrix_conv.cpp +++ b/common/qp_interface/src/osqp_csc_matrix_conv.cpp @@ -21,7 +21,7 @@ #include #include -namespace qp +namespace autoware::common { CSC_Matrix calCSCMatrix(const Eigen::MatrixXd & mat) { @@ -114,21 +114,21 @@ CSC_Matrix calCSCMatrixTrapezoidal(const Eigen::MatrixXd & mat) void printCSCMatrix(const CSC_Matrix & csc_mat) { std::cout << "["; - for (const c_float & val : csc_mat.m_vals) { + for (const c_float & val : csc_mat.vals_) { std::cout << val << ", "; } std::cout << "]\n"; std::cout << "["; - for (const c_int & row : csc_mat.m_row_idxs) { + for (const c_int & row : csc_mat.row_idxs_) { std::cout << row << ", "; } std::cout << "]\n"; std::cout << "["; - for (const c_int & col : csc_mat.m_col_idxs) { + for (const c_int & col : csc_mat.col_idxs_) { std::cout << col << ", "; } std::cout << "]\n"; } -} // namespace qp +} // namespace autoware::common diff --git a/common/qp_interface/src/osqp_interface.cpp b/common/qp_interface/src/osqp_interface.cpp index 81d2ffeeaaed5..0e8cb7e13988a 100644 --- a/common/qp_interface/src/osqp_interface.cpp +++ b/common/qp_interface/src/osqp_interface.cpp @@ -18,27 +18,29 @@ #include -namespace qp -{ -OSQPInterface::OSQPInterface(const bool enable_warm_start, const c_float eps_abs, const bool polish) -: QPInterface(enable_warm_start), m_work{nullptr, OSQPWorkspaceDeleter} -{ - m_settings = std::make_unique(); - m_data = std::make_unique(); - - if (m_settings) { - osqp_set_default_settings(m_settings.get()); - m_settings->alpha = 1.6; // Change alpha parameter - m_settings->eps_rel = 1.0E-4; - m_settings->eps_abs = eps_abs; - m_settings->eps_prim_inf = 1.0E-4; - m_settings->eps_dual_inf = 1.0E-4; - m_settings->warm_start = true; - m_settings->max_iter = 4000; - m_settings->verbose = false; - m_settings->polish = polish; +namespace autoware::common +{ +OSQPInterface::OSQPInterface( + const bool enable_warm_start, const c_float eps_abs, const c_float eps_rel, const bool polish, + const bool verbose) +: QPInterface(enable_warm_start), work_{nullptr, OSQPWorkspaceDeleter} +{ + settings_ = std::make_unique(); + data_ = std::make_unique(); + + if (settings_) { + osqp_set_default_settings(settings_.get()); + settings_->alpha = 1.6; // Change alpha parameter + settings_->eps_rel = eps_rel; + settings_->eps_abs = eps_abs; + settings_->eps_prim_inf = 1.0E-4; + settings_->eps_dual_inf = 1.0E-4; + settings_->warm_start = true; + settings_->max_iter = 4000; + settings_->verbose = verbose; + settings_->polish = polish; } - m_exitflag = 0; + exitflag_ = 0; } OSQPInterface::OSQPInterface( @@ -61,8 +63,8 @@ OSQPInterface::OSQPInterface( OSQPInterface::~OSQPInterface() { - if (m_data->P) free(m_data->P); - if (m_data->A) free(m_data->A); + if (data_->P) free(data_->P); + if (data_->A) free(data_->A); } void OSQPInterface::initializeProblemImpl( @@ -89,30 +91,30 @@ void OSQPInterface::initializeCSCProblemImpl( /********************** * OBJECTIVE FUNCTION **********************/ - m_param_n = static_cast(q.size()); - m_data->m = static_cast(l.size()); + param_n_ = static_cast(q.size()); + data_->m = static_cast(l.size()); /***************** * POPULATE DATA *****************/ - m_data->n = m_param_n; - if (m_data->P) free(m_data->P); - m_data->P = csc_matrix( - m_data->n, m_data->n, static_cast(P_csc.m_vals.size()), P_csc.m_vals.data(), - P_csc.m_row_idxs.data(), P_csc.m_col_idxs.data()); - m_data->q = q_dyn; - if (m_data->A) free(m_data->A); - m_data->A = csc_matrix( - m_data->m, m_data->n, static_cast(A_csc.m_vals.size()), A_csc.m_vals.data(), - A_csc.m_row_idxs.data(), A_csc.m_col_idxs.data()); - m_data->l = l_dyn; - m_data->u = u_dyn; + data_->n = param_n_; + if (data_->P) free(data_->P); + data_->P = csc_matrix( + data_->n, data_->n, static_cast(P_csc.vals_.size()), P_csc.vals_.data(), + P_csc.row_idxs_.data(), P_csc.col_idxs_.data()); + data_->q = q_dyn; + if (data_->A) free(data_->A); + data_->A = csc_matrix( + data_->m, data_->n, static_cast(A_csc.vals_.size()), A_csc.vals_.data(), + A_csc.row_idxs_.data(), A_csc.col_idxs_.data()); + data_->l = l_dyn; + data_->u = u_dyn; // Setup workspace OSQPWorkspace * workspace; - m_exitflag = osqp_setup(&workspace, m_data.get(), m_settings.get()); - m_work.reset(workspace); - m_work_initialized = true; + exitflag_ = osqp_setup(&workspace, data_.get(), settings_.get()); + work_.reset(workspace); + work__initialized = true; } void OSQPInterface::OSQPWorkspaceDeleter(OSQPWorkspace * ptr) noexcept @@ -124,67 +126,67 @@ void OSQPInterface::OSQPWorkspaceDeleter(OSQPWorkspace * ptr) noexcept void OSQPInterface::updateEpsAbs(const double eps_abs) { - m_settings->eps_abs = eps_abs; // for default setting - if (m_work_initialized) { - osqp_update_eps_abs(m_work.get(), eps_abs); // for current work + settings_->eps_abs = eps_abs; // for default setting + if (work__initialized) { + osqp_update_eps_abs(work_.get(), eps_abs); // for current work } } void OSQPInterface::updateEpsRel(const double eps_rel) { - m_settings->eps_rel = eps_rel; // for default setting - if (m_work_initialized) { - osqp_update_eps_rel(m_work.get(), eps_rel); // for current work + settings_->eps_rel = eps_rel; // for default setting + if (work__initialized) { + osqp_update_eps_rel(work_.get(), eps_rel); // for current work } } void OSQPInterface::updateMaxIter(const int max_iter) { - m_settings->max_iter = max_iter; // for default setting - if (m_work_initialized) { - osqp_update_max_iter(m_work.get(), max_iter); // for current work + settings_->max_iter = max_iter; // for default setting + if (work__initialized) { + osqp_update_max_iter(work_.get(), max_iter); // for current work } } void OSQPInterface::updateVerbose(const bool is_verbose) { - m_settings->verbose = is_verbose; // for default setting - if (m_work_initialized) { - osqp_update_verbose(m_work.get(), is_verbose); // for current work + settings_->verbose = is_verbose; // for default setting + if (work__initialized) { + osqp_update_verbose(work_.get(), is_verbose); // for current work } } void OSQPInterface::updateRhoInterval(const int rho_interval) { - m_settings->adaptive_rho_interval = rho_interval; // for default setting + settings_->adaptive_rho_interval = rho_interval; // for default setting } void OSQPInterface::updateRho(const double rho) { - m_settings->rho = rho; - if (m_work_initialized) { - osqp_update_rho(m_work.get(), rho); + settings_->rho = rho; + if (work__initialized) { + osqp_update_rho(work_.get(), rho); } } void OSQPInterface::updateAlpha(const double alpha) { - m_settings->alpha = alpha; - if (m_work_initialized) { - osqp_update_alpha(m_work.get(), alpha); + settings_->alpha = alpha; + if (work__initialized) { + osqp_update_alpha(work_.get(), alpha); } } void OSQPInterface::updateScaling(const int scaling) { - m_settings->scaling = scaling; + settings_->scaling = scaling; } void OSQPInterface::updatePolish(const bool polish) { - m_settings->polish = polish; - if (m_work_initialized) { - osqp_update_polish(m_work.get(), polish); + settings_->polish = polish; + if (work__initialized) { + osqp_update_polish(work_.get(), polish); } } @@ -195,9 +197,9 @@ void OSQPInterface::updatePolishRefinementIteration(const int polish_refine_iter return; } - m_settings->polish_refine_iter = polish_refine_iter; - if (m_work_initialized) { - osqp_update_polish_refine_iter(m_work.get(), polish_refine_iter); + settings_->polish_refine_iter = polish_refine_iter; + if (work__initialized) { + osqp_update_polish_refine_iter(work_.get(), polish_refine_iter); } } @@ -208,9 +210,9 @@ void OSQPInterface::updateCheckTermination(const int check_termination) return; } - m_settings->check_termination = check_termination; - if (m_work_initialized) { - osqp_update_check_termination(m_work.get(), check_termination); + settings_->check_termination = check_termination; + if (work__initialized) { + osqp_update_check_termination(work_.get(), check_termination); } } @@ -222,11 +224,11 @@ bool OSQPInterface::setWarmStart( bool OSQPInterface::setPrimalVariables(const std::vector & primal_variables) { - if (!m_work_initialized) { + if (!work__initialized) { return false; } - const auto result = osqp_warm_start_x(m_work.get(), primal_variables.data()); + const auto result = osqp_warm_start_x(work_.get(), primal_variables.data()); if (result != 0) { std::cerr << "Failed to set primal variables for warm start" << std::endl; return false; @@ -237,11 +239,11 @@ bool OSQPInterface::setPrimalVariables(const std::vector & primal_variab bool OSQPInterface::setDualVariables(const std::vector & dual_variables) { - if (!m_work_initialized) { + if (!work__initialized) { return false; } - const auto result = osqp_warm_start_y(m_work.get(), dual_variables.data()); + const auto result = osqp_warm_start_y(work_.get(), dual_variables.data()); if (result != 0) { std::cerr << "Failed to set dual variables for warm start" << std::endl; return false; @@ -250,27 +252,6 @@ bool OSQPInterface::setDualVariables(const std::vector & dual_variables) return true; } -void OSQPInterface::logUnsolvedStatus(const std::string & prefix_message) const -{ - const int status = getStatus(); - if (status == 1) { - // No need to log since optimization was solved. - return; - } - - // create message - std::string output_message = ""; - if (prefix_message != "") { - output_message = prefix_message + " "; - } - - const auto status_message = getStatusMessage(); - output_message += "Optimization failed due to " + status_message; - - // log with warning - RCLCPP_WARN(rclcpp::get_logger("osqp_interface"), output_message.c_str()); -} - void OSQPInterface::updateP(const Eigen::MatrixXd & P_new) { /* @@ -283,14 +264,12 @@ void OSQPInterface::updateP(const Eigen::MatrixXd & P_new) c_int P_elem_N = P_sparse.nonZeros(); */ CSC_Matrix P_csc = calCSCMatrixTrapezoidal(P_new); - osqp_update_P( - m_work.get(), P_csc.m_vals.data(), OSQP_NULL, static_cast(P_csc.m_vals.size())); + osqp_update_P(work_.get(), P_csc.vals_.data(), OSQP_NULL, static_cast(P_csc.vals_.size())); } void OSQPInterface::updateCscP(const CSC_Matrix & P_csc) { - osqp_update_P( - m_work.get(), P_csc.m_vals.data(), OSQP_NULL, static_cast(P_csc.m_vals.size())); + osqp_update_P(work_.get(), P_csc.vals_.data(), OSQP_NULL, static_cast(P_csc.vals_.size())); } void OSQPInterface::updateA(const Eigen::MatrixXd & A_new) @@ -303,36 +282,34 @@ void OSQPInterface::updateA(const Eigen::MatrixXd & A_new) c_int A_elem_N = A_sparse.nonZeros(); */ CSC_Matrix A_csc = calCSCMatrix(A_new); - osqp_update_A( - m_work.get(), A_csc.m_vals.data(), OSQP_NULL, static_cast(A_csc.m_vals.size())); + osqp_update_A(work_.get(), A_csc.vals_.data(), OSQP_NULL, static_cast(A_csc.vals_.size())); return; } void OSQPInterface::updateCscA(const CSC_Matrix & A_csc) { - osqp_update_A( - m_work.get(), A_csc.m_vals.data(), OSQP_NULL, static_cast(A_csc.m_vals.size())); + osqp_update_A(work_.get(), A_csc.vals_.data(), OSQP_NULL, static_cast(A_csc.vals_.size())); } void OSQPInterface::updateQ(const std::vector & q_new) { std::vector q_tmp(q_new.begin(), q_new.end()); double * q_dyn = q_tmp.data(); - osqp_update_lin_cost(m_work.get(), q_dyn); + osqp_update_lin_cost(work_.get(), q_dyn); } void OSQPInterface::updateL(const std::vector & l_new) { std::vector l_tmp(l_new.begin(), l_new.end()); double * l_dyn = l_tmp.data(); - osqp_update_lower_bound(m_work.get(), l_dyn); + osqp_update_lower_bound(work_.get(), l_dyn); } void OSQPInterface::updateU(const std::vector & u_new) { std::vector u_tmp(u_new.begin(), u_new.end()); double * u_dyn = u_tmp.data(); - osqp_update_upper_bound(m_work.get(), u_dyn); + osqp_update_upper_bound(work_.get(), u_dyn); } void OSQPInterface::updateBounds( @@ -342,45 +319,50 @@ void OSQPInterface::updateBounds( std::vector u_tmp(u_new.begin(), u_new.end()); double * l_dyn = l_tmp.data(); double * u_dyn = u_tmp.data(); - osqp_update_bounds(m_work.get(), l_dyn, u_dyn); + osqp_update_bounds(work_.get(), l_dyn, u_dyn); +} + +int OSQPInterface::getIterationNumber() const +{ + return work_->info->iter; } -int OSQPInterface::getIteration() const +std::string OSQPInterface::getStatus() const { - return m_work->info->iter; + return "OSQP_SOLVED"; } -int OSQPInterface::getStatus() const +bool OSQPInterface::isSolved() const { - return static_cast(m_latest_work_info.status_val); + return latest_work_info_.status_val == 1; } int OSQPInterface::getPolishStatus() const { - return static_cast(m_latest_work_info.status_polish); + return static_cast(latest_work_info_.status_polish); } std::vector OSQPInterface::getDualSolution() const { - double * sol_y = m_work->solution->y; - std::vector dual_solution(sol_y, sol_y + m_data->m); + double * sol_y = work_->solution->y; + std::vector dual_solution(sol_y, sol_y + data_->m); return dual_solution; } std::vector OSQPInterface::optimizeImpl() { - osqp_solve(m_work.get()); + osqp_solve(work_.get()); - double * sol_x = m_work->solution->x; - double * sol_y = m_work->solution->y; - std::vector sol_primal(sol_x, sol_x + m_param_n); - std::vector sol_lagrange_multiplier(sol_y, sol_y + m_data->m); + double * sol_x = work_->solution->x; + double * sol_y = work_->solution->y; + std::vector sol_primal(sol_x, sol_x + param_n_); + std::vector sol_lagrange_multiplier(sol_y, sol_y + data_->m); - m_latest_work_info = *(m_work->info); + latest_work_info_ = *(work_->info); - if (!m_enable_warm_start) { - m_work.reset(); - m_work_initialized = false; + if (!enable_warm_start_) { + work_.reset(); + work__initialized = false; } return sol_primal; @@ -393,7 +375,17 @@ std::vector OSQPInterface::optimize( initializeCSCProblemImpl(P, A, q, l, u); const auto result = optimizeImpl(); + // show polish status if not successful + const int status_polish = static_cast(latest_work_info_.status_polish); + if (status_polish != 1) { + const auto msg = status_polish == 0 ? "unperformed" + : status_polish == -1 ? "unsuccessful" + : "unknown"; + std::cerr << "osqp polish process failed : " << msg << ". The result may be inaccurate" + << std::endl; + } + return result; } -} // namespace qp +} // namespace autoware::common diff --git a/common/qp_interface/src/proxqp_interface.cpp b/common/qp_interface/src/proxqp_interface.cpp index bf1f0229e1e21..507f3858cbf1b 100644 --- a/common/qp_interface/src/proxqp_interface.cpp +++ b/common/qp_interface/src/proxqp_interface.cpp @@ -14,12 +14,17 @@ #include "qp_interface/proxqp_interface.hpp" -namespace qp +namespace autoware::common { -ProxQPInterface::ProxQPInterface(const bool enable_warm_start, const double eps_abs) +using proxsuite::proxqp::QPSolverOutput; + +ProxQPInterface::ProxQPInterface( + const bool enable_warm_start, const double eps_abs, const double eps_rel, const bool verbose) : QPInterface(enable_warm_start) { - m_settings.eps_abs = eps_abs; + settings_.eps_abs = eps_abs; + settings_.eps_rel = eps_rel; + settings_.verbose = verbose; } void ProxQPInterface::initializeProblemImpl( @@ -31,28 +36,28 @@ void ProxQPInterface::initializeProblemImpl( const bool enable_warm_start = [&]() { if ( - !m_enable_warm_start // Warm start is designated - || !m_qp_ptr // QP pointer is initialized + !enable_warm_start_ // Warm start is designated + || !qp_ptr_ // QP pointer is initialized // The number of variables is the same as the previous one. - || !m_variables_num || - *m_variables_num != variables_num + || !variables_num_ || + *variables_num_ != variables_num // The number of constraints is the same as the previous one - || !m_constraints_num || *m_constraints_num != constraints_num) { + || !constraints_num_ || *constraints_num_ != constraints_num) { return false; } return true; }(); if (!enable_warm_start) { - m_qp_ptr = std::make_shared>( + qp_ptr_ = std::make_shared>( variables_num, 0, constraints_num); } - m_settings.initial_guess = + settings_.initial_guess = enable_warm_start ? proxsuite::proxqp::InitialGuessStatus::WARM_START_WITH_PREVIOUS_RESULT : proxsuite::proxqp::InitialGuessStatus::NO_INITIAL_GUESS; - m_qp_ptr->settings = m_settings; + qp_ptr_->settings = settings_; Eigen::SparseMatrix P_sparse(variables_num, constraints_num); P_sparse = P.sparseView(); @@ -69,53 +74,78 @@ void ProxQPInterface::initializeProblemImpl( Eigen::Map(u_std_vec.data(), u_std_vec.size()); if (enable_warm_start) { - m_qp_ptr->update( + qp_ptr_->update( P_sparse, eigen_q, proxsuite::nullopt, proxsuite::nullopt, A.sparseView(), eigen_l, eigen_u); } else { - m_qp_ptr->init( + qp_ptr_->init( P_sparse, eigen_q, proxsuite::nullopt, proxsuite::nullopt, A.sparseView(), eigen_l, eigen_u); } } void ProxQPInterface::updateEpsAbs(const double eps_abs) { - m_settings.eps_abs = eps_abs; + settings_.eps_abs = eps_abs; } void ProxQPInterface::updateEpsRel(const double eps_rel) { - m_settings.eps_rel = eps_rel; + settings_.eps_rel = eps_rel; } void ProxQPInterface::updateVerbose(const bool is_verbose) { - m_settings.verbose = is_verbose; + settings_.verbose = is_verbose; } -int ProxQPInterface::getIteration() const +bool ProxQPInterface::isSolved() const { - if (m_qp_ptr) { - return m_qp_ptr->results.info.iter; + if (qp_ptr_) { + return qp_ptr_->results.info.status == QPSolverOutput::PROXQP_SOLVED; } - return 0; + return false; } -int ProxQPInterface::getStatus() const +int ProxQPInterface::getIterationNumber() const { - if (m_qp_ptr) { - return static_cast(m_qp_ptr->results.info.status); + if (qp_ptr_) { + return qp_ptr_->results.info.iter; } return 0; } +std::string ProxQPInterface::getStatus() const +{ + if (qp_ptr_) { + if (qp_ptr_->results.info.status == QPSolverOutput::PROXQP_SOLVED) { + return "PROXQP_SOLVED"; + } + if (qp_ptr_->results.info.status == QPSolverOutput::PROXQP_MAX_ITER_REACHED) { + return "PROXQP_MAX_ITER_REACHED"; + } + if (qp_ptr_->results.info.status == QPSolverOutput::PROXQP_PRIMAL_INFEASIBLE) { + return "PROXQP_PRIMAL_INFEASIBLE"; + } + // if (qp_ptr_->results.info.status == QPSolverOutput::PROXQP_SOLVED_CLOSEST_PRIMAL_FEASIBLE) { + // return "PROXQP_SOLVED_CLOSEST_PRIMAL_FEASIBLE"; + // } + if (qp_ptr_->results.info.status == QPSolverOutput::PROXQP_DUAL_INFEASIBLE) { + return "PROXQP_DUAL_INFEASIBLE"; + } + if (qp_ptr_->results.info.status == QPSolverOutput::PROXQP_NOT_RUN) { + return "PROXQP_NOT_RUN"; + } + } + return "None"; +} + std::vector ProxQPInterface::optimizeImpl() { - m_qp_ptr->solve(); + qp_ptr_->solve(); std::vector result; - for (Eigen::Index i = 0; i < m_qp_ptr->results.x.size(); ++i) { - result.push_back(m_qp_ptr->results.x[i]); + for (Eigen::Index i = 0; i < qp_ptr_->results.x.size(); ++i) { + result.push_back(qp_ptr_->results.x[i]); } return result; } -} // namespace qp +} // namespace autoware::common diff --git a/common/qp_interface/src/qp_interface.cpp b/common/qp_interface/src/qp_interface.cpp index e7a69bac0c67c..afd26132d7769 100644 --- a/common/qp_interface/src/qp_interface.cpp +++ b/common/qp_interface/src/qp_interface.cpp @@ -18,7 +18,7 @@ #include #include -namespace qp +namespace autoware::common { void QPInterface::initializeProblem( const Eigen::MatrixXd & P, const Eigen::MatrixXd & A, const std::vector & q, @@ -54,8 +54,8 @@ void QPInterface::initializeProblem( initializeProblemImpl(P, A, q, l, u); - m_variables_num = q.size(); - m_constraints_num = l.size(); + variables_num_ = q.size(); + constraints_num_ = l.size(); } std::vector QPInterface::optimize( @@ -67,4 +67,4 @@ std::vector QPInterface::optimize( return result; } -} // namespace qp +} // namespace autoware::common diff --git a/common/qp_interface/test/test_csc_matrix_conv.cpp b/common/qp_interface/test/test_csc_matrix_conv.cpp index fc604d762d2c4..a6bd5e3df0be1 100644 --- a/common/qp_interface/test/test_csc_matrix_conv.cpp +++ b/common/qp_interface/test/test_csc_matrix_conv.cpp @@ -23,42 +23,42 @@ TEST(TestCscMatrixConv, Nominal) { - using qp::calCSCMatrix; - using qp::CSC_Matrix; + using autoware::common::calCSCMatrix; + using autoware::common::CSC_Matrix; Eigen::MatrixXd rect1(1, 2); rect1 << 0.0, 1.0; const CSC_Matrix rect_m1 = calCSCMatrix(rect1); - ASSERT_EQ(rect_m1.m_vals.size(), size_t(1)); - EXPECT_EQ(rect_m1.m_vals[0], 1.0); - ASSERT_EQ(rect_m1.m_row_idxs.size(), size_t(1)); - EXPECT_EQ(rect_m1.m_row_idxs[0], c_int(0)); - ASSERT_EQ(rect_m1.m_col_idxs.size(), size_t(3)); // nb of columns + 1 - EXPECT_EQ(rect_m1.m_col_idxs[0], c_int(0)); - EXPECT_EQ(rect_m1.m_col_idxs[1], c_int(0)); - EXPECT_EQ(rect_m1.m_col_idxs[2], c_int(1)); + ASSERT_EQ(rect_m1.vals_.size(), size_t(1)); + EXPECT_EQ(rect_m1.vals_[0], 1.0); + ASSERT_EQ(rect_m1.row_idxs_.size(), size_t(1)); + EXPECT_EQ(rect_m1.row_idxs_[0], c_int(0)); + ASSERT_EQ(rect_m1.col_idxs_.size(), size_t(3)); // nb of columns + 1 + EXPECT_EQ(rect_m1.col_idxs_[0], c_int(0)); + EXPECT_EQ(rect_m1.col_idxs_[1], c_int(0)); + EXPECT_EQ(rect_m1.col_idxs_[2], c_int(1)); Eigen::MatrixXd rect2(2, 4); rect2 << 1.0, 0.0, 3.0, 0.0, 0.0, 6.0, 7.0, 0.0; const CSC_Matrix rect_m2 = calCSCMatrix(rect2); - ASSERT_EQ(rect_m2.m_vals.size(), size_t(4)); - EXPECT_EQ(rect_m2.m_vals[0], 1.0); - EXPECT_EQ(rect_m2.m_vals[1], 6.0); - EXPECT_EQ(rect_m2.m_vals[2], 3.0); - EXPECT_EQ(rect_m2.m_vals[3], 7.0); - ASSERT_EQ(rect_m2.m_row_idxs.size(), size_t(4)); - EXPECT_EQ(rect_m2.m_row_idxs[0], c_int(0)); - EXPECT_EQ(rect_m2.m_row_idxs[1], c_int(1)); - EXPECT_EQ(rect_m2.m_row_idxs[2], c_int(0)); - EXPECT_EQ(rect_m2.m_row_idxs[3], c_int(1)); - ASSERT_EQ(rect_m2.m_col_idxs.size(), size_t(5)); // nb of columns + 1 - EXPECT_EQ(rect_m2.m_col_idxs[0], c_int(0)); - EXPECT_EQ(rect_m2.m_col_idxs[1], c_int(1)); - EXPECT_EQ(rect_m2.m_col_idxs[2], c_int(2)); - EXPECT_EQ(rect_m2.m_col_idxs[3], c_int(4)); - EXPECT_EQ(rect_m2.m_col_idxs[4], c_int(4)); + ASSERT_EQ(rect_m2.vals_.size(), size_t(4)); + EXPECT_EQ(rect_m2.vals_[0], 1.0); + EXPECT_EQ(rect_m2.vals_[1], 6.0); + EXPECT_EQ(rect_m2.vals_[2], 3.0); + EXPECT_EQ(rect_m2.vals_[3], 7.0); + ASSERT_EQ(rect_m2.row_idxs_.size(), size_t(4)); + EXPECT_EQ(rect_m2.row_idxs_[0], c_int(0)); + EXPECT_EQ(rect_m2.row_idxs_[1], c_int(1)); + EXPECT_EQ(rect_m2.row_idxs_[2], c_int(0)); + EXPECT_EQ(rect_m2.row_idxs_[3], c_int(1)); + ASSERT_EQ(rect_m2.col_idxs_.size(), size_t(5)); // nb of columns + 1 + EXPECT_EQ(rect_m2.col_idxs_[0], c_int(0)); + EXPECT_EQ(rect_m2.col_idxs_[1], c_int(1)); + EXPECT_EQ(rect_m2.col_idxs_[2], c_int(2)); + EXPECT_EQ(rect_m2.col_idxs_[3], c_int(4)); + EXPECT_EQ(rect_m2.col_idxs_[4], c_int(4)); // Example from http://netlib.org/linalg/html_templates/node92.html Eigen::MatrixXd square2(6, 6); @@ -66,59 +66,59 @@ TEST(TestCscMatrixConv, Nominal) 0.0, 3.0, 0.0, 8.0, 7.0, 5.0, 0.0, 0.0, 8.0, 0.0, 9.0, 9.0, 13.0, 0.0, 4.0, 0.0, 0.0, 2.0, -1.0; const CSC_Matrix square_m2 = calCSCMatrix(square2); - ASSERT_EQ(square_m2.m_vals.size(), size_t(19)); - EXPECT_EQ(square_m2.m_vals[0], 10.0); - EXPECT_EQ(square_m2.m_vals[1], 3.0); - EXPECT_EQ(square_m2.m_vals[2], 3.0); - EXPECT_EQ(square_m2.m_vals[3], 9.0); - EXPECT_EQ(square_m2.m_vals[4], 7.0); - EXPECT_EQ(square_m2.m_vals[5], 8.0); - EXPECT_EQ(square_m2.m_vals[6], 4.0); - EXPECT_EQ(square_m2.m_vals[7], 8.0); - EXPECT_EQ(square_m2.m_vals[8], 8.0); - EXPECT_EQ(square_m2.m_vals[9], 7.0); - EXPECT_EQ(square_m2.m_vals[10], 7.0); - EXPECT_EQ(square_m2.m_vals[11], 9.0); - EXPECT_EQ(square_m2.m_vals[12], -2.0); - EXPECT_EQ(square_m2.m_vals[13], 5.0); - EXPECT_EQ(square_m2.m_vals[14], 9.0); - EXPECT_EQ(square_m2.m_vals[15], 2.0); - EXPECT_EQ(square_m2.m_vals[16], 3.0); - EXPECT_EQ(square_m2.m_vals[17], 13.0); - EXPECT_EQ(square_m2.m_vals[18], -1.0); - ASSERT_EQ(square_m2.m_row_idxs.size(), size_t(19)); - EXPECT_EQ(square_m2.m_row_idxs[0], c_int(0)); - EXPECT_EQ(square_m2.m_row_idxs[1], c_int(1)); - EXPECT_EQ(square_m2.m_row_idxs[2], c_int(3)); - EXPECT_EQ(square_m2.m_row_idxs[3], c_int(1)); - EXPECT_EQ(square_m2.m_row_idxs[4], c_int(2)); - EXPECT_EQ(square_m2.m_row_idxs[5], c_int(4)); - EXPECT_EQ(square_m2.m_row_idxs[6], c_int(5)); - EXPECT_EQ(square_m2.m_row_idxs[7], c_int(2)); - EXPECT_EQ(square_m2.m_row_idxs[8], c_int(3)); - EXPECT_EQ(square_m2.m_row_idxs[9], c_int(2)); - EXPECT_EQ(square_m2.m_row_idxs[10], c_int(3)); - EXPECT_EQ(square_m2.m_row_idxs[11], c_int(4)); - EXPECT_EQ(square_m2.m_row_idxs[12], c_int(0)); - EXPECT_EQ(square_m2.m_row_idxs[13], c_int(3)); - EXPECT_EQ(square_m2.m_row_idxs[14], c_int(4)); - EXPECT_EQ(square_m2.m_row_idxs[15], c_int(5)); - EXPECT_EQ(square_m2.m_row_idxs[16], c_int(1)); - EXPECT_EQ(square_m2.m_row_idxs[17], c_int(4)); - EXPECT_EQ(square_m2.m_row_idxs[18], c_int(5)); - ASSERT_EQ(square_m2.m_col_idxs.size(), size_t(7)); // nb of columns + 1 - EXPECT_EQ(square_m2.m_col_idxs[0], c_int(0)); - EXPECT_EQ(square_m2.m_col_idxs[1], c_int(3)); - EXPECT_EQ(square_m2.m_col_idxs[2], c_int(7)); - EXPECT_EQ(square_m2.m_col_idxs[3], c_int(9)); - EXPECT_EQ(square_m2.m_col_idxs[4], c_int(12)); - EXPECT_EQ(square_m2.m_col_idxs[5], c_int(16)); - EXPECT_EQ(square_m2.m_col_idxs[6], c_int(19)); + ASSERT_EQ(square_m2.vals_.size(), size_t(19)); + EXPECT_EQ(square_m2.vals_[0], 10.0); + EXPECT_EQ(square_m2.vals_[1], 3.0); + EXPECT_EQ(square_m2.vals_[2], 3.0); + EXPECT_EQ(square_m2.vals_[3], 9.0); + EXPECT_EQ(square_m2.vals_[4], 7.0); + EXPECT_EQ(square_m2.vals_[5], 8.0); + EXPECT_EQ(square_m2.vals_[6], 4.0); + EXPECT_EQ(square_m2.vals_[7], 8.0); + EXPECT_EQ(square_m2.vals_[8], 8.0); + EXPECT_EQ(square_m2.vals_[9], 7.0); + EXPECT_EQ(square_m2.vals_[10], 7.0); + EXPECT_EQ(square_m2.vals_[11], 9.0); + EXPECT_EQ(square_m2.vals_[12], -2.0); + EXPECT_EQ(square_m2.vals_[13], 5.0); + EXPECT_EQ(square_m2.vals_[14], 9.0); + EXPECT_EQ(square_m2.vals_[15], 2.0); + EXPECT_EQ(square_m2.vals_[16], 3.0); + EXPECT_EQ(square_m2.vals_[17], 13.0); + EXPECT_EQ(square_m2.vals_[18], -1.0); + ASSERT_EQ(square_m2.row_idxs_.size(), size_t(19)); + EXPECT_EQ(square_m2.row_idxs_[0], c_int(0)); + EXPECT_EQ(square_m2.row_idxs_[1], c_int(1)); + EXPECT_EQ(square_m2.row_idxs_[2], c_int(3)); + EXPECT_EQ(square_m2.row_idxs_[3], c_int(1)); + EXPECT_EQ(square_m2.row_idxs_[4], c_int(2)); + EXPECT_EQ(square_m2.row_idxs_[5], c_int(4)); + EXPECT_EQ(square_m2.row_idxs_[6], c_int(5)); + EXPECT_EQ(square_m2.row_idxs_[7], c_int(2)); + EXPECT_EQ(square_m2.row_idxs_[8], c_int(3)); + EXPECT_EQ(square_m2.row_idxs_[9], c_int(2)); + EXPECT_EQ(square_m2.row_idxs_[10], c_int(3)); + EXPECT_EQ(square_m2.row_idxs_[11], c_int(4)); + EXPECT_EQ(square_m2.row_idxs_[12], c_int(0)); + EXPECT_EQ(square_m2.row_idxs_[13], c_int(3)); + EXPECT_EQ(square_m2.row_idxs_[14], c_int(4)); + EXPECT_EQ(square_m2.row_idxs_[15], c_int(5)); + EXPECT_EQ(square_m2.row_idxs_[16], c_int(1)); + EXPECT_EQ(square_m2.row_idxs_[17], c_int(4)); + EXPECT_EQ(square_m2.row_idxs_[18], c_int(5)); + ASSERT_EQ(square_m2.col_idxs_.size(), size_t(7)); // nb of columns + 1 + EXPECT_EQ(square_m2.col_idxs_[0], c_int(0)); + EXPECT_EQ(square_m2.col_idxs_[1], c_int(3)); + EXPECT_EQ(square_m2.col_idxs_[2], c_int(7)); + EXPECT_EQ(square_m2.col_idxs_[3], c_int(9)); + EXPECT_EQ(square_m2.col_idxs_[4], c_int(12)); + EXPECT_EQ(square_m2.col_idxs_[5], c_int(16)); + EXPECT_EQ(square_m2.col_idxs_[6], c_int(19)); } TEST(TestCscMatrixConv, Trapezoidal) { - using qp::calCSCMatrixTrapezoidal; - using qp::CSC_Matrix; + using autoware::common::calCSCMatrixTrapezoidal; + using autoware::common::CSC_Matrix; Eigen::MatrixXd square1(2, 2); Eigen::MatrixXd square2(3, 3); @@ -129,33 +129,33 @@ TEST(TestCscMatrixConv, Trapezoidal) const CSC_Matrix square_m1 = calCSCMatrixTrapezoidal(square1); // Trapezoidal: skip the lower left triangle (2.0 in this example) - ASSERT_EQ(square_m1.m_vals.size(), size_t(3)); - EXPECT_EQ(square_m1.m_vals[0], 1.0); - EXPECT_EQ(square_m1.m_vals[1], 2.0); - EXPECT_EQ(square_m1.m_vals[2], 4.0); - ASSERT_EQ(square_m1.m_row_idxs.size(), size_t(3)); - EXPECT_EQ(square_m1.m_row_idxs[0], c_int(0)); - EXPECT_EQ(square_m1.m_row_idxs[1], c_int(0)); - EXPECT_EQ(square_m1.m_row_idxs[2], c_int(1)); - ASSERT_EQ(square_m1.m_col_idxs.size(), size_t(3)); - EXPECT_EQ(square_m1.m_col_idxs[0], c_int(0)); - EXPECT_EQ(square_m1.m_col_idxs[1], c_int(1)); - EXPECT_EQ(square_m1.m_col_idxs[2], c_int(3)); + ASSERT_EQ(square_m1.vals_.size(), size_t(3)); + EXPECT_EQ(square_m1.vals_[0], 1.0); + EXPECT_EQ(square_m1.vals_[1], 2.0); + EXPECT_EQ(square_m1.vals_[2], 4.0); + ASSERT_EQ(square_m1.row_idxs_.size(), size_t(3)); + EXPECT_EQ(square_m1.row_idxs_[0], c_int(0)); + EXPECT_EQ(square_m1.row_idxs_[1], c_int(0)); + EXPECT_EQ(square_m1.row_idxs_[2], c_int(1)); + ASSERT_EQ(square_m1.col_idxs_.size(), size_t(3)); + EXPECT_EQ(square_m1.col_idxs_[0], c_int(0)); + EXPECT_EQ(square_m1.col_idxs_[1], c_int(1)); + EXPECT_EQ(square_m1.col_idxs_[2], c_int(3)); const CSC_Matrix square_m2 = calCSCMatrixTrapezoidal(square2); - ASSERT_EQ(square_m2.m_vals.size(), size_t(3)); - EXPECT_EQ(square_m2.m_vals[0], 2.0); - EXPECT_EQ(square_m2.m_vals[1], 5.0); - EXPECT_EQ(square_m2.m_vals[2], 6.0); - ASSERT_EQ(square_m2.m_row_idxs.size(), size_t(3)); - EXPECT_EQ(square_m2.m_row_idxs[0], c_int(0)); - EXPECT_EQ(square_m2.m_row_idxs[1], c_int(1)); - EXPECT_EQ(square_m2.m_row_idxs[2], c_int(1)); - ASSERT_EQ(square_m2.m_col_idxs.size(), size_t(4)); - EXPECT_EQ(square_m2.m_col_idxs[0], c_int(0)); - EXPECT_EQ(square_m2.m_col_idxs[1], c_int(0)); - EXPECT_EQ(square_m2.m_col_idxs[2], c_int(2)); - EXPECT_EQ(square_m2.m_col_idxs[3], c_int(3)); + ASSERT_EQ(square_m2.vals_.size(), size_t(3)); + EXPECT_EQ(square_m2.vals_[0], 2.0); + EXPECT_EQ(square_m2.vals_[1], 5.0); + EXPECT_EQ(square_m2.vals_[2], 6.0); + ASSERT_EQ(square_m2.row_idxs_.size(), size_t(3)); + EXPECT_EQ(square_m2.row_idxs_[0], c_int(0)); + EXPECT_EQ(square_m2.row_idxs_[1], c_int(1)); + EXPECT_EQ(square_m2.row_idxs_[2], c_int(1)); + ASSERT_EQ(square_m2.col_idxs_.size(), size_t(4)); + EXPECT_EQ(square_m2.col_idxs_[0], c_int(0)); + EXPECT_EQ(square_m2.col_idxs_[1], c_int(0)); + EXPECT_EQ(square_m2.col_idxs_[2], c_int(2)); + EXPECT_EQ(square_m2.col_idxs_[3], c_int(3)); try { const CSC_Matrix rect_m1 = calCSCMatrixTrapezoidal(rect1); @@ -166,10 +166,10 @@ TEST(TestCscMatrixConv, Trapezoidal) } TEST(TestCscMatrixConv, Print) { - using qp::calCSCMatrix; - using qp::calCSCMatrixTrapezoidal; - using qp::CSC_Matrix; - using qp::printCSCMatrix; + using autoware::common::calCSCMatrix; + using autoware::common::calCSCMatrixTrapezoidal; + using autoware::common::CSC_Matrix; + using autoware::common::printCSCMatrix; Eigen::MatrixXd square1(2, 2); Eigen::MatrixXd rect1(1, 2); square1 << 1.0, 2.0, 2.0, 4.0; diff --git a/common/qp_interface/test/test_osqp_interface.cpp b/common/qp_interface/test/test_osqp_interface.cpp index 5ce1f0eb064a1..9a02dd4b00934 100644 --- a/common/qp_interface/test/test_osqp_interface.cpp +++ b/common/qp_interface/test/test_osqp_interface.cpp @@ -39,44 +39,44 @@ namespace TEST(TestOsqpInterface, BasicQp) { - using qp::calCSCMatrix; - using qp::calCSCMatrixTrapezoidal; - using qp::CSC_Matrix; + using autoware::common::calCSCMatrix; + using autoware::common::calCSCMatrixTrapezoidal; + using autoware::common::CSC_Matrix; - auto check_result = []( - const auto & solution, const int solution_status, const int polish_status) { - EXPECT_EQ(solution_status, 1); - EXPECT_EQ(polish_status, 1); + auto check_result = + [](const auto & solution, const std::string & status, const int polish_status) { + EXPECT_EQ(status, "OSQP_SOLVED"); + EXPECT_EQ(polish_status, 1); - static const auto ep = 1.0e-8; + static const auto ep = 1.0e-8; - ASSERT_EQ(solution.size(), size_t(2)); - EXPECT_NEAR(solution[0], 0.3, ep); - EXPECT_NEAR(solution[1], 0.7, ep); - }; + ASSERT_EQ(solution.size(), size_t(2)); + EXPECT_NEAR(solution[0], 0.3, ep); + EXPECT_NEAR(solution[1], 0.7, ep); + }; const Eigen::MatrixXd P = (Eigen::MatrixXd(2, 2) << 4, 1, 1, 2).finished(); const Eigen::MatrixXd A = (Eigen::MatrixXd(4, 2) << 1, 1, 1, 0, 0, 1, 0, 1).finished(); const std::vector q = {1.0, 1.0}; - const std::vector l = {1.0, 0.0, 0.0, -qp::INF}; - const std::vector u = {1.0, 0.7, 0.7, qp::INF}; + const std::vector l = {1.0, 0.0, 0.0, -autoware::common::OSQP_INF}; + const std::vector u = {1.0, 0.7, 0.7, autoware::common::OSQP_INF}; { // Define problem during optimization - qp::OSQPInterface osqp(false, 1e-6); + autoware::common::OSQPInterface osqp(false, 1e-6); const auto solution = osqp.QPInterface::optimize(P, A, q, l, u); - const auto solution_status = osqp.getStatus(); + const auto status = osqp.getStatus(); const auto polish_status = osqp.getPolishStatus(); - check_result(solution, solution_status, polish_status); + check_result(solution, status, polish_status); } { // Define problem during initialization - qp::OSQPInterface osqp(false, 1e-6); + autoware::common::OSQPInterface osqp(false, 1e-6); const auto solution = osqp.QPInterface::optimize(P, A, q, l, u); - const auto solution_status = osqp.getStatus(); + const auto status = osqp.getStatus(); const auto polish_status = osqp.getPolishStatus(); - check_result(solution, solution_status, polish_status); + check_result(solution, status, polish_status); } { @@ -87,7 +87,7 @@ TEST(TestOsqpInterface, BasicQp) std::vector q_ini(2, 0.0); std::vector l_ini(4, 0.0); std::vector u_ini(4, 0.0); - qp::OSQPInterface osqp(false, 1e-6); + autoware::common::OSQPInterface osqp(false, 1e-6); osqp.QPInterface::optimize(P_ini, A_ini, q_ini, l_ini, u_ini); } @@ -95,12 +95,12 @@ TEST(TestOsqpInterface, BasicQp) // Define problem during initialization with csc matrix CSC_Matrix P_csc = calCSCMatrixTrapezoidal(P); CSC_Matrix A_csc = calCSCMatrix(A); - qp::OSQPInterface osqp(false, 1e-6); + autoware::common::OSQPInterface osqp(false, 1e-6); const auto solution = osqp.optimize(P_csc, A_csc, q, l, u); - const auto solution_status = osqp.getStatus(); + const auto status = osqp.getStatus(); const auto polish_status = osqp.getPolishStatus(); - check_result(solution, solution_status, polish_status); + check_result(solution, status, polish_status); } { @@ -111,7 +111,7 @@ TEST(TestOsqpInterface, BasicQp) std::vector q_ini(2, 0.0); std::vector l_ini(4, 0.0); std::vector u_ini(4, 0.0); - qp::OSQPInterface osqp(false, 1e-6); + autoware::common::OSQPInterface osqp(false, 1e-6); osqp.optimize(P_ini_csc, A_ini_csc, q_ini, l_ini, u_ini); // Redefine problem before optimization @@ -119,9 +119,9 @@ TEST(TestOsqpInterface, BasicQp) CSC_Matrix A_csc = calCSCMatrix(A); const auto solution = osqp.optimize(P_csc, A_csc, q, l, u); - const auto solution_status = osqp.getStatus(); + const auto status = osqp.getStatus(); const auto polish_status = osqp.getPolishStatus(); - check_result(solution, solution_status, polish_status); + check_result(solution, status, polish_status); } // add warm startup @@ -132,7 +132,7 @@ TEST(TestOsqpInterface, BasicQp) std::vector q_ini(2, 0.0); std::vector l_ini(4, 0.0); std::vector u_ini(4, 0.0); - qp::OSQPInterface osqp(true, 1e-6); // enable warm start + autoware::common::OSQPInterface osqp(true, 1e-6); // enable warm start osqp.optimize(P_ini_csc, A_ini_csc, q_ini, l_ini, u_ini); // Redefine problem before optimization @@ -140,9 +140,9 @@ TEST(TestOsqpInterface, BasicQp) CSC_Matrix A_csc = calCSCMatrix(A); { const auto solution = osqp.optimize(P_csc, A_csc, q, l, u); - const auto solution_status = osqp.getStatus(); + const auto status = osqp.getStatus(); const auto polish_status = osqp.getPolishStatus(); - check_result(solution, solution_status, polish_status); + check_result(solution, status, polish_status); osqp.updateCheckTermination(1); const auto primal_val = solution; @@ -155,9 +155,9 @@ TEST(TestOsqpInterface, BasicQp) { const auto solution = osqp.optimize(P_csc, A_csc, q, l, u); - const auto solution_status = osqp.getStatus(); + const auto status = osqp.getStatus(); const auto polish_status = osqp.getPolishStatus(); - check_result(solution, solution_status, polish_status); + check_result(solution, status, polish_status); } // NOTE: This should be true, but currently optimize function reset the workspace, which diff --git a/common/qp_interface/test/test_proxqp_interface.cpp b/common/qp_interface/test/test_proxqp_interface.cpp index c28678e5c872a..a8b1bb29dafa0 100644 --- a/common/qp_interface/test/test_proxqp_interface.cpp +++ b/common/qp_interface/test/test_proxqp_interface.cpp @@ -40,7 +40,9 @@ namespace TEST(TestProxqpInterface, BasicQp) { - auto check_result = [](const auto & solution) { + auto check_result = [](const auto & solution, const std::string & status) { + EXPECT_EQ(status, "PROXQP_SOLVED"); + static const auto ep = 1.0e-8; ASSERT_EQ(solution.size(), size_t(2)); EXPECT_NEAR(solution[0], 0.3, ep); @@ -55,23 +57,26 @@ TEST(TestProxqpInterface, BasicQp) { // Define problem during optimization - qp::ProxQPInterface proxqp(false, 1e-9); + autoware::common::ProxQPInterface proxqp(false, 1e-9, 1e-9, false); const auto solution = proxqp.QPInterface::optimize(P, A, q, l, u); - check_result(solution); + const auto status = proxqp.getStatus(); + check_result(solution, status); } { // Define problem during optimization with warm start - qp::ProxQPInterface proxqp(true, 1e-9); + autoware::common::ProxQPInterface proxqp(true, 1e-9, 1e-9, false); { const auto solution = proxqp.QPInterface::optimize(P, A, q, l, u); - check_result(solution); - EXPECT_NE(proxqp.getIteration(), 1); + const auto status = proxqp.getStatus(); + check_result(solution, status); + EXPECT_NE(proxqp.getIterationNumber(), 1); } { const auto solution = proxqp.QPInterface::optimize(P, A, q, l, u); - check_result(solution); - EXPECT_EQ(proxqp.getIteration(), 0); + const auto status = proxqp.getStatus(); + check_result(solution, status); + EXPECT_EQ(proxqp.getIterationNumber(), 0); } } }