如何在moveit环境中导入stl文件

moveit官网给的示例中有讲解如何在moveit环境中加入自定义障碍Planning Scene ROS API — moveit_tutorials Noetic documentation (ros-planning.github.io)，

但是并没有讲怎么添加stl文件这种，而且moveit的官网不像opencv那种那样对每个函数的讲解特别特别详细，所以可能看起来比较麻烦，我整理了一个用Planning Scene ROS API实现的把stl导入自己的运动规划组中去。

#include <iostream>
#include <ros/ros.h>
#include <moveit/move_group_interface/move_group_interface.h>
#include <geometry_msgs/Pose.h>
#include <tf/LinearMath/Quaternion.h>
#include <moveit/planning_scene_interface/planning_scene_interface.h>
#include <geometric_shapes/shape_operations.h>

using namespace std;

int main(int argc, char **argv) {
    // 初始化节点
    string nodeName = "aubo5_scene_node";
    ros::init(argc, argv, nodeName);
    ros::NodeHandle node;
    ros::AsyncSpinner spinner(1);
    spinner.start();

    moveit::planning_interface::MoveGroupInterface group("cr20");
    moveit::planning_interface::PlanningSceneInterface scene;

    // 加载STL文件并创建网格
    shapes::Mesh* m = shapes::createMeshFromResource("file:///home/robot/场景1.stl");
    shape_msgs::Mesh mesh;
    shapes::ShapeMsg mesh_msg;
    shapes::constructMsgFromShape(m, mesh_msg);
    mesh = boost::get<shape_msgs::Mesh>(mesh_msg);

    // 创建碰撞对象
    moveit_msgs::CollisionObject collision_object;
    collision_object.header.frame_id = group.getPlanningFrame();
    collision_object.id = "my_mesh_object";
    collision_object.meshes.push_back(mesh);

    // 设置对象的位置和姿态
    geometry_msgs::Pose mesh_pose;
    mesh_pose.orientation.w = 1.0;
    mesh_pose.position.x = 0.1;
    mesh_pose.position.y = 0.2;
    mesh_pose.position.z = 0.3;
    collision_object.mesh_poses.push_back(mesh_pose);

    collision_object.operation = collision_object.ADD;

    // 在场景中添加障碍物
    std::vector<moveit_msgs::CollisionObject> collision_objects;
    collision_objects.push_back(collision_object);
    scene.applyCollisionObjects(collision_objects);

    ros::waitForShutdown();
    return 0;
}