Skip to content

Commit

Permalink
ci(pre-commit): autofix
Browse files Browse the repository at this point in the history
  • Loading branch information
@pre-commit-ci @YamatoAndo
pre-commit-ci[bot] authored and YamatoAndo committed Jan 21, 2022
1 parent 0721408 commit 7a89cd8
Show file tree
Hide file tree
Showing 3 changed files with 153 additions and 31 deletions.
169 changes: 144 additions & 25 deletions localization/pose_initializer/config/pose_initializer.param.yaml
View file
Original file line number Diff line number Diff line change
@@ -1,46 +1,165 @@
/**:
ros__parameters:

# from initialpose (Rviz's 2DPoseEstimate)
initialpose_particle_covariance:
[
4.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 4.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.01, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.01, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.01, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 1.0,
4.0,
0.0,
0.0,
0.0,
0.0,
0.0,
0.0,
4.0,
0.0,
0.0,
0.0,
0.0,
0.0,
0.0,
0.01,
0.0,
0.0,
0.0,
0.0,
0.0,
0.0,
0.01,
0.0,
0.0,
0.0,
0.0,
0.0,
0.0,
0.01,
0.0,
0.0,
0.0,
0.0,
0.0,
0.0,
1.0,
]

# from gnss
gnss_particle_covariance:
[
1.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 1.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.01, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.01, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.01, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 10.0,
1.0,
0.0,
0.0,
0.0,
0.0,
0.0,
0.0,
1.0,
0.0,
0.0,
0.0,
0.0,
0.0,
0.0,
0.01,
0.0,
0.0,
0.0,
0.0,
0.0,
0.0,
0.01,
0.0,
0.0,
0.0,
0.0,
0.0,
0.0,
0.01,
0.0,
0.0,
0.0,
0.0,
0.0,
0.0,
10.0,
]

# from service
service_particle_covariance:
[
1.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 1.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.01, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.01, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.01, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 10.0,
1.0,
0.0,
0.0,
0.0,
0.0,
0.0,
0.0,
1.0,
0.0,
0.0,
0.0,
0.0,
0.0,
0.0,
0.01,
0.0,
0.0,
0.0,
0.0,
0.0,
0.0,
0.01,
0.0,
0.0,
0.0,
0.0,
0.0,
0.0,
0.01,
0.0,
0.0,
0.0,
0.0,
0.0,
0.0,
10.0,
]

# output
output_pose_covariance:
[
1.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 1.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.01, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.01, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.01, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.2,
1.0,
0.0,
0.0,
0.0,
0.0,
0.0,
0.0,
1.0,
0.0,
0.0,
0.0,
0.0,
0.0,
0.0,
0.01,
0.0,
0.0,
0.0,
0.0,
0.0,
0.0,
0.01,
0.0,
0.0,
0.0,
0.0,
0.0,
0.0,
0.01,
0.0,
0.0,
0.0,
0.0,
0.0,
0.0,
0.2,
]
1 change: 0 additions & 1 deletion localization/pose_initializer/include/pose_initializer/pose_initializer_core.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -95,7 +95,6 @@ class PoseInitializer : public rclcpp::Node
std::array<double, 36> gnss_particle_covariance_;
std::array<double, 36> service_particle_covariance_;
std::array<double, 36> output_pose_covariance_;

};

#endif // POSE_INITIALIZER__POSE_INITIALIZER_CORE_HPP_
14 changes: 9 additions & 5 deletions localization/pose_initializer/src/pose_initializer_core.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -47,29 +47,33 @@ PoseInitializer::PoseInitializer()
enable_gnss_callback_ = this->declare_parameter("enable_gnss_callback", true);

this->declare_parameter("initialpose_particle_covariance");
std::vector<double> initialpose_particle_covariance = this->get_parameter("initialpose_particle_covariance").as_double_array();
std::vector<double> initialpose_particle_covariance =
this->get_parameter("initialpose_particle_covariance").as_double_array();
for (std::size_t i = 0; i < initialpose_particle_covariance.size(); ++i) {
initialpose_particle_covariance_[i] = initialpose_particle_covariance[i];
}

this->declare_parameter("gnss_particle_covariance");
std::vector<double> gnss_particle_covariance = this->get_parameter("gnss_particle_covariance").as_double_array();
std::vector<double> gnss_particle_covariance =
this->get_parameter("gnss_particle_covariance").as_double_array();
for (std::size_t i = 0; i < gnss_particle_covariance.size(); ++i) {
gnss_particle_covariance_[i] = gnss_particle_covariance[i];
}

this->declare_parameter("service_particle_covariance");
std::vector<double> service_particle_covariance = this->get_parameter("service_particle_covariance").as_double_array();
std::vector<double> service_particle_covariance =
this->get_parameter("service_particle_covariance").as_double_array();
for (std::size_t i = 0; i < service_particle_covariance.size(); ++i) {
service_particle_covariance_[i] = service_particle_covariance[i];
}

this->declare_parameter("output_pose_covariance");
std::vector<double> output_pose_covariance = this->get_parameter("output_pose_covariance").as_double_array();
std::vector<double> output_pose_covariance =
this->get_parameter("output_pose_covariance").as_double_array();
for (std::size_t i = 0; i < output_pose_covariance.size(); ++i) {
output_pose_covariance_[i] = output_pose_covariance[i];
}

// We can't use _1 because pcl leaks an alias to boost::placeholders::_1, so it would be ambiguous
initial_pose_sub_ = this->create_subscription<geometry_msgs::msg::PoseWithCovarianceStamped>(
"initialpose", 10,
Expand Down

0 comments on commit 7a89cd8

Please sign in to comment.