Branch data Line data Source code
1 : : // Copyright 2019 the Autoware Foundation
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 : :
18 : : //lint -e537 NOLINT cpplint wants this due to std::make_unique
19 : : #include <rclcpp/node_options.hpp>
20 : : #include <voxel_grid_nodes/voxel_cloud_node.hpp>
21 : : #include <voxel_grid_nodes/algorithm/voxel_cloud_approximate.hpp>
22 : : #include <voxel_grid_nodes/algorithm/voxel_cloud_centroid.hpp>
23 : : #include <common/types.hpp>
24 : : #include <rclcpp_components/register_node_macro.hpp>
25 : :
26 : : #include <memory>
27 : : #include <string>
28 : : #include <algorithm>
29 : :
30 : : using autoware::common::types::bool8_t;
31 : : using autoware::common::types::uchar8_t;
32 : : using autoware::common::types::float32_t;
33 : :
34 : : namespace autoware
35 : : {
36 : : namespace perception
37 : : {
38 : : namespace filters
39 : : {
40 : : namespace voxel_grid_nodes
41 : : {
42 : : ////////////////////////////////////////////////////////////////////////////////
43 : 16 : VoxelCloudNode::VoxelCloudNode(
44 : 16 : const rclcpp::NodeOptions & node_options)
45 : : : Node("voxel_grid_cloud_node", node_options),
46 : : m_sub_ptr{create_subscription<Message>(
47 : : "points_in",
48 [ + - ]: 16 : rclcpp::QoS(
49 [ + - + - ]: 32 : static_cast<size_t>(static_cast<int>(declare_parameter("subscription.qos.history_depth",
50 [ + - ]: 32 : 10)))
51 : : ).durability(
52 : : parse_durability_parameter(
53 [ + - + - : 48 : declare_parameter("subscription.qos.durability", "volatile")
+ - + - +
- - - -
- ]
54 : : )
55 [ + - + - ]: 16 : )
56 : : ,
57 : 0 : std::bind(&VoxelCloudNode::callback, this, std::placeholders::_1)
58 : : )},
59 : : m_pub_ptr{create_publisher<Message>(
60 : : "points_downsampled",
61 [ + - ]: 16 : rclcpp::QoS(
62 [ + - + - : 32 : static_cast<size_t>(static_cast<int>(declare_parameter("publisher.qos.history_depth", 10)))
+ - ]
63 : : ).durability(
64 : : parse_durability_parameter(
65 [ + - + - : 48 : declare_parameter("publisher.qos.durability", "volatile")
+ - + - +
- - - -
- ]
66 : : )
67 [ + - + - ]: 16 : )
68 : : )},
69 [ + - + - : 164 : m_has_failed{false}
+ - + - +
- - + + -
+ - + - +
- - + + -
- - + - ]
70 : : {
71 : : // Build config manually (messages only have default constructors)
72 : : voxel_grid::PointXYZ min_point;
73 [ + - + - : 48 : min_point.x = static_cast<float32_t>(declare_parameter("config.min_point.x").get<float32_t>());
+ - + - +
- ]
74 [ + - + - : 42 : min_point.y = static_cast<float32_t>(declare_parameter("config.min_point.y").get<float32_t>());
+ - + - +
- ]
75 [ + - + - : 39 : min_point.z = static_cast<float32_t>(declare_parameter("config.min_point.z").get<float32_t>());
+ - + - +
- + - ]
76 : : voxel_grid::PointXYZ max_point;
77 [ + - + - : 36 : max_point.x = static_cast<float32_t>(declare_parameter("config.max_point.x").get<float32_t>());
+ - + - +
- ]
78 [ + - + - : 33 : max_point.y = static_cast<float32_t>(declare_parameter("config.max_point.y").get<float32_t>());
+ - + - +
- ]
79 [ + - + - : 30 : max_point.z = static_cast<float32_t>(declare_parameter("config.max_point.z").get<float32_t>());
+ - + - +
- + - ]
80 : : voxel_grid::PointXYZ voxel_size;
81 [ + - + - : 27 : voxel_size.x = static_cast<float32_t>(declare_parameter("config.voxel_size.x").get<float32_t>());
+ - + - +
- ]
82 [ + - + - : 24 : voxel_size.y = static_cast<float32_t>(declare_parameter("config.voxel_size.y").get<float32_t>());
+ - + - +
- ]
83 [ + - + - : 21 : voxel_size.z = static_cast<float32_t>(declare_parameter("config.voxel_size.z").get<float32_t>());
+ - + - +
- ]
84 : : const std::size_t capacity =
85 [ + - + - : 12 : static_cast<std::size_t>(declare_parameter("config.capacity").get<std::size_t>());
+ - + - -
+ ]
86 [ + - ]: 6 : const voxel_grid::Config cfg{min_point, max_point, voxel_size, capacity};
87 : : // Init
88 [ + - + - : 28 : init(cfg, declare_parameter("is_approximate").get<bool8_t>());
+ - + - +
- - + ]
89 : 6 : }
90 : :
91 : : ////////////////////////////////////////////////////////////////////////////////
92 [ - + ]: 564 : void VoxelCloudNode::callback(const sensor_msgs::msg::PointCloud2::SharedPtr msg)
93 : : {
94 : : try {
95 [ - + ]: 564 : m_voxelgrid_ptr->insert(*msg);
96 [ # # # # ]: 0 : m_pub_ptr->publish(m_voxelgrid_ptr->get());
97 : 1128 : } catch (const std::exception & e) {
98 [ + - + - ]: 564 : std::string err_msg{get_name()};
99 [ + - + - : 1128 : err_msg += ": " + std::string(e.what());
- + - - ]
100 [ - + - - : 2820 : RCLCPP_ERROR(this->get_logger(), err_msg.c_str());
- - - - -
- - - - -
- - - - +
- + - + -
+ - + - +
- - - ]
101 [ + - ]: 564 : m_has_failed = true;
102 : 0 : } catch (...) {
103 [ - - ]: 0 : std::string err_msg{"Unknown error occurred in "};
104 [ - - ]: 0 : err_msg += get_name();
105 [ - - - - : 0 : RCLCPP_ERROR(this->get_logger(), err_msg.c_str());
- - - - -
- - - - -
- - - - -
- - - - -
- - - - -
- - - ]
106 : 0 : throw;
107 : : }
108 : 564 : }
109 : : ////////////////////////////////////////////////////////////////////////////////
110 : 6 : void VoxelCloudNode::init(const voxel_grid::Config & cfg, const bool8_t is_approximate)
111 : : {
112 : : // construct voxel grid
113 [ - + ]: 6 : if (is_approximate) {
114 : 0 : m_voxelgrid_ptr = std::make_unique<algorithm::VoxelCloudApproximate>(cfg);
115 : : } else {
116 : 12 : m_voxelgrid_ptr = std::make_unique<algorithm::VoxelCloudCentroid>(cfg);
117 : : }
118 : 6 : }
119 : :
120 : : /////////////////////////////////////////////////////////////////////////////
121 : 32 : rmw_qos_durability_policy_t parse_durability_parameter(
122 : : const std::string & durability)
123 : : {
124 [ + + ]: 32 : if (durability == "transient_local") {
125 : : return RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL;
126 [ - + ]: 10 : } else if (durability == "volatile") {
127 : : return RMW_QOS_POLICY_DURABILITY_VOLATILE;
128 : : }
129 : :
130 : : throw std::runtime_error(
131 [ # # # # : 0 : "Durability setting '" + durability + "' is not supported."
# # # # ]
132 [ # # ]: 0 : "Please try 'volatile' or 'transient_local'.");
133 : : }
134 : : } // namespace voxel_grid_nodes
135 : : } // namespace filters
136 : : } // namespace perception
137 : : } // namespace autoware
138 : :
139 : : RCLCPP_COMPONENTS_REGISTER_NODE(
140 : : autoware::perception::filters::voxel_grid_nodes::VoxelCloudNode)
|
