From 01d5b082f8312eae734af0298bcf0c1ef4bff0c5 Mon Sep 17 00:00:00 2001 From: Alexander Xydes Date: Mon, 4 Apr 2022 11:07:05 -0700 Subject: [PATCH] Adding simple charging example to plansys2_simple_example to help illustrate a bug in the plan dotgraph output by plansys2_executor. Signed-off-by: Alexander Xydes --- plansys2_simple_example/CMakeLists.txt | 4 + .../launch/plansys2_simple_example_launch.py | 22 +++++- .../pddl/charging_problem.pddl | 56 +++++++++++++ .../pddl/domain_charging.pddl | 79 +++++++++++++++++++ .../src/patrol_action_node.cpp | 68 ++++++++++++++++ 5 files changed, 227 insertions(+), 2 deletions(-) create mode 100644 plansys2_simple_example/pddl/charging_problem.pddl create mode 100644 plansys2_simple_example/pddl/domain_charging.pddl create mode 100644 plansys2_simple_example/src/patrol_action_node.cpp diff --git a/plansys2_simple_example/CMakeLists.txt b/plansys2_simple_example/CMakeLists.txt index edb6e33..8a52ca0 100644 --- a/plansys2_simple_example/CMakeLists.txt +++ b/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} diff --git a/plansys2_simple_example/launch/plansys2_simple_example_launch.py b/plansys2_simple_example/launch/plansys2_simple_example_launch.py index 512f8f5..b79c51c 100644 --- a/plansys2_simple_example/launch/plansys2_simple_example_launch.py +++ b/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,10 +72,21 @@ 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) @@ -77,5 +94,6 @@ def generate_launch_description(): ld.add_action(move_cmd) ld.add_action(charge_cmd) ld.add_action(ask_charge_cmd) + ld.add_action(patrol_cmd) return ld diff --git a/plansys2_simple_example/pddl/charging_problem.pddl b/plansys2_simple_example/pddl/charging_problem.pddl new file mode 100644 index 0000000..c845aab --- /dev/null +++ b/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) + ) +) \ No newline at end of file diff --git a/plansys2_simple_example/pddl/domain_charging.pddl b/plansys2_simple_example/pddl/domain_charging.pddl new file mode 100644 index 0000000..5a29ecd --- /dev/null +++ b/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))) + ) +) diff --git a/plansys2_simple_example/src/patrol_action_node.cpp b/plansys2_simple_example/src/patrol_action_node.cpp new file mode 100644 index 0000000..4eef5af --- /dev/null +++ b/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 +#include + +#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(); + + 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; +}