Adding simple charging example to plansys2_simple_example to help ill…

plansys2_simple_example/CMakeLists.txt

@@ -26,12 +26,16 @@ ament_target_dependencies(charge_action_node ${dependencies})
 add_executable(ask_charge_action_node src/ask_charge_action_node.cpp)
 ament_target_dependencies(ask_charge_action_node ${dependencies})
 
+add_executable(patrol_action_node src/patrol_action_node.cpp)
+ament_target_dependencies(patrol_action_node ${dependencies})
+
 install(DIRECTORY launch pddl DESTINATION share/${PROJECT_NAME})
 
 install(TARGETS
   move_action_node
   charge_action_node
   ask_charge_action_node
+  patrol_action_node
   ARCHIVE DESTINATION lib
   LIBRARY DESTINATION lib
   RUNTIME DESTINATION lib/${PROJECT_NAME}

plansys2_simple_example/launch/plansys2_simple_example_launch.py

@@ -27,6 +27,12 @@ def generate_launch_description():
     # Get the launch directory
     example_dir = get_package_share_directory('plansys2_simple_example')
     namespace = LaunchConfiguration('namespace')
+    domain = LaunchConfiguration('domain')
+
+    declare_domain_cmd = DeclareLaunchArgument(
+        'domain',
+        default_value='simple_example',
+        description='domain')
 
     declare_namespace_cmd = DeclareLaunchArgument(
         'namespace',
@@ -39,7 +45,7 @@ def generate_launch_description():
             'launch',
             'plansys2_bringup_launch_monolithic.py')),
         launch_arguments={
-          'model_file': example_dir + '/pddl/simple_example.pddl',
+          'model_file': [example_dir, '/pddl/', domain, '.pddl'],
           'namespace': namespace
           }.items())
 
@@ -66,16 +72,28 @@ def generate_launch_description():
         name='ask_charge_action_node',
         namespace=namespace,
         output='screen',
-        parameters=[])   # Create the launch description and populate
+        parameters=[])
+
+    patrol_cmd = Node(
+        package='plansys2_simple_example',
+        executable='patrol_action_node',
+        name='patrol_action_node',
+        namespace=namespace,
+        output='screen',
+        parameters=[])
+
+    # Create the launch description and populate
     ld = LaunchDescription()
 
     ld.add_action(declare_namespace_cmd)
+    ld.add_action(declare_domain_cmd)
 
     # Declare the launch options
     ld.add_action(plansys2_cmd)
 
     ld.add_action(move_cmd)
     ld.add_action(charge_cmd)
     ld.add_action(ask_charge_cmd)
+    ld.add_action(patrol_cmd)
 
     return ld

plansys2_simple_example/pddl/charging_problem.pddl

@@ -0,0 +1,56 @@
+(define (problem charging-problem)
+  (:domain charging)
+
+  ;; Instantiate the objects.
+  (:objects
+    leia - robot
+    entrance kitchen bedroom dining bathroom chargingroom - waypoint
+  )
+
+  (:init
+    ; Define the initial state predicates.
+    (connected entrance dining)
+    (connected dining entrance)
+
+    (connected dining kitchen)
+    (connected kitchen dining)
+
+    (connected dining bedroom)
+    (connected bedroom dining)
+
+    (connected bathroom bedroom)
+    (connected bedroom bathroom)
+
+    (connected chargingroom kitchen)
+    (connected kitchen chargingroom)
+
+    (charger_at chargingroom)
+    (robot_at leia kitchen)
+
+    ; Define the initial functions.
+    (= (speed leia) 5)
+    (= (max_range leia) 10)
+    (= (state_of_charge leia) 20)
+
+    (= (distance entrance dining) 2)
+    (= (distance dining entrance) 2)
+    (= (distance dining kitchen) 2)
+    (= (distance kitchen dining) 2)
+    (= (distance kitchen chargingroom) 1)
+    (= (distance chargingroom kitchen) 1)
+    (= (distance dining bedroom) 2)
+    (= (distance bedroom dining) 2)
+    (= (distance bedroom bathroom) 2)
+    (= (distance bathroom bedroom) 2)
+  )
+
+  (:goal (and
+    (patrolled bathroom)
+    (patrolled kitchen)
+    (patrolled entrance)
+  ))
+
+  (:metric
+    minimize (total-time)
+  )
+)

plansys2_simple_example/pddl/domain_charging.pddl

@@ -0,0 +1,79 @@
+(define (domain charging)
+
+  ;; Specify the features of PDDL that the domain uses.
+  (:requirements :typing :fluents :durative-actions :negative-preconditions)
+
+  ;; Specify the domain's parameter types.
+  (:types robot waypoint - object)
+
+  ;; Specify the domains's predicates.
+  ;; robot_at   - Is ?r (robot) at ?wp (waypoint)?
+  ;; charger_at - Is there a charger at ?wp (waypoint)?
+  ;; connected  - Is ?wp1 (waypoint) connected to ?wp2 (waypoint)?
+  ;; patrolled  - Has ?wp (waypoint) been patrolled?
+  (:predicates
+    (robot_at ?r - robot ?wp - waypoint)
+    (charger_at ?wp - waypoint)
+    (connected ?wp1 ?wp2 - waypoint)
+    (patrolled ?wp - waypoint)
+  )
+
+  ;; Declare static functions that can be used in action expressions.
+  ;; speed(?r)            - speed of ?r (robot)
+  ;; max_range(?r)        - range of ?r (robot) given a fully charged battery
+  ;; state_of_charge(?r)  - available battery of ?r (robot) as a percentage
+  ;; distance(?wp1, ?wp2) - distance between ?wp1 (waypoint) and ?wp2 (waypoint)
+  (:functions
+    (speed ?r - robot)
+    (max_range ?r - robot)
+    (state_of_charge ?r - robot)
+    (distance ?wp1 ?wp2 - waypoint)
+  )
+
+  ;; Move ?r (robot) from ?wp1 (waypoint) to ?wp2 (waypoint).
+  ;; duration = distance(?wp1, ?wp2) / speed(?r)
+  ;; condition - at start -> ?wp1 (waypoint) is connected to ?wp2 (waypoint)
+  ;;             at start -> ?r (robot) is at ?wp1 (waypoint)
+  ;;             at start -> state_of_charge(?r) >= distance(?wp1, ?wp2) / max_range(?r)
+  ;; efect - at start -> ?r (robot) is NOT at ?wp1 (waypoint)
+  ;;         at start -> state_of_charge(?r) -= distance(?wp1, ?wp2) / max_range(?r)
+  ;;         at end   -> ?r (robot) is at ?wp2 (waypoint)
+  (:durative-action move
+    :parameters (?r - robot ?wp1 ?wp2 - waypoint)
+    :duration (= ?duration (/ (distance ?wp1 ?wp2)
+                              (speed ?r)))
+    :condition (and (at start (connected ?wp1 ?wp2))
+                    (at start (robot_at ?r ?wp1))
+                    (at start (>= (state_of_charge ?r)
+                                  (* 100 (/ (distance ?wp1 ?wp2) (max_range ?r))))))
+    :effect (and (at start (not (robot_at ?r ?wp1)))
+                 (at start (decrease (state_of_charge ?r)
+                                     (* 100 (/ (distance ?wp1 ?wp2) (max_range ?r)))))
+                 (at end (robot_at ?r ?wp2)))
+  )
+
+  ;; Patrol ?wp (waypoint) with ?r (robot).
+  ;; duration = 5
+  ;; condition - at start -> ?r (robot) is at ?wp (waypoint)
+  ;; efect - at end -> ?wp (waypoint) has been patrolled
+  (:durative-action patrol
+    :parameters (?r - robot ?wp - waypoint)
+    :duration (= ?duration 5)
+    :condition (and (over all (robot_at ?r ?wp)))
+    :effect (and (at end (patrolled ?wp)))
+  )
+
+  ;; Charge ?r (robot) at ?wp (waypoint).
+  ;; duration = 5
+  ;; condition - at start -> ?r (robot) is at ?wp (waypoint)
+  ;;             at start -> ?wp (waypoint) has a charger
+  ;; efect - at end -> state_of_charge(?r) = 100
+  (:durative-action charge
+    :parameters (?r - robot ?wp - waypoint)
+    :duration (= ?duration 5)
+    :condition (and (at start (<= (state_of_charge ?r) 100))
+                    (over all (robot_at ?r ?wp))
+                    (over all (charger_at ?wp)))
+    :effect (and (at end (assign (state_of_charge ?r) 100)))
+  )
+)

plansys2_simple_example/src/patrol_action_node.cpp

@@ -0,0 +1,68 @@
+// Copyright 2019 Intelligent Robotics Lab
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <memory>
+#include <algorithm>
+
+#include "plansys2_executor/ActionExecutorClient.hpp"
+
+#include "rclcpp/rclcpp.hpp"
+#include "rclcpp_action/rclcpp_action.hpp"
+
+using namespace std::chrono_literals;
+
+class PatrolAction : public plansys2::ActionExecutorClient
+{
+public:
+  PatrolAction()
+  : plansys2::ActionExecutorClient("charge", 500ms)
+  {
+    progress_ = 0.0;
+  }
+
+private:
+  void do_work()
+  {
+    if (progress_ < 1.0) {
+      progress_ += 0.05;
+      send_feedback(progress_, "Patrol running");
+    } else {
+      finish(true, 1.0, "Patrol completed");
+
+      progress_ = 0.0;
+      std::cout << std::endl;
+    }
+
+    std::cout << "\r\e[K" << std::flush;
+    std::cout << "Patrolling ... [" << std::min(100.0, progress_ * 100.0) << "%]  " <<
+      std::flush;
+  }
+
+  float progress_;
+};
+
+int main(int argc, char ** argv)
+{
+  rclcpp::init(argc, argv);
+  auto node = std::make_shared<PatrolAction>();
+
+  node->set_parameter(rclcpp::Parameter("action_name", "patrol"));
+  node->trigger_transition(lifecycle_msgs::msg::Transition::TRANSITION_CONFIGURE);
+
+  rclcpp::spin(node->get_node_base_interface());
+
+  rclcpp::shutdown();
+
+  return 0;
+}