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。

评论 2
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值