Problem publishing octomap to the planning scene

edit

Hello dear community,

I am having a problem with publishing my octomap to the planning scene for obstacle avoidance. Can anyone help me with what i did wrong in my code please? I am using ROS Hydro.

At the moment when i launch this node, ROS publishes the nodes to the output stream. (See Fig. 1) Is there a way to resolve this? I want to publish the data continuously but not transform the data every time.

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

#include <sstream>

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

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

#include <octomap/OcTree.h>

int main(int argc, char **argv)
{
  ros::init(argc, argv, "octomap_publisher");

  ros::NodeHandle n;

  //publisher for the planning scene
  ros::Publisher octomap_pub = n.advertise<moveit_msgs::PlanningScene>("planning_scene", 1);
  ros::Rate loop_rate(1);

  int count = 0;
  while (ros::ok())
  {
        static octomap_msgs::Octomap octomap;
        static bool msgGenerated = false;

        //Turn the octomap .bt file into an octree format, which is needed by BinaryMapToMsg

        if ( msgGenerated == false)
        {
              octomap::OcTree myOctomap("/Dir1/Dir2/.../SOLMeasurePolygon.bt");
              octomap_msgs::binaryMapToMsg(myOctomap, octomap);
              msgGenerated = true;
        }

      moveit_msgs::PlanningScene planning_scene;
      planning_scene.world.octomap.header.frame_id = "odom_combined";
      planning_scene.world.octomap.octomap.header.frame_id = "odom_combined";
      planning_scene.world.octomap.octomap.binary = true;
      planning_scene.world.octomap.octomap.id = "OcTree";
      planning_scene.world.octomap.octomap.data = octomap.data;


      ROS_INFO("Adding the octomap into the world.");
      octomap_pub.publish(planning_scene);

    ros::spinOnce();

    loop_rate.sleep();
    ++count;
  } 
  return 0;
}

Fig. 1: The terminal when i launch the node

Kind regards

Joris Beuls