Tuesday, April 9, 2013

Converting Nodes to Nodelets in ROS

In this post I will guide you guys, how can you port nodes to nodelets in ROS. 
The nodelet package is designed to provide a way to run multiple algorithms in the same process with zero copy transport between algorithms nodelet_from_ros_wiki
Let's take an very simple example of subscribing to color image from kinect and publishing it un-modified.  

// The following snippets are written using C++

//Entrypoint in main.cpp

int main (int argc, char** argv)
ros::init(argc, argv,"node_name");
ros::NodeHandle node;
ros::NodeHandle priv_nh("~");
   package_name::node_class  class_object(node,priv_nh);

return 0;



// include all the header files here

namespace package_name
      class node_class
                node_class(ros::NodeHandle node,ros::NodeHandle private_nh);   
                void image_callback(const sensor_msgs::ImageConstPtr& msg);
               image_transport::Subscriber image_sub_;
               image_transport::Publisher image_pub_; 


//Define the node class
// node_class.cpp

namespace package_name
     node_class::node_class (ros::NodeHandle ndoe, ros::NodeHandle private_nh)
         image_transport::ImageTransport it_(node);
        image_sub_ = it_.subscribe("/camera/rgb/image_rect_color",1,&node_class::image_callback,this) ;

     } //constructor

// define the image_callback, for now I just display the input image as it is, you can do all the modifications to the input image here and then publish it.

void node_class::image_callback(const sensor_msgs::ImageConstPtr& msg)
cv_bridge::CvImagePtr cv_ptr;
cv_ptr = cv_bridge::toCvCopy(msg, enc::BGR8);
catch (cv_bridge::Exception& e)
ROS_ERROR("cv_bridge exception: %s", e.what());

cv::imshow("in image", cv_ptr->image);

} // image_callback

} // namespace


#include <nodelet/nodelet.h>
#include "node_class.h"
namespace package_name{
class nodelet_class: public nodelet::Nodelet

   virtual void onInit();
   boost::shared_ptr<node_class> inst_;



//Define the nodelet class

#include <nodelet/nodelet.h>
#include <pluginlib/class_list_macros.h>
#include "nodelet_class.h"

namespace package_name

void nodelet_class::onInit()

NODELET_DEBUG("Initializing nodelet");
inst_.reset(new node_class(getNodeHandle(), getPrivateNodeHandle()));


PLUGINLIB_DECLARE_CLASS(package_name,nodelet_class, package_name :nodelet_class, nodelet::Nodelet)

// now we have to export/add our nodelet in manifest.xml

  <nodelet plugin="${prefix}/nodelet_plugins.xml" />


// Define nodelet as plugin in nodelet_plugins.xml

<library path="lib/libnodelet_class">
  <class name="package_name/nodelet_class" type="package_name::nodelet_class" base_class_type="nodelet::Nodelet">
  This is my nodelet.

//Finally we have to write a launch file for our nodelet in launch_nodelet.launch

  <node pkg="nodelet" type="nodelet" name="standalone_nodelet"  args="manager" output="screen"/>

  <node pkg="nodelet" type="nodelet" name="nodelet_class" args="load package_name/ nodelet_class standalone_nodelet" output="screen">

##Note: Make sure you add a library for the nodelet_class which will be loaded while launching the nodelet in CMakeLists.txt 
rosbuild_add_library(nodelet_class  #add here the paths to all your source files)

Now launch the nodelet and enjoy faster image handling in ROS !!