void SampleSensor::Update(TimeValue dt)
{
//check to see if it is time to write an update to this sensor
m_sampleTimeLeft -= dt;
//if we still have time left, skip writing an update
if (m_sampleTimeLeft > 0.0)
return;
//Sending the image is dependent on the frame rate and if the user has closed the connection
if((frame % (int) (100 / sendRate) == 0) && running) {
// Get Video Data and Send as ZMQ Message
std::vector<SensorPtr> sensors = SensorManager::GetSingleton().GetAllSensors();
for (uint32 i = 0; i < sensors.size(); ++i)
{
if (sensors[i]->GetSensorType() != "CameraSensor")
continue;
CameraSensor* pCam = static_cast<CameraSensor*>(sensors[i].Get());
const std::vector<LensData>& lens = pCam->GetLensData();
if (lens.size() == 0)
continue;
const LensData& thisLens = lens[0];
const LensParams & lensParams = pCam->GetLensParams()[0];
if (thisLens.m_renderRequest.m_pOutputBuffer != NULL) {
//Pull the dimensions of the camera
int size, sizeX, sizeY;
sizeX = lensParams.m_resolutionX;
sizeY = lensParams.m_resolutionY;
size = sizeX * sizeY * 3;
//Make a buffer for the compressed jpeg
void *buf = malloc(size);
//Set the Quality Factor
jpge::params params;
params.m_quality = quality_factor;
//Compress the image to improve transfer speed
if(compress_image_to_jpeg_file_in_memory(buf, size, sizeX, sizeY, 3, thisLens.m_renderRequest.m_pOutputBuffer, params)) {
//Put the compressed data into a ZMQ message and send it over the socket
zmq::message_t image (size);
memcpy((void *) image.data(), buf, size);
socket_.send (image);
}
else {
LogMessage("Failed to compress image", kLogMsgError);
}
}
}
}
frame++;
m_sampleTimeLeft += m_sampleStep;
}
static bool icvEncodeJpg(
std::string* dst, const char* data, int size,
int width, int height, int channels, int quality /* =90 */,
int width_step /* =0 */
) {
if(dst == NULL || data == NULL || size <= 0) {
return false;
}
if(width <= 0 || height <= 0) {
return false;
}
if(channels != 1 && channels != 3) {
return false;
}
if(quality <= 0 || quality > 100) {
return false;
}
if(width_step < width*channels) {
width_step = width*channels;
}
std::string tmp;
const char* pSrcData = data;
jpge::params comp_params;
if(channels == 3) {
comp_params.m_subsampling = jpge::H2V2; // RGB
comp_params.m_quality = quality;
if(width_step > width*channels) {
tmp.resize(width*height*3);
for(int i = 0; i < height; ++i) {
char* ppTmp = (char*)tmp.data() + i*width*channels;
char* ppSrc = (char*)data + i*width_step;
memcpy(ppTmp, ppSrc, width*channels);
}
pSrcData = tmp.data();
}
} else {
comp_params.m_subsampling = jpge::Y_ONLY; // Gray
comp_params.m_quality = quality;
tmp.resize(width*height*3);
for(int i = 0; i < height; ++i) {
char* ppTmp = (char*)tmp.data() + i*width*3;
char* ppSrc = (char*)data + i*width_step;
for(int j = 0; j < width; ++j) {
ppTmp[j*3+0] = ppSrc[j];
ppTmp[j*3+1] = ppSrc[j];
ppTmp[j*3+2] = ppSrc[j];
}
}
channels = 3;
pSrcData = tmp.data();
}
int buf_size = ((width*height*3)>1024)? (width*height*3): 1024;
dst->resize(buf_size);
bool rv = compress_image_to_jpeg_file_in_memory(
(void*)dst->data(), buf_size, width, height, channels,
(const jpge::uint8*)pSrcData,
comp_params
);
if(!rv) {
dst->clear();
return false;
}
dst->resize(buf_size);
return true;
}
