Branch data Line data Source code
1 : : // Copyright 2018-2020 the Autoware Foundation, Arm Limited
2 : : //
3 : : // Licensed under the Apache License, Version 2.0 (the "License");
4 : : // you may not use this file except in compliance with the License.
5 : : // You may obtain a copy of the License at
6 : : //
7 : : // http://www.apache.org/licenses/LICENSE-2.0
8 : : //
9 : : // Unless required by applicable law or agreed to in writing, software
10 : : // distributed under the License is distributed on an "AS IS" BASIS,
11 : : // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12 : : // See the License for the specific language governing permissions and
13 : : // limitations under the License.
14 : : //
15 : : // Co-developed by Tier IV, Inc. and Apex.AI, Inc.
16 : : #include <common/types.hpp>
17 : : #include <lidar_utils/point_cloud_utils.hpp>
18 : : #include <ray_ground_classifier_nodes/ray_ground_classifier_cloud_node.hpp>
19 : : #include <rclcpp/rclcpp.hpp>
20 : : #include <rclcpp_components/register_node_macro.hpp>
21 : : #include <stdlib.h>
22 : :
23 : : #include <limits>
24 : : #include <string>
25 : :
26 : : namespace autoware
27 : : {
28 : : namespace perception
29 : : {
30 : : namespace filters
31 : : {
32 : : namespace ray_ground_classifier_nodes
33 : : {
34 : : ////////////////////////////////////////////////////////////////////////////////
35 : : using autoware::common::types::PointXYZI;
36 : : using autoware::common::types::float32_t;
37 : : using autoware::perception::filters::ray_ground_classifier::PointPtrBlock;
38 : :
39 : : using std::placeholders::_1;
40 : :
41 : : using autoware::common::lidar_utils::has_intensity_and_throw_if_no_xyz;
42 : :
43 : : using autoware::common::lidar_utils::CloudModifier;
44 : :
45 : 5 : RayGroundClassifierCloudNode::RayGroundClassifierCloudNode(
46 : 5 : const rclcpp::NodeOptions & node_options)
47 : : : Node("ray_ground_classifier", node_options),
48 [ + - - - ]: 5 : m_classifier(ray_ground_classifier::Config{
49 [ + - + - : 10 : static_cast<float32_t>(declare_parameter("classifier.sensor_height_m").get<float32_t>()),
+ - ]
50 : : static_cast<float32_t>(declare_parameter(
51 [ + - + - : 10 : "classifier.max_local_slope_deg").get<float32_t>()),
+ - + - +
- - - ]
52 : : static_cast<float32_t>(declare_parameter(
53 [ + - + - : 10 : "classifier.max_global_slope_deg").get<float32_t>()),
+ - + - +
- - - ]
54 : : static_cast<float32_t>(declare_parameter(
55 [ + - + - : 10 : "classifier.nonground_retro_thresh_deg").get<float32_t>()),
+ - + - +
- - - ]
56 : : static_cast<float32_t>(declare_parameter(
57 [ + - + - : 10 : "classifier.min_height_thresh_m").get<float32_t>()),
+ - + - +
- - - ]
58 : : static_cast<float32_t>(declare_parameter(
59 [ + - + - : 10 : "classifier.max_global_height_thresh_m").get<float32_t>()),
+ - + - +
- - - ]
60 : : static_cast<float32_t>(declare_parameter(
61 [ + - + - : 10 : "classifier.max_last_local_ground_thresh_m").get<float32_t>()),
+ - + - +
- - - ]
62 : : static_cast<float32_t>(declare_parameter(
63 [ + - + - : 15 : "classifier.max_provisional_ground_distance_m").get<float32_t>())
+ - + - +
- + - -
- ]
64 : : }),
65 [ + - - - ]: 5 : m_aggregator(ray_ground_classifier::RayAggregator::Config{
66 : : static_cast<float32_t>(declare_parameter(
67 [ + - + - : 10 : "aggregator.min_ray_angle_rad").get<float32_t>()),
+ - ]
68 : : static_cast<float32_t>(declare_parameter(
69 [ + - + - : 10 : "aggregator.max_ray_angle_rad").get<float32_t>()),
+ - + - +
- - - ]
70 [ + - + - : 10 : static_cast<float32_t>(declare_parameter("aggregator.ray_width_rad").get<float32_t>()),
+ - + - +
- - - ]
71 : : static_cast<std::size_t>(
72 [ + - + - : 15 : declare_parameter("aggregator.max_ray_points").get<std::size_t>())
+ - + - +
- + - -
- ]
73 : : }),
74 [ + - + - : 10 : m_pcl_size(static_cast<uint32_t>(declare_parameter("pcl_size").get<uint32_t>())),
+ - + - ]
75 [ + - + - : 10 : m_frame_id(declare_parameter("frame_id").get<std::string>().c_str()),
+ - ]
76 : : m_has_failed(false),
77 [ + - + - : 15 : m_timeout(std::chrono::milliseconds{declare_parameter("cloud_timeout_ms").get<uint16_t>()}),
+ - + - ]
78 : : m_raw_sub_ptr(create_subscription<PointCloud2>(
79 : : "points_in",
80 [ + - ]: 5 : rclcpp::QoS(10), std::bind(&RayGroundClassifierCloudNode::callback, this, _1))),
81 : : m_ground_pub_ptr(create_publisher<PointCloud2>(
82 [ + - ]: 5 : "points_ground", rclcpp::QoS(10))),
83 : : m_nonground_pub_ptr(create_publisher<PointCloud2>(
84 [ + - + - : 80 : "points_nonground", rclcpp::QoS(10)))
+ - + - +
- + - + -
+ - - + +
- + - + -
+ - + - +
- + - + -
+ - + - +
- + - + -
- - - - -
- - - ]
85 : : {
86 : : // initialize messages
87 [ + - ]: 5 : CloudModifier{
88 [ + - ]: 5 : m_ground_msg, m_frame_id}.reserve(m_pcl_size);
89 [ + - - - ]: 5 : CloudModifier{
90 [ + - ]: 5 : m_nonground_msg, m_frame_id}.reserve(m_pcl_size);
91 : 5 : }
92 : : ////////////////////////////////////////////////////////////////////////////////
93 : : void
94 : 97 : RayGroundClassifierCloudNode::callback(const PointCloud2::SharedPtr msg)
95 : : {
96 : 97 : PointXYZIF pt_tmp;
97 : 97 : pt_tmp.id = static_cast<uint16_t>(PointXYZIF::END_OF_SCAN_ID);
98 : 97 : const ray_ground_classifier::PointXYZIFR eos_pt{&pt_tmp};
99 : :
100 : : try {
101 [ + - ]: 97 : CloudModifier ground_msg_modifier{m_ground_msg};
102 : : CloudModifier nonground_msg_modifier{
103 [ + - ]: 97 : m_nonground_msg};
104 : :
105 : : // Reset messages and aggregator to ensure they are in a good state
106 [ + - ]: 97 : reset();
107 : : // Verify header
108 [ + + ]: 97 : if (msg->header.frame_id != m_ground_msg.header.frame_id) {
109 : : throw std::runtime_error(
110 : : "RayGroundClassifierCloudNode: raw topic from unexpected "
111 [ + - - + : 190 : "frame (expected '" + m_ground_msg.header.frame_id +
- - ]
112 [ + - + - : 380 : "', got '" + msg->header.frame_id + "')");
+ - + - -
+ - + - -
- - ]
113 : : }
114 : : // Verify the consistency of PointCloud msg
115 : 2 : const auto data_length = msg->width * msg->height * msg->point_step;
116 [ + - - + ]: 2 : if ((msg->data.size() != msg->row_step) || (data_length != msg->row_step)) {
117 [ # # ]: 0 : throw std::runtime_error("RayGroundClassifierCloudNode: Malformed PointCloud2");
118 : : }
119 : : // Verify the point cloud format and assign correct point_step
120 [ + - - + ]: 2 : if (!has_intensity_and_throw_if_no_xyz(msg)) {
121 [ # # # # : 0 : RCLCPP_WARN(
# # # # #
# # # # #
# # # # #
# # # # #
# # # # #
# ]
122 : : this->get_logger(),
123 : : "RayGroundClassifierNode Warning: PointCloud doesn't have intensity field");
124 : : }
125 : : // Harvest timestamp
126 : 2 : m_nonground_msg.header.stamp = msg->header.stamp;
127 : 2 : m_ground_msg.header.stamp = msg->header.stamp;
128 : : // Add all points to aggregator
129 : : // Iterate through the data, but skip intensity in case the point cloud does not have it.
130 : : // For example:
131 : : //
132 : : // point_step = 4
133 : : // x y z i a b c x y z i a b c
134 : : // ^------ ^------
135 : : size_t num_ready = 0;
136 : : bool8_t has_encountered_unknown_exception = false;
137 : : bool8_t abort = false;
138 : :
139 : 2 : std::size_t point_step = msg->point_step;
140 [ + + ]: 22 : for (std::size_t idx = 0U; idx < msg->data.size(); idx += point_step) {
141 [ - + ]: 20 : if (abort) {
142 : 0 : continue;
143 : : }
144 : : try {
145 : : PointXYZIF * pt;
146 : : // TODO(c.ho) Fix below deviation after #2131 is in
147 : : //lint -e{925, 9110} Need to convert pointers and use bit for external API NOLINT
148 : : pt = reinterpret_cast<PointXYZIF *>(&msg->data[idx]);
149 : : // don't bother inserting the points almost (0,0).
150 : : // Too many of those makes the bin 0 overflow
151 [ - + ]: 20 : if ((fabsf(pt->x) > std::numeric_limits<decltype(pt->x)>::epsilon()) ||
152 [ # # ]: 0 : (fabsf(pt->y) > std::numeric_limits<decltype(pt->y)>::epsilon()))
153 : : {
154 [ + - - + ]: 20 : if (!m_aggregator.insert(pt)) {
155 [ # # ]: 0 : m_aggregator.end_of_scan();
156 : : }
157 : : } else {
158 [ # # # # : 0 : nonground_msg_modifier.push_back(PointXYZI{pt->x, pt->y, pt->z, pt->intensity});
# ]
159 : : }
160 : 0 : } catch (const std::runtime_error & e) {
161 : 0 : m_has_failed = true;
162 [ - - - - : 0 : RCLCPP_INFO(this->get_logger(), e.what());
- - - - -
- - - - -
- - - - -
- - - - -
- - - - -
- ]
163 : : abort = true;
164 : 0 : } catch (const std::exception & e) {
165 : 0 : m_has_failed = true;
166 [ - - - - : 0 : RCLCPP_INFO(this->get_logger(), e.what());
- - - - -
- - - - -
- - - - -
- - - - -
- - - - -
- ]
167 : : abort = true;
168 [ - - ]: 0 : } catch (...) {
169 [ - - - - : 0 : RCLCPP_INFO(
- - - - -
- - - - -
- - - - -
- - - - -
- - - - -
- ]
170 : : this->get_logger(),
171 : : "RayGroundClassifierCloudNode has encountered an unknown failure");
172 : : abort = true;
173 : : has_encountered_unknown_exception = true;
174 : : }
175 : : }
176 : :
177 : : // if abort, we skip all remaining the parallel work to be able to return/throw
178 [ + - ]: 2 : if (!abort) {
179 [ + - ]: 2 : m_aggregator.end_of_scan();
180 [ + - ]: 2 : num_ready = m_aggregator.get_ready_ray_count();
181 : :
182 : : // Partition each ray
183 [ + + ]: 22 : for (size_t i = 0; i < num_ready; i++) {
184 [ - + ]: 20 : if (abort) {
185 : 0 : continue;
186 : : }
187 : : // Note: if an exception occurs in this loop, the aggregator can get into a bad state
188 : : // (e.g. overrun capacity)
189 : : PointPtrBlock ground_blk;
190 : : PointPtrBlock nonground_blk;
191 : : try {
192 [ + - + - ]: 20 : auto ray = m_aggregator.get_next_ray();
193 : : // partition: should never fail, guaranteed to have capacity via other checks
194 [ + - ]: 20 : m_classifier.partition(ray, ground_blk, nonground_blk);
195 : :
196 : : // Add ray to point clouds
197 [ + + ]: 40 : for (auto & ground_point : ground_blk) {
198 : 20 : ground_msg_modifier.push_back(
199 : 20 : PointXYZI{
200 [ + - ]: 20 : ground_point->x, ground_point->y, ground_point->z, ground_point->intensity});
201 : : }
202 [ - + ]: 20 : for (auto & nonground_point : nonground_blk) {
203 : 0 : nonground_msg_modifier.push_back(
204 [ # # ]: 0 : PointXYZI{
205 : 0 : nonground_point->x, nonground_point->y, nonground_point->z,
206 [ # # ]: 0 : nonground_point->intensity});
207 : : }
208 : 0 : } catch (const std::runtime_error & e) {
209 : 0 : m_has_failed = true;
210 [ - - - - : 0 : RCLCPP_INFO(this->get_logger(), e.what());
- - - - -
- - - - -
- - - - -
- - - - -
- - - - -
- ]
211 : : abort = true;
212 : 0 : } catch (const std::exception & e) {
213 : 0 : m_has_failed = true;
214 [ - - - - : 0 : RCLCPP_INFO(this->get_logger(), e.what());
- - - - -
- - - - -
- - - - -
- - - - -
- - - - -
- ]
215 : : abort = true;
216 [ - - ]: 0 : } catch (...) {
217 [ - - - - : 0 : RCLCPP_INFO(
- - - - -
- - - - -
- - - - -
- - - - -
- - - - -
- ]
218 : : this->get_logger(),
219 : : "RayGroundClassifierCloudNode has encountered an unknown failure");
220 : : abort = true;
221 : : has_encountered_unknown_exception = true;
222 : : }
223 : : }
224 : : }
225 : :
226 [ - + ]: 2 : if (has_encountered_unknown_exception) {
227 : 0 : m_has_failed = true;
228 [ - - - - : 95 : RCLCPP_INFO(
- - - - -
- - - - -
- - - - -
- - - - -
- - - - -
- + - - ]
229 : : this->get_logger(),
230 : : "RayGroundClassifierCloudNode has encountered an unknown failure");
231 : 0 : throw;
232 : : }
233 : :
234 [ - + ]: 2 : if (abort) {
235 : 0 : return;
236 : : }
237 : :
238 : : // publish: nonground first for the possible microseconds of latency
239 [ + - ]: 2 : m_nonground_pub_ptr->publish(m_nonground_msg);
240 [ + - ]: 2 : m_ground_pub_ptr->publish(m_ground_msg);
241 : 190 : } catch (const std::runtime_error & e) {
242 : 95 : m_has_failed = true;
243 [ - + - - : 475 : RCLCPP_INFO(this->get_logger(), e.what());
- - - - -
- - - - -
- - - - +
- + - + -
+ - + - +
- ]
244 : 0 : } catch (const std::exception & e) {
245 : 0 : m_has_failed = true;
246 [ - - - - : 0 : RCLCPP_INFO(this->get_logger(), e.what());
- - - - -
- - - - -
- - - - -
- - - - -
- - - - -
- ]
247 : 0 : } catch (...) {
248 [ - - - - : 0 : RCLCPP_INFO(
- - - - -
- - - - -
- - - - -
- - - - -
- - - - -
- ]
249 : : this->get_logger(),
250 : : "RayGroundClassifierCloudNode has encountered an unknown failure");
251 : 0 : throw;
252 : : }
253 : : }
254 : : ////////////////////////////////////////////////////////////////////////////////
255 : 97 : void RayGroundClassifierCloudNode::reset()
256 : : {
257 : : // reset aggregator: Needed in case an error is thrown during partitioning of cloud
258 : : // which would lead to filled rays and overflow during next callback
259 : 97 : m_aggregator.reset();
260 : : // reset messages
261 : 97 : CloudModifier modifier1{m_ground_msg};
262 : : modifier1.clear();
263 : :
264 : 97 : CloudModifier modifier2{m_nonground_msg};
265 : : modifier2.clear();
266 : 97 : }
267 : : } // namespace ray_ground_classifier_nodes
268 : : } // namespace filters
269 : : } // namespace perception
270 : : } // namespace autoware
271 : :
272 : : RCLCPP_COMPONENTS_REGISTER_NODE(
273 : : autoware::perception::filters::ray_ground_classifier_nodes::RayGroundClassifierCloudNode)
|
