Autoware.Auto
map_publisher.hpp
Go to the documentation of this file.
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 #ifndef NDT_NODES__MAP_PUBLISHER_HPP_
18 #define NDT_NODES__MAP_PUBLISHER_HPP_
19 
22 #include <sensor_msgs/msg/point_cloud2.hpp>
23 #include <ndt/ndt_map.hpp>
24 #include <tf2_geometry_msgs/tf2_geometry_msgs.h>
25 #include <tf2_msgs/msg/tf_message.hpp>
26 #include <tf2_ros/static_transform_broadcaster.h>
28 #include <string>
29 #include <memory>
30 #include "common/types.hpp"
31 
34 
35 namespace autoware
36 {
37 namespace localization
38 {
39 namespace ndt_nodes
40 {
41 
44 class NDT_NODES_PUBLIC NDTMapPublisherNode : public rclcpp::Node
45 {
46 public:
52  explicit NDTMapPublisherNode(
53  const rclcpp::NodeOptions & node_options
54  );
55 
62  void run();
63 
64 private:
70  void init(
71  const std::string & map_frame,
72  const std::string & map_topic,
73  const std::string & viz_map_topic);
74 
75  void publish_earth_to_map_transform(ndt::geocentric_pose_t pose);
76 
79  void publish();
80 
82  void reset();
83 
85  void downsample_pc();
86 
87  rclcpp::Publisher<tf2_msgs::msg::TFMessage>::SharedPtr m_pub_earth_map;
88  rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr m_pub;
89  std::unique_ptr<ndt::DynamicNDTMap> m_ndt_map_ptr;
92  sensor_msgs::msg::PointCloud2 m_downsampled_pc;
93  const std::string m_pcl_file_name;
94  const std::string m_yaml_file_name;
95  const bool8_t m_viz_map;
96  rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr m_viz_pub;
97  std::unique_ptr<MapConfig> m_map_config_ptr;
98  std::unique_ptr<MapConfig> m_viz_map_config_ptr;
99  std::unique_ptr<VoxelGrid> m_voxelgrid_ptr;
100  // Workaround. TODO(yunus.caliskan): Remove in #380
101  rclcpp::TimerBase::SharedPtr m_visualization_timer{nullptr};
102  rclcpp::TimerBase::SharedPtr m_transform_pub_timer{nullptr};
103 };
104 
105 } // namespace ndt_nodes
106 } // namespace localization
107 } // namespace autoware
108 
109 #endif // NDT_NODES__MAP_PUBLISHER_HPP_
autoware::localization::ndt_nodes::NDTMapPublisherNode
Definition: map_publisher.hpp:44
ndt_map_publisher.hpp
types.hpp
This file includes common type definition.
voxel_cloud_centroid.hpp
This file defines an instance of the VoxelCloudBase interface.
autoware::perception::filters::voxel_grid_nodes::algorithm::VoxelCloudCentroid
An instantiation of VoxelCloudBase for CentroidVoxels.
Definition: voxel_cloud_centroid.hpp:35
autoware::common::types::bool8_t
bool bool8_t
Definition: types.hpp:39
autoware
This file defines the lanelet2_map_provider_node class.
Definition: quick_sort.hpp:24
visibility_control.hpp
ndt_nodes
Definition: ndt_nodes.launch.py:1
autoware::perception::filters::voxel_grid::Config
A configuration class for the VoxelGrid data structure, also includes some helper functionality for c...
Definition: perception/filters/voxel_grid/include/voxel_grid/config.hpp:52
autoware::localization::ndt::geocentric_pose_t
Definition: ndt_map_publisher.hpp:52
autoware::localization::ndt::StaticNDTMap
Definition: ndt_map.hpp:131
ndt_map.hpp
autoware::common::types::float64_t
double float64_t
Definition: types.hpp:47
autoware::perception::filters::filter_node_base::PointCloud2
sensor_msgs::msg::PointCloud2 PointCloud2
Definition: filter_node_base.cpp:32