Tutorial
========

This tutorial uses Turtlesim.
If you are not familiar with it, we recommend you complete the following tutorial `turtlesim`_.

.. _turtlesim: https://docs.ros.org/en/galactic/Tutorials/Beginner-CLI-Tools/Introducing-Turtlesim/Introducing-Turtlesim.html

In this tutorial we will create a simple skillset made of three resources, two events and two skills.
Its main purpose is to move a single turtle named Donatello.
It can either move forward or rotate.
Moreover, our turtle can have only one entity controlling it at the same time: the skillset or the teleop.

Robot Language Model
--------------------

Data
~~~~

In this simple example, we are only interested in the position of the turtle.
It will be published each time its value changes and every second.


.. code-block:: RobotLanguage
 :linenos:

    data {
        pose: Pose period 1.0
    }

Resources
~~~~~~~~~

The skillset has three resources: *Authority*, *Move* and *Rotate*.
The *Authority* resource is used to guaranty the turtle can be moved (or rotated) by the skillset only if the teleop is not controlling it.
The resource *Move* (or *Rotate*) is used to prevent the turtle from receiving two *move_forward* (or *rotate_angle*) orders at the same time.

.. code-block:: RobotLanguage
 :linenos:

    resource {
        Authority {
            state { Teleop Skill }
            initial Teleop
            transition all
        }
        Move {
            state { Moving NotMoving }
            initial NotMoving
            transition all
        }
        Rotate {
            state { Rotating NotRotating }
            initial NotRotating
            transition all
        }
    }

Event
~~~~~

Only the *Authority* resource can be changed 'manually' the others are only modified by the skills.
Consequently, we just need two events: one to give the authority to the skillset and another one to give the authority to the teleop.

.. code-block:: RobotLanguage
 :linenos:

    event {
        authority_to_skill {
            guard Authority == Teleop
            effect Authority -> Skill
        }
        authority_to_teleop {
            effect Authority -> Teleop
        }
    }

Skill
~~~~~

The first skill objective is to move the turtle forward to a specific distance and with a specific speed.
Thus, its inputs are *distance* and *speed*.
The turtle can only move if it is not already moving (precondition *not_moving*) and if it has the authority (precondition *has_authority*).
When the skill starts the turtle's *Move* status changes to *Moving*.
If the teleop takes the authority, then the skill will end with an invariant failure.

.. code-block:: RobotLanguage
 :linenos:

    skill MoveForward {
        input {
            distance: Float
            speed: Float
        }
        precondition {
            has_authority: Authority == Skill
            not_moving: Move == NotMoving
        }
        start Move -> Moving
        invariant has_authority {
            guard Authority == Skill
            effect Move -> NotMoving
        }
        interrupt {
            interrupting false
            effect Move -> NotMoving
        }
        success completed {
            effect Move -> NotMoving
        }
    }

The *rotate* skill is similar to the *move* skill.

.. code-block:: RobotLanguage
 :linenos:

    skill RotateAngle {
        input {
            angle: Float
            speed: Float
        }
        precondition {
            has_authority: Authority == Skill
            not_rotating: Rotate == NotRotating
        }
        start Rotate -> Rotating
        invariant has_authority {
            guard Authority == Skill
            effect Rotate -> NotRotating
        }
        interrupt {
            interrupting false
            effect Rotate -> NotRotating
        }
        success completed {
            effect Rotate -> NotRotating
        }
    }

Complete Model
~~~~~~~~~~~~~~

The complete model can be found `here`_.

.. _here: ../files/turtle.rl

Code Generation
---------------

Create Workspace
~~~~~~~~~~~~~~~~

At first, let's create an empty workspace:

.. code-block:: bash

    mkdir -p turtle_ws/src

In order to generate the corresponding ROS2 code, we need to define the different types used by the skillset.
In our case the *Float* type and the *Pose*.
Moreover, it is mandatory to indicate the destination folder.
This information is given to robot language tool by a `JSON file`_:

.. _JSON file: ../files/turtle.json

.. code-block:: json
 :linenos:

    {
        "folder": "path_to/turtle_ws/src",
        "type": [
            {
                "name": "Float",
                "idl": "float64"
            },
            {
                "name": "Pose",
                "package": "geometry_msgs",
                "message": "Pose2D"
            }
        ]
    }


Generate Skillset
~~~~~~~~~~~~~~~~~

Then, we can generate the skillset code with the following command:

.. code-block:: bash

    python3 -m robot_language turtle.rl -g turtle.json

This will generate two packages: *turtle_skillset_interfaces* and *turtle_skillset*.
The package *turtle_skillset_interfaces* contains all the messages used to interact with the skillset.

The package *turtle_skillset* contains the class *TurtleNode* that implements the skillset's generic behavior.
In the file `Node.hpp`_ you will find all the interesting methods to implement your project.

.. _Node.hpp: ../files/Node.hpp

Generate User Package
~~~~~~~~~~~~~~~~~~~~~

Once the generic skillset behavior generated, we want to create a specific implementation of the skillset for 'Donatello'.
Thus, we will generate and empty package using the generic *turtle_skillset* package.

.. code-block:: bash

    python3 -m robot_language turtle.rl -g turtle.json -p donatello

The command above will create a package named 'donatello' containing a single node that exetends the generic *turtle_skillset* node.
The specific behavior of donatello will be added in this project.

Preliminary tests
~~~~~~~~~~~~~~~~~

Once generated, the new project can be build and run.
Obviously, its specific behavior (related to turtlesim) is not yet defined, but the generic behavior of the skillset is fully functional.

.. code-block:: bash

    colcon build
    source install/setup.bash
    ros2 run donatello donatello_node


With another terminate you can watch the skillset status.

.. code-block:: bash

    ros2 topic echo /donatello_node/turtle_skillset/status


The skillset status is updated each time inner data, skill, or resource changes.
In our case, the skillset is not doing anything, thus we have to ask to 'get_status':

.. code-block:: bash

    ros2 topic pub -1 /donatello_node/turtle_skillset/status_request std_msgs/msg/Empty "{}"

Then the previous terminal shows the inner status of the skillset:

.. code-block:: bash

    stamp:
      sec: 1670607563
      nanosec: 749749793
    resources:
    - name: Authority
      state: Teleop
    - name: Move
      state: NotMoving
    - name: Rotate
      state: NotRotating
    skill_move_forward:
      name: MoveForward
      id: ''
      state: 0
      input:
        distance: 0.0
        speed: 0.0
    skill_rotate_angle:
      name: RotateAngle
      id: ''
      state: 0
      input:
        angle: 0.0
        speed: 0.0
    info: best turtle ever
    ---


Skillset Implementation
-----------------------

ROS2 package configuration
~~~~~~~~~~~~~~~~~~~~~~~~~~

Package dependency
^^^^^^^^^^^^^^^^^^

The 'donatello' project needs to use the turtlesim package in order to control the turtle.
Thus, we'll have to add the dependency in the 'package.xml' file:

.. code-block:: xml

  <exec_depend>std_srvs</exec_depend>
  <exec_depend>turtlesim</exec_depend>

The package configuration file can be found here: `package.xml`_.

.. _package.xml: ../files/package.xml


Cmake dependency
^^^^^^^^^^^^^^^^

We also need to add those dependencies in the 'CMakeLists.txt' file :

.. code-block:: shell

    ...
    find_package(std_srvs REQUIRED)
    find_package(turtlesim REQUIRED)
    ...
    ament_target_dependencies(donatello
        rclcpp std_msgs turtle_skillset_interfaces turtle_skillset
        std_srvs turtlesim
    )  
    ...
    ament_target_dependencies(donatello_debug
        rclcpp std_msgs turtle_skillset_interfaces turtle_skillset
        std_srvs turtlesim
    )
    ... 


The cmake configuration file can be found here: `CMakeLists.txt`_.

.. _CMakeLists.txt: ../files/CMakeLists.txt


Skillset Data
~~~~~~~~~~~~~


The skillset 'turtle' contains a data representing the pose of the turtle.
The turtlesim node produces a pose message each time the turtle moves.
But the message type produced by the turtlesim node is not the one expected.
Thus, in order to update properly the skillset data we need to:

1. subscribe to the turtlesim pose topic of the donatello turtle;
2. convert the pose into the expected message type;
3. update the corresponding pose of the skillset data.

Subscribe to the turtlesim pose
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^

First, we need to include the turtlesim pose message:

.. code-block:: C++

    #include "turtlesim/msg/pose.hpp"

Then, we need to declare a private field for the subscription:

.. code-block:: C++

    rclcpp::Subscription<turtlesim::msg::Pose>::SharedPtr turtlesim_pose_sub_;

Then, we need a callback for this subscription:

.. code-block:: C++

    void pose_callback_(turtlesim::msg::Pose::UniquePtr msg);

Finally, we need to initialize the subscription field properly in the 'DonatelloNode' constructor:

.. code-block:: C++

    turtlesim_pose_sub_ = this->create_subscription<turtlesim::msg::Pose>(
        "/donatello/pose", 10, [this](turtlesim::msg::Pose::UniquePtr msg)
        { this->pose_callback_(std::move(msg)); });


Convert the Pose
^^^^^^^^^^^^^^^^

In the pose callback, we need to read the incoming pose and translate it into a 'geometry_msgs' Pose2D message:

.. code-block:: C++

    void DonatelloNode::pose_callback_(turtlesim::msg::Pose::UniquePtr msg)
    {
        geometry_msgs::msg::Pose2D data;
        data.x = msg->x;
        data.y = msg->x;
        data.theta = msg->theta;
    ...

Update the Skillset Data
^^^^^^^^^^^^^^^^^^^^^^^^

Each skillset data is published when it is updated (and periodically if a period is specified in the robot language model file).
To prevent the data from being published when the turtle is not moving, we need to check whether the pose changed before updating the data.

.. code-block:: C++

    ...
        geometry_msgs::msg::Pose2D old = this->get_data_pose().value;
        if (old.x != data.x || old.y != data.y || old.theta != data.theta)
        {
            this->set_data_pose(data);
        }
    }

Move Forward Skill
~~~~~~~~~~~~~~~~~~

Both the *'move forward'* and the *'rotate angle'* skills are implemented using the *'teleport'* service provided by the turtlesim node.
The behavior of this skill is simple: while running, the skill periodically moves the turtle according to its speed input.
If the total distance is reached, the skill terminates with a success.

Thus, we need to:

1. create a client for the teleport service;
2. create a timer for the periodic behavior of the skill;
3. move the turtle until the required distance is reached.

Teleport Client
^^^^^^^^^^^^^^^

We need to declare a private field for the teleport service client:


.. code-block:: C++

    rclcpp::Client<turtlesim::srv::TeleportRelative>::SharedPtr teleport_relative_;

And initialize it in the skillset constructor:

.. code-block:: C++

    teleport_relative_ = this->create_client<turtlesim::srv::TeleportRelative>("/donatello/teleport_relative");

To simplify the code we add a private method to teleport the turtle.
Its main objective is to call the teleport service according to the specified parameters:

.. code-block:: C++

    void teleport_(double linera, double angular);
    void teleport_callback_(rclcpp::Client<turtlesim::srv::TeleportRelative>::SharedFutureWithRequest future);

.. code-block:: C++

    void DonatelloNode::teleport_(double linear, double angular)
    {
        auto request = std::make_shared<turtlesim::srv::TeleportRelative::Request>();
        request->linear = linear;
        request->angular = angular;

        while (!this->teleport_relative_->wait_for_service(1s))
        {
            if (!rclcpp::ok())
            {
                RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), "Interrupted while waiting for the service. Exiting.");
                return;
            }
            RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "service not available, waiting again...");
        }

        auto result = this->teleport_relative_->async_send_request(request, [this](rclcpp::Client<turtlesim::srv::TeleportRelative>::SharedFutureWithRequest future)
                                                                   { this->teleport_callback_(future); });
    }

    void DonatelloNode::teleport_callback_(rclcpp::Client<turtlesim::srv::TeleportRelative>::SharedFutureWithRequest future)
    {
        (void)future;
    }

Periodic Timer
^^^^^^^^^^^^^^

Since the behavior of the skill is periodic, we need to create a periodic *'wall timer'*.
Thus, we have to declare the corresponding attribute.
Additionnaly, we must add a variable memorizing the current distance done by Donatello:

.. code-block:: C++

    double move_forward_distance_;
    rclcpp::TimerBase::SharedPtr move_forward_timer_;

A callback for the wall timer is also needed :

.. code-block:: C++

    void skill_move_forward_callback_();

It is important to notice that the skill is not running at start.
That's why we have to cancel the corresponding timer in the constructor.

.. code-block:: C++

    move_forward_distance_ = 0.0;
    move_forward_timer_ = this->create_wall_timer(1s, [this]()
                                                  { this->skill_move_forward_callback_(); });
    move_forward_timer_->cancel();

The timer starts when the skill starts, in the *on_start* method of the skill.
First, uncomment the method to override it:

.. code-block:: C++

    void skill_move_forward_on_start();

Then, we can define start the timer:

.. code-block:: C++

    void DonatelloNode::skill_move_forward_on_start()
    {
        RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "skill_move_forward_on_start");

        auto input = this->skill_move_forward_input();
        this->move_forward_distance_ = input->distance;

        this->move_forward_timer_->reset();
    }

It is also mandatory to stop the timer of the skill when it ends.
Uncomment those hooks:

.. code-block:: C++

    void skill_move_forward_invariant_has_authority_hook();
    void skill_move_forward_interrupt_hook();

And stop the timer:

.. code-block:: C++

    void DonatelloNode::skill_move_forward_invariant_has_authority_hook()
    {
        move_forward_timer_->cancel();
    }

    void DonatelloNode::skill_move_forward_interrupt_hook()
    {
        move_forward_timer_->cancel();
    }

Finally, we have to define the periodic behavior of the *move_forward* skill in the timer callback function:

.. code-block:: C++

    void DonatelloNode::skill_move_forward_callback_()
    {
        RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "skill_move_forward_timer");
        auto input = this->skill_move_forward_input();

        this->teleport_(input->speed, 0.0);
        move_forward_distance_ -= input->speed;
        if (move_forward_distance_ <= 0.0)
        {
            move_forward_timer_->cancel();
            this->skill_move_forward_success_completed();
        }
    }

Rotate Angle Skill
~~~~~~~~~~~~~~~~~~

The implementation of the *rotate_angle* skill is similar to the *move_forward* skill.
Thus, it will not be detailed.

Authority Resource
~~~~~~~~~~~~~~~~~~

The main objective of the Authority resource is to indicate which entity can move/rotate the turtle.
At start the resource is set to *Teleop*, thus if we want to allow the node to move/rotate the turtle, the skill we must acquire the resource.
It can be done using the event '*authority_to_skill*' by throwing the corresponding topic.

.. code-block:: bash

    ros2 topic pub -1 /donatello_node/turtle_skillset/event_request turtle_skillset_interfaces/msg/EventRequest "{id: '', name: 'authority_to_skill'}"

The authority can also be given to the *Teleop* with the event '*authority_to_teleop*' by the same operation.
But it would be more interesting if the resource can be automatically set to *Teleop* each time a turtlesim teleop command is sent.

To achieve this goal we must :
1. subscribe to the '*cmd_vel*' topic of the turtle;
2. call the '*authority_to_teleop*' event.

.. code-block:: C++

    turtlesim_cmd_vel_sub_ = this->create_subscription<geometry_msgs::msg::Twist>(
        "/donatello/cmd_vel", 10, [this](geometry_msgs::msg::Twist::UniquePtr msg)
        { this->cmd_vel_callback_(std::move(msg)); });

    ...

    void cmd_vel_callback_(geometry_msgs::msg::Twist::UniquePtr msg);

.. code-block:: C++

    void DonatelloNode::cmd_vel_callback_(geometry_msgs::msg::Twist::UniquePtr msg)
    {
        (void)msg;
        this->event_authority_to_teleop();
    }    

Conclusion
~~~~~~~~~~

We have successfully implemented our skillset.
The C++ header file of the node can be found here `Node.hpp`_.
The C++ source file of the node can be found here `Node.cpp`_.

.. _Node.hpp: ../files/Node.hpp
.. _Node.cpp: ../files/Node.cpp


Running the skillset
--------------------

Prepare the workspace
~~~~~~~~~~~~~~~~~~~~~

First 'build and install' the workspace:

.. code-block:: bash

    colcon build
    source install/setup.bash

Prepare turtlesim
~~~~~~~~~~~~~~~~~

In another terminal we need to launch the turtlesim node, remove the default turtle and add the Donatello :

.. code-block:: bash

    ros2 run turtlesim turtlesim_node

In another terminal:

.. code-block:: bash

    ros2 service call /kill turtlesim/srv/Kill "name: turtle1"
    ros2 service call /kill turtlesim/srv/Kill "{name: donatello}"
    ros2 service call /clear std_srvs/srv/Empty "{}"
    ros2 service call /spawn turtlesim/srv/Spawn "{x: 5.0, y: 5.0, name: 'donatello'}"
    ros2 service call /donatello/set_pen turtlesim/srv/SetPen "{r: 75, g: 0, b: 130, width: 5}"    

The turtlesim display must be the following one:

.. figure:: ./figures/turtle_1.png
   :alt: Principle
   :scale: 75%
   :align: center

Run the Skillset node
~~~~~~~~~~~~~~~~~~~~~

In another terminal we can launch the skillset node we defined (don't forget to source the install script: 'source install/setup.bash').

.. code-block:: bash
    
    ros2 run donatello donatello_node

Once started we can see all the topics provided by the skillset:

.. code-block:: bash
    
    ros2 topic list
    ...
    /donatello_node/turtle_skillset/data/pose
    /donatello_node/turtle_skillset/data/pose/request
    /donatello_node/turtle_skillset/data/pose/response
    /donatello_node/turtle_skillset/event_request
    /donatello_node/turtle_skillset/event_response
    /donatello_node/turtle_skillset/skill/move_forward/interrupt
    /donatello_node/turtle_skillset/skill/move_forward/request
    /donatello_node/turtle_skillset/skill/move_forward/response
    /donatello_node/turtle_skillset/skill/rotate_angle/interrupt
    /donatello_node/turtle_skillset/skill/rotate_angle/request
    /donatello_node/turtle_skillset/skill/rotate_angle/response
    /donatello_node/turtle_skillset/status
    /donatello_node/turtle_skillset/status_request

We can track the position of Donatello with the pose data of the skillset :

.. code-block:: bash
    
    ros2 topic echo /donatello_node/turtle_skillset/data/pose

As specified in the model, the pose is published every second if Donatello is not moving.

Skillset Status
^^^^^^^^^^^^^^^

We can observe the skillset status while sending commands:

.. code-block:: bash
    
    ros2 topic echo /donatello_node/turtle_sllset/status

In another terminal, let's take the Authority resource by sending the corresponding event request :

.. code-block:: bash

    ros2 topic pub -1 /donatello_node/turtle_skillset/event_request turtle_skillset_interfaces/msg/EventRequest "{id: '', name: 'authority_to_skill'}"

As a result, we can observe that the Authority resource is now set to *Skill* and none of the skills are running.

.. code-block:: bash

    stamp:
      sec: 1671527463
      nanosec: 575236660
    resources:
    - name: Authority
      state: Skill
    - name: Move
      state: NotMoving
    - name: Rotate
      state: NotRotating
    skill_move_forward:
      name: MoveForward
      id: ''
      state: 0
      input:
        distance: 0.0
        speed: 0.0
    skill_rotate_angle:
      name: RotateAngle
      id: ''
      state: 0
      input:
        angle: 0.0
        speed: 0.0
    info: best turtle ever

Running Skill
^^^^^^^^^^^^^

Now, we want to start the 'move forward' skill.
In order to see the results of the request we can observe the corresponding topic:

.. code-block:: bash

    ros2 topic echo /donatello_node/turtle_skillset/skill/move_forward/response 
  
The request can be sent :

.. code-block:: bash

    ros2 topic pub -1 /donatello_node/turtle_skillset/skill/move_forward/request turtle_skillset_interfaces/msg/SkillMoveForwardRequest "{id: '', input: { distance: 2.0, speed: 0.2 }}"

Donatello starts moving and 10 seconds later, the skillset is completed successfully:

.. code-block:: bash

    id: ''
    result: 0
    has_authority: true
    not_moving: true
    name: completed
    effect: true
    postcondition: true
    ---

The turtlesim display must be the following one:

.. figure:: ./figures/turtle_2.png
   :alt: Principle
   :scale: 75%
   :align: center

Teleop Authority
^^^^^^^^^^^^^^^^

The Authority resource is used in the invariants of the skills.
Consequently, if the resource is set to 'Teleop', the skill must be stopped with an invariant failure.
To test this behavior we ask Donetello to rotate slowly for a long time.
In another terminal we will launch the turtlesim_teleop node.
One the telop sends a command, we expect the skill to be stopped.

Get skill response:

.. code-block:: bash

    ros2 topic echo /donatello_node/turtle_skillset/skill/rotate_angle/response

Run the turtlesim teleop node :

.. code-block:: bash

    ros2 run turtlesim turtle_teleop_key --ros-args --remap turtle1/cmd_vel:=donatello/cmd_vel

Start the rotate skill :

.. code-block:: bash

    ros2 topic pub -1 /donatello_node/turtle_skillset/skill/rotate_angle/request turtle_skillset_interfaces/msg/SkillRotateAngleRequest "{id: '', input: { angle: 314, speed: 0.5}}"

The turtle starts rotating.
Then we can press any key of in the telop terminal.
The skill is stopped and the response ('result: 5') indicates an invariant failure. The name of the invariant that failed is 'has_authority' ('name: has_authority').

.. code-block:: bash

    id: ''
    result: 5
    has_authority: true
    not_rotating: true
    name: has_authority
    effect: true
    postcondition: true
    ---