Revision: 8831
http://playerstage.svn.sourceforge.net/playerstage/?rev=8831&view=rev
Author: jpgr87
Date: 2010-07-25 20:00:24 +0000 (Sun, 25 Jul 2010)
Log Message:
-----------
Applied patch #3034305: Player SVN: more camera fixes
Modified Paths:
--------------
code/player/trunk/client_libs/libplayerc/dev_camera.c
code/player/trunk/server/drivers/camera/CMakeLists.txt
code/player/trunk/server/drivers/camera/compress/cameracompress.cc
Added Paths:
-----------
code/player/trunk/server/drivers/camera/camerareq/
code/player/trunk/server/drivers/camera/camerareq/CMakeLists.txt
code/player/trunk/server/drivers/camera/camerareq/camerareq.cc
Modified: code/player/trunk/client_libs/libplayerc/dev_camera.c
===================================================================
--- code/player/trunk/client_libs/libplayerc/dev_camera.c 2010-07-23
17:12:01 UTC (rev 8830)
+++ code/player/trunk/client_libs/libplayerc/dev_camera.c 2010-07-25
20:00:24 UTC (rev 8831)
@@ -121,12 +121,18 @@
device->fdiv = data->fdiv;
device->compression = data->compression;
device->image_count = data->image_count;
- device->image = realloc(device->image,
sizeof(device->image[0])*device->image_count);
-
- if (device->image)
- memcpy(device->image, data->image, device->image_count);
- else
- PLAYERC_ERR1("failed to allocate memory for image, needed %u bytes\n",
sizeof(device->image[0])*device->image_count);
+ if (device->image_count > 0)
+ {
+ device->image = realloc(device->image,
sizeof(device->image[0])*device->image_count);
+ if (device->image)
+ memcpy(device->image, data->image, device->image_count);
+ else
+ PLAYERC_ERR1("failed to allocate memory for image, needed %u bytes\n",
sizeof(device->image[0])*device->image_count);
+ } else
+ {
+ if (device->image) free(device->image);
+ device->image = NULL;
+ }
}
else
PLAYERC_WARN2("skipping camera message with unknown type/subtype: %s/%d\n",
@@ -278,11 +284,18 @@
device->fdiv = data->fdiv;
device->compression = data->compression;
device->image_count = data->image_count;
- device->image = realloc(device->image, (sizeof device->image[0]) *
device->image_count);
- if (device->image)
- memcpy(device->image, data->image, device->image_count);
- else
- PLAYERC_ERR1("failed to allocate memory for image, needed %u bytes\n",
(sizeof device->image[0]) * device->image_count);
+ if (device->image_count > 0)
+ {
+ device->image = realloc(device->image, (sizeof device->image[0]) *
device->image_count);
+ if (device->image)
+ memcpy(device->image, data->image, device->image_count);
+ else
+ PLAYERC_ERR1("failed to allocate memory for image, needed %u bytes\n",
(sizeof device->image[0]) * device->image_count);
+ } else
+ {
+ if (device->image) free(device->image);
+ device->image = NULL;
+ }
player_camera_data_t_free(data);
return 0;
}
Modified: code/player/trunk/server/drivers/camera/CMakeLists.txt
===================================================================
--- code/player/trunk/server/drivers/camera/CMakeLists.txt 2010-07-23
17:12:01 UTC (rev 8830)
+++ code/player/trunk/server/drivers/camera/CMakeLists.txt 2010-07-25
20:00:24 UTC (rev 8831)
@@ -1,6 +1,7 @@
ADD_SUBDIRECTORY (v4l)
ADD_SUBDIRECTORY (v4l2)
ADD_SUBDIRECTORY (1394)
+ADD_SUBDIRECTORY (camerareq)
ADD_SUBDIRECTORY (camfilter)
ADD_SUBDIRECTORY (compress)
ADD_SUBDIRECTORY (cvcam)
Added: code/player/trunk/server/drivers/camera/camerareq/CMakeLists.txt
===================================================================
--- code/player/trunk/server/drivers/camera/camerareq/CMakeLists.txt
(rev 0)
+++ code/player/trunk/server/drivers/camera/camerareq/CMakeLists.txt
2010-07-25 20:00:24 UTC (rev 8831)
@@ -0,0 +1,2 @@
+PLAYERDRIVER_OPTION (camerareq build_camerareq ON)
+PLAYERDRIVER_ADD_DRIVER (camerareq build_camerareq SOURCES camerareq.cc)
Added: code/player/trunk/server/drivers/camera/camerareq/camerareq.cc
===================================================================
--- code/player/trunk/server/drivers/camera/camerareq/camerareq.cc
(rev 0)
+++ code/player/trunk/server/drivers/camera/camerareq/camerareq.cc
2010-07-25 20:00:24 UTC (rev 8831)
@@ -0,0 +1,269 @@
+/*
+ * Player - One Hell of a Robot Server
+ * Copyright (C) 2000 Brian Gerkey et al.
+ *
+ * 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: Keeps on emitting PLAYER_CAMERA_REQ_GET_IMAGE request with given
+// interval; all received image frames are published on provided camera
+// interface. Typically used with point-and-shoot digicam devices
+// to simulate live image.
+// Author: Paul Osmialowski
+// Date: 25 Jul 2010
+//
+/////////////////////////////////////////////////////////////////////////////
+
+/** @ingroup drivers */
+/** @{ */
+/** @defgroup driver_camerareq camerareq
+ * @brief PLAYER_CAMERA_REQ_GET_IMAGE request emitter
+
+The camerareq driver keeps on emitting PLAYER_CAMERA_REQ_GET_IMAGE request with
+given interval; all received image frames are published on provided camera
+interface. Typically used with point-and-shoot digicam devices to simulate live
+image.
+
+...@par Compile-time dependencies
+
+- none
+
+...@par Provides
+
+- @ref interface_camera
+
+...@par Requires
+
+- @ref interface_camera
+
+...@par Configuration requests
+
+- none
+
+...@par Configuration file options
+
+- interval (float)
+ - Default: 10.0 (seconds)
+
+- sleep_nsec (integer)
+ - Default: 100000000 (=100ms which gives max 10 fps)
+ - timespec value for nanosleep()
+
+...@par Example
+
+...@verbatim
+driver
+(
+ name "camerareq"
+ requires ["camera:1"]
+ provides ["camera:0"]
+)
+...@endverbatim
+
+...@author Paul Osmialowski
+
+*/
+/** @} */
+
+#include <stddef.h>
+#include <time.h>
+#include <assert.h>
+#include <string.h>
+#include <pthread.h>
+#include <libplayercore/playercore.h>
+
+class Camerareq : public ThreadedDriver
+{
+public:
+ // Constructor; need that
+ Camerareq(ConfigFile * cf, int section);
+ virtual ~Camerareq();
+
+ virtual int MainSetup();
+ virtual void MainQuit();
+
+ // This method will be invoked on each incoming message
+ virtual int ProcessMessage(QueuePointer & resp_queue,
+ player_msghdr * hdr,
+ void * data);
+
+private:
+ virtual void Main();
+ player_devaddr_t p_camera_addr;
+ player_devaddr_t r_camera_addr;
+ Device * r_camera_dev;
+ double interval;
+ int sleep_nsec;
+};
+
+Camerareq::Camerareq(ConfigFile * cf, int section)
+ : ThreadedDriver(cf, section, true, PLAYER_MSGQUEUE_DEFAULT_MAXLEN)
+{
+ memset(&(this->p_camera_addr), 0, sizeof(player_devaddr_t));
+ memset(&(this->r_camera_addr), 0, sizeof(player_devaddr_t));
+ this->r_camera_dev = NULL;
+ this->interval = 0.0;
+ this->sleep_nsec = 0;
+ if (cf->ReadDeviceAddr(&(this->p_camera_addr), section, "provides",
PLAYER_CAMERA_CODE, -1, NULL))
+ {
+ this->SetError(-1);
+ return;
+ }
+ if (this->AddInterface(this->p_camera_addr))
+ {
+ this->SetError(-1);
+ return;
+ }
+ if (cf->ReadDeviceAddr(&this->r_camera_addr, section, "requires",
PLAYER_CAMERA_CODE, -1, NULL) != 0)
+ {
+ this->SetError(-1);
+ return;
+ }
+ this->interval = cf->ReadFloat(section, "interval", 10.0);
+ this->sleep_nsec = cf->ReadInt(section, "sleep_nsec", 100000000);
+ if ((this->sleep_nsec) <= 0)
+ {
+ PLAYER_ERROR("Invalid sleep_nsec value");
+ this->SetError(-1);
+ return;
+ }
+}
+
+Camerareq::~Camerareq() { }
+
+int Camerareq::MainSetup()
+{
+ if (Device::MatchDeviceAddress(this->r_camera_addr, this->p_camera_addr))
+ {
+ PLAYER_ERROR("attempt to subscribe to self");
+ return -1;
+ }
+ this->r_camera_dev = deviceTable->GetDevice(this->r_camera_addr);
+ if (!this->r_camera_dev)
+ {
+ PLAYER_ERROR("unable to locate suitable camera device");
+ return -1;
+ }
+ if (this->r_camera_dev->Subscribe(this->InQueue) != 0)
+ {
+ PLAYER_ERROR("unable to subscribe to camera device");
+ this->r_camera_dev = NULL;
+ return -1;
+ }
+ return 0;
+}
+
+void Camerareq::MainQuit()
+{
+ if (this->r_camera_dev) this->r_camera_dev->Unsubscribe(this->InQueue);
+ this->r_camera_dev = NULL;
+}
+
+Driver * Camerareq_Init(ConfigFile * cf, int section)
+{
+ return reinterpret_cast<Driver *>(new Camerareq(cf, section));
+}
+
+void Camerareq::Main()
+{
+ struct timespec ts;
+ double last_time = 0.0;
+ double t;
+ Message * msg = NULL;
+ player_camera_data_t * img = NULL;
+
+ for (;;)
+ {
+ ts.tv_sec = 0;
+ ts.tv_nsec = this->sleep_nsec;
+ nanosleep(&ts, NULL);
+ pthread_testcancel();
+ this->ProcessMessages();
+ pthread_testcancel();
+ GlobalTime->GetTimeDouble(&t);
+ if ((t - last_time) > (this->interval))
+ {
+ msg = this->r_camera_dev->Request(this->InQueue, PLAYER_MSGTYPE_REQ,
PLAYER_CAMERA_REQ_GET_IMAGE, NULL, 0, NULL, true); // threaded = true
+ if (!msg)
+ {
+ PLAYER_WARN("failed to send request");
+ pthread_testcancel();
+ continue;
+ }
+ if (!(msg->GetDataSize() > 0))
+ {
+ PLAYER_ERROR("empty data received");
+ delete msg;
+ msg = NULL;
+ pthread_testcancel();
+ continue;
+ }
+ img = reinterpret_cast<player_camera_data_t *>(msg->GetPayload());
+ if (!img)
+ {
+ PLAYER_WARN("NULL image received");
+ delete msg;
+ msg = NULL;
+ pthread_testcancel();
+ continue;
+ }
+ if (((img->width) > 0) && ((img->height) > 0) && ((img->image_count) >
0) && (img->image))
+ {
+ this->Publish(this->p_camera_addr, PLAYER_MSGTYPE_DATA,
PLAYER_CAMERA_DATA_STATE, reinterpret_cast<void *>(img), 0,
&(msg->GetHeader()->timestamp), true); // copy = true
+ }
+ delete msg;
+ msg = NULL;
+ img = NULL;
+ last_time = t;
+ }
+ pthread_testcancel();
+ }
+}
+
+int Camerareq::ProcessMessage(QueuePointer & resp_queue, player_msghdr * hdr,
void * data)
+{
+ player_msghdr_t newhdr;
+ Message * msg;
+
+ assert(hdr);
+ if (Message::MatchMessage(hdr, PLAYER_MSGTYPE_DATA,
PLAYER_CAMERA_DATA_STATE, this->r_camera_addr))
+ {
+ assert(data);
+ this->Publish(this->p_camera_addr, PLAYER_MSGTYPE_DATA,
PLAYER_CAMERA_DATA_STATE, data, 0, &(hdr->timestamp), true); // copy = true
+ return 0;
+ } else if (Message::MatchMessage(hdr, PLAYER_MSGTYPE_REQ, -1,
this->p_camera_addr))
+ {
+ hdr->addr = this->r_camera_addr;
+ msg = this->r_camera_dev->Request(this->InQueue, hdr->type, hdr->subtype,
data, 0, NULL, true); // threaded = true
+ if (!msg)
+ {
+ PLAYER_WARN("failed to forward request");
+ return -1;
+ }
+ newhdr = *(msg->GetHeader());
+ newhdr.addr = this->p_camera_addr;
+ this->Publish(resp_queue, &newhdr, msg->GetPayload(), true); // copy =
true, do not dispose published data as we're disposing whole source message in
the next line
+ delete msg;
+ return 0;
+ }
+ return -1;
+}
+
+void camerareq_Register(DriverTable *table)
+{
+ table->AddDriver("camerareq", Camerareq_Init);
+}
Modified: code/player/trunk/server/drivers/camera/compress/cameracompress.cc
===================================================================
--- code/player/trunk/server/drivers/camera/compress/cameracompress.cc
2010-07-23 17:12:01 UTC (rev 8830)
+++ code/player/trunk/server/drivers/camera/compress/cameracompress.cc
2010-07-25 20:00:24 UTC (rev 8831)
@@ -89,6 +89,7 @@
/** @} */
#include <string.h>
+#include <stdio.h>
#include <stdlib.h>
#ifndef WIN32
#include <unistd.h>
@@ -104,6 +105,8 @@
{
// Constructor
public: CameraCompress( ConfigFile* cf, int section);
+ // Destructor
+ public: virtual ~CameraCompress();
// Setup/shutdown routines.
public: virtual int MainSetup();
@@ -129,7 +132,7 @@
bool camera_subscribed;
// Output (compressed) camera data
- private: player_camera_data_t data;
+ private: player_camera_data_t imgdata;
// Image quality for JPEG compression
private: double quality;
@@ -142,7 +145,6 @@
private: int request_only;
};
-
Driver *CameraCompress_Init(ConfigFile *cf, int section)
{
return ((Driver*) (new CameraCompress(cf, section)));
@@ -156,7 +158,8 @@
CameraCompress::CameraCompress( ConfigFile *cf, int section)
: ThreadedDriver(cf, section, true, PLAYER_MSGQUEUE_DEFAULT_MAXLEN,
PLAYER_CAMERA_CODE)
{
- this->data.image = NULL;
+ this->imgdata.image_count = 0;
+ this->imgdata.image = NULL;
this->frameno = 0;
this->camera = NULL;
@@ -177,6 +180,16 @@
return;
}
+CameraCompress::~CameraCompress()
+{
+ if (this->imgdata.image)
+ {
+ delete []this->imgdata.image;
+ this->imgdata.image = NULL;
+ this->imgdata.image_count = 0;
+ }
+}
+
int CameraCompress::MainSetup()
{
// Subscribe to the camera.
@@ -203,10 +216,11 @@
{
camera->Unsubscribe(InQueue);
- if (this->data.image)
+ if (this->imgdata.image)
{
- delete []this->data.image;
- this->data.image = NULL;
+ delete []this->imgdata.image;
+ this->imgdata.image = NULL;
+ this->imgdata.image_count = 0;
}
}
@@ -215,45 +229,33 @@
int CameraCompress::ProcessMessage(QueuePointer & resp_queue, player_msghdr *
hdr,
void * data)
{
- player_camera_data_t * imgData;
+ player_msghdr_t newhdr;
Message * msg;
assert(hdr);
if (Message::MatchMessage(hdr, PLAYER_MSGTYPE_DATA,
PLAYER_CAMERA_DATA_STATE, this->camera_id))
{
assert(data);
- imgData = reinterpret_cast<player_camera_data_t * >(data);
if (this->request_only) return 0;
if ((!(this->check_timestamps)) || (this->camera_time != hdr->timestamp))
{
this->camera_time = hdr->timestamp;
- if (!(this->ProcessImage(*imgData))) this->Publish(device_addr,
PLAYER_MSGTYPE_DATA, PLAYER_CAMERA_DATA_STATE, reinterpret_cast<void
*>(&(this->data)), 0, &this->camera_time);
- // don't delete anything here! this->data.image is required and is
deleted somewhere else
+ if (!(this->ProcessImage(*(reinterpret_cast<player_camera_data_t
*>(data))))) this->Publish(this->device_addr, PLAYER_MSGTYPE_DATA,
PLAYER_CAMERA_DATA_STATE, reinterpret_cast<void *>(&(this->imgdata)), 0,
&(this->camera_time));
+ // don't delete anything here! this->imgdata.image is required and is
deleted somewhere else
}
return 0;
- } else if (Message::MatchMessage(hdr, PLAYER_MSGTYPE_REQ,
PLAYER_CAMERA_REQ_GET_IMAGE, this->device_addr))
+ } else if (Message::MatchMessage(hdr, PLAYER_MSGTYPE_REQ, -1,
this->device_addr))
{
- msg = this->camera->Request(this->InQueue, PLAYER_MSGTYPE_REQ,
PLAYER_CAMERA_REQ_GET_IMAGE, NULL, 0, NULL);
- if (!msg) return -1;
- if ((msg->GetDataSize()) < (sizeof(player_camera_data_t)))
+ hdr->addr = this->camera_id;
+ msg = this->camera->Request(this->InQueue, hdr->type, hdr->subtype, data,
0, NULL, true); // threaded = true
+ if (!msg)
{
- delete msg;
+ PLAYER_WARN("failed to forward request");
return -1;
}
- imgData = reinterpret_cast<player_camera_data_t *>(msg->GetPayload());
- if (!imgData)
- {
- delete msg;
- return -1;
- }
- this->camera_time = hdr->timestamp;
- if (!(this->ProcessImage(*imgData))) this->Publish(this->device_addr,
- resp_queue,
- PLAYER_MSGTYPE_RESP_ACK,
-
PLAYER_CAMERA_REQ_GET_IMAGE,
- reinterpret_cast<void
*>(&(this->data)),
- 0, &this->camera_time);
- // don't delete anything here! this->data.image is required and is deleted
somewhere else
+ newhdr = *(msg->GetHeader());
+ newhdr.addr = this->device_addr;
+ this->Publish(resp_queue, &newhdr, msg->GetPayload(), true); // copy =
true, do not dispose published data as we're disposing whole source message in
the next line
delete msg;
return 0;
}
@@ -284,7 +286,7 @@
if ((rawdata.width <= 0) || (rawdata.height <= 0))
{
- if (!(this->data.image)) return -1;
+ if (!(this->imgdata.image)) return -1;
} else if (rawdata.compression == PLAYER_CAMERA_COMPRESS_RAW)
{
switch (rawdata.bpp)
@@ -293,7 +295,7 @@
l = (rawdata.width) * (rawdata.height);
ptr = buffer = new unsigned char[(rawdata.width) * (rawdata.height) * 3];
assert(buffer);
- ptr1 = (unsigned char *)(rawdata.image);
+ ptr1 = reinterpret_cast<unsigned char *>(rawdata.image);
for (i = 0; i < l; i++)
{
ptr[0] = *ptr1;
@@ -304,13 +306,13 @@
ptr = buffer;
break;
case 24:
- ptr = (unsigned char *)(rawdata.image);
+ ptr = reinterpret_cast<unsigned char *>(rawdata.image);
break;
case 32:
l = (rawdata.width) * (rawdata.height);
ptr = buffer = new unsigned char[(rawdata.width) * (rawdata.height) * 3];
assert(buffer);
- ptr1 = (unsigned char *)(rawdata.image);
+ ptr1 = reinterpret_cast<unsigned char *>(rawdata.image);
for (i = 0; i < l; i++)
{
ptr[0] = ptr1[0];
@@ -324,35 +326,35 @@
PLAYER_WARN("unsupported image depth (not good)");
return -1;
}
- if (this->data.image) delete []this->data.image;
- this->data.image = new unsigned char[(rawdata.width) * (rawdata.height) *
3];
- assert(this->data.image);
- this->data.image_count = jpeg_compress( (char *)(this->data.image),
- (char *)ptr,
- rawdata.width,
- rawdata.height,
- (rawdata.width) * (rawdata.height)
* 3,
- (int)(this->quality*100));
- this->data.width = (rawdata.width);
- this->data.height = (rawdata.height);
- this->data.bpp = 24;
- this->data.format = PLAYER_CAMERA_FORMAT_RGB888;
- this->data.fdiv = (rawdata.fdiv);
- this->data.compression = PLAYER_CAMERA_COMPRESS_JPEG;
- this->data.image_count = (this->data.image_count);
+ if (this->imgdata.image) delete []this->imgdata.image;
+ this->imgdata.image = new unsigned char[(rawdata.width) * (rawdata.height)
* 3];
+ assert(this->imgdata.image);
+ this->imgdata.image_count = jpeg_compress(reinterpret_cast<char
*>(this->imgdata.image),
+ reinterpret_cast<char *>(ptr),
+ rawdata.width,
+ rawdata.height,
+ (rawdata.width) *
(rawdata.height) * 3,
+ static_cast<int>(this->quality *
100));
+ this->imgdata.width = (rawdata.width);
+ this->imgdata.height = (rawdata.height);
+ this->imgdata.bpp = 24;
+ this->imgdata.format = PLAYER_CAMERA_FORMAT_RGB888;
+ this->imgdata.fdiv = (rawdata.fdiv);
+ this->imgdata.compression = PLAYER_CAMERA_COMPRESS_JPEG;
+ this->imgdata.image_count = (this->imgdata.image_count);
} else
{
- if (this->data.image) delete []this->data.image;
- this->data.image = new unsigned char[rawdata.image_count];
- assert(this->data.image);
- memcpy(this->data.image, rawdata.image, rawdata.image_count);
- this->data.width = (rawdata.width);
- this->data.height = (rawdata.height);
- this->data.bpp = (rawdata.bpp);
- this->data.format = (rawdata.format);
- this->data.fdiv = (rawdata.fdiv);
- this->data.compression = (rawdata.compression);
- this->data.image_count = (rawdata.image_count);
+ if (this->imgdata.image) delete []this->imgdata.image;
+ this->imgdata.image = new unsigned char[rawdata.image_count];
+ assert(this->imgdata.image);
+ memcpy(this->imgdata.image, rawdata.image, rawdata.image_count);
+ this->imgdata.width = (rawdata.width);
+ this->imgdata.height = (rawdata.height);
+ this->imgdata.bpp = (rawdata.bpp);
+ this->imgdata.format = (rawdata.format);
+ this->imgdata.fdiv = (rawdata.fdiv);
+ this->imgdata.compression = (rawdata.compression);
+ this->imgdata.image_count = (rawdata.image_count);
}
if (buffer) delete []buffer;
buffer = NULL;
@@ -367,7 +369,7 @@
fp = fopen(filename, "w+");
if (fp)
{
- ret = fwrite(this->data.image, 1, this->data.image_count, fp);
+ ret = fwrite(this->imgdata.image, 1, this->imgdata.image_count, fp);
if (ret < 0) PLAYER_ERROR("Failed to save frame");
fclose(fp);
}
This was sent by the SourceForge.net collaborative development platform, the
world's largest Open Source development site.
------------------------------------------------------------------------------
This SF.net email is sponsored by Sprint
What will you do first with EVO, the first 4G phone?
Visit sprint.com/first -- http://p.sf.net/sfu/sprint-com-first
_______________________________________________
Playerstage-commit mailing list
[email protected]
https://lists.sourceforge.net/lists/listinfo/playerstage-commit
