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 01/16] 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 02/16] 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 03/16] 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 04/16] 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 05/16] 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 06/16] 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 07/16] 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 08/16] 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 09/16] 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 10/16] 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 11/16] 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 12/16] 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 13/16] 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 14/16] 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 15/16] 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 16/16] 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(); +}