#include "usg_common.h"
|
#include "IndirectAlg.h"
|
#include "graph.h"
|
|
#define PI 3.14159265
|
|
using namespace std;
|
|
|
struct Status {
|
|
// target id
|
std::string id;
|
//第一次出现的时间
|
time_t firstTime;
|
//上次出现的时间
|
time_t lastTime;
|
//第一次逆行的时间
|
time_t firstInvalTime;
|
//上次逆行的时间
|
time_t lastInvalTime;
|
//第一次正确行的时间
|
time_t firstValTime;
|
//上次逆=正确行的时间
|
time_t lastValTime;
|
//上一次出现的坐标
|
Point lastCoordinate;
|
// Status(std::string _id, time_t ftime, time_t ltime, Point coordinate):
|
// id(_id), firstInvalTime(ftime), lastInvalTime(ltime), lastCoordinate(coordinate){}
|
};
|
|
|
IndirectAlg::IndirectAlg() {}
|
|
IndirectAlg::IndirectAlg(const initializer_list<Edge> & edges, const time_t & _keepTime, const Vector2 & _direction):
|
frame(edges), keepTime(_keepTime), direction(_direction), mterminate(false) {
|
|
mthread = std::thread(std::bind(&IndirectAlg::threadRoutine, this));
|
|
}
|
|
IndirectAlg::IndirectAlg(const initializer_list<Point> & points, const time_t & _keepTime, const Vector2 & _direction):
|
keepTime(_keepTime), direction(_direction), mterminate(false) {
|
if (points.size() > 0) {
|
std::vector<Edge> edges;
|
initializer_list<Point>::iterator iter;
|
for(iter = points.begin(); iter < points.end(); iter++ ) {
|
edges.push_back({*iter, *(iter +1)});
|
}
|
edges.push_back({*(points.end()), *(points.begin())});
|
frame = Figure(edges);
|
}
|
|
|
mthread = std::thread(std::bind(&IndirectAlg::threadRoutine, this));
|
|
}
|
|
IndirectAlg::~IndirectAlg() {
|
//销毁定时线程
|
mterminate.store(true);
|
mthread.join();
|
//销毁map
|
map<string, Status *>::iterator iter;
|
Status *status = nullptr;
|
for(iter = statusMap.begin(); iter != statusMap.end(); iter++) {
|
//cout<<iter->first<<' '<<iter->second<<endl;
|
status = iter->second;
|
if (status != nullptr) {
|
delete status;
|
statusMap.erase(iter);
|
}
|
}
|
}
|
|
void IndirectAlg::threadRoutine() {
|
//定时清理不在监控区的对象
|
//sleep(this->keepTime);
|
map<string, Status *>::iterator iter;
|
Status *status = nullptr;
|
while (!mterminate.load()) {
|
std::this_thread::sleep_for(std::chrono::seconds(this->keepTime * 2));
|
//sleep(this->keepTime);
|
for(iter = statusMap.begin(); iter != statusMap.end(); iter++) {
|
|
status = iter->second;
|
if (status != nullptr) {
|
// cout<< "iter " << iter->first <<' '<< iter->second << status->lastTime << endl;
|
// printf("%ld, %ld, %ld\n", time(NULL), status->lastTime, (time(0) - status->lastTime));
|
if(difftime(time(NULL), status->lastTime) > this->keepTime*2) {
|
std::cout << status->id << "脱离监控区, remove from map\n";
|
delete status;
|
statusMap.erase(iter);
|
}
|
}
|
}
|
}
|
|
}
|
|
|
/**
|
* for debug
|
*/
|
void IndirectAlg::printRecord(int tag, Record &record) {
|
// printf("%d %ld : {%f, %f}\n", tag, record.timestamp, record.coordinate.x, record.coordinate.y);
|
}
|
|
/**
|
* 是否逆行
|
*/
|
bool IndirectAlg::isRetrograde(Record &record) {
|
if(record.id.length() == 0) {
|
return false;
|
}
|
|
Status *status = nullptr;
|
|
map<string, Status *>::iterator statusIter = statusMap.find(record.id);
|
if( statusIter != statusMap.end() ) {
|
status = statusIter->second;
|
}
|
|
if (status == nullptr) {
|
// 第一次出现
|
if(frame.contains(record.coordinate)) {
|
status = new Status({
|
.id = record.id,
|
.firstTime = record.timestamp,
|
.lastTime = record.timestamp,
|
.firstInvalTime = 0,
|
.lastInvalTime = 0,
|
.firstValTime = 0,
|
.lastValTime = 0,
|
.lastCoordinate = record.coordinate});
|
|
statusMap.insert({record.id, status});
|
printRecord(1, record);
|
}
|
printRecord(2, record);
|
return false;
|
}
|
|
Vector2 currentDirection = getVecor2(status -> lastCoordinate, record.coordinate);
|
if(difftime(record.timestamp, status->lastTime) >= this->keepTime) {
|
//printf("update lastCoordinate curtime=%ld, lastTime=%ld, diff=%f\n", record.timestamp, status->lastTime, difftime(record.timestamp, status->lastTime));
|
status->lastTime = record.timestamp;
|
status->lastCoordinate = record.coordinate;
|
}
|
|
if (currentDirection.x == 0 && currentDirection.y == 0) {
|
//没动
|
printRecord(3, record);
|
return false;
|
}
|
|
//status -> lastCoordinate = record.coordinate;
|
//逆行判断 cos小于0即两个向量的夹角大于90度,表示逆行了
|
if(frame.contains(record.coordinate) && getVector2Angle(direction, currentDirection) <= 0) {
|
if(status->firstInvalTime ==0) {
|
//第一次发生逆行
|
status->firstInvalTime = status->lastInvalTime = record.timestamp;
|
//printf("%第一次发生逆行的时间=%ld\n", status->firstInvalTime);
|
printRecord(4, record);
|
return false;
|
|
} else {
|
status->lastInvalTime = record.timestamp;
|
//printf("%后续发生逆行的时间=%ld\n", status->lastInvalTime);
|
//逆行时间超过keeptime
|
if (difftime(status->lastInvalTime, status->firstInvalTime) >= keepTime) {
|
status->firstValTime = status->lastValTime = 0;
|
printRecord(5, record);
|
return true;
|
}
|
printRecord(6, record);
|
return false;
|
}
|
} else {
|
|
if(status->firstValTime ==0) {
|
//记录第一次正确行走时间
|
status->firstValTime = status->lastValTime = record.timestamp;
|
printRecord(7, record);
|
} else {
|
status->lastValTime = record.timestamp;
|
//正确行走时间超过keeptime
|
if (difftime(status->lastValTime, status->firstValTime) >= keepTime) {
|
status->firstInvalTime = status->lastInvalTime = 0;
|
printRecord(8, record);
|
}
|
}
|
|
return false;
|
}
|
|
}
|
