Skip to content

Commit

Permalink
Preemption sonar issues (#260)
Browse files Browse the repository at this point in the history 
* init

* init

* test

* update

* update

* update

* update

* update

* update

* update

* update
  • Loading branch information
@dan-du-car
dan-du-car authored Oct 7, 2021
1 parent 16a2321 commit 8121e18
Show file tree
Hide file tree
Showing 5 changed files with 38 additions and 49 deletions.
10 changes: 2 additions & 8 deletions src/v2i-hub/PreemptionPlugin/src/PreemptionPlugin.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -105,11 +105,7 @@ void PreemptionPlugin::HandleBasicSafetyMessage(BsmMessage &msg, routeable_messa
bsmTmpID = (int32_t)((buf[0] << 24) + (buf[1] << 16) + (buf[2] << 8) + buf[3]);


//std::vector<int>::iterator it = std::find(allowedList.begin(),allowedList.end(),bsmTmpID);

//if( it != allowedList.end())
//{
if (bsm->partII != NULL) {
if (bsm->partII != NULL) {
if (bsm->partII[0].list.count >= partII_Value_PR_SpecialVehicleExtensions ) {
try {
if(bsm->partII[0].list.array[1]->partII_Value.choice.SpecialVehicleExtensions.vehicleAlerts != NULL){
Expand All @@ -125,9 +121,7 @@ void PreemptionPlugin::HandleBasicSafetyMessage(BsmMessage &msg, routeable_messa
PLOG(logDEBUG)<<"Standard Exception:; Vehicle alerts Unavailable";
}
}
}
//}

}
}

int PreemptionPlugin::Main()
Expand Down
2 changes: 1 addition & 1 deletion src/v2i-hub/PreemptionPlugin/src/PreemptionPlugin.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -43,7 +43,7 @@ class PreemptionPlugin: public PluginClient
}
// PreemptionPlugin(PreemptionPlugin &&fp) noexcept {
// }
PreemptionPlugin const & operator=(PreemptionPlugin &&fp) {
PreemptionPlugin const & operator=(PreemptionPlugin &&fp) noexcept{

}

Expand Down
57 changes: 26 additions & 31 deletions src/v2i-hub/PreemptionPlugin/src/include/PreemptionPluginWorker.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -7,70 +7,67 @@
//==========================================================================

#include "PreemptionPluginWorker.hpp"

#include <memory>
using namespace std;

namespace PreemptionPlugin {

void PreemptionPluginWorker::ProcessMapMessageFile(std::string path){
void PreemptionPluginWorker::ProcessMapMessageFile(const std::string &path){

if(path != ""){
try {
boost::property_tree::read_json(path, geofence_data);

BOOST_FOREACH( boost::property_tree::ptree::value_type const& v, geofence_data.get_child( "data" ) ) {
assert(v.first.empty()); // array elements have no names
boost::property_tree::ptree subtree = v.second;
list <double> geox;
list <double> geoy;

BOOST_FOREACH( boost::property_tree::ptree::value_type const& u, subtree.get_child( "geox" ) ) {
assert(u.first.empty()); // array elements have no names
// std::cout << u.second.get<double>("") << std::endl;
double d = u.second.get<double>("");
geox.push_back(d);
}

BOOST_FOREACH( boost::property_tree::ptree::value_type const& u, subtree.get_child( "geoy" ) ) {
assert(u.first.empty()); // array elements have no names
double d = u.second.get<double>("");
geoy.push_back(d);
}

GeofenceObject* geofenceObject = new GeofenceObject(geox,geoy,subtree.get<double>("PreemptCall"),subtree.get<double>("HeadingMin"),subtree.get<double>("HeadingMax"));
GeofenceObject geofenceObject(geox,geoy, static_cast<int>(subtree.get<double>("PreemptCall")),static_cast<int>(subtree.get<double>("HeadingMin")),static_cast<int>(subtree.get<double>("HeadingMax")));

GeofenceSet.push_back(geofenceObject);
GeofenceSet.push_back(&geofenceObject);

}
}
catch(...) {
std::cout << "Caught exception from reading a file";
PLUGIN_LOG(logERROR, "Preemptionworker") << "Caught exception from reading a file";
}
}

}

bool PreemptionPluginWorker::CarInGeofence(double x, double y, double geox[], double geoy[], int GeoCorners) {
int i, j=GeoCorners-1 ;
bool oddNodes ;
bool PreemptionPluginWorker::CarInGeofence(long double x,long double y, std::vector<double> geox, std::vector<double> geoy, long GeoCorners) const{
long i = 0 ;
long j = GeoCorners-1;
bool oddNodes = false;

for (i=0; i<GeoCorners; i++) {
if ((geoy[i]< y && geoy[j]>=y
|| geoy[j]< y && geoy[i]>=y)
&& (geox[i]<=x || geox[j]<=x)) {
oddNodes^=(geox[i]+(y-geoy[i])/(geoy[j]-geoy[i])*(geox[j]-geox[i])<x); }
j=i; }

if ((geoy.at(i)< y && geoy.at(j)>=y
|| geoy.at(j)< y && geoy.at(i)>=y)
&& (geox.at(i)<=x || geox.at(j)<=x) && (geox.at(i)+(y-geoy.at(i))/(geoy.at(j)-geoy.at(i))*(geox.at(j)-geox.at(i))<x)) {
oddNodes = !oddNodes;
}
j=i;
}

return oddNodes;
}

void PreemptionPluginWorker::VehicleLocatorWorker(BsmMessage* msg){

double micro = 10000000.0;

PreemptionObject* po = new PreemptionObject;

VehicleCoordinate* vehicle_coordinate = new VehicleCoordinate;
auto po = std::make_shared<PreemptionObject>();
auto vehicle_coordinate = std::make_shared<VehicleCoordinate>();

auto bsm = msg->get_j2735_data();
int32_t bsmTmpID;
Expand All @@ -84,16 +81,14 @@ namespace PreemptionPlugin {

for (auto const& it: GeofenceSet) {

double geox[it->geox.size()];
int k = 0;
std::vector<double> geox;
for (double const &i: it->geox) {
geox[k++] = i;
geox.push_back(i);
}

double geoy[it->geoy.size()];
k = 0;
std::vector<double> geoy;
for (double const &i: it->geoy) {
geoy[k++] = i;
geoy.push_back(i);
}

bool in_geo = CarInGeofence(vehicle_coordinate->lon, vehicle_coordinate->lat, geoy, geox, it->geox.size());
Expand All @@ -119,7 +114,7 @@ namespace PreemptionPlugin {

};

void PreemptionPluginWorker::PreemptionPlaner(PreemptionObject* po){
void PreemptionPluginWorker::PreemptionPlaner(std::shared_ptr<PreemptionObject> po){

if(po->approach == "1") {

Expand All @@ -146,7 +141,7 @@ namespace PreemptionPlugin {
std::cout << " Finished PreemptionPlaner" << std::endl;
};

void PreemptionPluginWorker::TurnOnPreemption(PreemptionObject* po){
void PreemptionPluginWorker::TurnOnPreemption(std::shared_ptr<PreemptionObject> po){
std::string preemption_plan_flag = "1";

std::asctime(std::localtime(&(po->time)));
Expand All @@ -163,7 +158,7 @@ namespace PreemptionPlugin {
}
}

void PreemptionPluginWorker::TurnOffPreemption(PreemptionObject* po){
void PreemptionPluginWorker::TurnOffPreemption(std::shared_ptr<PreemptionObject> po){
std::string preemption_plan, preemption_plan_flag = "";
preemption_plan = preemption_map[po ->vehicle_id].preemption_plan;
preemption_plan_flag = "0";
Expand Down
16 changes: 8 additions & 8 deletions src/v2i-hub/PreemptionPlugin/src/include/PreemptionPluginWorker.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -40,9 +40,9 @@ namespace PreemptionPlugin {

public:
struct PreemptionObject {
std::string approach; // 0: egress 1: ingress
std::string preemption_plan;
int vehicle_id;
std::string approach = ""; // 0: egress 1: ingress
std::string preemption_plan = "";
int vehicle_id = 0;
std::time_t time = std::time(nullptr);
};

Expand Down Expand Up @@ -72,12 +72,12 @@ namespace PreemptionPlugin {

std::map <int,PreemptionObject> preemption_map;

void ProcessMapMessageFile(std::string path);
void ProcessMapMessageFile(const std::string &path);
void VehicleLocatorWorker(BsmMessage* msg);
void PreemptionPlaner(PreemptionObject* po);
void TurnOnPreemption(PreemptionObject* po);
void TurnOffPreemption(PreemptionObject* po);
bool CarInGeofence(double x, double y, double geox[], double geoy[], int GeoCorners);
void PreemptionPlaner(std::shared_ptr<PreemptionObject> po);
void TurnOnPreemption(std::shared_ptr<PreemptionObject> po);
void TurnOffPreemption(std::shared_ptr<PreemptionObject> po);
bool CarInGeofence(long double x, long double y, std::vector<double> geox, std::vector<double> geoy, long GeoCorners) const;

std::string ip_with_port;
int snmp_version = SNMP_VERSION_1;
Expand Down
2 changes: 1 addition & 1 deletion src/v2i-hub/PreemptionPlugin/test/PreemptionTest.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -45,7 +45,7 @@ class PreemptionTest : public testing::Test

bool CarInGeofence(double x, double y, double geox[], double geoy[], int GeoCorners) {
int i, j=GeoCorners-1 ;
bool oddNodes ;
bool oddNodes = 0 ;

for (i=0; i<GeoCorners; i++) {
if ((geoy[i]< y && geoy[j]>=y
Expand Down

0 comments on commit 8121e18

Please sign in to comment.