Exemplo n.º 1
0
    @Override
    public List<LogicalWaypoint> getLogicalWaypoints() {

      EnvironmentKnowledgeModule knowledge = agent.getEnvironmentKnowledge();
      // convert the exit path into a list of logical waypoints
      List<LogicalWaypoint> waypoints = new ArrayList<LogicalWaypoint>();

      if (singletonPath) {
        //                System.out.println("getLink =" + onlyLink.getId());
        //                System.out.println("agent area =" + agent.getCurrentAreaId());
        //                System.out.println("goal area =" + goalAreaId);

        IbevacLogicalWaypoint linkWaypoint =
            new IbevacLogicalWaypoint(
                onlyLink,
                knowledge.resolveAreaById(agent.getCurrentAreaId()),
                knowledge.resolveAreaById(goalAreaId),
                space,
                agent);
        waypoints.add(linkWaypoint);
        if (goalAreaId != -1) {
          IbevacLogicalWaypoint areaWaypoint =
              (new IbevacLogicalWaypoint(
                      null, knowledge.resolveAreaById(goalAreaId), null, space, agent))
                  .setInitialWayPoint(linkWaypoint.getWP1());
          waypoints.add(areaWaypoint);
        }
        return waypoints;
      }
      if (path == null) {
        return null;
      }

      List<RoomEdge> edges = path.getEdgeList();

      //            System.out.println(edges);
      CLink startLink = knowledge.resolveLinkById(path.getStartVertex());
      SpatialWaypoint lastPoint = new SpatialWaypoint(agent.getPosition());

      // If the agent is currently not at a getLink then create a waypoint for the getLink
      if (agent.getCurrentAreaId() != startLink.getId()) {

        Integer otherAreaId =
            (agent.getCurrentAreaId() == startLink.getConnectingAreas().get(0))
                ? startLink.getConnectingAreas().get(1)
                : startLink.getConnectingAreas().get(0);
        IbevacLogicalWaypoint lwp =
            new IbevacLogicalWaypoint(
                startLink,
                knowledge.resolveAreaById(agent.getCurrentAreaId()),
                knowledge.resolveAreaById(otherAreaId),
                space,
                agent);
        waypoints.add(lwp);
        lastPoint = lwp.getWP1();
      }

      CLink prevLink = startLink;

      for (RoomEdge edge : edges) {
        // get both areas that are connected by the edge (there is at least one)
        //                CArea area0 = knowledge.resolveAreaById(edge.id0());
        //                CArea area1 = knowledge.resolveAreaById(edge.id1());
        //                System.out.println(edge);
        CArea room = knowledge.resolveAreaById(edge.areaId());
        if (room instanceof CStaircase) {
          // must be a staircase. we just break here.
          // after teleporting a new path has to be calculated
          //                    assert (area0 instanceof CStaircase && area1 instanceof CStaircase);

          // create a logical waypoint, using the links and the areas
          assert edge.getRoom0() != null && edge.getRoom1() != null;
          IbevacLogicalWaypoint wp =
              new IbevacLogicalWaypoint(null, edge.getRoom0(), edge.getRoom1(), space, agent);
          waypoints.add(wp);

          break;
        }

        CLink fromLink = knowledge.resolveLinkById(edge.id0());
        CLink toLink = knowledge.resolveLinkById(edge.id1());
        //                System.out.println("prev" + prevLink.getId());
        //                System.out.println(fromLink.getId());
        //                System.out.println(toLink.getId());

        assert (prevLink.getId() == fromLink.getId() || prevLink.getId() == toLink.getId());

        // by definition, we want area0 to be the next one and area1 the one thereafter
        // if it's not the case swap...
        if (prevLink.getId() != fromLink.getId()) {
          CLink temp = fromLink;
          fromLink = toLink;
          toLink = temp;
        } // get the getLink that is associated with this edge (if any)
        prevLink = toLink;
        //                assert agent.getCurrentAreaId() == fromLink.getConnectingAreas().get(0) ||
        // agent.getCurrentAreaId() == fromLink.getConnectingAreas().get(1);

        // create a logical waypoint, using the links and the areas
        IbevacLogicalWaypoint areaWaypoint =
            (new IbevacLogicalWaypoint(null, edge.area(), null, space, agent))
                .setInitialWayPoint(lastPoint);
        waypoints.add(areaWaypoint);
        assert toLink.getConnectingAreas() != null;
        if (toLink.getConnectingAreas().size() > 1) {
          Integer otherAreaId =
              (edge.areaId() == toLink.getConnectingAreas().get(0))
                  ? toLink.getConnectingAreas().get(1)
                  : toLink.getConnectingAreas().get(0);
          IbevacLogicalWaypoint lwp =
              new IbevacLogicalWaypoint(
                  toLink, edge.area(), knowledge.resolveAreaById(otherAreaId), space, agent);
          waypoints.add(lwp);
          lastPoint = lwp.getWP1();
        } else {
          IbevacLogicalWaypoint lwp =
              new IbevacLogicalWaypoint(toLink, edge.area(), null, space, agent);
          waypoints.add(lwp);
          lastPoint = lwp.getWP1();
        }
      }
      if (goalAreaId != -1) {
        waypoints.add(
            new IbevacLogicalWaypoint(
                    null, knowledge.resolveAreaById(this.goalAreaId), null, space, agent)
                .setInitialWayPoint(lastPoint));
      }

      return waypoints;
    }
Exemplo n.º 2
0
  private EscapePath determineEscapePath(Collection<Integer> goalAreaIds) {
    // get the knowledge object and the current area id, we'll need it further down
    CompleteKnowledgeInverted knowledge =
        (CompleteKnowledgeInverted) agent.getEnvironmentKnowledge();
    int currentAreaId = agent.getCurrentAreaId();
    //        System.out.println("deterimining an exit path");
    //        int firstLink = currentAreaId;
    GraphPath<Integer, RoomEdge> finalPath = null;
    double minWeight = Double.MAX_VALUE;
    //        GraphPath<Integer, RoomEdge> tempPath;
    //        int tempRoom;
    int goalArea = -1;

    for (Integer goalAreaId : goalAreaIds) {
      // is current area a room?
      //            System.out.println("here" + goalAreaId + "from" + currentAreaId);
      if (knowledge.resolveAreaById(currentAreaId) == null) {
        // currently on a getLink
        //
        //                System.out.println("getLink");
        KShortestPaths<Integer, RoomEdge> ksp =
            new KShortestPaths<Integer, RoomEdge>(knowledge.getGraph(), currentAreaId, 1);
        //                KShortestPaths<Integer, RoomEdge> ksp1 = new
        // KShortestPaths<>(knowledge.getGraph(), roomId1, 1);
        TreeMap<Double, GraphPath<Integer, RoomEdge>> paths =
            new TreeMap<Double, GraphPath<Integer, RoomEdge>>();
        for (CLink connectingLink : knowledge.getConnectingLinks(goalAreaId)) {
          for (GraphPath<Integer, RoomEdge> path : ksp.getPaths(connectingLink.getId())) {
            paths.put(path.getWeight(), path);
          }
        }
        //                List<GraphPath<Integer, RoomEdge>> paths1 = ksp1.getPaths(goalAreaId);

        // there should be at least one path to the exit from each room
        assert (paths != null && !paths.isEmpty());
        GraphPath<Integer, RoomEdge> route = paths.firstEntry().getValue();

        if (minWeight >= route.getWeight()) {
          finalPath = route;
          minWeight = route.getWeight();
          goalArea = goalAreaId;
        }

      } // if not, then it must be a area
      else {
        //                System.out.println("area");
        for (CLink homeLink : knowledge.getConnectingLinks(currentAreaId)) {
          // System.out.println(knowledge.getGraph().edgeSet());
          KShortestPaths<Integer, RoomEdge> ksp =
              new KShortestPaths<Integer, RoomEdge>(knowledge.getGraph(), homeLink.getId(), 1);
          //                    System.out.println("home getLink " + homeLink.getId());
          TreeMap<Double, GraphPath<Integer, RoomEdge>> paths =
              new TreeMap<Double, GraphPath<Integer, RoomEdge>>();
          for (CLink goalLink : knowledge.getConnectingLinks(goalAreaId)) {
            //                        System.out.println("goal getLink " + goalLink.getId());
            if (goalLink.getId() == homeLink.getId()) {
              return new EscapePathImpl(goalLink, goalAreaId);
            }
            if (ksp.getPaths(goalLink.getId()) == null) {
              //                            System.out.println("no path to exit");
              continue;
            }
            for (GraphPath<Integer, RoomEdge> path : ksp.getPaths(goalLink.getId())) {

              paths.put(path.getWeight(), path);
            }
          }
          //                List<GraphPath<Integer, RoomEdge>> paths1 = ksp1.getPaths(goalAreaId);

          // there should be at least one path to the exit from each room
          assert (paths != null && !paths.isEmpty());
          GraphPath<Integer, RoomEdge> route = paths.firstEntry().getValue();

          if (minWeight >= route.getWeight()) {
            finalPath = route;
            minWeight = route.getWeight();
            //                        firstLink = homeConnectingLink;
            goalArea = goalAreaId;
          }
        }

        // there is at least one path to the exit

      }
    }
    //        if (finalPath == null) {
    //            System.out.println(((IbevacAgent) this.agent).getId() + "," +
    // goalAreaIds.iterator().next());
    //        }
    //        assert (finalPath != null);
    //        //TODO : change this stupid hack;
    //        agent.setCurrentGoal(new IbevacLogicalWaypoint(null,
    // knowledge.resolveAreaById(goalArea), null, space, agent).getWP1().getPoint());
    return new EscapePathImpl(finalPath, goalArea);
  }