cmucam2.cc

机器人仿真软件

////////////////////////////////////////////////////////////////////////////////
int Cmucam2::ProcessMessage (MessageQueue* resp_queue, 
								player_msghdr * hdr,
								void * data)
{
	assert(hdr);
	assert(data);

	if (Message::MatchMessage(hdr, PLAYER_MSGTYPE_CMD, 0, ptz_id))
	{
		player_ptz_cmd_t & command = 
				*reinterpret_cast<player_ptz_cmd_t * > (data);

		if(pan_position != (short)ntohs((unsigned short)(command.pan)))
		{
			pan_position = (short)ntohs((unsigned short)(command.pan));
			if( abs(pan_position) <= 90 )
				// Pan value must be negated.
				set_servo_position(fd, 0, -1*pan_position);
		}
		if(tilt_position != (short)ntohs((unsigned short)(command.tilt)))
		{
			tilt_position = (short)ntohs((unsigned short)(command.tilt));
			if( abs(tilt_position) <= 90 )
				// Tilt value must be negated.
				set_servo_position(fd, 1, -1*tilt_position);
		}

		Publish (this->ptz_id, resp_queue,
				 PLAYER_MSGTYPE_RESP_ACK,
				 hdr->subtype);
		return 0;
	}	

	if (Message::MatchMessage(hdr, PLAYER_MSGTYPE_REQ, 
		PLAYER_PTZ_REQ_AUTOSERVO, ptz_id))
	{
		player_ptz_req_control_mode *servo = (player_ptz_req_control_mode*)data;
		auto_servoing (fd, servo->mode);
		if(servo->mode)
			PLAYER_MSG0 (1, "Auto servoing is enabled.");
		else
			PLAYER_MSG0 (1, "Auto servoing is disabled.");

		Publish (this->ptz_id, resp_queue,
				 PLAYER_MSGTYPE_RESP_ACK,
				 PLAYER_PTZ_REQ_AUTOSERVO);
		return PLAYER_MSGTYPE_RESP_ACK;
	}

	if (Message::MatchMessage(hdr, PLAYER_MSGTYPE_REQ, 
		PLAYER_BLOBFINDER_REQ_SET_COLOR, blobfinder_id))
	{
		player_blobfinder_color_config_t color_config;
		color_config = *((player_blobfinder_color_config_t*)data);

		color[0].rmin = color_config.rmin;
		color[0].rmax = color_config.rmax;
		color[0].gmin = color_config.gmin;
		color[0].gmax = color_config.gmax;
		color[0].bmin = color_config.bmin;
		color[0].bmax = color_config.bmax;
		
		PLAYER_MSG6 (1, 
		"Cmucam2_blobfinder received new tracking color: [%d,%d,%d,%d,%d,%d]",
					 color[0].rmin, color[0].rmax,
					 color[0].gmin, color[0].gmax,
					 color[0].bmin, color[0].bmax);

		Publish (this->blobfinder_id, resp_queue,
				 PLAYER_MSGTYPE_RESP_ACK,
				 PLAYER_BLOBFINDER_REQ_SET_COLOR);
		return PLAYER_MSGTYPE_RESP_ACK;
	}
	
	if (Message::MatchMessage(hdr, PLAYER_MSGTYPE_REQ, 
		PLAYER_BLOBFINDER_REQ_SET_IMAGER_PARAMS, blobfinder_id))
	{
		player_blobfinder_imager_config player_ic;
		player_ic = *((player_blobfinder_imager_config*)data);
		
		stop_tracking (fd);

		imager_config ic;

		ic.brightness = (int16_t)player_ic.brightness;
		ic.contrast   = (int16_t)player_ic.contrast;
		ic.autogain   = (int16_t)player_ic.autogain;
		ic.colormode  = (int8_t)player_ic.colormode;

		if (set_imager_config (fd, ic) != 1)
			PLAYER_WARN ("Cmucam2_blobfinder set_imager_params failed!");

		color_config cc;
		track_blob (fd, cc);
		
		Publish (this->blobfinder_id, resp_queue,
				 PLAYER_MSGTYPE_RESP_ACK,
				 PLAYER_BLOBFINDER_REQ_SET_IMAGER_PARAMS);
		return PLAYER_MSGTYPE_RESP_ACK;
	}
	
	return -1;
}



////////////////////////////////////////////////////////////////////////////////
void Cmucam2::Main()
{
	memset (&blobfinder_data, 0, sizeof (blobfinder_data));
	memset (&ptz_data,        0, sizeof (ptz_data       ));
	memset (&cam_data,        0, sizeof (cam_data       ));

	for(;;)
	{
    // handle commands and requests/replies -----------------------------------
		pthread_testcancel();
		ProcessMessages();
		pthread_testcancel();

    // get data ---------------------------------------------------------------
		this->RefreshDataPtz ();

		if (BlobORCamera == 1)
			this->RefreshDataBlobfinder ();
		if (BlobORCamera == 2)
			this->RefreshDataCamera     ();
	}

	pthread_exit(NULL);
}

////////////////////////////////////////////////////////////////////////////////
void Cmucam2::RefreshDataBlobfinder ()
{
	packet_t                  blob_info;
	player_blobfinder_blob_t  blob;
	int                       blobs_observed;

	memset (&this->blobfinder_data, 0, sizeof (this->blobfinder_data));

	blobfinder_data.width       = IMAGE_WIDTH;
	blobfinder_data.height      = IMAGE_HEIGHT;
	blobfinder_data.blobs_count = num_of_blobs;

	blobs_observed = 0;

	for (int i = 0; i < num_of_blobs; i++)
	{
		track_blob (fd, color[0]);
		if (!get_t_packet (fd, &blob_info))
			pthread_exit (NULL);

		stop_tracking (fd);

		get_blob (blob_info, &blob, color[i]);
		if (blob.area > 0)
			blobs_observed++;

		blob.id     = 0;
		memcpy (&blobfinder_data.blobs[i], &blob, sizeof (blob));
	}

	blobfinder_data.blobs_count = blobs_observed;
  
	/* got the data. now fill it in */
	Publish (this->blobfinder_id, NULL, PLAYER_MSGTYPE_DATA, 
			 PLAYER_BLOBFINDER_DATA_BLOBS, &blobfinder_data, 
			 sizeof (player_blobfinder_data), NULL);
	return;
}

////////////////////////////////////////////////////////////////////////////////
void Cmucam2::RefreshDataPtz        ()
{
	memset (&this->ptz_data, 0, sizeof (this->ptz_data));

	ptz_data.zoom      = 45;                  // cmucam does not have these 
	ptz_data.panspeed  = 0;
	ptz_data.tiltspeed = 0;
	ptz_data.zoom      = ptz_data.zoom;
	ptz_data.panspeed  = ptz_data.panspeed;
	ptz_data.tiltspeed = ptz_data.tiltspeed;
 
	ptz_data.pan   = -1*get_servo_position (fd, 0);
	ptz_data.tilt  = -1*get_servo_position (fd, 1);
	ptz_data.pan   = ptz_data.pan;
	ptz_data.tilt  = ptz_data.tilt; 

	Publish (this->ptz_id, NULL, PLAYER_MSGTYPE_DATA, PLAYER_PTZ_DATA_STATE, 
			 &ptz_data, sizeof (player_ptz_data_t), NULL);
	
	return;
}

////////////////////////////////////////////////////////////////////////////////
void Cmucam2::RefreshDataCamera     ()
{
	packet_f*              camera_packet;

        camera_packet = (packet_f*)malloc(sizeof(packet_f));
        assert(camera_packet);
  
	memset (&this->cam_data, 0, sizeof (this->cam_data));
	memset (camera_packet,  0, sizeof (packet_f));

	if (read_image (fd, -1, camera_packet) != 0)
        {
          free(camera_packet);
          pthread_exit (NULL);
        }

	get_image (camera_packet, &cam_data);
        free(camera_packet);

	Publish (this->cam_id, NULL, PLAYER_MSGTYPE_DATA, PLAYER_CAMERA_DATA_STATE, 
			 &cam_data, sizeof (player_camera_data_t), NULL);
	return;
}

/**************************************************************************	
                           *** GET BLOB ***
**************************************************************************/
/* Description: This function uses CMUcam's T packet for tracking to get
   the blob information as described by Player
   Parameters:  cam_packet: camera's T packet generated during tracking
                color: the color range used in tracking
   Returns:     The Player format for blob information
*/

void Cmucam2::get_blob (packet_t cam_packet, 
						player_blobfinder_blob_t *blob,
						color_config range)
{
	// a descriptive color for the blob
	unsigned char red   = (range.rmin + range.rmax)/2; 
	unsigned char green = (range.gmin + range.gmax)/2;
	unsigned char blue  = (range.bmin + range.bmax)/2;
  
	(*blob).color  = red << 16 + green << 8 + blue;
	// the number of pixels in the blob
	(*blob).area   = cam_packet.blob_area;
	// setting the bounding box for the blob
	(*blob).x      = 2*cam_packet.middle_x;
	(*blob).y      = cam_packet.middle_y;
	(*blob).left   = 2*cam_packet.left_x;
	// highest and lowest y-value for top and bottom
	(*blob).right  = 2*cam_packet.right_x;
	(*blob).top    = (cam_packet.left_y > cam_packet.right_y) ? 
			cam_packet.left_y : cam_packet.right_y;
	(*blob).bottom = (cam_packet.left_y <= cam_packet.right_y) ? 
			cam_packet.left_y : cam_packet.right_y;
}

/**************************************************************************	
                           *** GET IMAGE ***
**************************************************************************/
/* Description: This function uses CMUcam's F packet to get an image
   Parameters:  packet: camera's F packet in raw format
   Returns:     The Player format for image data
*/

void Cmucam2::get_image (packet_f* cam_packet, 
						 player_camera_data_t *cam_data)
{
	int x = 0;
	int y = 0;
  
	(*cam_data).width       = cam_packet->xsize;
	(*cam_data).height      = cam_packet->ysize;
	(*cam_data).bpp         = 24;
	(*cam_data).format      = 5;    // PLAYER_CAMERA_FORMAT_RGB888
	(*cam_data).fdiv        = 1;
	(*cam_data).compression = 0;    // PLAYER_CAMERA_COMPRESS_RAW
	(*cam_data).image_count = (*cam_data).width * 2 * (*cam_data).height;

	for (y = 0; y < (int32_t)(*cam_data).height; y++)
	{
		for (x = 0; x < (int32_t)(*cam_data).width; x++)
		{
			int red   = cam_packet->rows[y].rgb[x].r << 16;
			int green = cam_packet->rows[y].rgb[x].g << 8;
			int blue  = cam_packet->rows[y].rgb[x].b;
			(*cam_data).image[y * (*cam_data).width * 2 + x * 2]
					= (red + green + blue);
			(*cam_data).image[y * (*cam_data).width * 2 + x * 2 + 1] 
					= (red + green + blue);
		}
	}

	return;
}