Permalink
Find file Copy path
Fetching contributors…
Cannot retrieve contributors at this time
121 lines (99 sloc) 3.63 KB
/*
* Player - One Hell of a Robot Server
* Copyright (C) 2004, 2005 Richard Vaughan
*
*
* 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
*
*/
/*
* Desc: A plugin driver for Player that gives access to Stage devices.
* Author: Richard Vaughan
* Date: 10 December 2004
* CVS: $Id$
*/
// DOCUMENTATION
/** @addtogroup player
@par Blobfinder interface
- PLAYER_BLOBFINDER_DATA_BLOBS
*/
// CODE
#include "p_driver.h"
using namespace Stg;
InterfaceBlobfinder::InterfaceBlobfinder(player_devaddr_t addr, StgDriver *driver, ConfigFile *cf,
int section)
: InterfaceModel(addr, driver, cf, section, "blobfinder")
{
// nothing to do for now
}
void InterfaceBlobfinder::Publish(void)
{
player_blobfinder_data_t bfd;
bzero(&bfd, sizeof(bfd));
ModelBlobfinder *blobmod = (ModelBlobfinder *)this->mod;
const std::vector<Stg::ModelBlobfinder::Blob> &blobs = blobmod->GetBlobs();
if (blobs.size() > 0) {
// and set the image width * height
bfd.width = blobmod->scan_width;
bfd.height = blobmod->scan_height;
bfd.blobs_count = blobs.size();
bfd.blobs = new player_blobfinder_blob_t[blobs.size()];
// now run through the blobs, packing them into the player buffer
// counting the number of blobs in each channel and making entries
// in the acts header
unsigned int b;
for (b = 0; b < blobs.size(); b++) {
// useful debug - leave in
/*
cout << "blob "
<< " left: " << blobs[b].left
<< " right: " << blobs[b].right
<< " top: " << blobs[b].top
<< " bottom: " << blobs[b].bottom
<< " color: " << hex << blobs[b].color << dec
<< endl;
*/
int dx = blobs[b].right - blobs[b].left;
int dy = blobs[b].top - blobs[b].bottom;
bfd.blobs[b].x = blobs[b].left + dx / 2;
bfd.blobs[b].y = blobs[b].bottom + dy / 2;
bfd.blobs[b].left = blobs[b].left;
bfd.blobs[b].right = blobs[b].right;
bfd.blobs[b].top = blobs[b].top;
bfd.blobs[b].bottom = blobs[b].bottom;
bfd.blobs[b].color = ((uint8_t)(blobs[b].color.r * 255.0) << 16)
+ ((uint8_t)(blobs[b].color.g * 255.0) << 8)
+ ((uint8_t)(blobs[b].color.b * 255.0));
bfd.blobs[b].area = fabs( dx * dy );
bfd.blobs[b].range = blobs[b].range;
}
}
// should change player interface to support variable-lenght blob data
// size_t size = sizeof(bfd) - sizeof(bfd.blobs) + bcount * sizeof(bfd.blobs[0]);
this->driver->Publish(this->addr, PLAYER_MSGTYPE_DATA, PLAYER_BLOBFINDER_DATA_BLOBS, &bfd,
sizeof(bfd), NULL);
if (bfd.blobs)
delete[] bfd.blobs;
}
int InterfaceBlobfinder::ProcessMessage(QueuePointer &resp_queue, player_msghdr_t *hdr, void *data)
{
// todo: handle configuration requests
// else
{
// Don't know how to handle this message.
PRINT_WARN2("blobfinder doesn't support msg with type/subtype %d/%d", hdr->type, hdr->subtype);
return (-1);
}
}