1+ // Copyright 2024 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+ #include " include/aw_pose_covariance_modifier_node.hpp"
16+ #include < rclcpp/rclcpp.hpp>
17+
18+
19+ AWPoseCovarianceModifierNode::AWPoseCovarianceModifierNode ()
20+ : Node(" AWPoseCovarianceModifierNode"
21+ {
22+ trusted_pose_with_cov_sub_ = this ->create_subscription <geometry_msgs::msg::PoseWithCovarianceStamped>(
23+ " input_trusted_pose_with_cov_topic" 10000 ,std::bind (&AWPoseCovarianceModifierNode::trusted_pose_with_cov_callback,this ,std::placeholders::_1));
24+
25+
26+ new_pose_estimator_pub_ = this ->create_publisher <geometry_msgs::msg::PoseWithCovarianceStamped>(" output_pose_with_covariance_topic" 10 );
27+
28+ client_ = this ->create_client <std_srvs::srv::SetBool>(" /localization/pose_estimator/covariance_modifier"
29+
30+ startNDTCovModifier = AWPoseCovarianceModifierNode::callNDTCovarianceModifier ();
31+ if (startNDTCovModifier == 1 ){
32+ RCLCPP_INFO (get_logger (), " NDT pose covariance modifier activated ..."
33+
34+ }
35+
36+ }
37+
38+ bool AWPoseCovarianceModifierNode::callNDTCovarianceModifier (){
39+
40+ while (!client_->wait_for_service (std::chrono::seconds (1 ))) {
41+ if (!rclcpp::ok ()) {
42+ RCLCPP_ERROR (get_logger (), " Interrupted while waiting for the service. Exiting."
43+ return false ;
44+ }
45+ RCLCPP_INFO (get_logger (), " Service not available, waiting again..."
46+ }
47+
48+ auto request = std::make_shared<std_srvs::srv::SetBool::Request>();
49+ request->data = true ;
50+
51+ auto future_result = client_->async_send_request (request);
52+ if (rclcpp::spin_until_future_complete (get_node_base_interface (), future_result) ==
53+ rclcpp::FutureReturnCode::SUCCESS)
54+ {
55+ auto response = future_result.get ();
56+ RCLCPP_INFO (get_logger (), " Response: %d" success );
57+ return true ;
58+ }
59+ else {
60+ RCLCPP_ERROR (get_logger (), " Failed to receive response."
61+ return false ;
62+ }
63+ }
64+ void AWPoseCovarianceModifierNode::trusted_pose_with_cov_callback (const geometry_msgs::msg::PoseWithCovarianceStamped::ConstSharedPtr &msg) {
65+
66+ geometry_msgs::msg::PoseWithCovarianceStamped pose_estimator_pose = *msg;
67+
68+ trusted_pose_rmse_ = (std::sqrt (pose_estimator_pose.pose .covariance [0 ]) + std::sqrt (pose_estimator_pose.pose .covariance [7 ]) ) / 2 ;
69+ trusted_pose_yaw_rmse_in_degrees_ = std::sqrt (pose_estimator_pose.pose .covariance [35 ]) * 180 / M_PI;
70+
71+ if (trusted_pose_rmse_ > 0.25 ){
72+ RCLCPP_INFO (this ->get_logger ()," Trusted Pose RMSE is under the threshold. It will not be used as a pose source."
73+ }
74+ else {
75+
76+ if (trusted_pose_yaw_rmse_in_degrees_ >= 0.3 ){
77+ pose_estimator_pose.pose .covariance [35 ] = 1000000 ;
78+ }
79+
80+ new_pose_estimator_pub_->publish (pose_estimator_pose);
81+
82+ }
83+
84+ }
85+ int main (int argc, char *argv[]) {
86+ rclcpp::init (argc, argv);
87+ rclcpp::spin (std::make_shared<AWPoseCovarianceModifierNode>());
88+ rclcpp::shutdown ();
89+ return 0 ;
90+ }
0 commit comments