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[bot] committed Nov 8, 2022
1 parent 43fd452 commit 2b777d7
Showing 1 changed file with 6 additions and 15 deletions.
21 changes: 6 additions & 15 deletions localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -38,9 +38,7 @@
#include <thread>

NDTScanMatcher::NDTScanMatcher()
: Node("ndt_scan_matcher"),
ndt_ptr_(new NormalDistributionsTransform),
map_frame_("map")
: Node("ndt_scan_matcher"), ndt_ptr_(new NormalDistributionsTransform), map_frame_("map")
{
ndt_params_.trans_epsilon = this->declare_parameter<double>("trans_epsilon");
ndt_params_.step_size = this->declare_parameter<double>("step_size");
Expand Down Expand Up @@ -101,16 +99,9 @@ NDTScanMatcher::NDTScanMatcher()
ndt_ptr_ptr_ = std::make_shared<std::shared_ptr<NormalDistributionsTransform>>(ndt_ptr_);
tf2_listener_module_ = std::make_shared<Tf2ListenerModule>(this);
ndt_scan_matching_module_ = std::make_unique<NDTScanMatchingModule>(
this,
&ndt_ptr_mtx_,
ndt_ptr_ptr_,
tf2_listener_module_,
map_frame_,
main_callback_group,
initial_pose_callback_group,
&key_value_stdmap_);
this, &ndt_ptr_mtx_, ndt_ptr_ptr_, tf2_listener_module_, map_frame_, main_callback_group,
initial_pose_callback_group, &key_value_stdmap_);
std::cout << "KOJI ndt_scan_matcher ctor " << *ndt_ptr_ptr_ << std::endl;

}

void NDTScanMatcher::timer_diagnostic()
Expand Down Expand Up @@ -222,7 +213,7 @@ void NDTScanMatcher::callback_map_points(
ndt_ptr_mtx_.lock();
(*ndt_ptr_ptr_).swap(new_ndt_ptr);
ndt_ptr_mtx_.unlock();
std::cout << "KOJI ndt_scan_matcher callbackMap " << *ndt_ptr_ptr_ << std::endl;
std::cout << "KOJI ndt_scan_matcher callbackMap " << *ndt_ptr_ptr_ << std::endl;
}

geometry_msgs::msg::PoseWithCovarianceStamped NDTScanMatcher::align_using_monte_carlo(
Expand Down Expand Up @@ -257,7 +248,8 @@ geometry_msgs::msg::PoseWithCovarianceStamped NDTScanMatcher::align_using_monte_

auto sensor_points_mapTF_ptr = std::make_shared<pcl::PointCloud<PointSource>>();
pcl::transformPointCloud(*ndt_ptr->getInputSource(), *sensor_points_mapTF_ptr, ndt_result.pose);
publish_monte_carlo_point_cloud(initial_pose_with_cov.header.stamp, map_frame_, sensor_points_mapTF_ptr);
publish_monte_carlo_point_cloud(
initial_pose_with_cov.header.stamp, map_frame_, sensor_points_mapTF_ptr);
}

auto best_particle_ptr = std::max_element(
Expand All @@ -273,7 +265,6 @@ geometry_msgs::msg::PoseWithCovarianceStamped NDTScanMatcher::align_using_monte_
return result_pose_with_cov_msg;
}


void NDTScanMatcher::publish_monte_carlo_point_cloud(
const rclcpp::Time & sensor_ros_time, const std::string & frame_id,
const std::shared_ptr<const pcl::PointCloud<PointSource>> & sensor_points_mapTF_ptr)
Expand Down

0 comments on commit 2b777d7

Please sign in to comment.