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