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;
}