2024-9-23_L/catkin_ws/src/lstrobot_planning/src/add_octomap.cpp

#include <ros/ros.h>
#include <moveit/move_group_interface/move_group_interface.h>
#include <moveit/planning_scene_interface/planning_scene_interface.h>
#include <octomap_msgs/Octomap.h>
#include <octomap_msgs/conversions.h>
#include <octomap/OcTree.h>

void appendScene(const std::string& octoFile)
{
    ros::NodeHandle nh("~");

    // 创建 MoveIt MoveGroup Interface
    moveit::planning_interface::MoveGroupInterface group("manipulator");
    std::string frameId = group.getPlanningFrame();

    // 从文件加载 OctoMap
    octomap::OcTree* octree = new octomap::OcTree(octoFile);

    // 将 Octomap 数据转换为 ROS 消息
    octomap_msgs::Octomap octomap_msg;
    if (!octomap_msgs::fullMapToMsg(*octree, octomap_msg))
    {
        ROS_ERROR("Failed to convert Octomap to ROS message.");
        return;
    }

    // 创建 PlanningSceneInterface 实例
    moveit::planning_interface::PlanningSceneInterface psi;

    // 创建 PlanningScene 消息
    moveit_msgs::PlanningScene planning_scene_msg;
    planning_scene_msg.is_diff = true;

    // 设置 Octomap 的相关信息
    planning_scene_msg.world.octomap.header.frame_id = "base_link";
    planning_scene_msg.world.octomap.octomap = octomap_msg;

    // 设置Octomap的原点位置
    // planning_scene_msg.world.octomap.origin.position.x = 0.0;
    // planning_scene_msg.world.octomap.origin.position.y = 0.0;
    // planning_scene_msg.world.octomap.origin.position.z = 0.0;

    // // 设置有效的四元数
    // planning_scene_msg.world.octomap.origin.orientation.x = 0.0;
    // planning_scene_msg.world.octomap.origin.orientation.y = 0.0;
    // planning_scene_msg.world.octomap.origin.orientation.z = 0.0;
    // planning_scene_msg.world.octomap.origin.orientation.w = 1.0;

    planning_scene_msg.world.octomap.header.stamp = ros::Time::now();

    // 将 PlanningScene 消息应用到 MoveIt 中
    psi.applyPlanningScene(planning_scene_msg);

    delete octree; // 释放内存
}

int main(int argc, char **argv)
{
    ros::init(argc, argv, "append_scene_node");
    ros::NodeHandle nh;

    // 检查是否传入了OctoMap文件名参数
    if (argc < 2)
    {
        ROS_ERROR("Usage: %s [octomap_file]", argv[0]);
        return 1;
    }

    std::string octoFile = argv[1];

    appendScene(octoFile);

    // 确保所有更改都已发送给MoveIt!
    ros::Duration(1.0).sleep(); // 等待一段时间以确保消息被处理

    ROS_INFO("Planning scene updated with OctoMap.");

    return 0;
}