moveit环境引入代码

moveit环境引入:

代码:


#include <iostream>
#include <ros/ros.h>//moveit环境引入
#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>using namespace std;double deg2rad(double deg) {return deg * M_PI / 180.0;
}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("manipulator_i5");//场景管理器moveit::planning_interface::PlanningSceneInterface scene;//障碍物集合std::vector<moveit_msgs::CollisionObject> objects;//单个障碍物moveit_msgs::CollisionObject obj;//操作类型obj.operation = obj.ADD;obj.id = "box";obj.header.frame_id = group.getPlanningFrame();//坐标系//形状shape_msgs::SolidPrimitive shape;shape.type = shape.BOX;//盒子形状shape.dimensions.push_back(1);shape.dimensions.push_back(2);shape.dimensions.push_back(3);obj.primitives.push_back(shape);//位置和姿态geometry_msgs::Pose pose;pose.position.x = 0.1;pose.position.y = 0.2;pose.position.z = 0.3;// 姿态(四元素)(欧拉角转四元素)tf::Quaternion quat;quat.setRPY(deg2rad(0), deg2rad(0), deg2rad(0));pose.orientation.x = quat.x();pose.orientation.y = quat.y();pose.orientation.z = quat.z();pose.orientation.w = quat.w();obj.primitive_poses.push_back(pose);objects.push_back(obj);std::vector<moveit_msgs::ObjectColor> colors;moveit_msgs::ObjectColor color;color.id = "box";// [0,255] int, float32 [0, 1]color.color.r = 255 / 255.0;color.color.g = 0;color.color.b = 0;color.color.a = 1;colors.push_back(color);//在场景中添加障碍物
//    scene.addCollisionObjects(objects);scene.applyCollisionObjects(objects, colors);//阻塞ros::waitForShutdown();return 0;
}

moveit环境引入代码相关推荐

python软件包自带的集成开发环境-Python的10大集成开发环境和代码编辑器（指南）...
使用IDLE或者Python Shell来编写Python是非常适合于简单程序的,但是这些工具往往将大型的编程项目变成一个个充满绝望和沮丧的"坑".使用一款集成开发环境甚至是一款好 ...
python自带的集成开发环境是什么-Python的10大集成开发环境和代码编辑器（指南）...
使用IDLE或者Python Shell来编写Python是非常适合于简单程序的,但是这些工具往往将大型的编程项目变成一个个充满绝望和沮丧的"坑".使用一款集成开发环境甚至是一款好 ...
linux vim ctags,Linux环境上代码阅读与编写的利器-vim+ctags+cscope
Linux环境下代码阅读与编写的利器----vim+ctags+cscope 所谓工欲善其事,必先利其器. 从事Linux程序开发,特别是Linux驱动程序的开发,不管是通过windows下虚拟一个L ...
markdown引入代码_第 09 篇：让博客支持 Markdown 语法和代码高亮
作者:HelloGitHub-追梦人物文中涉及的示例代码,已同步更新到HelloGitHub-Team 仓库[1] 为了让博客文章具有良好的排版,显示更加丰富的格式,我们使用 Markdown 语法 ...
QT与VS环境下代码文件编码格式使用utf-8与带BOM的utf-8的差异
QT与VS环境下代码文件编码格式使用utf-8与带BOM的utf-8的差异使用QT进行开发的时候,有时候通常会遇到使用中文的时候,就算格式为utf-8,也会出现编译错误的问题,一般无从下手,这几天看 ...
入职开发很少写代码_如何简化开发人员入职：将开发环境作为代码
入职开发很少写代码 Imagine that only a decade ago system administrators deployed, configured, and maintained ...
Python的10大集成开发环境和代码编辑器（指南）
分享一篇文章,原文链接:Python IDEs and Code Editors (Guide). 使用IDLE或者Python Shell来编写Python是非常适合于简单程序的,但是这些工具往往将 ...
Win32环境下代码注入与API钩子的实现
Win32环境下代码注入与API钩子的实现本文详细的介绍了在Visual Studio(以下简称VS)下实现API钩子的编程方法,阅读本文需要基础:有操作系统的基本知识(进程管理,内存管理),会在V ...
JavaScript组件之JQuery(A~Z)教程(基于Asp.net运行环境)[示例代码下载](一)
(一).概述现在有好多比较优秀的客户端脚本语言组件, 如: Prototype.YUI.jQuery.mootools.Bindows, Scriptaculous, FCKEditor 等, 都非 ...

moveit环境引入代码

moveit环境引入代码相关推荐

最新文章

热门文章