Problem publishing octomap to the planning scene

asked 2015-02-11 08:17:42 -0600

J.Beuls gravatar image

updated 2015-03-19 14:04:19 -0600

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 image description

Kind regards

Joris Beuls

edit retag flag offensive close merge delete

Comments

Just to be clear: These nodes are nodes in the octomap tree, not ROS nodes.

dornhege gravatar imagedornhege ( 2015-03-18 05:54:10 -0600 )edit

Yes, thank you. I knew it weren't ROS nodes as rosnode list doesn't give me allot of nodes. But i didn't know what was really meant by this. Is this conversion of octomap to octree taking so long?

J.Beuls gravatar imageJ.Beuls ( 2015-03-18 09:24:55 -0600 )edit
2

Why don't you initialise your msg outside your while loop? I'm not sure there is a need to keep publishing the message. Depending on who is listening of course. Even if you need to keep republishing: read your file outside the while, init msg, then only pub() inside while loop.

gvdhoorn gravatar imagegvdhoorn ( 2015-03-19 15:01:39 -0600 )edit

I did that because i want to be able to change the message being published when the input changes. This is code i wanted to add later. I will try what you told me. Thank you for the feedback.

J.Beuls gravatar imageJ.Beuls ( 2015-03-21 05:05:46 -0600 )edit

Well "I want to publish the data continuously" and "but not transform the data every time" then seem to be conflicting statements. Updated data will need to be transformed (I think you mean converted) every time. Static data can be converted once, then repeatedly published.

gvdhoorn gravatar imagegvdhoorn ( 2015-03-21 08:31:08 -0600 )edit

Also, for updating a MoveIt (collision) scene using depth data, it is customary to use one of the Octomap updater plugins described in moveit/wiki/3D_Sensors. You would just need to make sure your sensor publishes the expected topics.

gvdhoorn gravatar imagegvdhoorn ( 2015-03-21 08:42:36 -0600 )edit