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 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 a given _geometric_ pose. The Nav2's NavigateToPoseNavigator becomes then the functional layer of our TurtleBot3 in this simple demo.

We provide a virtual machine that comes with all the software to execute the example already generated and built.

Run the example

There are three main steps to execute the demo with ROS 2, which are shortly described below.

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 that the value for parameter default_bt_xml_filename assumes that you configured Papyrus for Robotics to work in the ws-nav2-example-ready workspace in the virtual machine. You may need to change it accordingly if you work in another workspace.)

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