Skip to content

Commit 2b0fe2b

Browse files
add set_parameters callback
Signed-off-by: melike <melike@leodrive.ai>
1 parent 11deedd commit 2b0fe2b

File tree

8 files changed

+93
-60
lines changed

8 files changed

+93
-60
lines changed

localization/aw_pose_covariance_modifier_node/CMakeLists.txt

+3-4
Original file line numberDiff line numberDiff line change
@@ -5,14 +5,13 @@ find_package(autoware_cmake REQUIRED)
55
autoware_package()
66
find_package(rclcpp REQUIRED)
77
find_package(geometry_msgs REQUIRED)
8-
find_package(std_srvs REQUIRED)
9-
8+
find_package(rcl_interfaces REQUIRED)
109
set(NODE_NAME ${PROJECT_NAME})
1110
set(EXEC_NAME ${PROJECT_NAME}_exe)
1211

1312
add_executable(${PROJECT_NAME} src/aw_pose_covariance_modifier_node.cpp)
1413

15-
ament_target_dependencies(${PROJECT_NAME} rclcpp geometry_msgs std_srvs)
14+
ament_target_dependencies(${PROJECT_NAME} rclcpp geometry_msgs rcl_interfaces)
1615

1716
install(TARGETS
1817
${PROJECT_NAME}
@@ -30,4 +29,4 @@ include_directories(src/include/)
3029

3130

3231
ament_auto_package(INSTALL_TO_SHARE
33-
launch)
32+
launch)

localization/aw_pose_covariance_modifier_node/package.xml

+2-2
Original file line numberDiff line numberDiff line change
@@ -17,9 +17,9 @@
1717
<depend>geometry_msgs</depend>
1818
<depend>rclcpp</depend>
1919
<depend>rclcpp_components</depend>
20-
<depend>std_srvs</depend>
20+
<depend>rcl_interfaces</depend>
2121

2222
<export>
2323
<build_type>ament_cmake</build_type>
2424
</export>
25-
</package>
25+
</package>

localization/aw_pose_covariance_modifier_node/src/aw_pose_covariance_modifier_node.cpp

+42-24
Original file line numberDiff line numberDiff line change
@@ -29,39 +29,57 @@ AWPoseCovarianceModifierNode::AWPoseCovarianceModifierNode() : Node("AWPoseCovar
2929
"output_pose_with_covariance_topic", 10);
3030

3131
client_ =
32-
this->create_client<std_srvs::srv::SetBool>("/localization/pose_estimator/covariance_modifier");
32+
this->create_client<rcl_interfaces::srv::SetParameters>("/localization/pose_estimator/ndt_scan_matcher/set_parameters");
3333

34-
startNDTCovModifier = AWPoseCovarianceModifierNode::callNDTCovarianceModifier();
35-
if (startNDTCovModifier == 1) {
34+
while (!client_->wait_for_service(std::chrono::seconds(1)) && rclcpp::ok()) {
35+
RCLCPP_INFO(
36+
this->get_logger(),
37+
"Waiting for aw_pose_covariance_modifier_node service...");
38+
}
39+
40+
activateNDTCovModifier = AWPoseCovarianceModifierNode::callNDTCovarianceModifier();
41+
if (activateNDTCovModifier == 1) {
3642
RCLCPP_INFO(get_logger(), "NDT pose covariance modifier activated ...");
3743
}
44+
else{
45+
RCLCPP_WARN(get_logger(), "Failed to enable NDT pose covariance modifier ...");
46+
}
3847
}
3948

4049
bool AWPoseCovarianceModifierNode::callNDTCovarianceModifier()
4150
{
42-
while (!client_->wait_for_service(std::chrono::seconds(1))) {
43-
if (!rclcpp::ok()) {
44-
RCLCPP_ERROR(get_logger(), "Interrupted while waiting for the service. Exiting.");
45-
return false;
46-
}
47-
RCLCPP_INFO(get_logger(), "Service not available, waiting again...");
48-
}
4951

50-
auto request = std::make_shared<std_srvs::srv::SetBool::Request>();
51-
request->data = true;
52+
auto request = std::make_shared<rcl_interfaces::srv::SetParameters::Request>();
5253

53-
auto future_result = client_->async_send_request(request);
54-
if (
55-
rclcpp::spin_until_future_complete(get_node_base_interface(), future_result) ==
56-
rclcpp::FutureReturnCode::SUCCESS) {
57-
auto response = future_result.get();
58-
RCLCPP_INFO(get_logger(), "Response: %d", response->success);
59-
return true;
60-
} else {
61-
RCLCPP_ERROR(get_logger(), "Failed to receive response.");
62-
return false;
63-
}
54+
rcl_interfaces::msg::Parameter parameter;
55+
parameter.name = "aw_pose_covariance_modifier.enable";
56+
parameter.value.type = rcl_interfaces::msg::ParameterType::PARAMETER_BOOL;
57+
parameter.value.bool_value = true;
58+
59+
request->parameters.push_back(parameter);
60+
61+
client_->async_send_request(
62+
request, [this](rclcpp::Client<rcl_interfaces::srv::SetParameters>::SharedFuture result_) {
63+
auto result_status = result_.wait_for(std::chrono::seconds(1));
64+
65+
if (result_status == std::future_status::ready) {
66+
auto response = result_.get();
67+
68+
if (response && response->results.data()) {
69+
RCLCPP_INFO(this->get_logger(), "aw_pose_covariance_modifier.enable parameter set successfully.");
70+
return true;
71+
} else {
72+
RCLCPP_ERROR(this->get_logger(), "An error occurred while setting the aw_pose_covariance_modifier.enable parameter.");
73+
return false;
74+
}
75+
} else {
76+
RCLCPP_ERROR(this->get_logger(), "The request was not completed within the specified time.");
77+
return false;
78+
}
79+
});
80+
return true;
6481
}
82+
6583
void AWPoseCovarianceModifierNode::trusted_pose_with_cov_callback(
6684
const geometry_msgs::msg::PoseWithCovarianceStamped::ConstSharedPtr & msg)
6785
{
@@ -91,4 +109,4 @@ int main(int argc, char * argv[])
91109
rclcpp::spin(std::make_shared<AWPoseCovarianceModifierNode>());
92110
rclcpp::shutdown();
93111
return 0;
94-
}
112+
}

localization/aw_pose_covariance_modifier_node/src/include/aw_pose_covariance_modifier_node.hpp

+4-5
Original file line numberDiff line numberDiff line change
@@ -17,8 +17,7 @@
1717
#include <rclcpp/rclcpp.hpp>
1818

1919
#include <geometry_msgs/msg/pose_with_covariance_stamped.hpp>
20-
#include <std_srvs/srv/set_bool.hpp>
21-
20+
#include <rcl_interfaces/srv/set_parameters.hpp>
2221
#include <string>
2322

2423
class AWPoseCovarianceModifierNode : public rclcpp::Node
@@ -30,7 +29,7 @@ class AWPoseCovarianceModifierNode : public rclcpp::Node
3029
trusted_pose_with_cov_sub_;
3130
rclcpp::Publisher<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr
3231
new_pose_estimator_pub_;
33-
rclcpp::Client<std_srvs::srv::SetBool>::SharedPtr client_;
32+
rclcpp::Client<rcl_interfaces::srv::SetParameters>::SharedPtr client_;
3433

3534
void trusted_pose_with_cov_callback(
3635
const geometry_msgs::msg::PoseWithCovarianceStamped::ConstSharedPtr & msg);
@@ -39,7 +38,7 @@ class AWPoseCovarianceModifierNode : public rclcpp::Node
3938
private:
4039
double trusted_pose_rmse_;
4140
double trusted_pose_yaw_rmse_in_degrees_;
42-
bool startNDTCovModifier = 0;
41+
bool activateNDTCovModifier = 0;
4342
};
4443

45-
#endif // AW_POSE_COVARIANCE_MODIFIER_NODE_HPP_
44+
#endif // AW_POSE_COVARIANCE_MODIFIER_NODE_HPP_

localization/ndt_scan_matcher/CMakeLists.txt

+1
Original file line numberDiff line numberDiff line change
@@ -2,6 +2,7 @@ cmake_minimum_required(VERSION 3.14)
22
project(ndt_scan_matcher)
33

44
find_package(autoware_cmake REQUIRED)
5+
find_package(rcl_interfaces REQUIRED)
56
autoware_package()
67

78
# Compile flags for SIMD instructions

localization/ndt_scan_matcher/include/ndt_scan_matcher/ndt_scan_matcher_core.hpp

+8-5
Original file line numberDiff line numberDiff line change
@@ -42,6 +42,7 @@
4242
#include <tf2/transform_datatypes.h>
4343
#include <tf2_ros/buffer.h>
4444
#include <tf2_ros/transform_listener.h>
45+
#include "rcl_interfaces/srv/set_parameters.hpp"
4546

4647
#ifdef ROS_DISTRO_GALACTIC
4748
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
@@ -83,9 +84,9 @@ class NDTScanMatcher : public rclcpp::Node
8384
void service_trigger_node(
8485
const std_srvs::srv::SetBool::Request::SharedPtr req,
8586
std_srvs::srv::SetBool::Response::SharedPtr res);
86-
void activate_pose_covariance_modifier(
87-
const std_srvs::srv::SetBool::Request::SharedPtr req,
88-
std_srvs::srv::SetBool::Response::SharedPtr res);
87+
void setParametersCallback(
88+
const rcl_interfaces::srv::SetParameters::Request::SharedPtr request,
89+
rcl_interfaces::srv::SetParameters::Response::SharedPtr response);
8990
void callback_timer();
9091
void callback_sensor_points(
9192
sensor_msgs::msg::PointCloud2::ConstSharedPtr sensor_points_msg_in_sensor_frame);
@@ -178,7 +179,7 @@ class NDTScanMatcher : public rclcpp::Node
178179

179180
rclcpp::Service<tier4_localization_msgs::srv::PoseWithCovarianceStamped>::SharedPtr service_;
180181
rclcpp::Service<std_srvs::srv::SetBool>::SharedPtr service_trigger_node_;
181-
rclcpp::Service<std_srvs::srv::SetBool>::SharedPtr server_covariance_modifier_;
182+
rclcpp::Service<rcl_interfaces::srv::SetParameters>::SharedPtr set_parameters_service_;
182183

183184
tf2_ros::TransformBroadcaster tf2_broadcaster_;
184185
tf2_ros::Buffer tf2_buffer_;
@@ -215,9 +216,11 @@ class NDTScanMatcher : public rclcpp::Node
215216

216217
std::array<double, 36> covariance_modifier(std::array<double, 36> & in_ndt_covariance);
217218

218-
// To be used for timeout control
219+
// To be used for AW Pose Covariance Modifier - Trusted Pose timeout control
219220
bool checkTrustedPoseTimeout();
220221
rclcpp::Time trustedPoseCallbackTime;
222+
std::mutex mutex_cov_modifier_;
223+
void createTrustedSourcePoseSubscriber();
221224

222225
HyperParameters param_;
223226
};

localization/ndt_scan_matcher/package.xml

+1
Original file line numberDiff line numberDiff line change
@@ -42,6 +42,7 @@
4242
<depend>tier4_localization_msgs</depend>
4343
<depend>tree_structured_parzen_estimator</depend>
4444
<depend>visualization_msgs</depend>
45+
<depend>rcl_interfaces</depend>
4546

4647
<test_depend>ament_cmake_cppcheck</test_depend>
4748
<test_depend>ament_lint_auto</test_depend>

localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp

+32-20
Original file line numberDiff line numberDiff line change
@@ -71,7 +71,6 @@ NDTScanMatcher::NDTScanMatcher()
7171
tf2_listener_(tf2_buffer_),
7272
ndt_ptr_(new NormalDistributionsTransform),
7373
state_ptr_(new std::map<std::string, std::string>),
74-
activate_pose_covariance_modifier_(false),
7574
is_activated_(false),
7675
param_(this)
7776
{
@@ -103,12 +102,6 @@ NDTScanMatcher::NDTScanMatcher()
103102
std::bind(&NDTScanMatcher::callback_sensor_points, this, std::placeholders::_1),
104103
sensor_sub_opt);
105104

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-
112105
// Only if regularization is enabled, subscribe to the regularization base pose
113106
if (param_.ndt_regularization_enable) {
114107
// NOTE: The reason that the regularization subscriber does not belong to the
@@ -182,11 +175,10 @@ NDTScanMatcher::NDTScanMatcher()
182175
&NDTScanMatcher::service_trigger_node, this, std::placeholders::_1, std::placeholders::_2),
183176
rclcpp::ServicesQoS().get_rmw_qos_profile(), sensor_callback_group);
184177

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",
187180
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),
190182
rclcpp::ServicesQoS().get_rmw_qos_profile(), sensor_callback_group);
191183

192184
ndt_ptr_->setParams(param_.ndt);
@@ -200,17 +192,37 @@ NDTScanMatcher::NDTScanMatcher()
200192

201193
logger_configure_ = std::make_unique<tier4_autoware_utils::LoggerLevelConfigure>(this);
202194
}
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)
207198
{
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 &param : request->parameters) {
202+
203+
if(param.name == "aw_pose_covariance_modifier.enable"){
213204

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+
}
214226
void NDTScanMatcher::publish_diagnostic()
215227
{
216228
diagnostic_msgs::msg::DiagnosticStatus diag_status_msg;

0 commit comments

Comments
 (0)