moveit里机械臂如何在RVIZ内通过代码的形式添加模型

传统模型在RVIZ内添加模型

传统模式下在rviz里添加模型，需要在Scene Objects下手动的选择，这样做也可以达成想要的目的。比如导入立方体，圆柱体，圆锥，也可以导入自己设计的模型，以dae文件的方式。如下图所示。

代码形式在RVIZ内添加模型

参考moveit官网给的代码，在moveit_tutorials文件夹—doc文件夹–pick_place文件夹里，官方给了一个例子如何在rviz内添加模型，但模型的种类仅限于正方体，圆柱体，圆锥之类，无法利用该例子直接添加自定义的dae文件到rviz内。于是，在整合了各方资源后打算记录一下这个过程。

废话不多说，直接进入主题

强调下，程序依照moveit官方的pick_place_tutorial.cpp进行改动。

1.添加头文件

#include <moveit_msgs/DisplayRobotState.h>
#include <moveit_msgs/DisplayTrajectory.h>
#include <moveit_msgs/AttachedCollisionObject.h>
#include <moveit_msgs/CollisionObject.h>
#include <moveit_msgs/PlanningScene.h>

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

别问我 添加这些头文件干嘛的，问我也不知道。

2. 选择添加想要的dae或者STL文件

比如 我选择了Link_1.STL这个文件，我就想在rviz里添加我自定义的一个桌子。
为什么要把这个文件放在这个位置，因为下面程序需要找到这个STL文件的路径，才能将文件导入rviz里。
那路径如何确定呢
打开这个文件下，原来机械臂的urdf文件，它已经帮你配置好了，你只需要cv一下就好了。
如下图所示，就是urdf下Link_1.STL
的位置。

3. 编写程序

void addShelf(){
	moveit_msgs::CollisionObject collision_object;
  	ros::NodeHandle nh;
	collision_object.header.frame_id = "Link_0";
  	ros::Publisher planning_scene_diff_publisher;

  	planning_scene_diff_publisher = nh.advertise<moveit_msgs::PlanningScene>("planning_scene", 1);
	while(planning_scene_diff_publisher.getNumSubscribers() < 1)
	{
	  ros::WallDuration sleep_t(0.5);
	  sleep_t.sleep();
	}
	/* The id of the object is used to identify it. */
	collision_object.id = "shelf";

// /home/tza/jaka_ws/src/jaka_robot_v2.2/src/jaka_description/meshes/jaka_zu5_meshes/Link_1.STL

	shapes::Mesh* m = shapes::createMeshFromResource("package://jaka_description/meshes/jaka_zu5_meshes/Link_1.STL");
	shape_msgs::Mesh shelf_mesh;
	shapes::ShapeMsg shelf_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 = 0.5;
	// shelf_pose.orientation.x = 0;
	// shelf_pose.orientation.y = 0;
	// shelf_pose.orientation.z = 0;
	shelf_pose.position.x =  1.0;
	shelf_pose.position.y =  0.0;
	shelf_pose.position.z =  0;

	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;
	planning_scene_diff_publisher.publish(planning_scene);
}

在main（）函数里执行这个api函数。！！！

4.结果演示

rviz里显示了我们的机械臂的关节1。