程序员经验分享-hwhale

ROS2中将octomap发布到Moveit中作为碰撞物体

2023-05-16

1.安装准备

这里假设你已经装好了ROS2以及Moveit2（都用二进制安装就行，不用从源码安转），没有安装好的，可以按照鱼香ROS的教程安装，两三行命令就搞定了。
我的ROS2版本为humble，请根据你使用的实际版本替换。
安装pcl

sudo apt install libpcl-dev
sudo apt-get install ros-humble-pcl*

安装octomap相关库

sudo apt-get install ros-humble-octomap*

下面这个也安装一下

sudo apt-get install ros-humble-moveit-ros-perception

2.bt文件读取

octomap信息可能来自话题（由设备或者其他节点发布），或者从bt文件中读取。这里出于便于演示等原因，选择从文件中读取。文件读取时要注意使用的bt文件是txt还是bin的，要选择对应的读取方式，具体请查看代码。
这个octomap的功能还是用之前做的机械手环境来演示。
以下所使用到的【geb079.bt】文件来自于octomap的github。而【octomap.bt】文件是通过以下函数创建：

void createOctoMap()
{
    // 创建OcTree对象，并设置分辨率为0.05
    octomap::OcTree* tree = new octomap::OcTree(0.05);

    // 设置障碍物的大小和位置
    octomap::point3d origin(0, 0, 0);
    octomap::point3d obstacle(0, 0, 0); // 1 1 1
    double size = 0.2;

    // 在OcTree中添加一个立方体障碍物
    tree->updateNode(octomap::point3d(origin.x() + obstacle.x() - size, origin.y() + obstacle.y() - size, origin.z() + obstacle.z() - size), true);
    tree->updateNode(octomap::point3d(origin.x() + obstacle.x() + size, origin.y() + obstacle.y() - size, origin.z() + obstacle.z() - size), true);
    tree->updateNode(octomap::point3d(origin.x() + obstacle.x() + size, origin.y() + obstacle.y() + size, origin.z() + obstacle.z() - size), true);
    tree->updateNode(octomap::point3d(origin.x() + obstacle.x() - size, origin.y() + obstacle.y() + size, origin.z() + obstacle.z() - size), true);
    tree->updateNode(octomap::point3d(origin.x() + obstacle.x() - size, origin.y() + obstacle.y() - size, origin.z() + obstacle.z() + size), true);
    tree->updateNode(octomap::point3d(origin.x() + obstacle.x() + size, origin.y() + obstacle.y() - size, origin.z() + obstacle.z() + size), true);
    tree->updateNode(octomap::point3d(origin.x() + obstacle.x() + size, origin.y() + obstacle.y() + size, origin.z() + obstacle.z() + size), true);
    tree->updateNode(octomap::point3d(origin.x() + obstacle.x() - size, origin.y() + obstacle.y() + size, origin.z() + obstacle.z() + size), true);

    // 将OcTree保存为octomap.bt文件
    std::string filename = "octomap.bt";
    auto ret = tree->writeBinary(filename);
//    auto ret = tree->write(filename);

    qDebug() << "write octomap.bt:" << ret;
    // 删除OcTree对象
    delete tree;
}

假如要将读取、接收到的octomap通过代码的方式“输出”到moveit中，让其参与碰撞检测、路径规划，一般有两种方式：

2.1通过moveit_msgs::msg::CollisionObject加入场景中

这种方式，是通过 octomap–》pointCloud–》mesh 的方式进行的，比较麻烦。而且在pointCloud–》mesh这一步遇到了一些问题，转出来的是面片，而非实体mesh。后面再研究研究

// 这个函数还是有问题的，弄出来的是面片，而不是mesh，后续还需要改进
void pclPointCloudToShapeMsgsMesh(const pcl::PointCloud<pcl::PointXYZ> *cloud,
                                  shape_msgs::msg::Mesh &mesh)
{
    // 创建三角形索引数组
    pcl::PolygonMesh triangles;
    pcl::Vertices vertices;

    for (size_t i = 0; i < cloud->size() - 2; ++i)
    {
        vertices.vertices.clear();
        vertices.vertices.push_back(i);
        vertices.vertices.push_back(i + 1);
        vertices.vertices.push_back(i + 2);
        triangles.polygons.push_back(vertices);
    }

    // 拷贝点云数据到Mesh消息中
    mesh.vertices.resize(cloud->size());
    for (int i = 0; i < cloud->size(); ++i)
    {
        geometry_msgs::msg::Point p;
        p.x = (*cloud)[i].x;
        p.y = (*cloud)[i].y;
        p.z = (*cloud)[i].z;
        mesh.vertices[i] = p;
    }

    // 拷贝三角形索引数据到Mesh消息中
    mesh.triangles.resize(triangles.polygons.size());
    for (int i = 0; i < triangles.polygons.size(); ++i)
    {
        shape_msgs::msg::MeshTriangle t;
        t.vertex_indices[0] = triangles.polygons[i].vertices[0];
        t.vertex_indices[1] = triangles.polygons[i].vertices[1];
        t.vertex_indices[2] = triangles.polygons[i].vertices[2];
        mesh.triangles[i] = t;
    }
}

// 将octoMap作为碰撞物体导入到moveit中
void appendCollisionObject(QString octoFile)
{
    auto const node = std::make_shared<rclcpp::Node>(
                "hello_moveit",
                rclcpp::NodeOptions().automatically_declare_parameters_from_overrides(true));

    //创建 MoveIt MoveGroup Interface，用于连接、获取MoveGroup的信息
    using moveit::planning_interface::MoveGroupInterface;
    auto move_group_interface = MoveGroupInterface(node, "my_group"); // 估计要依附一个节点，才能进行interface的创建

    // 将碰撞物体加到scene中
    moveit::planning_interface::PlanningSceneInterface planning_scene_interface;

    auto frameId = move_group_interface.getPlanningFrame();
    qDebug() << "frame id:" << frameId.data();
    {
        std::shared_ptr<octomap::OcTree> octree = std::make_shared<octomap::OcTree>(octomap::OcTree(octoFile.toStdString()));
        if (octree == nullptr) {
          throw std::runtime_error("Could not read Octomap data from file " + octoFile.toStdString());
        }

        // octTree转点云
        pcl::PointCloud<pcl::PointXYZ> cloud;
        // Traverse all the leaf nodes and add them to the point cloud if they are occupied
        for (auto it = octree->begin_leafs(); it != octree->end_leafs(); ++it) {
            if (octree->isNodeOccupied(*it)) {
                pcl::PointXYZ pt;
                pt.x = it.getX();
                pt.y = it.getY();
                pt.z = it.getZ();
                cloud.points.push_back(pt);
            }
        }

        // 点云转mesh
        shape_msgs::msg::Mesh output_mesh;
        pclPointCloudToShapeMsgsMesh(&cloud, output_mesh);

        geometry_msgs::msg::Pose octomap_pose;
        octomap_pose.orientation.w = 1.0;
        octomap_pose.position.x = 0.2;
        octomap_pose.position.y = 0.2;
        octomap_pose.position.z = 0.2;

        // 直接往octomap_object里面塞mesh
        moveit_msgs::msg::CollisionObject octomap_object;
        octomap_object.id = "my_octomap_object";
        octomap_object.header.frame_id = frameId;
        octomap_object.meshes.push_back(output_mesh);
        octomap_object.mesh_poses.push_back(octomap_pose);
        octomap_object.operation = moveit_msgs::msg::CollisionObject::ADD;


        // 将碰撞对象添加到collision_objects中，然后传递给applyCollisionObjects函数
        std::vector<moveit_msgs::msg::CollisionObject> collision_objects;
        collision_objects.push_back(octomap_object);
        qDebug() << "applyCollisionObjects" <<  planning_scene_interface.applyCollisionObjects(collision_objects);
    }
}

2.2通过moveit_msgs::msg::PlanningScene加入（替换）场景

目前我使用的是这种，效果良好。不过可能会存在一些冲突，等后面研究得更加透彻之后再修改吧。

void appendScene(QString octoFile)
{
    auto const node = std::make_shared<rclcpp::Node>(
                "hello_moveit1",
                rclcpp::NodeOptions().automatically_declare_parameters_from_overrides(true));


    //创建 MoveIt MoveGroup Interface，用于连接、获取MoveGroup的信息
    using moveit::planning_interface::MoveGroupInterface;
    auto move_group_interface = MoveGroupInterface(node, "my_group"); // 估计要依附一个节点，才能进行interface的创建
    auto frameId = move_group_interface.getPlanningFrame();

    // Load an OctoMap from file
    octomap::OcTree* octree = new octomap::OcTree(octoFile.toStdString());

    // 将Octomap数据转换为ROS2消息
    octomap_msgs::msg::Octomap octomap_msg;
    octomap_msgs::binaryMapToMsg(*octree, octomap_msg);

    // 创建PlanningSceneMonitor实例, 用于获取 AllowedCollisionMatrix
    auto planning_scene_monitor = std::make_shared<planning_scene_monitor::PlanningSceneMonitor>(node,
        "robot_description");
    collision_detection::AllowedCollisionMatrix acm = planning_scene_monitor->getPlanningScene()->getAllowedCollisionMatrix();
//    // 假设我们需要允许"obstacle_1"和"obstacle_2"之间的碰撞
//    acm.setEntry("obstacle_1", "obstacle_2", true);

    // 将 collision_detection::AllowedCollisionMatrix 转成 moveit_msgs::msg::AllowedCollisionMatrix
    moveit_msgs::msg::AllowedCollisionMatrix acm_msg;
    acm.getMessage(acm_msg);

    moveit_msgs::msg::PlanningScene planning_scene_msg;
    planning_scene_msg.world.octomap.header.frame_id = frameId;  // Replace "map" with your frame ID
    planning_scene_msg.world.octomap.origin.orientation.w = 1.0;
    planning_scene_msg.world.octomap.octomap = octomap_msg;
    planning_scene_msg.allowed_collision_matrix = acm_msg;

    // 将PlanningScene 加（替换）到moveit中
    moveit::planning_interface::PlanningSceneInterface planning_scene_interface;
    planning_scene_interface.applyPlanningScene(planning_scene_msg);
}

假如想从点云转octomap，也非常简单。转成octomap之后，也就可以通过上面的方式导入moveIt中了。函数参考这里。

// pointCloud转成octomap
void pointCloudToOctoMap(const pcl::PointCloud<pcl::PointXYZ> *cloud,
                                  octomap_msgs::msg::Octomap  &octoMap)
{
    // create octomap
    octomap::OcTree tree(0.05); // set resolution of the octomap
    for (const auto& point : cloud->points)
    {
      tree.updateNode(octomap::point3d(point.x, point.y, point.z), true);
    }
    tree.updateInnerOccupancy(); // update inner node occupancy

   // convert octomap to octomap_msgs::msg::Octomap
    octomap_msgs::binaryMapToMsg(tree, octoMap);
}

3.效果演示

视频演示可以看这里：【MoveIt2中导入octomap】
在这里插入图片描述

参考
https://blog.csdn.net/qq_27865227/article/details/125002311

本文内容由网友自发贡献，版权归原作者所有，本站不承担相应法律责任。如您发现有涉嫌抄袭侵权的内容，请联系:hwhale#tublm.com(使用前将#替换为@)

ROS2中将octomap发布到Moveit中作为碰撞物体 的相关文章

  • 什么是寄存器？（STM32）

    什么是寄存器 xff1f 我们现在在开发STM32时 xff0c 已经很少用到寄存器编程 xff0c 更多的使用ST公司所提供的标准库和最新的HAL库进行编程实现 xff0c 但是不管是标准库还是HAL库都是在原来的寄存器层面上进行了封装

  • 计算机网络——物理层（一）

    物理层 xff08 部分 xff09 机械特性 xff1a 指明接口所用接线器的形状和尺寸 引脚数目和排列 固定和锁定装置等 电气特性 xff1a 指明在接口电缆的各条线上出现的电压的范围 功能特性 xff1a 指明某条线上出现的某一电平的

  • 洛谷P5717-三角形分类

    洛谷P5717 三角形分类 题目 这道题更像是初中题 xff0c 但是怎么能完整的按照题目的意思来解决呢 xff0c 说实话这个题卡了我有一会儿 xff0c 要做一次性做出这个题 xff0c 我觉得需要搞清楚if if 和if else i

  • 洛谷P1424-小鱼的航程（改进版）

    洛谷P1424 小鱼的航程 xff08 改进版 xff09 这个题我第一次做的时候 xff0c 有两个没过 xff0c 后来检查的时候发现原来是没有考虑开始的时间是不是周六周日 xff0c 如果是周六要在原来的天数上 2 xff0c 如果是

  • freertos-简介（一）

    FreeRTOS 裸机 不带任何操作系统 只能先打完游戏回复信息 实时性差 xff0c 程序轮流执行delay空等待 xff0c CPU不执行其他代码结构臃肿 xff0c 实现功能都在while循环 RTOS 实时操作系统 会执行打游戏一个

  • PCB设计过程中AD使用流程详解（超详细）

    PCB设计过程中AD使用流程详解 xff08 超详细 xff09 1 设计前期部分 规则设定 xff1a Preference system file type关联文件 xff08 所有关联 xff09 PCB editor General

  • python面向对象编程

    符合python风格的对象 先来看一个向量类的例子 span class token keyword class span span class token class name Vector2d span span class token

  • DIY 一个树莓派无人机

    学习目标 xff1a DIY 一个树莓派无人机 这篇文章来源于DevicePlus com英语网站的翻译稿 提示 xff1a 这里可以添加学习目标 学习内容 xff1a 提示 xff1a 这里可以添加要学的内容 今天 xff0c 我们将利用

  • Linux开源杀毒软件CLamAV介绍

    Linux开源杀毒软件CLamAV介绍 很多用户可能不知道在Linux上会有计算机病毒 xff0c 虽然Linux上的病毒不像在Windows上那么常见 xff0c 但实际上 xff0c 很多重要系统均采用Linux系统作为服务器的操作系统

  • vrpn_cient_ros发送频率无法调整提供解决思路

    最近写了节点来订阅ros client ros 但是发现在launch文件修改update frequency不起作用 xff0c 然而我又需要通过串口给下位机发送数据 xff0c 频率一快 xff0c 串口直接堵死了 怎么调都freque

  • 封装CopyFileEx函数，实现文件复制中的暂停，控速，获取进度

    封装CopyFileEx函数 xff0c 实现文件复制中的暂停 xff0c 控速 xff0c 获取进度等 前段时间无意间想到如何控制文件复制过程的复制速度 xff0c 并且能实时获得复制进度 对于一个几兆甚至更小的文件 xff0c 调用AP

  • 字符串结束符'\0' -何时自动加- 字符串定义方法

    转载 字符串定义方法 有两种方法 1 用字符数组 xff1b 2 用字符指针 xff1b 对应两种定义方法 xff0c 有不同的初始化以及赋值方法 对字符数组 xff0c 有以下几种定义方法 xff1a 1 char str 61 34 1

  • 基于Airsim的sitl模拟环境配置（ubuntu 16.04）

    基于Airsim的sitl模拟环境配置 xff08 ubuntu 16 04 xff09 sitl仿真 xff0c 软件在环仿真可以不使用任何硬件就可以进行模拟飞行或驾驶 xff0c 实验室获取数据非常有用 基于Airsim的sitl模拟需

  • F450机架组装及飞控安装细节

    http tieba baidu com p 5342947735

  • 富斯i6接收机及PPM编码器​​​​​​​接线

    没有完成发射机和接收机对码 xff0c 则需要按照如下过程对码 xff1a 1 将对码线连接到接收机上的 B VCC 接口 2 将电源线连接到接收机上任意其他接口 3 打开发射机电源 xff0c 同时常按发射机 BINDKEY 键 xff0

  • Pixhawk指示灯和安全开关含义

    Pixhawk指示灯的含义 红灯和蓝灯闪 xff1a 初始化中 请稍等 黄灯双闪 xff1a 错误 系统拒绝解锁 蓝灯闪 xff1a 已加锁 xff0c GPS搜星中 自动导航 xff0c 悬停 xff0c 还有返回出发点模式需要GPS锁定

  • MP地面站提示

    PIX飞控或者APM飞控在装机后 xff0c 经常遇到不能解锁的情况 xff0c 地面站会有提示 xff0c 下面列出了可能出现的情况 xff0c 可以一一对应的排除故障 当然 xff0c 你也可以在地面站设置解锁不自检 xff0c 不过安

  • win7下 pixhawk (ardupilot) 的编译

    前几天都在搞pixhawk源码编译问题 xff0c 什么在window下用Console或者eclipse xff0c 还是在Ubuntu下 xff0c 都做了 xff0c 而且把 mk文件都看了 xff0c 结果还是有bug 总结一下三种

  • pixhawk 基于UART5 的NSH环境搭建

    元器件 xff1a pixhawk 六针杜邦线 USB转uart模块 PC机上的串口调试软件 xff08 比如 Putty xff09 STEP 1 xff1a 制作调试通信线 xff08 六针杜邦线和USB转uart模块 xff09 xf

  • 操作系统 | 用户态和内核态的切换（中断、系统调用与过程（库函数）调用）

    文章目录 中断过程调用系统调用过程调用和系统调用的区别 中断 用户态 内核态之间的切换是怎么实现的 用户态 内核态 是通过中断实现的 并且 中断是唯一途径 核心态 用户态 的切换是通过执行一个特权指令 xff0c 将程序状态字 PSW 的标

随机推荐

热门标签