fasr2.cc

Go to the documentation of this file.

#include <pthread.h>

#include "stage.hh"
using namespace Stg;

// generic planner implementation
#include "astar/astar.h"

static const bool verbose = false;

// navigation control params
static const double cruisespeed = 0.4;
static const double avoidspeed = 0.05;
static const double avoidturn = 0.5;
static const double minfrontdistance = 0.7;
static const double stopdist = 0.5;
static const unsigned int avoidduration = 10;
static const unsigned int PAYLOAD = 1;

static unsigned int MetersToCell(meters_t m, meters_t size_m, unsigned int size_c)
{
  m += size_m / 2.0; // shift to local coords
  m /= size_m / (float)size_c; // scale
  return (unsigned int)floor(m); // quantize
}

static meters_t CellToMeters(unsigned int c, meters_t size_m, unsigned int size_c)
{
  meters_t cell_size = size_m / (float)size_c;
  meters_t m = c * cell_size; // scale
  m -= size_m / 2.0; // shift to local coords

  return m + cell_size / 2.0; // offset to cell center
}

class Robot {
public:
  class Task {
  public:
    Model *source;
    Model *sink;
    unsigned int participants;

    Task(Model *source, Model *sink) : source(source), sink(sink), participants(0) {}
  };

  static std::vector<Task> tasks;

private:
  class Node;

  class Edge {
  public:
    Node *to;
    double cost;

    Edge(Node *to, double cost = 1.0) : to(to), cost(cost) {}
  };

  class Node {
  public:
    Pose pose;
    double value;
    std::vector<Edge *> edges;

    Node(Pose pose, double value = 0) : pose(pose), value(value), edges() {}
    ~Node()
    {
      FOR_EACH (it, edges) {
        delete *it;
      }
    }

    void AddEdge(Edge *edge)
    {
      assert(edge);
      edges.push_back(edge);
    }

    void Draw() const;
  };

  class Graph {
  public:
    std::vector<Node *> nodes;

    Graph() {}
    ~Graph()
    {
      FOR_EACH (it, nodes) {
        delete *it;
      }
    }

    void AddNode(Node *node) { nodes.push_back(node); }
    void PopFront()
    {
      const std::vector<Node *>::iterator &it = nodes.begin();
      delete *it;
      nodes.erase(it);
    }

    void Draw() const
    {
      glPointSize(3);
      FOR_EACH (it, nodes) {
        (*it)->Draw();
      }
    }

    bool GoodDirection(const Pose &pose, meters_t range, radians_t &heading_result)
    {
      // find the node with the lowest value within range of the given
      // pose and return the absolute heading from pose to that node

      if (nodes.empty())
        return 0; // a null guess

      Node *best_node = NULL;

      // find the closest node
      FOR_EACH (it, nodes) {
        Node *node = *it;
        double dist = hypot(node->pose.x - pose.x, node->pose.y - pose.y);

        // if it's in range, and either its the first we have found,
        // or it has a lower value than the current best
        if (dist < range && (best_node == NULL || node->value < best_node->value)) {
          best_node = node;
        }
      }

      if (best_node == NULL) {
        fprintf(stderr, "FASR warning: no nodes in range");
        return false;
      }

      // else
      heading_result = atan2(best_node->pose.y - pose.y, best_node->pose.x - pose.x);

      // add a little bias to one side (the left) - creates two lanes
      // of robot traffic
      heading_result = normalize(heading_result + 0.25);

      return true;
    }
  };

  class GraphVis : public Visualizer {
  public:
    Graph **graphpp;

    explicit GraphVis(Graph **graphpp) : Visualizer("graph", "vis_graph"), graphpp(graphpp) {}
    virtual ~GraphVis() {}
    virtual void Visualize(Model *mod, Camera *)
    {
      if (*graphpp == NULL)
        return;

      glPushMatrix();

      Gl::pose_inverse_shift(mod->GetGlobalPose());

      // mod->PushColor( 1,0,0,1 );

      Color c = mod->GetColor();
      c.a = 0.4;

      mod->PushColor(c);
      (*graphpp)->Draw();
      mod->PopColor();

      glPopMatrix();
    }
  };

private:
  long int wait_started_at;

  ModelPosition *pos;
  ModelRanger *laser;
  ModelRanger *sonar;
  ModelFiducial *fiducial;

  unsigned int task;

  Model *fuel_zone;
  int avoidcount;

  // int work_get, work_put;
  bool charger_ahoy;
  double charger_bearing;
  double charger_range;
  double charger_heading;

  enum { MODE_WORK = 0, MODE_DOCK, MODE_UNDOCK, MODE_QUEUE } mode;

  radians_t docked_angle;

  static pthread_mutex_t planner_mutex;

  Model *goal;
  Pose cached_goal_pose;

  Graph *graphp;
  GraphVis graphvis;
  // unsigned int node_interval;
  // unsigned int node_interval_countdown;

  static const unsigned int map_width;
  static const unsigned int map_height;
  static uint8_t *map_data;
  static Model *map_model;

  bool fiducial_sub;
  bool laser_sub;
  bool sonar_sub;

  bool force_recharge;

public:
  Robot(ModelPosition *pos, Model *fuel)
      // Model* pool )
      : wait_started_at(-1),
        pos(pos),
        laser((ModelRanger *)pos->GetChild("ranger:1")),
        sonar((ModelRanger *)pos->GetChild("ranger:0")),
        fiducial((ModelFiducial *)pos->GetUnusedModelOfType("fiducial")),
        task(random() % tasks.size()), // choose a task at random
        fuel_zone(fuel),
        // pool_zone(pool),
        avoidcount(0),
        // randcount(0),
        // work_get(0),
        // work_put(0),
        charger_ahoy(false),
        charger_bearing(0),
        charger_range(0),
        charger_heading(0),
        mode(MODE_WORK),
        docked_angle(0),
        goal(tasks[task].source),
        cached_goal_pose(),
        graphp(NULL),
        graphvis(&graphp),
        // node_interval( 20 ),
        // node_interval_countdown( node_interval ),
        fiducial_sub(false),
        laser_sub(false),
        sonar_sub(false),
        force_recharge(false)
  {
    // need at least these models to get any work done
    // (pos must be good, as we used it in the initialization list)
    assert(laser);
    assert(fiducial);
    assert(sonar);
    assert(goal);

    // match the color of my destination
    pos->SetColor(tasks[task].source->GetColor());

    tasks[task].participants++;

    EnableLaser(true);

    // we access all other data from the position callback
    pos->AddCallback(Model::CB_UPDATE, (model_callback_t)UpdateCallback, this);
    pos->Subscribe();

    pos->AddVisualizer(&graphvis, true);

    if (map_data == NULL) {
      map_data = new uint8_t[map_width * map_height * 2];

      // MUST clear the data, since Model::Rasterize() only enters
      // non-zero pixels
      memset(map_data, 0, sizeof(uint8_t) * map_width * map_height);

      // get the map
      map_model = pos->GetWorld()->GetModel("cave");
      assert(map_model);
      Geom g = map_model->GetGeom();

      map_model->Rasterize(map_data, map_width, map_height, g.size.x / (float)map_width,
                           g.size.y / (float)map_height);

      // fix the node costs for astar: 0=>1, 1=>9

      unsigned int sz = map_width * map_height;
      for (unsigned int i = 0; i < sz; i++) {
        if (map_data[i] == 0)
          map_data[i] = 1;
        else if (map_data[i] == 1)
          map_data[i] = 9;
        else
          printf("FASR: bad value %d in map at index %d\n", (int)map_data[i], (int)i);

        assert((map_data[i] == 1) || (map_data[i] == 9));
      }
    }

    //( goal );
    // puts("");
  }

  void Enable(Stg::Model *model, bool &sub, bool on)
  {
    if (on && !sub) {
      sub = true;
      model->Subscribe();
    }

    if (!on && sub) {
      sub = false;
      model->Unsubscribe();
    }
  }

  void EnableSonar(bool on) { Enable(sonar, sonar_sub, on); }
  void EnableLaser(bool on) { Enable(laser, laser_sub, on); }
  void EnableFiducial(bool on) { Enable(fiducial, fiducial_sub, on); }
  static std::map<std::pair<uint64_t, uint64_t>, Graph *> plancache;

  uint64_t Pt64(const ast::point_t &pt)
  {
    // quantize the position a bit to reduce planning frequency
    uint64_t x = pt.x / 1;
    uint64_t y = pt.y / 1;

    return (x << 32) + y;
  }

  void CachePlan(const ast::point_t &start, const ast::point_t &goal, Graph *graph)
  {
    std::pair<uint64_t, uint64_t> key(Pt64(start), Pt64(goal));
    plancache[key] = graph;
  }

  Graph *LookupPlan(const ast::point_t &start, const ast::point_t &goal)
  {
    std::pair<uint64_t, uint64_t> key(Pt64(start), Pt64(goal));
    return plancache[key];
  }

  void Plan(Pose sp)
  {
    // change my color to that of my destination
    // pos->SetColor( dest->GetColor() );

    Pose pose = pos->GetPose();
    Geom g = map_model->GetGeom();

    ast::point_t start(MetersToCell(pose.x, g.size.x, map_width),
                       MetersToCell(pose.y, g.size.y, map_height));
    ast::point_t goal(MetersToCell(sp.x, g.size.x, map_width),
                      MetersToCell(sp.y, g.size.y, map_height));

    // printf( "searching from (%.2f, %.2f) [%d, %d]\n", pose.x, pose.y, start.x, start.y );
    // printf( "searching to   (%.2f, %.2f) [%d, %d]\n", sp.x, sp.y, goal.x, goal.y );

    // astar() is not reentrant, so we protect it thus

    // graph.nodes.clear(); // whatever happens, we clear the old plan

    // pthread_mutex_lock( &planner_mutex );

    // check to see if we have a path planned for these positions already
    // printf( "plancache @ %p size %d\n", &plancache, (int)plancache.size() );

    // if( graphp )
    // delete graphp;

    graphp = LookupPlan(start, goal);

    if (!graphp) // no plan cached
    {
      static float misses = 0;
      misses++;

      std::vector<ast::point_t> path;
      bool result = ast::astar(map_data, map_width, map_height, start, goal, path);

      if (!result)
        printf("FASR2 warning: plan failed to find path from (%.2f,%.2f) to (%.2f,%.2f)\n", pose.x,
               pose.y, sp.x, sp.y);

      graphp = new Graph();

      unsigned int dist = 0;

      Node *last_node = NULL;

      for (std::vector<ast::point_t>::reverse_iterator rit = path.rbegin(); rit != path.rend();
           ++rit) {
        // printf( "%d, %d\n", it->x, it->y );

        Node *node = new Node(Pose(CellToMeters(rit->x, g.size.x, map_width),
                                   CellToMeters(rit->y, g.size.y, map_height), 0, 0),
                              dist++); // value stored at node

        graphp->AddNode(node);

        if (last_node)
          last_node->AddEdge(new Edge(node));

        last_node = node;
      }

      CachePlan(start, goal, graphp);
    } else {
      static float hits = 0;
      hits++;
      // puts( "FOUND CACHED PLAN" );
    }

    // printf( "hits/misses %.2f\n", hits/misses );
    // pthread_mutex_unlock( &planner_mutex );
  }

  void Dock()
  {
    const meters_t creep_distance = 0.5;

    if (charger_ahoy) {
      // drive toward the charger
      // orient term helps to align with the way the charger is pointing
      const double orient = normalize(M_PI - (charger_bearing - charger_heading));
      const double a_goal = normalize(charger_bearing - 2.0 * orient);

      if (charger_range > creep_distance) {
        if (!ObstacleAvoid()) {
          pos->SetXSpeed(cruisespeed);
          pos->SetTurnSpeed(a_goal);
        }
      } else // we are very close!
      {
        pos->SetTurnSpeed(a_goal);
        pos->SetXSpeed(0.02); // creep towards it

        if (charger_range < 0.08) // close enough
        {
          pos->Stop();
          docked_angle = pos->GetPose().a;
          EnableLaser(false);
        }

        if (pos->Stalled()) // touching
          pos->SetXSpeed(-0.01); // back off a bit
      }
    } else {
      // printf( "FASR: %s docking but can't see a charger\n", pos->Token() );
      pos->Stop();
      EnableFiducial(false);
      mode = MODE_WORK; // should get us back on track eventually
    }

    // if the battery is charged, go back to work
    if (Full()) {
      // printf( "fully charged, now back to work\n" );
      mode = MODE_UNDOCK;
      EnableSonar(true); // enable the sonar to see behind us
      EnableLaser(true);
      EnableFiducial(true);
      force_recharge = false;
    }
  }

  void SetTask(unsigned int t)
  {
    task = t;
    tasks[task].participants++;
  }

  void UnDock()
  {
    const meters_t back_off_distance = 0.2;
    const meters_t back_off_speed = -0.02;
    const radians_t undock_rotate_speed = 0.3;
    const meters_t wait_distance = 0.2;
    const unsigned int BACK_SENSOR_FIRST = 10;
    const unsigned int BACK_SENSOR_LAST = 13;

    // make sure the required sensors are running
    assert(sonar_sub);
    assert(fiducial_sub);

    if (charger_range < back_off_distance) {
      pos->SetXSpeed(back_off_speed);

      pos->Say("");

      // stay put while anything is close behind
      const std::vector<ModelRanger::Sensor> &sensors = sonar->GetSensors();

      for (unsigned int s = BACK_SENSOR_FIRST; s <= BACK_SENSOR_LAST; ++s)
        if (sensors[s].ranges.size() < 1 || sensors[s].ranges[0] < wait_distance) {
          pos->Say("Waiting...");
          pos->SetXSpeed(0.0);
          return;
        }
    } else { // we've backed up enough

      double heading_error = normalize(pos->GetPose().a - (docked_angle + M_PI));

      if (fabs(heading_error) > 0.05) {
        // turn
        pos->SetXSpeed(0);
        pos->SetTurnSpeed(undock_rotate_speed * sgn(-heading_error));
      } else {
        // we're pointing the right way, so we're done
        mode = MODE_WORK;

        // if we're not working on a drop-off
        if (pos->GetFlagCount() == 0) {
          // pick a new task at random
          SetTask(random() % tasks.size());
          SetGoal(tasks[task].source);
        } else
          SetGoal(tasks[task].sink);

        EnableFiducial(false);
        EnableSonar(false);
      }
    }
  }

  bool ObstacleAvoid()
  {
    bool obstruction = false;
    bool stop = false;

    // find the closest distance to the left and right and check if
    // there's anything in front
    double minleft = 1e6;
    double minright = 1e6;

    // Get the data
    // const std::vector<ModelLaser::Sample>& scan = laser->GetSamples();

    const std::vector<meters_t> &scan = laser->GetSensors()[0].ranges;

    uint32_t sample_count = scan.size();

    for (uint32_t i = 0; i < sample_count; i++) {
      if (verbose)
        printf("%.3f ", scan[i]);

      if ((i > (sample_count / 4)) && (i < (sample_count - (sample_count / 4)))
          && scan[i] < minfrontdistance) {
        if (verbose)
          puts("  obstruction!");
        obstruction = true;
      }

      if (scan[i] < stopdist) {
        if (verbose)
          puts("  stopping!");
        stop = true;
      }

      if (i > sample_count / 2)
        minleft = std::min(minleft, scan[i]);
      else
        minright = std::min(minright, scan[i]);
    }

    if (verbose) {
      puts("");
      printf("minleft %.3f \n", minleft);
      printf("minright %.3f\n ", minright);
    }

    if (obstruction || stop || (avoidcount > 0)) {
      if (verbose)
        printf("Avoid %d\n", avoidcount);

      pos->SetXSpeed(stop ? 0.0 : avoidspeed);

      /* once we start avoiding, select a turn direction and stick
         with it for a few iterations */
      if (avoidcount < 1) {
        if (verbose)
          puts("Avoid START");
        avoidcount = random() % avoidduration + avoidduration;

        if (minleft < minright) {
          pos->SetTurnSpeed(-avoidturn);
          if (verbose)
            printf("turning right %.2f\n", -avoidturn);
        } else {
          pos->SetTurnSpeed(+avoidturn);
          if (verbose)
            printf("turning left %2f\n", +avoidturn);
        }
      }

      avoidcount--;

      return true; // busy avoding obstacles
    }

    return false; // didn't have to avoid anything
  }

  void SetGoal(Model *goal)
  {
    if (goal != this->goal) {
      this->goal = goal;
      Plan(goal->GetPose());
      pos->SetColor(goal->GetColor());
    }
  }

  void Work()
  {
    if (!ObstacleAvoid()) {
      if (verbose)
        puts("Cruise");

      if (Hungry())
        SetGoal(fuel_zone);

      const Pose pose = pos->GetPose();
      double a_goal = 0;

      // if the graph fails to offer advice or the goal has moved a
      // ways since last time we planned
      if (graphp == NULL || (graphp->GoodDirection(pose, 5.0, a_goal) == 0)
          || (goal->GetPose().Distance(cached_goal_pose) > 0.5)) {
        // printf( "%s replanning from (%.2f,%.2f) to %s at (%.2f,%.2f) in Work()\n",
        //        pos->Token(),
        //        pose.x, pose.y,
        //        goal->Token(),
        //        goal->GetPose().x,
        //        goal->GetPose().y );
        Plan(goal->GetPose());
        cached_goal_pose = goal->GetPose();
      }

      // if we are low on juice - find the direction to the recharger instead
      if (Hungry()) {
        EnableFiducial(true);

        while (graphp->GoodDirection(pose, 5.0, a_goal) == 0) {
          printf("%s replanning in Work()\n", pos->Token());
        }

        if (charger_ahoy) // I see a charger while hungry!
          mode = MODE_DOCK;
      }

      const double a_error = normalize(a_goal - pose.a);
      pos->SetTurnSpeed(a_error);
      pos->SetXSpeed(cruisespeed);
    }
  }

  bool Hungry() { return (force_recharge || pos->FindPowerPack()->ProportionRemaining() < 0.2); }
  bool Full() { return (pos->FindPowerPack()->ProportionRemaining() > 0.95); }
  // static callback wrapper
  static int UpdateCallback(ModelRanger *, Robot *robot) { return robot->Update(); }
  int Update(void)
  {
    if (strcmp(pos->Token(), "position:0") == 0) {
      static int seconds = 0;

      int timenow = pos->GetWorld()->SimTimeNow() / 1e6;

      if (timenow - seconds > 5) {
        seconds = timenow;

        // report the task participation
        //                                              printf( "time: %d sec\n", seconds );

        //                                              unsigned int sz = tasks.size();
        //                                              for( unsigned int i=0; i<sz; ++i )
        //                                                      printf( "\t task[%u] %3u (%s)\n",
        //                                                                                      i, tasks[i].participants, tasks[i].source->Token() );
      }
    }

    Pose pose = pos->GetPose();

    // assume we can't see the charger
    charger_ahoy = false;

    // if the fiducial is enabled
    if (fiducial_sub) {
      std::vector<ModelFiducial::Fiducial> &fids = fiducial->GetFiducials();

      if (fids.size() > 0) {
        ModelFiducial::Fiducial *closest = NULL;

        for (unsigned int i = 0; i < fids.size(); i++) {
          // printf( "fiducial %d is %d at %.2f m %.2f radians\n",
          //      i, f->id, f->range, f->bearing );

          ModelFiducial::Fiducial *f = &fids[i];

          // find the closest
          if (f->id == 2
              && (closest == NULL || f->range < closest->range)) // I see a charging station
            closest = f;
        }

        if (closest) { // record that I've seen it and where it is
          charger_ahoy = true;

          // printf( "AHOY %s\n", pos->Token() );

          charger_bearing = closest->bearing;
          charger_range = closest->range;
          charger_heading = closest->geom.a;
        }
      }
    }

    // printf( "Pose: [%.2f %.2f %.2f %.2f]\n",
    //  pose.x, pose.y, pose.z, pose.a );

    // pose.z += 0.0001;
    // pos->SetPose( pose );

    if (goal == tasks[task].source) {
      // if we're close to the source we get a flag
      Pose sourcepose = goal->GetPose();
      Geom sourcegeom = goal->GetGeom();

      meters_t dest_dist = hypot(sourcepose.x - pose.x, sourcepose.y - pose.y);

      // when we get close enough, we start waiting
      if (dest_dist < sourcegeom.size.x / 2.0) // nearby
        if (wait_started_at < 0 && pos->GetFlagCount() == 0) {
          wait_started_at = pos->GetWorld()->SimTimeNow() / 1e6; // usec to seconds
          // printf( "%s begain waiting at %ld seconds\n", pos->Token(), wait_started_at );
        }

      if (wait_started_at > 0) {
        // long int waited = (pos->GetWorld()->SimTimeNow() / 1e6) - wait_started_at;

        // leave with small probability
        if (drand48() < 0.0005) {
          // printf( "%s abandoning task %s after waiting %ld seconds\n",
          //            pos->Token(), goal->Token(), waited );

          force_recharge = true; // forces hungry to return true
          tasks[task].participants--;
          wait_started_at = -1;
          return 0;
        }
      }

      // when we get onto the square
      if (dest_dist < sourcegeom.size.x / 2.0 && pos->GetFlagCount() < PAYLOAD) {
        // transfer a chunk from source to robot
        pos->PushFlag(goal->PopFlag());

        if (pos->GetFlagCount() == 1 && wait_started_at > 0) {
          // stop waiting, since we have received our first flag
          wait_started_at = -1;
        }

        if (pos->GetFlagCount() == PAYLOAD)
          SetGoal(tasks[task].sink); // we're done working
      }
    } else if (goal == tasks[task].sink) {
      // if we're close to the sink we lose a flag
      Pose sinkpose = goal->GetPose();
      Geom sinkgeom = goal->GetGeom();

      if (hypot(sinkpose.x - pose.x, sinkpose.y - pose.y) < sinkgeom.size.x / 2.0
          && pos->GetFlagCount() > 0) {
        // puts( "dropping" );
        // transfer a chunk between robot and goal

        Model::Flag *f = pos->PopFlag();
        f->SetColor(Color(0, 1, 0)); // delivered flags are green
        goal->PushFlag(f);

        if (pos->GetFlagCount() == 0)
          SetGoal(tasks[task].source); // we're done dropping off
      }
    } else {
      assert(goal == fuel_zone); //|| goal == pool_zone );
    }

    switch (mode) {
    case MODE_DOCK:
      // puts( "DOCK" );
      Dock();
      break;

    case MODE_WORK:
      // puts( "WORK" );
      Work();
      break;

    case MODE_UNDOCK:
      // puts( "UNDOCK" );
      UnDock();
      break;

    default: printf("unrecognized mode %d\n", int(mode));
    }

    return 0; // run again
  }

  static int FlagIncr(Model *mod, Robot *)
  {
    printf("model %s collected flag\n", mod->Token());
    return 0;
  }

  static int FlagDecr(Model *mod, Robot *)
  {
    printf("model %s dropped flag\n", mod->Token());
    return 0;
  }
};

void Robot::Node::Draw() const
{
  // print value
  // char buf[32];
  // snprintf( buf, 32, "%.0f", value );
  // Gl::draw_string( pose.x, pose.y+0.2, 0.1, buf );

  // glBegin( GL_POINTS );
  // glVertex2f( pose.x, pose.y );
  // glEnd();

  glBegin(GL_LINES);
  FOR_EACH (it, edges) {
    glVertex2f(pose.x, pose.y);
    glVertex2f((*it)->to->pose.x, (*it)->to->pose.y);
  }
  glEnd();
}

// STATIC VARS
pthread_mutex_t Robot::planner_mutex = PTHREAD_MUTEX_INITIALIZER;
const unsigned int Robot::map_width(32);
const unsigned int Robot::map_height(32);
uint8_t *Robot::map_data(NULL);
Model *Robot::map_model(NULL);

std::map<std::pair<uint64_t, uint64_t>, Robot::Graph *> Robot::plancache;

std::vector<Robot::Task> Robot::tasks;

void split(const std::string &text, const std::string &separators, std::vector<std::string> &words)
{
  const int n = text.length();
  int start = text.find_first_not_of(separators);
  while ((start >= 0) && (start < n)) {
    int stop = text.find_first_of(separators, start);
    if ((stop < 0) || (stop > n))
      stop = n;
    words.push_back(text.substr(start, stop - start));
    start = text.find_first_not_of(separators, stop + 1);
  }
}

// Stage calls this when the model starts up
extern "C" int Init(ModelPosition *mod, CtrlArgs *args)
{
  // some init for only the first controller
  if (Robot::tasks.size() == 0) {
    srandom(time(NULL));

    // tokenize the worldfile ctrl argument string into words
    std::vector<std::string> words;
    split(args->worldfile, std::string(" \t"), words);
    // expecting at least one task color name
    assert(words.size() > 1);

    const World *w = mod->GetWorld();

    // make an array of tasks by reading the controller arguments
    for (unsigned int s = 1; s < words.size(); s++)
      Robot::tasks.push_back(
          Robot::Task(w->GetModel(words[s] + "_source"), w->GetModel(words[s] + "_sink")));
  }

  new Robot(
      mod, mod->GetWorld()->GetModel("fuel_zone")); //   mod->GetWorld()->GetModel( "pool_zone" ) );

  return 0; // ok
}