Commit 1843f038 authored by Erik Andresen's avatar Erik Andresen

waypoint and connection stuff

parent a0f8bc8e
This source diff could not be displayed because it is too large. You can view the blob instead.
......@@ -2,7 +2,7 @@
<geometry version="0.5" caption="corner" xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance" xsi:noNamespaceSchemaLocation="http://xsd.jupedsim.org/jps_geometry.xsd" unit="m">
<rooms>
<room id="0" caption="hall">
<subroom id="0" caption="Startoffice" class="corridor">
<subroom id="0" caption="Startoffice" class="Office">
<polygon caption="wall">
<vertex px="2.62" py="3.34"/>
<vertex px="-0.38" py="3.34"/>
......@@ -20,7 +20,7 @@
<vertex px="-0.38" py="1.34"/>
</polygon>
</subroom>
<subroom id="1" caption="Office2" class="subroom">
<subroom id="1" caption="Office2" class="Office">
<polygon caption="wall">
<vertex px="2.62" py="1.34"/>
<vertex px="2.62" py="-0.66"/>
......@@ -38,7 +38,7 @@
<vertex px="-0.38" py="-0.66"/>
</polygon>
</subroom>
<subroom id="2" caption="Office3" class="subroom">
<subroom id="2" caption="Office3" class="Office">
<polygon caption="wall">
<vertex px="-0.38" py="-3.66"/>
<vertex px="2.62" py="-3.66"/>
......@@ -56,7 +56,7 @@
<vertex px="-0.38" py="-3.66"/>
</polygon>
</subroom>
<subroom id="3" caption="Hallway1" class="corridor">
<subroom id="3" caption="Hallway1" class="Corridor">
<polygon caption="wall">
<vertex px="5.62" py="-3.66"/>
<vertex px="2.62" py="-3.66"/>
......@@ -82,7 +82,7 @@
<vertex px="5.62" py="-2.66"/>
</polygon>
</subroom>
<subroom id="4" caption="Hallway2" class="corridor">
<subroom id="4" caption="Hallway2" class="Corridor">
<polygon caption="wall">
<vertex px="2.62" py="4.34"/>
<vertex px="2.62" py="6.34"/>
......@@ -104,7 +104,7 @@
<vertex px="5.62" py="14.34"/>
</polygon>
</subroom>
<subroom id="5" caption="Office4" class="subroom">
<subroom id="5" caption="Office4" class="Office">
<polygon caption="wall">
<vertex px="2.62" py="3.34"/>
<vertex px="-0.38" py="3.34"/>
......@@ -122,7 +122,7 @@
<vertex px="-0.38" py="3.34"/>
</polygon>
</subroom>
<subroom id="6" caption="Office5" class="subroom">
<subroom id="6" caption="Office5" class="Office">
<polygon caption="wall">
<vertex px="2.62" py="6.34"/>
<vertex px="-0.38" py="6.34"/>
......@@ -144,7 +144,7 @@
<vertex px="2.62" py="14.34"/>
</polygon>
</subroom>
<subroom id="7" caption="Hallway3" class="corridor">
<subroom id="7" caption="Hallway3" class="Corridor">
<polygon caption="wall">
<vertex px="15.62" py="11.34"/>
<vertex px="15.62" py="14.34"/>
......@@ -162,7 +162,7 @@
<vertex px="7.62" py="11.34"/>
</polygon>
</subroom>
<subroom id="8" caption="Hallway4" class="corridor">
<subroom id="8" caption="Hallway4" class="Corridor">
<polygon caption="wall">
<vertex px="5.62" py="6.34"/>
<vertex px="12.62" py="6.34"/>
......@@ -176,7 +176,7 @@
<vertex px="12.62" py="3.34"/>
</polygon>
</subroom>
<subroom id="9" caption="ExitHallway" class="corridor">
<subroom id="9" caption="ExitHallway" class="Corridor">
<polygon caption="wall">
<vertex px="15.62" py="6.34"/>
<vertex px="15.62" py="-3.66"/>
......@@ -198,7 +198,7 @@
<vertex px="12.62" py="-2.66"/>
</polygon>
</subroom>
<subroom id="10" caption="Office 6" class="subroom">
<subroom id="10" caption="Office 6" class="Office">
<polygon caption="wall">
<vertex px="5.62" py="6.34"/>
<vertex px="5.62" py="11.34"/>
......@@ -220,7 +220,7 @@
<vertex px="7.62" py="11.34"/>
</polygon>
</subroom>
<subroom id="11" caption="Entrance" class="corridor">
<subroom id="11" caption="Entrance" class="Corridor">
<polygon caption="wall">
<vertex px="12.62" py="-3.66"/>
<vertex px="12.62" py="-6.66"/>
......@@ -234,7 +234,7 @@
<vertex px="17.62" py="-3.66"/>
</polygon>
</subroom>
<subroom id="12" caption="Office 7" class="subroom">
<subroom id="12" caption="Office 7" class="Office">
<polygon caption="wall">
<vertex px="5.62" py="3.34"/>
<vertex px="9.12" py="3.34"/>
......@@ -252,7 +252,7 @@
<vertex px="9.12" py="-0.66"/>
</polygon>
</subroom>
<subroom id="13" caption="Office 8" class="subroom">
<subroom id="13" caption="Office 8" class="Office">
<polygon caption="wall">
<vertex px="9.12" py="-3.66"/>
<vertex px="5.62" py="-3.66"/>
......@@ -270,7 +270,7 @@
<vertex px="9.12" py="-0.66"/>
</polygon>
</subroom>
<subroom id="14" caption="Office 9" class="subroom">
<subroom id="14" caption="Office 9" class="Office">
<polygon caption="wall">
<vertex px="9.12" py="3.34"/>
<vertex px="12.62" py="3.34"/>
......@@ -288,7 +288,7 @@
<vertex px="12.62" py="0.34"/>
</polygon>
</subroom>
<subroom id="15" caption="Office 10" class="subroom">
<subroom id="15" caption="Office 10" class="Office">
<polygon caption="wall">
<vertex px="9.12" py="-3.66"/>
<vertex px="12.62" py="-3.66"/>
......@@ -381,19 +381,15 @@
</transition>
</transitions>
<landmarks>
<landmark id="0" caption="Landmark0" type="NaN" room1_id="0" subroom1_id="3" px="3.77667" py="-2.27">
<landmark id="0" caption="Statue" type="NaN" room1_id="0" subroom1_id="3" px="3.63667" py="-2.29333">
<associations>
<association id="1" caption="Waypoint" type="NaN" px="3.94333" py="4.62" a="3.61667" b="2.40333" connectedwith="0"/>
<association id="1" caption="Turn left and walk through corridor!" type="NaN" px="3.02167" py="4.795" a="5.65833" b="1.855" connectedwith="0"/>
<association id="2" caption="Use corridor to your right!" type="NaN" px="14.7076" py="4.38375" a="3.63592" b="2.75042"/>
</associations>
</landmark>
<landmark id="2" caption="Landmark1" type="NaN" room1_id="0" subroom1_id="4" px="2.65667" py="5.64">
<landmark id="3" caption="Strange Window" type="NaN" room1_id="0" subroom1_id="9" px="14.9067" py="8.88333">
<associations>
<association id="3" caption="Waypoint" type="NaN" px="14.3733" py="4.81833" a="3.52333" b="3.885" connectedwith="2"/>
</associations>
</landmark>
<landmark id="4" caption="Landmark2" type="NaN" room1_id="0" subroom1_id="8" px="13.8567" py="6.29333">
<associations>
<association id="5" caption="Waypoint" type="NaN" px="14.945" py="-4.50333" a="4.725" b="3.01" />
<association id="4" caption="Entrance is here!" type="NaN" px="16.8408" py="-6.16175" a="5.474" b="1.65025"/>
</associations>
</landmark>
</landmarks>
......
......@@ -152,7 +152,7 @@ void CognitiveMapStorage::ParseLandmarks()
std::string asso_y = xmltoa(xAsso->Attribute("py"),"-1");
std::string asso_a = xmltoa(xAsso->Attribute("a"),"-1");
std::string asso_b = xmltoa(xAsso->Attribute("b"),"-1");
//std::string connection = xmltoa(xAsso->Attribute("connectedwith"),"-1");
int connection = std::stoi(xmltoa(xAsso->Attribute("connectedwith"),"-1"));
//std::string priority = xmltoa(xAsso->Attribute("priority"),"-1");
ptrWaypoint waypoint (new Waypoint(Point(std::stod(asso_x),std::stod(asso_y)),
......@@ -162,8 +162,11 @@ void CognitiveMapStorage::ParseLandmarks()
waypoint->SetCaption(asso_caption);
//waypoint->AddConnection(std::stoi(connection));
//waypoint->SetPriority(std::stod(priority));
bool connected=false;
if (connection==landmark->GetId())
connected=true;
landmark->AddAssociation(std::make_shared<Association>(landmark,waypoint,connected));
landmark->AddAssociation(std::make_shared<Association>(landmark,waypoint,true));
}
......
......@@ -14,8 +14,10 @@ Association::Association(ptrWaypoint waypoint, ptrWaypoint associated_waypoint,
{
_waypoint=waypoint;
_associatedWaypoint=associated_waypoint;
if (connected)
_connection = std::make_shared<Connection>(_waypoint, _associatedWaypoint);
else
_connection=nullptr;
......
......@@ -39,7 +39,7 @@ CognitiveMap::~CognitiveMap()
void CognitiveMap::UpdateMap()
{
AddWaypoints(TriggerAssoziations(LookForLandmarks()));
AddWaypoints(TriggerAssociations(LookForLandmarks()));
if (_waypContainerSorted.empty())
return;
if (_waypContainerSorted.top()->WaypointReached(_YAHPointer.GetPos()))
......@@ -112,18 +112,18 @@ std::vector<ptrLandmark> CognitiveMap::LookForLandmarks()
for (ptrLandmark landmark:_landmarksSubConcious)
{
if (landmark->GetRoom()==sub_room)
if (landmark->GetRoom()==sub_room && std::find(_landmarks.begin(), _landmarks.end(), landmark)==_landmarks.end())
{
landmarks_found.push_back(landmark);
}
}
AddLandmarks(landmarks_found);
return landmarks_found;
}
Waypoints CognitiveMap::TriggerAssoziations(const std::vector<ptrLandmark> &landmarks)
Waypoints CognitiveMap::TriggerAssociations(const std::vector<ptrLandmark> &landmarks)
{
Waypoints waypoints;
for (ptrLandmark landmark:landmarks)
......@@ -133,8 +133,10 @@ Waypoints CognitiveMap::TriggerAssoziations(const std::vector<ptrLandmark> &land
{
if (association->GetWaypointAssociation(landmark)!=nullptr)
waypoints.push_back(association->GetWaypointAssociation(landmark));
else if (association->GetConnectionAssoziation()!=nullptr)
if (association->GetConnectionAssoziation()!=nullptr)
{
AddConnection(association->GetConnectionAssoziation());
}
}
}
return waypoints;
......@@ -178,8 +180,8 @@ void CognitiveMap::AssessDoors()
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()));
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()));
......@@ -322,10 +324,11 @@ void CognitiveMap::WriteToFile()
{
char tmp1[CLENGTH] = "";
sprintf(tmp1, "<landmark ID=\"%d\"\t"
"caption=\"%s\"\t"
"x=\"%.6f\"\ty=\"%.6f\"\t"
"z=\"%.6f\"\t"
"rA=\"%.2f\"\trB=\"%.2f\"/>\n",
landmark->GetId(), landmark->GetPos().GetX(),
landmark->GetId(), landmark->GetCaption().c_str(), landmark->GetPos().GetX(),
landmark->GetPos().GetY(),0.0 ,landmark->GetA(), landmark->GetB());
data.append(tmp1);
......@@ -341,21 +344,16 @@ void CognitiveMap::WriteToFile()
current=true;
}
std::string conString="";
// for (int connection:waypoint->GetConnections())
// {
// conString+=std::to_string(connection);
// conString+=",";
// }
char tmp2[CLENGTH] = "";
sprintf(tmp2, "<waypoint ID=\"%d\"\t"
"caption=\"%s\"\t"
"x=\"%.6f\"\ty=\"%.6f\"\t"
"z=\"%.6f\"\t"
"rA=\"%.2f\"\trB=\"%.2f\"\tcurrent=\"%i\"\tconnection=\"%s\"/>\n",
waypoint->GetId(), waypoint->GetPos().GetX(),
"rA=\"%.2f\"\trB=\"%.2f\"\tcurrent=\"%i\"/>\n",
waypoint->GetId(),waypoint->GetCaption().c_str(), waypoint->GetPos().GetX(),
waypoint->GetPos().GetY(),0.0 ,waypoint->GetA(), waypoint->GetB(),
current,conString.c_str());
current);
data.append(tmp2);
}
......@@ -370,6 +368,18 @@ 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());
data.append(tmp4);
}
data.append("</frame>\n");
_outputhandler->WriteToFile(data);
}
......
......@@ -64,8 +64,8 @@ public:
void AddLandmarksSC(std::vector<ptrLandmark> landmarks);
void AddLandmarks(std::vector<ptrLandmark> landmarks);
std::vector<ptrLandmark> LookForLandmarks();
// Waypoints and assoziations
Waypoints TriggerAssoziations(const std::vector<ptrLandmark> &landmarks);
// Waypoints and associations
Waypoints TriggerAssociations(const std::vector<ptrLandmark> &landmarks);
void AddWaypoints(Waypoints waypoints);
void AssessDoors();
// Calculations
......@@ -74,6 +74,7 @@ public:
ptrGraphNetwork GetGraphNetwork() const;
double ShortestPathDistance(const GraphEdge *edge, const ptrWaypoint waypoint);
const Point& GetOwnPos();
//WriteXML
void WriteToFile();
//evaluate waypoints
......
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