Skip to content

Commit 27ae11a

Browse files
committed
Replace pointer to ndt by reference, output error when the input ndt_ptr is null
Signed-off-by: anhnv3991 <anh.nguyen.2@tier4.jp>
1 parent 56dc552 commit 27ae11a

File tree

2 files changed

+45
-56
lines changed

2 files changed

+45
-56
lines changed

localization/ndt_scan_matcher/include/ndt_scan_matcher/map_update_module.hpp

+4-12
Original file line numberDiff line numberDiff line change
@@ -43,31 +43,23 @@ class MapUpdateModule
4343
{
4444
using PointSource = pcl::PointXYZ;
4545
using PointTarget = pcl::PointXYZ;
46-
using NormalDistributionsTransform =
47-
pclomp::MultiGridNormalDistributionsTransform<PointSource, PointTarget>;
48-
using NdtPtrType = std::shared_ptr<NormalDistributionsTransform>;
46+
using NdtType = pclomp::MultiGridNormalDistributionsTransform<PointSource, PointTarget>;
47+
using NdtPtrType = std::shared_ptr<NdtType>;
4948

5049
public:
5150
MapUpdateModule(
52-
rclcpp::Node * node, std::mutex * ndt_ptr_mutex,
53-
std::shared_ptr<NormalDistributionsTransform> & ndt_ptr,
51+
rclcpp::Node * node, std::mutex * ndt_ptr_mutex, NdtPtrType & ndt_ptr,
5452
HyperParameters::DynamicMapLoading param);
5553

5654
private:
5755
friend class NDTScanMatcher;
5856

5957
// Update the specified NDT
60-
void update_ndt(
61-
NdtPtrType & ndt,
62-
const std::vector<autoware_map_msgs::msg::PointCloudMapCellWithID> & maps_to_add,
63-
const std::vector<std::string> & map_ids_to_remove);
58+
void update_ndt(const geometry_msgs::msg::Point & position, NdtType & ndt);
6459
void update_map(const geometry_msgs::msg::Point & position);
6560
[[nodiscard]] bool should_update_map(const geometry_msgs::msg::Point & position);
6661
void publish_partial_pcd_map();
6762

68-
// Pre-fetch NDT map in a child thread
69-
void prefetch_map(const geometry_msgs::msg::Point & position, NdtPtrType & ndt);
70-
7163
rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr loaded_pcd_pub_;
7264

7365
rclcpp::Client<autoware_map_msgs::srv::GetDifferentialPointCloudMap>::SharedPtr

localization/ndt_scan_matcher/src/map_update_module.cpp

+41-44
Original file line numberDiff line numberDiff line change
@@ -16,7 +16,7 @@
1616

1717
MapUpdateModule::MapUpdateModule(
1818
rclcpp::Node * node, std::mutex * ndt_ptr_mutex,
19-
std::shared_ptr<NormalDistributionsTransform> & ndt_ptr, HyperParameters::DynamicMapLoading param)
19+
NdtPtrType & ndt_ptr, HyperParameters::DynamicMapLoading param)
2020
: ndt_ptr_(ndt_ptr),
2121
ndt_ptr_mutex_(ndt_ptr_mutex),
2222
logger_(node->get_logger()),
@@ -29,9 +29,10 @@ MapUpdateModule::MapUpdateModule(
2929
pcd_loader_client_ =
3030
node->create_client<autoware_map_msgs::srv::GetDifferentialPointCloudMap>("pcd_loader_service");
3131

32-
secondary_ndt_ptr_.reset(new NormalDistributionsTransform);
32+
secondary_ndt_ptr_.reset(new NdtType);
3333

3434
if (ndt_ptr_) {
35+
RCLCPP_ERROR_STREAM_THROTTLE(logger_, *clock_, 1000, "Attempt to update a null NDT pointer.");
3536
*secondary_ndt_ptr_ = *ndt_ptr_;
3637
}
3738

@@ -62,35 +63,6 @@ bool MapUpdateModule::should_update_map(const geometry_msgs::msg::Point & positi
6263
return distance > param_.update_distance;
6364
}
6465

65-
void MapUpdateModule::prefetch_map(const geometry_msgs::msg::Point & position, NdtPtrType & ndt)
66-
{
67-
auto request = std::make_shared<autoware_map_msgs::srv::GetDifferentialPointCloudMap::Request>();
68-
69-
request->area.center_x = static_cast<float>(position.x);
70-
request->area.center_y = static_cast<float>(position.y);
71-
request->area.radius = static_cast<float>(param_.map_radius);
72-
request->cached_ids = ndt->getCurrentMapIDs();
73-
74-
while (!pcd_loader_client_->wait_for_service(std::chrono::seconds(1)) && rclcpp::ok()) {
75-
RCLCPP_INFO(logger_, "Waiting for pcd loader service. Check the pointcloud_map_loader.");
76-
}
77-
78-
// send a request to map_loader
79-
auto result{pcd_loader_client_->async_send_request(
80-
request,
81-
[](rclcpp::Client<autoware_map_msgs::srv::GetDifferentialPointCloudMap>::SharedFuture) {})};
82-
83-
std::future_status status = result.wait_for(std::chrono::seconds(0));
84-
while (status != std::future_status::ready) {
85-
RCLCPP_INFO(logger_, "waiting response");
86-
if (!rclcpp::ok()) {
87-
return;
88-
}
89-
status = result.wait_for(std::chrono::seconds(1));
90-
}
91-
update_ndt(ndt, result.get()->new_pointcloud_with_ids, result.get()->ids_to_remove);
92-
}
93-
9466
void MapUpdateModule::update_map(const geometry_msgs::msg::Point & position)
9567
{
9668
// If the current position is super far from the previous loading position,
@@ -99,11 +71,11 @@ void MapUpdateModule::update_map(const geometry_msgs::msg::Point & position)
9971
ndt_ptr_mutex_->lock();
10072
auto param = ndt_ptr_->getParams();
10173

102-
ndt_ptr_.reset(new NormalDistributionsTransform);
74+
ndt_ptr_.reset(new NdtType);
10375

10476
ndt_ptr_->setParams(param);
10577

106-
prefetch_map(position, ndt_ptr_);
78+
update_ndt(position, *ndt_ptr_);
10779
ndt_ptr_mutex_->unlock();
10880
need_rebuild_ = false;
10981
} else {
@@ -112,7 +84,7 @@ void MapUpdateModule::update_map(const geometry_msgs::msg::Point & position)
11284
// the main ndt_ptr_) overlap, the latency of updating/alignment reduces partly.
11385
// If the updating is done the main ndt_ptr_, either the update or the NDT
11486
// align will be blocked by the other.
115-
prefetch_map(position, secondary_ndt_ptr_);
87+
update_ndt(position, *secondary_ndt_ptr_);
11688

11789
ndt_ptr_mutex_->lock();
11890
auto input_source = ndt_ptr_->getInputSource();
@@ -121,7 +93,7 @@ void MapUpdateModule::update_map(const geometry_msgs::msg::Point & position)
12193
ndt_ptr_mutex_->unlock();
12294
}
12395

124-
secondary_ndt_ptr_.reset(new NormalDistributionsTransform);
96+
secondary_ndt_ptr_.reset(new NdtType);
12597
*secondary_ndt_ptr_ = *ndt_ptr_;
12698

12799
// Memorize the position of the last update
@@ -131,39 +103,64 @@ void MapUpdateModule::update_map(const geometry_msgs::msg::Point & position)
131103
publish_partial_pcd_map();
132104
}
133105

134-
void MapUpdateModule::update_ndt(
135-
NdtPtrType & ndt,
136-
const std::vector<autoware_map_msgs::msg::PointCloudMapCellWithID> & maps_to_add,
137-
const std::vector<std::string> & map_ids_to_remove)
106+
void MapUpdateModule::update_ndt(const geometry_msgs::msg::Point & position, NdtType & ndt)
138107
{
108+
auto request = std::make_shared<autoware_map_msgs::srv::GetDifferentialPointCloudMap::Request>();
109+
110+
request->area.center_x = static_cast<float>(position.x);
111+
request->area.center_y = static_cast<float>(position.y);
112+
request->area.radius = static_cast<float>(param_.map_radius);
113+
request->cached_ids = ndt.getCurrentMapIDs();
114+
115+
while (!pcd_loader_client_->wait_for_service(std::chrono::seconds(1)) && rclcpp::ok()) {
116+
RCLCPP_INFO(logger_, "Waiting for pcd loader service. Check the pointcloud_map_loader.");
117+
}
118+
119+
// send a request to map_loader
120+
auto result{pcd_loader_client_->async_send_request(
121+
request,
122+
[](rclcpp::Client<autoware_map_msgs::srv::GetDifferentialPointCloudMap>::SharedFuture) {})};
123+
124+
std::future_status status = result.wait_for(std::chrono::seconds(0));
125+
while (status != std::future_status::ready) {
126+
RCLCPP_INFO(logger_, "waiting response");
127+
if (!rclcpp::ok()) {
128+
return;
129+
}
130+
status = result.wait_for(std::chrono::seconds(1));
131+
}
132+
133+
auto& maps_to_add = result.get()->new_pointcloud_with_ids;
134+
auto& map_ids_to_remove = result.get()->ids_to_remove;
135+
139136
RCLCPP_INFO(
140137
logger_, "Update map (Add: %lu, Remove: %lu)", maps_to_add.size(), map_ids_to_remove.size());
141138
if (maps_to_add.empty() && map_ids_to_remove.empty()) {
142139
RCLCPP_INFO(logger_, "Skip map update");
143140
return;
144141
}
145-
const auto exe_start_time = std::chrono::system_clock::now();
146142

143+
const auto exe_start_time = std::chrono::system_clock::now();
147144
const size_t add_size = maps_to_add.size();
148-
149145
// Perform heavy processing outside of the lock scope
150146
std::vector<pcl::shared_ptr<pcl::PointCloud<PointTarget>>> points_pcl(add_size);
147+
151148
for (size_t i = 0; i < add_size; i++) {
152149
points_pcl[i] = pcl::make_shared<pcl::PointCloud<PointTarget>>();
153150
pcl::fromROSMsg(maps_to_add[i].pointcloud, *points_pcl[i]);
154151
}
155152

156153
// Add pcd
157154
for (size_t i = 0; i < add_size; i++) {
158-
ndt->addTarget(points_pcl[i], maps_to_add[i].cell_id);
155+
ndt.addTarget(points_pcl[i], maps_to_add[i].cell_id);
159156
}
160157

161158
// Remove pcd
162159
for (const std::string & map_id_to_remove : map_ids_to_remove) {
163-
ndt->removeTarget(map_id_to_remove);
160+
ndt.removeTarget(map_id_to_remove);
164161
}
165162

166-
ndt->createVoxelKdtree();
163+
ndt.createVoxelKdtree();
167164

168165
const auto exe_end_time = std::chrono::system_clock::now();
169166
const auto duration_micro_sec =

0 commit comments

Comments
 (0)