Skip to content

Commit 2fba5f5

Browse files
pre-commit-ci[bot]pre-commit-ci[bot]
committed
style(pre-commit): autofix
1 parent bfa8b58 commit 2fba5f5

File tree

5 files changed

+69
-106
lines changed

5 files changed

+69
-106
lines changed

‎sensing/autoware_crop_box_filter/CMakeLists.txt

-2
Original file line numberDiff line numberDiff line change
@@ -26,5 +26,3 @@ ament_auto_package(INSTALL_TO_SHARE
2626
launch
2727
config
2828
)
29-
30-

‎sensing/autoware_crop_box_filter/README.md

+22-26
Original file line numberDiff line numberDiff line change
@@ -12,41 +12,37 @@ The `autoware_crop_box_filter` is implemented as a autoware core node that subsc
1212

1313
### Input
1414

15-
| Name | Type | Description |
16-
| ----------------- | ------------------------------- | ----------------- |
17-
| `~/input/points` | `sensor_msgs::msg::PointCloud2` | reference points |
15+
| Name | Type | Description |
16+
| ---------------- | ------------------------------- | ---------------- |
17+
| `~/input/points` | `sensor_msgs::msg::PointCloud2` | reference points |
1818

1919
### Output
2020

21-
| Name | Type | Description |
22-
| ----------------- | ------------------------------- | --------------- |
23-
| `~/output/points` | `sensor_msgs::msg::PointCloud2` | filtered points |
21+
| Name | Type | Description |
22+
| -------------------- | ------------------------------------ | -------------------- |
23+
| `~/output/points` | `sensor_msgs::msg::PointCloud2` | filtered points |
2424
| `~/crop_box_polygon` | `geometry_msgs::msg::PolygonStamped` | bounding box polygon |
2525

2626
## Parameters
2727

2828
### Launch file Parameters
2929

30-
| Name | Type | Default Value | Description |
31-
| ------------------ | ------ | ------------- | ------------------------------------- |
32-
| `input_frame` | string | " " | the frame id in which filtering is performed |
33-
| `output_frame` | string | " " | output frame id of the filtered points |
34-
| `input_pointcloud_frame` | string | " " | frame id of input pointcloud |
35-
| `max_queue_size` | int | 5 | max buffer size of input/output topics |
36-
| `crop_box_filter_param_file` | string | " " | path to the parameter file for the node |
37-
30+
| Name | Type | Default Value | Description |
31+
| ---------------------------- | ------ | ------------- | -------------------------------------------- |
32+
| `input_frame` | string | " " | the frame id in which filtering is performed |
33+
| `output_frame` | string | " " | output frame id of the filtered points |
34+
| `input_pointcloud_frame` | string | " " | frame id of input pointcloud |
35+
| `max_queue_size` | int | 5 | max buffer size of input/output topics |
36+
| `crop_box_filter_param_file` | string | " " | path to the parameter file for the node |
3837

3938
### Node Parameters
4039

41-
| Name | Type | Default Value | Description |
42-
| ------------------ | ------ | ------------- | ------------------------------------- |
43-
| `min_x` | double | -5.0 | minimum x value of the crop box |
44-
| `min_y` | double | -5.0 | minimum y value of the crop box |
45-
| `min_z` | double | -5.0 | minimum z value of the crop box |
46-
| `max_x` | double | 5.0 | maximum x value of the crop box |
47-
| `max_y` | double | 5.0 | maximum y value of the crop box |
48-
| `max_z` | double | 5.0 | maximum z value of the crop box |
49-
| `negative` | bool | true | if true, points inside the box are removed, otherwise points outside the box are removed |
50-
51-
52-
40+
| Name | Type | Default Value | Description |
41+
| ---------- | ------ | ------------- | ---------------------------------------------------------------------------------------- |
42+
| `min_x` | double | -5.0 | minimum x value of the crop box |
43+
| `min_y` | double | -5.0 | minimum y value of the crop box |
44+
| `min_z` | double | -5.0 | minimum z value of the crop box |
45+
| `max_x` | double | 5.0 | maximum x value of the crop box |
46+
| `max_y` | double | 5.0 | maximum y value of the crop box |
47+
| `max_z` | double | 5.0 | maximum z value of the crop box |
48+
| `negative` | bool | true | if true, points inside the box are removed, otherwise points outside the box are removed |

‎sensing/autoware_crop_box_filter/include/autoware/crop_box_filter/crop_box_filter_node.hpp

+16-20
Original file line numberDiff line numberDiff line change
@@ -55,36 +55,33 @@
5555
#ifndef AUTOWARE__CROP_BOX_FILTER__CROP_BOX_FILTER_NODE_HPP_
5656
#define AUTOWARE__CROP_BOX_FILTER__CROP_BOX_FILTER_NODE_HPP_
5757

58-
#include <geometry_msgs/msg/polygon_stamped.hpp>
59-
#include <sensor_msgs/msg/point_cloud2.hpp>
60-
#include <sensor_msgs/point_cloud2_iterator.hpp>
61-
58+
#include <autoware/point_types/types.hpp>
6259
#include <autoware_utils/ros/debug_publisher.hpp>
6360
#include <autoware_utils/ros/managed_transform_buffer.hpp>
6461
#include <autoware_utils/ros/published_time_publisher.hpp>
6562
#include <autoware_utils/system/stop_watch.hpp>
6663

67-
#include <autoware/point_types/types.hpp>
64+
#include <geometry_msgs/msg/polygon_stamped.hpp>
65+
#include <sensor_msgs/msg/point_cloud2.hpp>
66+
#include <sensor_msgs/point_cloud2_iterator.hpp>
6867

6968
#include <memory>
70-
#include <vector>
7169
#include <string>
7270
#include <utility>
71+
#include <vector>
7372

7473
namespace autoware::crop_box_filter
7574
{
7675
class CropBoxFilter : public rclcpp::Node
7776
{
77+
using PointCloud2 = sensor_msgs::msg::PointCloud2;
78+
using PointCloud2ConstPtr = sensor_msgs::msg::PointCloud2::ConstSharedPtr;
7879

79-
using PointCloud2 = sensor_msgs::msg::PointCloud2;
80-
using PointCloud2ConstPtr = sensor_msgs::msg::PointCloud2::ConstSharedPtr;
81-
82-
using PointCloud = pcl::PointCloud<pcl::PointXYZ>;
83-
using PointCloudPtr = PointCloud::Ptr;
84-
using PointCloudConstPtr = PointCloud::ConstPtr;
80+
using PointCloud = pcl::PointCloud<pcl::PointXYZ>;
81+
using PointCloudPtr = PointCloud::Ptr;
82+
using PointCloudConstPtr = PointCloud::ConstPtr;
8583

8684
private:
87-
8885
// member variable declaration & definitions *************************************
8986

9087
/** \brief The managed transform buffer. */
@@ -141,16 +138,16 @@ using PointCloudConstPtr = PointCloud::ConstPtr;
141138
std::unique_ptr<autoware_utils::PublishedTimePublisher> published_time_publisher_;
142139

143140
// function declaration *************************************
144-
141+
145142
void publish_crop_box_polygon();
146-
143+
147144
void pointcloud_callback(const PointCloud2ConstPtr cloud);
148145

149146
/** \brief Parameter service callback */
150147
rcl_interfaces::msg::SetParametersResult param_callback(const std::vector<rclcpp::Parameter> & p);
151148

152-
/** \brief Return whether the input PointCloud2 data has the same layout than PointXYZI. That is to
153-
* say whether you can memcpy from the PointCloud2 data buffer to a PointXYZI */
149+
/** \brief Return whether the input PointCloud2 data has the same layout than PointXYZI. That is
150+
* to say whether you can memcpy from the PointCloud2 data buffer to a PointXYZI */
154151
bool is_data_layout_compatible_with_point_xyzi(const PointCloud2 & input);
155152

156153
/** \brief Return whether the input PointCloud2 data has the same layout than PointXYZIRC. That is
@@ -161,8 +158,8 @@ using PointCloudConstPtr = PointCloud::ConstPtr;
161158
* is to say whether you can memcpy from the PointCloud2 data buffer to a PointXYZIRADRT */
162159
bool is_data_layout_compatible_with_point_xyziradrt(const PointCloud2 & input);
163160

164-
/** \brief Return whether the input PointCloud2 data has the same layout than PointXYZIRCAEDT. That
165-
* is to say whether you can memcpy from the PointCloud2 data buffer to a PointXYZIRCAEDT */
161+
/** \brief Return whether the input PointCloud2 data has the same layout than PointXYZIRCAEDT.
162+
* That is to say whether you can memcpy from the PointCloud2 data buffer to a PointXYZIRCAEDT */
166163
bool is_data_layout_compatible_with_point_xyzircaedt(const PointCloud2 & input);
167164

168165
bool is_valid(const PointCloud2ConstPtr & cloud);
@@ -180,7 +177,6 @@ using PointCloudConstPtr = PointCloud::ConstPtr;
180177
}
181178
return false;
182179
}
183-
184180

185181
public:
186182
PCL_MAKE_ALIGNED_OPERATOR_NEW

‎sensing/autoware_crop_box_filter/package.xml

+1-2
Original file line numberDiff line numberDiff line change
@@ -17,7 +17,6 @@
1717
<maintainer email="lxg19892021@gmail.com">Xingang Liu</maintainer>
1818
<license>Apache License 2.0</license>
1919

20-
2120
<!-- Original authors -->
2221
<author>Open Perception</author>
2322
<author email="julius@kammerl.de">Julius Kammerl</author>
@@ -28,10 +27,10 @@
2827

2928
<depend>autoware_point_types</depend>
3029
<depend>autoware_utils</depend>
30+
<depend>geometry_msgs</depend>
3131
<depend>rclcpp</depend>
3232
<depend>rclcpp_components</depend>
3333
<depend>sensor_msgs</depend>
34-
<depend>geometry_msgs</depend>
3534

3635
<test_depend>ament_lint_auto</test_depend>
3736
<test_depend>autoware_lint_common</test_depend>

‎sensing/autoware_crop_box_filter/src/crop_box_filter_node.cpp

+30-56
Original file line numberDiff line numberDiff line change
@@ -74,60 +74,47 @@ CropBoxFilter::CropBoxFilter(const rclcpp::NodeOptions & node_options)
7474

7575
// get transform info for pointcloud
7676
{
77-
78-
tf_input_orig_frame_ = static_cast<std::string>(declare_parameter("input_pointcloud_frame", "base_link"));
77+
tf_input_orig_frame_ =
78+
static_cast<std::string>(declare_parameter("input_pointcloud_frame", "base_link"));
7979
tf_input_frame_ = static_cast<std::string>(declare_parameter("input_frame", "base_link"));
8080
tf_output_frame_ = static_cast<std::string>(declare_parameter("output_frame", "base_link"));
8181

8282
// Always consider static TF if in & out frames are same
8383
bool has_static_tf_only = false;
8484
if (tf_input_frame_ == tf_output_frame_) {
85-
8685
RCLCPP_INFO(
8786
this->get_logger(),
8887
"Input and output frames are the same. Overriding has_static_tf_only to true.");
89-
88+
9089
has_static_tf_only = true;
9190
}
9291
managed_tf_buffer_ =
9392
std::make_unique<autoware_utils::ManagedTransformBuffer>(this, has_static_tf_only);
9493

95-
96-
if (tf_input_orig_frame_ == tf_input_frame_)
97-
{
94+
if (tf_input_orig_frame_ == tf_input_frame_) {
9895
need_preprocess_transform_ = false;
9996
eigen_transform_preprocess_ = Eigen::Matrix4f::Identity(4, 4);
100-
}
101-
else
102-
{
97+
} else {
10398
if (!managed_tf_buffer_->get_transform(
104-
tf_input_frame_, tf_input_orig_frame_, eigen_transform_preprocess_))
105-
{
99+
tf_input_frame_, tf_input_orig_frame_, eigen_transform_preprocess_)) {
106100
RCLCPP_ERROR(
107-
this->get_logger(),
108-
"Cannot get transform from %s to %s. Please check your TF tree.",tf_input_orig_frame_.c_str() , tf_input_frame_.c_str());
109-
}
110-
else
111-
{
101+
this->get_logger(), "Cannot get transform from %s to %s. Please check your TF tree.",
102+
tf_input_orig_frame_.c_str(), tf_input_frame_.c_str());
103+
} else {
112104
need_preprocess_transform_ = true;
113105
}
114106
}
115107

116-
if (tf_input_frame_ == tf_output_frame_)
117-
{
108+
if (tf_input_frame_ == tf_output_frame_) {
118109
need_postprocess_transform_ = false;
119110
eigen_transform_postprocess_ = Eigen::Matrix4f::Identity(4, 4);
120-
}else
121-
{
111+
} else {
122112
if (!managed_tf_buffer_->get_transform(
123-
tf_output_frame_, tf_input_frame_, eigen_transform_postprocess_))
124-
{
113+
tf_output_frame_, tf_input_frame_, eigen_transform_postprocess_)) {
125114
RCLCPP_ERROR(
126-
this->get_logger(),
127-
"Cannot get transform from %s to %s. Please check your TF tree.",tf_input_frame_.c_str() , tf_output_frame_.c_str());
128-
}
129-
else
130-
{
115+
this->get_logger(), "Cannot get transform from %s to %s. Please check your TF tree.",
116+
tf_input_frame_.c_str(), tf_output_frame_.c_str());
117+
} else {
131118
need_postprocess_transform_ = true;
132119
}
133120
}
@@ -166,31 +153,26 @@ CropBoxFilter::CropBoxFilter(const rclcpp::NodeOptions & node_options)
166153
// set parameter service callback
167154
{
168155
using std::placeholders::_1;
169-
set_param_res_ = this->add_on_set_parameters_callback(
170-
std::bind(&CropBoxFilter::param_callback, this, _1));
156+
set_param_res_ =
157+
this->add_on_set_parameters_callback(std::bind(&CropBoxFilter::param_callback, this, _1));
171158
}
172159

173160
// set input pointcloud callback
174161
{
175162
sub_input_ = this->create_subscription<PointCloud2>(
176-
"input", rclcpp::SensorDataQoS().keep_last(max_queue_size_),
163+
"input", rclcpp::SensorDataQoS().keep_last(max_queue_size_),
177164
std::bind(&CropBoxFilter::pointcloud_callback, this, std::placeholders::_1));
178165
}
179166

180-
181167
RCLCPP_DEBUG(this->get_logger(), "[Filter Constructor] successfully created.");
182168
}
183169

184-
185-
186-
187170
void CropBoxFilter::pointcloud_callback(const PointCloud2ConstPtr cloud)
188171
{
189172
// check whether the pointcloud is valid
190173
if (
191174
!is_data_layout_compatible_with_point_xyzircaedt(*cloud) &&
192-
!is_data_layout_compatible_with_point_xyzirc(*cloud))
193-
{
175+
!is_data_layout_compatible_with_point_xyzirc(*cloud)) {
194176
RCLCPP_ERROR(
195177
get_logger(),
196178
"The pointcloud layout is not compatible with PointXYZIRCAEDT or PointXYZIRC. Aborting");
@@ -223,14 +205,13 @@ void CropBoxFilter::pointcloud_callback(const PointCloud2ConstPtr cloud)
223205
"received.",
224206
cloud->width * cloud->height, cloud->header.frame_id.c_str());
225207
// pointcloud check finished
226-
208+
227209
// pointcloud processing
228210
auto output = PointCloud2();
229211

230212
std::scoped_lock lock(mutex_);
231213
stop_watch_ptr_->toc("processing_time", true);
232214

233-
234215
int x_offset = cloud->fields[pcl::getFieldIndex(*cloud, "x")].offset;
235216
int y_offset = cloud->fields[pcl::getFieldIndex(*cloud, "y")].offset;
236217
int z_offset = cloud->fields[pcl::getFieldIndex(*cloud, "z")].offset;
@@ -242,8 +223,7 @@ void CropBoxFilter::pointcloud_callback(const PointCloud2ConstPtr cloud)
242223

243224
// pointcloud processing loop
244225
for (size_t global_offset = 0; global_offset + cloud->point_step <= cloud->data.size();
245-
global_offset += cloud->point_step)
246-
{
226+
global_offset += cloud->point_step) {
247227
Eigen::Vector4f point;
248228
Eigen::Vector4f point_preprocessed;
249229
std::memcpy(&point[0], &cloud->data[global_offset + x_offset], sizeof(float));
@@ -266,22 +246,18 @@ void CropBoxFilter::pointcloud_callback(const PointCloud2ConstPtr cloud)
266246
point[1] > param_.min_y && point[1] < param_.max_y &&
267247
point[0] > param_.min_x && point[0] < param_.max_x;
268248
if ((!param_.negative && point_is_inside) || (param_.negative && !point_is_inside)) {
269-
270249
// apply post-transform if needed
271-
if(need_postprocess_transform_)
272-
{
273-
if(need_preprocess_transform_)
274-
{
250+
if (need_postprocess_transform_) {
251+
if (need_preprocess_transform_) {
275252
Eigen::Vector4f point_postprocessed = eigen_transform_postprocess_ * point_preprocessed;
276253

277254
memcpy(&output.data[output_size], &cloud->data[global_offset], cloud->point_step);
278255

279256
std::memcpy(&output.data[output_size + x_offset], &point_postprocessed[0], sizeof(float));
280257
std::memcpy(&output.data[output_size + y_offset], &point_postprocessed[1], sizeof(float));
281258
std::memcpy(&output.data[output_size + z_offset], &point_postprocessed[2], sizeof(float));
282-
283-
}else
284-
{
259+
260+
} else {
285261
Eigen::Vector4f point_postprocessed = eigen_transform_postprocess_ * point;
286262

287263
memcpy(&output.data[output_size], &cloud->data[global_offset], cloud->point_step);
@@ -290,9 +266,7 @@ void CropBoxFilter::pointcloud_callback(const PointCloud2ConstPtr cloud)
290266
std::memcpy(&output.data[output_size + y_offset], &point_postprocessed[1], sizeof(float));
291267
std::memcpy(&output.data[output_size + z_offset], &point_postprocessed[2], sizeof(float));
292268
}
293-
}
294-
else
295-
{
269+
} else {
296270
memcpy(&output.data[output_size], &cloud->data[global_offset], cloud->point_step);
297271

298272
if (need_preprocess_transform_) {
@@ -325,8 +299,7 @@ void CropBoxFilter::pointcloud_callback(const PointCloud2ConstPtr cloud)
325299
output.row_step = static_cast<uint32_t>(output.data.size() / output.height);
326300

327301
// publish polygon if subscribers exist
328-
if(crop_box_polygon_pub_->get_subscription_count() > 0)
329-
{
302+
if (crop_box_polygon_pub_->get_subscription_count() > 0) {
330303
publish_crop_box_polygon();
331304
}
332305

@@ -419,7 +392,8 @@ rcl_interfaces::msg::SetParametersResult CropBoxFilter::param_callback(
419392
new_param.max_x = get_param(p, "max_x", new_param.max_x) ? new_param.max_x : param_.max_x;
420393
new_param.max_y = get_param(p, "max_y", new_param.max_y) ? new_param.max_y : param_.max_y;
421394
new_param.max_z = get_param(p, "max_z", new_param.max_z) ? new_param.max_z : param_.max_z;
422-
new_param.negative = get_param(p, "negative", new_param.negative) ? new_param.negative : param_.negative;
395+
new_param.negative =
396+
get_param(p, "negative", new_param.negative) ? new_param.negative : param_.negative;
423397

424398
param_ = new_param;
425399

@@ -620,7 +594,7 @@ bool CropBoxFilter::is_data_layout_compatible_with_point_xyzircaedt(const PointC
620594
return same_layout;
621595
}
622596

623-
bool CropBoxFilter::is_valid( const PointCloud2ConstPtr & cloud)
597+
bool CropBoxFilter::is_valid(const PointCloud2ConstPtr & cloud)
624598
{
625599
if (cloud->width * cloud->height * cloud->point_step != cloud->data.size()) {
626600
RCLCPP_WARN(

0 commit comments

Comments
 (0)