humanoidagent.java

2004年robotcup世界冠军源代码

// Copyright (C) 2002 Takeshi Morimoto <[e-mail]>
// All rights reserved.
// Edited By Omid Aghazadeh
package yab.agent;

import java.net.*;
import java.util.*;
import yab.agent.*;
import yab.agent.object.*;
import MRL.Utilities.Partitioning.Path;

//saeed 85-2-3
public abstract class HumanoidAgent
    extends Agent {
  protected HumanoidAgent(int agentType, InetAddress address, int port) {
    super(agentType, address, port);
  }

  private Humanoid self() {
    return (Humanoid) world.self;
  }

  protected int hearingLimit() {
    return HEARING_LIMIT_OF_HUMANOID;
  }


//  protected int utteranceLimit() {
//    return 2;
//  }

  protected void move(MotionlessObject destination) throws
      ActionCommandException {
    move(Collections.singleton(destination));
  }

  protected void policeMove(MotionlessObject destination) throws
      ActionCommandException {
    policeMove(Collections.singleton(destination));
  }

  protected void trafficMove(MotionlessObject destination) throws
      ActionCommandException {
    trafficMove(Collections.singleton(destination));
  }


  protected void move(Collection destinations) throws ActionCommandException {
//    move(getRoute(destinations));
    trafficMove(destinations);
  }

  protected void policeMove(Collection destinations) throws
      ActionCommandException {
    move(getPoliceRoute(destinations));
  }

  protected void trafficMove(Collection destinations) throws ActionCommandException {
    move(getTrafficRoute(destinations));
  }

  protected Route prevRoute = null;
  protected void move(Route routePlan) throws ActionCommandException {
    prevRoute = routePlan;
    self().setAct(MOVE_ACT);
    //System.out.println("POS : " + self().motionlessPosition().id + " , ROUTE PLAN GENERATED : " + routePlan);
  //    System.out.println(self() + " , Route Plan Length : " + routePlan.getLength());
    move(routePlan.toIDs());
  }

  protected void move(int[] routePlan) throws ActionCommandException {
    socket().akMove(self().id, routePlan);
    throw new ActionCommandException();
  }

  /** This method returns the minimum cost route reaching one of
   *  <code>destinations</code> in consideration for a blockade
   *  located at the midpoint of a road where the controlled object
   *  is.
   *  This method routes, estimating cost of a route based upon its
   *  passability:
   *  <ul>
   *      <li>if the agent has seen a road and the road is passable,
   *      cost for reaching the road is low</li>
   *      <li>if the agent has seen a road and the road is not
   *      passable, the cost is high</li>
   *      <li>if the agent has not seen a road but the road may be
   *      passable, the cost is the middle</li>
   *  </ul>
   * @see Router.CostFunction#cost(MotionlessObject, MotionlessObject) cost
   */
  protected Route getRoute(Collection destinations) {
    MotionlessObject pos = self().motionlessPosition();
    if (! (pos instanceof Road) ||PASSABLE_C.eval((Road)pos)) //  ( (Road) pos).passableLines() >= 1
      return Router.get(pos, destinations, RELIABILITY_COST_FUNCTION);
    Route route = Router.get(outsidePosition(), destinations,
                             RELIABILITY_COST_FUNCTION);
    modifyOriginToCurrentPosition(route);

    return route;
  }

  protected Route getPoliceRoute(Collection destinations) {
    MotionlessObject pos = self().motionlessPosition();
    if (! (pos instanceof Road) ||PASSABLE_C.eval((Road)pos)) //  ( (Road) pos).passableLines() >= 1
      return Router.get(pos, destinations, POLICE_COST_FUNCTION_MOVE);
    Route route = Router.get(outsidePosition(), destinations,
                             POLICE_COST_FUNCTION_MOVE);
    modifyOriginToCurrentPosition(route);

    return route;

  }

  protected Route getTrafficRoute(Collection destinations) {
    MotionlessObject pos = self().motionlessPosition();
    if (! (pos instanceof Road) || PASSABLE_C.eval((Road)pos)) //  ( (Road) pos).passableLines() >= 1
      return Router.get(pos, destinations, MOVING_COST_FUNCTION);
    Route route = Router.get(outsidePosition(), destinations,
                             MOVING_COST_FUNCTION);
    modifyOriginToCurrentPosition(route);

    return route;
  }

  protected Node outsidePosition(){
    return outsidePosition(self());
  }

  public static Node outsidePosition(MovingObject mo) {
    MotionlessObject mop = mo.motionlessPosition();
    if(mop == null) return null;
    if (mop instanceof Road){
      Road rd = (Road) mop;
      return (mo.positionExtra() < rd.length() / 2)
          ? (Node)rd.head() : (Node)rd.tail();

    }
    if (mop instanceof Building) return (Node)((Building)mop).entrance();
    if (mop instanceof Node)return (Node)mop;
    throw new Error ("Should Be Returned Already");
  }

  public void modifyOriginToCurrentPosition(Route route) {
    MotionlessObject pos = self().motionlessPosition();
    for (Route rt = route; true; rt = rt.previous()) {
      if (rt.end == pos) {
        rt.setPrevious(null);
        return;
      }
      if (rt.previous() == null) {
        rt.setPrevious(new Route(pos));
        return;
      }
    }
  }

  // Was Private!
  public static final Router.CostFunction RELIABILITY_COST_FUNCTION
      = new Router.CostFunction() {
        final float WEIGHT = Integer.MAX_VALUE;
    public float cost(MotionlessObject from, MotionlessObject to) {
      float c = 1;
      if (! (to instanceof Road))
        return c;
      Road rd = (Road) to;
      PointObject po = (PointObject) from;
      if (rd.passableLinesFrom(po) == 0)
        return c * WEIGHT * WEIGHT;
      return rd.hasBeenSeen() ? c : c * WEIGHT; // Cycle 3+
    }
  };

    public final Router.CostFunction POLICE_EXPLORING_COST_FUNCTION
      = new Router.CostFunction() {
        final float WEIGHT = Integer.MAX_VALUE;
        final float openC = 15;
        final float notSeenC = 1;
        final float trafficC = 20;
        final float hasBlocks = 3;
        final float notPassableC = WEIGHT;
        final Set trafficSet = world.inTraffic;
        public float cost(MotionlessObject from, MotionlessObject to) {
          float c = 1;
          if (! (to instanceof Road))
            return c;
          Road rd = (Road) to;
          if(rd.head()==(PointObject) from)
          {
              if(rd.linesToTail()<=0) return notPassableC;
          }
          else if(rd.tail()==(PointObject) from)
          {
              if(rd.linesToHead()<=0) return notPassableC;
          }
          c*=rd.length();
          if(trafficSet.contains(rd))
            return c*trafficC;
          PointObject po = (PointObject) from;
          if (rd.passableLinesFrom(po) == 0)
            return c*hasBlocks;
          return rd.hasBeenSeen() ? c * openC : c*notSeenC ; // Cycle 3+
        }
  };

   /// not detect cycle in trrafic
    /**
     * todo : Must Decide About Coefficients .
     */
   public final Router.CostFunction POLICE_COST_FUNCTION_MOVE
      = new Router.CostFunction() {
        final float WEIGHT = Integer.MAX_VALUE;
        final float notPassable = WEIGHT;
        final float trafficC = 10f;
        final float trafficCoef = 10f;
        final float notSeenC = 5f;
        final float notSeenCoef = 1.25f;
        final float hasBlocksC = 10f;
        final Set trafficSet = world.inTraffic;
    public float cost(MotionlessObject from, MotionlessObject to) {
      float c = 1f;
      if (! (to instanceof Road))
      {
          if(to instanceof Node)
          {
              if(from instanceof Road)
              {
                  Road fr = (Road) from;
                  if(to==fr.head() && fr.linesToHead()<=0)
                    return notPassable;
                  else if(to==fr.tail() && fr.linesToTail()<=0)
                    return notPassable;
              }

          }
          return c;
      }
      Road rd = (Road) to;
      if(rd.head()==(PointObject) from)
      {