Adkrawcz/humble

.gitignore

@@ -0,0 +1 @@
+.vscode/

CMakeLists.txt

@@ -1,35 +1,50 @@
-cmake_minimum_required(VERSION 2.8.3)
+cmake_minimum_required(VERSION 3.8)
 project(rcprg_smach)
 
-
-find_package(catkin REQUIRED COMPONENTS
-  actionlib
-  actionlib_msgs
-  std_msgs
-  geometry_msgs
-  move_base_msgs
-  rospy
-  tf
-  message_generation
+# Default to C++17
+if(NOT CMAKE_CXX_STANDARD)
+  set(CMAKE_CXX_STANDARD 17)
+endif()
+
+find_package(ament_cmake REQUIRED)
+find_package(rclpy REQUIRED)
+find_package(actionlib_msgs REQUIRED)
+find_package(std_msgs REQUIRED)
+find_package(geometry_msgs REQUIRED)
+# find_package(move_base_msgs REQUIRED) port to ROS2
+find_package(tf2 REQUIRED)
+find_package(tf2_ros REQUIRED)
+find_package(builtin_interfaces REQUIRED)
+find_package(rosidl_default_generators REQUIRED)
+
+set(dependencies
+    rclpy
+    actionlib_msgs
+    std_msgs
+    geometry_msgs
+    # move_base_msgs
+    tf2
+    tf2_ros
 )
 
-catkin_python_setup()
-
+# rosidl_generate_interfaces(${PROJECT_NAME}
+#   "msgs and services"
+#   DEPENDENCIES ${dependencies}
+# )
 
-catkin_package(
-    CATKIN_DEPENDS
-    rospy
-)
+ament_export_dependencies(rosidl_default_runtime)
 
 include_directories(
-  ${catkin_INCLUDE_DIRS}
+  include
 )
 
 install(DIRECTORY launch/
-  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch
-  PATTERN ".svn" EXCLUDE)
+  DESTINATION share/${PROJECT_NAME}/launch
+)
+
+# Uncomment if you have node executables
+# install(TARGETS your_node_targets
+#   DESTINATION lib/${PROJECT_NAME}
+# )
 
-#install(PROGRAMS
-#        nodes/rosplan_sys_control
-#        DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
-#        )
+ament_package()

nodes/bring_goods.py

@@ -0,0 +1,102 @@
+#!/usr/bin/env python3
+# encoding: utf8
+
+import sys
+
+import rclpy
+import smach
+import smach_ros
+
+import rcprg_smach.conversation
+import rcprg_smach.bring_goods
+import rcprg_smach.smach_rcprg as smach_rcprg
+from rcprg_smach.dynamic_agent import DynAgent
+
+import rcprg_kb.places_xml as kb_p
+
+from rcprg_smach.task_manager import PoseDescription
+
+from pl_nouns.dictionary_client import DisctionaryServiceClient
+
+
+class Cleanup(smach_rcprg.State):
+    def __init__(self, conversation_interface):
+        self.conversation_interface = conversation_interface
+        smach_rcprg.State.__init__(self, outcomes=['ok', 'shutdown'])
+
+    def execute(self, userdata):
+        node.get_logger().info(
+            f'{node.get_name()}: Executing state: {self.__class__.__name__}')
+        print('Cleanup.execute')
+        self.conversation_interface.stop()
+        return 'ok'
+
+
+class MainSM(smach_rcprg.StateMachine):
+    def __init__(self, node):
+        self.node = node
+        smach_rcprg.StateMachine.__init__(
+            self, outcomes=['Finished', 'shutdown'])
+
+        places_xml_filename = node.get_parameter('/kb_places_xml').value
+        sim_mode = node.get_parameter('/sim_mode').value
+        assert sim_mode in ['sim', 'gazebo', 'real']
+
+        print(f'Reading KB for places from file "{places_xml_filename}"')
+        kb_places = kb_p.PlacesXmlParser(places_xml_filename).getKB()
+
+        if len(sys.argv) < 3:
+            raise Exception('Too few arguments: ' + str(sys.argv))
+
+        goods_name = None
+        for idx in range(1, len(sys.argv), 2):
+            if sys.argv[idx] == 'przedmiot':
+                goods_name = sys.argv[idx+1]
+
+        if goods_name is None:
+            raise Exception(
+                'Argument "goods_name" is missing in argv: ' + str(sys.argv))
+
+        dictionary = DisctionaryServiceClient()
+        goods_name_m = dictionary.getCases(goods_name).getCase('mianownik')
+
+        self.conversation_interface = rcprg_smach.conversation.ConversationMachine([
+            ('ack',             'projects/incare-dialog-agent/agent/intents/ef92199b-d298-470c-8df3-1e1047dd70d1'),
+            ('ack_i_took',      'projects/incare-dialog-agent/agent/intents/181621b6-e91e-4244-a925-c5dc32ee1f1b'),
+            ('ack_i_gave',      'projects/incare-dialog-agent/agent/intents/d017cbd0-93f8-45b2-996e-043cdccab629'),
+            ('q_current_task',  'projects/incare-dialog-agent/agent/intents/8f45359d-ee47-4e10-a1b2-de3f3223e5b4'),
+            ('q_load',          'projects/incare-dialog-agent/agent/intents/b8743ab9-08a1-49e8-a534-abb65155c507'),
+            ('turn_around',     'projects/incare-dialog-agent/agent/intents/b4cb9f2e-2589-44dd-af14-a8f899c40ec0'),
+        ], sim_mode)
+        self.conversation_interface.start()
+
+        self.userdata.goal = goods_name_m
+
+        with self:
+            smach_rcprg.StateMachine.add('BringGoods',
+                                         rcprg_smach.bring_goods.BringGoods(
+                                             sim_mode, self.conversation_interface, kb_places),
+                                         transitions={'FINISHED': 'Cleanup', 'PREEMPTED': 'Cleanup', 'FAILED': 'Cleanup',
+                                                      'shutdown': 'shutdown'},
+                                         remapping={'goal': 'goal'})
+
+            smach_rcprg.StateMachine.add('Cleanup',
+                                         Cleanup(self.conversation_interface),
+                                         transitions={
+                                             'ok': 'Finished', 'shutdown': 'shutdown'},
+                                         remapping={})
+
+    def shutdownRequest(self):
+        self.conversation_interface.stop()
+        self.request_preempt()
+
+
+def main():
+    da = DynAgent('bring_goods')
+    da.run(MainSM())
+
+    return 0
+
+
+if __name__ == '__main__':
+    main()