Branch data Line data Source code
1 : : // Copyright 2020 Apex.AI, Inc.
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 : :
17 : : /// \copyright Copyright 2020 Apex.AI, Inc.
18 : : /// All rights reserved.
19 : :
20 : : #ifndef NDT_MAPPING_NODES__NDT_MAPPING_NODES_HPP_
21 : : #define NDT_MAPPING_NODES__NDT_MAPPING_NODES_HPP_
22 : :
23 : : #include <ndt_mapping_nodes/visibility_control.hpp>
24 : : #include <sensor_msgs/msg/point_cloud2.hpp>
25 : : #include <point_cloud_mapping/point_cloud_map.hpp>
26 : : #include <point_cloud_mapping/policies.hpp>
27 : : #include <ndt/ndt_localizer.hpp>
28 : : #include <optimization/newtons_method_optimizer.hpp>
29 : : #include <optimization/line_search/more_thuente_line_search.hpp>
30 : : #include <helper_functions/float_comparisons.hpp>
31 : : #include <tf2_msgs/msg/tf_message.hpp>
32 : : #include <lidar_utils/point_cloud_utils.hpp>
33 : :
34 : : #include <string>
35 : : #include <limits>
36 : : #include <memory>
37 : : #include <utility>
38 : :
39 : : namespace autoware
40 : : {
41 : : namespace mapping
42 : : {
43 : : namespace ndt_mapping_nodes
44 : : {
45 : : using Optimizer = common::optimization::NewtonsMethodOptimizer<
46 : : common::optimization::MoreThuenteLineSearch>;
47 : : using NDTMap = localization::ndt::DynamicNDTMap;
48 : : using VoxelMap = point_cloud_mapping::DualVoxelMap<NDTMap>;
49 : : using Localizer = localization::ndt::P2DNDTLocalizer<Optimizer, NDTMap>;
50 : : using P2DNDTConfig = localization::ndt::P2DNDTLocalizerConfig;
51 : : using WritePolicy = mapping::point_cloud_mapping::CapacityTrigger;
52 : : using ClearPolicy = mapping::point_cloud_mapping::CapacityTrigger;
53 : : using PrefixPolicy = mapping::point_cloud_mapping::TimeStampPrefixGenerator;
54 : :
55 : : /// \brief Mapper node implementation that localizes using a `P2DNDTLocalizer` and accumulates
56 : : /// the registered scans into a `DualVoxelMap`.
57 : : /// \tparam WriteTriggerPolicyT Policy specifying when to write the map into a file
58 : : /// \tparam ClearTriggerPolicyT Policy specifying when to clear the map.
59 : : /// \tparam PrefixGeneratorT Functor that generates the full filename prefix given a base prefix.
60 : : template<class WriteTriggerPolicyT = WritePolicy,
61 : : class ClearTriggerPolicyT = ClearPolicy,
62 : : class PrefixGeneratorT = PrefixPolicy>
63 : : class NDT_MAPPING_NODES_PUBLIC P2DNDTVoxelMapperNode : public rclcpp::Node
64 : : {
65 : : public:
66 : : using PoseWithCovarianceStamped = geometry_msgs::msg::PoseWithCovarianceStamped;
67 : : using Cloud = sensor_msgs::msg::PointCloud2;
68 : : using RegistrationSummary = localization::localization_common::OptimizedRegistrationSummary;
69 : : using PointXYZI = autoware::common::types::PointXYZI;
70 : : // Static asserts to make sure the policies are valid
71 : : static_assert(
72 : : std::is_base_of<mapping::point_cloud_mapping::TriggerPolicyBase<WriteTriggerPolicyT>,
73 : : WriteTriggerPolicyT>::value,
74 : : "Write trigger policy must implement the `TriggerPolicyBase` interface.");
75 : :
76 : : static_assert(
77 : : std::is_base_of<mapping::point_cloud_mapping::TriggerPolicyBase<ClearTriggerPolicyT>,
78 : : ClearTriggerPolicyT>::value,
79 : : "Clear trigger policy must implement the `TriggerPolicyBase` interface.");
80 : :
81 : : static_assert(
82 : : std::is_base_of<mapping::point_cloud_mapping::PrefixGeneratorBase<PrefixGeneratorT>,
83 : : PrefixGeneratorT>::value,
84 : : "Prefix generator policy must implement the `PrefixGeneratorBase` interface.");
85 : :
86 : :
87 : 1 : explicit P2DNDTVoxelMapperNode(const rclcpp::NodeOptions & options)
88 : : : Node{"ndt_mapper_node", options},
89 : : m_observation_sub{create_subscription<Cloud>(
90 : : "points_in",
91 : : rclcpp::QoS{rclcpp::KeepLast{
92 [ + - + - : 2 : static_cast<size_t>(declare_parameter("observation_sub.history_depth").template
+ - ]
93 : : get<size_t>())}},
94 [ # # ]: 0 : [this](typename Cloud::ConstSharedPtr msg) {observation_callback(msg);})},
95 : : m_pose_publisher(
96 : : create_publisher<PoseWithCovarianceStamped>(
97 : : "ndt_pose",
98 : : rclcpp::QoS{rclcpp::KeepLast{
99 [ + - + - : 2 : static_cast<size_t>(declare_parameter(
+ - + - ]
100 : : "pose_pub.history_depth").template get<size_t>())}})),
101 [ + - + - : 11 : m_base_fn_prefix{this->declare_parameter("file_name_prefix").template get<std::string>()}
+ - + - +
- + - + -
+ - + - +
- + - + -
+ - + - +
- + - + -
+ - + - +
- + - + -
- - - - -
- - - -
- ]
102 : : {
103 [ + - ]: 1 : init();
104 : 1 : }
105 : :
106 : 1 : ~P2DNDTVoxelMapperNode()
107 : : {
108 [ - + ]: 1 : if (m_map_ptr->size() > 0U) {
109 : 0 : const auto & file_name_prefix = m_prefix_generator.get(m_base_fn_prefix);
110 [ # # # # : 0 : RCLCPP_DEBUG(get_logger(), "The map is written to" + file_name_prefix + ".pcd");
# # # # #
# ]
111 : 0 : m_map_ptr->write(file_name_prefix);
112 : : }
113 [ - + ]: 3 : }
114 : :
115 : : private:
116 : 1 : void init()
117 : : {
118 : 18 : auto get_point_param = [this](const std::string & config_name_prefix) {
119 : : perception::filters::voxel_grid::PointXYZ point;
120 [ + - + - : 16 : point.x = static_cast<float32_t>(declare_parameter(config_name_prefix + ".x").
+ - + + ]
121 : : template get<float32_t>());
122 [ + - + - : 16 : point.y = static_cast<float32_t>(this->declare_parameter(config_name_prefix + ".y").
+ - + + ]
123 : : template get<float32_t>());
124 [ + - + - : 16 : point.z = static_cast<float32_t>(this->declare_parameter(config_name_prefix + ".z").
+ - + + ]
125 : : template get<float32_t>());
126 : 6 : return point;
127 : : };
128 : :
129 : :
130 : 3 : const auto parse_grid_config = [this, get_point_param](const std::string & prefix) {
131 : : // Fetch map configuration
132 [ + + ]: 2 : const auto capacity = static_cast<std::size_t>(
133 [ + - + - : 4 : this->declare_parameter(prefix + ".capacity").template get<std::size_t>());
+ - ]
134 : :
135 [ + - ]: 2 : return perception::filters::voxel_grid::Config{get_point_param(
136 [ + - + - : 4 : prefix + ".min_point"), get_point_param(prefix + ".max_point"),
+ + - - ]
137 [ + - + - : 5 : get_point_param(prefix + ".voxel_size"), capacity};
+ - + + +
+ - - -
- ]
138 : : };
139 : :
140 : : // Fetch localizer configuration
141 [ + - ]: 1 : P2DNDTConfig localizer_config{
142 [ + - + - : 2 : static_cast<uint32_t>(this->declare_parameter("localizer.scan.capacity").
+ - ]
143 : : template get<uint32_t>()),
144 : : std::chrono::milliseconds(
145 : : static_cast<uint64_t>(
146 [ + - + - : 2 : this->declare_parameter("localizer.guess_time_tolerance_ms").template get<uint64_t>()))
+ - + - +
- - - ]
147 : : };
148 : :
149 [ + - + - : 3 : const auto outlier_ratio{this->declare_parameter(
+ - + - ]
150 : : "localizer.optimization.outlier_ratio").template get<float64_t>()};
151 : :
152 [ + - ]: 1 : const common::optimization::OptimizationOptions optimization_options{
153 : : static_cast<uint64_t>(
154 [ + - + - : 2 : this->declare_parameter("localizer.optimizer.max_iterations").template get<uint64_t>()),
+ - ]
155 [ + - + - : 2 : this->declare_parameter("localizer.optimizer.score_tolerance").template get<float64_t>(),
+ - + - +
- - - ]
156 [ + - + - : 2 : this->declare_parameter(
+ - + - +
- - - ]
157 : : "localizer.optimizer.parameter_tolerance").template get<float64_t>(),
158 [ + - + - : 2 : this->declare_parameter("localizer.optimizer.gradient_tolerance").template get<float64_t>()
+ - + - +
- - - ]
159 : : };
160 : :
161 [ + - + - : 2 : m_localizer_ptr = std::make_unique<Localizer>(
+ - - - ]
162 : : localizer_config,
163 : : Optimizer{
164 : : common::optimization::MoreThuenteLineSearch{
165 [ + - + - : 2 : static_cast<float32_t>(this->declare_parameter(
+ - ]
166 : : "localizer.optimizer.line_search.step_max")
167 : : .template get<float32_t>()),
168 [ + - + - : 2 : static_cast<float32_t>(this->declare_parameter(
+ - + - +
- - - ]
169 : : "localizer.optimizer.line_search.step_min")
170 : : .template get<float32_t>()),
171 : : common::optimization::MoreThuenteLineSearch::OptimizationDirection::kMaximization
172 : : },
173 : : optimization_options},
174 : : outlier_ratio);
175 [ + - + - : 2 : const auto & map_frame_id = this->declare_parameter("map.frame_id").template get<std::string>();
+ - ]
176 [ + - + - ]: 2 : m_map_ptr = std::make_unique<VoxelMap>(
177 [ + - + - : 2 : parse_grid_config("map"), map_frame_id,
- + - - ]
178 [ + - + - : 2 : NDTMap{parse_grid_config("localizer.map")});
- + - - ]
179 : :
180 [ + - + - : 3 : if (this->declare_parameter("publish_map_increment").template get<bool8_t>()) {
+ - + - +
- ]
181 [ + - + - : 4 : m_increment_publisher = this->template create_publisher<sensor_msgs::msg::PointCloud2>(
+ - + - +
- + - -
- ]
182 : : "points_registered",
183 : : rclcpp::QoS{rclcpp::KeepLast{
184 [ + - + - : 2 : static_cast<size_t>(this->declare_parameter("map_increment_pub.history_depth").template
+ - + - ]
185 : : get<size_t>())}});
186 : : }
187 : :
188 [ + - + - : 2 : if (declare_parameter("publish_tf").template get<bool>()) {
+ - - + +
- ]
189 [ + - + - : 2 : m_tf_publisher = create_publisher<tf2_msgs::msg::TFMessage>(
+ - + - +
- - + ]
190 : : "/tf",
191 : : rclcpp::QoS{rclcpp::KeepLast{m_pose_publisher->get_queue_size()}});
192 : : }
193 : :
194 : : m_previous_transform.transform.rotation.set__w(1.0);
195 : 1 : m_previous_transform.header.frame_id = m_map_ptr->frame_id();
196 : : using autoware::common::lidar_utils::CloudModifier;
197 : 1 : CloudModifier msg_initializer{m_cached_increment,
198 : : map_frame_id};
199 : 1 : }
200 : :
201 : 0 : void observation_callback(Cloud::ConstSharedPtr msg_ptr)
202 : : {
203 : : try {
204 [ # # ]: 0 : RegistrationSummary summary{};
205 [ # # ]: 0 : m_previous_transform.header.stamp = msg_ptr->header.stamp;
206 : :
207 : : geometry_msgs::msg::PoseWithCovarianceStamped pose_out;
208 [ # # ]: 0 : if (m_map_ptr->empty()) {
209 : : // If the map is empty, get the initial pose for inserting the increment into the map.
210 : : // If the map is empty in further iterations, then throw as we don't know where to place
211 : : // the scan anymore.
212 [ # # ]: 0 : pose_out = get_initial_pose_once();
213 : : } else {
214 : : // Register the measurement only if there is a valid map.
215 [ # # ]: 0 : pose_out = m_localizer_ptr->register_measurement(
216 [ # # ]: 0 : *msg_ptr, m_previous_transform, m_map_ptr->localizer_map(), &summary);
217 : : }
218 : :
219 [ # # # # ]: 0 : if (!validate_output(summary)) {
220 [ # # # # : 0 : RCLCPP_WARN(get_logger(), "Invalid pose estimate. The result is ignored.");
# # # # #
# # # # #
# # # # #
# # # # #
# # # # #
# ]
221 : : return;
222 : : }
223 : : // Transform the measurement into the map frame and insert it into the map.
224 [ # # ]: 0 : const auto & increment = get_map_increment(*msg_ptr, pose_out);
225 [ # # ]: 0 : m_pose_publisher->publish(pose_out);
226 : :
227 [ # # ]: 0 : if (m_increment_publisher) {
228 [ # # ]: 0 : m_increment_publisher->publish(increment);
229 : : }
230 [ # # ]: 0 : if (m_tf_publisher) {
231 [ # # # # ]: 0 : publish_tf(pose_to_transform(pose_out, msg_ptr->header.frame_id));
232 : : }
233 [ # # ]: 0 : if (m_write_trigger.ready(*m_map_ptr)) {
234 : 0 : const auto & file_name_prefix = m_prefix_generator.get(m_base_fn_prefix);
235 [ # # ]: 0 : m_map_ptr->write(file_name_prefix);
236 [ # # # # : 0 : RCLCPP_DEBUG(get_logger(), "The map is written to" + file_name_prefix + ".pcd");
# # # # #
# # # # #
# # # # #
# # # # #
# # # # #
# # # # #
# # # # #
# # # #
# ]
237 : : }
238 [ # # ]: 0 : if (m_clear_trigger.ready(*m_map_ptr)) {
239 [ # # # # : 0 : RCLCPP_DEBUG(get_logger(), "The map is cleared.");
# # # # #
# # # # #
# # # # #
# # # # #
# # # # #
# # # ]
240 : : m_map_ptr->clear();
241 : : }
242 : :
243 : : // Update the map after a possible clearance and not before so that the map is never fully
244 : : // empty.
245 [ # # ]: 0 : m_map_ptr->update(increment);
246 [ # # # # ]: 0 : m_previous_transform = pose_to_transform(pose_out, msg_ptr->header.frame_id);
247 : 0 : } catch (const std::runtime_error & e) {
248 [ - - - - : 0 : RCLCPP_ERROR(get_logger(), "Failed to register the measurement: ", e.what());
- - - - -
- - - - -
- - - - -
- - - - -
- - - - -
- ]
249 : : }
250 : : }
251 : :
252 : 0 : bool8_t validate_output(
253 : : const RegistrationSummary & summary)
254 : : {
255 [ # # # ]: 0 : switch (summary.optimization_summary().termination_type()) {
256 : : case common::optimization::TerminationType::FAILURE:
257 : : // Numerical failure, result is unusable.
258 : : return false;
259 : 0 : case common::optimization::TerminationType::NO_CONVERGENCE:
260 : : // In practice, it's hard to come up with a perfect termination criterion for ndt
261 : : // optimization and even non-convergence may be a decent effort in localizing the
262 : : // vehicle. Hence the result is not discarded on non-convergence.
263 [ # # # # : 0 : RCLCPP_DEBUG(this->get_logger(), "NDT mapper optimizer failed to converge.");
# # # # #
# ]
264 : : return true;
265 : 0 : default:
266 : 0 : return true;
267 : : }
268 : : }
269 : :
270 : : /// Transform the observed point cloud into the map frame using the registered
271 : : /// pose.
272 : : /// \param observation Point cloud observation.
273 : : /// \param registered_pose Registered pose of the observation.
274 : : /// \return Pointer to the transformed point cloud;
275 : 0 : const Cloud & get_map_increment(
276 : : const Cloud & observation,
277 : : const PoseWithCovarianceStamped & registered_pose)
278 : : {
279 : : using autoware::common::lidar_utils::CloudView;
280 : 0 : CloudView obs_view{observation};
281 : 0 : reset_cached_msg(obs_view.size());
282 : : // Convert pose to transform for `doTransform()`
283 : 0 : geometry_msgs::msg::TransformStamped tf;
284 : 0 : tf.header.stamp = registered_pose.header.stamp;
285 [ # # ]: 0 : tf.header.frame_id = registered_pose.header.frame_id;
286 [ # # ]: 0 : tf.child_frame_id = observation.header.frame_id;
287 : : const auto & trans = registered_pose.pose.pose.position;
288 : : const auto & rot = registered_pose.pose.pose.orientation;
289 [ # # ]: 0 : tf.transform.translation.set__x(trans.x).set__y(trans.y).set__z(trans.z);
290 [ # # ]: 0 : tf.transform.rotation.set__x(rot.x).set__y(rot.y).set__z(rot.z).set__w(rot.w);
291 : :
292 [ # # ]: 0 : tf2::doTransform(observation, m_cached_increment, tf);
293 : 0 : return m_cached_increment;
294 : : }
295 : :
296 : : /// Publish the given transform
297 [ # # ]: 0 : void publish_tf(const geometry_msgs::msg::TransformStamped & transform)
298 : : {
299 : : tf2_msgs::msg::TFMessage tf_message;
300 [ # # ]: 0 : tf_message.transforms.emplace_back(transform);
301 [ # # ]: 0 : m_tf_publisher->publish(tf_message);
302 : 0 : }
303 : :
304 : : /// Clear the cached pc2 message used for storing the transformed point clouds
305 : 0 : void reset_cached_msg(std::size_t size)
306 : : {
307 : : using autoware::common::lidar_utils::CloudModifier;
308 : 0 : CloudModifier inc_modifier{m_cached_increment};
309 : : inc_modifier.clear();
310 : 0 : inc_modifier.resize(size);
311 : 0 : }
312 : :
313 : : /// Convert a `PoseWithCovarianceStamped` into a `TransformStamped`
314 : : geometry_msgs::msg::TransformStamped pose_to_transform(
315 : : const PoseWithCovarianceStamped & pose_msg,
316 : : const std::string & child_frame_id = "base_link")
317 : : {
318 : : geometry_msgs::msg::TransformStamped tf_stamped{};
319 : : tf_stamped.header = pose_msg.header;
320 : : tf_stamped.child_frame_id = child_frame_id;
321 : : const auto & tf_trans = pose_msg.pose.pose.position;
322 : : const auto & tf_rot = pose_msg.pose.pose.orientation;
323 : : tf_stamped.transform.translation.set__x(tf_trans.x).set__y(tf_trans.y).
324 : : set__z(tf_trans.z);
325 : : tf_stamped.transform.rotation.set__x(tf_rot.x).set__y(tf_rot.y).set__z(tf_rot.z).
326 : : set__w(tf_rot.w);
327 : : return tf_stamped;
328 : : }
329 : :
330 : : /// Convert a `TransformStamped` into a `PoseWithCovarianceStamped`
331 : 0 : PoseWithCovarianceStamped transform_to_pose(
332 : : const geometry_msgs::msg::TransformStamped & transform_msg)
333 : : {
334 : : PoseWithCovarianceStamped pose;
335 : : pose.header = transform_msg.header;
336 : : const auto & pose_trans = transform_msg.transform.translation;
337 : : const auto & pose_rot = transform_msg.transform.rotation;
338 : 0 : pose.pose.pose.position.set__x(pose_trans.x).set__y(pose_trans.y).
339 : 0 : set__z(pose_trans.z);
340 : 0 : pose.pose.pose.orientation.set__x(pose_rot.x).set__y(pose_rot.y).set__z(pose_rot.z).
341 : 0 : set__w(pose_rot.w);
342 : 0 : return pose;
343 : : }
344 : :
345 : : /// Get the initial pose (identity). If the map is already initialized once, throw.
346 : 0 : geometry_msgs::msg::PoseWithCovarianceStamped get_initial_pose_once()
347 : : {
348 [ # # ]: 0 : if (!m_map_initialized) {
349 : 0 : m_map_initialized = true;
350 : 0 : return transform_to_pose(m_previous_transform);
351 : : } else {
352 : : throw std::runtime_error(
353 : : "Current map is initialized yet empty, the scan cannot be "
354 [ # # ]: 0 : "registered.");
355 : : }
356 : : }
357 : :
358 : : std::unique_ptr<Localizer> m_localizer_ptr;
359 : : std::unique_ptr<VoxelMap> m_map_ptr;
360 : : typename rclcpp::Subscription<Cloud>::SharedPtr m_observation_sub;
361 : : typename rclcpp::Publisher<PoseWithCovarianceStamped>::SharedPtr m_pose_publisher;
362 : : typename rclcpp::Publisher<tf2_msgs::msg::TFMessage>::SharedPtr m_tf_publisher{nullptr};
363 : : rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr m_increment_publisher{nullptr};
364 : : geometry_msgs::msg::TransformStamped m_previous_transform;
365 : : Cloud m_cached_increment;
366 : : WriteTriggerPolicyT m_write_trigger{};
367 : : ClearTriggerPolicyT m_clear_trigger{};
368 : : PrefixGeneratorT m_prefix_generator{};
369 : : bool8_t m_map_initialized{false};
370 : : std::string m_base_fn_prefix;
371 : : };
372 : :
373 : : } // namespace ndt_mapping_nodes
374 : : } // namespace mapping
375 : : } // namespace autoware
376 : :
377 : : #endif // NDT_MAPPING_NODES__NDT_MAPPING_NODES_HPP_
|
