cmucam2.cc

机器人仿真软件

/*
 *  Player - One Hell of a Robot Server
 *  Copyright (C) 2000
 *     Brian Gerkey, Kasper Stoy, Richard Vaughan, & Andrew Howard
 *
 *  This program is free software; you can redistribute it and/or modify
 *  it under the terms of the GNU General Public License as published by
 *  the Free Software Foundation; either version 2 of the License, or
 *  (at your option) any later version.
 *
 *  This program is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *  GNU General Public License for more details.
 *
 *  You should have received a copy of the GNU General Public License
 *  along with this program; if not, write to the Free Software
 *  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
 *
 */

/*
 * $Id: cmucam2.cc,v 1.14 2006/02/28 05:15:41 gerkey Exp $
 */

/** @ingroup drivers */
/** @{ */
/** @defgroup driver_cmucam2 cmucam2
 * @brief CMUCam2 pan-tilt-zoom blob-tracking camera

The cmucam2 driver connects over a serial port to a CMUCam2. Presents a
@ref interface_blobfinder interface and a @ref interface_ptz
interface and can track multiple color blobs (plus an additional 
@ref interface_camera for getting image data). Color tracking parameters are 
defined in Player's config file (see below for an example).


@par Compile-time dependencies

- none

@par Provides

- @ref interface_blobfinder : the blobs detected by the CMUCam2
- @ref interface_ptz : control of the servos that pan and tilt
  the CMUCam2
- @ref interface_camera : snapshot images taken by the CMUCam2

@par Requires

- none

@par Supported configuration requests

- The @ref interface_ptz interface supports:
  - PLAYER_PTZ_REQ_AUTOSERVO
- The @ref interface_blobfinder interface supports:
  - PLAYER_BLOBFINDER_REQ_SET_COLOR
  - PLAYER_BLOBFINDER_REQ_SET_IMAGER_PARAMS

@par Configuration file options

- devicepath (string)
  - Default: NULL
  - Serial port where the CMUCam2 is connected
- num_blobs (integer)
  - Default: 1
  - Number of colors to track; you must also include this many color%d options
- color%d (float tuple)
  - Default: none
  - Each color%d is a tuple [rmin rmax gmin gmax bmin bmax] of min/max
    values for red, green, and blue, which defines a region in RGB space
    that the CMUCam2 will track.
- bloborcamera (integer)
  - Default: 1
  - Set bloborcamera to 1 if you want the blobfinder/ptz active, or set it
    to 2 if you want camera/ptz active. (this will be changed in the future)
  
@par Example 

@verbatim
driver
(
  name "cmucam2"
  provides ["blobfinder:0" "ptz:0" "camera:0"]
  devicepath "/dev/ttyS1"
  bloborcamera 1
  num_blobs 2
# values must be between 40 and 240 (!)
  color0 [  red_min red_max blue_min blue_max green_min green_max] )
# values must be between 40 and 240 (!)
  color1 [  red_min red_max blue_min blue_max green_min green_max] )  
)
@endverbatim

@author Pouya Bastani, Richard Vaughan, Radu Bogdan Rusu
 */
/** @} */


#include <assert.h>
#include <stdio.h>
#include <unistd.h> /* close(2),fcntl(2),getpid(2),usleep(3),execvp(3),fork(2)*/
#include <netdb.h> /* for gethostbyname(3) */
#include <sys/types.h>  /* for socket(2) */
#include <sys/socket.h>  /* for socket(2) */
#include <signal.h>  /* for kill(2) */
#include <fcntl.h>  /* for fcntl(2) */
#include <string.h>  /* for strncpy(3),memcpy(3) */
#include <stdlib.h>  /* for atexit(3),atoi(3) */
#include <pthread.h>  /* for pthread stuff */

#include <libplayercore/playercore.h>

#include "camera.h"

#define MAX_CHANNELS 32

class Cmucam2:public Driver 
{
	private:

		// Descriptive colors for each channel.
		uint32_t colors[MAX_CHANNELS];
		void get_blob (packet_t cam_packet, player_blobfinder_blob_t *blob,
					   color_config range);
		void get_image (packet_f* cam_packet, player_camera_data_t *image);
		int fd;
		int num_of_blobs;
		const char* devicepath;
		color_config color[PLAYER_BLOBFINDER_MAX_BLOBS];

		// Blobfinder interface (provides)
		player_devaddr_t         blobfinder_id;
		player_blobfinder_data_t blobfinder_data;

		// PTZ interface (provides)
		player_devaddr_t         ptz_id;
		player_ptz_data_t        ptz_data;
	
		// Camera interface (provides)
		player_devaddr_t         cam_id;
		player_camera_data_t     cam_data;

		int pan_position;
		int tilt_position;

		void CheckConfigPtz        ();
		void CheckConfigBlobfinder ();

		void RefreshDataBlobfinder ();
		void RefreshDataPtz        ();
		void RefreshDataCamera     ();

		int BlobORCamera;

	public:

		// constructor 
		//
		Cmucam2( ConfigFile* cf, int section);

		// Process incoming messages from clients 
		virtual int ProcessMessage(MessageQueue * resp_queue, 
								   player_msghdr * hdr, 
								   void * data);

		virtual void Main();
  
		int Setup();
		int Shutdown();
};

////////////////////////////////////////////////////////////////////////////////
// a factory creation function
Driver* Cmucam2_Init( ConfigFile* cf, int section)
{
	return((Driver*)(new Cmucam2( cf, section)));
}

////////////////////////////////////////////////////////////////////////////////
// a driver registration function
void 
Cmucam2_Register(DriverTable* table)
{
	table->AddDriver("cmucam2", Cmucam2_Init);
}

Cmucam2::Cmucam2( ConfigFile* cf, int section)
	: Driver(cf, section)
{
	memset (&this->blobfinder_id, 0, sizeof (player_devaddr_t));
	memset (&this->ptz_id,        0, sizeof (player_devaddr_t));
	memset (&this->cam_id,        0, sizeof (player_devaddr_t));
			
	BlobORCamera = cf->ReadInt (section, "bloborcamera", 1);
	if ((BlobORCamera != 1) || (BlobORCamera != 2))
		BlobORCamera = 1;
	
	// Outgoing blobfinder interface
	if(cf->ReadDeviceAddr(&(this->blobfinder_id), section, "provides",
	   PLAYER_BLOBFINDER_CODE, -1, NULL) == 0)
	{
		if(this->AddInterface(this->blobfinder_id) != 0)
		{
			this->SetError(-1);
			return;
		}
	}
  
	// Outgoing camera interface
	if(cf->ReadDeviceAddr(&(this->cam_id), section, "provides",
	   PLAYER_CAMERA_CODE, -1, NULL) == 0)
	{
		if(this->AddInterface(this->cam_id) != 0)
		{
			this->SetError(-1);
			return;
		}
	}
	
	// Outgoing ptz interface
	if(cf->ReadDeviceAddr(&(this->ptz_id), section, "provides",
	   PLAYER_PTZ_CODE, -1, NULL) == 0)
	{
		if(this->AddInterface(this->ptz_id) != 0)
		{
			this->SetError(-1);
			return;
		}
	}

	if (BlobORCamera == 1)
	{
		num_of_blobs = cf->ReadInt (section, "num_blobs", 1);
		char variable[20];

		for (int i = 0; i < num_of_blobs; i++)
		{
			sprintf (variable, "color%d", i);   
			color[i].rmin = (int)cf->ReadTupleFloat (section, variable, 0, 16);
			color[i].rmax = (int)cf->ReadTupleFloat (section, variable, 1, 16);
			color[i].gmin = (int)cf->ReadTupleFloat (section, variable, 2, 16);
			color[i].gmax = (int)cf->ReadTupleFloat (section, variable, 3, 16);
			color[i].bmin = (int)cf->ReadTupleFloat (section, variable, 4, 16);
			color[i].bmax = (int)cf->ReadTupleFloat (section, variable, 5, 16);
		}
	}
	else
	{
		num_of_blobs = 0;
		color[0].rmin = 0; color[0].rmax = 0;
		color[0].gmin = 0; color[0].gmax = 0;
		color[0].bmin = 0; color[0].bmax = 0;
	}

	pan_position = 0;
	tilt_position = 0;
 
	if(!(this->devicepath = (char*)cf->ReadString(section, "devicepath", NULL)))
	{
		PLAYER_ERROR("must specify devicepath");
		this->SetError(-1);
		return;
	}
}
    
////////////////////////////////////////////////////////////////////////////////
int Cmucam2::Setup()
{
	fflush(stdout);

	fd = open_port((char*)devicepath);        // opening the serial port
	if(fd<0)                           // if not successful, stop
	{
		printf("Camera connection failed!\n");
		return -1; 
	}
	auto_servoing(fd, 0);

	/* now spawn reading thread */
	StartThread();

	return(0);
}


////////////////////////////////////////////////////////////////////////////////
int Cmucam2::Shutdown()
{
	StopThread();
	stop_tracking(fd);
	close_port(fd);                 // close the serial port
	return(0);
}