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);
   ros::spin();

return 0;


}



//node_class.h


// include all the header files here

namespace package_name
{
      class node_class
      {
      public: 
                node_class(ros::NodeHandle node,ros::NodeHandle private_nh);   
                ~node_class(){};
                void image_callback(const sensor_msgs::ImageConstPtr& msg);
     private:
               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;
try
{
cv_ptr = cv_bridge::toCvCopy(msg, enc::BGR8);
}
catch (cv_bridge::Exception& e)
{
ROS_ERROR("cv_bridge exception: %s", e.what());
return;
}

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

image_pub_.publish(cv_ptr->toImageMsg());
} // image_callback



} // namespace


//nodelet_class.h


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

public:
nodelet_class(){}
~nodelet_class(){}
   virtual void onInit();
   boost::shared_ptr<node_class> inst_;

    };

}



//Define the nodelet class
//nodelet_class.cpp


#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

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

</export>



// 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">
  <description>
  This is my nodelet.
  </description>
  </class>
</library>



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



<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">
  </node>                 
</launch>






##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 !!