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..98b2a7f --- /dev/null +++ b/plansys2_match_example/launch/commands @@ -0,0 +1,10 @@ +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..a2e32dc --- /dev/null +++ b/plansys2_match_example/launch/plansys2_match_example_launch.py @@ -0,0 +1,93 @@ +# 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=[]) + + 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', + name='mend_fuse_action_node', + namespace=namespace, + 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() + + ld.add_action(declare_namespace_cmd) + + # Declare the launch options + 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 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..e51bef6 --- /dev/null +++ b/plansys2_match_example/src/light_match_action_node.cpp @@ -0,0 +1,71 @@ +// 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; + duration_ = 8.0; + } + + private: + void do_work() + { + auto elapsed_time = now() - get_start_time(); + progress_ = elapsed_time.seconds() / duration_; + if (progress_ < 1.0) { + 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) << "%] " << + " elapsed_time: " << elapsed_time.seconds() << + std::flush; + } + + float progress_; + double duration_; +}; + +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..7780be1 --- /dev/null +++ b/plansys2_match_example/src/mend_fuse_action_node.cpp @@ -0,0 +1,72 @@ +// 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 + +#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_ = 5.0; + } + + private: + void do_work() + { + auto elapsed_time = now() - get_start_time(); + progress_ = elapsed_time.seconds() / duration_; + if (progress_ < 1.0) { + 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) << "%] " << + " elapsed_time: " << elapsed_time.seconds() << + std::flush; + } + + float progress_; + double 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; +}