The first planning package

Overview

It is common for PlanSys2 to have a package that contains all the elements to plan and execute in a domain:

  • A PDDL model.

  • A node that implements each action, probably in different executables.

  • A launcher that executes the actions and launches PlanSys2 with the appropriate domain.

  • Possibly (not in this tutorial) a node that initiates knowledge and manages when to execute a plan and its goals.

In this tutorial we are going to run again and to analyze the package of the simple example shown in: ref: getting_started.

Tutorial Steps

0- Requisites

Clone in your workspace and build the PlanSys2 examples

cd <your_workspace>
git clone -b <ros2-distro>-devel https://github.com/IntelligentRoboticsLabs/ros2_planning_system_examples.git src
colcon build --symlink-install
rosdep install --from-paths src --ignore-src -r -y
colcon build --symlink-install

1- Running the example

  • Open a new terminal and run PlanSys2:

    ros2 launch plansys2_simple_example plansys2_simple_example_launch.py

    This launch PlanSys2 this simple PDDL domain, and some processes that implements a fake version of the domain’s actions.

  • Open a new terminal and run the PlanSys2 terminal:

    ros2 run plansys2_terminal plansys2_terminal

  • In the PlanSys2 terminal shell, copy and paste the next commands to init the knowledge of the system and set the goal:

    set instance leia robot
set instance entrance room
set instance kitchen room
set instance bedroom room
set instance dinning room
set instance bathroom room
set instance chargingroom room

set predicate (connected entrance dinning)
set predicate (connected dinning entrance)

set predicate (connected dinning kitchen)
set predicate (connected kitchen dinning)

set predicate (connected dinning bedroom)
set predicate (connected bedroom dinning)

set predicate (connected bathroom bedroom)
set predicate (connected bedroom bathroom)

set predicate (connected chargingroom kitchen)
set predicate (connected kitchen chargingroom)

set predicate (charging_point_at chargingroom)
set predicate (battery_low leia)
set predicate (robot_at leia entrance)

set goal (and(robot_at leia bathroom))

  • In the PlanSys2 terminal shell, run the plan:

    run

In the PlanSys2 terminal you’ll be able to see the plan. In both terminal, you’ll see the current actions executed and its level of completion. As soon as the plan execution finished, the terminal will be available again.

2- Package structure

Go to <your_workspace>/ros2_planning_system_examples/plansys2_simple_example. This is the ROS2 package that contains the example of this tutorial. The structure is the usual of a ROS2 package, with a package.xml and a CMakeLists.txt as usual. Besides, we have the next directories:

  • pddl The directory with the PDDL file that contains the domain. It is the same of the ref: terminal_usage.

  • launch It contains the launcher of this example:

  • src It contains the implementation of the three actions of the domain.

3- Actions implementation

The actions in the domain are move, charge and ask_charge. It will contains a “fake” implementation. Each node that implements an action is called action performer. Each action will take some seconds to execute, only incrementing a progress value and displaying it in the terminal. Each action in PlanSys2 is a managed node , also known as lifecycle node. When active, it will iterativelly call a function to perform the job. Let’s analyze the code of move action in src/move_action_node.cpp:

 1class MoveAction : public plansys2::ActionExecutorClient
 2{
 3public:
 4  MoveAction()
 5  : plansys2::ActionExecutorClient("move", 250ms)
 6  {
 7    progress_ = 0.0;
 8  }
 9
10private:
11  void do_work()
12  {
13    if (progress_ < 1.0) {
14      progress_ += 0.02;
15      send_feedback(progress_, "Move running");
16    } else {
17      finish(true, 1.0, "Move completed");
18
19      progress_ = 0.0;
20      std::cout << std::endl;
21    }
22
23    std::cout << "\r\e[K" << std::flush;
24    std::cout << "Moving ... [" << std::min(100.0, progress_ * 100.0) << "%]  " <<
25      std::flush;
26  }
27
28  float progress_;
29};
30
31int main(int argc, char ** argv)
32{
33  rclcpp::init(argc, argv);
34  auto node = std::make_shared<MoveAction>();
35
36  node->set_parameter(rclcpp::Parameter("action", "move"));
37  node->trigger_transition(lifecycle_msgs::msg::Transition::TRANSITION_CONFIGURE);
38
39  rclcpp::spin(node->get_node_base_interface());
40
41  rclcpp::shutdown();
42
43  return 0;
44}

  • MoveAction is a plansys2::ActionExecutorClient (defined here), that inherit from rclcpp_cascade_lifecycle::CascadeLifecycleNode. This is, basically, a rclcpp_lifecycle::LifecycleNode, but with an additional property: it can activate in cascade another rclcpp_cascade_lifecycle::CascadeLifecycleNode when it is active. It’s useful when an action requires to activate a node that process a sensor information. It will only be active while the action that requires its output is active. See this repo for more info.

    : plansys2::ActionExecutorClient("move", 250ms)

This indicates that each 250ms (4Hz) the method do_work() will be called.

  • plansys2::ActionExecutorClient has an API, with these relevant protected methods for the actions:

    const std::vector<std::string> & get_arguments();
void send_feedback(float completion, const std::string & status = "");
void finish(bool success, float completion, const std::string & status = "");

get_arguments() returns the list of arguments of an action. For example, when executing (move leia chargingroom kitchen), it will return this vector of strings: {"leia", "chargingroom", "kitchen"}

This code is sending feedback of its completion. When finished, finish method indicates that the action has finished, and send back if it succesfully finished, the completion value in [0-1] and optional info. Then, the node will pass to inactive state.

if (progress_ < 1.0) {
  progress_ += 0.02;
  send_feedback(progress_, "Move running");
} else {
  finish(true, 1.0, "Move completed");

  progress_ = 0.0;
  std::cout << std::endl;
}

  • The action node, once created, must pass to inactive state to be ready to execute.

    auto node = std::make_shared<MoveAction>();

node->set_parameter(rclcpp::Parameter("action", "move"));
node->trigger_transition(lifecycle_msgs::msg::Transition::TRANSITION_CONFIGURE);

rclcpp::spin(node->get_node_base_interface());

The parameter action sets what action implements the node object.

4- Launcher

The launcher must include the PlanSys2 launcher, selecting the domain, and it runs the executables that implements the PDDL actions:

 1def generate_launch_description():
 2    example_dir = get_package_share_directory('plansys2_simple_example')
 3    namespace = LaunchConfiguration('namespace')
 4
 5    declare_namespace_cmd = DeclareLaunchArgument(
 6        'namespace',
 7        default_value='',
 8        description='Namespace')
 9
10    plansys2_cmd = IncludeLaunchDescription(
11        PythonLaunchDescriptionSource(os.path.join(
12            get_package_share_directory('plansys2_bringup'),
13            'launch',
14            'plansys2_bringup_launch_monolithic.py')),
15        launch_arguments={
16          'model_file': example_dir + '/pddl/simple_example.pddl',
17          'namespace': namespace
18          }.items())
19
20    move_cmd = Node(
21        package='plansys2_simple_example',
22        executable='move_action_node',
23        name='move_action_node',
24        namespace=namespace,
25        output='screen',
26        parameters=[])
27
28    charge_cmd = Node(
29        package='plansys2_simple_example',
30        executable='charge_action_node',
31        name='charge_action_node',
32        namespace=namespace,
33        output='screen',
34        parameters=[])
35
36    ask_charge_cmd = Node(
37        package='plansys2_simple_example',
38        executable='ask_charge_action_node',
39        name='ask_charge_action_node',
40        namespace=namespace,
41        output='screen',
42        parameters=[])   # Create the launch description and populate
43    ld = LaunchDescription()
44
45    ld.add_action(declare_namespace_cmd)
46
47    # Declare the launch options
48    ld.add_action(plansys2_cmd)
49
50    ld.add_action(move_cmd)
51    ld.add_action(charge_cmd)
52    ld.add_action(ask_charge_cmd)
53
54    return ld