Commit 5a16da31 authored by Erik Andresen's avatar Erik Andresen

Added script for plotting isovists and some in code documentation for AIRouter

parent 782cfa89
......@@ -66,16 +66,12 @@ AIRouter::~AIRouter()
int AIRouter::FindExit(Pedestrian * p)
{
// Update isovist and return next target every given update step
if (std::fmod(std::roundf(p->GetGlobalTime()*100),std::roundf(1.0/AI_UPDATE_RATE*100))==0.0)// && p->GetGlobalTime()>0)
{
//Log->Write(std::to_string(p->GetGlobalTime()));
//sensor_manager->execute(p, SensorManager::PERIODIC);
// Find next destination
int status = FindDestination(p);
//(*cm_storage)[p]->UpdateSubRoom();
return status;
}
......@@ -84,30 +80,32 @@ int AIRouter::FindExit(Pedestrian * p)
int AIRouter::FindDestination(Pedestrian * p)
{
// Discover doors
//sensor_manager->execute(p, SensorManager::NO_WAY);
//check if there is a way to the outside the pedestrian knows (in the cognitive map)
//--------------------COGMAP----------------------------
//Update isovists
// Update (red) currently visible polygon
(*brain_storage)[p]->GetPerceptionAbilities().UpdateCurrentEnvironment();
// Update (blue) already explored polygon
(*brain_storage)[p]->GetPerceptionAbilities().UpdateSeenEnv();
// update cogmap
(*brain_storage)[p]->GetCognitiveMap().UpdateMap();
//Find next appropriate landmark
(*brain_storage)[p]->GetCognitiveMap().FindNextTarget();
// find possible crossings in seenEnvironment e.g. to proceed to landmark
std::vector<NavLine> navLines = (*brain_storage)[p]->GetPerceptionAbilities().GetPossibleNavLines(
(*brain_storage)[p]->GetPerceptionAbilities().GetSeenEnvironmentAsPointVec(building->GetRoom(p->GetRoomID())));
NavLine nextNvLine=(*brain_storage)[p]->FindApprCrossing(navLines);
// std::ofstream myfile2;
// std::string str2 = "./isovists/navLinesBefore_pos"+std::to_string(p->GetID())+"_"+std::to_string((int)(std::round(p->GetGlobalTime()*100)))+".txt";
// myfile2.open (str2);
// myfile2 << std::to_string(nextNvLine.GetCentre()._x) << " " << std::to_string(nextNvLine.GetCentre()._y) << std::endl;
// myfile2.close();
std::ofstream myfile2;
std::string str2 = "./isovists/navLinesBefore_pos"+std::to_string(p->GetID())+"_"+std::to_string((int)(std::round(p->GetGlobalTime()*100)))+".txt";
myfile2.open (str2);
myfile2 << std::to_string(nextNvLine.GetCentre()._x) << " " << std::to_string(nextNvLine.GetCentre()._y) << std::endl;
myfile2.close();
// find possible crossings in currentEnv to proceed to nextNvLine
......@@ -118,11 +116,11 @@ int AIRouter::FindDestination(Pedestrian * p)
// std::ofstream myfile;
// std::string str = "./isovists/navLines_pos"+std::to_string(p->GetID())+"_"+std::to_string((int)(std::round(p->GetGlobalTime()*100)))+".txt";
// myfile.open (str);
// myfile << std::to_string(nextNvLine.GetCentre()._x) << " " << std::to_string(nextNvLine.GetCentre()._y) << std::endl;
// myfile.close();
std::ofstream myfile;
std::string str = "./isovists/navLines_pos"+std::to_string(p->GetID())+"_"+std::to_string((int)(std::round(p->GetGlobalTime()*100)))+".txt";
myfile.open (str);
myfile << std::to_string(nextNvLine.GetCentre()._x) << " " << std::to_string(nextNvLine.GetCentre()._y) << std::endl;
myfile.close();
p->SetExitLine(&nextNvLine);
......
......@@ -25,22 +25,24 @@ public:
const NavLine* GetNextNavLine(const NavLine *nextTarget);
bool HlineReached() const;
// Sort crossings by considering general knowledge
std::vector<const NavLine*> SortConGeneralKnow(const std::vector<const NavLine *> &navLines);
//select appropriate crossings
//select appropriate crossing to unknown area
const NavLine FindApprCrossing(const std::vector<NavLine> &navLines);
// Select appropriate visible crossing
const NavLine FindApprVisibleCrossing(const NavLine& navLine, const std::vector<NavLine> &navLines);
// Get Operational target (correct visible crossing)
const NavLine TargetToHead(const NavLine *visibleCrossing) const;
//implement sign instruction
// Sort crossings by considering percepted signs
std::vector<const NavLine *> SortConSignage(const std::vector<const NavLine *>& navLines);
const Sign* DetermineDecisiveSign(const std::vector<const Sign *> &signs);
double OrthogonalDistanceFromTo(const Sign& sign1, const Sign& sign2) const;
double OrthogonalDistanceFromTo(const NavLine* navLine, const Sign &sign) const;
double OrthogonalDistanceFromTo(const NavLine* navLine, const Point& pos, const double& angle) const;
private:
const Building* _b;
const Pedestrian* _ped;
......
......@@ -75,24 +75,24 @@ const std::unordered_map<ptrFloor, BoostPolygon> &VisualSystem::UpdateSeenEnv()
_seenEnvironment.at(_building->GetRoom(_ped->GetRoomID())) = _currentEnv;
}
// std::ofstream myfile2;
// std::string str2 = "./isovists/seen_env_"+std::to_string(_ped->GetID())+"_"+std::to_string((int)(std::round(_ped->GetGlobalTime()*100)))+".txt";
// myfile2.open (str2);
// for(auto it1 = boost::begin(boost::geometry::exterior_ring(_seenEnvironment.at(_building->GetRoom(_ped->GetRoomID()))));
// it1 != boost::end(boost::geometry::exterior_ring(_seenEnvironment.at(_building->GetRoom(_ped->GetRoomID()))));
// ++it1)
// {
// myfile2 << it1->_x << " " << it1->_y << std::endl;
// //seenEnvPolygon.push_back(*it1);
std::ofstream myfile2;
std::string str2 = "./isovists/seen_env_"+std::to_string(_ped->GetID())+"_"+std::to_string((int)(std::round(_ped->GetGlobalTime()*100)))+".txt";
myfile2.open (str2);
for(auto it1 = boost::begin(boost::geometry::exterior_ring(_seenEnvironment.at(_building->GetRoom(_ped->GetRoomID()))));
it1 != boost::end(boost::geometry::exterior_ring(_seenEnvironment.at(_building->GetRoom(_ped->GetRoomID()))));
++it1)
{
myfile2 << it1->_x << " " << it1->_y << std::endl;
//seenEnvPolygon.push_back(*it1);
// }
// myfile2.close();
}
myfile2.close();
// std::ofstream myfile3;
// std::string str3 = "./isovists/real_pos"+std::to_string(_ped->GetID())+"_"+std::to_string((int)(std::round(_ped->GetGlobalTime()*100)))+".txt";
// myfile3.open (str3);
// myfile3 << std::to_string(_ped->GetPos()._x) << " " << std::to_string(_ped->GetPos()._y) << std::endl;
// myfile3.close();
std::ofstream myfile3;
std::string str3 = "./isovists/real_pos"+std::to_string(_ped->GetID())+"_"+std::to_string((int)(std::round(_ped->GetGlobalTime()*100)))+".txt";
myfile3.open (str3);
myfile3 << std::to_string(_ped->GetPos()._x) << " " << std::to_string(_ped->GetPos()._y) << std::endl;
myfile3.close();
return _seenEnvironment;
......@@ -167,18 +167,18 @@ const BoostPolygon &VisualSystem::UpdateCurrentEnvironment()
// }
// std::ofstream myfile;
// std::string str = "./isovists/current_env_"+std::to_string(_ped->GetID())+"_"+std::to_string((int)(std::round(_ped->GetGlobalTime()*100)))+".txt";
// myfile.open (str);
// for(auto it1 = boost::begin(boost::geometry::exterior_ring(currentEnv));
// it1 != boost::end(boost::geometry::exterior_ring(currentEnv));
// ++it1)
// {
// myfile << it1->_x << " " << it1->_y << std::endl;
// //seenEnvPolygon.push_back(*it1);
std::ofstream myfile;
std::string str = "./isovists/current_env_"+std::to_string(_ped->GetID())+"_"+std::to_string((int)(std::round(_ped->GetGlobalTime()*100)))+".txt";
myfile.open (str);
for(auto it1 = boost::begin(boost::geometry::exterior_ring(currentEnv));
it1 != boost::end(boost::geometry::exterior_ring(currentEnv));
++it1)
{
myfile << it1->_x << " " << it1->_y << std::endl;
//seenEnvPolygon.push_back(*it1);
// }
// myfile.close();
}
myfile.close();
boost::geometry::correct(currentEnv);
......
# -*- coding: utf-8 -*-
"""
Created on Fri Jul 29 09:40:56 2016
@author: Erik
"""
import matplotlib.pyplot as plt
#x=[2081.023,2080.851,2082.237,2082.408,2081.023]
#y=[-14346.094,-14345.927,-14344.506,-14344.673,-14346.094]
#
#
#plt.plot(x,y,'o--')
#
#x2=[2080.975,2080.975,2090.675,2090.675,2080.975]
#y2=[-14346.220,-14345.98,-14345.98,-14346.220,-14346.220]
#
#plt.plot(x2,y2,'ro--')
###############################Geometrie########################
import numpy as np
data=np.loadtxt("./isovists/walls.txt")
#data_landmarks=np.loadtxt("./isovists/landmarks.txt")
#print (data)
ped=1
import matplotlib.pylab as plt
from matplotlib.patches import Ellipse
n=0
j=1
while n<=6000:
plt.figure()
ax=plt.gca()
plt.axis('off')
lowerx=min([min(data[:,0]),min(data[:,2])])
upperx=max([min(data[:,0]),max(data[:,2])])
lowery=min([min(data[:,1]),min(data[:,3])])
uppery=max([min(data[:,1]),max(data[:,3])])
plt.xlim([lowerx-1,upperx+5])
plt.ylim([lowery-5,uppery+1])
for i in range(len(data)):
ax.plot([data[i,0],data[i,2]],[data[i,1],data[i,3]],"k",lw=1)
# string='./isovists/landmarks_%d_%d.txt'%(ped,n)
# data_landmarks=np.loadtxt(string)
#
#
#
# #ellipse = Ellipse(xy=(157.18, 68.4705), width=0.036, height=0.012,
# # edgecolor='r', fc='None', lw=2)
# #ax.add_patch(ellipse)
# #print(np.shape(data_landmarks))
# if (np.shape(data_landmarks)==(7,)):
# ax.plot(data_landmarks[0],data_landmarks[1],'ys')
#
# ellipse = Ellipse(xy=(data_landmarks[2], data_landmarks[3]), width=data_landmarks[4],
# height=data_landmarks[5], edgecolor='r', fc='None', lw=2)
# ax.add_patch(ellipse)
#
# if (data_landmarks[6]==0):
# stringl="Landmark"
# elif (data_landmarks[6]==1):
# stringl="Next Target"
# else:
# stringl="Visited Landmark"
#
# ax.text(data_landmarks[2]-1.25, data_landmarks[3]-0.2,stringl,color="k",fontsize=9)
#
# else:
# ax.plot(data_landmarks[:,0],data_landmarks[:,1],'ys')
##############################Visibility Graph#########################
string='./isovists/current_env_%d_%d.txt'%(ped,n)
data2=np.loadtxt(string)
x=np.zeros(len(data2)+1)
y=np.zeros(len(data2)+1)
x[:-1]=data2[:,0]
y[:-1]=data2[:,1]
x[-1]=data2[0,0]
y[-1]=data2[0,1]
ax.fill(x,y,'r',alpha=0.8)
string='./isovists/seen_env_%d_%d.txt'%(ped,n)
data2=np.loadtxt(string)
#plt.fill(x,y)#,'r',lw=2)
x=np.zeros(len(data2)+1)
y=np.zeros(len(data2)+1)
x[:-1]=data2[:,0]
y[:-1]=data2[:,1]
x[-1]=data2[0,0]
y[-1]=data2[0,1]
ax.fill(x,y,'b',alpha=0.4)
string='./isovists/real_pos%d_%d.txt'%(ped,n)
data3=np.loadtxt(string)
ax.plot(data3[0],data3[1],'ro')
string='./isovists/navLines_pos%d_%d.txt'%(ped,n)
data4=np.loadtxt(string)
ax.plot(data4[0],data4[1],'m*')
string='./isovists/navLinesBefore_pos%d_%d.txt'%(ped,n)
data5=np.loadtxt(string)
ax.plot(data5[0],data5[1],'b*')
string='./isovists/pictures/img%d.png'%j
plt.savefig(string)
plt.close()
j+=1
n+=20
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment