Problem publishing octomap to the planning scene
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
Just to be clear: These nodes are nodes in the octomap tree, not ROS nodes.
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?
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.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.
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.
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.