Papyrus/customizations/robotics/demos/nav2

Context

This example realizes a simulated mobile robot with battery management use case, inspired by Release 1 of RobMoSys ITP SCOPE.

In the nominal scenario, the mobile robot shall perform a patrolling mission between two known points, fictitiously associated to imaginary "kitchen" and "living" places, to be visited indefinitely one after the other.

Then the behavioral logics is as follow:

• The robot checks if the battery level is below 30% of its capacity or if the battery is charging.
• If the battery level is below 30% and it is not already under charge, the robot goes to the charging station.
• If the battery level is charging and the robot is at the charging station, the robot waits until it is no longer under charge.
• If the battery level is above 30% and it is not under charge, it goes to the current destination foreseen in its patrolling mission.

The following behavior tree corresponds to an executable specification of this behavioral logics

Our mobile robot system is concretely made up of three components:

• the battery driver, which checks (i) whether the current percentage of battery level is above a given minimum, and (ii) whether the battery is currently under charge;
• the mobile robot high-level controller (called AGV in the image); it realizes a motion skill MoveTo, that takes the symbolic name of the desired place to reach, converts it to a geometric pose (if known) and then moves to that pose while avoiding obstacles.
• the sequencer (implicitly associated to a system model, so not explicitly modeled), which executes the behavior tree task and demands to the battery driver and the AGV the execution of their skills.

The simulated robot is a TurtleBot3 and its navigation capabilities are realized by the Nav2 stack of ROS 2. More precisely, we assume the scenario described in the Nav2's Getting Started section and use the algorithms provided by Nav2 to make the robot move to the desired geometric pose.

The Nav2's NavigateToPoseNavigator becomes then the functional layer of our TurtleBot3 in this simple demo. This is an interesting point, as it allows us to show how to integrate existing algorithms without re-developing all the code from scratch and, in particular, how the code-generator of Papyrus for Robotics uses Callback Groups to avoid deadlocks in case of nested action (or even service) calls.

We provide a virtual machine that comes with all the software to execute the example already generated and built, and that can also be used as a "ready" environment to develop ROS 2 applications using Papyrus for Robotics. For this virtual machine, username is “user” and password is “p”. User “user” is allowed to use “sudo”.

Run the example

Video tutorial

Here's a full (<5min.) video tutorial that shows how to run the example using the code shipped in the virtual machine.

Recap of the main steps

As described in the video tutorial, there are three main steps to execute the demo with ROS2. They are shortly recap below and assume that Papyrus for Robotics is up and configured to work in the ws-nav2-example-ready workspace.

Step 1: Prepare the system for execution

• Open a new terminal and activate the functional layer of this demo, which executes Nav2 navigation algorithms in a simulated environment.

  source /opt/ros/humble/setup.bash
  export TURTLEBOT3_MODEL=waffle
  export GAZEBO_MODEL_PATH=$GAZEBO_MODEL_PATH:/opt/ros/humble/share/turtlebot3_gazebo/models
  ros2 launch nav2_bringup tb3_simulation_launch.py headless:=True autostart:=True

As we use Nav2 defaults, Nav2 waits for you to give the robot an approximate starting position. Set the initial pose by clicking the "2D Pose Estimate" button in RViz. Then click bottom-right on the map and set the orientation by dragging forward the green arrow. See the Nav2 Getting Started Guide for full details.

• In Papyrus for Robotics, right-click on the System bringup system model, Robotics→Launch and activate ROS2 code.

Step 2: Configuration and activation of the sequencer (and execution of the task)

At this point, the system battery driver and AGV controller components are ready to be activated by the sequencer, to execute their skills and realize the task. The sequencer though, is still in an unconfigured state. In fact, the sequencer can potentially be able to perform multiple tasks specified as behavior trees and it is up to the user (or an external application) to decide the current task to be run (configuration) and when (activation).

• Open a new terminal and configure the sequencer to execute the patrolling behavior tree (with a 1Hz ticking frequency, as there is no need for more performance in this example)

  source /opt/ros/humble/setup.bash
  source /home/user/devel/ros2_humble_ws/sequencer_ws/install/local_setup.bash
  ros2 param set /SystemSequencer default_bt_xml_filename "/home/user/devel/papyrus-robotics/ws-nav2-example-ready/bringup/models/tasks/Bringup.behaviortreeschema"
  ros2 param set /SystemSequencer bt_loop_duration 1000
  ros2 lifecycle set /SystemSequencer configure

(Note, you may need to change the value for parameter default_bt_xml_filename accordingly, if you work in a workspace different from ws-nav2-example-ready.)

• In the same terminal, activate the sequencer and run the task

  ros2 lifecycle set /SystemSequencer activate
  ros2 action send_goal /SystemSequencer/execute_bt bt_sequencer_msgs/action/ExecuteBt "{}"

The mobile robot arm starts now moving between "kitchen" and "living" places (nominal task).

Step 3: Inject artificial battery drains to test the recovery strategy

To test the recovery strategy in case of low-battery level, you will have to change 2 parameter values of battery component. As a prerequisite, open a new terminal and source /opt/ros/humble/setup.bash

• If the battery level is below 30% and it is not already under charge, the robot goes to the charging station.

  • To Test this behavioral logics type this command in the terminal:

      ros2 param set /battery current_battery_level 20
    

    The robot is clearly not already under charge and, as the above command sets its battery level below 30%, the robot switches from the execution of nominal task to the execution of the recovery behavior — "go to the charging station and wait there".
  • Once the robot is arrived at the charging station, put it under charge

      ros2 param set /battery battery_under_charge true
    

• If the battery level is charging and the robot is at the charging station, the robot waits until it is no longer under charge.

  • To Test this behavioral logics type this command in the terminal:

      ros2 param set /battery current_battery_level 100
    

    Even though the battery level is above 30% (here even 100%), as the robot is still under charge (battery_under_charge is still true), the sequencer does not switch back to execute the nominal task.

• If the battery level is above 30% and it is not under charge, it goes to the current destination foreseen in its patrolling mission.

  • To Test this behavioral logics type this command in the terminal:

      ros2 param set /battery battery_under_charge false
    

    The robot is no longer under charge and the sequencer switches back to the execution of the patrolling mission (nominal task).