Skip to content

Commit f796041

Browse files
refactor(ground_segmentation)!: fix namespace and directory structure (autowarefoundation#7744)
* refactor: update namespace in ground_segmentation files Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp> * refactor: update namespace in ground_segmentation files Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp> * refactor: update ground_segmentation namespace and file structure Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp> * style(pre-commit): autofix * refactor: update ground_segmentation plugin names scheme Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp> * refactor: update ransac tester Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp> --------- Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp> Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>
1 parent a7b8a10 commit f796041

File tree

14 files changed

+98
-89
lines changed

14 files changed

+98
-89
lines changed

launch/tier4_perception_launch/launch/obstacle_segmentation/ground_segmentation/ground_segmentation.launch.py

+15-3
Original file line numberDiff line numberDiff line change
@@ -82,6 +82,12 @@ def create_additional_pipeline(self, lidar_name):
8282
"margin_min_z"
8383
]
8484
)
85+
# Get the plugin name from the full plugin path
86+
ground_segmentation_plugin_name = self.ground_segmentation_param[
87+
f"{lidar_name}_ground_filter"
88+
]["plugin"]
89+
ground_segmentation_plugin_name = ground_segmentation_plugin_name.split("::")[-1]
90+
8591
components = []
8692
components.append(
8793
ComposableNode(
@@ -110,7 +116,7 @@ def create_additional_pipeline(self, lidar_name):
110116
components.append(
111117
ComposableNode(
112118
package="ground_segmentation",
113-
plugin=self.ground_segmentation_param[f"{lidar_name}_ground_filter"]["plugin"],
119+
plugin="autoware::ground_segmentation::" + ground_segmentation_plugin_name,
114120
name=f"{lidar_name}_ground_filter",
115121
remappings=[
116122
("input", f"{lidar_name}/range_cropped/pointcloud"),
@@ -203,7 +209,7 @@ def create_ransac_pipeline(self):
203209
components.append(
204210
ComposableNode(
205211
package="ground_segmentation",
206-
plugin="ground_segmentation::RANSACGroundFilterComponent",
212+
plugin="autoware::ground_segmentation::" + "RANSACGroundFilterComponent",
207213
name="ransac_ground_filter",
208214
namespace="plane_fitting",
209215
remappings=[
@@ -228,6 +234,12 @@ def create_common_pipeline(self, input_topic, output_topic):
228234
self.vehicle_info["min_height_offset"]
229235
+ self.ground_segmentation_param["common_crop_box_filter"]["parameters"]["margin_min_z"]
230236
)
237+
# Get the plugin name from the full plugin path
238+
ground_segmentation_plugin_name = self.ground_segmentation_param["common_ground_filter"][
239+
"plugin"
240+
]
241+
ground_segmentation_plugin_name = ground_segmentation_plugin_name.split("::")[-1]
242+
231243
components = []
232244
components.append(
233245
ComposableNode(
@@ -256,7 +268,7 @@ def create_common_pipeline(self, input_topic, output_topic):
256268
components.append(
257269
ComposableNode(
258270
package="ground_segmentation",
259-
plugin=self.ground_segmentation_param["common_ground_filter"]["plugin"],
271+
plugin="autoware::ground_segmentation::" + ground_segmentation_plugin_name,
260272
name="common_ground_filter",
261273
remappings=[
262274
("input", "range_cropped/pointcloud"),

perception/ground_segmentation/CMakeLists.txt

+14-14
Original file line numberDiff line numberDiff line change
@@ -21,39 +21,39 @@ include_directories(
2121
${GRID_MAP_INCLUDE_DIR}
2222
)
2323

24-
ament_auto_add_library(ground_segmentation SHARED
25-
src/ray_ground_filter_nodelet.cpp
26-
src/ransac_ground_filter_nodelet.cpp
27-
src/scan_ground_filter_nodelet.cpp
24+
ament_auto_add_library(${PROJECT_NAME} SHARED
25+
src/ray_ground_filter/node.cpp
26+
src/ransac_ground_filter/node.cpp
27+
src/scan_ground_filter/node.cpp
2828
)
2929

30-
target_link_libraries(ground_segmentation
30+
target_link_libraries(${PROJECT_NAME}
3131
${Boost_LIBRARIES}
3232
${OpenCV_LIBRARIES}
3333
${PCL_LIBRARIES}
3434
)
3535

3636
if(OPENMP_FOUND)
37-
set_target_properties(ground_segmentation PROPERTIES
37+
set_target_properties(${PROJECT_NAME} PROPERTIES
3838
COMPILE_FLAGS ${OpenMP_CXX_FLAGS}
3939
LINK_FLAGS ${OpenMP_CXX_FLAGS}
4040
)
4141
endif()
4242

4343
# ========== Ground Filter ==========
4444
# -- Ray Ground Filter --
45-
rclcpp_components_register_node(ground_segmentation
46-
PLUGIN "ground_segmentation::RayGroundFilterComponent"
45+
rclcpp_components_register_node(${PROJECT_NAME}
46+
PLUGIN "autoware::ground_segmentation::RayGroundFilterComponent"
4747
EXECUTABLE ray_ground_filter_node)
4848

4949
# -- RANSAC Ground Filter --
50-
rclcpp_components_register_node(ground_segmentation
51-
PLUGIN "ground_segmentation::RANSACGroundFilterComponent"
50+
rclcpp_components_register_node(${PROJECT_NAME}
51+
PLUGIN "autoware::ground_segmentation::RANSACGroundFilterComponent"
5252
EXECUTABLE ransac_ground_filter_node)
5353

5454
# -- Scan Ground Filter --
55-
rclcpp_components_register_node(ground_segmentation
56-
PLUGIN "ground_segmentation::ScanGroundFilterComponent"
55+
rclcpp_components_register_node(${PROJECT_NAME}
56+
PLUGIN "autoware::ground_segmentation::ScanGroundFilterComponent"
5757
EXECUTABLE scan_ground_filter_node)
5858

5959

@@ -88,7 +88,7 @@ if(BUILD_TESTING)
8888
)
8989

9090
target_link_libraries(test_ray_ground_filter
91-
ground_segmentation
91+
${PROJECT_NAME}
9292
${YAML_CPP_LIBRARIES}
9393
)
9494

@@ -97,7 +97,7 @@ if(BUILD_TESTING)
9797
)
9898

9999
target_link_libraries(test_scan_ground_filter
100-
ground_segmentation
100+
${PROJECT_NAME}
101101
${YAML_CPP_LIBRARIES}
102102
)
103103
ament_add_ros_isolated_gtest(test_ransac_ground_filter

perception/ground_segmentation/config/ground_segmentation.param.yaml

+1-1
Original file line numberDiff line numberDiff line change
@@ -16,7 +16,7 @@
1616
negative: False
1717

1818
common_ground_filter:
19-
plugin: "ground_segmentation::ScanGroundFilterComponent"
19+
plugin: "autoware::ground_segmentation::ScanGroundFilterComponent"
2020
parameters:
2121
global_slope_max_angle_deg: 10.0
2222
local_slope_max_angle_deg: 13.0 # recommended 30.0 for non elevation_grid_mode

perception/ground_segmentation/launch/scan_ground_filter.launch.py

+1-1
Original file line numberDiff line numberDiff line change
@@ -36,7 +36,7 @@ def launch_setup(context, *args, **kwargs):
3636
nodes = [
3737
ComposableNode(
3838
package="ground_segmentation",
39-
plugin="ground_segmentation::ScanGroundFilterComponent",
39+
plugin="autoware::ground_segmentation::ScanGroundFilterComponent",
4040
name="scan_ground_filter",
4141
remappings=[
4242
("input", LaunchConfiguration("input/pointcloud")),

perception/ground_segmentation/src/ransac_ground_filter_nodelet.cpp perception/ground_segmentation/src/ransac_ground_filter/node.cpp

+14-14
Original file line numberDiff line numberDiff line change
@@ -12,7 +12,7 @@
1212
// See the License for the specific language governing permissions and
1313
// limitations under the License.
1414

15-
#include "ground_segmentation/ransac_ground_filter_nodelet.hpp"
15+
#include "node.hpp"
1616

1717
#include <pcl_ros/transforms.hpp>
1818

@@ -54,15 +54,6 @@ Eigen::Vector3d getArbitraryOrthogonalVector(const Eigen::Vector3d & input)
5454
return unit_vec;
5555
}
5656

57-
ground_segmentation::PlaneBasis getPlaneBasis(const Eigen::Vector3d & plane_normal)
58-
{
59-
ground_segmentation::PlaneBasis basis;
60-
basis.e_z = plane_normal;
61-
basis.e_x = getArbitraryOrthogonalVector(plane_normal);
62-
basis.e_y = basis.e_x.cross(basis.e_z);
63-
return basis;
64-
}
65-
6657
geometry_msgs::msg::Pose getDebugPose(const Eigen::Affine3d & plane_affine)
6758
{
6859
geometry_msgs::msg::Pose debug_pose;
@@ -78,8 +69,17 @@ geometry_msgs::msg::Pose getDebugPose(const Eigen::Affine3d & plane_affine)
7869
}
7970
} // namespace
8071

81-
namespace ground_segmentation
72+
namespace autoware::ground_segmentation
8273
{
74+
PlaneBasis getPlaneBasis(const Eigen::Vector3d & plane_normal)
75+
{
76+
PlaneBasis basis;
77+
basis.e_z = plane_normal;
78+
basis.e_x = getArbitraryOrthogonalVector(plane_normal);
79+
basis.e_y = basis.e_x.cross(basis.e_z);
80+
return basis;
81+
}
82+
8383
using pointcloud_preprocessor::get_param;
8484

8585
RANSACGroundFilterComponent::RANSACGroundFilterComponent(const rclcpp::NodeOptions & options)
@@ -203,7 +203,7 @@ Eigen::Affine3d RANSACGroundFilterComponent::getPlaneAffine(
203203
pcl::PointXYZ centroid_point;
204204
centroid.get(centroid_point);
205205
Eigen::Translation<double, 3> trans(centroid_point.x, centroid_point.y, centroid_point.z);
206-
const ground_segmentation::PlaneBasis basis = getPlaneBasis(plane_normal);
206+
const PlaneBasis basis = getPlaneBasis(plane_normal);
207207
Eigen::Matrix3d rot;
208208
rot << basis.e_x.x(), basis.e_y.x(), basis.e_z.x(), basis.e_x.y(), basis.e_y.y(), basis.e_z.y(),
209209
basis.e_x.z(), basis.e_y.z(), basis.e_z.z();
@@ -396,7 +396,7 @@ rcl_interfaces::msg::SetParametersResult RANSACGroundFilterComponent::paramCallb
396396
return result;
397397
}
398398

399-
} // namespace ground_segmentation
399+
} // namespace autoware::ground_segmentation
400400

401401
#include <rclcpp_components/register_node_macro.hpp>
402-
RCLCPP_COMPONENTS_REGISTER_NODE(ground_segmentation::RANSACGroundFilterComponent)
402+
RCLCPP_COMPONENTS_REGISTER_NODE(autoware::ground_segmentation::RANSACGroundFilterComponent)

perception/ground_segmentation/include/ground_segmentation/ransac_ground_filter_nodelet.hpp perception/ground_segmentation/src/ransac_ground_filter/node.hpp

+5-5
Original file line numberDiff line numberDiff line change
@@ -12,8 +12,8 @@
1212
// See the License for the specific language governing permissions and
1313
// limitations under the License.
1414

15-
#ifndef GROUND_SEGMENTATION__RANSAC_GROUND_FILTER_NODELET_HPP_
16-
#define GROUND_SEGMENTATION__RANSAC_GROUND_FILTER_NODELET_HPP_
15+
#ifndef RANSAC_GROUND_FILTER__NODE_HPP_
16+
#define RANSAC_GROUND_FILTER__NODE_HPP_
1717

1818
#include "pointcloud_preprocessor/filter.hpp"
1919

@@ -38,7 +38,7 @@
3838
#include <string>
3939
#include <vector>
4040

41-
namespace ground_segmentation
41+
namespace autoware::ground_segmentation
4242
{
4343
struct PlaneBasis
4444
{
@@ -129,6 +129,6 @@ class RANSACGroundFilterComponent : public pointcloud_preprocessor::Filter
129129
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
130130
explicit RANSACGroundFilterComponent(const rclcpp::NodeOptions & options);
131131
};
132-
} // namespace ground_segmentation
132+
} // namespace autoware::ground_segmentation
133133

134-
#endif // GROUND_SEGMENTATION__RANSAC_GROUND_FILTER_NODELET_HPP_
134+
#endif // RANSAC_GROUND_FILTER__NODE_HPP_

perception/ground_segmentation/include/ground_segmentation/gencolors.hpp perception/ground_segmentation/src/ray_ground_filter/gencolors.hpp

+6-7
Original file line numberDiff line numberDiff line change
@@ -53,19 +53,18 @@
5353
//
5454
//M*/
5555

56-
#ifndef GROUND_SEGMENTATION__GENCOLORS_HPP_
57-
#define GROUND_SEGMENTATION__GENCOLORS_HPP_
56+
#ifndef RAY_GROUND_FILTER__GENCOLORS_HPP_
57+
#define RAY_GROUND_FILTER__GENCOLORS_HPP_
5858

5959
#include <opencv2/core/core.hpp>
60+
#include <opencv2/opencv.hpp>
6061

6162
#include <opencv2/core/core_c.h>
6263

6364
#include <iostream>
6465
#include <vector>
65-
// #include <precomp.hpp>
66-
#include <opencv2/opencv.hpp>
6766

68-
namespace ray_ground_filter
67+
namespace autoware::ray_ground_filter
6968
{
7069
using namespace cv; // NOLINT
7170

@@ -159,5 +158,5 @@ inline void generateColors(std::vector<Scalar> & colors, size_t count, size_t fa
159158
colors[i] = Scalar(c.x, c.y, c.z);
160159
}
161160
}
162-
} // namespace ray_ground_filter
163-
#endif // GROUND_SEGMENTATION__GENCOLORS_HPP_
161+
} // namespace autoware::ray_ground_filter
162+
#endif // RAY_GROUND_FILTER__GENCOLORS_HPP_

perception/ground_segmentation/src/ray_ground_filter_nodelet.cpp perception/ground_segmentation/src/ray_ground_filter/node.cpp

+7-6
Original file line numberDiff line numberDiff line change
@@ -29,12 +29,12 @@
2929
* v1.0: amc-nu (abrahammonrroy@yahoo.com)
3030
*/
3131

32-
#include "ground_segmentation/ray_ground_filter_nodelet.hpp"
32+
#include "node.hpp"
3333

3434
#include <string>
3535
#include <vector>
3636

37-
namespace ground_segmentation
37+
namespace autoware::ground_segmentation
3838
{
3939
using pointcloud_preprocessor::get_param;
4040

@@ -250,8 +250,9 @@ void RayGroundFilterComponent::ClassifyPointCloud(
250250
// // Enable the dynamic reconfigure service
251251
// has_service = true;
252252
// srv_ = boost::make_shared<
253-
// dynamic_reconfigure::Server<ground_segmentation::RayGroundFilterConfig> >(nh);
254-
// dynamic_reconfigure::Server<ground_segmentation::RayGroundFilterConfig>::CallbackType f =
253+
// dynamic_reconfigure::Server<autoware::ground_segmentation::RayGroundFilterConfig> >(nh);
254+
// dynamic_reconfigure::Server<autoware::ground_segmentation::RayGroundFilterConfig>::CallbackType
255+
// f =
255256
// boost::bind(&RayGroundFilterComponent::config_callback, this, _1, _2);
256257
// srv_->setCallback(f);
257258
// return (true);
@@ -394,7 +395,7 @@ rcl_interfaces::msg::SetParametersResult RayGroundFilterComponent::paramCallback
394395
return result;
395396
}
396397

397-
} // namespace ground_segmentation
398+
} // namespace autoware::ground_segmentation
398399

399400
#include <rclcpp_components/register_node_macro.hpp>
400-
RCLCPP_COMPONENTS_REGISTER_NODE(ground_segmentation::RayGroundFilterComponent)
401+
RCLCPP_COMPONENTS_REGISTER_NODE(autoware::ground_segmentation::RayGroundFilterComponent)

perception/ground_segmentation/include/ground_segmentation/ray_ground_filter_nodelet.hpp perception/ground_segmentation/src/ray_ground_filter/node.hpp

+12-13
Original file line numberDiff line numberDiff line change
@@ -42,8 +42,8 @@
4242
* v1.0: amc-nu (abrahammonrroy@yahoo.com)
4343
*/
4444

45-
#ifndef GROUND_SEGMENTATION__RAY_GROUND_FILTER_NODELET_HPP_
46-
#define GROUND_SEGMENTATION__RAY_GROUND_FILTER_NODELET_HPP_
45+
#ifndef RAY_GROUND_FILTER__NODE_HPP_
46+
#define RAY_GROUND_FILTER__NODE_HPP_
4747

4848
#include <sensor_msgs/msg/point_cloud2.hpp>
4949

@@ -58,14 +58,7 @@
5858
#include <tf2_eigen/tf2_eigen.hpp>
5959
#endif
6060

61-
#include <tf2_ros/transform_listener.h>
62-
63-
#include <chrono>
64-
#include <string>
65-
#include <vector>
66-
// #include <pcl_ros/point_cloud.h>
67-
68-
#include "ground_segmentation/gencolors.hpp"
61+
#include "gencolors.hpp"
6962
#include "pointcloud_preprocessor/filter.hpp"
7063

7164
#include <opencv2/core.hpp>
@@ -76,11 +69,17 @@
7669
#include <boost/geometry/geometries/point_xy.hpp>
7770
#include <boost/optional.hpp>
7871

72+
#include <tf2_ros/transform_listener.h>
73+
74+
#include <chrono>
75+
#include <string>
76+
#include <vector>
77+
7978
namespace bg = boost::geometry;
8079
using Point = bg::model::d2::point_xy<double>;
8180
using Polygon = bg::model::polygon<Point>;
8281

83-
namespace ground_segmentation
82+
namespace autoware::ground_segmentation
8483
{
8584
class RayGroundFilterComponent : public pointcloud_preprocessor::Filter
8685
{
@@ -206,6 +205,6 @@ class RayGroundFilterComponent : public pointcloud_preprocessor::Filter
206205
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
207206
explicit RayGroundFilterComponent(const rclcpp::NodeOptions & options);
208207
};
209-
} // namespace ground_segmentation
208+
} // namespace autoware::ground_segmentation
210209

211-
#endif // GROUND_SEGMENTATION__RAY_GROUND_FILTER_NODELET_HPP_
210+
#endif // RAY_GROUND_FILTER__NODE_HPP_

0 commit comments

Comments
 (0)