hpp-fcl 凸几何体碰撞检测返回的结果错误

[chendaoming99]

相同的问题，我在 github 上也提交了 issue

我正在使用与 ROS Humble 和 Ubuntu 22.04 兼容的hpp-fcl，版本2.4.0。

C++ 版本和 Python 版本都是通过apt install ros-humble-hpp-fcl命令安装的。

我正在尝试测试一个箱子和一个从.obj文件加载的凸几何体之间的碰撞检测。

这是我的凸几何体的 .obj:

你可以从这里下载到该 .obj 文件 here

我首先写了一个Python程序，它运行得很好。你可以使用"w, s, a, d, q, e"键来移动箱子，当箱子进入凸形内部时，它会打印出"collision detected"（碰撞检测到）

<details>
<summary>here is my python code</summary>

import numpy as np
import trimesh
import rclpy
import hppfcl as fcl
from pynput import keyboard

from rclpy.node import Node
from rclpy.qos import QoSProfile, DurabilityPolicy
from visualization_msgs.msg import Marker

class CollisionTest(Node):
    def __init__(self):
        super().__init__('collision_test')
        # config qos profile
        qos_profile = QoSProfile(depth=10)
        qos_profile.durability = DurabilityPolicy.TRANSIENT_LOCAL

        self.box_publisher = self.create_publisher(Marker, 'box', 10)
        self.env_publisher = self.create_publisher(Marker, 'env', qos_profile)

        self.timer = self.create_timer(0.1, self.timer_callback)

        self.box_position = np.array([0.0, 0.0, 0.0])
        self.step = 0.03

        self.init_box()
        self.init_env()

        # Setup keyboard listener
        self.listener = keyboard.Listener(on_press=self.on_press)
        self.listener.start()

    def init_box(self):
        # Initialize box collision object
        box_shape = fcl.Box(0.1, 0.1, 0.1)
        self.box_obj = fcl.CollisionObject(box_shape)

        # Initialize box marker
        self.box_marker = Marker()
        self.box_marker.header.frame_id = 'map'
        self.box_marker.ns = 'collision_test'
        self.box_marker.id = 0
        self.box_marker.type = Marker.CUBE
        self.box_marker.action = Marker.ADD
        self.box_marker.scale.x = 0.1
        self.box_marker.scale.y = 0.1
        self.box_marker.scale.z = 0.1
        self.box_marker.color.a = 1.0
        self.box_marker.color.r = 0.0
        self.box_marker.color.g = 1.0
        self.box_marker.color.b = 0.0

    def init_env(self):
        file_path = "/path/to/convex_hull.obj"

        scale = 1.0

        # Publish markers
        env_marker = Marker()
        env_marker.header.frame_id = 'map'
        env_marker.ns = 'collision_test'
        env_marker.id = 0
        env_marker.type = Marker.MESH_RESOURCE
        env_marker.mesh_resource = "file://" + file_path
        env_marker.action = Marker.ADD
        env_marker.scale.x = scale
        env_marker.scale.y = scale
        env_marker.scale.z = scale
        env_marker.color.r = 0.1
        env_marker.color.g = 0.0
        env_marker.color.b = 0.7
        env_marker.color.a = 0.3

        self.env_publisher.publish(env_marker)

        # Initialize environment collision object
        mesh = trimesh.load_mesh(file_path)
        mesh.apply_scale(scale)

        verts = fcl.StdVec_Vec3f()
        faces = fcl.StdVec_Triangle()

        verts.extend(mesh.vertices)
        for f in mesh.faces:
            faces.append(fcl.Triangle(int(f[0]), int(f[1]), int(f[2])))

        convex = fcl.Convex(verts, faces)
        self.env_objs = [fcl.CollisionObject(convex)]


    def move_box(self):
        # Move the box marker
        self.box_marker.pose.position.x = self.box_position[0]
        self.box_marker.pose.position.y = self.box_position[1]
        self.box_marker.pose.position.z = self.box_position[2]

        self.box_publisher.publish(self.box_marker)

        # Move the box collision object
        transform = fcl.Transform3f()
        transform.setTranslation(self.box_position)
        self.box_obj.setTransform(transform)

        # Check collision
        for env_obj in self.env_objs:
            if self.check_collision(self.box_obj, env_obj):
                self.get_logger().info('Collision detected!')
                break

    def check_collision(self, obj1, obj2):
        request = fcl.CollisionRequest()
        result = fcl.CollisionResult()
        fcl.collide(obj1, obj2, request, result)
        return result.isCollision()

    def on_press(self, key):
        try:
            if key.char == 'w':
                self.box_position[1] += self.step
            elif key.char == 's':
                self.box_position[1] -= self.step
            elif key.char == 'a':
                self.box_position[0] -= self.step
            elif key.char == 'd':
                self.box_position[0] += self.step
            elif key.char == 'q':
                self.box_position[2] += self.step
            elif key.char == 'e':
                self.box_position[2] -= self.step
        except AttributeError:
            pass

    def timer_callback(self):
        self.move_box()

def main(args=None):
    print(f"hpp-fcl version: {fcl.__version__}")
    rclpy.init(args=args)
    node = CollisionTest()
    rclpy.spin(node)
    rclpy.shutdown()

if __name__ == '__main__':
    main()

</details>

然而，当我尝试用C++重写代码时，事情变得有些奇怪。它在错误的地方打印出“collision detection”（碰撞检测）。

这里有一个短视频来展示这个问题：

https://github.com/humanoid-path-planner/hpp-fcl/assets/69784031/efc4783c-fbb8-4476-acb7-0e45e3930c60

<details>
<summary>here is my c++ code</summary>

#include <Eigen/Geometry>
#include <termios.h>
#include <unistd.h>
#include <sys/select.h>
#include <fstream>

#include <rclcpp/rclcpp.hpp>
#include <visualization_msgs/msg/marker.hpp>
#include <geometry_msgs/msg/point.hpp>

#include <hpp/fcl/collision.h>
#include <hpp/fcl/distance.h>
#include <hpp/fcl/shape/convex.h>
#include <hpp/fcl/shape/geometric_shapes.h>
#include <hpp/fcl/mesh_loader/loader.h>
#include <hpp/fcl/BVH/BVH_model.h>

#include <assimp/Importer.hpp>
#include <assimp/scene.h>
#include <assimp/postprocess.h>

namespace fcl = hpp::fcl;
using Convex_t = fcl::Convex<fcl::Triangle>;

class CollisionTest : public rclcpp::Node {
 private:
  rclcpp::Publisher<visualization_msgs::msg::Marker>::SharedPtr box_publisher_;
  rclcpp::Publisher<visualization_msgs::msg::Marker>::SharedPtr env_publisher_;
  rclcpp::TimerBase::SharedPtr timer_;
  Eigen::Vector3d box_position_ = Eigen::Vector3d::Zero();
  double step = 0.03;

  visualization_msgs::msg::Marker box_marker_;
  fcl::CollisionObject box_obj_{ nullptr };
  std::vector<fcl::CollisionObject> env_objs_{};

  void init_collision_objs(aiNode* node, const aiScene* scene, visualization_msgs::msg::Marker& marker) {
    double scale_factor = 1;

    for (u_int i = 0; i < node->mNumMeshes; i++) {
      aiMesh* mesh = scene->mMeshes[node->mMeshes[i]];

      fcl::Vec3f* vertices = new fcl::Vec3f[mesh->mNumVertices];

      for (u_int j = 0; j < mesh->mNumVertices; j++) {
        double x = mesh->mVertices[j].x * scale_factor;
        double y = mesh->mVertices[j].y * scale_factor;
        double z = mesh->mVertices[j].z * scale_factor;
        vertices[j] = fcl::Vec3f(x, y, z);
      }

      fcl::Triangle* triangles = new fcl::Triangle[mesh->mNumFaces];
      for (u_int j = 0; j < mesh->mNumFaces; j++) {
        aiFace face = mesh->mFaces[j];
        triangles[j] = fcl::Triangle(face.mIndices[0], face.mIndices[1], face.mIndices[2]);

        // add points to marker
        for (int k = 0; k < 3; k++) {
          geometry_msgs::msg::Point p;
          p.x = vertices[face.mIndices[k]].x();
          p.y = vertices[face.mIndices[k]].y();
          p.z = vertices[face.mIndices[k]].z();
          marker.points.push_back(p);
        }
      }

      // auto convex =
      //     std::make_shared<Convex_t>(true, vertices, mesh->mNumVertices, triangles, mesh->mNumFaces);
      auto convex = std::shared_ptr<Convex_t>(
          new Convex_t(true, vertices, mesh->mNumVertices, triangles, mesh->mNumFaces));

      // auto tf = fcl::Transform3f::Identity();
      fcl::CollisionObject obj(convex);
      env_objs_.emplace_back(obj);
    }

    for (unsigned int i = 0; i < node->mNumChildren; i++) {
      init_collision_objs(node->mChildren[i], scene, marker);
    }
  }

  void init_env() {
    std::string file_path = "/home/developer/dd6ax/src/dd6ax_utils/test/dragon0_convex_hull.obj";

    // double angle_in_degrees = 90.0;
    // Eigen::AngleAxisd rotation_axis(angle_in_degrees * M_PI / 180.0, Eigen::Vector3d::UnitX());
    // Eigen::Quaterniond rotation_quaternion = Eigen::Quaterniond(rotation_axis);
    // rotation_quaternion.normalize();

    Assimp::Importer importer;
    const aiScene* scene = importer.ReadFile(file_path, aiProcess_Triangulate | aiProcess_FlipUVs);
    if (!scene || scene->mFlags & AI_SCENE_FLAGS_INCOMPLETE || !scene->mRootNode) {
      std::cerr << "Error::Assimp:: " << importer.GetErrorString() << std::endl;
      return;
    }

    visualization_msgs::msg::Marker marker;
    marker.header.frame_id = "map";
    marker.header.stamp = this->now();
    marker.ns = "collision_test";
    marker.id = 0;
    marker.type = visualization_msgs::msg::Marker::TRIANGLE_LIST;
    // marker.type = visualization_msgs::msg::Marker::MESH_RESOURCE;
    // marker.mesh_resource = "file://" + file_path;
    marker.action = visualization_msgs::msg::Marker::ADD;

    marker.scale.x = 1.0;
    marker.scale.y = 1.0;
    marker.scale.z = 1.0;

    marker.color.r = 0.1;
    marker.color.g = 0.0;
    marker.color.b = 0.7;
    marker.color.a = 0.3;

    init_collision_objs(scene->mRootNode, scene, marker);

    RCLCPP_INFO(this->get_logger(), "Publishing marker with %zu points", marker.points.size());
    env_publisher_->publish(std::move(marker));
  }

  void init_box() {
    // init box collision object
    box_obj_ =
        fcl::CollisionObject(std::make_shared<fcl::Box>(0.1, 0.1, 0.1), fcl::Transform3f::Identity());

    // init box marker
    box_marker_.header.frame_id = "map";
    box_marker_.header.stamp = this->now();
    box_marker_.ns = "collision_test";
    box_marker_.id = 0;
    box_marker_.type = visualization_msgs::msg::Marker::CUBE;
    box_marker_.action = visualization_msgs::msg::Marker::ADD;
    box_marker_.pose.orientation.x = 0.0;
    box_marker_.pose.orientation.y = 0.0;
    box_marker_.pose.orientation.z = 0.0;
    box_marker_.pose.orientation.w = 1.0;
    box_marker_.scale.x = 0.1;
    box_marker_.scale.y = 0.1;
    box_marker_.scale.z = 0.1;
    box_marker_.color.a = 1.0;
    box_marker_.color.r = 0.0;
    box_marker_.color.g = 1.0;
    box_marker_.color.b = 0.0;
  }

  void get_box_position() {
    fd_set readfds;
    FD_ZERO(&readfds);
    FD_SET(STDIN_FILENO, &readfds);

    // Wait up to 20 milliseconds for input
    struct timeval timeout = { 0, 20 * 1000 };
    int result = select(STDIN_FILENO + 1, &readfds, NULL, NULL, &timeout);

    if (result <= 0) {
      return;
    }

    if (!FD_ISSET(STDIN_FILENO, &readfds)) {
      return;
    }

    char c;
    if (read(STDIN_FILENO, &c, 1) <= 0) {
      return;
    }

    switch (c) {
      case 'w':
        box_position_.y() += step;
        break;
      case 's':
        box_position_.y() -= step;
        break;
      case 'a':
        box_position_.x() -= step;
        break;
      case 'd':
        box_position_.x() += step;
        break;
      case 'q':
        box_position_.z() += step;
        break;
      case 'e':
        box_position_.z() -= step;
        break;
      default:
        break;
    }
  }

  void move_box() {
    // move the box marker
    box_marker_.pose.position.x = box_position_.x();
    box_marker_.pose.position.y = box_position_.y();
    box_marker_.pose.position.z = box_position_.z();

    box_publisher_->publish(box_marker_);

    // move the box collision object
    auto fcl_tf = fcl::Transform3f::Identity();
    fcl_tf.setTranslation(box_position_);
    box_obj_.setTransform(fcl_tf);

    // check collision
    for (const auto& env_obj : env_objs_) {
      if (check_collision(box_obj_, env_obj)) {
        RCLCPP_INFO(this->get_logger(), "Collision detected!");
      }
      break;
    }
    // double distance = get_distance(box_obj_, env_obj);
    // RCLCPP_INFO(this->get_logger(), "Distance: %f", distance);
  }

  bool check_collision(fcl::CollisionObject obj1, fcl::CollisionObject obj2) {
    fcl::CollisionRequest request;
    fcl::CollisionResult result;
    fcl::collide(&obj1, &obj2, request, result);
    return result.isCollision();
  }

  double get_distance(fcl::CollisionObject obj1, fcl::CollisionObject obj2) {
    fcl::DistanceRequest request;
    fcl::DistanceResult result;
    fcl::distance(&obj1, &obj2, request, result);
    return result.min_distance;
  }

  void timer_callback() {
    get_box_position();

    move_box();
  }

 public:
  CollisionTest() : Node("collision_test") {
    // set the terminal to unbuffered input
    struct termios tty;
    memset(&tty, 0, sizeof tty);
    if (tcgetattr(STDIN_FILENO, &tty) != 0) {
      RCLCPP_ERROR(this->get_logger(), "Error from tcgetattr");
    }
    tty.c_lflag &= ~ICANON;
    tty.c_lflag &= ~ECHO;
    tty.c_cc[VMIN] = 1;   // Minimum of one character for getc
    tty.c_cc[VTIME] = 0;  // Wait indefinetly
    if (tcsetattr(STDIN_FILENO, TCSANOW, &tty) != 0) {
      RCLCPP_ERROR(this->get_logger(), "Error from tcsetattr");
    }

    // init publishers and timer
    box_publisher_ = create_publisher<visualization_msgs::msg::Marker>("box", 10);
    init_box();
    timer_ = create_wall_timer(std::chrono::milliseconds(100),
                               std::bind(&CollisionTest::timer_callback, this));

    env_publisher_ = create_publisher<visualization_msgs::msg::Marker>(
        "env", rclcpp::QoS(rclcpp::KeepLast(10)).transient_local());

    init_env();
  }

  ~CollisionTest() {
    // reset the terminal to buffered input
    struct termios tty;
    tcgetattr(STDIN_FILENO, &tty);
    tty.c_lflag |= ICANON;
    tty.c_lflag |= ECHO;
    tcsetattr(STDIN_FILENO, TCSANOW, &tty);
  };
};

int main(int argc, char* argv[]) {
  std::cout << "HPP-FCL version is: " << HPP_FCL_VERSION << std::endl;
  rclcpp::init(argc, argv);
  rclcpp::spin(std::make_shared<CollisionTest>());
  rclcpp::shutdown();

  return 0;
}

</details>

我将init_collision_objs()函数写成递归函数的原因是，我将来会处理一个.obj文件中的一组几何体。凸形的顶点是正确的，因为TRIANGLE_LIST可视化标记正常工作。

感谢大佬们的帮助