【MoveIt!】MoveIt!环境导入自定义的stl文件

MoveIt!环境导入自定义的stl文件

MoveIt! 官方教程里有从C++代码生成一个物体，可以用来抓取或者避障，但是只能定义一些简单的几何体，如何导入自己的stl文件呢，参考代码。本文截取部分如下：

相关的头文件

#include <geometric_shapes/shapes.h>
#include <geometric_shapes/shape_messages.h>
#include <geometric_shapes/shape_operations.h>
#include <geometric_shapes/mesh_operations.h>
           

相关代码

void AddObjects::addShelf(){
    // put in collision avoidance data
    moveit_msgs::CollisionObject collision_object;
    collision_object.header.frame_id = arm.getPlanningFrame();

    /* The id of the object is used to identify it. */
    collision_object.id = "shelf";
    // 定义一个mesh文件指针，从createMeshFromResource的函数名可知，将由一个文件生成我们的mesh物体
    // 此处的路径必须是ROS中可以找到的包路径
    shapes::Mesh* m = shapes::createMeshFromResource("package://apc/collision_objects/pod_lowres.stl");
    shape_msgs::Mesh shelf_mesh;
    shapes::ShapeMsg shelf_mesh_msg;
    // 从mesh文件指针创建mesh的msg
    shapes::constructMsgFromShape(m,shelf_mesh_msg);
    shelf_mesh = boost::get<shape_msgs::Mesh>(shelf_mesh_msg);

    /* A pose for the box (specified relative to frame_id) */
    geometry_msgs::Pose shelf_pose;
    shelf_pose.orientation.w = ;
    shelf_pose.orientation.x = ;
    shelf_pose.orientation.y = ;
    shelf_pose.orientation.z = ;
    shelf_pose.position.x =  ;
    shelf_pose.position.y =  ;
    shelf_pose.position.z =  -;

    collision_object.meshes.push_back(shelf_mesh);
    collision_object.mesh_poses.push_back(shelf_pose);
    collision_object.operation = collision_object.ADD;

    ROS_INFO("Add an shelf into the world");

    moveit_msgs::PlanningScene planning_scene;
    planning_scene.world.collision_objects.push_back(collision_object);
    planning_scene.is_diff = true;
    // 此处发布到MoveIt!的场景中才可以进行处理
    planning_scene_diff_publisher.publish(planning_scene);
}
           

深入了解

下面我们再看一下moveit_msgs/CollisionObject.msg

collision object属性很多，包括类型、尺寸、位置、操作等，primitives和primitive_poses就是我们用的比较多的定义基本形状尺寸和位置，meshes和mesh_poses则通过某些方法生成，生成方式很多，具体可以看API函数。

Raw Message Definition
# a header, used for interpreting the poses
Header header

# 此处的id就是我们在程序中操作物体所需要的id
# the id of the object (name used in MoveIt)
string id

# 程序中定义的基本形状尺寸和位置
# The object type in a database of known objects
object_recognition_msgs/ObjectType type

# the the collision geometries associated with the object;
# their poses are with respect to the specified header

# solid geometric primitives
# 形状尺寸
shape_msgs/SolidPrimitive[] primitives
# 位置
geometry_msgs/Pose[] primitive_poses

# meshes mesh文件
shape_msgs/Mesh[] meshes
geometry_msgs/Pose[] mesh_poses

# bounding planes (equation is specified, but the plane can be oriented using an additional pose)
shape_msgs/Plane[] planes
geometry_msgs/Pose[] plane_poses

# Adds the object to the planning scene. If the object previously existed, it is replaced.
byte ADD=

# Removes the object from the environment entirely (everything that matches the specified id)
byte REMOVE=

# Append to an object that already exists in the planning scene. If the does not exist, it is added.
byte APPEND=

# If an object already exists in the scene, new poses can be sent (the geometry arrays must be left empty)
# if solely moving the object is desired
byte MOVE=

# Operation to be performed
byte operation