load a pre-built octomap into MoveIt !!

asked 2021-11-12 10:03:37 -0500

Contour gravatar image


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);




  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.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):

    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

edit retag flag offensive close merge delete



I'd try spinOnce() as it handles the events and returns immediately, spin() blocks until ROS invokes a shutdown

osilva gravatar image osilva  ( 2021-11-12 13:44:49 -0500 )edit

Thanks a lot osilva! I added the spinOnce() in the while loop and it actually worked. But the RVIZ still could not receive the messages from my publishers, though I found the topic had been subsribe by /move_group/. It seems that the subscriber only get the messages from octomap_full once, because I only get one response.

Contour gravatar image Contour  ( 2021-11-12 19:54:00 -0500 )edit

Try to see when you run the Python version what behaviour you get to understand what you are missing. If you can compare the two. I suspect C++ code runs faster so you may need to consider that too

osilva gravatar image osilva  ( 2021-11-13 07:17:11 -0500 )edit

Thanks, I’ll try it. If I make a progress, I will update the info here.

Contour gravatar image Contour  ( 2021-11-13 10:06:25 -0500 )edit

I sovled this problem when I use the class structure. Send ros::NodeHandle nh address to private: ros::NodeHandle nh_ in the construtor function.

Contour gravatar image Contour  ( 2021-12-09 01:40:29 -0500 )edit

Great thank you for sharing. Pls consider documenting this as an answer so others benefit in the future

osilva gravatar image osilva  ( 2021-12-09 01:54:43 -0500 )edit