From eecb577aa9635716f327af152effa9f1500afde4 Mon Sep 17 00:00:00 2001 From: ys-kid Date: Tue, 1 Nov 2022 17:59:01 +0900 Subject: [PATCH 1/4] Publish odom with covariance. --- apps/hdl_localization_nodelet.cpp | 13 ++++++++++--- include/hdl_localization/pose_estimator.hpp | 2 +- src/hdl_localization/pose_estimator.cpp | 3 ++- 3 files changed, 13 insertions(+), 5 deletions(-) diff --git a/apps/hdl_localization_nodelet.cpp b/apps/hdl_localization_nodelet.cpp index 0b42a755..2f4b1134 100644 --- a/apps/hdl_localization_nodelet.cpp +++ b/apps/hdl_localization_nodelet.cpp @@ -265,7 +265,8 @@ class HdlLocalizationNodelet : public nodelet::Nodelet { } // correct - auto aligned = pose_estimator->correct(stamp, filtered); + double fitness_score; + auto aligned = pose_estimator->correct(stamp, filtered, &fitness_score); if(aligned_pub.getNumSubscribers()) { aligned->header.frame_id = "map"; @@ -277,7 +278,7 @@ class HdlLocalizationNodelet : public nodelet::Nodelet { publish_scan_matching_status(points_msg->header, aligned); } - publish_odometry(points_msg->header.stamp, pose_estimator->matrix()); + publish_odometry(points_msg->header.stamp, pose_estimator->matrix(), fitness_score); } /** @@ -397,7 +398,7 @@ class HdlLocalizationNodelet : public nodelet::Nodelet { * @param stamp timestamp * @param pose odometry pose to be published */ - void publish_odometry(const ros::Time& stamp, const Eigen::Matrix4f& pose) { + void publish_odometry(const ros::Time& stamp, const Eigen::Matrix4f& pose, const double fitness_score) { // broadcast the transform over tf if(enable_tf){ if(tf_buffer.canTransform(robot_odom_frame_id, odom_child_frame_id, ros::Time(0))) { @@ -438,10 +439,16 @@ class HdlLocalizationNodelet : public nodelet::Nodelet { odom.header.frame_id = "map"; tf::poseEigenToMsg(Eigen::Isometry3d(pose.cast()), odom.pose.pose); + for(int i=0; i<36; i++){ + odom.pose.covariance[i] = fitness_score; + } odom.child_frame_id = odom_child_frame_id; odom.twist.twist.linear.x = 0.0; odom.twist.twist.linear.y = 0.0; odom.twist.twist.angular.z = 0.0; + for(int i=0; i<36; i++){ + odom.twist.covariance[i] = fitness_score; + } pose_pub.publish(odom); } diff --git a/include/hdl_localization/pose_estimator.hpp b/include/hdl_localization/pose_estimator.hpp index 4080a395..07fe7531 100644 --- a/include/hdl_localization/pose_estimator.hpp +++ b/include/hdl_localization/pose_estimator.hpp @@ -62,7 +62,7 @@ class PoseEstimator { * @param cloud input cloud * @return cloud aligned to the globalmap */ - pcl::PointCloud::Ptr correct(const ros::Time& stamp, const pcl::PointCloud::ConstPtr& cloud); + pcl::PointCloud::Ptr correct(const ros::Time& stamp, const pcl::PointCloud::ConstPtr& cloud, double* fitness_score); /* getters */ ros::Time last_correction_time() const; diff --git a/src/hdl_localization/pose_estimator.cpp b/src/hdl_localization/pose_estimator.cpp index 7031e0d2..030496a0 100644 --- a/src/hdl_localization/pose_estimator.cpp +++ b/src/hdl_localization/pose_estimator.cpp @@ -133,7 +133,7 @@ void PoseEstimator::predict_odom(const Eigen::Matrix4f& odom_delta) { * @param cloud input cloud * @return cloud aligned to the globalmap */ -pcl::PointCloud::Ptr PoseEstimator::correct(const ros::Time& stamp, const pcl::PointCloud::ConstPtr& cloud) { +pcl::PointCloud::Ptr PoseEstimator::correct(const ros::Time& stamp, const pcl::PointCloud::ConstPtr& cloud, double* fitness_score) { last_correction_stamp = stamp; Eigen::Matrix4f no_guess = last_observation; @@ -177,6 +177,7 @@ pcl::PointCloud::Ptr PoseEstimator::correct(const ros::Ti pcl::PointCloud::Ptr aligned(new pcl::PointCloud()); registration->setInputSource(cloud); registration->align(*aligned, init_guess); + *fitness_score = registration->getFitnessScore(); Eigen::Matrix4f trans = registration->getFinalTransformation(); Eigen::Vector3f p = trans.block<3, 1>(0, 3); From 45f9892eddb8cb4a44ba783ce6b3356d37c20b6e Mon Sep 17 00:00:00 2001 From: ys-kid Date: Wed, 2 Nov 2022 17:47:55 +0900 Subject: [PATCH 2/4] Add param --- apps/hdl_localization_nodelet.cpp | 5 +++-- launch/hdl_localization.launch | 1 + 2 files changed, 4 insertions(+), 2 deletions(-) diff --git a/apps/hdl_localization_nodelet.cpp b/apps/hdl_localization_nodelet.cpp index 2f4b1134..d29a3efd 100644 --- a/apps/hdl_localization_nodelet.cpp +++ b/apps/hdl_localization_nodelet.cpp @@ -438,16 +438,17 @@ class HdlLocalizationNodelet : public nodelet::Nodelet { odom.header.stamp = stamp; odom.header.frame_id = "map"; + double fitness_score_coefficient = private_nh.param("fitness_score_coefficient", 1.0); tf::poseEigenToMsg(Eigen::Isometry3d(pose.cast()), odom.pose.pose); for(int i=0; i<36; i++){ - odom.pose.covariance[i] = fitness_score; + odom.pose.covariance[i] = fitness_score_coefficient * fitness_score; } odom.child_frame_id = odom_child_frame_id; odom.twist.twist.linear.x = 0.0; odom.twist.twist.linear.y = 0.0; odom.twist.twist.angular.z = 0.0; for(int i=0; i<36; i++){ - odom.twist.covariance[i] = fitness_score; + odom.twist.covariance[i] = fitness_score_coefficient * fitness_score; } pose_pub.publish(odom); diff --git a/launch/hdl_localization.launch b/launch/hdl_localization.launch index fbf06606..7256785c 100644 --- a/launch/hdl_localization.launch +++ b/launch/hdl_localization.launch @@ -62,6 +62,7 @@ + From c0562de9517f38c7af924315810d91461af77fb8 Mon Sep 17 00:00:00 2001 From: ys-kid Date: Wed, 2 Nov 2022 19:43:37 +0900 Subject: [PATCH 3/4] Adjust covariance --- apps/hdl_localization_nodelet.cpp | 12 ++++++++++-- launch/hdl_localization.launch | 2 +- 2 files changed, 11 insertions(+), 3 deletions(-) diff --git a/apps/hdl_localization_nodelet.cpp b/apps/hdl_localization_nodelet.cpp index d29a3efd..972f2d2c 100644 --- a/apps/hdl_localization_nodelet.cpp +++ b/apps/hdl_localization_nodelet.cpp @@ -441,14 +441,22 @@ class HdlLocalizationNodelet : public nodelet::Nodelet { double fitness_score_coefficient = private_nh.param("fitness_score_coefficient", 1.0); tf::poseEigenToMsg(Eigen::Isometry3d(pose.cast()), odom.pose.pose); for(int i=0; i<36; i++){ - odom.pose.covariance[i] = fitness_score_coefficient * fitness_score; + if(i%7==0){ + odom.pose.covariance[i] = fitness_score_coefficient * fitness_score; + }else{ + odom.pose.covariance[i] = 0.0; + } } odom.child_frame_id = odom_child_frame_id; odom.twist.twist.linear.x = 0.0; odom.twist.twist.linear.y = 0.0; odom.twist.twist.angular.z = 0.0; for(int i=0; i<36; i++){ - odom.twist.covariance[i] = fitness_score_coefficient * fitness_score; + if(i%7==0){ + odom.twist.covariance[i] = fitness_score_coefficient * fitness_score; + }else{ + odom.twist.covariance[i] = 0.0; + } } pose_pub.publish(odom); diff --git a/launch/hdl_localization.launch b/launch/hdl_localization.launch index 7256785c..d79c4d3d 100644 --- a/launch/hdl_localization.launch +++ b/launch/hdl_localization.launch @@ -62,7 +62,7 @@ - + From 7e497a9cb8456b944dc122a095bfcc04d48fa65f Mon Sep 17 00:00:00 2001 From: jsupratman13 Date: Wed, 2 Nov 2022 20:17:35 +0900 Subject: [PATCH 4/4] fix bug & clean up --- apps/hdl_localization_nodelet.cpp | 127 +++++++++++++++--------------- 1 file changed, 62 insertions(+), 65 deletions(-) diff --git a/apps/hdl_localization_nodelet.cpp b/apps/hdl_localization_nodelet.cpp index 972f2d2c..ba52a166 100644 --- a/apps/hdl_localization_nodelet.cpp +++ b/apps/hdl_localization_nodelet.cpp @@ -38,10 +38,8 @@ class HdlLocalizationNodelet : public nodelet::Nodelet { public: using PointT = pcl::PointXYZI; - HdlLocalizationNodelet() : tf_buffer(), tf_listener(tf_buffer) { - } - virtual ~HdlLocalizationNodelet() { - } + HdlLocalizationNodelet() : tf_buffer(), tf_listener(tf_buffer) {} + virtual ~HdlLocalizationNodelet() {} void onInit() override { nh = getNodeHandle(); @@ -71,7 +69,7 @@ class HdlLocalizationNodelet : public nodelet::Nodelet { // global localization use_global_localization = private_nh.param("use_global_localization", true); - if(use_global_localization) { + if (use_global_localization) { NODELET_INFO_STREAM("wait for global localization services"); ros::service::waitForService("/hdl_global_localization/set_global_map"); ros::service::waitForService("/hdl_global_localization/query"); @@ -90,7 +88,7 @@ class HdlLocalizationNodelet : public nodelet::Nodelet { double ndt_neighbor_search_radius = private_nh.param("ndt_neighbor_search_radius", 2.0); double ndt_resolution = private_nh.param("ndt_resolution", 1.0); - if(reg_method == "NDT_OMP") { + if (reg_method == "NDT_OMP") { NODELET_INFO("NDT_OMP is selected"); pclomp::NormalDistributionsTransform::Ptr ndt(new pclomp::NormalDistributionsTransform()); ndt->setTransformationEpsilon(0.01); @@ -111,12 +109,12 @@ class HdlLocalizationNodelet : public nodelet::Nodelet { ndt->setNeighborhoodSearchMethod(pclomp::KDTREE); } return ndt; - } else if(reg_method.find("NDT_CUDA") != std::string::npos) { + } else if (reg_method.find("NDT_CUDA") != std::string::npos) { NODELET_INFO("NDT_CUDA is selected"); boost::shared_ptr> ndt(new fast_gicp::NDTCuda); ndt->setResolution(ndt_resolution); - if(reg_method.find("D2D") != std::string::npos) { + if (reg_method.find("D2D") != std::string::npos) { ndt->setDistanceMode(fast_gicp::NDTDistanceMode::D2D); } else if (reg_method.find("P2D") != std::string::npos) { ndt->setDistanceMode(fast_gicp::NDTDistanceMode::P2D); @@ -157,14 +155,18 @@ class HdlLocalizationNodelet : public nodelet::Nodelet { delta_estimater.reset(new DeltaEstimater(create_registration())); // initialize pose estimator - if(private_nh.param("specify_init_pose", true)) { + if (private_nh.param("specify_init_pose", true)) { NODELET_INFO("initialize pose estimator with specified parameters!!"); - pose_estimator.reset(new hdl_localization::PoseEstimator(registration, + pose_estimator.reset(new hdl_localization::PoseEstimator( + registration, ros::Time::now(), Eigen::Vector3f(private_nh.param("init_pos_x", 0.0), private_nh.param("init_pos_y", 0.0), private_nh.param("init_pos_z", 0.0)), - Eigen::Quaternionf(private_nh.param("init_ori_w", 1.0), private_nh.param("init_ori_x", 0.0), private_nh.param("init_ori_y", 0.0), private_nh.param("init_ori_z", 0.0)), - private_nh.param("cool_time_duration", 0.5) - )); + Eigen::Quaternionf( + private_nh.param("init_ori_w", 1.0), + private_nh.param("init_ori_x", 0.0), + private_nh.param("init_ori_y", 0.0), + private_nh.param("init_ori_z", 0.0)), + private_nh.param("cool_time_duration", 0.5))); } } @@ -184,12 +186,12 @@ class HdlLocalizationNodelet : public nodelet::Nodelet { */ void points_callback(const sensor_msgs::PointCloud2ConstPtr& points_msg) { std::lock_guard estimator_lock(pose_estimator_mutex); - if(!pose_estimator) { + if (!pose_estimator) { NODELET_ERROR("waiting for initial pose input!!"); return; } - if(!globalmap) { + if (!globalmap) { NODELET_ERROR("globalmap has not been received!!"); return; } @@ -198,7 +200,7 @@ class HdlLocalizationNodelet : public nodelet::Nodelet { pcl::PointCloud::Ptr pcl_cloud(new pcl::PointCloud()); pcl::fromROSMsg(*points_msg, *pcl_cloud); - if(pcl_cloud->empty()) { + if (pcl_cloud->empty()) { NODELET_ERROR("cloud is empty!!"); return; } @@ -206,35 +208,33 @@ class HdlLocalizationNodelet : public nodelet::Nodelet { // transform pointcloud into odom_child_frame_id std::string tfError; pcl::PointCloud::Ptr cloud(new pcl::PointCloud()); - if(this->tf_buffer.canTransform(odom_child_frame_id, pcl_cloud->header.frame_id, stamp, ros::Duration(0.1), &tfError)) - { - if(!pcl_ros::transformPointCloud(odom_child_frame_id, *pcl_cloud, *cloud, this->tf_buffer)) { - NODELET_ERROR("point cloud cannot be transformed into target frame!!"); - return; - } - }else - { - NODELET_ERROR(tfError.c_str()); + if (this->tf_buffer.canTransform(odom_child_frame_id, pcl_cloud->header.frame_id, stamp, ros::Duration(0.1), &tfError)) { + if (!pcl_ros::transformPointCloud(odom_child_frame_id, *pcl_cloud, *cloud, this->tf_buffer)) { + NODELET_ERROR("point cloud cannot be transformed into target frame!!"); return; + } + } else { + NODELET_ERROR(tfError.c_str()); + return; } auto filtered = downsample(cloud); last_scan = filtered; - if(relocalizing) { + if (relocalizing) { delta_estimater->add_frame(filtered); } Eigen::Matrix4f before = pose_estimator->matrix(); // predict - if(!use_imu) { + if (!use_imu) { pose_estimator->predict(stamp); } else { std::lock_guard lock(imu_data_mutex); auto imu_iter = imu_data.begin(); - for(imu_iter; imu_iter != imu_data.end(); imu_iter++) { - if(stamp < (*imu_iter)->header.stamp) { + for (imu_iter; imu_iter != imu_data.end(); imu_iter++) { + if (stamp < (*imu_iter)->header.stamp) { break; } const auto& acc = (*imu_iter)->linear_acceleration; @@ -248,15 +248,15 @@ class HdlLocalizationNodelet : public nodelet::Nodelet { // odometry-based prediction ros::Time last_correction_time = pose_estimator->last_correction_time(); - if(private_nh.param("enable_robot_odometry_prediction", false) && !last_correction_time.isZero()) { + if (private_nh.param("enable_robot_odometry_prediction", false) && !last_correction_time.isZero()) { geometry_msgs::TransformStamped odom_delta; - if(tf_buffer.canTransform(odom_child_frame_id, last_correction_time, odom_child_frame_id, stamp, robot_odom_frame_id, ros::Duration(0.1))) { + if (tf_buffer.canTransform(odom_child_frame_id, last_correction_time, odom_child_frame_id, stamp, robot_odom_frame_id, ros::Duration(0.1))) { odom_delta = tf_buffer.lookupTransform(odom_child_frame_id, last_correction_time, odom_child_frame_id, stamp, robot_odom_frame_id, ros::Duration(0)); - } else if(tf_buffer.canTransform(odom_child_frame_id, last_correction_time, odom_child_frame_id, ros::Time(0), robot_odom_frame_id, ros::Duration(0))) { + } else if (tf_buffer.canTransform(odom_child_frame_id, last_correction_time, odom_child_frame_id, ros::Time(0), robot_odom_frame_id, ros::Duration(0))) { odom_delta = tf_buffer.lookupTransform(odom_child_frame_id, last_correction_time, odom_child_frame_id, ros::Time(0), robot_odom_frame_id, ros::Duration(0)); } - if(odom_delta.header.stamp.isZero()) { + if (odom_delta.header.stamp.isZero()) { NODELET_WARN_STREAM("failed to look up transform between " << cloud->header.frame_id << " and " << robot_odom_frame_id); } else { Eigen::Isometry3d delta = tf2::transformToEigen(odom_delta); @@ -268,13 +268,13 @@ class HdlLocalizationNodelet : public nodelet::Nodelet { double fitness_score; auto aligned = pose_estimator->correct(stamp, filtered, &fitness_score); - if(aligned_pub.getNumSubscribers()) { + if (aligned_pub.getNumSubscribers()) { aligned->header.frame_id = "map"; aligned->header.stamp = cloud->header.stamp; aligned_pub.publish(aligned); } - if(status_pub.getNumSubscribers()) { + if (status_pub.getNumSubscribers()) { publish_scan_matching_status(points_msg->header, aligned); } @@ -293,12 +293,12 @@ class HdlLocalizationNodelet : public nodelet::Nodelet { registration->setInputTarget(globalmap); - if(use_global_localization) { + if (use_global_localization) { NODELET_INFO("set globalmap for global localization!"); hdl_global_localization::SetGlobalMap srv; pcl::toROSMsg(*globalmap, srv.request.global_map); - if(!set_global_map_service.call(srv)) { + if (!set_global_map_service.call(srv)) { NODELET_INFO("failed to set global map"); } else { NODELET_INFO("done"); @@ -311,7 +311,7 @@ class HdlLocalizationNodelet : public nodelet::Nodelet { * @param */ bool relocalize(std_srvs::EmptyRequest& req, std_srvs::EmptyResponse& res) { - if(last_scan == nullptr) { + if (last_scan == nullptr) { NODELET_INFO_STREAM("no scan has been received"); return false; } @@ -324,7 +324,7 @@ class HdlLocalizationNodelet : public nodelet::Nodelet { pcl::toROSMsg(*scan, srv.request.cloud); srv.request.max_num_candidates = 1; - if(!query_global_localization_service.call(srv) || srv.response.poses.empty()) { + if (!query_global_localization_service.call(srv) || srv.response.poses.empty()) { relocalizing = false; NODELET_INFO_STREAM("global localization failed"); return false; @@ -365,14 +365,12 @@ class HdlLocalizationNodelet : public nodelet::Nodelet { std::lock_guard lock(pose_estimator_mutex); const auto& p = pose_msg->pose.pose.position; const auto& q = pose_msg->pose.pose.orientation; - pose_estimator.reset( - new hdl_localization::PoseEstimator( - registration, - ros::Time::now(), - Eigen::Vector3f(p.x, p.y, p.z), - Eigen::Quaternionf(q.w, q.x, q.y, q.z), - private_nh.param("cool_time_duration", 0.5)) - ); + pose_estimator.reset(new hdl_localization::PoseEstimator( + registration, + ros::Time::now(), + Eigen::Vector3f(p.x, p.y, p.z), + Eigen::Quaternionf(q.w, q.x, q.y, q.z), + private_nh.param("cool_time_duration", 0.5))); } /** @@ -381,7 +379,7 @@ class HdlLocalizationNodelet : public nodelet::Nodelet { * @return downsampled cloud */ pcl::PointCloud::ConstPtr downsample(const pcl::PointCloud::ConstPtr& cloud) const { - if(!downsample_filter) { + if (!downsample_filter) { return cloud; } @@ -400,8 +398,8 @@ class HdlLocalizationNodelet : public nodelet::Nodelet { */ void publish_odometry(const ros::Time& stamp, const Eigen::Matrix4f& pose, const double fitness_score) { // broadcast the transform over tf - if(enable_tf){ - if(tf_buffer.canTransform(robot_odom_frame_id, odom_child_frame_id, ros::Time(0))) { + if (enable_tf) { + if (tf_buffer.canTransform(robot_odom_frame_id, odom_child_frame_id, ros::Time(0))) { geometry_msgs::TransformStamped map_wrt_frame = tf2::eigenToTransform(Eigen::Isometry3d(pose.inverse().cast())); map_wrt_frame.header.stamp = stamp; map_wrt_frame.header.frame_id = odom_child_frame_id; @@ -438,12 +436,12 @@ class HdlLocalizationNodelet : public nodelet::Nodelet { odom.header.stamp = stamp; odom.header.frame_id = "map"; - double fitness_score_coefficient = private_nh.param("fitness_score_coefficient", 1.0); + double fitness_score_coefficient = private_nh.param("odom_covariance_coefficient", 1.0); tf::poseEigenToMsg(Eigen::Isometry3d(pose.cast()), odom.pose.pose); - for(int i=0; i<36; i++){ - if(i%7==0){ + for (int i = 0; i < 36; i++) { + if (i % 7 == 0) { odom.pose.covariance[i] = fitness_score_coefficient * fitness_score; - }else{ + } else { odom.pose.covariance[i] = 0.0; } } @@ -451,12 +449,12 @@ class HdlLocalizationNodelet : public nodelet::Nodelet { odom.twist.twist.linear.x = 0.0; odom.twist.twist.linear.y = 0.0; odom.twist.twist.angular.z = 0.0; - for(int i=0; i<36; i++){ - if(i%7==0){ + for (int i = 0; i < 36; i++) { + if (i % 7 == 0) { odom.twist.covariance[i] = fitness_score_coefficient * fitness_score; - }else{ + } else { odom.twist.covariance[i] = 0.0; - } + } } pose_pub.publish(odom); @@ -477,10 +475,10 @@ class HdlLocalizationNodelet : public nodelet::Nodelet { int num_inliers = 0; std::vector k_indices; std::vector k_sq_dists; - for(int i = 0; i < aligned->size(); i++) { + for (int i = 0; i < aligned->size(); i++) { const auto& pt = aligned->at(i); registration->getSearchMethodTarget()->nearestKSearch(pt, 1, k_indices, k_sq_dists); - if(k_sq_dists[0] < max_correspondence_dist * max_correspondence_dist) { + if (k_sq_dists[0] < max_correspondence_dist * max_correspondence_dist) { num_inliers++; } } @@ -492,19 +490,19 @@ class HdlLocalizationNodelet : public nodelet::Nodelet { std::vector errors(6, 0.0); - if(pose_estimator->wo_prediction_error()) { + if (pose_estimator->wo_prediction_error()) { status.prediction_labels.push_back(std_msgs::String()); status.prediction_labels.back().data = "without_pred"; status.prediction_errors.push_back(tf2::eigenToTransform(Eigen::Isometry3d(pose_estimator->wo_prediction_error().get().cast())).transform); } - if(pose_estimator->imu_prediction_error()) { + if (pose_estimator->imu_prediction_error()) { status.prediction_labels.push_back(std_msgs::String()); status.prediction_labels.back().data = use_imu ? "imu" : "motion_model"; status.prediction_errors.push_back(tf2::eigenToTransform(Eigen::Isometry3d(pose_estimator->imu_prediction_error().get().cast())).transform); } - if(pose_estimator->odom_prediction_error()) { + if (pose_estimator->odom_prediction_error()) { status.prediction_labels.push_back(std_msgs::String()); status.prediction_labels.back().data = "odom"; status.prediction_errors.push_back(tf2::eigenToTransform(Eigen::Isometry3d(pose_estimator->odom_prediction_error().get().cast())).transform); @@ -562,7 +560,6 @@ class HdlLocalizationNodelet : public nodelet::Nodelet { ros::ServiceClient set_global_map_service; ros::ServiceClient query_global_localization_service; }; -} - +} // namespace hdl_localization PLUGINLIB_EXPORT_CLASS(hdl_localization::HdlLocalizationNodelet, nodelet::Nodelet)