@@ -71,7 +71,6 @@ NDTScanMatcher::NDTScanMatcher()
71
71
tf2_listener_(tf2_buffer_),
72
72
ndt_ptr_(new NormalDistributionsTransform),
73
73
state_ptr_(new std::map<std::string, std::string>),
74
- activate_pose_covariance_modifier_(false ),
75
74
is_activated_(false ),
76
75
param_(this )
77
76
{
@@ -103,12 +102,6 @@ NDTScanMatcher::NDTScanMatcher()
103
102
std::bind (&NDTScanMatcher::callback_sensor_points, this , std::placeholders::_1),
104
103
sensor_sub_opt);
105
104
106
- trusted_source_pose_sub_ =
107
- this ->create_subscription <geometry_msgs::msg::PoseWithCovarianceStamped>(
108
- " input_trusted_pose_topic" , 100 ,
109
- std::bind (&NDTScanMatcher::callback_trusted_source_pose, this , std::placeholders::_1),
110
- initial_pose_sub_opt);
111
-
112
105
// Only if regularization is enabled, subscribe to the regularization base pose
113
106
if (param_.ndt_regularization_enable ) {
114
107
// NOTE: The reason that the regularization subscriber does not belong to the
@@ -182,11 +175,10 @@ NDTScanMatcher::NDTScanMatcher()
182
175
&NDTScanMatcher::service_trigger_node, this , std::placeholders::_1, std::placeholders::_2),
183
176
rclcpp::ServicesQoS ().get_rmw_qos_profile (), sensor_callback_group);
184
177
185
- server_covariance_modifier_ = this ->create_service <std_srvs ::srv::SetBool >(
186
- " /localization/pose_estimator/covariance_modifier " ,
178
+ set_parameters_service_ = this ->create_service <rcl_interfaces ::srv::SetParameters >(
179
+ " ndt_scan_matcher/set_parameters " ,
187
180
std::bind (
188
- &NDTScanMatcher::activate_pose_covariance_modifier, this , std::placeholders::_1,
189
- std::placeholders::_2),
181
+ &NDTScanMatcher::setParametersCallback, this , std::placeholders::_1, std::placeholders::_2),
190
182
rclcpp::ServicesQoS ().get_rmw_qos_profile (), sensor_callback_group);
191
183
192
184
ndt_ptr_->setParams (param_.ndt );
@@ -200,17 +192,37 @@ NDTScanMatcher::NDTScanMatcher()
200
192
201
193
logger_configure_ = std::make_unique<tier4_autoware_utils::LoggerLevelConfigure>(this );
202
194
}
203
-
204
- void NDTScanMatcher::activate_pose_covariance_modifier (
205
- const std_srvs::srv::SetBool::Request::SharedPtr req,
206
- std_srvs::srv::SetBool::Response::SharedPtr res)
195
+ void NDTScanMatcher::setParametersCallback (
196
+ const rcl_interfaces::srv::SetParameters::Request::SharedPtr request,
197
+ rcl_interfaces::srv::SetParameters::Response::SharedPtr response)
207
198
{
208
- activate_pose_covariance_modifier_ = req-> data ;
209
- RCLCPP_INFO ( this -> get_logger (), " Pose Covariance Modifier Activated ... " );
210
- res-> success = true ;
211
- res-> message = " Covariance Modifier Activated for NDT " ;
212
- }
199
+ std::lock_guard<std::mutex> lock (mutex_cov_modifier_) ;
200
+
201
+ for ( const auto ¶m : request-> parameters ) {
202
+
203
+ if (param. name == " aw_pose_covariance_modifier.enable " ){
213
204
205
+ activate_pose_covariance_modifier_ = param.value .bool_value ;
206
+ RCLCPP_INFO (this ->get_logger (), " aw_pose_covariance_modifier.enable set to : %d" , activate_pose_covariance_modifier_);
207
+
208
+ rcl_interfaces::msg::SetParametersResult result;
209
+ result.successful = true ;
210
+ result.reason = " Parameter successfully set." ;
211
+
212
+ response->results .push_back (result);
213
+ }
214
+ }
215
+
216
+ if (activate_pose_covariance_modifier_ == 1 ){
217
+ NDTScanMatcher::createTrustedSourcePoseSubscriber ();
218
+ }
219
+ }
220
+ void NDTScanMatcher::createTrustedSourcePoseSubscriber (){
221
+ trusted_source_pose_sub_ =
222
+ this ->create_subscription <geometry_msgs::msg::PoseWithCovarianceStamped>(
223
+ " input_trusted_pose_topic" , 100 ,
224
+ std::bind (&NDTScanMatcher::callback_trusted_source_pose, this , std::placeholders::_1));
225
+ }
214
226
void NDTScanMatcher::publish_diagnostic ()
215
227
{
216
228
diagnostic_msgs::msg::DiagnosticStatus diag_status_msg;
0 commit comments