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

Monday, June 18, 2012

Creating OpenNI '.oni' files from OpenCV/Jpeg Images

I had been thinking of tweaking OpenNI samples to work with modified data. It could be as a result of post processing on the Depth/Rgb images in Matlab or OpenCV. Mostly we record data from OpenNI in the form of '.oni' file and then want to process them in Matlab or OpenCV. For this we need to convert the images to OpenCV images and then can save to disk. Later load these images in Matlab or OpenCV and do processing and then resave them as '.oni' files to run the OpenNI samples on the modified data. 


For the first part you can either use OpenNI and convert them to OpenCV images and write them to disk. 




You can convert images to OpenCV images in the following way. 
   

         cv::Mat colorArr[3];
        cv::Mat colorImage;
        const XnRGB24Pixel* pImageRow;
        const XnRGB24Pixel* pPixel;

        imageGen.SetPixelFormat(XN_PIXEL_FORMAT_RGB24 );  //
        xn::ImageGenerator imageGen;
        imageGen.GetMetaData(imageMD);    //xn::ImageMetaData imageMD;
        pImageRow = imageMD.RGB24Data();

        colorArr[0] = cv::Mat(imageMD.YRes(),imageMD.XRes(),CV_8U);
        colorArr[1] = cv::Mat(imageMD.YRes(),imageMD.XRes(),CV_8U);
        colorArr[2] = cv::Mat(imageMD.YRes(),imageMD.XRes(),CV_8U);

        for (int y=0; y<imageMD.YRes(); y++)
       {
              pPixel = pImageRow;
              uchar* Bptr = colorArr[0].ptr<uchar>(y);
              uchar* Gptr = colorArr[1].ptr<uchar>(y);
              uchar* Rptr = colorArr[2].ptr<uchar>(y);
              for(int x=0;x<imageMD.XRes();++x , ++pPixel)
              {
                        Bptr[x] = pPixel->nBlue;
                        Gptr[x] = pPixel->nGreen;
                        Rptr[x] = pPixel->nRed;
              }
              pImageRow += imageMD.XRes();
        }
        cv::merge(colorArr,3,colorImage); 

The second way is to directly use the Matlab Toolboox for kinect and save the images from there. 



For the second part i.e how to compile these images as an '.oni' file , I used the OpenNI program  NiRecordSynthetic. So I provide a dummy .oni file as an input and then change its data according to my new depth maps and record a new .oni file out of it. This is what I do in the transformMD() function of this program

***************************************************************

   char filename[100];

        static int count = 1;

        sprintf(filename,"filename.png",count++); // here i assume that images are numbered in a sequence
        IplImage* img=0;
         img=cvLoadImage(filename,CV_LOAD_IMAGE_ANYDEPTH |      CV_LOAD_IMAGE_ANYCOLOR); // Reading 16 bit depth maps

          if(!img) printf("Could not load image file: %s\n",filename); /
      
        DepthMap& depthMap = depthMD.WritableDepthMap();




for (XnUInt32 y = 0; y < depthMap.YRes(); y++)
{
for (XnUInt32 x = 0; x < depthMap.XRes(); x++)
{

CvScalar s;
              s=cvGet2D(img,y,x); // getting the value in s 
             depthMap(x,y) = s.val[0]; // setting the value to the modified depth pixel 


}
}

cvReleaseImage(&img);
*****************************************************************************

Wednesday, May 2, 2012

Installing Skeltrack on Ubuntu


Skeltrack  is an Open Source skeletal tracker and tracks up to 7 joints currently: head shoulders, elbows, and hands .Openni and Microsoft SDK trackers are not open source, so this might provide developers a good platform to tweak the algorithm and parameters according to their application :)

How to install skeltrack on ubuntu 11.10
Sourcehttp://pastebin.com/KyF9AaBt

#need clutter 1.8 or greater
# make sure the glut-dev is there, I forget the package name
sudo apt-get install git-core cmake  pkg-config build-essential libxmu-dev libxi-dev libusb-1.0-0-dev
git clone git://github.com/OpenKinect/libfreenect.git
cd libfreenect
mkdir build
cd build
cmake ..
make
sudo make install
sudo ldconfig /usr/local/lib64/
#sudo glview
cd ../.. #back to src
sudo aptitude install gobject-introspection gtk-doc-tools

PKG_CONFIG_PATH=/usr/local/lib64/pkgconfig/:/usr/local/lib/pkgconfig:/usr/lib/pkgconfig
export PKG_CONFIG_PATH
git clone git://gitorious.org/gfreenect/gfreenect.git
cd gfreenect
git clean -f
libtoolize -v --copy --install
aclocal
autoconf
./autogen.sh
make
sudo make install
sudo ldconfig /usr/local/lib/
cd ..

sudo aptitude install libclutter-1.0-dev
export PKG_CONFIG_PATH
git clone https://github.com/joaquimrocha/Skeltrack.git
cd Skeltrack
git clean -f
libtoolize -v --copy --install
aclocal
autoconf
./autogen.sh
./configure --enable-examples=yes
make
sudo make install
cd examples
./test-kinect

If you encounter the following error

error while loading shared libraries: libfreenect.so.0.0: 
cannot open shared object file: No such file or directory



You need to refresh your ldconfig cache. The easiest way to do this is to create
 a file usr-local-libs.conf (or whatever name you wish) with the following lines:
/usr/local/lib64
/usr/local/lib
Switch to root account and move it to /etc/ld.so.conf.d/usr-local-libs.conf
Then update the ldconfig cache:
$ su root
$ mv ~/usr-local-libs.conf /etc/ld.so.conf.d/usr-local-libs.conf
$ /sbin/ldconfig -v


Source: http://openkinect.org/wiki/Getting_Started

Friday, April 20, 2012

ROS ppl_detection

ppl_detection is a ROS package for detecting and tracking people based on kinect data. Today i gave it a try and only encountered one problem.
To make it running do the following steps.

Checkout the source code in a directory {make sure this directory is added to the ROS_PATH}
Change the following line in kinect.launch 
<node pkg="rviz" name="rviz" type="rviz" args="--display-config $(find trainer)/kinect_detect.vcg"/>

to

<node pkg="rviz" name="rviz" type="rviz" args="--display-config $(find ppl_detection)/kinect_detect.vcg"/>



Change the folllowing line in kinect_detect.cpp 
"svm_models/" + model_name + ".model"
to

 "./svm_models/" + model_name + ".model"
Build the package
  • rosmake ppl_detection
  • roslaunch ppl_detection kinect_launch
  • rosrun ppl_detection kinect_detect
Hopefully you will see the rgb point cloud in rviz and if a person is in the scene you will see a bounding box with the text 'human'


Thursday, April 19, 2012

Running ROS Openni Tracker (Offline) with Recorded data '.oni' files

It is always more convenient to run the tracker on a recorded data.  I recorded an '.oni' file using Openni Sample 'NiViewer'. Initialized the Context manually rather than from the 'xml' file.

You need to make the following changes in your openni_tracker.cpp

Note:
I had created a ros package mimicking the openni_tracker {i.e with same dependencies}. Then i created an Eclipse project for it. It makes debugging and changing the code easier.



// Declare .oni player

xn::Player   g_Player;  




Instead of

/*
    string configFilename = ros::package::getPath("openni_tracker") + "/openni_tracker.xml";
    XnStatus nRetVal = g_Context.InitFromXmlFile(configFilename.c_str());
    CHECK_RC(nRetVal, "InitFromXml");

*/


    // added for Reading '.oni' file

    XnStatus nRetVal = g_Context.Init();
    CHECK_RC(nRetVal, "Init");
    nRetVal = g_Context.OpenFileRecording("your .oni file", g_Player);
    if (nRetVal != XN_STATUS_OK)
    {
    printf("Can't open recording %s\n", xnGetStatusString(nRetVal));
    return 1;
    }

  • make
  •  run openni_tracker
  • rosrun rviz rviz 
Hopefully you will see the skeleton.
Following video shows ros openni tracker on recorded data :)




Running OpenNi tracker on ROS Bag files/ Subscribed Depth topics
/ / OpenNI Context
xn::Context g_context;
g_context.Init ();
// OR initialize from an XML file
nRetVal = nRetVal = xnLogInitFromXmlFile(SAMPLE_XML_PATH);
/ / Depth generator
xn::DepthGenerator depth_generator;
depth_generator.Create (g_context);
/ / Mock depth generator
xn::MockDepthGenerator g_mock_depth;
g_mock_depth.CreateBasedOn (depth_generator, "the mock-depth" );
/ / Create user generator using mock depth generator
xn::Query g_Query;
g_Query.AddNeededNode ( "mock-depth" );
xn::UserGenerator g_user;
g_user.Create (g_context, & g_Query);
//following runs in a while loop
while (ros::ok())
{
/ / Update data
g_context.WaitOneUpdateAll (depth_generator);
/ / Get original depth map
xn::DepthMetaData depthMD;
depth_generator.GetMetaData (depthMD);
/ / Make data writable and modify
depthMD.MakeDataWritable ();



/ / Modify data ....
//write a function which changes the depth data in your original depth map
transformDepthMD(DepthMetaData& depthMD)
/ / set the data of the mock-depth generator and pose will tracking will run on this
g_mock_depth.SetData (depthMD);
//rest of the code
.............
............
}
//the function for modifying depth data could like this
void transformDepthMD(DepthMetaData& depthMD)
{
          xn::Depthmap & depthMap = depthMD.WritableDepthMap ();
          for ( unsigned int y = 0; y < depthMap.YRes (); + + y)
                   for ( unsigned int x = 0; x < depthMap.XRes (); + + x)
                           depthMap (x, y) = 0; // replace the values of the depth map by recorded depth map
}




Wednesday, April 18, 2012

Building OpenNI from Source on Linux

You will find lot of tutorials on building OpenNI from source on ubuntu.
I encountered only one error and had to go through number of blogs to figure it out.

Error is something like this

/usr/bin/ld: ./x64-Release/NiSimpleViewer.o: undefined reference to symbol 'glEnd'
/usr/bin/ld: note: 'glEnd' is defined in DSO /usr/lib/libGL.so.1 so try adding it to the linker command line
/usr/lib/libGL.so.1: could not read symbols: Invalid operation
collect2: ld returned 1 exit status

Solution

Goto OpenNI/Platform/Linux/Build/Samples/

In each sample try to find the line

USED_LIBS += glut

and replace it with

 USED_LIBS += glut GL

hope it helps :)

Wednesday, April 11, 2012

Openni Tracker with ROS

I have read on many posts that guys sometimes have problem using the openni_tracker, to me also it appeared at first that openni tracker is not working properly or something. There are few things to keep in mind.
Openni_tracker will not show anything except console messages, like Calibration started, Tracking started. To visualize the tracker run rviz. And add TF.
TF transforms will not be published until the program has started tracking the user. Openni_tracker works fine if the camera can see the whole body or till knees at least.


Kinect Driver and Samples Installation on Ubuntu 11.04

Nice tutorial for the drivers installation on Ubuntu


http://tirokartblog.wordpress.com/2011/06/22/kinect-driver-and-samples-installation-on-ubuntu-11-04/