Using a planning controller

Overview

In the previous examples, we have started the knowledge of PlanSys2, and we have calculated and executed plans interactively using the terminal. Obviously, when PlanSys2 is embedded in an application, interactive execution is not convenient.

A planning controller is a program that: * Initiates, consults, and updates knowledge. * Sets goals. * Makes requests to execute the plan.

This program controls the robot’s mission. It is usually implemented as a finite state machine and decides what the next goal to be achieved is.

In this tutorial, we will integrate PlanSys2 and Nav2 to make a robot patrol its environment. There are 5 waypoints: wp_control, wp_1, wp_2, wp_3, and wp_4. Each one has different coordinates.

  • wp_control is the junction to which all other waypoints are connected. A patrol always goes through wp_control, since the waypoints are not connected to each other.

  • Patrolling a waypoint consists of moving to the waypoint and, once there, rotating for a few seconds to perceive the environment.

  • During a patrol, all waypoints will be patrolled. Once patrolled, the patrol will begin again.

In the tutorial’s first steps, we will use a test component that simulates the navigation process, and at the end of the tutorial, we will launch the real Nav2 and a simulator.

Tutorial Steps

0- Requisites

If you haven’t done yet, 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 with a node that simulates de action of navigation. So, the execution of Nav2 will not take place in the first steps of this tutorial:

    ros2 launch plansys2_patrol_navigation_example patrol_example_fakesim_launch.py

  • Open a new terminal and run:

    ros2 run plansys2_patrol_navigation_example patrolling_controller_node

This will start the mission. You will see in the first terminal how the plans are calculated and how the actions are executed. In the last terminal you will see the execution of the actions too.

2- Patrol action performer

The action patrol (ros2_planning_system_examples/plansys2_patrol_navigation_example/src/patrol_action_node.cpp) contains the actions that makes the robot spin.

 1 class Patrol : public plansys2::ActionExecutorClient
 2 {
 3 public:
 4   Patrol()
 5   : plansys2::ActionExecutorClient("patrol", 1s)
 6   {
 7   }
 8
 9   rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn
10   on_activate(const rclcpp_lifecycle::State & previous_state)
11   {
12     progress_ = 0.0;
13
14     cmd_vel_pub_ = this->create_publisher<geometry_msgs::msg::Twist>("/cmd_vel", 10);
15     cmd_vel_pub_->on_activate();
16
17     return ActionExecutorClient::on_activate(previous_state);
18   }
19
20   rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn
21   on_deactivate(const rclcpp_lifecycle::State & previous_state)
22   {
23     cmd_vel_pub_->on_deactivate();
24
25     return ActionExecutorClient::on_deactivate(previous_state);
26   }
27
28 private:
29   void do_work()
30   {
31     if (progress_ < 1.0) {
32       progress_ += 0.1;
33
34       send_feedback(progress_, "Patrol running");
35
36       geometry_msgs::msg::Twist cmd;
37       cmd.linear.x = 0.0;
38       cmd.linear.y = 0.0;
39       cmd.linear.z = 0.0;
40       cmd.angular.x = 0.0;
41       cmd.angular.y = 0.0;
42       cmd.angular.z = 0.5;
43
44       cmd_vel_pub_->publish(cmd);
45     } else {
46       geometry_msgs::msg::Twist cmd;
47       cmd.linear.x = 0.0;
48       cmd.linear.y = 0.0;
49       cmd.linear.z = 0.0;
50       cmd.angular.x = 0.0;
51       cmd.angular.y = 0.0;
52       cmd.angular.z = 0.0;
53
54       cmd_vel_pub_->publish(cmd);
55
56       finish(true, 1.0, "Patrol completed");
57     }
58   }
59
60   float progress_;
61
62   rclcpp_lifecycle::LifecyclePublisher<geometry_msgs::msg::Twist>::SharedPtr cmd_vel_pub_;
63 };
64
65 int main(int argc, char ** argv)
66 {
67   rclcpp::init(argc, argv);
68   auto node = std::make_shared<Patrol>();
69
70   node->set_parameter(rclcpp::Parameter("action", "patrol"));
71   node->trigger_transition(lifecycle_msgs::msg::Transition::TRANSITION_CONFIGURE);
72
73   rclcpp::spin(node->get_node_base_interface());
74
75   rclcpp::shutdown();
76
77   return 0;
78 }

This action runs with a frequency of 1Hz when activated. In each step, it increases its progress by 10% (line 32), sending speed commands to the robot through the topic /cmd_vel that make it spin (lines 36-44). When it completes the action, it stops the robot (lines 47-54). In each cycle it sends a feedback (line 34) or declares that the action has already finished (line 56).

3- Move action performer

The action move (ros2_planning_system_examples/plansys2_patrol_navigation_example/src/move_action_node.cpp) contains the actions that sends navigation requests to Nav2 using the ROS2 action navigate_to_pose. This example is quite complex if you are not familiar with ROS2 actions, so we will not enter in details here, only selected pieces of code.

 1  MoveAction()
 2  : plansys2::ActionExecutorClient("move", 500ms)
 3  {
 4    geometry_msgs::msg::PoseStamped wp;
 5    wp.header.frame_id = "/map";
 6    wp.pose.position.x = 0.0;
 7    wp.pose.position.y = -2.0;
 8    wp.pose.position.z = 0.0;
 9    wp.pose.orientation.x = 0.0;
10    wp.pose.orientation.y = 0.0;
11    wp.pose.orientation.z = 0.0;
12    wp.pose.orientation.w = 1.0;
13    waypoints_["wp1"] = wp;
14
15    wp.pose.position.x = 1.8;
16    wp.pose.position.y = 0.0;
17    waypoints_["wp2"] = wp;
18
19    wp.pose.position.x = 0.0;
20    wp.pose.position.y = 2.0;
21    waypoints_["wp3"] = wp;
22
23    wp.pose.position.x = -0.5;
24    wp.pose.position.y = -0.5;
25    waypoints_["wp4"] = wp;
26
27    wp.pose.position.x = -2.0;
28    wp.pose.position.y = -0.4;
29    waypoints_["wp_control"] = wp;
30
31    using namespace std::placeholders;
32    pos_sub_ = create_subscription<geometry_msgs::msg::PoseWithCovarianceStamped>(
33      "/amcl_pose",
34      10,
35      std::bind(&MoveAction::current_pos_callback, this, _1));
36  }
37
38  ...
39  private:
40    std::map<std::string, geometry_msgs::msg::PoseStamped> waypoints_;

The coordinates of each waypoint are initialized and inserted in waypoints_.

 1 rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn
 2 on_activate(const rclcpp_lifecycle::State & previous_state)
 3 {
 4   ...
 5
 6   auto wp_to_navigate = get_arguments()[2];
 7   goal_pos_ = waypoints_[wp_to_navigate];
 8   navigation_goal_.pose = goal_pos_;
 9
10   ...
11
12   send_goal_options.feedback_callback = [this](
13     NavigationGoalHandle::SharedPtr,
14     NavigationFeedback feedback) {
15       send_feedback(
16         std::min(1.0, std::max(0.0, 1.0 - (feedback->distance_remaining / dist_to_move))),
17         "Move running");
18     };
19
20   send_goal_options.result_callback = [this](auto) {
21       finish(true, 1.0, "Move completed");
22     };
23
24  ...
25 }

  • If we execute the action (move leia wp_control wp_1), the third argument (get_arguments()[2]) is the waypoint to navigate, wp_1. We use this id to get the coordinate from waypoints_ for sending it in the Nav2 action.

  • When receiving feedback from the Nav2 ROS2 action, we send feedback about the execution of the move action.

  • When Nav2 considers the navigation complete, we call finish to finalize the execution of move action.

3- Mission controller

The mission controller (ros2_planning_system_examples/plansys2_patrol_navigation_example/src/patrolling_controller_node.cpp) initializes the knowledge in PlanSys2 and implements a FSM to run patrolling plans. It uses four PlanSys2 clients to interact with PlanSys2:

void init()
  {
    domain_expert_ = std::make_shared<plansys2::DomainExpertClient>(shared_from_this());
    planner_client_ = std::make_shared<plansys2::PlannerClient>(shared_from_this());
    problem_expert_ = std::make_shared<plansys2::ProblemExpertClient>(shared_from_this());
    executor_client_ = std::make_shared<plansys2::ExecutorClient>(shared_from_this());
    init_knowledge();
  }

private:
  std::shared_ptr<plansys2::DomainExpertClient> domain_expert_;
  std::shared_ptr<plansys2::PlannerClient> planner_client_;
  std::shared_ptr<plansys2::ProblemExpertClient> problem_expert_;
  std::shared_ptr<plansys2::ExecutorClient> executor_client_;

Using these clients, we can add instances and predicates to PlanSys2:

void init_knowledge()
{
  problem_expert_->addInstance(plansys2::Instance{"r2d2", "robot"});
  problem_expert_->addInstance(plansys2::Instance{"wp_control", "waypoint"});
  problem_expert_->addInstance(plansys2::Instance{"wp1", "waypoint"});
  problem_expert_->addInstance(plansys2::Instance{"wp2", "waypoint"});
  problem_expert_->addInstance(plansys2::Instance{"wp3", "waypoint"});
  problem_expert_->addInstance(plansys2::Instance{"wp4", "waypoint"});

  problem_expert_->addPredicate(plansys2::Predicate("(robot_at r2d2 wp_control)"));
  problem_expert_->addPredicate(plansys2::Predicate("(connected wp_control wp1)"));
  problem_expert_->addPredicate(plansys2::Predicate("(connected wp1 wp_control)"));
  problem_expert_->addPredicate(plansys2::Predicate("(connected wp_control wp2)"));
  problem_expert_->addPredicate(plansys2::Predicate("(connected wp2 wp_control)"));
  problem_expert_->addPredicate(plansys2::Predicate("(connected wp_control wp3)"));
  problem_expert_->addPredicate(plansys2::Predicate("(connected wp3 wp_control)"));
  problem_expert_->addPredicate(plansys2::Predicate("(connected wp_control wp4)"));
  problem_expert_->addPredicate(plansys2::Predicate("(connected wp4 wp_control)"));
}

In the step method (called at 5Hz) there is the implementation of the FSM. Each switch case contains: * The code to execute in the state

case PATROL_WP1:
  {
    auto feedback = executor_client_->getFeedBack();

    for (const auto & action_feedback : feedback.action_execution_status) {
      std::cout << "[" << action_feedback.action << " " <<
        action_feedback.completion * 100.0 << "%]";
    }

  • The condition to transitate to another state and the code executed before the start of the new state. The important part here is how we set the new goal for the new state (using setGoal), how we compute a new plan, and how we call the executor to execute the plan to achieve it (using the non-blocking call executePlan):

    if (executor_client_->getResult().value().success) {
  std::cout << "Plan successful finished " << std::endl;

  // Cleanning up
  problem_expert_->removePredicate(plansys2::Predicate("(patrolled wp1)"));

  // Set the goal for next state
  problem_expert_->setGoal(plansys2::Goal("(and(patrolled wp2))"));

  // Compute the plan
  auto domain = domain_expert_->getDomain();
  auto problem = problem_expert_->getProblem();
  auto plan = planner_client_->getPlan(domain, problem);

  // Execute the plan
  if (executor_client_->executePlan(plan.value())) {
    state_ = PATROL_WP2;
  }
} else {
  ...
}

4- Running the example with Nav2

  • Make sure that Nav2 is correctly installed and it is working.

  • Open a new terminal and run PlanSys2 with a node that simulates de action of navigation. So, the execution of Nav2 will not take place in the first steps of this tutorial:

    ros2 launch plansys2_patrol_navigation_example patrol_example_launch.py

  • Open a new terminal and run:

    ros2 run plansys2_patrol_navigation_example patrolling_controller_node