上传右转报警的逻辑

EventsRos.cpp

@@ -130,7 +130,17 @@ int EventsRos::loadConfig(ros::NodeHandle& nh)
     //nh.param<int32_t>("delete_obj_interval", config.delete_obj_interval, 200);
     config.flow_circle_interval = m_config["flow_circle_interval"].as< int32_t>();
     //nh.param<int32_t>("flow_circle_interval", config.flow_circle_interval, 60000);
-    
+    if(m_config["big_truck_name_list"])
+        config.big_truck_name_list = m_config["big_truck_name_list"].as<std::string>();
+    if(m_config["turn_right_alter_status"])
+        config.turn_right_alter_status = m_config["turn_right_alter_status"].as< int32_t>();
+    if(config.turn_right_alter_status == 1)
+    {
+        std::string csv = m_config["turn_right_alter_csv"].as<std::string>();
+        std::string file_csv = m_root_dir + "/" + csv;
+        SDK_LOG(SDK_INFO, "load turn right csv = %s",file_csv.c_str());
+        config.LoadTurnRightCSV(file_csv);
+    }
 
 #if defined (_ROS_XISHAN_) || defined(_ROS_HEFEI_R_)
 #else

events/JfxEvents.cpp

@@ -1469,6 +1469,8 @@ void JfxEvents::ProcessSendEvents(TrkObjsPtr& objsPtr)
 void JfxEvents::ProcessSendEvents2(TrkObjsPtr& objsPtr)
 {
     uint64_t curTime = m_curTime;
+    std::map<int,int> isTurnRight;
+    std::map<int, std::vector<double>> turnRightPoint;
     //std::map<int64_t, std::vector<eventInfo> > curEventLists;//记录这次将要发送的事件列表
     for (auto& obj : objsPtr->objs)//处理当前帧的物品信息
     {
@@ -1989,6 +1991,65 @@ void JfxEvents::ProcessSendEvents2(TrkObjsPtr& objsPtr)
             }
         }// if (objA.isInMap == 1)
 
+        if(m_cfg.turn_right_alter_status == 1)
+        {
+            if(m_cfg.IsBigTruck(obj.name) == 0)
+            {
+                int ret = m_cfg.CheckInArea(obj.lon,obj.lat);
+                if(ret != -1)
+                {
+                    if(isTurnRight.find(ret) == isTurnRight.end())
+                    {
+                        isTurnRight[ret] = 1;
+                    }
+                    else
+                    {
+                        isTurnRight[ret]++;
+                    }
+                    std::vector<double> point;
+                    point.push_back(obj.lon);
+                    point.push_back(obj.lat);
+                    turnRightPoint[ret] = point;
+                }
+            }
+        }
+
+    }
+
+    for(auto& iter : m_turnRightStatus)
+    {
+        if(iter.second > 0 && isTurnRight.find(iter.first) == isTurnRight.end())//记录上次有右转事件，这一次没有说明右转结束了
+        {
+            turnRightAlterInfo turn = {};
+            turn.timestamp = objsPtr->timestamp;
+            turn.orien = iter.first;
+            turn.status = 0;
+            turn.longitude = m_turnRightPoints[iter.first][0];
+            turn.latitude = m_turnRightPoints[iter.first][1];
+            objsPtr->turnRight.emplace_back(turn);
+            iter.second = 0;
+        }
+    }
+    for(auto iter: isTurnRight)
+    {
+        if(m_turnRightStatus.find(iter.first) == m_turnRightStatus.end())
+            m_turnRightStatus[iter.first] = 0;
+        if(m_turnRightStatus[iter.first] == 0)//说明之前没有大车右转，现在有了
+        {
+            turnRightAlterInfo turn = {};
+            turn.timestamp = objsPtr->timestamp;
+            turn.orien = iter.first;
+            turn.status = 1;
+            turn.longitude = turnRightPoint[iter.first][0];
+            turn.latitude = turnRightPoint[iter.first][1];
+            objsPtr->turnRight.emplace_back(turn);
+            m_turnRightStatus[iter.first] = iter.second;
+        }
+        else
+        {
+            m_turnRightStatus[iter.first] = iter.second;
+        }
+        m_turnRightPoints[iter.first] = turnRightPoint[iter.first];
     }
 
     for (auto& iter : m_frameObjs)

events/JfxEvents.h

@@ -278,4 +278,7 @@ public:
     TrafficJam m_totelTrafficJam;
 
     int m_cross_id = 0;//路口id
+
+    std::map<int,int> m_turnRightStatus;
+    std::map<int,std::vector<double>> m_turnRightPoints;
 };

events/config.cpp

@@ -3,6 +3,11 @@
 #include "config.h"
 #include "LogBase.h"
 #include <algorithm>
+#include <iostream>
+#include <fstream>
+#include <string>
+#include <vector>
+#include <sstream>
 
 void Split(std::string& input, std::string separator, std::vector<std::string>& out)
 {
@@ -18,6 +23,16 @@ void Split(std::string& input, std::string separator, std::vector<std::string>&
     }
 }
 
+int IsInMultiArea(std::vector<std::vector<double>> &poly, const std::vector<double> &pt) //返回1就是在多边形里，0是在多边形外
+{
+    size_t i, j, c = 0;
+    for (i = 0, j = poly.size() - 1; i < poly.size(); j = i++) {
+        if (((poly[i][1] > pt[1]) != (poly[j][1] > pt[1])) &&
+            (pt[0] < (poly[j][0] - poly[i][0]) * (pt[1] - poly[i][1]) / (poly[j][1] - poly[i][1]) + poly[i][0]))
+            c = !c;
+    }
+    return c;
+}
 
 void EventsConfig::PacketArea()
 {
@@ -54,6 +69,7 @@ void EventsConfig::PacketArea()
     Split(bicyle_name_list, ";", bicyleLists);
     Split(person_name_list, ";", personLists);
     Split(motorcycle_name_list, ";", motorcycleLists);
+    Split(big_truck_name_list, ";", bigTruckList);
     //max_high_speed *= 0.1;
     //min_low_speed *= 0.1;
     //min_park_speed *= 0.1;
@@ -77,6 +93,10 @@ void EventsConfig::PacketArea()
     {
         SDK_LOG(SDK_INFO, "load nonmotor name = %s", iter.c_str());
     }
+    for (auto iter : bigTruckList)
+    {
+        SDK_LOG(SDK_INFO, "load big_truck_name_list name = %s", iter.c_str());
+    }    
 }
 int EventsConfig::IsInArea(double& lat, double& lon)
 {
@@ -137,4 +157,77 @@ int EventsConfig::IsMotorBicycle(std::string& name)
             return 0;
     }
     return 1;
-}
\ No newline at end of file
+}
+int EventsConfig::IsBigTruck(std::string& name)
+{
+    for (auto iter : bigTruckList)
+    {
+        if (name == iter)
+            return 0;
+    }
+    return 1;    
+}
+
+int EventsConfig::LoadTurnRightCSV(std::string& csv)
+{
+    std::ifstream file;
+	file.open(csv, std::ios::in);
+	std::string line1;
+	std::getline(file, line1);//掠过第一行数据
+
+    while(1)
+    {
+        std::string line;
+        std::getline(file, line);
+        std::vector<std::string> values;
+        Split(line,",",values);
+        if(values.size() != 4)
+            break;
+        int idx = std::stoi(values[0]);
+        double lon = std::stod(values[1]);
+        double lat = std::stod(values[2]);
+        int index = std::stoi(values[3]);
+        if(m_turnRightArea.find(index) == m_turnRightArea.end())
+        {
+            std::map<int,std::vector<double>> area;
+            m_turnRightArea[index] = area;
+        }
+        std::vector<double> point;
+        point.push_back(lon);
+        point.push_back(lat);
+        m_turnRightArea[index][idx] = point;
+    }
+    for(auto iter: m_turnRightArea)
+    {
+        int size = iter.second.size();
+        std::vector<std::vector<double>> area;
+        for(int i = 1; i <= size; i++)
+        {
+            if(iter.second.find(i) == iter.second.end())
+            {
+                SDK_LOG(SDK_INFO, "load turn right csv error, index = %d, i = %d, no find", iter.first,i);
+                break;
+            }
+            area.push_back((iter.second)[i]);
+            SDK_LOG(SDK_INFO, "load turn right csv index = %d, i = %d, lon = %.8f,lat = %.8f",iter.first,i,(iter.second)[i][0],(iter.second)[i][1]);
+        }
+        m_turnRightCheckArea[iter.first] = area;
+    }
+    return 0;
+}
+
+int EventsConfig::CheckInArea(double lon, double lat)
+{
+    std::vector<double> point;
+    point.push_back(lon);
+    point.push_back(lat);
+    for(auto iter : m_turnRightCheckArea)
+    {
+        int ret = IsInMultiArea(iter.second,point);
+        if(ret == 1)
+        {
+            return iter.first;
+        }
+    }
+    return -1;
+}

events/config.h

 #pragma once
 #include <string>
 #include <vector>
+#include <map>
 
 
 struct ForbiddenArea
@@ -16,8 +17,9 @@ struct ForbiddenArea
 /**
 * @brief 配置信息结构
 */
-struct EventsConfig
+class EventsConfig
 {
+public:
     bool daemon;
     int32_t threadNum;
     int32_t maxTaskNum;
@@ -50,6 +52,7 @@ struct EventsConfig
     std::string bicyle_name_list;//非机动车所有类型列表
     std::string person_name_list;//行人所有类型列表
     std::string motorcycle_name_list;//摩托车类型
+    std::string big_truck_name_list;//大车的类型
     int32_t check_park_time;//停车之后多长时间认为是非法停车
     int32_t traffic_jam_interval;//大于这个数量之后多长时间后判断为拥堵
     float traffic_jam_mim_speed;//拥堵检测最小速度
@@ -63,11 +66,14 @@ struct EventsConfig
     int delete_obj_interval;//删除物体延迟时间
     int flow_circle_interval = 60000;//流量统计时间间隔
 
+    int turn_right_alter_status = 0;//大车右转警报是否开启，0是关闭，1是开启
+
     std::vector<ForbiddenArea> forbiddenAreas;
     std::vector<std::string> nonmotorLists;
     std::vector<std::string> bicyleLists;
     std::vector<std::string> personLists;
     std::vector<std::string> motorcycleLists;
+    std::vector<std::string> bigTruckList;
 
     void PacketArea();
     int IsInArea(double& lat, double& lon);
@@ -76,4 +82,12 @@ struct EventsConfig
     int IsPersion(std::string& name);
     int IsCar(std::string& name);//只检查汽车类，去除摩托车
     int IsMotorBicycle(std::string& name);//检查摩托车
+    int IsBigTruck(std::string& name);//检查是否是大车,是大车返回0，不是大车返回1
+
+    int LoadTurnRightCSV(std::string& csv);
+    int CheckInArea(double lon, double lat);
+
+    std::map<int,std::map<int,std::vector<double>> > m_turnRightArea;
+    std::map<int,std::vector<std::vector<double>> > m_turnRightCheckArea;
+
 };
\ No newline at end of file

msg/EventInfo.h

@@ -50,6 +50,22 @@ AJSON(trafficJamInfo,
     length
 )
 
+typedef struct _TurnRightAlterInfo {
+    uint64_t timestamp;//时间戳
+    int32_t orien; // 区域标号
+    int32_t status;//事件状态，包括1开始，0结束
+    uint64_t longitude = 0;
+    uint64_t latitude = 0;
+}turnRightAlterInfo;
+
+AJSON(turnRightAlterInfo,
+    timestamp,
+    orien,
+    status,
+    longitude,
+    latitude
+)
+
 typedef struct _TrkObj {
     int32_t frameNum;
     std::string radar_id;
@@ -173,6 +189,7 @@ typedef struct _TrkObjs {
     std::string mecNo; // MecNo 工控机id
     std::vector<TrkObj> objs;
     std::vector<trafficJamInfo> traffic;
+    std::vector<turnRightAlterInfo> turnRight;
     std::vector<flowEvent> flowInfo;
     std::vector<uint64_t> pt;
 }TrkObjs;