16
16
17
17
MapUpdateModule::MapUpdateModule (
18
18
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)
20
20
: ndt_ptr_(ndt_ptr),
21
21
ndt_ptr_mutex_(ndt_ptr_mutex),
22
22
logger_(node->get_logger ()),
@@ -29,9 +29,10 @@ MapUpdateModule::MapUpdateModule(
29
29
pcd_loader_client_ =
30
30
node->create_client <autoware_map_msgs::srv::GetDifferentialPointCloudMap>(" pcd_loader_service" );
31
31
32
- secondary_ndt_ptr_.reset (new NormalDistributionsTransform );
32
+ secondary_ndt_ptr_.reset (new NdtType );
33
33
34
34
if (ndt_ptr_) {
35
+ RCLCPP_ERROR_STREAM_THROTTLE (logger_, *clock_, 1000 , " Attempt to update a null NDT pointer." );
35
36
*secondary_ndt_ptr_ = *ndt_ptr_;
36
37
}
37
38
@@ -62,35 +63,6 @@ bool MapUpdateModule::should_update_map(const geometry_msgs::msg::Point & positi
62
63
return distance > param_.update_distance ;
63
64
}
64
65
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
-
94
66
void MapUpdateModule::update_map (const geometry_msgs::msg::Point & position)
95
67
{
96
68
// 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)
99
71
ndt_ptr_mutex_->lock ();
100
72
auto param = ndt_ptr_->getParams ();
101
73
102
- ndt_ptr_.reset (new NormalDistributionsTransform );
74
+ ndt_ptr_.reset (new NdtType );
103
75
104
76
ndt_ptr_->setParams (param);
105
77
106
- prefetch_map (position, ndt_ptr_);
78
+ update_ndt (position, * ndt_ptr_);
107
79
ndt_ptr_mutex_->unlock ();
108
80
need_rebuild_ = false ;
109
81
} else {
@@ -112,7 +84,7 @@ void MapUpdateModule::update_map(const geometry_msgs::msg::Point & position)
112
84
// the main ndt_ptr_) overlap, the latency of updating/alignment reduces partly.
113
85
// If the updating is done the main ndt_ptr_, either the update or the NDT
114
86
// align will be blocked by the other.
115
- prefetch_map (position, secondary_ndt_ptr_);
87
+ update_ndt (position, * secondary_ndt_ptr_);
116
88
117
89
ndt_ptr_mutex_->lock ();
118
90
auto input_source = ndt_ptr_->getInputSource ();
@@ -121,7 +93,7 @@ void MapUpdateModule::update_map(const geometry_msgs::msg::Point & position)
121
93
ndt_ptr_mutex_->unlock ();
122
94
}
123
95
124
- secondary_ndt_ptr_.reset (new NormalDistributionsTransform );
96
+ secondary_ndt_ptr_.reset (new NdtType );
125
97
*secondary_ndt_ptr_ = *ndt_ptr_;
126
98
127
99
// Memorize the position of the last update
@@ -131,39 +103,64 @@ void MapUpdateModule::update_map(const geometry_msgs::msg::Point & position)
131
103
publish_partial_pcd_map ();
132
104
}
133
105
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)
138
107
{
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
+
139
136
RCLCPP_INFO (
140
137
logger_, " Update map (Add: %lu, Remove: %lu)" , maps_to_add.size (), map_ids_to_remove.size ());
141
138
if (maps_to_add.empty () && map_ids_to_remove.empty ()) {
142
139
RCLCPP_INFO (logger_, " Skip map update" );
143
140
return ;
144
141
}
145
- const auto exe_start_time = std::chrono::system_clock::now ();
146
142
143
+ const auto exe_start_time = std::chrono::system_clock::now ();
147
144
const size_t add_size = maps_to_add.size ();
148
-
149
145
// Perform heavy processing outside of the lock scope
150
146
std::vector<pcl::shared_ptr<pcl::PointCloud<PointTarget>>> points_pcl (add_size);
147
+
151
148
for (size_t i = 0 ; i < add_size; i++) {
152
149
points_pcl[i] = pcl::make_shared<pcl::PointCloud<PointTarget>>();
153
150
pcl::fromROSMsg (maps_to_add[i].pointcloud , *points_pcl[i]);
154
151
}
155
152
156
153
// Add pcd
157
154
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 );
159
156
}
160
157
161
158
// Remove pcd
162
159
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);
164
161
}
165
162
166
- ndt-> createVoxelKdtree ();
163
+ ndt. createVoxelKdtree ();
167
164
168
165
const auto exe_end_time = std::chrono::system_clock::now ();
169
166
const auto duration_micro_sec =
0 commit comments