Build a octomap_server nodelet [closed]

edit

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