Commit 396a2dc9 authored by Erik Andresen's avatar Erik Andresen

approaches for waypoint learning

parent 8834dd3e
This diff is collapsed.
......@@ -6,7 +6,7 @@
<seed>12542</seed>
<max_sim_time>900</max_sim_time>
<!-- geometry file -->
<geometry>landmarktest4.xml</geometry>
<geometry>tgftestcognitivemap.xml</geometry>
<!-- trajectories file and format -->
<trajectories format="xml-plain" fps="8">
<!--file location="trajectories.xml" /-->
......@@ -59,36 +59,45 @@
</routing>
<!--persons information and distribution -->
<agents operational_model_id="1">
<agents operational_model_id="3">
<agents_distribution>
<group group_id="1" agent_parameter_id="1" room_id="0" subroom_id="0" number="1" router_id="7" />
<group group_id="1" agent_parameter_id="1" room_id="0" subroom_id="0" number="20" router_id="1" />
<group group_id="2" agent_parameter_id="1" room_id="0" subroom_id="1" number="20" router_id="1" />
</agents_distribution>
</agents>
<!-- These parameters may be overwritten -->
<operational_models>
<model operational_model_id="1" description="gcfm">
<operational_models>
<model operational_model_id="3" description="Tordeux2015">
<model_parameters>
<solver>euler</solver>
<periodic>1</periodic>
<stepsize>0.01</stepsize>
<exit_crossing_strategy>4</exit_crossing_strategy>
<linkedcells enabled="true" cell_size="2.2" />
<force_ped nu="0.3" dist_max="3" disteff_max="2" interpolation_width="0.1" />
<force_wall nu="0.2" dist_max="3" disteff_max="2" interpolation_width="0.1" />
<exit_crossing_strategy>3</exit_crossing_strategy>
<linkedcells enabled="true" cell_size="30"/>
<force_ped a="5" D="0.1"/>
<force_wall a="5" D="0.02"/>
</model_parameters>
<agent_parameters agent_parameter_id="1">
<v0 mu="1.24" sigma="0.001" />
<bmax mu="0.25" sigma="0.001" />
<bmin mu="0.20" sigma="0.001" />
<amin mu="0.18" sigma="0.001" />
<tau mu="0.5" sigma="0.001" />
<atau mu="0.5" sigma="0.001" />
<v0 mu="1.34" sigma="0.0" />
<bmax mu="0.15" sigma="0.0" /> <!-- this is l/2, assuming peds are circles with constant radius-->
<bmin mu="0.15" sigma="0.0" />
<amin mu="0.15" sigma="0.0" />
<tau mu="0.5" sigma="0.0" />
<atau mu="0." sigma="0.0" />
<T mu="1" sigma="0.0" />
</agent_parameters>
<agent_parameters agent_parameter_id="2">
<v0 mu="0.01" sigma="0.0" />
<bmax mu="0.15" sigma="0.0" /> <!-- this is l/2, assuming peds are circles with constant radius-->
<bmin mu="0.15" sigma="0.0" />
<amin mu="0.15" sigma="0.0" />
<tau mu="0.5" sigma="0.0" />
<atau mu="0." sigma="0.0" />
<T mu="1" sigma="0.0" />
</agent_parameters>
</model>
</operational_models>
<route_choice_models>
......@@ -100,7 +109,7 @@
</router>
<router router_id="7" description="cognitive_map">
<sensors>
<sensor sensor_id="1" description="Room2Corridor"/>
<!--sensor sensor_id="1" description="Room2Corridor"/-->
<!--sensor sensor_id="1" description="Smoke" p_field_path="D:\JuPedSim\jpscore\inputfiles\cognitive_map\pFields\" update_time="30" final_time="90"/-->
</sensors>
<cognitive_map status="empty"/>
......
......@@ -28,8 +28,11 @@ CognitiveMap::CognitiveMap(ptrBuilding b, ptrPed ped)
_outputhandler->WriteToFileHeader(ped->GetID(),fps);
_frame=0;
_YAHPointer.SetPed(ped);
_YAHPointer.SetPos(_ped->GetPos());
_createdWayP=-1;
}
CognitiveMap::~CognitiveMap()
......@@ -40,18 +43,27 @@ CognitiveMap::~CognitiveMap()
void CognitiveMap::UpdateMap()
{
AddWaypoints(TriggerAssociations(LookForLandmarks()));
if (_waypContainerSorted.empty())
if (_waypContainerTargetsSorted.empty())
return;
if (_waypContainerSorted.top()->WaypointReached(_YAHPointer.GetPos()))
if (_waypContainerTargetsSorted.top()->WaypointReached(_YAHPointer.GetPos()))
{
//ptrWaypoint cWaypoint = _waypContainerSorted.top();
//ptrWaypoint cWaypoint = _waypContainerTargetsSorted.top();
WaypointReached(_waypContainerTargetsSorted.top());
//cWaypoint->SetPriority(_waypContainerTargetsSorted.size());
_waypContainerTargetsSorted.pop();
SubRoom * sub_room = _building->GetRoom(_ped->GetRoomID())->GetSubRoom(_ped->GetSubRoomID());
GraphVertex * vertex = (*_network->GetNavigationGraph())[sub_room];
const GraphVertex::EdgesContainer edges = *(vertex->GetAllEdges());
//cWaypoint->SetPriority(_waypContainerSorted.size());
_waypContainerSorted.pop();
for (GraphEdge* edge:edges)
{
edge->SetFactor(1,"SpatialKnowledge");
}
//_waypContainerSorted.push(cWaypoint);
//_waypContainerTargetsSorted.push(cWaypoint);
//_waypContainer.pop();
//Log->Write("Prio:\t"+std::to_string(_waypContainerSorted.top()->GetPriority()));
//Log->Write("Prio:\t"+std::to_string(_waypContainerTargetsSorted.top()->GetPriority()));
}
}
......@@ -99,6 +111,8 @@ void CognitiveMap::AddLandmarks(std::vector<ptrLandmark> landmarks)
else
{
_landmarks.push_back(landmark);
_landmarks.back()->SetPos(_landmarks.back()->GetPos()-_YAHPointer.GetPosDiff());
}
}
......@@ -115,6 +129,7 @@ std::vector<ptrLandmark> CognitiveMap::LookForLandmarks()
if (landmark->GetRoom()==sub_room && std::find(_landmarks.begin(), _landmarks.end(), landmark)==_landmarks.end())
{
landmarks_found.push_back(landmark);
WaypointReached(landmark);
}
}
......@@ -132,10 +147,14 @@ Waypoints CognitiveMap::TriggerAssociations(const std::vector<ptrLandmark> &land
for (ptrAssociation association:associations)
{
if (association->GetWaypointAssociation(landmark)!=nullptr)
{
waypoints.push_back(association->GetWaypointAssociation(landmark));
//pos-yahpointer diff
waypoints.back()->SetPos(waypoints.back()->GetPos()-_YAHPointer.GetPosDiff());
}
if (association->GetConnectionAssoziation()!=nullptr)
{
AddConnection(association->GetConnectionAssoziation());
//AddConnection(association->GetConnectionAssoziation());
}
}
}
......@@ -157,9 +176,18 @@ void CognitiveMap::AddWaypoints(Waypoints waypoints)
}
waypoint->SetPriority(n);
_waypContainer.push_back(waypoint);
_waypContainerSorted.push(waypoint);
//Add as new target
if (!waypoint->Visited())
_waypContainerTargetsSorted.push(waypoint);
//Add as already visited
else
{
WaypointReached(waypoint);
}
n++;
}
}
......@@ -172,16 +200,16 @@ void CognitiveMap::AssessDoors()
//const Point rpWaypoint=_waypContainer[0]->GetPos();
if (!_waypContainer.empty())
if (!_waypContainerTargetsSorted.empty())
{
std::vector<GraphEdge* > sortedEdges = SortConShortestPath(_waypContainerSorted.top(),edges);
std::vector<GraphEdge* > sortedEdges = SortConShortestPath(_waypContainerTargetsSorted.top(),edges);
//Log->Write(std::to_string(nextDoor->GetCrossing()->GetID()));
for (unsigned int i=0; i<sortedEdges.size(); ++i)
{
sortedEdges[i]->SetFactor(0.5+0.1*i,"SpatialKnowledge");
Log->Write("INFO:\t "+std::to_string(sortedEdges[i]->GetCrossing()->GetID()));
Log->Write("INFO:\t "+std::to_string(sortedEdges[i]->GetFactor()));
sortedEdges[i]->SetFactor(0.6+0.1*i,"SpatialKnowledge");
// Log->Write("INFO:\t "+std::to_string(sortedEdges[i]->GetCrossing()->GetID()));
// Log->Write("INFO:\t "+std::to_string(sortedEdges[i]->GetFactor()));
}
//Log->Write(std::to_string(nextDoor->GetCrossing()->GetID()));
......@@ -338,9 +366,9 @@ void CognitiveMap::WriteToFile()
for (ptrWaypoint waypoint:_waypContainer)
{
current=false;
if (!_waypContainerSorted.empty())
if (!_waypContainerTargetsSorted.empty())
{
if (waypoint==_waypContainerSorted.top())
if (waypoint==_waypContainerTargetsSorted.top())
current=true;
}
......@@ -368,22 +396,47 @@ void CognitiveMap::WriteToFile()
data.append(tmp3);
for (ptrConnection connection:_connections)
{
char tmp4[CLENGTH]="";
sprintf(tmp4, "<connection "
"Landmark_WaypointID1=\"%d\"\tLandmark_WaypointID2=\"%d\"/>\n",
connection->GetWaypoints().first->GetId(),
connection->GetWaypoints().second->GetId());
// for (ptrConnection connection:_connections)
// {
// char tmp4[CLENGTH]="";
// sprintf(tmp4, "<connection "
// "Landmark_WaypointID1=\"%d\"\tLandmark_WaypointID2=\"%d\"/>\n",
// connection->GetWaypoints().first->GetId(),
// connection->GetWaypoints().second->GetId());
data.append(tmp4);
}
// data.append(tmp4);
// }
data.append("</frame>\n");
_outputhandler->WriteToFile(data);
}
void CognitiveMap::SetNewWaypoint()
{
double a= 2.0;
double b=2.0;
ptrWaypoint wayP = std::make_shared<Waypoint>(_YAHPointer.GetPos(),a,b,_createdWayP);
_createdWayP--;
wayP->SetVisited(true);
std::vector<ptrWaypoint> vec;
vec.push_back(wayP);
AddWaypoints(vec);
}
void CognitiveMap::WaypointReached(ptrWaypoint waypoint)
{
//build connection
if (!_waypContainerRecentlyVisited.empty())
AddConnection(_waypContainerRecentlyVisited.back(),waypoint);
_waypContainerRecentlyVisited.push(waypoint);
if (_waypContainerRecentlyVisited.size()>2)
_waypContainerRecentlyVisited.pop();
}
std::vector<ptrConnection> CognitiveMap::GetAllConnections() const
{
std::vector<ptrConnection> con{ std::begin(_connections), std::end(_connections) };
......
......@@ -78,6 +78,11 @@ public:
void WriteToFile();
//evaluate waypoints
// Set new waypoints
void SetNewWaypoint();
void WaypointReached(ptrWaypoint waypoint);
//Connections
std::vector<ptrConnection> GetAllConnections() const;
void AddConnection(const ptrConnection &connection);
......@@ -92,13 +97,15 @@ private:
Associations _assoContainer;
std::vector<ptrLandmark> _landmarksSubConcious;
std::vector<ptrLandmark> _landmarks;
SortedWaypoints _waypContainerSorted;
SortedWaypoints _waypContainerTargetsSorted;
std::queue<ptrWaypoint> _waypContainerRecentlyVisited;
Waypoints _waypContainer;
YouAreHerePointer _YAHPointer;
ptrOutputHandler _outputhandler;
Connections _connections;
int _frame;
int _createdWayP;
};
......
......@@ -2,7 +2,7 @@
#include "associations.h"
Landmark::Landmark(Point pos, ptrRoom room):Waypoint(pos,2.0,2.0,room)
Landmark::Landmark(Point pos, ptrRoom room):Waypoint(pos,2.0,2.0,-2,room)
{
......
......@@ -9,7 +9,7 @@ Waypoint::Waypoint(Point pos, ptrRoom room)
_visited=false;
}
Waypoint::Waypoint(Point pos, double a, double b, ptrRoom room)
Waypoint::Waypoint(Point pos, double a, double b, int id, ptrRoom room)
{
_exactPos=pos;
_a=a;
......@@ -17,6 +17,7 @@ Waypoint::Waypoint(Point pos, double a, double b, ptrRoom room)
_room=room;
_priority=1.0;
_visited=false;
_id=id;
}
Waypoint::~Waypoint()
......
......@@ -16,7 +16,7 @@ class Waypoint
using Waypoints = std::vector<ptrWaypoint>;
public:
Waypoint(Point pos, ptrRoom room=nullptr);
Waypoint(Point pos, double a, double b, ptrRoom room=nullptr);
Waypoint(Point pos, double a, double b, int id=-1, ptrRoom room=nullptr);
~Waypoint();
//Setter
......
#include "youareherepointer.h"
#include "../../../pedestrian/Pedestrian.h"
#include <cmath>
#include <iostream>
......@@ -11,6 +12,7 @@ YouAreHerePointer::YouAreHerePointer()
_angle=0.0;
_pos=Point(0.0,0.0);
_oldpos=Point(0.0,0.0);
}
YouAreHerePointer::~YouAreHerePointer()
......@@ -28,6 +30,11 @@ const double &YouAreHerePointer::GetDirection() const
return _angle;
}
const ptrPed YouAreHerePointer::GetPed() const
{
return _ped;
}
void YouAreHerePointer::SetPos(const Point &point)
{
_pos=point;
......@@ -45,6 +52,11 @@ void YouAreHerePointer::SetDirection(const double &angle)
//std::cout << std::to_string(angle*180/3.14) << std::endl;
}
void YouAreHerePointer::SetPed(const ptrPed ped)
{
_ped=ped;
}
void YouAreHerePointer::UpdateYAH(const Point &move)
{
_oldpos=_pos;
......@@ -61,3 +73,8 @@ void YouAreHerePointer::UpdateYAH(const Point &move)
}
const Point YouAreHerePointer::GetPosDiff()
{
return _ped->GetPos()-_pos;
}
......@@ -2,6 +2,8 @@
#define YOUAREHEREPOINTER_H
#include "../../../geometry/Point.h"
class Pedestrian;
using ptrPed = const Pedestrian*;
// Ellipse representing the approximative position of a pedestrian in his cognitive map
......@@ -14,22 +16,29 @@ public:
//Getter
const Point& GetPos() const;
const double& GetDirection() const;
const ptrPed GetPed() const;
//Setter
void SetPos(const Point& point);
void SetDirection(const double& angle);
void SetPed(const ptrPed ped);
//Update
void UpdateYAH(const Point& move);
//Difference pos real-cogmap
const Point GetPosDiff();
private:
Point _pos;
Point _oldpos;
ptrPed _ped;
//Direction
double _angle;
//Region
};
......
......@@ -252,6 +252,11 @@ const GraphEdge * GraphVertex::GetLocalCheapestDestination(const Point & positio
if(act_edge == NULL || act_edge->GetWeight(position) > edges.top().second->GetWeight(position)) {
act_edge = edges.top().second;
}
else if(act_edge->GetWeight(position) == edges.top().second->GetWeight(position))
{
if(act_edge->GetApproximateDistance(position) > edges.top().second->GetApproximateDistance(position))
act_edge = edges.top().second;
}
edges.pop();
}
......
......@@ -64,7 +64,7 @@ void LastDestinationsSensor::execute(const Pedestrian * pedestrian, CognitiveMap
if((*it)->GetSrc() != NULL && (*it)->GetDest() != NULL && (*it)->GetCrossing() != NULL) {
GraphEdge * back_edge = (*ng[(*it)->GetDest()->GetSubRoom()])[(*it)->GetCrossing()];
if(back_edge != NULL && rated.find(back_edge) == rated.end()) {
back_edge->SetFactor(3000.0 /(i*3) + 100, GetName());
back_edge->SetFactor(2,GetName());//3000.0 /(i*3) + 100, GetName());
rated.insert(back_edge);
}
}
......
......@@ -70,7 +70,7 @@ void RoomToFloorSensor::execute(const Pedestrian * pedestrian, CognitiveMap * co
if((*it_edges)->GetDest() == NULL || (*it_edges)->GetDest()->GetSubRoom()->GetType() == (*it_edges)->GetSrc()->GetSubRoom()->GetType()) {
(*it_edges)->SetFactor(1.0, GetName());
} else {
if((*it_edges)->GetDest()->GetSubRoom()->GetType() == "corridor")
if((*it_edges)->GetDest()->GetSubRoom()->GetType() == "Corridor")
(*it_edges)->SetFactor(.3 , GetName());
else
(*it_edges)->SetFactor(5.0 , GetName());
......
......@@ -25,6 +25,11 @@ void Locater::execute(const Pedestrian * ped, CognitiveMap * cogMap) const
//std::cout << std::to_string(ped->GetGlobalTime()) << std::endl;
cogMap->UpdateYAHPointer(ped->GetV()*_updateInterval);
// if (std::fmod(ped->GetGlobalTime(),5.0)==0.0)
// {
// cogMap->SetNewWaypoint();
// }
cogMap->WriteToFile();
}
......
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