Branch data Line data Source code
1 : : // Copyright 2019-2020 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 : : #include <common/types.hpp>
18 : : #include <ndt_nodes/map_publisher.hpp>
19 : : #include <rclcpp_components/register_node_macro.hpp>
20 : : #include <point_cloud_msg_wrapper/point_cloud_msg_wrapper.hpp>
21 : : #include <tf2_geometry_msgs/tf2_geometry_msgs.h>
22 : : #include <tf2/LinearMath/Quaternion.h>
23 : : #include <lidar_utils/point_cloud_utils.hpp>
24 : :
25 : : #include <memory>
26 : : #include <string>
27 : :
28 : : using autoware::common::types::bool8_t;
29 : : using autoware::common::types::float32_t;
30 : : using autoware::common::types::float64_t;
31 : :
32 : : namespace autoware
33 : : {
34 : : namespace localization
35 : : {
36 : : namespace ndt_nodes
37 : : {
38 : : /// Clear the given pointcloud message
39 : 3 : void reset_pc_msg(sensor_msgs::msg::PointCloud2 & msg)
40 : : {
41 : : using autoware::common::lidar_utils::CloudModifier;
42 : 3 : CloudModifier{msg}.clear();
43 : 3 : }
44 : :
45 : 4 : NDTMapPublisherNode::NDTMapPublisherNode(
46 : : const rclcpp::NodeOptions & node_options
47 : 4 : )
48 : : : Node("ndt_map_publisher_node", node_options),
49 [ + - + - : 8 : m_pcl_file_name(declare_parameter("map_pcd_file").get<std::string>()),
+ - ]
50 [ + - + - : 8 : m_yaml_file_name(declare_parameter("map_yaml_file").get<std::string>()),
+ - - - ]
51 [ + - + - : 16 : m_viz_map(declare_parameter("viz_map", false))
+ - + - +
- + - + -
+ - + - +
- - + + -
- - ]
52 : : {
53 : : using PointXYZ = perception::filters::voxel_grid::PointXYZ;
54 : : PointXYZ min_point;
55 : 4 : min_point.x =
56 [ + - + - : 12 : static_cast<float32_t>(declare_parameter("map_config.min_point.x").get<float32_t>());
+ - + - +
- ]
57 : 4 : min_point.y =
58 [ + - + - : 12 : static_cast<float32_t>(declare_parameter("map_config.min_point.y").get<float32_t>());
+ - + - +
- ]
59 : 4 : min_point.z =
60 [ + - + - : 12 : static_cast<float32_t>(declare_parameter("map_config.min_point.z").get<float32_t>());
+ - + - +
- + - ]
61 : : PointXYZ max_point;
62 : 4 : max_point.x =
63 [ + - + - : 12 : static_cast<float32_t>(declare_parameter("map_config.max_point.x").get<float32_t>());
+ - + - +
- ]
64 : 4 : max_point.y =
65 [ + - + - : 12 : static_cast<float32_t>(declare_parameter("map_config.max_point.y").get<float32_t>());
+ - + - +
- ]
66 : 4 : max_point.z =
67 [ + - + - : 12 : static_cast<float32_t>(declare_parameter("map_config.max_point.z").get<float32_t>());
+ - + - +
- + - ]
68 : : PointXYZ voxel_size;
69 : 4 : voxel_size.x =
70 [ + - + - : 12 : static_cast<float32_t>(declare_parameter("map_config.voxel_size.x").get<float32_t>());
+ - + - +
- ]
71 : 4 : voxel_size.y =
72 [ + - + - : 12 : static_cast<float32_t>(declare_parameter("map_config.voxel_size.y").get<float32_t>());
+ - + - +
- ]
73 : 4 : voxel_size.z =
74 [ + - + - : 12 : static_cast<float32_t>(declare_parameter("map_config.voxel_size.z").get<float32_t>());
+ - + - +
- ]
75 : : const std::size_t capacity =
76 [ + - + - : 12 : static_cast<std::size_t>(declare_parameter("map_config.capacity").get<std::size_t>());
+ - + - +
- ]
77 [ + - + - : 8 : const std::string map_frame = declare_parameter("map_frame").get<std::string>();
+ - + - -
- ]
78 [ + - ]: 4 : const std::string map_topic = "ndt_map";
79 [ + - ]: 4 : const std::string viz_map_topic = "viz_ndt_map";
80 : :
81 [ + - ]: 4 : m_map_config_ptr = std::make_unique<MapConfig>(min_point, max_point, voxel_size, capacity);
82 [ + - ]: 4 : init(map_frame, map_topic, viz_map_topic);
83 [ + - ]: 4 : run();
84 : 4 : }
85 : :
86 : 4 : void NDTMapPublisherNode::init(
87 : : const std::string & map_frame,
88 : : const std::string & map_topic,
89 : : const std::string & viz_map_topic)
90 : : {
91 : 4 : m_ndt_map_ptr = std::make_unique<ndt::DynamicNDTMap>(*m_map_config_ptr);
92 : :
93 : : using autoware::common::lidar_utils::CloudModifier;
94 : : CloudModifier initializer{
95 : 4 : m_source_pc, map_frame};
96 : :
97 [ + - + - ]: 8 : m_pub = create_publisher<sensor_msgs::msg::PointCloud2>(
98 : : map_topic,
99 [ + - + - : 8 : rclcpp::QoS(rclcpp::KeepLast(5U)).transient_local());
+ - ]
100 : :
101 [ + + ]: 4 : if (m_viz_map) { // create a publisher for map_visualization
102 : : using PointXYZ = perception::filters::voxel_grid::PointXYZ;
103 : :
104 [ + - ]: 6 : m_viz_pub = create_publisher<sensor_msgs::msg::PointCloud2>(
105 [ + - + - : 6 : viz_map_topic, rclcpp::QoS(rclcpp::KeepLast(5U)).transient_local());
+ - ]
106 : :
107 : : // TODO(jwhitleywork) Hard-coded voxel size and capacity.
108 : : // To be removed when #380 is in.
109 : : PointXYZ viz_voxel_size;
110 : 3 : viz_voxel_size.x = 0.4F;
111 : 3 : viz_voxel_size.y = 0.4F;
112 : 3 : viz_voxel_size.z = 0.4F;
113 : 3 : m_viz_map_config_ptr = std::make_unique<MapConfig>(
114 : 3 : m_map_config_ptr->get_min_point(),
115 : 3 : m_map_config_ptr->get_max_point(),
116 : : viz_voxel_size,
117 : : 10000000U);
118 : :
119 : : // Initialize Voxel Grid and output message for downsampling map
120 : : using autoware::common::lidar_utils::CloudModifier;
121 : : CloudModifier downsampled_pc_initializer{
122 : 3 : m_downsampled_pc, map_frame};
123 : 3 : m_voxelgrid_ptr = std::make_unique<VoxelGrid>(*m_viz_map_config_ptr);
124 : :
125 [ - + ]: 3 : if (m_downsampled_pc.width > 0U) {
126 : 0 : m_viz_pub->publish(m_downsampled_pc);
127 : : }
128 : : }
129 : :
130 [ + - + - : 8 : m_pub_earth_map = create_publisher<tf2_msgs::msg::TFMessage>(
- + ]
131 : : "/tf_static",
132 [ + - + - : 8 : rclcpp::QoS(rclcpp::KeepLast(5U)).transient_local());
+ - ]
133 : 4 : }
134 : :
135 : 4 : void NDTMapPublisherNode::run()
136 : : {
137 : 4 : ndt::geocentric_pose_t pose = ndt::load_map(m_yaml_file_name, m_pcl_file_name, m_source_pc);
138 : 4 : publish_earth_to_map_transform(pose);
139 : 4 : m_ndt_map_ptr->insert(m_source_pc);
140 : 4 : m_ndt_map_ptr->serialize_as<SerializedMap>(m_map_pc);
141 : :
142 [ + + ]: 4 : if (m_viz_map) {
143 : 3 : reset_pc_msg(m_downsampled_pc);
144 : 3 : downsample_pc();
145 [ + - ]: 3 : if (m_downsampled_pc.width > 0U) {
146 : 3 : m_viz_pub->publish(m_downsampled_pc);
147 : : }
148 : : }
149 : 4 : publish();
150 : 4 : }
151 : :
152 : 4 : void NDTMapPublisherNode::publish_earth_to_map_transform(ndt::geocentric_pose_t pose)
153 : : {
154 : 8 : geometry_msgs::msg::TransformStamped tf;
155 : :
156 : 4 : tf.transform.translation.x = pose.x;
157 : 4 : tf.transform.translation.y = pose.y;
158 : 4 : tf.transform.translation.z = pose.z;
159 : :
160 : : tf2::Quaternion q;
161 : 4 : q.setRPY(pose.roll, pose.pitch, pose.yaw);
162 [ + - + - : 4 : tf.header.stamp = rclcpp::Clock().now();
+ - + - ]
163 [ + - ]: 4 : tf.transform.rotation = tf2::toMsg(q);
164 : : tf.header.frame_id = "earth";
165 : : tf.child_frame_id = "map";
166 : :
167 : : tf2_msgs::msg::TFMessage static_tf_msg;
168 [ + - ]: 4 : static_tf_msg.transforms.push_back(tf);
169 : :
170 [ + - + - : 8 : m_transform_pub_timer = create_wall_timer(
- + + - -
- ]
171 : : std::chrono::seconds(1),
172 [ + - + - ]: 24 : [this, static_tf_msg]() {
173 : 12 : m_pub_earth_map->publish(static_tf_msg);
174 : : });
175 : :
176 [ + - ]: 4 : m_pub_earth_map->publish(static_tf_msg);
177 : 4 : }
178 : :
179 [ + - ]: 3 : void NDTMapPublisherNode::downsample_pc()
180 : : {
181 : : try {
182 [ + - ]: 3 : m_voxelgrid_ptr->insert(m_source_pc);
183 [ + - + - ]: 3 : m_downsampled_pc = m_voxelgrid_ptr->get();
184 : 0 : } catch (const std::exception & e) {
185 [ - - ]: 0 : std::string err_msg{e.what()};
186 [ - - - - : 0 : RCLCPP_ERROR(this->get_logger(), err_msg.c_str());
- - - - -
- - - - -
- - - - -
- - - - -
- - - - -
- - - ]
187 : 0 : } catch (...) {
188 [ - - ]: 0 : std::string err_msg{"Unknown error occurred in "};
189 [ - - ]: 0 : err_msg += get_name();
190 [ - - - - : 0 : RCLCPP_ERROR(this->get_logger(), err_msg.c_str());
- - - - -
- - - - -
- - - - -
- - - - -
- - - - -
- - - ]
191 : 0 : throw;
192 : : }
193 : 3 : }
194 : :
195 : 4 : void NDTMapPublisherNode::publish()
196 : : {
197 [ + - ]: 4 : if (m_map_pc.width > 0U) {
198 : 4 : m_pub->publish(m_map_pc);
199 : : }
200 : 4 : }
201 : :
202 : 0 : void NDTMapPublisherNode::reset()
203 : : {
204 : 0 : reset_pc_msg(m_map_pc);
205 : 0 : reset_pc_msg(m_source_pc);
206 : :
207 [ # # ]: 0 : if (m_viz_map) {
208 : 0 : reset_pc_msg(m_downsampled_pc);
209 : : }
210 : :
211 : 0 : m_ndt_map_ptr->clear();
212 : 0 : }
213 : :
214 : : } // namespace ndt_nodes
215 : : } // namespace localization
216 : : } // namespace autoware
217 : :
218 : : RCLCPP_COMPONENTS_REGISTER_NODE(autoware::localization::ndt_nodes::NDTMapPublisherNode)
|
