Match example

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()

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

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

plansys2_match_example/package.xml

@@ -0,0 +1,30 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>plansys2_match_example</name>
+  <version>0.0.0</version>
+
+  <description>Simple implementation of the Match Cellar problem.</description>
+
+  <maintainer email="[email protected]">Josh Zapf</maintainer>
+
+  <license>Apache License, Version 2.0</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <depend>rclcpp</depend>
+  <depend>rclcpp_action</depend>
+  <depend>plansys2_msgs</depend>
+  <depend>plansys2_executor</depend>
+
+  <exec_depend>plansys2_bringup</exec_depend>
+  <exec_depend>plansys2_terminal</exec_depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+  <test_depend>ament_cmake_gtest</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

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)))
+  )
+)

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))
+)

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 <memory>
+#include <algorithm>
+
+#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<LightMatchAction>();
+
+  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;
+}

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 <memory>
+#include <algorithm>
+#include <cmath>
+
+#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<MendFuseAction>();
+
+  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;
+}