// Additive inverse:
template <typename T> Measurement<T> Measurement<T>::operator - ()
{
Measurement newMeasurement(*this);
newMeasurement.value *= static_cast<T>(-1.0);
return newMeasurement;
}
void SerialScale::dataAvailable()
{
responseBuffer.append(readAll());
if(responseBuffer.contains("\x0D"))
{
if(responseBuffer.contains("!"))
{
responseBuffer.clear();
}
else
{
/*1000:*/
#line 189 "./scales.w"
QStringList responseParts= QString(responseBuffer.simplified()).split(' ');
if(responseParts.size()> 2)
{
responseParts.removeFirst();
responseParts.replace(0,QString("-%1").arg(responseParts[0]));
}
double weight= responseParts[0].toDouble();
Units::Unit unit= Units::Unitless;
if(responseParts[1]=="lb")
{
unit= Units::Pound;
}
else if(responseParts[1]=="kg")
{
unit= Units::Kilogram;
}
else if(responseParts[1]=="g")
{
unit= Units::Gram;
}
else if(responseParts[1]=="oz")
{
unit= Units::Ounce;
}
emit newMeasurement(weight,unit);
/*:1000*/
#line 161 "./scales.w"
responseBuffer.clear();
}
}
}
/* -----------------------------------------------------------------------------
* Callback function called when new detections are received
*/
void TrackerKalmanNode::newDataCallback(const but_objdet_msgs::DetectionArrayConstPtr &detArrayMsg)
{
//ROS_ERROR("%d",detArrayMsg->detections.size());
int detClass;
for(unsigned int i = 0; i < detArrayMsg->detections.size(); i++) {
detClass = detArrayMsg->detections[i].m_class;
int detId = detArrayMsg->detections[i].m_id;
// Check if the current detection is already in the memory
_DetMem::iterator it = detectionMem[detClass].find(detId);
// When it was found
if(it != detectionMem[detClass].end()) {
//ROS_ERROR("Object ID found!");
detectionMem[detClass][detId].det = detArrayMsg->detections[i];
detectionMem[detClass][detId].ttl++;
int time = rosTimeToMs(detArrayMsg->header.stamp);
int timeFromLastUpdate = time - detectionMem[detClass][detId].msTime;
detectionMem[detClass][detId].msTime = time;
// Update
Mat newMeasurement(1, 4, CV_32F);
newMeasurement.at<float>(0) = detectionMem[detClass][detId].det.m_bb.x;
newMeasurement.at<float>(1) = detectionMem[detClass][detId].det.m_bb.y;
newMeasurement.at<float>(2) = detectionMem[detClass][detId].det.m_bb.width;
newMeasurement.at<float>(3) = detectionMem[detClass][detId].det.m_bb.height;
}
// When it wasn't found => add it to memory
else {
// ROS_ERROR("Object ID not found!");
detectionMem[detClass][detId].det = detArrayMsg->detections[i];
detectionMem[detClass][detId].ttl = defaultTtl;
detectionMem[detClass][detId].kf = new TrackerKalman();
detectionMem[detClass][detId].msTime = rosTimeToMs(detArrayMsg->header.stamp);
// Initialization with the first measurement
Mat initMeasurement(1, 4, CV_32F);
initMeasurement.at<float>(0) = detectionMem[detClass][detId].det.m_bb.x;
initMeasurement.at<float>(1) = detectionMem[detClass][detId].det.m_bb.y;
initMeasurement.at<float>(2) = detectionMem[detClass][detId].det.m_bb.width;
initMeasurement.at<float>(3) = detectionMem[detClass][detId].det.m_bb.height;
detectionMem[detClass][detId].kf->init(initMeasurement, true);
}
}
// Decrease TTL to all saved detections
_DetMem::iterator it;
vector<int> toBeRemoved; // Vector of IDs whose detection has TTL = 0
for (it = detectionMem[detClass].begin(); it != detectionMem[detClass].end(); it++) {
it->second.ttl--;
//std::cout << "TTL: " << it->second.ttl << " " << detectionMem.size() << std::endl;
// If it didn't show up in the specified number of last detections
// or during the specified time period => mark it for removal
//TODO: not working, why? if(it->second.ttl == 0 || (rosTimeToMs(ros::Time::now()) - it->second.msTime) > defaultTtlTime) {
if(it->second.ttl == 0) {
toBeRemoved.push_back(it->second.det.m_id);
}
}
// Remove marked detections
for(unsigned int i = 0; i < toBeRemoved.size(); i++) {
delete detectionMem[detClass][toBeRemoved[i]].kf; // Free Kalman filter
detectionMem[detClass].erase(toBeRemoved[i]);
// ROS_ERROR("remove");
}
}
template <typename T> Measurement<T> Measurement<T>::operator / (const Measurement<T> &measurement)
{
Measurement newMeasurement(*this);
return newMeasurement /= measurement;
}
template <typename T> Measurement<T> Measurement<T>::operator - (Measurement<T> &measurement)
{
Measurement newMeasurement(*this);
return newMeasurement -= measurement;
}