void Stopwatch::send(const std::string& option) {
// only handle existing options
if (options.find(option) == options.end())
return;
// create option containter
de::fumanoids::message::Message msg;
msg.set_robotid(services.getID());
de::fumanoids::message::Debugging &dbg = *msg.MutableExtension(de::fumanoids::message::debugging);
de::fumanoids::message::Debugging::OptionStopwatch &sw = *dbg.add_optionstopwatch();
sw.set_option(option);
// send all valid stopwatches
for (StopwatchItems::const_iterator iter = options[option].begin();
iter != options[option].end();
++iter)
{
de::fumanoids::message::Debugging::OptionStopwatch::StopwatchValues &val = *sw.add_values();
val.set_name(iter->second.name);
val.set_duration(Millisecond(iter->second.lastValue).value());
val.set_minimum(Millisecond(iter->second.min).value());
val.set_maximum(Millisecond(iter->second.max).value());
val.set_mean(Millisecond(iter->second.mean).value());
}
services.getComm().broadcastMessage(msg);
}
int CTimeDate::MillisecondsSinceMidnight (void) const
// MillisecondsSinceMidnight
//
// Returns the number of milliseconds since midnight
{
return Millisecond()
+ (Second() * 1000)
+ (Minute() * 60 * 1000)
+ (Hour() * 60 * 60 * 1000);
}
bool CameraVideo::capture() {
// keep the requested interval
robottime_t currentTime = getCurrentTime();
robottime_t nextImageTime = lastImageCaptured + Millisecond(1./fps);
if (nextImageTime > currentTime)
delay(nextImageTime - currentTime);
cv::Mat cvImageBGR;
*video >> cvImageBGR;
uint16_t imageHeight = cvImageBGR.rows;
uint16_t imageWidth = cvImageBGR.cols;
#if defined IMAGEFORMAT_YUV422
// allocate an uint8_t array, initialize it to zero and provide a proper deleter
std::shared_ptr<uint8_t> yuv422(new uint8_t[imageWidth*imageHeight*2](), std::default_delete<uint8_t[]>());
if (!yuv422) {
ERROR("Could not allocate memory for image conversion.");
return false;
}
// convert image to yuv422
for (int x=0; x < imageWidth; x += 2) {
for (int y=0; y < imageHeight; y++) {
uint8_t y1, y2, u1, u2, v1, v2;
ColorConverter::rgb2yuv(cvImageBGR.at<cv::Vec3b>(y, x )[0], cvImageBGR.at<cv::Vec3b>(y, x )[1], cvImageBGR.at<cv::Vec3b>(y, x )[2], &y1, &u1, &v1);
ColorConverter::rgb2yuv(cvImageBGR.at<cv::Vec3b>(y, x+1)[0], cvImageBGR.at<cv::Vec3b>(y, x+1)[1], cvImageBGR.at<cv::Vec3b>(y, x+1)[2], &y2, &u2, &v2);
yuv422.get()[2*y*imageWidth + 2*x + 0] = y1;
yuv422.get()[2*y*imageWidth + 2*x + 1] = (v1 + v2)/2;
yuv422.get()[2*y*imageWidth + 2*x + 2] = y2;
yuv422.get()[2*y*imageWidth + 2*x + 3] = (u1 + u2)/2;
}
}
image->setImage(0, std::static_pointer_cast<void>(yuv422), imageWidth*imageHeight*2, imageWidth, imageHeight);
#else
#error("Video support not yet implemented for this image type.");
#endif
totalFrames++;
lastImageCaptured = getCurrentTime();
// proceed to next image
imageIdx++;
return true;
}
CString DateTime::FormatISO8601(T_enISO8601Format enFormat, bool bBasic, const TimeZone& tz) const throw()
{
CString cszDate;
switch(enFormat)
{
case formatY: return Format(_T("%Y"), tz);
case formatYM: return Format(bBasic ? _T("%Y%m") : _T("%Y-%m"), tz);
case formatYMD: return Format(bBasic ? _T("%Y%m%d") : _T("%Y-%m-%d"), tz);
case formatYMD_HM_Z:
cszDate = Format(bBasic ? _T("%Y%m%dT%H%M") : _T("%Y-%m-%dT%H:%M"), tz);
break;
case formatYMD_HMS_Z:
cszDate = Format(bBasic ? _T("%Y%m%dT%H%M%S") : _T("%Y-%m-%dT%H:%M:%S"), tz);
break;
case formatYMD_HMSF_Z:
{
cszDate = Format(bBasic ? _T("%Y%m%dT%H%M%S") : _T("%Y-%m-%dT%H:%M:%S"), tz);
CString cszFraction;
cszFraction.Format(_T(".%03u"), Millisecond());
cszDate += cszFraction;
}
break;
}
// add timezone
if (tz.StandardName() == _T("UTC"))
cszDate += _T("Z");
else
{
TimeSpan spTimezone = tz.GetUtcOffset(*this);
bool bNegative = spTimezone < TimeSpan(0, 0, 0, 0);
TimeSpan spTimezoneAbs = bNegative ? -spTimezone : spTimezone;
CString cszTimezone;
cszTimezone.Format(bBasic ? _T("%c%02u%02u") : _T("%c%02u:%02u"),
!bNegative ? _T('+') : _T('-'),
spTimezoneAbs.Hours(),
spTimezoneAbs.Minutes());
cszDate += cszTimezone;
}
return cszDate;
}
void Stopwatch::notifyStop(StopwatchItem& stopwatchItem, const std::string& option, const std::string& stopWatchName) {
if (stopwatchItem.option != option || stopwatchItem.name != stopWatchName) {
notifyStop(option, stopWatchName);
return;
}
stopwatchItem.stop = getCurrentMicroTime();
stopwatchItem.isValid = true;
// update the statistics of the item
stopwatchItem.n++;
Millisecond value = Millisecond(stopwatchItem.stop - stopwatchItem.start);
stopwatchItem.min = std::min(stopwatchItem.min, value);
stopwatchItem.max = std::max(stopwatchItem.max, value);
// update the mean iteratively
// c(n) = c(n-1) + (x_n - c(n-1))/n
stopwatchItem.mean += (value - stopwatchItem.mean) / (double)stopwatchItem.n;
stopwatchItem.lastValue = value;
}
TimeSpan DateTime::TimeOfDay() throw()
{
return TimeSpan(Hour(), Minute(), Second(), Millisecond());
}