load a pre-built octomap into MoveIt !!

edit

https://www.youtube.com/watch?v=Ib1IS...

I just follow the preference link above and successfully make the code recur. But when I want to covert the python code to C++ code, I found my code couldn't perform well. I mean, when I subscribe to the "octomap_full" topic, I couldn't get any response. My code trapped in a loop. If any one can figure out what happened?

My code:

#include <ros/ros.h>
#include "std_msgs/String.h"

#include <sstream>
#include <fstream>
#include <iostream>
#include <string>

#include <moveit/planning_scene/planning_scene.h>
#include <moveit/kinematic_constraints/utils.h>
#include <moveit_msgs/PlanningScene.h>
#include <moveit_msgs/PlanningSceneWorld.h>
#include <eigen_conversions/eigen_msg.h>

#include <octomap/octomap.h>
#include <octomap/OcTree.h>
#include <octomap_msgs/Octomap.h>
#include <octomap_msgs/conversions.h>


// https://www.youtube.com/watch?v=Ib1ISnLlD38
// https://answers.ros.org/question/214386/how-to-publish-a-message-in-a-callback-function/

void msgs_callback(const octomap_msgs::Octomap::ConstPtr &msg, moveit_msgs::PlanningScene &planning_scene_transfer)
{
  moveit_msgs::PlanningSceneWorld planning_scene_world;
  planning_scene_world.octomap.header.frame_id = "world";
  planning_scene_world.octomap.header.stamp = ros::Time::now();
  planning_scene_world.octomap.octomap = *msg;

  planning_scene_world.octomap.origin.position.x = 0.0;
  planning_scene_world.octomap.origin.orientation.w = 1.0;

  moveit_msgs::PlanningScene planning_scene;
  planning_scene.world = planning_scene_world;
  planning_scene.is_diff = true;

  planning_scene_transfer = planning_scene;
  ROS_INFO("subscribe successfully");
}




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

  moveit_msgs::PlanningScene planning_scene_transfer;

  // publisher for the planning scene
  ros::Publisher octomap_pub_1 = n.advertise<moveit_msgs::PlanningScene>("move_group/monitored_planning_scene", 1);
  ros::Publisher octomap_pub_2 = n.advertise<moveit_msgs::PlanningScene>("planning_scene", 1);
  // subscriber for octomap information
  ros::Subscriber octomap_sub = n.subscribe<octomap_msgs::Octomap>("octomap_full", 1, boost::bind(msgs_callback, _1, planning_scene_transfer));
  ROS_INFO("rokae_static_octomap_publish_node initialized!");
  ros::Rate loop_rate(0.25);

  while(ros::ok())
  {

    octomap_pub_1.publish(planning_scene_transfer);
    octomap_pub_2.publish(planning_scene_transfer);
    loop_rate.sleep();
  }

  ros::spin();

  return 0;
}

reference code:

#! /usr/bin/env python

import rospy
from octomap_msgs.msg import Octomap
from moveit_msgs.msg import PlanningScene, PlanningSceneWorld

class OctoHandler():
    mapMsg = None

    def __init__(self):
        rospy.init_node('moveit_octomap_handler')
        rospy.Subscriber('octomap_full', Octomap, self.cb, queue_size=1)
        pub = rospy.Publisher('move_group/monitored_planning_scene', PlanningScene, queue_size=1)
        pub2 = rospy.Publisher('/planning_scene', PlanningScene, queue_size=1)
        r = rospy.Rate(0.25)
        while(not rospy.is_shutdown()):
            if(self.mapMsg is not None):
                pub.publish(self.mapMsg)
                pub2.publish(self.mapMsg)
            else:
                pass
            r.sleep()

    def cb(self, msg):
        psw = PlanningSceneWorld()
        psw.octomap.header.stamp = rospy.Time.now()
        psw.octomap.header.frame_id = 'world'
        psw.octomap.octomap = msg

        psw.octomap.origin.position.x = 0
        psw.octomap.origin.orientation.w = 1

        ps = PlanningScene()
        ps.world = psw
        ps.is_diff = True
        self.mapMsg = ps

if __name__ == '__main__':
    octomap_object = OctoHandler()

thanks a lot if this problem can be solved in