fix(lidar-lidar-calib): fix crash when passing point cloud with zero …

…points (#111) * fix client dump Signed-off-by: asa-naki <[email protected]> * add pca size check Signed-off-by: asa-naki <[email protected]> * add pointcloud width check Signed-off-by: asa-naki <[email protected]> * add error stream output for request, success is 0 Signed-off-by: asa-naki <[email protected]> * fix pcl_sensor is zero pointcloud Signed-off-by: asa-naki <[email protected]> * fix check bool Signed-off-by: asa-naki <[email protected]> * update error message info Signed-off-by: asa-naki <[email protected]> * fix indent Signed-off-by: asa-naki <[email protected]> --------- Signed-off-by: asa-naki <[email protected]>
tier4 · Jun 28, 2023 · 18557aa · 18557aa
1 parent c8a0318
commit 18557aa
Show file tree

Hide file tree

Showing 5 changed files with 27 additions and 10 deletions.
diff --git a/sensor/extrinsic_calibration_client/src/extrinsic_calibration_client.cpp b/sensor/extrinsic_calibration_client/src/extrinsic_calibration_client.cpp
@@ -42,9 +42,10 @@ int main(int argc, char * argv[])
     std::make_shared<tier4_calibration_msgs::srv::ExtrinsicCalibrationManager::Request>();
   request->src_path = node->declare_parameter("src_path", "");
   request->dst_path = node->declare_parameter("dst_path", "");
-  auto result = client->async_send_request(request);
+  auto future = client->async_send_request(request);
 
-  if (rclcpp::spin_until_future_complete(node, result) == rclcpp::FutureReturnCode::SUCCESS) {
+  if (rclcpp::spin_until_future_complete(node, future) == rclcpp::FutureReturnCode::SUCCESS) {
+    auto result = future.get();
     RCLCPP_INFO_STREAM(
       node->get_logger(),
       "Received service message. success " << result.get()->success << " score " <<

diff --git a/sensor/extrinsic_calibration_manager/src/extrinsic_calibration_manager_node.cpp b/sensor/extrinsic_calibration_manager/src/extrinsic_calibration_manager_node.cpp
@@ -87,10 +87,15 @@ void ExtrinsicCalibrationManagerNode::calibrationRequestCallback(
         auto res = response_client.get();
         target_client.response = *res;
         target_client.estimated = true;
-        RCLCPP_INFO_STREAM(
-          this->get_logger(), "Received service message: " << target_client.child_frame <<
+        if(res->success){
+          RCLCPP_INFO_STREAM(
+            this->get_logger(), "Received service message: " << target_client.child_frame <<
             "(success = " << res->success <<
             " score = " << res->score << ")");
+        }else{
+          RCLCPP_ERROR_STREAM(
+            this->get_logger(), "calibration_failed: " << target_client.child_frame);
+        }
       };
 
     RCLCPP_INFO_STREAM(this->get_logger(), "Call service: " << target_client.child_frame);

diff --git a/sensor/extrinsic_map_based_calibrator/src/extrinsic_map_based_calibrator.cpp b/sensor/extrinsic_map_based_calibrator/src/extrinsic_map_based_calibrator.cpp
@@ -141,7 +141,7 @@ bool ExtrinsicMapBasedCalibrator::mapBasedCalibration(const tf2::Transform & tf_
     return false;
   }
   if (!sensor_pointcloud_msg_) {
-    RCLCPP_ERROR(this->get_logger(), "Can not received pandar left upper point cloud topic");
+    RCLCPP_ERROR(this->get_logger(), "Can not received sensor point cloud topic");
     return false;
   }
 
@@ -156,7 +156,11 @@ bool ExtrinsicMapBasedCalibrator::mapBasedCalibration(const tf2::Transform & tf_
     }
   }
   if (sensor_pointcloud_msg_->height == 0) {
-    RCLCPP_ERROR(this->get_logger(), "Can not received pandar left upper point cloud topic");
+    RCLCPP_ERROR(this->get_logger(), "Can not received sensor point cloud topic height = 0");
+    return false;
+  }
+  if (sensor_pointcloud_msg_->width == 0) {
+    RCLCPP_ERROR(this->get_logger(), "Can not received sensor point cloud topic width = 0");
     return false;
   }
 
@@ -167,12 +171,16 @@ bool ExtrinsicMapBasedCalibrator::mapBasedCalibration(const tf2::Transform & tf_
     if (!preprocessing(pcl_map, pcl_map_without_wall, pcl_sensor, tf_initial_pose)) {
       return false;
     }
-    grid_search_matching_.executeGridSearchMatching(pcl_map_without_wall, pcl_sensor);
+    if(!grid_search_matching_.executeGridSearchMatching(pcl_map_without_wall, pcl_sensor)){
+      return false;
+    }
   } else {
     if (!preprocessing(pcl_map, pcl_sensor, tf_initial_pose)) {
       return false;
     }
-    grid_search_matching_.executeGridSearchMatching(pcl_map, pcl_sensor);
+    if(!grid_search_matching_.executeGridSearchMatching(pcl_map, pcl_sensor)){
+      return false;
+    }
   }
 
   calibrated_sensor_result_ = grid_search_matching_.getRematchedResult();

diff --git a/sensor/extrinsic_map_based_calibrator/src/extrinsic_map_based_preprocessing.cpp b/sensor/extrinsic_map_based_calibrator/src/extrinsic_map_based_preprocessing.cpp
@@ -90,6 +90,9 @@ bool ExtrinsicMapBasedPreprocessing::extractGroundPlane(
 
   // Obtain an idea of the ground plane using PCA
   // under the asumption that the axis with less variance will be the ground plane normal
+  if(pointcloud->size() < 3){
+    return false;
+  }
   pcl::PCA<pcl::PointXYZ> pca;
   pca.setInputCloud(pointcloud);
   Eigen::MatrixXf vectors = pca.getEigenVectors();

diff --git a/sensor/extrinsic_map_based_calibrator/src/grid_search_matching.cpp b/sensor/extrinsic_map_based_calibrator/src/grid_search_matching.cpp
@@ -24,10 +24,10 @@ GridSearchMatching::GridSearchMatching() {}
 bool GridSearchMatching::executeGridSearchMatching(
   const PointCloudT::Ptr & map_pointcloud, const PointCloudT::Ptr & sensor_pointcloud)
 {
-  if (map_pointcloud == 0 || map_pointcloud->height == 0) {
+  if (map_pointcloud == 0 || map_pointcloud->height == 0|| map_pointcloud->width == 0) {
     std::cerr << "Map point cloud is empty" << std::endl;
     return false;
-  } else if (sensor_pointcloud == 0 || sensor_pointcloud->height == 0) {
+  } else if (sensor_pointcloud == 0 || sensor_pointcloud->height == 0||sensor_pointcloud->width == 0) {
     std::cerr << "Sensor point cloud is empty" << std::endl;
     return false;
   }