Skip to content

Commit b64959e

Browse files
pre-commit-ci[bot]xmfcx
authored andcommitted
style(pre-commit): autofix
1 parent 4533802 commit b64959e

File tree

8 files changed

+185
-168
lines changed

8 files changed

+185
-168
lines changed

launch/tier4_localization_launch/launch/pose_twist_estimator/pose_twist_estimator.launch.xml

-1
Original file line numberDiff line numberDiff line change
@@ -121,5 +121,4 @@
121121
<arg name="input_pointcloud" value="$(var override_input_pointcloud)"/>
122122
</include>
123123
</group>
124-
125124
</launch>
Original file line numberDiff line numberDiff line change
@@ -1 +1 @@
1-
# aw_pose_covariance_modifier
1+
# aw_pose_covariance_modifier

localization/aw_pose_covariance_modifier_node/launch/aw_pose_covariance_modifier_node.launch.xml

+3-5
Original file line numberDiff line numberDiff line change
@@ -3,10 +3,8 @@
33
<arg name="input_trusted_pose_with_cov_topic" default="/sensing/gnss/pose_with_covariance" description="Trusted pose input .. "/>
44
<arg name="output_pose_with_covariance_topic" default="/localization/pose_estimator/pose_with_covariance" description="Estimated self position with covariance"/>
55

6-
7-
<node pkg="aw_pose_covariance_modifier_node" exec="aw_pose_covariance_modifier_node" name="aw_pose_covariance_modifier_node" output="screen" >
8-
<remap from="input_trusted_pose_with_cov_topic" to="$(var input_trusted_pose_with_cov_topic)"/>
9-
<remap from="output_pose_with_covariance_topic" to="$(var output_pose_with_covariance_topic)"/>
10-
6+
<node pkg="aw_pose_covariance_modifier_node" exec="aw_pose_covariance_modifier_node" name="aw_pose_covariance_modifier_node" output="screen">
7+
<remap from="input_trusted_pose_with_cov_topic" to="$(var input_trusted_pose_with_cov_topic)"/>
8+
<remap from="output_pose_with_covariance_topic" to="$(var output_pose_with_covariance_topic)"/>
119
</node>
1210
</launch>

localization/aw_pose_covariance_modifier_node/package.xml

+2-2
Original file line numberDiff line numberDiff line change
@@ -14,12 +14,12 @@
1414

1515
<build_depend>rosidl_default_generators</build_depend>
1616

17+
<depend>geometry_msgs</depend>
1718
<depend>rclcpp</depend>
1819
<depend>rclcpp_components</depend>
19-
20-
<depend>geometry_msgs</depend>
2120
<depend>std_srvs</depend>
2221

22+
2323
<export>
2424
<build_type>ament_cmake</build_type>
2525
</export>

localization/aw_pose_covariance_modifier_node/src/aw_pose_covariance_modifier_node.cpp

+69-65
Original file line numberDiff line numberDiff line change
@@ -13,78 +13,82 @@
1313
// limitations under the License.
1414

1515
#include "include/aw_pose_covariance_modifier_node.hpp"
16-
#include <rclcpp/rclcpp.hpp>
1716

17+
#include <rclcpp/rclcpp.hpp>
1818

19-
AWPoseCovarianceModifierNode::AWPoseCovarianceModifierNode()
20-
: Node("AWPoseCovarianceModifierNode")
19+
AWPoseCovarianceModifierNode::AWPoseCovarianceModifierNode() : Node("AWPoseCovarianceModifierNode")
2120
{
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+
}
3638
}
3739

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;
6246
}
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+
}
6364
}
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;
8283
}
8384

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;
8494
}
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-
}

localization/aw_pose_covariance_modifier_node/src/include/aw_pose_covariance_modifier_node.hpp

+10-5
Original file line numberDiff line numberDiff line change
@@ -15,8 +15,10 @@
1515
#define AW_POSE_COVARIANCE_MODIFIER_NODE_HPP_
1616

1717
#include <rclcpp/rclcpp.hpp>
18-
#include <std_srvs/srv/set_bool.hpp>
18+
1919
#include <geometry_msgs/msg/pose_with_covariance_stamped.hpp>
20+
#include <std_srvs/srv/set_bool.hpp>
21+
2022
#include <string>
2123

2224
using namespace std::chrono_literals;
@@ -26,17 +28,20 @@ class AWPoseCovarianceModifierNode : public rclcpp::Node
2628
public:
2729
AWPoseCovarianceModifierNode();
2830

29-
rclcpp::Subscription<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr trusted_pose_with_cov_sub_;
30-
rclcpp::Publisher<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr new_pose_estimator_pub_;
31+
rclcpp::Subscription<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr
32+
trusted_pose_with_cov_sub_;
33+
rclcpp::Publisher<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr
34+
new_pose_estimator_pub_;
3135
rclcpp::Client<std_srvs::srv::SetBool>::SharedPtr client_;
3236

33-
void trusted_pose_with_cov_callback(const geometry_msgs::msg::PoseWithCovarianceStamped::ConstSharedPtr &msg);
37+
void trusted_pose_with_cov_callback(
38+
const geometry_msgs::msg::PoseWithCovarianceStamped::ConstSharedPtr & msg);
3439
bool callNDTCovarianceModifier();
40+
3541
private:
3642
double trusted_pose_rmse_;
3743
double trusted_pose_yaw_rmse_in_degrees_;
3844
bool startNDTCovModifier = 0;
3945
};
4046

41-
4247
#endif // AW_POSE_COVARIANCE_MODIFIER_NODE_HPP_

localization/ndt_scan_matcher/include/ndt_scan_matcher/ndt_scan_matcher_core.hpp

+8-6
Original file line numberDiff line numberDiff line change
@@ -140,7 +140,8 @@ class NDTScanMatcher : public rclcpp::Node
140140
rclcpp::TimerBase::SharedPtr map_update_timer_;
141141
rclcpp::Subscription<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr initial_pose_sub_;
142142
rclcpp::Subscription<sensor_msgs::msg::PointCloud2>::SharedPtr sensor_points_sub_;
143-
rclcpp::Subscription<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr regularization_pose_sub_;
143+
rclcpp::Subscription<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr
144+
regularization_pose_sub_;
144145
rclcpp::Subscription<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr
145146
trusted_source_pose_sub_;
146147

@@ -177,7 +178,7 @@ class NDTScanMatcher : public rclcpp::Node
177178

178179
rclcpp::Service<tier4_localization_msgs::srv::PoseWithCovarianceStamped>::SharedPtr service_;
179180
rclcpp::Service<std_srvs::srv::SetBool>::SharedPtr service_trigger_node_;
180-
rclcpp::Service<std_srvs::srv::SetBool>::SharedPtr server_covariance_modifier_;
181+
rclcpp::Service<std_srvs::srv::SetBool>::SharedPtr server_covariance_modifier_;
181182

182183
tf2_ros::TransformBroadcaster tf2_broadcaster_;
183184
tf2_ros::Buffer tf2_buffer_;
@@ -202,10 +203,11 @@ class NDTScanMatcher : public rclcpp::Node
202203
geometry_msgs::msg::PoseWithCovarianceStamped trusted_source_pose_;
203204
bool activate_pose_covariance_modifier_;
204205
bool close_ndt_pose_source_ = false;
205-
struct trusted_pose_{
206-
double pose_avarage_rmse_xy = 0.0;
207-
double yaw_rmse = 0.0;
208-
}trustedPose;
206+
struct trusted_pose_
207+
{
208+
double pose_avarage_rmse_xy = 0.0;
209+
double yaw_rmse = 0.0;
210+
} trustedPose;
209211

210212
std::atomic<bool> is_activated_;
211213
std::unique_ptr<MapUpdateModule> map_update_module_;

0 commit comments

Comments
 (0)