Tutorial

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

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.

   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.

   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.

   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.

   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.

   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.

Code Generation

Create Workspace

At first, let’s create an empty workspace:

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:

   {
       "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:

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.

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.

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.

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

With another terminate you can watch the skillset status.

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’:

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:

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:

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

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

Cmake dependency

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

...
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.

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:

#include "turtlesim/msg/pose.hpp"

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

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

Then, we need a callback for this subscription:

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

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

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:

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.

...
    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:

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

And initialize it in the skillset constructor:

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:

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

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:

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

A callback for the wall timer is also needed :

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.

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:

void skill_move_forward_on_start();

Then, we can define start the timer:

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:

void skill_move_forward_invariant_has_authority_hook();
void skill_move_forward_interrupt_hook();

And stop the timer:

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:

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.

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.

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);

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.

Running the skillset

Prepare the workspace

First ‘build and install’ the workspace:

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 :

ros2 run turtlesim turtlesim_node

In another terminal:

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:

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’).

ros2 run donatello donatello_node

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

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 :

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:

ros2 topic echo /donatello_node/turtle_sllset/status

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

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.

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:

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

The request can be sent :

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:

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

The turtlesim display must be the following one:

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:

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

Run the turtlesim teleop node :

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

Start the rotate skill :

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’).

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