From f099c05df44dc06ccf8b2312d2dbf1a6fd103ed9 Mon Sep 17 00:00:00 2001 From: Josh Zapf Date: Sat, 5 Nov 2022 23:05:40 -0700 Subject: [PATCH 1/5] Adding match example. --- plansys2_bt_example/COLCON_IGNORE | 0 plansys2_match_example/CMakeLists.txt | 44 +++++++++++ plansys2_match_example/launch/commands | 12 +++ .../launch/plansys2_match_example_launch.py | 75 +++++++++++++++++++ plansys2_match_example/package.xml | 30 ++++++++ plansys2_match_example/pddl/match_domain.pddl | 32 ++++++++ .../pddl/match_problem.pddl | 23 ++++++ .../src/light_match_action_node.cpp | 67 +++++++++++++++++ .../src/mend_fuse_action_node.cpp | 69 +++++++++++++++++ 9 files changed, 352 insertions(+) create mode 100644 plansys2_bt_example/COLCON_IGNORE create mode 100644 plansys2_match_example/CMakeLists.txt create mode 100644 plansys2_match_example/launch/commands create mode 100644 plansys2_match_example/launch/plansys2_match_example_launch.py create mode 100644 plansys2_match_example/package.xml create mode 100644 plansys2_match_example/pddl/match_domain.pddl create mode 100644 plansys2_match_example/pddl/match_problem.pddl create mode 100644 plansys2_match_example/src/light_match_action_node.cpp create mode 100644 plansys2_match_example/src/mend_fuse_action_node.cpp diff --git a/plansys2_bt_example/COLCON_IGNORE b/plansys2_bt_example/COLCON_IGNORE new file mode 100644 index 0000000..e69de29 diff --git a/plansys2_match_example/CMakeLists.txt b/plansys2_match_example/CMakeLists.txt new file mode 100644 index 0000000..76a5668 --- /dev/null +++ b/plansys2_match_example/CMakeLists.txt @@ -0,0 +1,44 @@ +cmake_minimum_required(VERSION 3.5) +project(plansys2_match_example) + +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(rclcpp_action REQUIRED) +find_package(plansys2_msgs REQUIRED) +find_package(plansys2_executor REQUIRED) + +set(CMAKE_CXX_STANDARD 17) + +set(dependencies + rclcpp + rclcpp_action + plansys2_msgs + plansys2_executor +) + +add_executable(light_match_action_node src/light_match_action_node.cpp) +ament_target_dependencies(light_match_action_node ${dependencies}) + +add_executable(mend_fuse_action_node src/mend_fuse_action_node.cpp) +ament_target_dependencies(mend_fuse_action_node ${dependencies}) + +install(DIRECTORY launch pddl DESTINATION share/${PROJECT_NAME}) + +install(TARGETS + light_match_action_node + mend_fuse_action_node + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION lib/${PROJECT_NAME} +) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() + + find_package(ament_cmake_gtest REQUIRED) +endif() + +ament_export_dependencies(${dependencies}) + +ament_package() diff --git a/plansys2_match_example/launch/commands b/plansys2_match_example/launch/commands new file mode 100644 index 0000000..dc68a23 --- /dev/null +++ b/plansys2_match_example/launch/commands @@ -0,0 +1,12 @@ +ros2 run plansys2_terminal plansys2_terminal + +set instance m1 match +set instance m2 match +set instance f1 fuse +set instance f2 fuse +set predicate (unused m1) +set predicate (unused m2) +set predicate (handfree) +set goal (and (mended f1) (mended f2)) +get plan +run diff --git a/plansys2_match_example/launch/plansys2_match_example_launch.py b/plansys2_match_example/launch/plansys2_match_example_launch.py new file mode 100644 index 0000000..e286a5a --- /dev/null +++ b/plansys2_match_example/launch/plansys2_match_example_launch.py @@ -0,0 +1,75 @@ +# 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. + +import os + +from ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node + + +def generate_launch_description(): + # Get the launch directory + example_dir = get_package_share_directory('plansys2_match_example') + namespace = LaunchConfiguration('namespace') + + declare_namespace_cmd = DeclareLaunchArgument( + 'namespace', + default_value='', + description='Namespace') + + plansys2_cmd = IncludeLaunchDescription( + PythonLaunchDescriptionSource(os.path.join( + get_package_share_directory('plansys2_bringup'), + 'launch', + 'plansys2_bringup_launch_monolithic.py')), + launch_arguments={ + 'model_file': example_dir + '/pddl/match_domain.pddl', + 'namespace': namespace, + 'bt_builder_plugin': 'STNBTBuilder' + }.items()) + + # Specify the actions + light_match_cmd = Node( + package='plansys2_match_example', + executable='light_match_action_node', + name='light_match_action_node', + namespace=namespace, + output='screen', + parameters=[]) + + mend_fuse_cmd = Node( + package='plansys2_match_example', + executable='mend_fuse_action_node', + name='mend_fuse_action_node', + namespace=namespace, + output='screen', + parameters=[]) + + # Create the launch description and populate + ld = LaunchDescription() + + ld.add_action(declare_namespace_cmd) + + # Declare the launch options + ld.add_action(plansys2_cmd) + + ld.add_action(light_match_cmd) + ld.add_action(mend_fuse_cmd) + + return ld diff --git a/plansys2_match_example/package.xml b/plansys2_match_example/package.xml new file mode 100644 index 0000000..bbf44d8 --- /dev/null +++ b/plansys2_match_example/package.xml @@ -0,0 +1,30 @@ + + + + plansys2_match_example + 0.0.0 + + Simple implementation of the Match Cellar problem. + + Josh Zapf + + Apache License, Version 2.0 + + ament_cmake + + rclcpp + rclcpp_action + plansys2_msgs + plansys2_executor + + plansys2_bringup + plansys2_terminal + + ament_lint_auto + ament_lint_common + ament_cmake_gtest + + + ament_cmake + + diff --git a/plansys2_match_example/pddl/match_domain.pddl b/plansys2_match_example/pddl/match_domain.pddl new file mode 100644 index 0000000..47e4f7f --- /dev/null +++ b/plansys2_match_example/pddl/match_domain.pddl @@ -0,0 +1,32 @@ +(define (domain matchcellar) + (:requirements :durative-actions :typing) + + (:types match fuse - object) + + (:predicates + (light ?m - match) + (handfree) + (unused ?m - match) + (mended ?f - fuse) + ) + + (:durative-action light_match + :parameters (?m - match) + :duration (= ?duration 8) + :condition (and (at start (unused ?m)) + (over all (light ?m))) + :effect (and (at start (not (unused ?m))) + (at start (light ?m)) + (at end (not (light ?m)))) + ) + + (:durative-action mend_fuse + :parameters (?f - fuse ?m - match) + :duration (= ?duration 5) + :condition (and (at start (handfree)) + (over all (light ?m))) + :effect (and (at start (not (handfree))) + (at end (mended ?f)) + (at end (handfree))) + ) +) diff --git a/plansys2_match_example/pddl/match_problem.pddl b/plansys2_match_example/pddl/match_problem.pddl new file mode 100644 index 0000000..4e9a9c9 --- /dev/null +++ b/plansys2_match_example/pddl/match_problem.pddl @@ -0,0 +1,23 @@ +(define (problem fixfuse) + (:domain matchcellar) + + (:objects + match1 match2 - match + fuse1 fuse2 - fuse + ) + + (:init + (unused match1) + (unused match2) + (handfree) + ) + + (:goal + (and + (mended fuse1) + (mended fuse2) + ) + ) + + (:metric minimize (total-time)) +) diff --git a/plansys2_match_example/src/light_match_action_node.cpp b/plansys2_match_example/src/light_match_action_node.cpp new file mode 100644 index 0000000..70c7462 --- /dev/null +++ b/plansys2_match_example/src/light_match_action_node.cpp @@ -0,0 +1,67 @@ +// 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 LightMatchAction : public plansys2::ActionExecutorClient +{ +public: + LightMatchAction() + : plansys2::ActionExecutorClient("light_match", 250ms) + { + progress_ = 0.0; + } + + private: + void do_work() + { + if (progress_ < 1.0) { + progress_ += 0.02; + send_feedback(progress_, "Light match running"); + } else { + finish(true, 1.0, "Light match completed"); + progress_ = 0.0; + std::cout << std::endl; + } + + std::cout << "\r\e[K" << std::flush; + std::cout << "Light match ... [" << 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", "light_match")); + node->trigger_transition(lifecycle_msgs::msg::Transition::TRANSITION_CONFIGURE); + + rclcpp::spin(node->get_node_base_interface()); + + rclcpp::shutdown(); + + return 0; +} diff --git a/plansys2_match_example/src/mend_fuse_action_node.cpp b/plansys2_match_example/src/mend_fuse_action_node.cpp new file mode 100644 index 0000000..269a50a --- /dev/null +++ b/plansys2_match_example/src/mend_fuse_action_node.cpp @@ -0,0 +1,69 @@ +// 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 MendFuseAction : public plansys2::ActionExecutorClient +{ +public: + MendFuseAction() + : plansys2::ActionExecutorClient("mend_fuse", 250ms) + { + progress_ = 0.0; + duration_ = std::chrono::duration(0); + } + + private: + void do_work() + { + if (progress_ < 1.0) { + progress_ += 0.02; + send_feedback(progress_, "Mend fuse running"); + } else { + finish(true, 1.0, "Mend fuse completed"); + progress_ = 0.0; + std::cout << std::endl; + } + + std::cout << "\r\e[K" << std::flush; + std::cout << "Mend fuse ... [" << std::min(100.0, progress_ * 100.0) << "%] " << + std::flush; + } + + float progress_; + std::chrono::duration duration_; +}; + +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + auto node = std::make_shared(); + + node->set_parameter(rclcpp::Parameter("action_name", "mend_fuse")); + node->trigger_transition(lifecycle_msgs::msg::Transition::TRANSITION_CONFIGURE); + + rclcpp::spin(node->get_node_base_interface()); + + rclcpp::shutdown(); + + return 0; +} From 4c5277621e406c8b76dc3f42c032f74dd2994306 Mon Sep 17 00:00:00 2001 From: Josh Zapf Date: Mon, 7 Nov 2022 10:31:22 -0800 Subject: [PATCH 2/5] Runnig for hard coded duration. --- plansys2_match_example/src/light_match_action_node.cpp | 7 +++++-- plansys2_match_example/src/mend_fuse_action_node.cpp | 10 ++++++---- 2 files changed, 11 insertions(+), 6 deletions(-) diff --git a/plansys2_match_example/src/light_match_action_node.cpp b/plansys2_match_example/src/light_match_action_node.cpp index 70c7462..ac13798 100644 --- a/plansys2_match_example/src/light_match_action_node.cpp +++ b/plansys2_match_example/src/light_match_action_node.cpp @@ -29,13 +29,15 @@ class LightMatchAction : public plansys2::ActionExecutorClient : plansys2::ActionExecutorClient("light_match", 250ms) { progress_ = 0.0; + duration_ = 8.0; } - + private: void do_work() { + auto elapsed_time = now() - get_start_time(); + progress_ = elapsed_time.seconds() / duration_; if (progress_ < 1.0) { - progress_ += 0.02; send_feedback(progress_, "Light match running"); } else { finish(true, 1.0, "Light match completed"); @@ -49,6 +51,7 @@ class LightMatchAction : public plansys2::ActionExecutorClient } float progress_; + double duration_; }; int main(int argc, char ** argv) diff --git a/plansys2_match_example/src/mend_fuse_action_node.cpp b/plansys2_match_example/src/mend_fuse_action_node.cpp index 269a50a..3b6b634 100644 --- a/plansys2_match_example/src/mend_fuse_action_node.cpp +++ b/plansys2_match_example/src/mend_fuse_action_node.cpp @@ -14,6 +14,7 @@ #include #include +#include #include "plansys2_executor/ActionExecutorClient.hpp" @@ -29,14 +30,15 @@ class MendFuseAction : public plansys2::ActionExecutorClient : plansys2::ActionExecutorClient("mend_fuse", 250ms) { progress_ = 0.0; - duration_ = std::chrono::duration(0); + duration_ = 5.0; } - + private: void do_work() { + auto elapsed_time = now() - get_start_time(); + progress_ = elapsed_time.seconds() / duration_; if (progress_ < 1.0) { - progress_ += 0.02; send_feedback(progress_, "Mend fuse running"); } else { finish(true, 1.0, "Mend fuse completed"); @@ -50,7 +52,7 @@ class MendFuseAction : public plansys2::ActionExecutorClient } float progress_; - std::chrono::duration duration_; + double duration_; }; int main(int argc, char ** argv) From 4a65eef7303827d7c7718282b0fc98d7a58726bd Mon Sep 17 00:00:00 2001 From: Josh Zapf Date: Tue, 22 Nov 2022 18:53:55 +0000 Subject: [PATCH 3/5] Printing elapsed time to screen. --- plansys2_match_example/src/light_match_action_node.cpp | 1 + plansys2_match_example/src/mend_fuse_action_node.cpp | 1 + 2 files changed, 2 insertions(+) diff --git a/plansys2_match_example/src/light_match_action_node.cpp b/plansys2_match_example/src/light_match_action_node.cpp index ac13798..e51bef6 100644 --- a/plansys2_match_example/src/light_match_action_node.cpp +++ b/plansys2_match_example/src/light_match_action_node.cpp @@ -47,6 +47,7 @@ class LightMatchAction : public plansys2::ActionExecutorClient std::cout << "\r\e[K" << std::flush; std::cout << "Light match ... [" << std::min(100.0, progress_ * 100.0) << "%] " << + " elapsed_time: " << elapsed_time.seconds() << std::flush; } diff --git a/plansys2_match_example/src/mend_fuse_action_node.cpp b/plansys2_match_example/src/mend_fuse_action_node.cpp index 3b6b634..7780be1 100644 --- a/plansys2_match_example/src/mend_fuse_action_node.cpp +++ b/plansys2_match_example/src/mend_fuse_action_node.cpp @@ -48,6 +48,7 @@ class MendFuseAction : public plansys2::ActionExecutorClient std::cout << "\r\e[K" << std::flush; std::cout << "Mend fuse ... [" << std::min(100.0, progress_ * 100.0) << "%] " << + " elapsed_time: " << elapsed_time.seconds() << std::flush; } From 8b9c0d638adcf5c4cbff9f05b91221fc127d3953 Mon Sep 17 00:00:00 2001 From: Josh Zapf Date: Sun, 19 Feb 2023 01:09:42 +0000 Subject: [PATCH 4/5] Removing ros2 run command from commands file. --- plansys2_match_example/launch/commands | 2 -- 1 file changed, 2 deletions(-) diff --git a/plansys2_match_example/launch/commands b/plansys2_match_example/launch/commands index dc68a23..98b2a7f 100644 --- a/plansys2_match_example/launch/commands +++ b/plansys2_match_example/launch/commands @@ -1,5 +1,3 @@ -ros2 run plansys2_terminal plansys2_terminal - set instance m1 match set instance m2 match set instance f1 fuse From 6f3b3580007cf1ba76d2977af5e83bb1b606d8b8 Mon Sep 17 00:00:00 2001 From: Josh Zapf Date: Sat, 4 Mar 2023 19:25:30 -0800 Subject: [PATCH 5/5] Adding additional nodes to allow for action concurrency. --- .../launch/plansys2_match_example_launch.py | 18 ++++++++++++++++++ 1 file changed, 18 insertions(+) diff --git a/plansys2_match_example/launch/plansys2_match_example_launch.py b/plansys2_match_example/launch/plansys2_match_example_launch.py index e286a5a..a2e32dc 100644 --- a/plansys2_match_example/launch/plansys2_match_example_launch.py +++ b/plansys2_match_example/launch/plansys2_match_example_launch.py @@ -53,6 +53,14 @@ def generate_launch_description(): output='screen', parameters=[]) + light_match_cmd_2 = Node( + package='plansys2_match_example', + executable='light_match_action_node', + name='light_match_action_node_2', + namespace=namespace, + output='screen', + parameters=[]) + mend_fuse_cmd = Node( package='plansys2_match_example', executable='mend_fuse_action_node', @@ -61,6 +69,14 @@ def generate_launch_description(): output='screen', parameters=[]) + mend_fuse_cmd_2 = Node( + package='plansys2_match_example', + executable='mend_fuse_action_node', + name='mend_fuse_action_node_2', + namespace=namespace, + output='screen', + parameters=[]) + # Create the launch description and populate ld = LaunchDescription() @@ -70,6 +86,8 @@ def generate_launch_description(): ld.add_action(plansys2_cmd) ld.add_action(light_match_cmd) + ld.add_action(light_match_cmd_2) ld.add_action(mend_fuse_cmd) + ld.add_action(mend_fuse_cmd_2) return ld