Clearpath Robotics Husky A200 Simulation environment released

Just a quick update on the latest release of my simulation environment for Clearpath Robotics’ Husky A200 4-wheel differential drive robot in ROS and Gazebo. I will be adding some tutorials on the same soon.

Here is the link to the source:
http://clearpath-ros-pkg.googlecode.com/svn/trunk/clearpath_husky/

This is what you should see when you run the simulation:

If you have any questions on the use of the same, please feel free to get back to me via comments and i will try to get back to you on the same.

ROS + OpenCV : Read a Video File using FFMPEG

In ROS diamondback, they disabled the video interface options. That means you can not capture video from a file or a Web Camera using the standard OpenCV function calls. For more details, please visit: http://answers.ros.org/question/1128/opencv-videocapture-not-working-in-diamondback.

This frustrated me to no end, so i decided to write my own class based on FFMPEG to read a video File. This program shows how to capture frames from a video file using FFMPEG, store them in OpenCV cv::Mat template and display the result in a HighGUI Window.

Way to run it

1. Download both the packages in to a folder: DOWNLOAD LINK , and make sure to add the folder to the ROS_PACKAGE_PATH. I have it in my HOME directory, so i added the following two lines at the end of my .bashrc:

   export ROS_PACKAGE_PATH=~/testROSOpenCVFFMPEG:$ROS_PACKAGE_PATH

   Make sure to restart the terminal window for it to take effect.

2. I have included a sample video located in the samples sub-folder. Make sure to change the path of the input Video file on the line 350: fileName=”/home/sid/testROSOpenCVFFMPEG/samples/testVideo.avi”; to the full path and name of the file for your case.
3. If you would like to save all the frames of the video as *.ppm files, make sure to comment out the code from line 302 to 308.
4. Open a new terminal window and type the following:

   $ sudo bash

   Provide your password, and you will then have root access.

   # roscd testROSOpenCVFFMPEG/

   # rosmake testROSOpenCVFFMPEG

   Make sure that the package built with no errors.
   To run it, type:

   # rosrun testROSOpenCVFFMPEG testROSOpenCVFFMPEG 

5. You should now see an OpenCV Window. The Window is waiting for you to press a key, so that it can grab the next frame from the video file. So to step, keep pressing “ANY” key. Have FUN with it! If you break it, let me know how you managed to do it, and i will try and fix……maybe

testROSOpenCVFFMPEG Source Code:

/*
Copyright (c) 2011, Siddhant Ahuja (Sid)
All rights reserved.

Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
    * Redistributions of source code must retain the above copyright
      notice, this list of conditions and the following disclaimer.
    * Redistributions in binary form must reproduce the above copyright
      notice, this list of conditions and the following disclaimer in the
      documentation and/or other materials provided with the distribution.
    * Neither the name of the <organization> nor the
      names of its contributors may be used to endorse or promote products
      derived from this software without specific prior written permission.

THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL <COPYRIGHT HOLDER> BE LIABLE FOR ANY
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
/*
This program shows how to capture frames from a video file using FFMPEG, store 
them in OpenCV cv::Mat template and display the result in a HighGUI Window.
*/

//Includes all the headers necessary to use the most common public pieces of the ROS system.
#include <ros/ros.h>
//Include all the FFMPEG header files.
extern "C" { 
	//Library containing decoders and encoders for audio/video codecs.
	#include <libavcodec/avcodec.h>
	//Library containing demuxers and muxers for multimedia container formats.
	#include <libavformat/avformat.h>
	//Library performing highly optimized image scaling and color space/pixel format conversion operations.
	#include <libswscale/swscale.h>
}
//Include headers for OpenCV Image processing.
#include <opencv2/imgproc/imgproc.hpp>
//Include headers for OpenCV GUI handling.
#include <opencv2/highgui/highgui.hpp>

using namespace std;

//Declare a string with the name of the window that we will create using OpenCV where processed images will be displayed.
static const char WINDOW[] = "Test ROS + OpenCV + FFMPEG";

//Create a Structure containing the OpenCV cv::Mat format and the status of error.
struct openCVFrameContext
{
	cv::Mat cvFrameBGR;
	bool errorStatus;
};

//This is the main class that contains all public and protected members.
class Capture_FFMPEG
{
	public:
		//Constructor.
		Capture_FFMPEG() {init();}
		
		//Deconstructor.
		~Capture_FFMPEG() { close();}
		
		//Function to open a Video File.
		virtual bool openVideoFile(const char* fileName);
		
		//Function to grab a frame and retrieve the data. This returns a structure that contains a matrix of type cv::Mat and the error status.
		virtual openCVFrameContext queryFrame();
		
		//Function to save the frame data as *.ppm file.
		virtual int saveFrame(AVFrame *pFrame, int width, int height, int iFrame);
		
	protected:
		//Function to initialize all protected variables.
		virtual void init();
		
		//Function to destroy all protected variables, structures and free up memory.
		virtual void close();
		
		char * fileName; //Variable to store the file name.
		int videoStream; //Variable to store the index of the Video Stream.
		int frameFinished; //Variable to determine if the frame data was fetched properly.
		AVFormatContext *pVFormatCtx;
		AVCodecContext *pVCodecCtx;
		AVCodec *pVCodec;
		AVFrame *pVFrame;
		AVFrame *pVFrameBGR;
		uint8_t *bufferBGR;
		AVPacket pVPacket;
		openCVFrameContext cvFrameContext;
		struct SwsContext *pVImgConvertCtx;
};

//Function to initialize all protected variables.
void Capture_FFMPEG::init()
{
	fileName=NULL;
	videoStream = -1;
	frameFinished = 0;	
};

//Function to destroy all protected variables, structures and free up memory.
void Capture_FFMPEG::close()
{
	printf("close:Destroying Variables. \n");
	
	// Free the AVFrame
	if(pVFrame)
	{
		printf("close:Freeing AVFrame. \n");
		av_free(pVFrame);
	}
	
	// Free the AVFrame
	if(pVFrameBGR)
	{
		printf("close:Freeing AVFrameBGR. \n");
		av_free(pVFrameBGR);
	}
	
  	// Free the packet that was allocated by av_read_frame.
  	if(&pVPacket)
  	{	
  		printf("close:Freeing Packet. \n");
  		av_free_packet(&pVPacket);
  	}
  	
  	// Free SwsContext.
  	if(pVImgConvertCtx)
  	{
  		printf("close:Freeing SWSContext. \n");
		sws_freeContext(pVImgConvertCtx);
	}
	
	// Close the codec.
	if(pVCodecCtx)
	{
		printf("close:Closing Codec. \n");
		avcodec_close(pVCodecCtx);
	}
	
	// Close the video file.
	if(pVFormatCtx)
	{
		printf("close:Closing Video File. \n");
  		av_close_input_file(pVFormatCtx);
  	}
};
//Function to open a Video File.
bool Capture_FFMPEG::openVideoFile(const char* fileName)
{
	bool errorStatus=false; //true: error.
		
	//Open the input file, and if unable to then return true. This function reads the file header and stores information about the file format in the AVFormatContext structure. The last three arguments are used to specify the file format, buffer size, and format options, but by setting this to NULL or 0, libavformat will auto-detect these.
	if(av_open_input_file(&pVFormatCtx, fileName, NULL, 0, NULL)!=0)
	{
		fprintf(stderr, "ERROR:openVideoFile:Could not open the video file\n");
		return errorStatus=true;
	}

	// Retrieve stream information. Populates pVFormatCtx->streams with the proper information.
	if(av_find_stream_info(pVFormatCtx)<0)
	{
		fprintf(stderr, "ERROR:openVideoFile:Could not open the stream\n");
		return errorStatus=true;
	}
	
	// Dump information about file onto standard error.
  	dump_format(pVFormatCtx, 0, fileName, 0);
	
	videoStream=-1;
	
	// Here we are only concerned with video streams, so find the index of the first video stream.
	for(int i=0; i< pVFormatCtx->nb_streams; i++)
	{
		if(pVFormatCtx->streams[i]->codec->codec_type==CODEC_TYPE_VIDEO) 
		{
			videoStream=i;
			break;
		}
	}
	
	//Check videoStream variable and if none found, throw error.
	if(videoStream<0)
	{
		fprintf(stderr, "ERROR:openVideoFile:Could not find a valid Video stream\n");
		return errorStatus=true;
	}
	
	//Get a pointer to the codec context for the video stream. The stream's information about the codec is in what we call the "codec context." This contains all the information about the codec that the stream is using, and now we have a pointer to it.
	pVCodecCtx=pVFormatCtx->streams[videoStream]->codec;
	
	// Find the actual codec and open it. Find the decoder for the video stream.
	pVCodec=avcodec_find_decoder(pVCodecCtx->codec_id);
	
	if(pVCodec==NULL) {
		fprintf(stderr, "ERROR:openVideoFile:Unsupported codec or codec not found!\n");
		return errorStatus=true;
	}
	printf("openVideoFile:Decoder: %s\n", pVCodec->name);
	
	// Open the codec.
	if(avcodec_open(pVCodecCtx, pVCodec)<0)
	{
		fprintf(stderr, "ERROR:openVideoFile:Could not open codec!\n");
		return errorStatus=true;
	}
	
	//Initialize a place to actually store the frame.
	pVFrame=avcodec_alloc_frame();
	
	// Allocate an AVFrame structure.
	pVFrameBGR=avcodec_alloc_frame();
	
	if(pVFrameBGR==NULL) {
		fprintf(stderr, "ERROR:openVideoFile:Could Not Allocate the frame!\n");
		return errorStatus=true;
	}
	
	//Allocate the place to put the raw data when we convert it. We use avpicture_get_size to get the size we need, and allocate the space manually.
	//Determine required buffer size.
	int numBytes=avpicture_get_size(PIX_FMT_BGR24, pVCodecCtx->width, pVCodecCtx->height);
	//printf("openVideoFile:Buffer Size: %d bytes\n", numBytes);
	
	//Allocate the Buffer.
	bufferBGR=(uint8_t *)av_malloc(numBytes*sizeof(uint8_t));
	
	// Assign appropriate parts of buffer to image planes in pVFrameBGR. Note that pVFrameBGR is an AVFrame, but AVFrame is a superset of AVPicture.
	avpicture_fill((AVPicture *)pVFrameBGR, bufferBGR, PIX_FMT_BGR24, pVCodecCtx->width, pVCodecCtx->height);
	
	// Create an OpenCV Matrix with 3 channels.
	cvFrameContext.cvFrameBGR.create(pVCodecCtx->height, pVCodecCtx->width ,CV_8UC(3));
	
	//Final return of Error Status.
	return errorStatus;
};

//Function to grab a frame and retrieve the data. This returns a structure that contains a matrix of type cv::Mat and the error status.
openCVFrameContext Capture_FFMPEG::queryFrame()
{
	cvFrameContext.errorStatus=false; //true: error.
	
	//Return the next frame of a stream. The information is stored as a packet in pkt.
	if(av_read_frame(pVFormatCtx, &pVPacket)<0)
	{
		fprintf(stderr, "ERROR:queryFrame:Could not Read Frame!\n");
		cvFrameContext.errorStatus=true;
		return cvFrameContext; // Codec not found.
	}
	
	//Check if the packet belonged to the video stream.
	if(pVPacket.stream_index==videoStream) 
	{
		//printf("queryFrame:Decoding Video.\n");
		// Decode the video frame from the input buffer buf of size buf_size. To decode it, it makes use of the videocodec which was coupled with avctx using avcodec_open(). The resulting decoded frame is stored in picture.
		if(avcodec_decode_video(pVCodecCtx, pVFrame, &frameFinished, pVPacket.data, pVPacket.size)<0)
		{
			fprintf(stderr, "ERROR:queryFrame:Could Not Decode Video!\n");
			cvFrameContext.errorStatus=true;
			return cvFrameContext;
		}
		
		// Did we get a video frame?
		if(frameFinished) 
		{
			// printf("queryFrame:Querying Frame Number %d Finished. Now Converting Color Space and Scaling\n", pVCodecCtx->frame_number);
			// Convert the image from its native format to BGR.
			// Return an SwsContext to be used in sws_scale.
			if(pVImgConvertCtx==NULL)
			{
				pVImgConvertCtx = sws_getContext(pVCodecCtx->width, pVCodecCtx->height, pVCodecCtx->pix_fmt, pVCodecCtx->width, pVCodecCtx->height, PIX_FMT_BGR24, SWS_BICUBIC, NULL, NULL, NULL);
			}
			
			if(pVImgConvertCtx==NULL)
			{
				fprintf(stderr, "ERROR:queryFrame:Cannot initialize the conversion context!\n");
				cvFrameContext.errorStatus=true;
				return cvFrameContext;
			}
			
			// Scales the data in src according to our settings in our SwsContext.
			sws_scale(pVImgConvertCtx, pVFrame->data, pVFrame->linesize, 0, pVCodecCtx->height, pVFrameBGR->data, pVFrameBGR->linesize);
			
			// Populate the OpenCV Matrix.
			for(int y=0; y<pVCodecCtx->height; y++)
			{
        			for (int x=0; x<pVCodecCtx->width; x++)
        			{
					cvFrameContext.cvFrameBGR.at<cv::Vec3b>(y,x)[0]=pVFrameBGR->data[0][y * pVFrameBGR->linesize[0] + x * 3 + 0];
					cvFrameContext.cvFrameBGR.at<cv::Vec3b>(y,x)[1]=pVFrameBGR->data[0][y * pVFrameBGR->linesize[0] + x * 3 + 1];
					cvFrameContext.cvFrameBGR.at<cv::Vec3b>(y,x)[2]=pVFrameBGR->data[0][y * pVFrameBGR->linesize[0] + x * 3 + 2];
					
				}
			}			
			
			// Save Frame as *.ppm file.
			/*if(saveFrame(pVFrameBGR, pVCodecCtx->width, pVCodecCtx->height, pVCodecCtx->frame_number)<0)
			{
				fprintf(stderr, "ERROR:queryFrame:Could not Save Frame!\n");
				cvFrameContext.errorStatus=true;
				return cvFrameContext;
			}*/
			
		}
	}
	
	//Final return.
	return cvFrameContext;
};

//Function to save the frame data as *.ppm file.
int Capture_FFMPEG::saveFrame(AVFrame *pFrame, int width, int height, int iFrame) {

	FILE *pFile;
	char szFilename[32];
	int  y;

	// Open file.
	sprintf(szFilename, "frame%d.ppm", iFrame);
	pFile=fopen(szFilename, "wb");
	if(pFile==NULL)
		return -1;

	// Write header.
	fprintf(pFile, "P6\n%d %d\n255\n", width, height);
	
	printf("saveFrame: %s of size %d x %d \n", szFilename, width, height);
	
	// Write pixel data.
	for(y=0; y<height; y++)
		fwrite(pFrame->data[0]+y*pFrame->linesize[0], 1, width*3, pFile);

	// Close file.
	fclose(pFile);
};

int main(int argc, char **argv)
{
	//Registers all available file formats and codecs with the library so they will be used automatically when a file with the corresponding format/codec is opened.
	av_register_all();
	
	//Declate the Name of the File.
	string fileName;
	fileName="/home/sid/testROSOpenCVFFMPEG/samples/testVideo.avi";
	
	//Create and initialize an object of class Capture_FFMPEG.
	Capture_FFMPEG *capture = new Capture_FFMPEG;
	
	//Create a Sturcture that stores the return output of queryFrame();
	openCVFrameContext testCVFrameContext;
	
	//Initialize the errorStatus to false.
	testCVFrameContext.errorStatus=false;
	
	//Open the stream capture.
	testCVFrameContext.errorStatus=capture->openVideoFile(fileName.c_str());
	
	if(testCVFrameContext.errorStatus==true)
	{
		return 1;
	}
	
	//OpenCV HighGUI call to create a display window on start-up.
	cv::namedWindow(WINDOW, CV_WINDOW_AUTOSIZE);
	cvMoveWindow(WINDOW, 50, 50); 
	
	while(!testCVFrameContext.errorStatus)
	{
		//Query Frame
		testCVFrameContext=capture->queryFrame();
		
		//Display the Data in the OpenCV HighGUI Window.
		cv::imshow(WINDOW, testCVFrameContext.cvFrameBGR);
		
		//Wait for Keyboard input. The function only works if there is at least one HighGUI window created and the window is active. If there are several HighGUI windows, any of them can be active.
		cv::waitKey(0);
	}
	
	//Destroy the object
	delete capture;
	
	//Final Return 
	return 0;
}

ROS Packages for Color Thresholding an Image

Just finished programming two packages needed to quickly determine the thresholds for an image :

• colorThresholdReconfigure: GUI based on wxWidgets helps determine the parameters
• colorThresholdProcess: Code that uses OpenCV to threshold image and show the result in a HighGUI window

When you run both the nodes, you will see the following GUI:

Way to run it

1. Download both the packages in to a folder: DOWNLOAD LINK , and make sure to add both the folders to the ROS_PACKAGE_PATH. I have them in my HOME directory, so i added the following two lines at the end of my .bashrc:

   export ROS_PACKAGE_PATH=~/colorThresholdReconfigure:$ROS_PACKAGE_PATH
   export ROS_PACKAGE_PATH=~/colorThresholdProcess:$ROS_PACKAGE_PATH

   Make sure to restart the terminal windows for it to take effect.

2. Next open a terminal window and run roscore.
3. Open another terminal window and type the following:

   $ sudo bash

   Provide your password, and you will then have root access.

   # roscd colorThresholdReconfigure/

   # rosmake colorThresholdReconfigure

   Make sure that the package built with no errors.
   To run it, type:

   # rosrun colorThresholdReconfigure colorThresholdReconfigure 

4. Open another terminal window and type the following:

   $ sudo bash

   Provide your password, and you will then have root access.

   # roscd colorThresholdProcess/

   # rosmake colorThresholdProcess

   Make sure that the package built with no errors.
   To run it, type:

   # rosrun colorThresholdProcess colorThresholdProcess 

5. Have FUN with it! If you break it, let me know how you managed to do it, and i will try and fix……maybe

colorThresholdReconfigure Source Code:

/*
Copyright (c) 2011, Siddhant Ahuja (Sid)
All rights reserved.

Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
    * Redistributions of source code must retain the above copyright
      notice, this list of conditions and the following disclaimer.
    * Redistributions in binary form must reproduce the above copyright
      notice, this list of conditions and the following disclaimer in the
      documentation and/or other materials provided with the distribution.
    * Neither the name of the <organization> nor the
      names of its contributors may be used to endorse or promote products
      derived from this software without specific prior written permission.

THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL <COPYRIGHT HOLDER> BE LIABLE FOR ANY
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
/*
This program provides a GUI interface for determining parameters needed to 
threshold an image.
*/

//Includes all the headers necessary to use the ROS system.
#include <ros/ros.h>
//Include headers for wxWidgets GUI handling.
#include <wx/wx.h>
#include <wx/aboutdlg.h>
#include <wx/spinctrl.h>

//Declare a list of unique IDs which would be used for event handlers.
enum
{
    	ID_About=1,
	ID_Mode, 
	ID_sourceColorSpace, 
	ID_convertToColorSpace, 
	ID_Output, 
	ID_minChannel_1, 
	ID_maxChannel_1, 
	ID_minChannel_2, 
	ID_maxChannel_2, 
	ID_minChannel_3, 
	ID_maxChannel_3
};


/*
Declare the App to set and get application-wide properties, implement the 
windowing system message or event loop, initiate application processing via 
wxApp::OnInit, and allow default processing of events not handled by other 
objects in the application.
*/

class CTRApp : public wxApp
{
public:
	//Initialization function.
	virtual bool OnInit();	
	//Exit function.
	int OnExit();

	//Global Variables.
	char** local_argv_;
};

/*
Declare macro IMPLEMENT_APP(appClass) to tell wxWidgets how to create an 
instance of the application class.
*/
IMPLEMENT_APP(CTRApp);
/*
Use DECLARE_APP(appClass) so that wxGetApp function (which returns a reference 
to your application object) is visible to other files.
*/
DECLARE_APP(CTRApp);

//Declare a frame-window class.
class CTRFrame: public wxFrame
{
public:
    	CTRFrame(const wxString& title, const wxPoint& pos, const wxSize& size);
protected: 
	//Called when the user clicks the About Option in the ToolBar.
    	void OnAbout(wxCommandEvent& event);
	void OnUpdateMode(wxCommandEvent& event);
	void OnUpdateSourceColorSpace(wxCommandEvent& event);
	void OnUpdateConvertToColorSpace(wxCommandEvent& event);
	void OnUpdateOutput(wxCommandEvent& event);
	void OnUpdateMinChannel_1(wxSpinEvent& event);
	void OnUpdateMaxChannel_1(wxSpinEvent& event);
	void OnUpdateMinChannel_2(wxSpinEvent& event);
	void OnUpdateMaxChannel_2(wxSpinEvent& event);
	void OnUpdateMinChannel_3(wxSpinEvent& event);
	void OnUpdateMaxChannel_3(wxSpinEvent& event);
	//Global Variables.
	wxString currImagePath;
private:
	//Every event handler needs its event table entry.
    	DECLARE_EVENT_TABLE();
};

//Declare Event Table.

BEGIN_EVENT_TABLE(CTRFrame, wxFrame)
	EVT_MENU(ID_About, CTRFrame::OnAbout)
	EVT_CHOICE(ID_Mode, CTRFrame::OnUpdateMode) 
	EVT_CHOICE(ID_sourceColorSpace, CTRFrame::OnUpdateSourceColorSpace) 
	EVT_CHOICE(ID_convertToColorSpace, CTRFrame::OnUpdateConvertToColorSpace) 
	EVT_CHOICE(ID_Output, CTRFrame::OnUpdateOutput)
	EVT_SPINCTRL(ID_minChannel_1, CTRFrame::OnUpdateMinChannel_1)
	EVT_SPINCTRL(ID_maxChannel_1, CTRFrame::OnUpdateMaxChannel_1)
	EVT_SPINCTRL(ID_minChannel_2, CTRFrame::OnUpdateMinChannel_2)
	EVT_SPINCTRL(ID_maxChannel_2, CTRFrame::OnUpdateMaxChannel_2)
	EVT_SPINCTRL(ID_minChannel_3, CTRFrame::OnUpdateMinChannel_3)
	EVT_SPINCTRL(ID_maxChannel_3, CTRFrame::OnUpdateMaxChannel_3)
END_EVENT_TABLE()

//Initialize the Frame.
CTRFrame::CTRFrame(const wxString& title, const wxPoint& pos, const wxSize& size): wxFrame( NULL, -1, title, pos, size )
{
	//Declare a File Menu.
	wxMenu *menuFile = new wxMenu;
	//Add Items to the array.
	menuFile->Append( ID_About, _("&About...") );
	//Declare the Menu Bar.
	wxMenuBar *menuBar = new wxMenuBar;
	//Add items from File Menu into the Menu Bar.
	menuBar->Append( menuFile, _("&File") );
	//Set the Menu Bar.
	SetMenuBar( menuBar );
	
	//Initialize All Local Variables
	int CTR_mode;
	int CTR_sourceColorSpace;
	int CTR_convertToColorSpace;
	int CTR_output;
	int CTR_minChannel_1;
	int CTR_maxChannel_1;
	int CTR_minChannel_2;
	int CTR_maxChannel_2;
	int CTR_minChannel_3;
	int CTR_maxChannel_3;
	std::string CTR_imagePath;

	//Populate all Local Variables
	ros::param::get("/CTR_mode", CTR_mode);	
	ros::param::get("/CTR_sourceColorSpace", CTR_sourceColorSpace);
	ros::param::get("/CTR_convertToColorSpace", CTR_convertToColorSpace);
	ros::param::get("/CTR_output", CTR_output);
	ros::param::get("/CTR_minChannel_1", CTR_minChannel_1);
	ros::param::get("/CTR_maxChannel_1", CTR_maxChannel_1);
	ros::param::get("/CTR_minChannel_2", CTR_minChannel_2);
	ros::param::get("/CTR_maxChannel_2", CTR_maxChannel_2);
	ros::param::get("/CTR_minChannel_3", CTR_minChannel_3);
	ros::param::get("/CTR_maxChannel_3", CTR_maxChannel_3);
	ros::param::get("/CTR_imagePath", CTR_imagePath);
	
	//Declare all Labels.
	
	wxStaticText *lblSelectMode = new wxStaticText(this, -1,_T("Mode"),wxPoint(10, 20), wxSize(100,25));	
	wxStaticText *lblSelectSourceColorSpace = new wxStaticText(this, -1,_T("Source Color Space"),wxPoint(10, 60), wxSize(155,25));
	wxStaticText *lblSelectConvertToColorSpace = new wxStaticText(this, -1,_T("Convert Color Space To"),wxPoint(10, 100), wxSize(155,25));
	wxStaticText *lblOutputType = new wxStaticText(this, -1,_T("Output Type"),wxPoint(10, 140), wxSize(100,25));	
	wxStaticText *lblMin = new wxStaticText(this, -1,_T("Min"),wxPoint(195, 180), wxSize(100,25));	
	wxStaticText *lblMax = new wxStaticText(this, -1,_T("Max"),wxPoint(320, 180), wxSize(100,25));
	wxStaticText *lblChannel_1 = new wxStaticText(this, -1,_T("Channel 1"),wxPoint(10, 220), wxSize(100,25));
	wxStaticText *lblChannel_2 = new wxStaticText(this, -1,_T("Channel 2"),wxPoint(10, 260), wxSize(100,25));
	wxStaticText *lblChannel_3 = new wxStaticText(this, -1,_T("Channel 3"),wxPoint(10, 300), wxSize(100,25));
	
	//Initialize all Controls
	
	wxChoice *modeSelect=new wxChoice(this, ID_Mode, wxPoint(195,15), wxSize(200,25));
	modeSelect->Append(_T("Select Mode"));
	modeSelect->Append(_T("Image"));
	modeSelect->SetSelection(CTR_mode);

	wxChoice *sourceColorSpaceSelect=new wxChoice(this, ID_sourceColorSpace, wxPoint(195,55), wxSize(200,25));
	sourceColorSpaceSelect->Append(_T("Select Color Space"));
	sourceColorSpaceSelect->Append(_T("BGR"));
	sourceColorSpaceSelect->Append(_T("RGB"));
	sourceColorSpaceSelect->SetSelection(CTR_sourceColorSpace);
	
	wxChoice *convertToColorSpaceSelect=new wxChoice(this, ID_convertToColorSpace, wxPoint(195,95), wxSize(200,25));
	convertToColorSpaceSelect->Append(_T("Select Color Space"));
	convertToColorSpaceSelect->Append(_T("BGR"));
	convertToColorSpaceSelect->Append(_T("RGB"));
	convertToColorSpaceSelect->Append(_T("Gray"));
	convertToColorSpaceSelect->Append(_T("HSV"));
	convertToColorSpaceSelect->Append(_T("HLS"));
	convertToColorSpaceSelect->Append(_T("CIE L*a*b*"));
	convertToColorSpaceSelect->Append(_T("CIE L*u*v*"));
	convertToColorSpaceSelect->Append(_T("CIE XYZ"));
	convertToColorSpaceSelect->Append(_T("YCrCb JPEG (a.k.a. YCC)"));
	convertToColorSpaceSelect->SetSelection(CTR_convertToColorSpace);

	wxChoice *outputTypeSelect=new wxChoice(this, ID_Output, wxPoint(195,135), wxSize(200,25));
	outputTypeSelect->Append(_T("Select Output Type"));
	outputTypeSelect->Append(_T("Color Preserve"));
	outputTypeSelect->Append(_T("Binarized"));
	outputTypeSelect->Append(_T("Binarized Inverted"));
	outputTypeSelect->SetSelection(CTR_output);

	wxSpinCtrl *minChannel_1 = new wxSpinCtrl(this, ID_minChannel_1, wxEmptyString, wxPoint(195,215), wxSize(65, 25), wxSP_ARROW_KEYS, 0, 255, CTR_minChannel_1 , _T("spinCtrlMinChannel_1"));

	wxSpinCtrl *maxChannel_1 = new wxSpinCtrl(this, ID_maxChannel_1, wxEmptyString, wxPoint(320,215), wxSize(65, 25), wxSP_ARROW_KEYS, 0, 255, CTR_maxChannel_1, _T("spinCtrlMaxChannel_1"));

	wxSpinCtrl *minChannel_2 = new wxSpinCtrl(this, ID_minChannel_2, wxEmptyString, wxPoint(195,255), wxSize(65, 25), wxSP_ARROW_KEYS, 0, 255, CTR_minChannel_2, _T("spinCtrlMinChannel_2"));

	wxSpinCtrl *maxChannel_2 = new wxSpinCtrl(this, ID_maxChannel_2, wxEmptyString, wxPoint(320,255), wxSize(65, 25), wxSP_ARROW_KEYS, 0, 255, CTR_maxChannel_2, _T("spinCtrlMaxChannel_2"));

	wxSpinCtrl *minChannel_3 = new wxSpinCtrl(this, ID_minChannel_3, wxEmptyString, wxPoint(195,295), wxSize(65, 25), wxSP_ARROW_KEYS, 0, 255, CTR_minChannel_3, _T("spinCtrlMinChannel_3"));

	wxSpinCtrl *maxChannel_3 = new wxSpinCtrl(this, ID_maxChannel_3, wxEmptyString, wxPoint(320,295), wxSize(65, 25), wxSP_ARROW_KEYS, 0, 255, CTR_maxChannel_3, _T("spinCtrlMaxChannel_3"));
	
	//Create th Status Bar
	CreateStatusBar();
	//Set Status Bar Text
	SetStatusText(wxT("Welcome to Color Threshold Reconfigure App"));
};

//Application Initialization function.
bool CTRApp::OnInit()
{
	ROS_INFO("Color Threshold Reconfigure App::Initializing");
	// Create local copy of argv, with regular char*s.
	local_argv_ =  new char*[ argc ];
	for ( int i = 0; i < argc; ++i )
	{
		local_argv_[ i ] = strdup( wxString( argv[ i ] ).mb_str() );
	}
	
	/**
	* The ros::init() function needs to see argc and argv so that it can perform
	* any ROS arguments and name remapping that were provided at the command line. For programmatic
	* remappings you can use a different version of init() which takes remappings
	* directly, but for most command-line programs, passing argc and argv is the easiest
	* way to do it.  The third argument to init() is the name of the node. Node names must be unique in a running system.
	* The name used here must be a base name, ie. it cannot have a / in it.
	* You must call one of the versions of ros::init() before using any other
	* part of the ROS system.
	*/
	ros::init(argc, local_argv_, "colorThresholdReconfigure");

	//Initialize all Session Wide Parameters

	ros::param::set("CTR_mode", 0);
	ros::param::set("CTR_sourceColorSpace", 0);
	ros::param::set("CTR_convertToColorSpace", 0);
	ros::param::set("CTR_output", 0);
	ros::param::set("CTR_minChannel_1", 0);
	ros::param::set("CTR_maxChannel_1", 255);
	ros::param::set("CTR_minChannel_2", 0);
	ros::param::set("CTR_maxChannel_2", 255);
	ros::param::set("CTR_minChannel_3", 0);
	ros::param::set("CTR_maxChannel_3", 255);
	ros::param::set("CTR_imagePath", "");
	
	//Declare the frame window
	CTRFrame *frame = new CTRFrame(_("Color Threshold Reconfigure App"), wxPoint(50, 50), wxSize(410,400) );
	//Show the frame
	frame->Show(TRUE);
	//Set it as the top most window
	SetTopWindow(frame);
	
	return TRUE;
};

//Application Exit function.
int CTRApp::OnExit()
{
	ROS_INFO("Color Threshold Reconfigure App::Exiting");
	for ( int i = 0; i < argc; ++i )
	{
		free( local_argv_[ i ] );
	}
	delete [] local_argv_;

	//Delete All Parameters
	ros::param::del("CTR_mode");
	ros::param::del("CTR_sourceColorSpace");
	ros::param::del("CTR_convertToColorSpace");
	ros::param::del("CTR_output");
	ros::param::del("CTR_minChannel_1");
	ros::param::del("CTR_maxChannel_1");
	ros::param::del("CTR_minChannel_2");
	ros::param::del("CTR_maxChannel_2");
	ros::param::del("CTR_minChannel_3");
	ros::param::del("CTR_maxChannel_3");
	ros::param::del("CTR_imagePath");
	return 0;
};

//Called when user clicks the About item in the menu
void CTRFrame::OnAbout(wxCommandEvent& WXUNUSED(event))
{
	ROS_INFO("Color Threshold Reconfigure App::Menu::About");
	wxAboutDialogInfo *info=new wxAboutDialogInfo;
    	info->SetName(_("Color Threshold Reconfigure App"));
    	info->SetVersion(_("1.0.0"));
    	info->SetDescription(_("This program provides a GUI interface for determining parameters needed to threshold an image"));
    	info->SetCopyright(_T("(C) 2011 Siddhant Ahuja (Sid) <siddahuja@gmail.com>"));
    	wxAboutBox(*info);
};

void CTRFrame::OnUpdateMode(wxCommandEvent& event)
{	
	wxChoice* choice= dynamic_cast<wxChoice*>(event.GetEventObject());
	int currModeValue=choice->GetCurrentSelection();
	wxString currModeString=choice->GetStringSelection();
	//Set Parameter
	ros::param::set("CTR_mode", currModeValue);
	ROS_INFO("Color Threshold Reconfigure App::Current Mode Selected < Value , String > : < %d, %s >",currModeValue, (const char*)currModeString.mb_str());

	switch (currModeValue)
	{
		case 0:
			{
				//Reset All Params
				ros::param::set("CTR_imagePath", "");
			}
			break;
		case 1:
			{
				ROS_INFO("Color Threshold Reconfigure App::Select Image");	
				wxFileDialog *imageOpen=new wxFileDialog(this, _T("Choose a file"), wxEmptyString, wxEmptyString, _T("All Supported Image Formats|*.bmp;*.dib;*.jpeg;*.jpg;*.jpe;*.jp2;*.png;*.pbm;*.pgm;*.ppm;*.sr;*.ras;*.tiff;*.tif"), wxFD_DEFAULT_STYLE, wxDefaultPosition, wxDefaultSize, _T("Open an Image"));
				// Creates a Open Image Dialog
				if (imageOpen->ShowModal() == wxID_OK) 
				{
					//ROS_INFO("Color Threshold Reconfigure App::Select Image::OK");	
					//Get Image Path
					wxString currImagePath=imageOpen->GetPath();
					ROS_INFO("Color Threshold Reconfigure App::Select Image::Current Image Path : %s", (const char*)currImagePath.mb_str());
					//Set Parameter
					ros::param::set("CTR_imagePath", (const char*)currImagePath.mb_str());
				}
				else
				{
					//Set Parameter
					ros::param::set("CTR_imagePath", "");
				}
				imageOpen->Destroy();
			}
			break;
		default:
			break;
	}
};

void CTRFrame::OnUpdateSourceColorSpace(wxCommandEvent& event)
{	
	wxChoice* choice= dynamic_cast<wxChoice*>(event.GetEventObject());
	int currColorSpaceValue=choice->GetCurrentSelection();
	wxString currColorSpaceString=choice->GetStringSelection();
	//Set Parameter
	ros::param::set("CTR_sourceColorSpace", currColorSpaceValue);
	ROS_INFO("Color Threshold Reconfigure App::Current Source Color Space Selected <Value,String> : < %d, %s >",currColorSpaceValue, (const char*)currColorSpaceString.mb_str());
};

void CTRFrame::OnUpdateConvertToColorSpace(wxCommandEvent& event)
{	
	wxChoice* choice= dynamic_cast<wxChoice*>(event.GetEventObject());
	int currColorSpaceValue=choice->GetCurrentSelection();
	wxString currColorSpaceString=choice->GetStringSelection();
	//Set Parameter
	ros::param::set("CTR_convertToColorSpace", currColorSpaceValue);
	ROS_INFO("Color Threshold Reconfigure App::Current Convert To Color Space Selected <Value,String> : < %d, %s >",currColorSpaceValue, (const char*)currColorSpaceString.mb_str());
};

void CTRFrame::OnUpdateOutput(wxCommandEvent& event)
{	
	wxChoice* choice= dynamic_cast<wxChoice*>(event.GetEventObject());
	int currOutputValue=choice->GetCurrentSelection();
	wxString currOutputString=choice->GetStringSelection();
	//Set Parameter
	ros::param::set("CTR_output", currOutputValue);
	ROS_INFO("Color Threshold Reconfigure App::Current Output Selected <Value,String> : < %d, %s >",currOutputValue, (const char*)currOutputString.mb_str());
};

void CTRFrame::OnUpdateMinChannel_1(wxSpinEvent& event)
{	
	wxSpinCtrl* spinCtrl= dynamic_cast<wxSpinCtrl*>(event.GetEventObject());
	int currMinChannel_1_Value=spinCtrl->GetValue();
	//Set Parameter
	ros::param::set("CTR_minChannel_1", currMinChannel_1_Value);
	ROS_INFO("Color Threshold Reconfigure App::Current Min Channel 1 Selected <Value> : < %d >",currMinChannel_1_Value);
};

void CTRFrame::OnUpdateMaxChannel_1(wxSpinEvent& event)
{	
	wxSpinCtrl* spinCtrl= dynamic_cast<wxSpinCtrl*>(event.GetEventObject());
	int currMaxChannel_1_Value=spinCtrl->GetValue();
	//Set Parameter
	ros::param::set("CTR_maxChannel_1", currMaxChannel_1_Value);
	ROS_INFO("Color Threshold Reconfigure App::Current Max Channel 1 Selected <Value> : < %d >",currMaxChannel_1_Value);
};

void CTRFrame::OnUpdateMinChannel_2(wxSpinEvent& event)
{	
	wxSpinCtrl* spinCtrl= dynamic_cast<wxSpinCtrl*>(event.GetEventObject());
	int currMinChannel_2_Value=spinCtrl->GetValue();
	//Set Parameter
	ros::param::set("CTR_minChannel_2", currMinChannel_2_Value);
	ROS_INFO("Color Threshold Reconfigure App::Current Min Channel 2 Selected <Value> : < %d >",currMinChannel_2_Value);
};

void CTRFrame::OnUpdateMaxChannel_2(wxSpinEvent& event)
{	
	wxSpinCtrl* spinCtrl= dynamic_cast<wxSpinCtrl*>(event.GetEventObject());
	int currMaxChannel_2_Value=spinCtrl->GetValue();
	//Set Parameter
	ros::param::set("CTR_maxChannel_2", currMaxChannel_2_Value);
	ROS_INFO("Color Threshold Reconfigure App::Current Max Channel 2 Selected <Value> : < %d >",currMaxChannel_2_Value);
};

void CTRFrame::OnUpdateMinChannel_3(wxSpinEvent& event)
{	
	wxSpinCtrl* spinCtrl= dynamic_cast<wxSpinCtrl*>(event.GetEventObject());
	int currMinChannel_3_Value=spinCtrl->GetValue();
	//Set Parameter
	ros::param::set("CTR_minChannel_3", currMinChannel_3_Value);
	ROS_INFO("Color Threshold Reconfigure App::Current Min Channel 3 Selected <Value> : < %d >",currMinChannel_3_Value);
};

void CTRFrame::OnUpdateMaxChannel_3(wxSpinEvent& event)
{	
	wxSpinCtrl* spinCtrl= dynamic_cast<wxSpinCtrl*>(event.GetEventObject());
	int currMaxChannel_3_Value=spinCtrl->GetValue();
	//Set Parameter
	ros::param::set("CTR_maxChannel_3", currMaxChannel_3_Value);
	ROS_INFO("Color Threshold Reconfigure App::Current Max Channel 3 Selected <Value> : < %d >",currMaxChannel_3_Value);
};

colorThresholdProcess Source Code:

/*
Copyright (c) 2011, Siddhant Ahuja (Sid)
All rights reserved.

Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
    * Redistributions of source code must retain the above copyright
      notice, this list of conditions and the following disclaimer.
    * Redistributions in binary form must reproduce the above copyright
      notice, this list of conditions and the following disclaimer in the
      documentation and/or other materials provided with the distribution.
    * Neither the name of the <organization> nor the
      names of its contributors may be used to endorse or promote products
      derived from this software without specific prior written permission.

THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL <COPYRIGHT HOLDER> BE LIABLE FOR ANY
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
/*
This program gets the system parameters saved from the GUI interface from 
package colorThresholdReconfigure and then uses OpenCV to threshold an image. 
The resultant thresholded image is displayed in a window
and also published as a topic.
*/

//Includes all the headers necessary to use the most common public pieces of the ROS system.
#include <ros/ros.h>
//Include headers for OpenCV Image processing
#include <opencv2/imgproc/imgproc.hpp>
//Include headers for OpenCV GUI handling
#include <opencv2/highgui/highgui.hpp>
using namespace std;

//Declare a string with the name of the window that we will create using OpenCV where processed images will be displayed.
static const char WINDOW[] = "Color Threshold Process";

//Initialize All Global Variables
int CTR_mode;
int CTR_sourceColorSpace;
int CTR_convertToColorSpace;
int CTR_output;
int CTR_minChannel_1;
int CTR_maxChannel_1;
int CTR_minChannel_2;
int CTR_maxChannel_2;
int CTR_minChannel_3;
int CTR_maxChannel_3;
std::string CTR_imagePath;

//Declare a 3 channel matrix that stores the input
cv::Mat inputImg;
//Declare a 3 channel matrix that stores the output
cv::Mat outputImg;
//Create a Matrix of Zeros to be shown by default in the OpenCV Window
cv::Mat imgZeros;
//Declare the Lookup Table that will store the color space conversion constants
cv::Mat LUT_ColorSpaceConvert(2, 9, CV_8UC1);

void imgConvertColorSpace()
{
	ROS_INFO("Color Threshold Process App::Convert Color Space");
	//ROS_INFO("Color Threshold Process App::Convert Color Space::Source Color Space : < value > : < %d >", CTR_sourceColorSpace);
	//ROS_INFO("Color Threshold Process App::Convert Color Space::Convert To Color Space : < value > : < %d >", CTR_convertToColorSpace);
	if(CTR_sourceColorSpace!=CTR_convertToColorSpace && CTR_sourceColorSpace!=0 && CTR_convertToColorSpace!=0)
	{
		//ROS_INFO("Color Threshold Process App::Convert Color Space::Source and convert to color spaces DO NOT match. Attempting to convert color space.");
		//Search in the Lookup Table to find the corresponding color conversion code
		unsigned char colorConversionCode = LUT_ColorSpaceConvert.at<unsigned char>(CTR_sourceColorSpace-1,CTR_convertToColorSpace-1);
		//ROS_INFO("Color Threshold Process App::Convert Color Space::Color Conversion Code Located: < value > : < %d >", (int)colorConversionCode);

		switch (colorConversionCode)
		{
			case 0: //Conversion between same color space
				ROS_WARN("Color Threshold Process App::Convert Color Space::Attempt to convert between same color spaces.");
				break;
			case 1: //No Direct Conversion available. Convert to intermittent space like BGR
				ROS_WARN("Color Threshold Process App::Convert Color Space::No Direct Conversion available. Convert to intermittent space like BGR.");
				break;
			default: //Convert color space and store the result
				cv::cvtColor(inputImg, outputImg, colorConversionCode, 0);
				break;
		}
	}
	else
	{
		//ROS_INFO("Color Threshold Process App::Convert Color Space::Source and convert to Color spaces match <or> One of color spaces not selected. Do nothing.");
	}
};

void imgThreshold()
{
	ROS_INFO("Color Threshold Process App::Image Threshold");
	//Declare common variables
	unsigned char ch1, ch2, ch3; //stores the channel 1, channel 2, and channel 3 data
	bool dstValue;

	//Perform threshold
	for (int i=0; i< outputImg.rows; i++)
	{
		for (int j = 0; j< outputImg.cols; j++)
		{
			if(outputImg.channels()>1)
			{
				//Populate color values
				ch1 = outputImg.at<cv::Vec3b>(i,j)[0];
				ch2 = outputImg.at<cv::Vec3b>(i,j)[1];
				ch3 = outputImg.at<cv::Vec3b>(i,j)[2];
				//check range
				if(ch1>=CTR_minChannel_1 && ch1<=CTR_maxChannel_1 && ch2>=CTR_minChannel_2 && ch2<=CTR_maxChannel_2 && ch3>=CTR_minChannel_3 && ch3<=CTR_maxChannel_3)
				{
					dstValue=true;
				}
				else
				{
					dstValue=false;
				}
				if(dstValue)
				{
					switch(CTR_output)
					{
						case 0: //do nothing
							break;
						case 1: //Color Preserve
							{
								outputImg.at<cv::Vec3b>(i,j)[0]=ch1;	
								outputImg.at<cv::Vec3b>(i,j)[1]=ch2;
								outputImg.at<cv::Vec3b>(i,j)[2]=ch3;
							}
							break;
						case 2: //Binarized
							{
								outputImg.at<cv::Vec3b>(i,j)[0]=255;
								outputImg.at<cv::Vec3b>(i,j)[1]=255;
								outputImg.at<cv::Vec3b>(i,j)[2]=255;
							}
							break;	
						case 3: //Binarized Inverted
							{
								outputImg.at<cv::Vec3b>(i,j)[0]=0;
								outputImg.at<cv::Vec3b>(i,j)[1]=0;
								outputImg.at<cv::Vec3b>(i,j)[2]=0;
							}
							break;	
						default:
							break;
					}
				}
				else
				{
					switch(CTR_output)
					{
						case 0: //do nothing
							break;
						case 1: //Color Preserve
							{
								outputImg.at<cv::Vec3b>(i,j)[0]=0;
								outputImg.at<cv::Vec3b>(i,j)[1]=0;
								outputImg.at<cv::Vec3b>(i,j)[2]=0;
							}
							break;
						case 2: //Binarized
							{
								outputImg.at<cv::Vec3b>(i,j)[0]=0;
								outputImg.at<cv::Vec3b>(i,j)[1]=0;
								outputImg.at<cv::Vec3b>(i,j)[2]=0;
							}
							break;	
						case 3: //Binarized Inverted
							{	
								outputImg.at<cv::Vec3b>(i,j)[0]=255;
								outputImg.at<cv::Vec3b>(i,j)[1]=255;
								outputImg.at<cv::Vec3b>(i,j)[2]=255;
							}
							break;	
						default:
							break;
					}
				}
			}
			else
			{
				//Gray Scale Image
				//Populate color values
				ch1 = outputImg.at<unsigned char>(i,j);
				//check range
				if(ch1>=CTR_minChannel_1 && ch1<=CTR_maxChannel_1)
				{
					dstValue=true;
				}
				else
				{
					dstValue=false;
				}
				if(dstValue)
				{
					switch(CTR_output)
					{
						case 0: //do nothing
							break;
						case 1: //Color Preserve
							{
								outputImg.at<unsigned char>(i,j)=ch1;
							}
							break;
						case 2: //Binarized
							{
								outputImg.at<unsigned char>(i,j)=255;
							}
							break;	
						case 3: //Binarized Inverted
							{
								outputImg.at<unsigned char>(i,j)=0;
							}
							break;	
						default:
							break;
					}
				}
				else
				{
					switch(CTR_output)
					{
						case 0: //do nothing
							break;
						case 1: //Color Preserve
							{
								outputImg.at<unsigned char>(i,j)=0;
							}
							break;
						case 2: //Binarized
							{
								outputImg.at<unsigned char>(i,j)=0;
							}
							break;	
						case 3: //Binarized Inverted
							{	
								outputImg.at<unsigned char>(i,j)=255;
							}
							break;	
						default:
							break;
					}
				}
			}
		}
	}
	
};

void populateLUTColorSpaceConvert()
{
	ROS_INFO("Color Threshold Process App::Populating Color Space Conversion Lookup Table");
	//Row/Col order: BGR, RGB, Gray, HSV, HLS, Lab, Luv, XYZ, YCC
	//0: Conversion between same color space
	//1: No Direct Conversion available. Convert to intermittent space like BGR
	
	//BGR to BGR
	LUT_ColorSpaceConvert.at<unsigned char>(0,0) = 0;
	//BGR to RGB
	LUT_ColorSpaceConvert.at<unsigned char>(0,1) = CV_BGR2RGB;
	//BGR to Gray
	LUT_ColorSpaceConvert.at<unsigned char>(0,2) = CV_BGR2GRAY;
	//BGR to HSV
	LUT_ColorSpaceConvert.at<unsigned char>(0,3) = CV_BGR2HSV;
	//BGR to HLS
	LUT_ColorSpaceConvert.at<unsigned char>(0,4) = CV_BGR2HLS;
	//BGR to Lab
	LUT_ColorSpaceConvert.at<unsigned char>(0,5) = CV_BGR2Lab;
	//BGR to Luv
	LUT_ColorSpaceConvert.at<unsigned char>(0,6) = CV_BGR2Luv;
	//BGR to XYZ
	LUT_ColorSpaceConvert.at<unsigned char>(0,7) = CV_BGR2XYZ;
	//BGR to YCC
	LUT_ColorSpaceConvert.at<unsigned char>(0,8) = CV_BGR2YCrCb;
	
	//RGB to BGR
	LUT_ColorSpaceConvert.at<unsigned char>(1,0) = CV_RGB2BGR;
	//RGB to RGB
	LUT_ColorSpaceConvert.at<unsigned char>(1,1) = 0;
	//RGB to Gray
	LUT_ColorSpaceConvert.at<unsigned char>(1,2) = CV_RGB2GRAY;
	//RGB to HSV
	LUT_ColorSpaceConvert.at<unsigned char>(1,3) = CV_RGB2HSV;
	//RGB to HLS
	LUT_ColorSpaceConvert.at<unsigned char>(1,4) = CV_RGB2HLS;
	//RGB to Lab
	LUT_ColorSpaceConvert.at<unsigned char>(1,5) = CV_RGB2Lab;
	//RGB to Luv
	LUT_ColorSpaceConvert.at<unsigned char>(1,6) = CV_RGB2Luv;
	//RGB to XYZ
	LUT_ColorSpaceConvert.at<unsigned char>(1,7) = CV_RGB2XYZ;
	//RGB to YCC
	LUT_ColorSpaceConvert.at<unsigned char>(1,8) = CV_RGB2YCrCb;
};

/**
* This tutorial demonstrates simple image conversion between ROS image message and OpenCV formats and image processing
*/
int main(int argc, char **argv)
{
	ROS_INFO("Color Threshold Process App::Initializing");
	
	/**
	* The ros::init() function needs to see argc and argv so that it can perform
	* any ROS arguments and name remapping that were provided at the command line. For programmatic
	* remappings you can use a different version of init() which takes remappings
	* directly, but for most command-line programs, passing argc and argv is the easiest
	* way to do it.  The third argument to init() is the name of the node. Node names must be unique in a running system.
	* The name used here must be a base name, ie. it cannot have a / in it.
	* You must call one of the versions of ros::init() before using any other
	* part of the ROS system.
	*/
        ros::init(argc, argv, "colorThresholdProcess");
        
        /**
	* NodeHandle is the main access point to communications with the ROS system.
	* The first NodeHandle constructed will fully initialize this node, and the last
	* NodeHandle destructed will close down the node.
	*/
        ros::NodeHandle nh;
        
        ros::Rate loop_rate(10);
       
	//OpenCV HighGUI call to create a display window on start-up.
	cv::namedWindow(WINDOW, CV_WINDOW_AUTOSIZE);
	cvMoveWindow(WINDOW, 460, 50); 
	
	//Create a Matrix of Zeros to be shown by default in the OpenCV Window
	imgZeros=cv::Mat::zeros(240, 320, CV_8UC3);
	
	//Populate the Lookup Table
	populateLUTColorSpaceConvert();

	while (ros::ok())
	{
		//Populate all Local Variables
		ros::param::get("/CTR_mode", CTR_mode);	
		ros::param::get("/CTR_sourceColorSpace", CTR_sourceColorSpace);
		ros::param::get("/CTR_convertToColorSpace", CTR_convertToColorSpace);
		ros::param::get("/CTR_output", CTR_output);
		ros::param::get("/CTR_minChannel_1", CTR_minChannel_1);
		ros::param::get("/CTR_maxChannel_1", CTR_maxChannel_1);
		ros::param::get("/CTR_minChannel_2", CTR_minChannel_2);
		ros::param::get("/CTR_maxChannel_2", CTR_maxChannel_2);
		ros::param::get("/CTR_minChannel_3", CTR_minChannel_3);
		ros::param::get("/CTR_maxChannel_3", CTR_maxChannel_3);
		ros::param::get("/CTR_imagePath", CTR_imagePath);
		
		switch(CTR_mode)
		{
			case 0: //No option selected
				{
					//ROS_INFO("Color Threshold Process App::CTR_mode < value > : < %d > ::Clear Window", CTR_mode);
					//Display a Blank Screen
					cv::imshow(WINDOW, imgZeros);
				}
				break;
			case 1: //Process an Image
				{
					
					ROS_INFO("Color Threshold Process App::CTR_mode < value > : < %d > ::Process an Image", CTR_mode);
					if(!CTR_imagePath.empty())
					{
						//ROS_INFO("Color Threshold Process App::CTR_imagePath < string > : < %s >", CTR_imagePath.c_str());
						
						inputImg = cv::imread(CTR_imagePath.c_str(),1);
						if(inputImg.empty())
						{
							ROS_ERROR("Color Threshold Process App::Could not read image!");
							return -1;
						}
						outputImg = inputImg.clone();
						imgConvertColorSpace();
						//ROS_INFO("Color Threshold Process App::Number of Channels < value > : < %d >", outputImg.channels());
						imgThreshold();
						cv::imshow(WINDOW, outputImg);
					}
					else
					{
						//Display a Blank Screen
						cv::imshow(WINDOW, imgZeros);
					}
						
				}
				break;
			default:
				break;
		}
		
		//Add some delay in miliseconds. The function only works if there is at least one HighGUI window created and the window is active. If there are several HighGUI windows, any of them can be active.
		cv::waitKey(3);	
		
		ros::spinOnce();
		loop_rate.sleep();
	}
	//OpenCV HighGUI call to destroy a display window on shut-down.
	cv::destroyWindow(WINDOW);
	
	//ROS_INFO is the replacement for printf/cout.
	ROS_INFO("Color Threshold Process::No error.");
	return 0;
}

ROS and FFMPEG talking to each other

So after a long time of struggling with getting video capture devices to work with ROS, i decided to just write my own code based on ffmpeg. For those of you not familiar with it, visit: ffmpeg.org Little did i know that i would run into even more errors trying to get my code working with ffmpeg.

The first problem was to somehow find the libavcodec, libavformat and libavutil libraries and link them to the ROS project. To do this, you must edit the CMakeLists.txt file and use target_link_libraries() function to link them. Here is what i have in mine:

cmake_minimum_required(VERSION 2.4.6)
include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake)

# Set the build type.  Options are:
#  Coverage       : w/ debug symbols, w/o optimization, w/ code-coverage
#  Debug          : w/ debug symbols, w/o optimization
#  Release        : w/o debug symbols, w/ optimization
#  RelWithDebInfo : w/ debug symbols, w/ optimization
#  MinSizeRel     : w/o debug symbols, w/ optimization, stripped binaries
#set(ROS_BUILD_TYPE RelWithDebInfo)

rosbuild_init()

#set the default path for built executables to the "bin" directory
set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin)
#set the default path for built libraries to the "lib" directory
set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib)

#uncomment if you have defined messages
#rosbuild_genmsg()
#uncomment if you have defined services
#rosbuild_gensrv()

set(SEARCH_LIBRARY_PATHS "/usr/lib/ /usr/local/lib/ /usr/lib32/ /usr/lib64/")

FIND_LIBRARY(LIBAVFORMAT_LIBRARY NAMES avformat PATHS ${SEARCH_LIBRARY_PATHS})
if(NOT LIBAVFORMAT_LIBRARY)
  message(FATAL_ERROR "libavformat library is missing")
else()
  message(STATUS "libavformat library found : " ${LIBAVFORMAT_LIBRARY})
endif()

FIND_LIBRARY(LIBAVCODEC_LIBRARY NAMES avcodec PATHS ${SEARCH_LIBRARY_PATHS})
if(NOT LIBAVCODEC_LIBRARY)
  message(FATAL_ERROR "libavcodec library is missing")
else()
  message(STATUS "libavcodec library found : " ${LIBAVCODEC_LIBRARY})
endif()

FIND_LIBRARY(LIBAVUTIL_LIBRARY NAMES avutil PATHS ${SEARCH_LIBRARY_PATHS})
if(NOT LIBAVUTIL_LIBRARY)
  message(FATAL_ERROR "libavutil library is missing")
else()
  message(STATUS "libavutil library found : " ${LIBAVUTIL_LIBRARY})
endif()
#common commands for building c++ executables and libraries
#rosbuild_add_library(${PROJECT_NAME} src/example.cpp)
#target_link_libraries(${PROJECT_NAME} another_library)
#rosbuild_add_boost_directories()
#rosbuild_link_boost(${PROJECT_NAME} thread)
rosbuild_add_executable(${PROJECT_NAME} src/main.cpp)

target_link_libraries(${PROJECT_NAME} ${LIBAVFORMAT_LIBRARY} ${LIBAVCODEC_LIBRARY} ${LIBAVUTIL_LIBRARY} )

Next in Ubuntu 11.04, when you install ffmpeg, it installs the useful header files in the following directories: /usr/include/libavcodec/ , /usr/include/libavformat/ and /usr/include/libavutil/

So, in your ROS code, you must provide the relative paths:

#include <libavcodec/avcodec.h>
#include <libavformat/avformat.h>
#include <libavutil/avutil.h>

Since, all the above files and libraries are actually programmed in C, to use them in C++ you must surround them with extern “C” { }, like the following:

extern "C" { 
#include <libavcodec/avcodec.h>
#include <libavformat/avformat.h>
#include <libavutil/avutil.h>
}

Also i kept on getting “error: ‘’ was not declared in this scope”. To resolve these issues, add the following to your code:

using namespace std;

Now you can use rosmake to compile your program and it should work!

Waterloo robot tears up the track

Source: Waterloo Daily Bulletin

Last weekend, July 23, the University of Waterloo’s Team Wavelab Robotics returned to the International Autonomous Robot Racing Competition at the University of British Columbia, with a redesigned version of the vehicle that crashed last year just before the competition and had to be rebuilt at lightning speed. This time, it was a clean sweep for the Waterloo team.

They won the Grand Award, and also captured first place in the Design, Drag Race, and Circuit competitions. Videos of the Circuit Race Training are here and here; a video of the Drag Race Competition is here.

Earlier this year, the joint UW Robotics and WAVELab Robotics team also did well at the International Ground Vehicles Competition at Oakland University in Rochester, with the best showing of any Canadian entry. The team is advised by mechanical and mechatronics engineering professors Steve Waslander and Sanjeev Bedi.

Robot Racing 2011 Videos

Hi

I have created a playlist of the Robot Racing 2011 videos here:

http://www.youtube.com/playlist?list=PL91D70F16D002C28A

Working with ROS and OpenCV

The goal of this tutorial is to get you up and running with ROS (Robot Operating System) environment and open source computer vision library OpenCV as fast as possible.

My environment

• Ubuntu 11.04
• ROS Diamondback Desktop Full installed. For instructions on how to install ROS, please refer to: http://www.ros.org/wiki/diamondback/Installation/Ubuntu
• Logitech QuickCam Pro 9000 Web camera

ROS supports many camera drivers which let us control the camera parameters like resolution, frame rate etc., and publish raw images for further processing by the code. Here, i will be using uvc_cam USB camera driver written by Eric Perko.

We will:

1. Launch the uvc_cam driver to start publishing the images.
2. Subscribe to these images using image_transport
3. Convert the images from ROS image message format to OpenCV image format using cv_bridge
4. Process them using OpenCV commands
5. Convert the images from OpenCV image message format to ROS image message using cv_bridge
6. Publish the processed images using image_transport

Software for Testing Webcam

Before moving ahead, it is important to make sure whether your webcam works or not in linux. There are some open-source programs like:

1. Cheeseuses the gstreamer library, which utlilizes the video4linux2 API to capture video and stills from a webcam. It can also apply some special effects. Type

   $ sudo apt-get install cheese

   to install it.

2. GUVCView is a graphical front-end for UVC drivers built using GTK+. It is way better than cheesefor controlling webcam and recording audio/video. Type

   $ sudo apt-get install guvcview

   to install it.

Install Camera Drivers to work with ROS

1. Open the terminal window, go to the directory where you would like to install the camera driver package. In my computer, I installed it in the home directory (go there by typing cd in the terminal window).
2. Download the uvc_campackage:

   $ git clone https://github.com/ericperko/uvc_cam.git

   This will create a directory called uvc_cam.

3. In order to compile and make/build this package you would need elevated permissions. Type:

   $ sudo bash

   and then enter the password. You should now have root access.

4. Before making the package you need to tell ROS where to find it. Type:

   # cd
   # gedit .bashrc

   A text editor should open up. Next add the following line at the end:

   export ROS_PACKAGE_PATH=~/uvc_cam:$ROS_PACKAGE_PATH

   Save and Exit the text editor.

5. Restart the terminal, get root access again and type:

   # rosmake --rosdep-install uvc_cam

   In the rosmake results you should notice:

   [ rosmake ] Results:
   [ rosmake ] Built 49 packages with 0 failures.

   If you notice some failures, you may not have root access.

Find which device is the camera connected to.

1. Open Terminal Window
2. Disconnect the camera from the computer and type the following in the terminal:

   $ ls /dev/video*

   If you dont have any other cameras connected to your computer you would see no output, else you would see something like

   /dev/video0

   Remember the number of devices listed in this step.

3. Now connect the camera to your computer and repeat step 2. You should see a new device added to the list like:

   /dev/video0

   So, the camera is connected to /dev/video0. We will used this information in the next section.

Create a new package for image processing

1. Open a new terminal window and in the home directory, type the following:

   $ roscreate-pkg tutorialROSOpenCV image_transport roscpp std_msgs opencv2 cv_bridge uvc_cam

   This will create a new directory called tutorialROSOpenCV.

2. We will now create a launch directory where we would store the file used to launch the uvc_camnode in order for it to start publishing the images. Change directory:

   $ cd tutorialROSOpenCV

   Now make a new directory under it called launch:

   $ mkdir launch

   This is where we will store the launch file.

3. Change directory:

   $ cd launch

   Create the new launch file by typing:

   $ gedit uvcCamLaunch.launch

   Paste the following in there:

   <launch>
       <node name="uvc_cam_node" pkg="uvc_cam" type="uvc_cam_node" output="screen">
           <param name="device" value="/dev/video0" />
           <param name="width" value="320" />
           <param name="height" value="240" />
           <param name="frame_rate" value="20" />
       </node>
   </launch>
   

   Save and exit. Notice the parameter device with value: /dev/video0. Change this to the actual device path. Refer to previous section.

4. Before making this package you need to tell ROS where to find it. Gain root access in the terminal and type:

   # cd
   # gedit .bashrc

   A text editor should open up. Next add the following line at the end:

   export ROS_PACKAGE_PATH=~/tutorialROSOpenCV:$ROS_PACKAGE_PATH

   Save and Exit the text editor.

5. Restart the terminal, gain root access, and type:

   # rosmake tutorialROSOpenCV

   In the rosmake results you should notice:

   [ rosmake ] Results:
   [ rosmake ] Built 50 packages with 0 failures.

   If you notice some failures, you may not have root access.

Test the Launch script
In order to make sure that the package uvc_cam is in fact publishing the images, we will now run the node and check the output.

1. In a new terminal type:

   $ roslaunch tutorialROSOpenCV uvcCamLaunch.launch

   If your webcam has an integrated light indicator, you should see it light up.

2. Now we will make sure that the images are being published. Open up another terminal window and type:

   $ rostopic list

   You should see the following topics being published:

   /camera/camera_info
   /camera/image_raw
   /camera/image_raw/compressed
   /camera/image_raw/compressed/parameter_descriptions
   /camera/image_raw/compressed/parameter_updates
   /camera/image_raw/theora
   /camera/image_raw/theora/parameter_descriptions
   /camera/image_raw/theora/parameter_updates
   /rosout
   /rosout_agg
   /uvc_cam_node/parameter_descriptions
   /uvc_cam_node/parameter_updates

3. Lets look at the topic /camera/image_raw. Type:

   $ rosrun image_view image_view image:=/camera/image_raw

   You should see a new window pop-up with a continuous streaming video capture output of the webcam. Press Ctrl-Cto exit. Lets look at the format of this topic. Type

   $ rostopic info /camera/image_raw

   You should see something like this:

   Type: sensor_msgs/Image
   
   Publishers:
    * /uvc_cam_node (http://username-computername:port/)
   
   Subscribers: None

   sensor_msgs/Image is a ROS image format which contains the header, height, width, step, and image data. Refer to: http://www.ros.org/doc/api/sensor_msgs/html/msg/Image.html for more information.

4. Lets look at the topic /camera/camera_info. Type:

   $ rostopic info /camera/camera_info

   You should see something like this:

   Type: sensor_msgs/CameraInfo
   
   Publishers:
    * /uvc_cam_node (http://username-computername:port/)
   
   Subscribers: None

   sensor_msgs/CameraInfo is a ROS information format which contains the header, height, width, distortion model params, binning params and ROI (region of interest) params. Refer to: http://www.ros.org/doc/api/sensor_msgs/html/msg/CameraInfo.htmlfor more info. You can view the contents of this topic by typing:

   rostopic echo -c /camera/camera_info

Write the code for image processing

1. In a new terminal window, type the following:

   $ roscd tutorialROSOpenCV/src

   We will now create the source file which will contain the code. Type:

   $ gedit main.cpp

   The following program inverts the colors of the image captured from the webcam. Paste the following code in the file:

   //Includes all the headers necessary to use the most common public pieces of the ROS system.
   #include <ros/ros.h>
   //Use image_transport for publishing and subscribing to images in ROS
   #include <image_transport/image_transport.h>
   //Use cv_bridge to convert between ROS and OpenCV Image formats
   #include <cv_bridge/cv_bridge.h>
   //Include some useful constants for image encoding. Refer to: http://www.ros.org/doc/api/sensor_msgs/html/namespacesensor__msgs_1_1image__encodings.html for more info.
   #include <sensor_msgs/image_encodings.h>
   //Include headers for OpenCV Image processing
   #include <opencv2/imgproc/imgproc.hpp>
   //Include headers for OpenCV GUI handling
   #include <opencv2/highgui/highgui.hpp>
   
   //Store all constants for image encodings in the enc namespace to be used later.
   namespace enc = sensor_msgs::image_encodings;
   
   //Declare a string with the name of the window that we will create using OpenCV where processed images will be displayed.
   static const char WINDOW[] = "Image Processed";
   
   //Use method of ImageTransport to create image publisher
   image_transport::Publisher pub;
   
   //This function is called everytime a new image is published
   void imageCallback(const sensor_msgs::ImageConstPtr& original_image)
   {
   	//Convert from the ROS image message to a CvImage suitable for working with OpenCV for processing
   	cv_bridge::CvImagePtr cv_ptr;
   	try
   	{
   		//Always copy, returning a mutable CvImage
   		//OpenCV expects color images to use BGR channel order.
   		cv_ptr = cv_bridge::toCvCopy(original_image, enc::BGR8);
   	}
   	catch (cv_bridge::Exception& e)
   	{
   		//if there is an error during conversion, display it
   		ROS_ERROR("tutorialROSOpenCV::main.cpp::cv_bridge exception: %s", e.what());
   		return;
   	}
   
   	//Invert Image
   	//Go through all the rows
   	for(int i=0; i<cv_ptr->image.rows; i++)
   	{
   		//Go through all the columns
   		for(int j=0; j<cv_ptr->image.cols; j++)
   		{
   			//Go through all the channels (b, g, r)
   			for(int k=0; k<cv_ptr->image.channels(); k++)
   			{
   				//Invert the image by subtracting image data from 255				
   				cv_ptr->image.data[i*cv_ptr->image.rows*4+j*3 + k] = 255-cv_ptr->image.data[i*cv_ptr->image.rows*4+j*3 + k];
   			}
   		}
   	}
   	
   
   	//Display the image using OpenCV
   	cv::imshow(WINDOW, cv_ptr->image);
   	//Add some delay in miliseconds. The function only works if there is at least one HighGUI window created and the window is active. If there are several HighGUI windows, any of them can be active.
   	cv::waitKey(3);
   	/**
   	* The publish() function is how you send messages. The parameter
   	* is the message object. The type of this object must agree with the type
   	* given as a template parameter to the advertise<>() call, as was done
   	* in the constructor in main().
   	*/
   	//Convert the CvImage to a ROS image message and publish it on the "camera/image_processed" topic.
           pub.publish(cv_ptr->toImageMsg());
   }
   
   /**
   * This tutorial demonstrates simple image conversion between ROS image message and OpenCV formats and image processing
   */
   int main(int argc, char **argv)
   {
   	/**
   	* The ros::init() function needs to see argc and argv so that it can perform
   	* any ROS arguments and name remapping that were provided at the command line. For programmatic
   	* remappings you can use a different version of init() which takes remappings
   	* directly, but for most command-line programs, passing argc and argv is the easiest
   	* way to do it.  The third argument to init() is the name of the node. Node names must be unique in a running system.
   	* The name used here must be a base name, ie. it cannot have a / in it.
   	* You must call one of the versions of ros::init() before using any other
   	* part of the ROS system.
   	*/
           ros::init(argc, argv, "image_processor");
   	/**
   	* NodeHandle is the main access point to communications with the ROS system.
   	* The first NodeHandle constructed will fully initialize this node, and the last
   	* NodeHandle destructed will close down the node.
   	*/
           ros::NodeHandle nh;
   	//Create an ImageTransport instance, initializing it with our NodeHandle.
           image_transport::ImageTransport it(nh);
   	//OpenCV HighGUI call to create a display window on start-up.
   	cv::namedWindow(WINDOW, CV_WINDOW_AUTOSIZE);
   	/**
   	* Subscribe to the "camera/image_raw" base topic. The actual ROS topic subscribed to depends on which transport is used. 
   	* In the default case, "raw" transport, the topic is in fact "camera/image_raw" with type sensor_msgs/Image. ROS will call 
   	* the "imageCallback" function whenever a new image arrives. The 2nd argument is the queue size.
   	* subscribe() returns an image_transport::Subscriber object, that you must hold on to until you want to unsubscribe. 
   	* When the Subscriber object is destructed, it will automatically unsubscribe from the "camera/image_raw" base topic.
   	*/
           image_transport::Subscriber sub = it.subscribe("camera/image_raw", 1, imageCallback);
   	//OpenCV HighGUI call to destroy a display window on shut-down.
   	cv::destroyWindow(WINDOW);
   	/**
   	* The advertise() function is how you tell ROS that you want to
   	* publish on a given topic name. This invokes a call to the ROS
   	* master node, which keeps a registry of who is publishing and who
   	* is subscribing. After this advertise() call is made, the master
   	* node will notify anyone who is trying to subscribe to this topic name,
   	* and they will in turn negotiate a peer-to-peer connection with this
   	* node.  advertise() returns a Publisher object which allows you to
   	* publish messages on that topic through a call to publish().  Once
   	* all copies of the returned Publisher object are destroyed, the topic
   	* will be automatically unadvertised.
   	*
   	* The second parameter to advertise() is the size of the message queue
   	* used for publishing messages.  If messages are published more quickly
   	* than we can send them, the number here specifies how many messages to
   	* buffer up before throwing some away.
   	*/
           pub = it.advertise("camera/image_processed", 1);
   	/**
   	* In this application all user callbacks will be called from within the ros::spin() call. 
   	* ros::spin() will not return until the node has been shutdown, either through a call 
   	* to ros::shutdown() or a Ctrl-C.
   	*/
           ros::spin();
   	//ROS_INFO is the replacement for printf/cout.
   	ROS_INFO("tutorialROSOpenCV::main.cpp::No error.");
   
   }

   Save and Exit.

2. Before we make the program, we have to edit the CMakeLists.txtfile to ensure it will build and make an executable file of the program above. Gain root access, and type:

   # roscd tutorialROSOpenCV/

   Then we will edit the CMakeLists file. Type:

   # gedit CMakeLists.txt

   At the end of the file add the following line:

   rosbuild_add_executable(tutorialROSOpenCV src/main.cpp)

   Save and Exit.

3. Now we can compile/make the package. Type:

   # rosmake tutorialROSOpenCV

   You should notice the following:

   [ rosmake ] Results:
   [ rosmake ] Built 50 packages with 0 failures.
   [ rosmake ] Summary output to directory

4. To run the project, type:

   # rosrun tutorialROSOpenCV tutorialROSOpenCV

   You should see a window being created titled Image Processed containing the inverted image from the webcam

5. In order to ensure that the processed image has been published as a rostopic, in a new terminal window type:

   # rostopic list

   You would see the following:

   /camera/camera_info
   /camera/image_processed
   /camera/image_processed/compressed
   /camera/image_processed/compressed/parameter_descriptions
   /camera/image_processed/compressed/parameter_updates
   /camera/image_processed/theora
   /camera/image_processed/theora/parameter_descriptions
   /camera/image_processed/theora/parameter_updates
   /camera/image_raw
   /camera/image_raw/compressed
   /camera/image_raw/compressed/parameter_descriptions
   /camera/image_raw/compressed/parameter_updates
   /camera/image_raw/theora
   /camera/image_raw/theora/parameter_descriptions
   /camera/image_raw/theora/parameter_updates
   /rosout
   /rosout_agg
   /uvc_cam_node/parameter_descriptions
   /uvc_cam_node/parameter_updates

   To check the image data in rostopic /camera/image_processed, type:

   $ rosrun image_view image_view image:=/camera/image_processed

   You should see a new window pop-up with a continuous streaming video capture output of the webcam with image inverted. Press Ctrl-C to exit.

Getting Started with OpenCV 2.3 in Microsoft Visual Studio 2010 in Windows 7 64-bit

- Prepared by Siddhant Ahuja

I was trying to install OpenCV 2.3 on my Windows 7 64-bit machine and i kept getting the error: The application was unable to start correctly (0xc000007b)

I tried chasing the problem down using freely available program Dependency Walker and it looked like my executable project was x86 whereas all the dll files it was loading up were x64. So after many tries including re-building OpenCV using CMake, changing dll links to x86, etc., i finally figured out how to solve this problem, which i have presented as a tutorial below.

This tutorial assumes that you have installed Microsoft Visual Studio 2010 Professional.  If you are a student you can get it for free at https://www.dreamspark.com/Products/Product.aspx?ProductId=25.

This tutorial contains the following:

• How to download and install the OpenCV library
• How to manually change the system path to include the bin file
• How to create a project
• How to configure Visual Studio to use OpenCV
• A sample program that displays a video from a camera

Downloading and Installing OpenCV

OpenCV is a computer vision library written mainly in C that focuses on real time video processing.  It is open source or free to the public.

1. Go to http://sourceforge.net/projects/opencvlibrary/files/opencv-win/2.3/
2. Select the OpenCV-2.3.0-win-superpack.exe file and download it.
3. Go the folder where you downloaded the executable file in Step 2 and select “Run as Administrator”.
4. The executable file is essentially an archive of files with an extractor built in. It will ask you to select the location where you would like it to extract all its contents. Select: C:\Program Files\ as the path and click Extract. It will create a folder called OpenCV2.3 with the path: C:\Program Files\OpenCV2.3

Manually Changing the System Path to Include the Bin File

1. To access the system path in Vista go to Control Panel\System and Security\System and on the left hand side select Advanced system settings this should bring up the Systems Properties dialogue.  Click on the Advanced tab and then the Environment Variables button.
2. This will bring up the Environment Variables dialogue.  In the System Variables box select Path and then Edit
3. When modifying the path know that it is sensitive to all characters including spaces.  To add the new bin, type a semicolon directly after the text that is there without adding in a space.  Next add the path to the bin file.  The path is the same as where you chose to install OpenCV back in step 4 of Downloading and Installing OpenCV section plus \bin. For example if you chose to install OpenCV in:
   

   C:\Program Files\OpenCV2.3

   For 64 bit Windows, add the following to the system path:

   ;C:\Program Files\OpenCV2.3\build\bin\;C:\Program Files\OpenCV2.3\build\x64\vc10\bin\

   Make sure to restart your computer so the changes can take effect.

Creating a Project

1. Click File → New →Project
2. In the “Installed Templates”  box choose Visual C++ → Win32
3. On the right choose Win32 Console Application
4. Enter a name for the project and click OK
5. Click finish in the dialogue box which appears

Configuring Visual Studio

1. Click Project → Properties to access the properties dialog
   
2. In the left box click on Configuration Properties and on the top right click on Configuration Manager
   
   
3. In the Configuration Manager dialog box that opens up, under Active Solution Platform combo box select New.
   
4. In the New Solution Platform dialog box that opens up, under Type or select the new platform, select x64, copy settings from Win32 and make sure that Create new project platforms is selected. Click OK.
   
5. You will notice that the in the Configuration Manager dialog box x64 platform has now been selected. Click Close.
   
6. In the left box choose Configuration Properties → C++ → General
7. In the right box, next to Additional Include Directories type the following text:

   C:\Program Files\OpenCV2.3\build\include;C:\Program Files\OpenCV2.3\build\include\opencv;%(AdditionalIncludeDirectories)

   IMPORTANT note that all these paths assume that you installed in the default location, if you installed in a different location; for example Program Files (x86) instead of Program Files, make sure you change these paths to reflect that.

   

8. Next in the felt box choose Configuration Properties → Linker → Input
9. In the right box, next to Additional Dependencies type the following text:

   "C:\Program Files\OpenCV2.3\build\x64\vc10\lib\opencv_core230d.lib";"C:\Program Files\OpenCV2.3\build\x64\vc10\lib\opencv_highgui230d.lib";"C:\Program Files\OpenCV2.3\build\x64\vc10\lib\opencv_video230d.lib";"C:\Program Files\OpenCV2.3\build\x64\vc10\lib\opencv_ml230d.lib";"C:\Program Files\OpenCV2.3\build\x64\vc10\lib\opencv_legacy230d.lib";"C:\Program Files\OpenCV2.3\build\x64\vc10\lib\opencv_imgproc230d.lib";%(AdditionalDependencies)

   IMPORTANT note that all these paths assume that you installed in the default location, if you installed in a different location; for example Program Files (x86) instead of Program Files, make sure you change these paths to reflect that.

10. Click Apply then OK

Sample Program

This program will capture video from a camera and display it.  Press escape to stop capturing and displaying frames.

It may be necessary to change the number in this line of code to select the camera source:

CvCapture* capture = cvCaptureFromCAM(1); // capture from video device #1

If you do not know the video device number just try 0, then 1,  2 or else -1. Copy and paste the following code:

#include "stdafx.h"
#include <highgui.h>

int _tmain(int argc, _TCHAR* argv[])
{
	int c;
	// allocate memory for an image
	IplImage *img;
	// capture from video device #1
	CvCapture* capture = cvCaptureFromCAM(1);
	// create a window to display the images
	cvNamedWindow("mainWin", CV_WINDOW_AUTOSIZE);
	// position the window
	cvMoveWindow("mainWin", 5, 5);
	while(1)
	{
		// retrieve the captured frame
		img=cvQueryFrame(capture);
		// show the image in the window
		cvShowImage("mainWin", img );
		// wait 10 ms for a key to be pressed
		c=cvWaitKey(10);
		// escape key terminates program
		if(c == 27)
		break;
	}
 return 0;
}

Student-built vehicle finds its own way

Source: The Daily Bulletin, University of Waterloo

A team of undergraduate and graduate students from the University of Waterloo Robotics Team and the Waterloo Autonomous Vehicles Lab are back from the recent Intelligent Ground Vehicle Competition with the best showing of any Canadian entry.

Seen in the photo, left to right, are faculty advisor Steven Waslander (mechanical and mechatronics engineering), PJ Mukhurjee, Sid Ahuja, Arun Das, Michael Tribou. Key team members not pictured include Peiyi Chen and Ryan Turner.

The 19th annual Intelligent Ground Vehicle Competition was held at Oakland University in Rochester, Michigan. It’s described as one of the largest and most prestigious competitions for custom autonomous vehicles in North America. The competition is run annually by the Associate for Unmanned Vehicle’s International, with sponsors including the US Department of Defense, Lockheed Martin, and General Dynamics.

The Waterloo entry — named Indrik — placed second out of 56 teams from the United States, Canada, India, Japan and the United Arab Emirates in the vehicle design competition, winding up just 4 points (out of a possible 300) behind the grand prize winner. Waterloo also finished 4th in the JAUS challenge, which requires successful implementation of an industrial calibre communications framework for autonomous vehicles.

According to Craig MacKenzie, a member of the team’s executive, the competitors are also looking forward to a top-ten finish in the autonomous navigation challenge, which requires vehicles to autonomously navigate between four GPS waypoints while dynamically avoiding obstacles including construction barrels and staggered fencing without human intervention. Full results will be released at the end of the month.

The competition also features a “fully autonomous” challenge (without GPS) requiring navigation of a simulated roadway strewn with barricades. A last-minute failure of the vision computer left Indrik unable to participate properly in that event.

However, the team’s solid mechanical design and software strategies were widely recognized by competitors. “Our custom suspension design and unique approach to mapping and path planning generated a lot of interest with the other teams and industry representatives,” says a report from Rochester. “We are eagerly looking forward to continued development and even more success in the future!”

Introduction to Kalman Filtering-Part 1

Why would anyone use it?

Suppose you have constructed a model that predicts the behavior of an actual physical process at any instance of time. This could be the trajectory of a projectile, or the path followed by your car, or any other process. You cannot guarantee that the model that you have constructed is 100% accurate, and you want to check to make sure if it behaves the way you predicted.

One way to do that is to take a couple of measurements of the system at various time intervals. However, the sensors used to take the measurements, or the person who is recording them cannot be trusted 100% of the time. So you are left with a couple of sources of error or noise.

So how do you combine the measurements and incorporate them in your model of the system so that you have a better estimate of the model or system? One way to do that is by employing Kalman Filtering. The upside is that you hopefully end up with some useful data or at least a less noisier version of it. However, the downside to filtering is that you add some delay.

In general the problem can be visually represented as follows:

So what is a filter?

Anything that blocks certain things and lets other things to go through, resulting in an enhancement or reduction of the quality or quantity of the original system.

Linear vs. Non-Linear Systems

Before we move on, it is important to consider what is a linear or a non-linear system, as we would be using these terms from now on. Given the inputs to the system, if the output is directly proportional to the inputs, or is a just a sum of all responses then the system is said to be linear. Whereas in a non-linear system the output can not be expressed as a linear combination (addition or scalar multiplication) of the inputs.

In other words, a function which depends on the variables and is said to be linear if and only if the following holds true:

• . This property if called additive.
• , where is just a constant. This property is called Homogeneity, which implies if we double the strength of the input, the output will double up too. If we triple the input, the output’s strength gets tripled too.

These two properties together describe a linear system. Examples of linear systems include amplification or attenuation of signals, motion of a spring-damper system, electrical circuits formed of resistors, capacitors, inductors, etc.

However, many of the processes that we observe in nature are typically non-linear, like a bird’s flight or heat transfer during cooking. In order to simplify calculations, often we approximate them as linear by making certain assumptions.

As shown in the first figure on top, the next state of the system depends on the initial or previous state, the controls applied to change the state and the error sources associated with it. For a linear system, we can generalize this by writing it as:

,

where, is the time instance, is the initial state, is the next state, is the control applied to change the state, is the associated noise, and are some constants, vectors or matrices. If we are describing a process, is often referred to as process noise.

Now, we consider the internal state of the system to be hidden, i.e. the estimate of the system state can only be carried out by observing and/or measuring the output of the system, represented by . However, there will be some noise associated with the measurements themselves. For a linear system, we can generalize this as:

where, the output is a function of the input , is a constant, vector or matrix, and is the measurement noise.

A Simple Example

Lets take a simple example of estimating the position of a robot driving on a surface. This would be our System. Making many assumptions here to simplify the case, lets say it moves along in a single direction with a certain constant velocity of and starts at an initial position of zero. We can now form its equation of motion as:

In other words, we can get its position at any time instant by adding the distance traveled in the given time to its initial position .

The the state of the system consists of two variables: position and velocity. This can be represented in vector form as:

In MATLAB it is now easy to plot the ground truth positions of the robot for 100 instances with a time step () of 0.1 seconds:

% Initial Position
p_0=0;
% Initial Velocity
v_0=5;
% Number of time instances or samples, in other words the duration of simulation
N=100;
% Time Step per instance/sample
dt=0.1;
% Generate a time vector formed of all tme instances with time steps
t=0:dt:dt*N;
% Generate the equation of motion for the robot and the ground truth
% positions for every time instance
p=p_0 + v_0*t;

% Plot the figure with the ground truth positions of the robot
figure;
plot(t,p,'g');
xlabel('Time (s)');
ylabel('Position (m)');
title('Ground Truth position of the robot');

Now, lets say we got someone to measure the position of the robot every 0.1 seconds. But, due to the inherent noise in the measuring device, or due to the lack of trust in the person recording its position, we get some measurement inaccuracies. We can model them here as Gaussian noise i.e. noise having a continuous probability distribution with a single mean value…or any bell shaped function.

So to simulate we add some random Gaussian noise to each one of the ground truth data points above. This is accomplished with the MATLAB code below:

% Add some random Gaussian noise with mean as the current true value of the
% position and a standard deviation of 3 meters, to each one of the Ground
% Truth positions above
m =zeros(0,N);
for k=1:N+1
    m(k) = p(k)+ 3*randn;
end;

% Plot the figure with the ground truth positions of the robot and the
% measurements with noise
figure;
plot(t,p,'g');
hold on;
plot(t,m,'r');
xlabel('Time (s)');
ylabel('Position (m)');
legend('GroundTruth position of the robot','Measurements with Noise');
title('Ground Truth Position with Noise');

Basics of Kalman Filtering

Kalman filter named after Rudolf E. Kalman is one such filter which can estimate the state of the system given the measurements, prediction, controls, and associated errors.