Skip to content

Commit

Permalink
feat: skip validation when obstacle pointcloud is empty
Browse files Browse the repository at this point in the history
Signed-off-by: yoshiri <yoshiyoshidetteiu@gmail.com>
  • Loading branch information
YoshiRi committed Mar 25, 2024
1 parent 7081a61 commit ab8f367
Showing 1 changed file with 8 additions and 3 deletions.
Original file line number Diff line number Diff line change
Expand Up @@ -326,15 +326,20 @@ void ObstaclePointCloudBasedValidator::onObjectsAndObstaclePointCloud(
// objects_pub_->publish(*input_objects);
return;
}
bool validation_is_ready = true;
if (!validator_->setKdtreeInputCloud(input_obstacle_pointcloud)) {
RCLCPP_WARN_THROTTLE(this->get_logger(), *this->get_clock(), 5, "cannot receive pointcloud");
return;
RCLCPP_WARN_THROTTLE(
this->get_logger(), *this->get_clock(), 5,
"obstacle pointcloud is empty! Can not validate objects.");
validation_is_ready = false;
}

for (size_t i = 0; i < transformed_objects.objects.size(); ++i) {
const auto & transformed_object = transformed_objects.objects.at(i);
const auto & object = input_objects->objects.at(i);
const auto validated = validator_->validate_object(transformed_object);
const auto validated =
validation_is_ready ? validator_->validate_object(transformed_object) : false;
validator_->validate_object(transformed_object);
if (debugger_) {
debugger_->addNeighborPointcloud(validator_->getDebugNeighborPointCloud());
debugger_->addPointcloudWithinPolygon(validator_->getDebugPointCloudWithinObject());
Expand Down

0 comments on commit ab8f367

Please sign in to comment.