{Previous tutorial}[link:files/doc/tutorials/02-GoForward_rdoc.html]
{Next tutorial}[link:files/doc/tutorials/04-EventPropagation_rdoc.html]
= Planning and following a path
We'll now use a (slightly) more complex system to make our robot move. The
robot will now have a goal, defined as a (x, y) point. It will generate a
trajectory which leads it to that goal, and then execute that trajectory.

This tutorial therefore shows the following:
* how multiple activities can be _temporally_ coordinated to make the robot
  reach a defined goal, and
* how the plan represents how one activity relates to another.

In this new robot, three activities will be used to make the robot reach
its goal. The plan will therefore represent various things:
* the three activities: the high-level activity which represent the goal of
  the robot; the path planning activity and the path execution activity.
* how these activities relate to each other. For that, Roby defines <it>
  task relations</it>.
* how the plan describes the temporal relations between these activities (i.e.
  when a given activity should be started).

To hold all these, we will create a new robot:

  roby robot PathPlan

== Defining the task models
This section will describe the task models, without the actual implementation
of the actual implementation of these activities. That implementation is
discussed later in that tutorial. The goal is to first make you grasp what the
task models, and the plan model is about and only then how the tasks can
actually control the robot itself.

* the +MoveTo+ task express the current goal of the robot, and holds the path
  data. Open <tt>tasks/move_to.rb</tt> and add the following:
    class MoveTo < Roby::Task
        terminates

        # The movement goal
        argument :goal
        # The generated path
        def path; data end
    end

* the +ComputePath+ task generates the path on behalf of a +MoveTo+. When
  successful, it updates the +data+ attribute of the +MoveTo+ task it is
  planning. It uses a standard task, Roby::ThreadTask, which allows to
  represent the execution of a separate thread into the main plan. Open
  <tt>tasks/compute_path.rb</tt> and add the following:

    require 'roby/thread_task'
    class ComputePath < Roby::ThreadTask
      # The movement goal
      argument :goal
      # The maximum speed limit
      argument :max_speed
    end

* finally, +TrackPath+ takes the path generated and follows it. Open
  <tt>tasks/track_path.rb</tt> and add the following:
    class TrackPath < Roby::Task
      terminates

      # The task holding the path data
      argument :path_task
    end

*Note*: the file names are "best practice" recommandations. They are not at all
required for the application to work.

== Building the movement plan
Let's add a +move_to+ action to our robot, which builds the plan corresponding
to the whole movement. The action definition, in
<tt>planners/PathPlan/main.rb</tt> would look like this:

  # Note: the method arguments are accessed through the +arguments+ hash
  method(:move_to) do
    # The goal point
    goal = Pos::Vector3D.new(*arguments.values_at(:x, :y))
    # The high-level representation of the movement
    move = MoveTo.new :goal => goal
    move.realized_by compute = ComputePath.new(:goal => goal, :max_speed => 1.0)
    move.realized_by track   = TrackPath.new(:path_task => move)

    move.on       :start,   compute, :start
    compute.on    :success, track,   :start
    track.forward :success, move,    :success

    move
  end

The first part creates the <em>task structure</em>, which expresses the
relationships of the different tasks of the plan. This simple plan uses only
one kind of relation, the RealizedBy relation. In this relation, the child task
(i.e. +compute+ and +track+) are simple activities which achieve the parent's higher-level
action.

The second part creates the <em>event structure</em>, which expresses how the
plan should respond to new situations. In our case, the three line describe the following:
* the path planning must be started when the movement is started. This uses the Signal
  event relation.
* the path execution must be started when the path planning has successfully finished, and
* the movement <it>has finished</it> when the path tracking <it>has finished</it>. This uses
  the Forward relation.

The difference between those two relations is subtle, so let's try to explain a bit more:
* in the first two cases, what the system must do is <it>executing a new action</it> in
  response to a new situation. When the +start+ event of +move+ is emitted, the +move+
  activity has just started (i.e. all necessary actions have been taken to start that
  new activity). The system should then make what is necessary to start computing the
  path: it calls the _command_ of the +start+ event of +compute+.
* in the third case, however, no specific action should be taken to end the
  +move+ task. Instead, the plan expresses that the +move+ task is finished
  <it>as soon as</it> the +track+ task is. Another way to put it is that the
  situation represented by the +success+ event of MoveTo is, in this particular
  plan, the same than the situation represented by the +success+ event of
  TrackPath. More generally, if +a+ is forwarded to +b+ all situations that lead
  to the emission of +a+ also lead to the emission of +b+. Or, in other words, that
  the situation represented by +b+ is a superset of the one represented by +a+
  (the equality, like here, is a particular case)

  link:../../images/event_generalization.png

  *Example*: in this plan, the +success+ event of a particular low-level action
  is forwarded in more high-level parts of the plan. This allows to actually
  link the low and high level parts of the plan and reason on that link.

Task relations allow the system to keep track of what a given task is useful for,
what are error conditions and how to react to errors. The next two tutorials will
describe these parts in more details.

== Running this unfinished controller

Let's run this controller. Launch the controller
  $ scripts/run PathPlan
  
In another terminal, launch the shell and start the move_to! action
  $ scripts/shell
  >> move_to! :x => 10, :y => 10
  => MoveTo{goal => Vector3D(x=10.000000,y=10.000000,z=0.000000)}:0x48350370[]
  >>
  !Roby::ChildFailedError
  !at [336040:01:45.419/186] in the failed event of ComputePath:0x483502e0
  !block not supplied (ArgumentError)
  !  /home/doudou/dev/roby/lib/roby/thread_task.rb:51:in `instance_eval',
  !    /home/doudou/dev/roby/lib/roby/thread_task.rb:61:in `value',
  !    /home/doudou/dev/roby/lib/roby/thread_task.rb:61:in the polling handler,
  !    /home/doudou/system/powerpc-linux/ruby-1.8.6/lib/ruby/site_ruby/1.8/rubygems/custom_require.rb:27:in `gem_original_require',
  !    /home/doudou/system/powerpc-linux/ruby-1.8.6/lib/ruby/site_ruby/1.8/rubygems/custom_require.rb:27:in `require',
  !    scripts/run:3
  !
  !The failed relation is
  !  MoveTo:0x48350370
  !    owners: Roby::Distributed
  !    arguments: {:goal=>Vector3D(x=10.000000,y=10.000000,z=0.000000)}
  !  realized_by ComputePath:0x483502e0
  !    owners: Roby::Distributed
  !    arguments: {:max_speed=>1.0,
  !     :goal=>Vector3D(x=10.000000,y=10.000000,z=0.000000)}
  !The following tasks have been killed:
  !  ComputePath:0x483502e0
  !  MoveTo:0x48350370

Mmmm... What happened ? The call to <tt>move_to!</tt> returned properly, which
means that the plan has been properly generated and the MoveTo high-level
action started. Nonetheless, an error occured.

The error message appeared because an ArgumentError exception has been raised
in <tt>thread_task.rb:51</tt> Looking at the documentation of Roby::ThreadTask,
we see that the definition of ComputePath has not called the Roby::ThreadTask.implementation
statement, and as such the polling handler failed. Roby answers to that by
emitting the +failed+ event of the problematic task.

The plan-related error (ChildFailedError) has then been generated by Roby's
plan analysis:
* a +realized_by+ relation between MoveTo and ComputePath exists, which means
  that MoveTo cannot be achieved without executing ComputePath first.
* ComputePath failed, so <i>in the current state of the plan</i>, the MoveTo
  action cannot be achieved either.

A more complete description of errors and, more importantly, of how to handle
them is given in the following tutorials.

== Implementation of +ComputePath+ and +TrackPath+
The first section did mainly explain how the plan represents the logical
relations between each tasks and each task's events. We will now get into the
details of actually implementing these tasks.

* First, we have to initialize the position in <tt>tasks/PathPlan.rb</tt>

    Roby::State.update do |s|
      s.pos = Roby::Pos::Vector3D.new
    end

* for +ComputePath+, we will simply generate a random set of points in-between
  the current robot position and the specified goal. In general (i.e. not here,
  but in a real case), this process takes time and as such cannot be done in
  one pass of the execution cycle. We will therefore use a thread to do it,
  leaving the actual thread management to Roby::ThreadTask:

    # The robot position at which we should start planning
    # the path
    attr_reader :start_point

    # Initialize start_point and call ThreadTask's start command
    event :start do |context|
      @start_point = State.pos.dup
      super
    end

    # Implementation of the computation thread
    implementation do
      path = [start_point]
      while goal.distance(path.last) > max_speed
        u = goal - path.last
        u /= u.length / max_speed
        path << path.last + u
      end
      path << goal

      Robot.info "#{path.size} points between #{start_point} and #{goal}"
      path
    end

    on :success do |ev|
      # Parents is a ValuSet, it has no #first method. Get
      # the first element with #find
      parents.find { true }.data = result
    end

  See Roby::ThreadTask to implement _interruptible_ external threads.

  Robot is a namespace which (among other things) can be used to access an
  application-specific logger set up by Roby itself. It answers to #debug,
  #info, #warning and #fatal, and by default is at the INFO level.  The Logger
  object itself is accessible at Robot.logger. Therefore, use
  Robot.logger.level= to change the logger level itself.

* as stated before, MoveTo does not require any special code. It is here
  only to represent a high level activity (the whole movement), not to actually
  execute it.

* TrackPath will then take the path data and execute the corresponding movement. For
  the purpose of this tutorial, it will simply move to the next point in the path
  at each execution cycle:

    # The current waypoint
    def current_waypoint; path_task.data[@waypoint_index] end

    poll do
      @waypoint_index ||= 0
      State.pos = current_waypoint
      @waypoint_index += 1
      if @waypoint_index == path_task.data.size
        emit :success
      end

      Robot.info "moved to #{current_waypoint}"
    end

= Next tutorial

The {next tutorial}[link:files/doc/tutorials/04-EventPropagation_rdoc.html] will
allow you to understand more by actually seeing what happens during the plan
execution. After this tutorial, you should be able to build simple task
models and simple plans, as well as execute them and understand the most common
error -- ChildFailedError.
---
vim: tw=80 et