MoveIT2 segfaults when adding a Mesh Collision object.

edit

I use tetgen to create an unbounded mesh. For now the points are manually specified:

            std::vector <geometry_msgs::msg::Point> meshPoints;
            geometry_msgs::msg::Point p1;
            p1.x = 0.652228;
            p1.y = -0.897207;
            p1.z =  1.16027;

            geometry_msgs::msg::Point p2;
            p2.x = 0.762599;
            p2.y = -0.897123;
            p2.z =  1.16035;

            geometry_msgs::msg::Point p3;
            p3.x = 0.762537;
            p3.y = -0.996545;
            p3.z =  1.16031;

            geometry_msgs::msg::Point p4;
            p4.x = 0.669485;
            p4.y = -0.996568;
            p4.z =  1.1602;

            geometry_msgs::msg::Point p5;
            p5.x = 0.669378;
            p5.y = -0.996639;
            p5.z =  1.22961;

            geometry_msgs::msg::Point p6;
            p6.x = 0.669379;
            p6.y = -0.899267;
            p6.z =   1.22973;

            geometry_msgs::msg::Point p7;
            p7.x = 0.762669;
            p7.y = -0.89918;
            p7.z =  1.22977;

            geometry_msgs::msg::Point p8;
            p8.x = 0.762651;
            p8.y = -0.937259;
            p8.z =  1.22973;
            meshPoints.push_back(p1);
            meshPoints.push_back(p2);
            meshPoints.push_back(p3);
            meshPoints.push_back(p4);
            meshPoints.push_back(p5);
            meshPoints.push_back(p6);
            meshPoints.push_back(p7);
            meshPoints.push_back(p8);

I then create a Collision object and attempt to add it to the scene:

            MeshBuilder m(meshPoints);
            shape_msgs::msg::Mesh mesh = m.getMesh();
            moveit_msgs::msg::CollisionObject collision_object;
            collision_object.id = "my_mesh_collision_object";
            collision_object.header.frame_id = move_group_->getPlanningFrame();
            collision_object.meshes.push_back(mesh);
            collision_object.mesh_poses.push_back(geometry_msgs::msg::Pose());
            collision_object.operation = moveit_msgs::msg::CollisionObject::ADD;
            moveit::planning_interface::PlanningSceneInterface planning_scene_interface;
            planning_scene_interface.applyCollisionObject(collision_object);

However, as a result of this, MoveIt completely loses it and segfaults. The output is as follows:

[move_group-1] #9    Object "/home/max/ws_moveit2/install/moveit_ros_move_group/lib/libmoveit_move_group_default_capabilities.so.2.5.4", at 0x7fd67ed0dab3, in rclcpp::Service<moveit_msgs::srv::ApplyPlanningScene>::handle_request(std::shared_ptr<rmw_request_id_s>, std::shared_ptr<void>)
[move_group-1] #8    Object "/home/max/ws_moveit2/install/moveit_ros_move_group/lib/libmoveit_move_group_default_capabilities.so.2.5.4", at 0x7fd67ed0d64f, in rclcpp::AnyServiceCallbac
k<moveit_msgs::srv::ApplyPlanningScene>::dispatch(std::shared_ptr<rclcpp::Service<moveit_msgs::srv::ApplyPlanningScene> > const&, std::shared_ptr<rmw_request_id_s> const&, std::shared_ptr<moveit_msgs::srv::ApplyPlanningScene_Request_<std::allocator<void> > >)
[move_group-1] #7    Object "/home/max/ws_moveit2/install/moveit_ros_move_group/lib/libmoveit_move_group_default_capabilities.so.2.5.4", at 0x7fd67ed071a3, in move_group::ApplyPlanning
SceneService::applyScene(std::shared_ptr<rmw_request_id_s> const&, std::shared_ptr<moveit_msgs::srv::ApplyPlanningScene_Request_<std::allocator<void> > > const&, std::shared_ptr<moveit_msgs::srv::ApplyPlanningScene_Response_<std::allocator<void> > > const&)
[move_group-1] #6    Object "/home/max/ws_moveit2/install/moveit_ros_planning/lib/libmoveit_planning_scene_monitor.so.2.5.4", at 0x7fd6b27d7fdc, in planning_scene_monitor::PlanningSceneMonitor::newPlanningSceneMessage(moveit_msgs::msg::PlanningScene_<std::allocator<void> > const&)
[move_group-1] #5    Object "/home/max/ws_moveit2/install/moveit_core/lib/libmoveit_planning_scene.so.2.5.4", at 0x7fd6b1b1a6b9, in planning_scene::PlanningScene::setPlanningSceneDiffMsg(moveit_msgs::msg::PlanningScene_<std::allocator<void> > const&)
[move_group-1] #4    Object "/home/max/ws_moveit2/install/moveit_core/lib/libmoveit_planning_scene.so.2.5.4", at 0x7fd6b1b19887, in planning_scene::PlanningScene::processCollisionObjectAdd(moveit_msgs::msg::CollisionObject_<std::allocator<void> > const&)
[move_group-1] #3    Object "/home/max/ws_moveit2/install/moveit_core/lib/libmoveit_planning_scene.so.2.5.4", at 0x7fd6b1b092b6, in planning_scene::PlanningScene::shapesAndPosesFromCol
lisionObjectMessage(moveit_msgs::msg::CollisionObject_<std::allocator<void> > const&, Eigen::Transform<double, 3, 1, 0>&, std::vector<std::shared_ptr<shapes::Shape const>, std::allocator<std::shared_ptr<shapes::Shape const> > >&, std::vector<Eigen::Transform<double, 3, 1, 0>, Eigen::aligned_allocator<Eigen::Transform<double, 3, 1, 0> > >&)
[move_group-1] #2    Object "/opt/ros/humble/lib/libgeometric_shapes.so.2.1 ...