ROS Resources: Documentation | Support | Discussion Forum | Service Status | Q&A answers.ros.org
Ask Your Question
0

Build a octomap_server nodelet [closed]

asked 2012-08-08 20:45:44 -0500

jep31 gravatar image

updated 2016-10-24 08:34:14 -0500

ngrennan gravatar image

Hi,

I have already written a subject about the octomap nodelet to know if it was a good idea to rewrite the node octomap_server to obtain a nodelet:

Create collision map from Kinect with nodelet

thanks for your answer but now my node doesn't work. The compilation and the execution works but there is nothing in the topic collision_map_out. I run the kinect with:

 roslaunch openni_launch openni.launch

And I use the camera_nodelet_manager of openni to run the other nodelets. Please help me to know why it doesn't work. I haven't any idea.

This is my launch file:

<launch>

 <node pkg="nodelet" type="nodelet" name="Octo" args="load nodelet_test/Octo camera_nodelet_manager" output="screen">
   <param name="resolution" value="0.05"/> 
   <param name="frame_id" type="string" value="openni_camera" />
   <param name="sensor_model/max_range" value="1.5" />  
   <param name="latch" value="true" />
  </node>

 <node pkg="nodelet" type="nodelet" name="Point_Cloud" args="load nodelet_test/Point_Cloud camera_nodelet_manager" output="screen">
  </node>

</launch>

my octomap_server nodelet:

#include <pluginlib/class_list_macros.h>
#include <nodelet/nodelet.h>
#include <ros/ros.h>
#include <octomap_server/OctomapServer.h>

#define USAGE "\nUSAGE: octomap_server <map.bt>\n" \
        "  map.bt: inital octomap 3D map file to read\n"

namespace nodelet_octomap_server

{

  class Octo : public nodelet::Nodelet
  {


  private:

    std::string mapFilename;
    octomap_server::OctomapServer * ms;  
    ros::Publisher pub;
    ros::Subscriber sub;

    virtual void onInit()
    {
      ROS_INFO("octo go");
      //ros::NodeHandle& nh = getPrivateNodeHandle();
      mapFilename = "";
      ms = new octomap_server::OctomapServer(mapFilename);  
    }

  };

  PLUGINLIB_DECLARE_CLASS(nodelet_test, Octo, nodelet_octomap_server::Octo, nodelet::Nodelet);

my point cloud nodelet because the remap doesn't work. So I should read a topic and write and a other topic with the good name. I know the good thing is with a remap:

<remap from="cloud_in" to="/camera/depth/points" />

in the launch file but it doesn't work and I don't know why:

#include <pluginlib/class_list_macros.h>
#include <nodelet/nodelet.h>
#include <ros/ros.h>
#include <octomap_server/OctomapServer.h>


#define USAGE "\nUSAGE: octomap_server <map.bt>\n" \
        "  map.bt: inital octomap 3D map file to read\n"

namespace nodelet_point_cloud

{
  typedef pcl::PointCloud<pcl::PointXYZRGB> PointCloud;

  class Point_Cloud : public nodelet::Nodelet
  {

  private:


    ros::Publisher pub;
    ros::Subscriber sub;

    virtual void onInit()
    {
      ROS_INFO("PointCloud go");
      ros::NodeHandle& nh = getPrivateNodeHandle();
      pub = nh.advertise<sensor_msgs::PointCloud2>("/cloud_in", 2);
      sub = nh.subscribe("/camera/depth/points",2, &Point_Cloud::callback, this);

    }
    void callback(const sensor_msgs::PointCloud2 &cloud)
    {
      //ROS_INFO("ptcloud callback");
      pub.publish(cloud);
    }

  };


  PLUGINLIB_DECLARE_CLASS(nodelet_test, Point_Cloud, nodelet_point_cloud::Point_Cloud, nodelet::Nodelet);
}

EDIT: I have this message:

[ WARN] [1345446392.735855305]: MessageFilter [target=/map ]: Dropped 100.00% of messages so far. Please turn the ros.octomap_server.message_notifier] rosconsole logger to DEBUG for more information.

I don't understand this and I don't know if it's a problem.

And with rxloggerlevel I can see:

[DEBUG] [1345445586.353342836]: MessageFilter [target=/map ]: Added message in frame /camera_depth_optical_frame at time 1345445586.335, count now 5

[DEBUG] [1345445586.383787374]: MessageFilter [target=/map ]: Removed oldest message because buffer is full, count now 5 (frame_id=/camera_depth_optical_frame, stamp=1345445586.198976)

[DEBUG] [1345445586.384167463]: MessageFilter [target=/map ]: Added message in frame /camera_depth_optical_frame at time 1345445586.367, count ...

(more)
edit retag flag offensive reopen merge delete

Closed for the following reason question is not relevant or outdated by tfoote
close date 2014-08-17 13:20:50.896387

2 Answers

Sort by ยป oldest newest most voted
0

answered 2012-08-13 00:23:59 -0500

AHornung gravatar image

updated 2012-08-20 02:14:26 -0500

I'm not completely sure what will happen to topics and namespaces in a nodelet. Could you debug the topic connections with "rosnode info" and "rxgraph"?

For debugging octomap_server, just use rxconsole and set the log level to "Debug". If it's receiving point clouds it will give you some info. Note that octomap_server requires a valid tf from your map frame to the sensor frame, otherwise nothing will happen.

Edit: This is the actual error you see:

[ WARN] [1345446392.735855305]: MessageFilter [target=/map ]: Dropped 100.00% of messages so far. Please turn the ros.octomap_server.message_notifier] rosconsole logger to DEBUG for more information.

There is no tf between your sensor message's header and the octomap_server map frame id. The callbacks for integrating measurement data are only called by a message filter when tf is available. Otherwise it will just drop the messages. So your transforms may be wrong (wrong time or frame names). Are you sure that all the parameters are set correctly?

I also noticed that you set the camera frame as the map frame for octomap_server. Is your camera static and not moving in the world? This is the only use case when that setting is correct.

However, these errors should all also appear in the non-nodelet case!

edit flag offensive delete link more

Comments

Thanks, now I have a static tf between world and camera_link. I have modify:

<param name="frame_id" type="string" value="openni_camera" />

<param name="frame_id" type="string" value="camera_link" />

I have edited my question with debug info

jep31 gravatar imagejep31 ( 2012-08-19 21:10:37 -0500 )edit

There is still something wrong with either the parameter or your tf tree - see my edit about the error you see.

AHornung gravatar imageAHornung ( 2012-08-20 02:15:40 -0500 )edit
0

answered 2012-11-18 16:55:55 -0500

Jep31, I believe your problems with not working remappings is due to not passing the node handle to the OctomapServer class on contruction.

I have also created a nodelet implementation of the octomap server class (the code is very similar to yours) and sent in the patch here on Googlecode

Besides adding the nodelet itself, this patch also modifies the OctomapServer class, so that an instance uses the node handle of the nodelet. This should enable the parameter (e.g. frame_id -> error: "MessageFilter [target=/map ]: Dropped 100.00% of messages ...") and topic remappings (point cloud input / collision map output).

edit flag offensive delete link more

Question Tools

Stats

Asked: 2012-08-08 20:45:44 -0500

Seen: 1,168 times

Last updated: Nov 18 '12