From 159274d9403cf5a7c01c01a2cc7bc5048870c096 Mon Sep 17 00:00:00 2001 From: Adam Krawczyk Date: Tue, 22 Aug 2023 18:47:15 +0200 Subject: [PATCH 1/4] Build with colcon on humble Signed-off-by: Adam Krawczyk --- .gitignore | 1 + CMakeLists.txt | 103 ++++++++++++++---- nodes/{bring_goods => bring_goods.py} | 0 ...ing_goods_tasker => bring_goods_tasker.py} | 0 nodes/{bring_jar => bring_jar.py} | 0 .../{bring_jar_tasker => bring_jar_tasker.py} | 0 nodes/{call => call.py} | 0 ...ide_human_tasker => guide_human_tasker.py} | 0 ...h_tasker => human_fell_approach_tasker.py} | 0 ...human_fell_tasker => human_fell_tasker.py} | 0 nodes/{idle => idle.py} | 0 nodes/{idle_tasker => idle_tasker.py} | 0 nodes/{move_to => move_to.py} | 0 nodes/{move_to_tasker => move_to_tasker.py} | 0 nodes/{stop => stop.py} | 0 nodes/{task_harmonizer => task_harmonizer.py} | 4 +- .../{test_bring_goods => test_bring_goods.py} | 0 nodes/{test_move_to => test_move_to.py} | 0 nodes/{test_wander => test_wander.py} | 0 nodes/{wander => wander.py} | 0 package.xml | 26 ++--- 21 files changed, 96 insertions(+), 38 deletions(-) create mode 100644 .gitignore rename nodes/{bring_goods => bring_goods.py} (100%) rename nodes/{bring_goods_tasker => bring_goods_tasker.py} (100%) rename nodes/{bring_jar => bring_jar.py} (100%) rename nodes/{bring_jar_tasker => bring_jar_tasker.py} (100%) rename nodes/{call => call.py} (100%) rename nodes/{guide_human_tasker => guide_human_tasker.py} (100%) rename nodes/{human_fell_approach_tasker => human_fell_approach_tasker.py} (100%) rename nodes/{human_fell_tasker => human_fell_tasker.py} (100%) rename nodes/{idle => idle.py} (100%) rename nodes/{idle_tasker => idle_tasker.py} (100%) rename nodes/{move_to => move_to.py} (100%) rename nodes/{move_to_tasker => move_to_tasker.py} (100%) rename nodes/{stop => stop.py} (100%) rename nodes/{task_harmonizer => task_harmonizer.py} (99%) rename nodes/{test_bring_goods => test_bring_goods.py} (100%) rename nodes/{test_move_to => test_move_to.py} (100%) rename nodes/{test_wander => test_wander.py} (100%) rename nodes/{wander => wander.py} (100%) diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..1d74e21 --- /dev/null +++ b/.gitignore @@ -0,0 +1 @@ +.vscode/ diff --git a/CMakeLists.txt b/CMakeLists.txt index 656d236..0e0da6d 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,35 +1,92 @@ -cmake_minimum_required(VERSION 2.8.3) +cmake_minimum_required(VERSION 3.5) project(rcprg_smach) +# Default to C++17 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 17) +endif() -find_package(catkin REQUIRED COMPONENTS - actionlib - actionlib_msgs - std_msgs - geometry_msgs - move_base_msgs - rospy - tf - message_generation -) +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) -catkin_python_setup() +set(dependencies + rclpy + actionlib_msgs + std_msgs + geometry_msgs + # move_base_msgs + tf2 + tf2_ros +) +# 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} +# ) + +ament_package() + + + +# cmake_minimum_required(VERSION 3.8) +# project(rcprg_smach) + +# find_package(ament_cmake REQUIRED) +# find_package(rclcpp REQUIRED) +# find_package(geometry_msgs REQUIRED) +# find_package(action_msgs REQUIRED) +# find_package(nav_msgs REQUIRED) +# find_package(tf2_ros REQUIRED) +# find_package(std_msgs REQUIRED) + +# set(CMAKE_CXX_STANDARD 14) + +# include_directories( +# include +# ) + +# add_executable(rcprg_smach_node +# src/rcprg_smach_node.cpp +# ) + +# ament_target_dependencies(rcprg_smach_node +# rclcpp +# geometry_msgs +# action_msgs +# nav_msgs +# tf2_ros +# std_msgs +# ) + +# install(TARGETS rcprg_smach_node +# DESTINATION lib/${PROJECT_NAME} +# ) + +# install(DIRECTORY launch +# DESTINATION share/${PROJECT_NAME} +# ) -#install(PROGRAMS -# nodes/rosplan_sys_control -# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -# ) +# ament_package() \ No newline at end of file diff --git a/nodes/bring_goods b/nodes/bring_goods.py similarity index 100% rename from nodes/bring_goods rename to nodes/bring_goods.py diff --git a/nodes/bring_goods_tasker b/nodes/bring_goods_tasker.py similarity index 100% rename from nodes/bring_goods_tasker rename to nodes/bring_goods_tasker.py diff --git a/nodes/bring_jar b/nodes/bring_jar.py similarity index 100% rename from nodes/bring_jar rename to nodes/bring_jar.py diff --git a/nodes/bring_jar_tasker b/nodes/bring_jar_tasker.py similarity index 100% rename from nodes/bring_jar_tasker rename to nodes/bring_jar_tasker.py diff --git a/nodes/call b/nodes/call.py similarity index 100% rename from nodes/call rename to nodes/call.py diff --git a/nodes/guide_human_tasker b/nodes/guide_human_tasker.py similarity index 100% rename from nodes/guide_human_tasker rename to nodes/guide_human_tasker.py diff --git a/nodes/human_fell_approach_tasker b/nodes/human_fell_approach_tasker.py similarity index 100% rename from nodes/human_fell_approach_tasker rename to nodes/human_fell_approach_tasker.py diff --git a/nodes/human_fell_tasker b/nodes/human_fell_tasker.py similarity index 100% rename from nodes/human_fell_tasker rename to nodes/human_fell_tasker.py diff --git a/nodes/idle b/nodes/idle.py similarity index 100% rename from nodes/idle rename to nodes/idle.py diff --git a/nodes/idle_tasker b/nodes/idle_tasker.py similarity index 100% rename from nodes/idle_tasker rename to nodes/idle_tasker.py diff --git a/nodes/move_to b/nodes/move_to.py similarity index 100% rename from nodes/move_to rename to nodes/move_to.py diff --git a/nodes/move_to_tasker b/nodes/move_to_tasker.py similarity index 100% rename from nodes/move_to_tasker rename to nodes/move_to_tasker.py diff --git a/nodes/stop b/nodes/stop.py similarity index 100% rename from nodes/stop rename to nodes/stop.py diff --git a/nodes/task_harmonizer b/nodes/task_harmonizer.py similarity index 99% rename from nodes/task_harmonizer rename to nodes/task_harmonizer.py index 825f783..9035a27 100644 --- a/nodes/task_harmonizer +++ b/nodes/task_harmonizer.py @@ -1,4 +1,4 @@ -#!/usr/bin/env python +#!/usr/bin/env python3 # encoding: utf8 import rospkg @@ -42,7 +42,7 @@ def __init__(self, task_harmoniser_agent, intent_topics, tasks_spec_list, cached self.task_request_response = None - print 'ConversationMachine.__init__: waiting for rico_says ActionServer...' + print('ConversationMachine.__init__: waiting for rico_says ActionServer...') #self.rico_says_client = actionlib.SimpleActionClient('rico_says', tiago_msgs.msg.SaySentenceAction) #self.rico_says_client.wait_for_server() print 'ConversationMachine.__init__: connected to rico_says ActionServer' diff --git a/nodes/test_bring_goods b/nodes/test_bring_goods.py similarity index 100% rename from nodes/test_bring_goods rename to nodes/test_bring_goods.py diff --git a/nodes/test_move_to b/nodes/test_move_to.py similarity index 100% rename from nodes/test_move_to rename to nodes/test_move_to.py diff --git a/nodes/test_wander b/nodes/test_wander.py similarity index 100% rename from nodes/test_wander rename to nodes/test_wander.py diff --git a/nodes/wander b/nodes/wander.py similarity index 100% rename from nodes/wander rename to nodes/wander.py diff --git a/package.xml b/package.xml index 1538195..5aeea17 100644 --- a/package.xml +++ b/package.xml @@ -1,24 +1,24 @@ - + rcprg_smach 0.0.0 The rcprg_smach package - - Dawid Seredynski - - TODO + + Dawid Seredynski + Adam Krawczyk - catkin - actionlib - actionlib_msgs + ament_cmake + + action_msgs diagnostic_msgs geometry_msgs - roscpp - move_base_msgs - rospy + rclcpp + nav_msgs std_msgs std_srvs - tf - + tf2_ros + + + From 4c20146a59845351bcc620bad20ec279f889b39e Mon Sep 17 00:00:00 2001 From: Adam Krawczyk Date: Mon, 16 Oct 2023 17:18:45 +0200 Subject: [PATCH 2/4] Update to ros2 Signed-off-by: Adam Krawczyk --- CMakeLists.txt | 46 +---- nodes/bring_goods.py | 70 ++++---- nodes/bring_goods_tasker.py | 153 ++++++++++------- nodes/bring_jar.py | 83 +++++---- nodes/bring_jar_tasker.py | 201 ++++++++++++---------- nodes/call.py | 106 +++++++----- nodes/guide_human_tasker.py | 211 ++++++++++++++--------- nodes/human_fell_approach_tasker.py | 202 ++++++++++++---------- nodes/human_fell_tasker.py | 194 ++++++++++++--------- nodes/idle.py | 43 +++-- nodes/idle_tasker.py | 228 ++++++++++--------------- nodes/move_to.py | 2 +- nodes/move_to_tasker.py | 2 +- nodes/stop.py | 2 +- nodes/task_harmonizer.py | 2 +- nodes/test_bring_goods.py | 2 +- nodes/test_move_to.py | 2 +- nodes/test_wander.py | 2 +- nodes/wander.py | 2 +- setup.py | 2 +- src/rcprg_smach/bring_goods_tasker.py | 82 +++------ src/rcprg_smach/bring_jar.py | 137 ++++++++------- src/rcprg_smach/conversation.py | 105 ++++++------ src/rcprg_smach/gh_csp.py | 2 +- src/rcprg_smach/guide_human.py | 2 +- src/rcprg_smach/hazard_detector.py | 2 +- src/rcprg_smach/human_fell.py | 82 ++++----- src/rcprg_smach/human_fell_approach.py | 2 +- src/rcprg_smach/manipulation.py | 2 +- src/rcprg_smach/navigation.py | 67 +------- src/rcprg_smach/ros_node_utils.py | 2 +- src/rcprg_smach/smach_rcprg.py | 4 +- src/rcprg_smach/suspend_bj.py | 2 +- src/rcprg_smach/suspend_gh.py | 2 +- src/rcprg_smach/task_manager.py | 73 ++++---- src/rcprg_smach/update_pose.py | 39 +++-- 36 files changed, 1095 insertions(+), 1065 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 0e0da6d..83c2df0 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,4 +1,4 @@ -cmake_minimum_required(VERSION 3.5) +cmake_minimum_required(VERSION 3.8) project(rcprg_smach) # Default to C++17 @@ -47,46 +47,4 @@ install(DIRECTORY launch/ # DESTINATION lib/${PROJECT_NAME} # ) -ament_package() - - - -# cmake_minimum_required(VERSION 3.8) -# project(rcprg_smach) - -# find_package(ament_cmake REQUIRED) -# find_package(rclcpp REQUIRED) -# find_package(geometry_msgs REQUIRED) -# find_package(action_msgs REQUIRED) -# find_package(nav_msgs REQUIRED) -# find_package(tf2_ros REQUIRED) -# find_package(std_msgs REQUIRED) - -# set(CMAKE_CXX_STANDARD 14) - -# include_directories( -# include -# ) - -# add_executable(rcprg_smach_node -# src/rcprg_smach_node.cpp -# ) - -# ament_target_dependencies(rcprg_smach_node -# rclcpp -# geometry_msgs -# action_msgs -# nav_msgs -# tf2_ros -# std_msgs -# ) - -# install(TARGETS rcprg_smach_node -# DESTINATION lib/${PROJECT_NAME} -# ) - -# install(DIRECTORY launch -# DESTINATION share/${PROJECT_NAME} -# ) - -# ament_package() \ No newline at end of file +ament_package() \ No newline at end of file diff --git a/nodes/bring_goods.py b/nodes/bring_goods.py index e593895..fe800b4 100755 --- a/nodes/bring_goods.py +++ b/nodes/bring_goods.py @@ -1,9 +1,9 @@ -#!/usr/bin/env python +#!/usr/bin/env python3 # encoding: utf8 import sys -import rospy +import rclpy import smach import smach_ros @@ -18,31 +18,31 @@ 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): - rospy.loginfo('{}: Executing state: {}'.format(rospy.get_name(), self.__class__.__name__)) - print 'Cleanup.execute' + node.get_logger().info( + f'{node.get_name()}: Executing state: {self.__class__.__name__}') + print('Cleanup.execute') self.conversation_interface.stop() return 'ok' -# -# The SM that govenrs the highest-level behaviour. -# class MainSM(smach_rcprg.StateMachine): - def __init__(self): - smach_rcprg.StateMachine.__init__(self, outcomes=['Finished', 'shutdown']) + def __init__(self, node): + self.node = node + smach_rcprg.StateMachine.__init__( + self, outcomes=['Finished', 'shutdown']) - places_xml_filename = rospy.get_param('/kb_places_xml') - sim_mode = str(rospy.get_param('/sim_mode')) + 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 'Reading KB for places from file "' + places_xml_filename + '"' + 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: @@ -54,49 +54,49 @@ def __init__(self): goods_name = sys.argv[idx+1] if goods_name is None: - raise Exception('Argument "goods_name" is missing in argv: ' + str(sys.argv)) - - if isinstance(goods_name, str): - goods_name = goods_name.decode('utf-8') - goods_name = goods_name.encode('utf-8').decode('utf-8') + 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) + ('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'}) + 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={ }) - + 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() ) + da = DynAgent('bring_goods') + da.run(MainSM()) return 0 + if __name__ == '__main__': main() diff --git a/nodes/bring_goods_tasker.py b/nodes/bring_goods_tasker.py index 5817fc3..5a7ce83 100755 --- a/nodes/bring_goods_tasker.py +++ b/nodes/bring_goods_tasker.py @@ -1,9 +1,12 @@ -#!/usr/bin/env python +#!/usr/bin/env python3 # encoding: utf8 import sys -import rospy +import rclpy +from rclpy.node import Node +from rclpy.executors import SingleThreadedExecutor + import smach import smach_ros @@ -26,12 +29,13 @@ import subprocess import imp + def ptf_csp(ptf_id): - print "calculating SP: ", ptf_id + print("calculating SP: ", ptf_id) # set flag 'self-terminate' to terminate DA if in Wait or Init state flag = None if ptf_id[0] == "scheduleParams": - return flag, ScheduleParams(cost = 1, completion_time=1,cost_per_sec=1,final_resource_state=RobotResource()) + return flag, ScheduleParams(cost=1, completion_time=1, cost_per_sec=1, final_resource_state=RobotResource()) elif ptf_id[0] == "suspendCondition": req = SuspendConditionsRequest() req = ptf_id[1] @@ -40,54 +44,63 @@ def ptf_csp(ptf_id): req = CostConditionsRequest() req = ptf_id[1] return flag, CostConditionsResponse(cost_per_sec=1, cost_to_complete=1) + + class MyTaskER(TaskER): - def __init__(self,da_state_name, da_name): + def __init__(self, da_state_name, da_name): self.name = da_name self.sim_mode = None self.conversation_interface = None self.kb_places = None - TaskER.__init__(self,da_state_name) - self.sis = smach_ros.IntrospectionServer(str("/"+self.name+"smach_view_server"), self, self.name) + TaskER.__init__(self, da_state_name) + self.sis = smach_ros.IntrospectionServer( + str("/"+self.name+"smach_view_server"), self, self.name) self.sis.start() - places_xml_filename = rospy.get_param('/kb_places_xml') - self.sim_mode = str(rospy.get_param('/sim_mode')) + places_xml_filename = self.get_parameter( + '/kb_places_xml').get_parameter_value().string_value + self.sim_mode = str(self.get_parameter( + '/sim_mode').get_parameter_value().string_value) assert self.sim_mode in ['sim', 'gazebo', 'real'] - print 'Reading KB for places from file "' + places_xml_filename + '"' + print('Reading KB for places from file "' + places_xml_filename + '"') self.kb_places = kb_p.PlacesXmlParser(places_xml_filename).getKB() - self.conversation_interface = rcprg_smach.conversation.ConversationMachine([ - ('ack', 'projects/incare-dialog-agent/agent/intents/ef92199b-d298-470c-8df3-1e1047dd70d1'), - ('ack_i_took', 'ack_i_took'), - ('ack_i_gave', 'ack_i_gave'), - ('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'), - ], self.sim_mode) - self.my_fsm = rcprg_smach.bring_goods_tasker.BringGoods(self.sim_mode, self.conversation_interface, self.kb_places) + ('ack', 'projects/incare-dialog-agent/agent/intents/ef92199b-d298-470c-8df3-1e1047dd70d1'), + ('ack_i_took', 'ack_i_took'), + ('ack_i_gave', 'ack_i_gave'), + ('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'), + ], self.sim_mode) + self.my_fsm = rcprg_smach.bring_goods_tasker.BringGoods( + self.sim_mode, self.conversation_interface, self.kb_places) + def shutdownRequest(self): - print ("my-tasker ----------------------- shutdown") + print("my-tasker ----------------------- shutdown") self.conversation_interface.stop() self.sis.stop() + def cleanup_tf(self): - rospy.loginfo('{}: Executing state: {}'.format(rospy.get_name(), self.__class__.__name__)) - print 'Cleanup.execute' + rospy.loginfo('{}: Executing state: {}'.format( + rospy.get_name(), self.__class__.__name__)) + print('Cleanup.execute') self.conversation_interface.stop() return 'ok' - def get_suspension_tf(self,susp_data): - print "My TASKER -- get_suspension_tf" - print "My TASKER" - print "My TASKER" - rospy.loginfo('{}: Executing state: {}'.format(rospy.get_name(), self.__class__.__name__)) - print 'GetSuspend.execute' - print "susp data: ", susp_data.getData() - print "susp data[0]: ", susp_data.getData()[0] + def get_suspension_tf(self, susp_data): + print("My TASKER -- get_suspension_tf") + print("My TASKER") + print("My TASKER") + rospy.loginfo('{}: Executing state: {}'.format( + rospy.get_name(), self.__class__.__name__)) + print('GetSuspend.execute') + print("susp data: ", susp_data.getData()) + print("susp data[0]: ", susp_data.getData()[0]) data = susp_data.getData() fsm_executable = None for idx in range(2, len(data), 2): - print data[idx] + print(data[idx]) if data[idx] == 'executable': fsm_executable = data[idx+1] elif data[idx] == 'rosrun': @@ -95,45 +108,52 @@ def get_suspension_tf(self,susp_data): ros_exec = data[idx+2] fsm_executable = "rosrun "+ros_pkg+" "+ros_exec if fsm_executable == None: - print "harmoniser did not specified executable for suspension behaviour, terminating task" + print( + "harmoniser did not specified executable for suspension behaviour, terminating task") fsm_executable = "terminate task" return fsm_executable - def exe_suspension_tf(self,fsm_es_in): - print "My TASKER -- exe_suspension_tf" - print "My TASKER" - print "My TASKER" - rospy.loginfo('{}: Executing state: {}'.format(rospy.get_name(), self.__class__.__name__)) - print 'ExecSuspension.execute' - #srv.shutdown() - print fsm_es_in + def exe_suspension_tf(self, fsm_es_in): + print("My TASKER -- exe_suspension_tf") + print("My TASKER") + print("My TASKER") + rospy.loginfo('{}: Executing state: {}'.format( + rospy.get_name(), self.__class__.__name__)) + print('ExecSuspension.execute') + # srv.shutdown() + print(fsm_es_in) if fsm_es_in == "terminate task": return 'shutdown' else: - p = subprocess.Popen(fsm_es_in , shell=True, stdout=subprocess.PIPE, stderr=subprocess.PIPE) + p = subprocess.Popen(fsm_es_in, shell=True, + stdout=subprocess.PIPE, stderr=subprocess.PIPE) # and you can block util the cmd execute finish p.wait() return 'FINISHED' + def wait_tf(self): - print "My TASKER -- wait_tf" - print "My TASKER" - print "My TASKER" + print("My TASKER -- wait_tf") + print("My TASKER") + print("My TASKER") pass + def update_task_tf(self): - print "My TASKER -- update_task_tf" - print "My TASKER" - print "My TASKER" + print("My TASKER -- update_task_tf") + print("My TASKER") + print("My TASKER") self.sis.stop() imp.reload(rcprg_smach.bring_goods_tasker) - print "SWAPPING" - self.swap_state('ExecFSM', rcprg_smach.bring_goods_tasker.BringGoods(self.sim_mode, self.conversation_interface, self.kb_places)) + print("SWAPPING") + self.swap_state('ExecFSM', rcprg_smach.bring_goods_tasker.BringGoods( + self.sim_mode, self.conversation_interface, self.kb_places)) self.sis.start() # self.my_fsm.set_initial_state(['MoveToKitchen']) pass + def initialise_tf(self): - print "My TASKER -- initialise" + print("My TASKER -- initialise") if len(sys.argv) < 3: raise Exception('Too few arguments: ' + str(sys.argv)) @@ -143,7 +163,8 @@ def initialise_tf(self): goods_name = sys.argv[idx+1] if goods_name is None: - raise Exception('Argument "goods_name" is missing in argv: ' + str(sys.argv)) + raise Exception( + 'Argument "goods_name" is missing in argv: ' + str(sys.argv)) if isinstance(goods_name, str): goods_name = goods_name.decode('utf-8') @@ -156,27 +177,33 @@ def initialise_tf(self): self.userdata.goal = goods_name_m self.userdata.fsm_es = "" + + def main(): + rclpy.init() da_name = None da_type = None da_id = None da_state_name = [] for idx in range(1, len(sys.argv), 2): - if sys.argv[idx] == 'da_name': - da_name = sys.argv[idx+1] - if sys.argv[idx] == 'da_type': - da_type = sys.argv[idx+1] - if sys.argv[idx] == 'da_id': - da_id = sys.argv[idx+1] + if sys.argv[idx] == 'da_name': + da_name = sys.argv[idx+1] + if sys.argv[idx] == 'da_type': + da_type = sys.argv[idx+1] + if sys.argv[idx] == 'da_id': + da_id = sys.argv[idx+1] if da_name == None or da_type == None or da_id == None: - print "DA: one of the parameters (, , or ) is not specified" + print( + "DA: one of the parameters (, , or ) is not specified") return 1 - da = DynAgent( da_name, da_id, da_type, ptf_csp, da_state_name ) - print "RUN BRING" - da.run( MyTaskER(da_state_name,da_name) ) - print "BG ENDED" + da = DynAgent(da_name, da_id, da_type, ptf_csp, da_state_name) + print("RUN BRING") + da.run(MyTaskER(da_state_name, da_name)) + print("BG ENDED") + rclpy.shutdown() return 0 + if __name__ == '__main__': main() - print "ALLLLLLL CLOOOOOSSSSEEEEED" + print("ALLLLLLL CLOOOOOSSSSEEEEED") diff --git a/nodes/bring_jar.py b/nodes/bring_jar.py index c15a89c..9b57fa7 100755 --- a/nodes/bring_jar.py +++ b/nodes/bring_jar.py @@ -1,9 +1,11 @@ -#!/usr/bin/env python +#!/usr/bin/env python3 # encoding: utf8 import sys -import rospy +import rclpy +from rclpy.node import Node + import smach import smach_ros @@ -18,15 +20,16 @@ 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): - rospy.loginfo('{}: Executing state: {}'.format(rospy.get_name(), self.__class__.__name__)) - print 'Cleanup.execute' + self.get_logger().info('{}: Executing state: {}'.format( + self.get_name(), self.__class__.__name__)) + print('Cleanup.execute') self.conversation_interface.stop() return 'ok' @@ -34,15 +37,20 @@ def execute(self, userdata): # The SM that govenrs the highest-level behaviour. # -class MainSM(smach_rcprg.StateMachine): + +class MainSM(Node, smach_rcprg.StateMachine): def __init__(self): - smach_rcprg.StateMachine.__init__(self, outcomes=['Finished', 'shutdown']) + Node.__init__(self, 'main_sm') + smach_rcprg.StateMachine.__init__( + self, outcomes=['Finished', 'shutdown']) - places_xml_filename = rospy.get_param('/kb_places_xml') - sim_mode = str(rospy.get_param('/sim_mode')) + # Adjust parameters for ROS2 + places_xml_filename = self.get_parameter('kb_places_xml').value + sim_mode = self.get_parameter('sim_mode').value assert sim_mode in ['sim', 'gazebo', 'real'] - print 'Reading KB for places from file "' + places_xml_filename + '"' + self.get_logger().info( + f'Reading KB for places from file "{places_xml_filename}"') kb_places = kb_p.PlacesXmlParser(places_xml_filename).getKB() if len(sys.argv) < 3: @@ -58,61 +66,68 @@ def __init__(self): if sys.argv[idx] == 'end_pose': end_pose = sys.argv[idx+1] - if object_container is None: - raise Exception('Argument "object_container" is missing in argv: ' + str(sys.argv)) + raise Exception( + 'Argument "object_container" is missing in argv: ' + str(sys.argv)) if isinstance(object_container, str): object_container = object_container.decode('utf-8') object_container = object_container.encode('utf-8').decode('utf-8') self.userdata.object_container = object_container if bring_destination is None: - raise Exception('Argument "bring_destination" is missing in argv: ' + str(sys.argv)) + raise Exception( + 'Argument "bring_destination" is missing in argv: ' + str(sys.argv)) if isinstance(bring_destination, str): bring_destination = bring_destination.decode('utf-8') bring_destination = bring_destination.encode('utf-8').decode('utf-8') self.userdata.bring_destination = bring_destination if end_pose is None: - raise Exception('Argument "end_pose" is missing in argv: ' + str(sys.argv)) + raise Exception( + 'Argument "end_pose" is missing in argv: ' + str(sys.argv)) if isinstance(end_pose, str): end_pose = end_pose.decode('utf-8') end_pose = end_pose.encode('utf-8').decode('utf-8') - self.userdata.end_pose = end_pose + self.userdata.end_pose = end_pose 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) + ('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() - with self: smach_rcprg.StateMachine.add('BringJar', - rcprg_smach.bring_jar.BringJar(sim_mode, self.conversation_interface, kb_places), - transitions={'FINISHED':'Cleanup', 'PREEMPTED':'Cleanup', 'FAILED': 'Cleanup', - 'shutdown':'shutdown'}, - remapping={'goal':'goal'}) + rcprg_smach.bring_jar.BringJar( + 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={ }) - + 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_jar' ) - da.run( MainSM() ) +def main(args=None): + rclpy.init(args=args) + + da = DynAgent('bring_jar') + da.run(MainSM()) + + rclpy.shutdown() return 0 + if __name__ == '__main__': main() diff --git a/nodes/bring_jar_tasker.py b/nodes/bring_jar_tasker.py index c3ba3cc..b76fc92 100755 --- a/nodes/bring_jar_tasker.py +++ b/nodes/bring_jar_tasker.py @@ -1,12 +1,16 @@ -#!/usr/bin/env python +#!/usr/bin/env python3 # encoding: utf8 import sys -import rospy +import rclpy +from rclpy.node import Node + import smach import smach_ros -import rospkg + +from ament_index_python.packages import get_package_share_directory + import rcprg_smach.conversation import rcprg_smach.bring_jar @@ -25,7 +29,7 @@ from tasker_msgs.msg import RobotResource, ScheduleParams from tasker_msgs.srv import SuspendConditionsRequest, SuspendConditionsResponse from tasker_msgs.srv import CostConditionsRequest, CostConditionsResponse -# for ExeSuspension state + import subprocess import imp @@ -33,26 +37,22 @@ sim_mode = 'sim' human_name = None + +def get_package_path(package_name): + try: + package_path = get_package_share_directory(package_name) + return package_path + except LookupError: + return None + + def ptf_csp(ptf_id): - print "calculating SP: ", ptf_id + print("calculating SP: ", ptf_id) global human_name - # human_posture = rospy.get_param("/"+human_name+"/actor_posture", "stand") - # # set flag 'self-terminate' to terminate DA if in Wait or Init state flag = None - # if human_posture == 'stand': - # flag = 'self-terminate' - # csp_handler = rcprg_smach.gh_csp.GH_csp(human_name) - # robot_pose_updater = rcprg_smach.update_pose.UpdatePose(sim_mode, kb_places, "Rico") - # robot_pose_updater.update_pose("/base_link") - # robot_pose = csp_handler.get_robot_pose(sim_mode, kb_places, "Rico") - # human_pose_updater = rcprg_smach.update_pose.UpdatePose(sim_mode, kb_places, human_name) - # human_frame = "/"+human_name - # human_pose_updater.update_pose(human_frame) - # human_pose = csp_handler.get_robot_pose(sim_mode, kb_places, human_name) if ptf_id[0] == "scheduleParams": - #return [flag, csp_handler.get_schedule_params_hf(robot_pose=robot_pose, human_pose=human_pose)] - return [flag, ScheduleParams(cost = 10, completion_time=1,cost_per_sec=1,final_resource_state=RobotResource())] + return [flag, ScheduleParams(cost=10, completion_time=1, cost_per_sec=1, final_resource_state=RobotResource())] elif ptf_id[0] == "suspendCondition": req = SuspendConditionsRequest() req = ptf_id[1] @@ -61,75 +61,91 @@ def ptf_csp(ptf_id): req = CostConditionsRequest() req = ptf_id[1] return [flag, CostConditionsResponse(cost_per_sec=1, cost_to_complete=1)] + + class PulledOut(): def __init__(self): self.value = False + def set(self, trigger): self.value = trigger + def get(self): return self.value -class MyTaskER(TaskER): - def __init__(self,da_state_name, da_name): + + +class MyTaskER(TaskER, Node): + def __init__(self, da_state_name, da_name): global kb_places global sim_mode self.name = unicode(da_name) - self.sim_mode = None self.conversation_interface = None - self.kb_places = None - TaskER.__init__(self,da_state_name) - self.sis = smach_ros.IntrospectionServer(unicode(str("/"+self.name+"smach_view_server")), self, unicode(self.name)) + + self.declare_parameter('kb_places_xml') + self.declare_parameter('sim_mode') + + TaskER.__init__(self, da_state_name) + self.sis = smach_ros.IntrospectionServer( + unicode(str("/"+self.name+"smach_view_server")), self, unicode(self.name)) self.sis.start() - places_xml_filename = rospy.get_param('/kb_places_xml') - self.sim_mode = str(rospy.get_param('/sim_mode')) + places_xml_filename = self.get_parameter('kb_places_xml').value + self.sim_mode = str(self.get_parameter('sim_mode').value) assert self.sim_mode in ['sim', 'gazebo', 'real'] sim_mode = self.sim_mode - print 'Reading KB for places from file "' + places_xml_filename + '"' + print('Reading KB for places from file "{}"'.format(places_xml_filename)) self.kb_places = kb_p.PlacesXmlParser(places_xml_filename).getKB() kb_places = self.kb_places self.pulled_out = PulledOut() self.conversation_interface = rcprg_smach.conversation.ConversationMachine([ - ('ack', 'projects/incare-dialog-agent/agent/intents/ef92199b-d298-470c-8df3-1e1047dd70d1'), - ('ack_i_took', 'ack_i_took'), - ('ack_i_gave', 'ack_i_gave'), - ('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'), - ], self.sim_mode) - self.my_fsm = rcprg_smach.bring_jar.BringJar(self.sim_mode, self.conversation_interface, self.kb_places, self.pulled_out) + ('ack', 'projects/incare-dialog-agent/agent/intents/ef92199b-d298-470c-8df3-1e1047dd70d1'), + ('ack_i_took', 'ack_i_took'), + ('ack_i_gave', 'ack_i_gave'), + ('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'), + ], self.sim_mode) + self.my_fsm = rcprg_smach.bring_jar.BringJar( + self.sim_mode, self.conversation_interface, self.kb_places, self.pulled_out) + def shutdownRequest(self): - print ("my-tasker ----------------------- shutdown") + print("my-tasker ----------------------- shutdown") self.conversation_interface.stop() self.sis.stop() self.sis.clear() + def cleanup_tf(self): - rospy.loginfo('{}: Executing state: {}'.format(rospy.get_name(), self.__class__.__name__)) - print 'Cleanup.execute' + rclpy.loginfo('{}: Executing state: {}'.format( + rclpy.get_name(), self.__class__.__name__)) + print('Cleanup.execute') self.conversation_interface.stop() self.sis.stop() self.sis.clear() return 'ok' - def get_suspension_tf(self,susp_data): - print "My TASKER -- get_suspension_tf" - print "My TASKER" - print "My TASKER" - rospy.loginfo('{}: Executing state: {}'.format(rospy.get_name(), self.__class__.__name__)) - print 'GetSuspend.execute' - print "susp data: ", susp_data.getData() - print "susp data[0]: ", susp_data.getData()[0] + def get_suspension_tf(self, susp_data): + print("My TASKER -- get_suspension_tf") + print("My TASKER") + print("My TASKER") + rclpy.loginfo('{}: Executing state: {}'.format( + rclpy.get_name(), self.__class__.__name__)) + print('GetSuspend.execute') + print("susp data: ", susp_data.getData()) + print("susp data[0]: ", susp_data.getData()[0]) data = susp_data.getData() fsm_executable = None rospack = rospkg.RosPack() # list all packages, equivalent to rospack list - rospack.list() - print "GET_SUSPEND:\nPULLED_OUT: ", self.pulled_out.get() + # rospack.list() + print("GET_SUSPEND:\nPULLED_OUT: ", self.pulled_out.get()) # if self.pulled_out.get() == True: - ts_path = rospack.get_path('rcprg_smach') - mod = imp.load_source("BringJarSuspension",ts_path+"/src/rcprg_smach/suspend_bj.py") + ts_path = get_package_path('rcprg_smach') + mod = imp.load_source("BringJarSuspension", + ts_path+"/src/rcprg_smach/suspend_bj.py") self.sis.stop() self.sis.clear() - self.swap_state(u'ExeSuspension', mod.BringJarSuspension(self.sim_mode, self.conversation_interface, kb_places, self.pulled_out.get())) + self.swap_state(u'ExeSuspension', mod.BringJarSuspension( + self.sim_mode, self.conversation_interface, kb_places, self.pulled_out.get())) self.sis.start() fsm_executable = self.userdata.object_container return fsm_executable @@ -147,44 +163,50 @@ def get_suspension_tf(self,susp_data): # fsm_executable = "terminate task" # return fsm_executable - def exe_suspension_tf(self,fsm_es_in): - print "My TASKER -- exe_suspension_tf" - print "My TASKER" - print "My TASKER" - rospy.loginfo('{}: Executing state: {}'.format(rospy.get_name(), self.__class__.__name__)) - print 'ExecSuspension.execute' - #srv.shutdown() - print fsm_es_in + def exe_suspension_tf(self, fsm_es_in): + print("My TASKER -- exe_suspension_tf") + print("My TASKER") + print("My TASKER") + rclpy.loginfo('{}: Executing state: {}'.format( + rclpy.get_name(), self.__class__.__name__)) + print('ExecSuspension.execute') + # srv.shutdown() + print(fsm_es_in) if fsm_es_in == None: return 'FINISHED' elif fsm_es_in == "terminate task": return 'shutdown' else: - p = subprocess.Popen(fsm_es_in , shell=True, stdout=subprocess.PIPE, stderr=subprocess.PIPE) + p = subprocess.Popen(fsm_es_in, shell=True, + stdout=subprocess.PIPE, stderr=subprocess.PIPE) # and you can block util the cmd execute finish p.wait() return 'FINISHED' + def wait_tf(self): - print "My TASKER -- wait_tf" - print "My TASKER" - print "My TASKER" + print("My TASKER -- wait_tf") + print("My TASKER") + print("My TASKER") pass + def update_task_tf(self): - print "My TASKER -- update_task_tf" - print "My TASKER" - print "My TASKER" + print("My TASKER -- update_task_tf") + print("My TASKER") + print("My TASKER") self.sis.stop() self.sis.clear() imp.reload(rcprg_smach.bring_jar) - print "SWAPPING" - self.swap_state('ExecFSM', rcprg_smach.bring_jar.BringJar(self.sim_mode, self.conversation_interface, self.kb_places, self.pulled_out)) + print("SWAPPING") + self.swap_state('ExecFSM', rcprg_smach.bring_jar.BringJar( + self.sim_mode, self.conversation_interface, self.kb_places, self.pulled_out)) self.sis.start() # self.my_fsm.set_initial_state(['MoveToKitchen']) pass + def initialise_tf(self): - print "My TASKER -- initialise" + print("My TASKER -- initialise") global human_name if len(sys.argv) < 3: raise Exception('Too few arguments: ' + str(sys.argv)) @@ -199,27 +221,29 @@ def initialise_tf(self): if sys.argv[idx] == 'end_pose': end_pose = sys.argv[idx+1] - if object_container is None: - raise Exception('Argument "object_container" is missing in argv: ' + str(sys.argv)) + raise Exception( + 'Argument "object_container" is missing in argv: ' + str(sys.argv)) if isinstance(object_container, str): object_container = object_container.decode('utf-8') object_container = object_container.encode('utf-8').decode('utf-8') self.userdata.object_container = object_container if bring_destination is None: - raise Exception('Argument "bring_destination" is missing in argv: ' + str(sys.argv)) + raise Exception( + 'Argument "bring_destination" is missing in argv: ' + str(sys.argv)) if isinstance(bring_destination, str): bring_destination = bring_destination.decode('utf-8') bring_destination = bring_destination.encode('utf-8').decode('utf-8') self.userdata.bring_destination = bring_destination if end_pose is None: - raise Exception('Argument "end_pose" is missing in argv: ' + str(sys.argv)) + raise Exception( + 'Argument "end_pose" is missing in argv: ' + str(sys.argv)) if isinstance(end_pose, str): end_pose = end_pose.decode('utf-8') end_pose = end_pose.encode('utf-8').decode('utf-8') - self.userdata.end_pose = end_pose + self.userdata.end_pose = end_pose # if isinstance(goods_name, str): # goods_name = goods_name.decode('utf-8') # goods_name = goods_name.encode('utf-8').decode('utf-8') @@ -230,26 +254,31 @@ def initialise_tf(self): self.conversation_interface.start() self.userdata.fsm_es = "" + + def main(): da_name = None da_type = None da_id = None da_state_name = [] for idx in range(1, len(sys.argv), 2): - if sys.argv[idx] == 'da_name': - da_name = sys.argv[idx+1] - if sys.argv[idx] == 'da_type': - da_type = sys.argv[idx+1] - if sys.argv[idx] == 'da_id': - da_id = sys.argv[idx+1] + if sys.argv[idx] == 'da_name': + da_name = sys.argv[idx+1] + if sys.argv[idx] == 'da_type': + da_type = sys.argv[idx+1] + if sys.argv[idx] == 'da_id': + da_id = sys.argv[idx+1] if da_name == None or da_type == None or da_id == None: - print "DA: one of the parameters (, , or ) is not specified" + print( + "DA: one of the parameters (, , or ) is not specified") return 1 - da = DynAgent( da_name, da_id, da_type, ptf_csp, da_state_name ) - print "RUN BRING" - da.run( MyTaskER(da_state_name,da_name) ) - print "BG ENDED" + + da = DynAgent(da_name, da_id, da_type, ptf_csp, da_state_name) + print("RUN BRING") + da.run(MyTaskER(da_state_name, da_name)) + print("BG ENDED") return 0 + if __name__ == '__main__': exit(main()) diff --git a/nodes/call.py b/nodes/call.py index d8bd6fe..1e8a88a 100755 --- a/nodes/call.py +++ b/nodes/call.py @@ -1,11 +1,12 @@ -#!/usr/bin/env python +#!/usr/bin/env python3 # encoding: utf8 import sys import time import math -import rospy +import rclpy +from rclpy.node import Node import smach import smach_ros @@ -21,6 +22,7 @@ from pl_nouns.dictionary_client import DisctionaryServiceClient + class CheckGoalProximity(smach_rcprg.State): def __init__(self, sim_mode, conversation_interface, kb_places): self.sim_mode = sim_mode @@ -28,11 +30,12 @@ def __init__(self, sim_mode, conversation_interface, kb_places): self.kb_places = kb_places smach_rcprg.State.__init__(self, input_keys=['goal', 'current_pose'], - outcomes=['ok', 'far_away', 'preemption', 'shutdown']) + outcomes=['ok', 'far_away', 'preemption', 'shutdown']) def execute(self, userdata): - rospy.loginfo('{}: Executing state: {}'.format(rospy.get_name(), self.__class__.__name__)) - print 'CheckGoalProximity.execute' + rclpy.loginfo('{}: Executing state: {}'.format( + rclpy.get_name(), self.__class__.__name__)) + print('CheckGoalProximity.execute') if self.sim_mode == 'sim': self.conversation_interface.startListening() @@ -44,27 +47,28 @@ def execute(self, userdata): mc_name = 'sim' goal_palce_name = userdata.goal.parameters['place_name'] - goal_pl = self.kb_places.getPlaceByName( goal_palce_name, mc_name ) + goal_pl = self.kb_places.getPlaceByName(goal_palce_name, mc_name) current_pose = userdata.current_pose.parameters['pose'] current_place_pos = (current_pose.position.x, current_pose.position.y) - #current_pl_id = self.kb_places.whatIsAt( current_place_pos, mc_name ) - #print 'CheckGoalProximity current_pl_id', current_pl_id - #current_pl = self.kb_places.getPlaceById(current_pl_id, mc_name) + # current_pl_id = self.kb_places.whatIsAt( current_place_pos, mc_name ) + # print 'CheckGoalProximity current_pl_id', current_pl_id + # current_pl = self.kb_places.getPlaceById(current_pl_id, mc_name) if goal_pl.getType() == 'point': pt_g = goal_pl.getPt() - #pt_c = current_pl.getPt() + # pt_c = current_pl.getPt() pt_c = current_place_pos - if math.sqrt( (pt_g[0]-pt_c[0])**2 + (pt_g[1]-pt_c[1])**2 ) < 2.0: + if math.sqrt((pt_g[0]-pt_c[0])**2 + (pt_g[1]-pt_c[1])**2) < 2.0: self.conversation_interface.startListening() return 'ok' else: return 'far_away' else: - raise Exception( 'Call to place of type "{}" is not supported.'.format(goal_pl.getType()) ) + raise Exception( + 'Call to place of type "{}" is not supported.'.format(goal_pl.getType())) if self.preempt_requested(): return 'preemption' @@ -74,6 +78,7 @@ def execute(self, userdata): return 'ok' + class Cleanup(smach_rcprg.State): def __init__(self, conversation_interface): self.conversation_interface = conversation_interface @@ -81,8 +86,9 @@ def __init__(self, conversation_interface): smach_rcprg.State.__init__(self, outcomes=['ok', 'shutdown']) def execute(self, userdata): - rospy.loginfo('{}: Executing state: {}'.format(rospy.get_name(), self.__class__.__name__)) - print 'Cleanup.execute' + rclpy.loginfo('{}: Executing state: {}'.format( + rclpy.get_name(), self.__class__.__name__)) + print('Cleanup.execute') self.conversation_interface.stop() return 'ok' @@ -90,16 +96,18 @@ def execute(self, userdata): # The SM that govenrs the highest-level behaviour. # + class MainSM(smach_rcprg.StateMachine): def __init__(self): - smach_rcprg.StateMachine.__init__(self, outcomes=['Finished', 'shutdown']) + smach_rcprg.StateMachine.__init__( + self, outcomes=['Finished', 'shutdown']) t1 = time.time() - places_xml_filename = rospy.get_param('/kb_places_xml') - sim_mode = str(rospy.get_param('/sim_mode')) + places_xml_filename = rclpy.get_param('/kb_places_xml') + sim_mode = str(rclpy.get_param('/sim_mode')) assert sim_mode in ['sim', 'gazebo', 'real'] - print 'Reading KB for places from file "' + places_xml_filename + '"' + print('Reading KB for places from file "' + places_xml_filename + '"') kb_places = kb_p.PlacesXmlParser(places_xml_filename).getKB() if len(sys.argv) < 3: @@ -111,66 +119,74 @@ def __init__(self): place_name = sys.argv[idx+1] if place_name is None: - raise Exception('Argument "place_name" is missing in argv: ' + str(sys.argv)) + raise Exception( + 'Argument "place_name" is missing in argv: ' + str(sys.argv)) if isinstance(place_name, str): place_name = place_name.decode('utf-8') t2 = time.time() - print 'reading KB for places took', (t2-t1) + print('reading KB for places took', (t2-t1)) dictionary = DisctionaryServiceClient() place_name_m = dictionary.getCases(place_name).getCase('mianownik') # Tests: - #print dictionary.getCases(u'łazienkę').getCase('mianownik') - #print dictionary.getCases(u'szczoteczką').getCase('mianownik') + # print dictionary.getCases(u'łazienkę').getCase('mianownik') + # print dictionary.getCases(u'szczoteczką').getCase('mianownik') t3 = time.time() - print 'reading Dictionary took', (t3-t2) + print('reading Dictionary took', (t3-t2)) self.conversation_interface = rcprg_smach.conversation.ConversationMachine([ - ('q_current_task', 'projects/incare-dialog-agent/agent/intents/8f45359d-ee47-4e10-a1b2-de3f3223e5b4'), - ],sim_mode) + ('q_current_task', 'projects/incare-dialog-agent/agent/intents/8f45359d-ee47-4e10-a1b2-de3f3223e5b4'), + ], sim_mode) self.conversation_interface.start() - self.userdata.goal = PoseDescription({'place_name':place_name_m}) + self.userdata.goal = PoseDescription({'place_name': place_name_m}) with self: smach_rcprg.StateMachine.add('RememberCurrentPose', rcprg_smach.navigation.RememberCurrentPose(sim_mode), - transitions={'ok':'CheckGoalProximity', 'preemption':'Cleanup', 'error': 'Cleanup', - 'shutdown':'shutdown'}, - remapping={'current_pose':'current_pose'}) + transitions={'ok': 'CheckGoalProximity', 'preemption': 'Cleanup', 'error': 'Cleanup', + 'shutdown': 'shutdown'}, + remapping={'current_pose': 'current_pose'}) smach_rcprg.StateMachine.add('CheckGoalProximity', - CheckGoalProximity(sim_mode, self.conversation_interface, kb_places), - transitions={'ok':'Cleanup', 'far_away':'MoveTo', 'preemption':'Cleanup', - 'shutdown':'shutdown'}, - remapping={'goal':'goal', 'current_pose':'current_pose'}) + CheckGoalProximity( + sim_mode, self.conversation_interface, kb_places), + transitions={'ok': 'Cleanup', 'far_away': 'MoveTo', 'preemption': 'Cleanup', + 'shutdown': 'shutdown'}, + remapping={'goal': 'goal', 'current_pose': 'current_pose'}) smach_rcprg.StateMachine.add('MoveTo', - rcprg_smach.tiago_torso_controller.MoveToComplexTorsoMid(sim_mode, self.conversation_interface, kb_places), - transitions={'FINISHED':'Cleanup', 'PREEMPTED':'Cleanup', 'FAILED': 'Cleanup', - 'shutdown':'shutdown'}, - remapping={'goal':'goal'}) + rcprg_smach.tiago_torso_controller.MoveToComplexTorsoMid( + 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={ }) + Cleanup(self.conversation_interface), + transitions={ + 'ok': 'Finished', 'shutdown': 'shutdown'}, + remapping={}) t4 = time.time() - print 'starting SMACH took', (t4-t3) + print('starting SMACH took', (t4-t3)) def shutdownRequest(self): self.conversation_interface.stop() self.request_preempt() -def main(): - da = DynAgent( 'call' ) - da.run( MainSM() ) - return 0 +def main(args=None): + rclpy.init(args=args) + + da = DynAgent('call') + da.run(MainSM()) + + rclpy.shutdown() + if __name__ == '__main__': main() diff --git a/nodes/guide_human_tasker.py b/nodes/guide_human_tasker.py index bf44325..3ba53d4 100755 --- a/nodes/guide_human_tasker.py +++ b/nodes/guide_human_tasker.py @@ -1,11 +1,12 @@ -#!/usr/bin/env python +#!/usr/bin/env python3 # encoding: utf8 import sys import math -import rospy -import rospkg +import rclpy +from rclpy.node import Node + import smach import smach_ros @@ -34,11 +35,12 @@ kb_places = None sim_mode = None human_name = None -guide_destination = None -greeted = None +guide_destination = None +greeted = None + def ptf_csp(ptf_id): - print "calculating SP: ", ptf_id + print("calculating SP: ", ptf_id) global kb_places global sim_mode global human_name @@ -48,25 +50,32 @@ def ptf_csp(ptf_id): flag = None csp_handler = rcprg_smach.gh_csp.GH_csp(human_name) is_human = False - robot_pose_updater = rcprg_smach.update_pose.UpdatePose(sim_mode, kb_places, "Rico") + robot_pose_updater = rcprg_smach.update_pose.UpdatePose( + sim_mode, kb_places, "Rico") robot_pose_updater.update_pose("/base_link", False, is_human) robot_pose = csp_handler.get_robot_pose(sim_mode, kb_places, "Rico") is_human = True - human_pose_updater = rcprg_smach.update_pose.UpdatePose(sim_mode, kb_places, human_name) + human_pose_updater = rcprg_smach.update_pose.UpdatePose( + sim_mode, kb_places, human_name) human_frame = "/"+human_name human_pose_updater.update_pose(human_frame, False, is_human) human_pose = csp_handler.get_robot_pose(sim_mode, kb_places, human_name) human_approach_pose = Pose() - human_yaw = euler_from_quaternion([human_pose.orientation.x,human_pose.orientation.y,human_pose.orientation.z,human_pose.orientation.w])[2] - human_approach_pose.position.x = human_pose.position.x +1*math.cos(human_yaw) - human_approach_pose.position.y = human_pose.position.y +1*math.sin(human_yaw) - (human_approach_pose.orientation.x,human_approach_pose.orientation.y,\ - human_approach_pose.orientation.z,human_approach_pose.orientation.w) = quaternion_from_euler(0,0,human_yaw-math.pi) + human_yaw = euler_from_quaternion( + [human_pose.orientation.x, human_pose.orientation.y, human_pose.orientation.z, human_pose.orientation.w])[2] + human_approach_pose.position.x = human_pose.position.x + \ + 1*math.cos(human_yaw) + human_approach_pose.position.y = human_pose.position.y + \ + 1*math.sin(human_yaw) + (human_approach_pose.orientation.x, human_approach_pose.orientation.y, + human_approach_pose.orientation.z, human_approach_pose.orientation.w) = quaternion_from_euler(0, 0, human_yaw-math.pi) # print "H_approach: ", human_approach_pose - destination_pose = csp_handler.get_robot_pose(sim_mode, kb_places, guide_destination) + destination_pose = csp_handler.get_robot_pose( + sim_mode, kb_places, guide_destination) if destination_pose == 'is-volumetric': - destination_pose = csp_handler.get_robot_pose(sim_mode, kb_places, guide_destination, human_name) + destination_pose = csp_handler.get_robot_pose( + sim_mode, kb_places, guide_destination, human_name) if ptf_id[0] == "scheduleParams": return [flag, csp_handler.get_schedule_params_gh(robot_pose, human_approach_pose, destination_pose, human_name)] # return ScheduleParams(cost = 1, completion_time=1,cost_per_sec=1,final_resource_state=RobotResource()) @@ -77,18 +86,25 @@ def ptf_csp(ptf_id): elif ptf_id[0] == "startCondition": req = CostConditionsRequest() req = ptf_id[1] - return [flag, csp_handler.get_cost_conditions_gh(req, human_approach_pose, human_name)]# CostConditionsResponse(cost_per_sec=1, cost_to_complete=1)] + # CostConditionsResponse(cost_per_sec=1, cost_to_complete=1)] + return [flag, csp_handler.get_cost_conditions_gh(req, human_approach_pose, human_name)] + + class Greeted(): def __init__(self): self.value = False + def set(self, trigger): self.value = trigger + def get(self): return self.value + + class MyTaskER(TaskER): - def __init__(self,da_state_name, da_name): + def __init__(self, da_state_name, da_name): self.name = unicode(da_name) - rospy.init_node(self.name) + rclpy.init_node(self.name) self.sim_mode = None self.conversation_interface = None global kb_places @@ -97,29 +113,30 @@ def __init__(self,da_state_name, da_name): global guide_destination global greeted kb_places = None - TaskER.__init__(self,da_state_name) - rospy.init_node(self.name) - self.sis = smach_ros.IntrospectionServer(unicode(str("/"+self.name+"smach_view_server")), self, unicode(self.name)) + TaskER.__init__(self, da_state_name) + rclpy.init_node(self.name) + self.sis = smach_ros.IntrospectionServer( + unicode(str("/"+self.name+"smach_view_server")), self, unicode(self.name)) self.sis.start() - places_xml_filename = rospy.get_param('/kb_places_xml') - self.sim_mode = str(rospy.get_param('/sim_mode')) + places_xml_filename = rclpy.get_param('/kb_places_xml') + self.sim_mode = str(rclpy.get_param('/sim_mode')) assert self.sim_mode in ['sim', 'gazebo', 'real'] sim_mode = self.sim_mode - print 'Reading KB for places from file "' + places_xml_filename + '"' + print('Reading KB for places from file "' + places_xml_filename + '"') kb_places = kb_p.PlacesXmlParser(places_xml_filename).getKB() - self.conversation_interface = rcprg_smach.conversation.ConversationMachine([ - ('ack', 'projects/incare-dialog-agent/agent/intents/ef92199b-d298-470c-8df3-1e1047dd70d1'), - ('ack_i_took', 'ack_i_took'), - ('ack_i_gave', 'ack_i_gave'), - ('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'), - ], self.sim_mode) + ('ack', 'projects/incare-dialog-agent/agent/intents/ef92199b-d298-470c-8df3-1e1047dd70d1'), + ('ack_i_took', 'ack_i_took'), + ('ack_i_gave', 'ack_i_gave'), + ('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'), + ], self.sim_mode) self.greeted = Greeted() greeted = self.greeted - self.my_fsm = rcprg_smach.guide_human.GuideHuman(self.sim_mode, self.conversation_interface, kb_places, self.greeted) + self.my_fsm = rcprg_smach.guide_human.GuideHuman( + self.sim_mode, self.conversation_interface, kb_places, self.greeted) human_name = None guide_destination = None for idx in range(1, len(sys.argv), 2): @@ -128,7 +145,8 @@ def __init__(self,da_state_name, da_name): if sys.argv[idx] == 'guide_destination': guide_destination = sys.argv[idx+1] if human_name is None: - raise Exception('Argument "human_name" is missing in argv: ' + str(sys.argv)) + raise Exception( + 'Argument "human_name" is missing in argv: ' + str(sys.argv)) if isinstance(human_name, str): human_name = human_name.decode('utf-8') @@ -136,25 +154,29 @@ def __init__(self,da_state_name, da_name): if isinstance(guide_destination, str): guide_destination = guide_destination.decode('utf-8') guide_destination = guide_destination.encode('utf-8').decode('utf-8') + def shutdownRequest(self): - print ("my-tasker ----------------------- shutdown") + print("my-tasker ----------------------- shutdown") self.conversation_interface.stop() self.sis.stop() self.sis.clear() + def cleanup_tf(self): - rospy.loginfo('{}: Executing state: {}'.format(rospy.get_name(), self.__class__.__name__)) - print 'Cleanup.execute' + rclpy.loginfo('{}: Executing state: {}'.format( + rclpy.get_name(), self.__class__.__name__)) + print('Cleanup.execute') self.conversation_interface.stop() return 'ok' - def get_suspension_tf(self,susp_data): - print "My TASKER -- get_suspension_tf" - print "My TASKER" - print "My TASKER" - rospy.loginfo('{}: Executing state: {}'.format(rospy.get_name(), self.__class__.__name__)) - print 'GetSuspend.execute' - print "susp data: ", susp_data.getData() - print "susp data[0]: ", susp_data.getData()[0] + def get_suspension_tf(self, susp_data): + print("My TASKER -- get_suspension_tf") + print("My TASKER") + print("My TASKER") + rclpy.loginfo('{}: Executing state: {}'.format( + rclpy.get_name(), self.__class__.__name__)) + print('GetSuspend.execute') + print("susp data: ", susp_data.getData()) + print("susp data[0]: ", susp_data.getData()[0]) data = susp_data.getData() global kb_places # fsm_executable = None @@ -173,15 +195,17 @@ def get_suspension_tf(self,susp_data): rospack = rospkg.RosPack() # list all packages, equivalent to rospack list - rospack.list() - print "GET_SUSPEND:GREETED: ", self.greeted.get() + rospack.list() + print("GET_SUSPEND:GREETED: ", self.greeted.get()) if self.greeted.get() == True: - # get the file path for rospy_tutorials + # get the file path for rclpy_tutorials ts_path = rospack.get_path('rcprg_smach') - mod = imp.load_source("SuspGH",ts_path+"/src/rcprg_smach/suspend_gh.py") + mod = imp.load_source( + "SuspGH", ts_path+"/src/rcprg_smach/suspend_gh.py") self.sis.stop() self.sis.clear() - self.swap_state(u'ExeSuspension', mod.SuspGH(self.sim_mode, self.conversation_interface, kb_places)) + self.swap_state(u'ExeSuspension', mod.SuspGH( + self.sim_mode, self.conversation_interface, kb_places)) self.sis.start() fsm_executable = self.userdata.human_name return fsm_executable @@ -189,45 +213,51 @@ def get_suspension_tf(self,susp_data): fsm_executable = None return fsm_executable - def exe_suspension_tf(self,fsm_es_in): - print "My TASKER -- exe_suspension_tf" - print "My TASKER" - print "My TASKER" - rospy.loginfo('{}: Executing state: {}'.format(rospy.get_name(), self.__class__.__name__)) - print 'ExecSuspension.execute' - #srv.shutdown() - print fsm_es_in + def exe_suspension_tf(self, fsm_es_in): + print("My TASKER -- exe_suspension_tf") + print("My TASKER") + print("My TASKER") + rclpy.loginfo('{}: Executing state: {}'.format( + rclpy.get_name(), self.__class__.__name__)) + print('ExecSuspension.execute') + # srv.shutdown() + print(fsm_es_in) if fsm_es_in == None: return 'FINISHED' elif fsm_es_in == "terminate task": return 'shutdown' else: - p = subprocess.Popen(fsm_es_in , shell=True, stdout=subprocess.PIPE, stderr=subprocess.PIPE) + p = subprocess.Popen(fsm_es_in, shell=True, + stdout=subprocess.PIPE, stderr=subprocess.PIPE) # and you can block util the cmd execute finish p.wait() return 'FINISHED' + def wait_tf(self): - print "My TASKER -- wait_tf" - print "My TASKER" - print "My TASKER" + print("My TASKER -- wait_tf") + print("My TASKER") + print("My TASKER") pass + def update_task_tf(self): - print "My TASKER -- update_task_tf" - print "My TASKER" - print "My TASKER" + print("My TASKER -- update_task_tf") + print("My TASKER") + print("My TASKER") self.sis.stop() self.sis.clear() global kb_places imp.reload(rcprg_smach.guide_human) - print "SWAPPING" - self.swap_state('ExecFSM', rcprg_smach.guide_human.GuideHuman(self.sim_mode, self.conversation_interface, kb_places,self.greeted)) + print("SWAPPING") + self.swap_state('ExecFSM', rcprg_smach.guide_human.GuideHuman( + self.sim_mode, self.conversation_interface, kb_places, self.greeted)) self.sis.start() # self.my_fsm.set_initial_state(['MoveToKitchen']) pass + def initialise_tf(self): - print "My TASKER -- initialise" + print("My TASKER -- initialise") if len(sys.argv) < 3: raise Exception('Too few arguments: ' + str(sys.argv)) @@ -239,10 +269,12 @@ def initialise_tf(self): guide_destination = sys.argv[idx+1] if human_name is None: - raise Exception('Argument "human_name" is missing in argv: ' + str(sys.argv)) + raise Exception( + 'Argument "human_name" is missing in argv: ' + str(sys.argv)) if guide_destination is None: - raise Exception('Argument "guide_destination" is missing in argv: ' + str(sys.argv)) - + raise Exception( + 'Argument "guide_destination" is missing in argv: ' + str(sys.argv)) + if isinstance(human_name, str): human_name = human_name.decode('utf-8') human_name = human_name.encode('utf-8').decode('utf-8') @@ -255,27 +287,36 @@ def initialise_tf(self): self.userdata.human_name = unicode(human_name) self.userdata.guide_destination = unicode(guide_destination) self.userdata.fsm_es = "" -def main(): + + +def main(args=None): + rclpy.init(args=args) + da_name = None da_type = None da_id = None da_state_name = [] + for idx in range(1, len(sys.argv), 2): - if sys.argv[idx] == 'da_name': - da_name = sys.argv[idx+1] - if sys.argv[idx] == 'da_type': - da_type = sys.argv[idx+1] - if sys.argv[idx] == 'da_id': - da_id = sys.argv[idx+1] - if da_name == None or da_type == None or da_id == None: - print "DA: one of the parameters (, , or ) is not specified" + if sys.argv[idx] == 'da_name': + da_name = sys.argv[idx+1] + if sys.argv[idx] == 'da_type': + da_type = sys.argv[idx+1] + if sys.argv[idx] == 'da_id': + da_id = sys.argv[idx+1] + + if da_name is None or da_type is None or da_id is None: + print( + "DA: one of the parameters (, , or ) is not specified") return 1 - da = DynAgent( da_name, da_id, da_type, ptf_csp, da_state_name) - print "RUN BRING" - da.run( MyTaskER(da_state_name,da_name) ) - print "BG ENDED" + + da = DynAgent(da_name, da_id, da_type, ptf_csp, da_state_name) + print("RUN BRING") + da.run(MyTaskER(da_state_name, da_name)) + print("BG ENDED") return 0 + if __name__ == '__main__': main() - print "ALLLLLLL CLOOOOOSSSSEEEEED" + print("ALLLLLLL CLOOOOOSSSSEEEEED") diff --git a/nodes/human_fell_approach_tasker.py b/nodes/human_fell_approach_tasker.py index 505eaec..92192a2 100755 --- a/nodes/human_fell_approach_tasker.py +++ b/nodes/human_fell_approach_tasker.py @@ -1,70 +1,72 @@ -#!/usr/bin/env python +#!/usr/bin/env python3 # encoding: utf8 - import sys -import math - -import rospy +import rclpy +from rclpy.node import Node import smach -import smach_ros +from smach_ros2 import IntrospectionServer from geometry_msgs.msg import Pose -from tf.transformations import quaternion_from_euler, euler_from_quaternion +from tf_transformations import quaternion_from_euler, euler_from_quaternion import math +# Import your custom modules here, you may need to adjust them for ROS2. import rcprg_smach.conversation import rcprg_smach.human_fell import rcprg_smach.smach_rcprg as smach_rcprg import rcprg_smach.gh_csp import rcprg_smach.update_pose - import rcprg_kb.places_xml as kb_p - from rcprg_smach.task_manager import PoseDescription from pl_nouns.dictionary_client import DisctionaryServiceClient from TaskER.TaskER import TaskER from TaskER.dynamic_agent import DynAgent from tasker_msgs.msg import RobotResource, ScheduleParams -from tasker_msgs.srv import SuspendConditionsRequest, SuspendConditionsResponse -from tasker_msgs.srv import CostConditionsRequest, CostConditionsResponse -# for ExeSuspension state +from tasker_msgs.srv import SuspendConditions +from tasker_msgs.srv import CostConditions + import subprocess -import imp -from geometry_msgs.msg import Pose -from tf.transformations import quaternion_from_euler, euler_from_quaternion +import importlib kb_places = None sim_mode = 'real' human_name = None -base_frame='/base_link' +base_frame = '/base_link' + + def ptf_csp(ptf_id): - print "calculating SP: ", ptf_id + print(f"calculating SP: {ptf_id}") global human_name - print "CSP -> human_name: ", human_name + print(f"CSP -> human_name: {human_name}") global base_frame - human_posture = rospy.get_param("/"+human_name+"/actor_posture", "stand") + human_posture = rclpy.get_param("/"+human_name+"/actor_posture", "stand") # set flag 'self-terminate' to terminate DA if in Wait or Init state flag = None if human_posture == 'stand': flag = 'self-terminate' csp_handler = rcprg_smach.gh_csp.GH_csp(human_name) - robot_pose_updater = rcprg_smach.update_pose.UpdatePose(sim_mode, kb_places, "Rico") + robot_pose_updater = rcprg_smach.update_pose.UpdatePose( + sim_mode, kb_places, "Rico") robot_pose_updater.update_pose(base_frame, False) robot_pose = csp_handler.get_robot_pose(sim_mode, kb_places, "Rico") - human_pose_updater = rcprg_smach.update_pose.UpdatePose(sim_mode, kb_places, human_name) + human_pose_updater = rcprg_smach.update_pose.UpdatePose( + sim_mode, kb_places, human_name) human_frame = "/"+human_name human_pose_updater.update_pose(human_frame, False) human_pose = csp_handler.get_robot_pose(sim_mode, kb_places, human_name) human_approach_pose = Pose() - human_yaw = euler_from_quaternion([human_pose.orientation.x,human_pose.orientation.y,human_pose.orientation.z,human_pose.orientation.w])[2] - human_approach_pose.position.x = human_pose.position.x +1*math.cos(human_yaw) - human_approach_pose.position.y = human_pose.position.y +1*math.sin(human_yaw) - (human_approach_pose.orientation.x,human_approach_pose.orientation.y,\ - human_approach_pose.orientation.z,human_approach_pose.orientation.w) = quaternion_from_euler(0,0,human_yaw-math.pi) + human_yaw = euler_from_quaternion( + [human_pose.orientation.x, human_pose.orientation.y, human_pose.orientation.z, human_pose.orientation.w])[2] + human_approach_pose.position.x = human_pose.position.x + \ + 1*math.cos(human_yaw) + human_approach_pose.position.y = human_pose.position.y + \ + 1*math.sin(human_yaw) + (human_approach_pose.orientation.x, human_approach_pose.orientation.y, + human_approach_pose.orientation.z, human_approach_pose.orientation.w) = quaternion_from_euler(0, 0, human_yaw-math.pi) if ptf_id[0] == "scheduleParams": return [flag, csp_handler.get_schedule_params_hf(robot_pose=robot_pose, human_pose=human_approach_pose)] - #ScheduleParams(cost = 10, completion_time=1,cost_per_sec=1,final_resource_state=RobotResource()) + # ScheduleParams(cost = 10, completion_time=1,cost_per_sec=1,final_resource_state=RobotResource()) elif ptf_id[0] == "suspendCondition": req = SuspendConditionsRequest() req = ptf_id[1] @@ -73,8 +75,10 @@ def ptf_csp(ptf_id): req = CostConditionsRequest() req = ptf_id[1] return [flag, CostConditionsResponse(cost_per_sec=1, cost_to_complete=1)] + + class MyTaskER(TaskER): - def __init__(self,da_state_name, da_name): + def __init__(self, da_state_name, da_name): global kb_places global sim_mode global human_name @@ -82,110 +86,124 @@ def __init__(self,da_state_name, da_name): self.sim_mode = None self.conversation_interface = None self.kb_places = None - TaskER.__init__(self,da_state_name) - self.sis = smach_ros.IntrospectionServer(unicode(str("/"+self.name+"smach_view_server")), self, unicode(self.name)) + TaskER.__init__(self, da_state_name) + self.sis = smach_ros.IntrospectionServer( + unicode(str("/"+self.name+"smach_view_server")), self, unicode(self.name)) self.sis.start() - places_xml_filename = rospy.get_param('/kb_places_xml') - self.sim_mode = str(rospy.get_param('/sim_mode')) + places_xml_filename = rclpy.get_param('/kb_places_xml') + self.sim_mode = str(rclpy.get_param('/sim_mode')) assert self.sim_mode in ['sim', 'gazebo', 'real'] sim_mode = self.sim_mode - print 'Reading KB for places from file "' + places_xml_filename + '"' + print(f'Reading KB for places from file "{places_xml_filename}"') self.kb_places = kb_p.PlacesXmlParser(places_xml_filename).getKB() kb_places = self.kb_places self.conversation_interface = rcprg_smach.conversation.ConversationMachine([ - ('ack', 'projects/incare-dialog-agent/agent/intents/ef92199b-d298-470c-8df3-1e1047dd70d1'), - ('ack_i_took', 'ack_i_took'), - ('ack_i_gave', 'ack_i_gave'), - ('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'), - ], self.sim_mode) + ('ack', 'projects/incare-dialog-agent/agent/intents/ef92199b-d298-470c-8df3-1e1047dd70d1'), + ('ack_i_took', 'ack_i_took'), + ('ack_i_gave', 'ack_i_gave'), + ('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'), + ], self.sim_mode) if len(sys.argv) < 2: raise Exception('Too few arguments: ' + str(sys.argv)) - print "ARGS:", sys.argv + print("ARGS:", sys.argv) goods_name = None for idx in range(1, len(sys.argv), 2): if sys.argv[idx] == 'human_name': human_name = sys.argv[idx+1] if human_name is None: - raise Exception('Argument "human_name" is missing in argv: ' + str(sys.argv)) + raise Exception( + 'Argument "human_name" is missing in argv: ' + str(sys.argv)) if isinstance(human_name, str): human_name = human_name.decode('utf-8') human_name = human_name.encode('utf-8').decode('utf-8') - - self.my_fsm = rcprg_smach.human_fell.HumanFell(self.sim_mode, self.conversation_interface, self.kb_places) + + self.my_fsm = rcprg_smach.human_fell.HumanFell( + self.sim_mode, self.conversation_interface, self.kb_places) + def shutdownRequest(self): - print ("my-tasker ----------------------- shutdown") + print("my-tasker ----------------------- shutdown") self.conversation_interface.stop() self.sis.stop() self.sis.clear() + def cleanup_tf(self): - rospy.loginfo('{}: Executing state: {}'.format(rospy.get_name(), self.__class__.__name__)) - print 'Cleanup.execute' + rclpy.loginfo('{}: Executing state: {}'.format( + rclpy.get_name(), self.__class__.__name__)) + print('Cleanup.execute') self.conversation_interface.stop() return 'ok' - def get_suspension_tf(self,susp_data): - print "My TASKER -- get_suspension_tf" - print "My TASKER" - print "My TASKER" - rospy.loginfo('{}: Executing state: {}'.format(rospy.get_name(), self.__class__.__name__)) - print 'GetSuspend.execute' - print "susp data: ", susp_data.getData() - print "susp data[0]: ", susp_data.getData()[0] + def get_suspension_tf(self, susp_data): + print("My TASKER -- get_suspension_tf") + print("My TASKER") + print("My TASKER") + rclpy.loginfo('{}: Executing state: {}'.format( + rclpy.get_name(), self.__class__.__name__)) + print('GetSuspend.execute') + print("susp data: ", susp_data.getData()) + print("susp data[0]: ", susp_data.getData()[0]) data = susp_data.getData() fsm_executable = None for idx in range(2, len(data), 2): - print data[idx] + print(data[idx]) if data[idx] == 'executable': fsm_executable = data[idx+1] - elif data[idx] == 'rosrun': + elif data[idx] == 'ros2run': ros_pkg = data[idx+1] ros_exec = data[idx+2] fsm_executable = "rosrun "+ros_pkg+" "+ros_exec if fsm_executable == None: - print "harmoniser did not specified executable for suspension behaviour, terminating task" + print( + "harmoniser did not specified executable for suspension behaviour, terminating task") fsm_executable = "terminate task" return fsm_executable - def exe_suspension_tf(self,fsm_es_in): - print "My TASKER -- exe_suspension_tf" - print "My TASKER" - print "My TASKER" - rospy.loginfo('{}: Executing state: {}'.format(rospy.get_name(), self.__class__.__name__)) - print 'ExecSuspension.execute' - #srv.shutdown() - print fsm_es_in + def exe_suspension_tf(self, fsm_es_in): + print("My TASKER -- exe_suspension_tf") + print("My TASKER") + print("My TASKER") + rclpy.loginfo('{}: Executing state: {}'.format( + rclpy.get_name(), self.__class__.__name__)) + print('ExecSuspension.execute') + # srv.shutdown() + print(fsm_es_in) if fsm_es_in == "terminate task": return 'shutdown' else: - p = subprocess.Popen(fsm_es_in , shell=True, stdout=subprocess.PIPE, stderr=subprocess.PIPE) + p = subprocess.Popen(fsm_es_in, shell=True, + stdout=subprocess.PIPE, stderr=subprocess.PIPE) # and you can block util the cmd execute finish p.wait() return 'FINISHED' + def wait_tf(self): - print "My TASKER -- wait_tf" - print "My TASKER" - print "My TASKER" + print("My TASKER -- wait_tf") + print("My TASKER") + print("My TASKER") pass + def update_task_tf(self): - print "My TASKER -- update_task_tf" - print "My TASKER" - print "My TASKER" + print("My TASKER -- update_task_tf") + print("My TASKER") + print("My TASKER") self.sis.stop() self.sis.clear() imp.reload(rcprg_smach.human_fell) - print "SWAPPING" - self.swap_state('ExecFSM', rcprg_smach.human_fell.HumanFell(self.sim_mode, self.conversation_interface, self.kb_places)) + print("SWAPPING") + self.swap_state('ExecFSM', rcprg_smach.human_fell.HumanFell( + self.sim_mode, self.conversation_interface, self.kb_places)) self.sis.start() # self.my_fsm.set_initial_state(['MoveToKitchen']) pass + def initialise_tf(self): - print "My TASKER -- initialise" + print("My TASKER -- initialise") global human_name # if isinstance(goods_name, str): @@ -199,27 +217,35 @@ def initialise_tf(self): self.userdata.human_name = human_name self.userdata.fsm_es = "" + + def main(): + rclpy.init() da_name = None da_type = None da_id = None da_state_name = [] + for idx in range(1, len(sys.argv), 2): - if sys.argv[idx] == 'da_name': - da_name = sys.argv[idx+1] - if sys.argv[idx] == 'da_type': - da_type = sys.argv[idx+1] - if sys.argv[idx] == 'da_id': - da_id = sys.argv[idx+1] + if sys.argv[idx] == 'da_name': + da_name = sys.argv[idx+1] + if sys.argv[idx] == 'da_type': + da_type = sys.argv[idx+1] + if sys.argv[idx] == 'da_id': + da_id = sys.argv[idx+1] + if da_name == None or da_type == None or da_id == None: - print "DA: one of the parameters (, , or ) is not specified" + print( + "DA: one of the parameters (, , or ) is not specified") return 1 - da = DynAgent( da_name, da_id, da_type, ptf_csp, da_state_name ) - print "RUN BRING" - da.run( MyTaskER(da_state_name,da_name) ) - print "BG ENDED" + + da = DynAgent(da_name, da_id, da_type, ptf_csp, da_state_name) + print("RUN BRING") + da.run(MyTaskER(da_state_name, da_name)) + print("BG ENDED") return 0 + if __name__ == '__main__': main() - print "ALLLLLLL CLOOOOOSSSSEEEEED" + print("ALLLLLLL CLOOOOOSSSSEEEEED") diff --git a/nodes/human_fell_tasker.py b/nodes/human_fell_tasker.py index a1a96f8..e602f0b 100755 --- a/nodes/human_fell_tasker.py +++ b/nodes/human_fell_tasker.py @@ -1,15 +1,16 @@ -#!/usr/bin/env python +#!/usr/bin/env python3 # encoding: utf8 import sys import math -import rospy +import rclpy +from rclpy.node import Node import smach -import smach_ros from geometry_msgs.msg import Pose +from tf2_ros import Buffer, TransformListener +from tf2_geometry_msgs import PoseStamped from tf.transformations import quaternion_from_euler, euler_from_quaternion -import math import rcprg_smach.conversation import rcprg_smach.human_fell @@ -36,37 +37,45 @@ kb_places = None sim_mode = 'real' human_name = None -base_frame='/base_link' +base_frame = 'base_link' + + def ptf_csp(ptf_id): - print "calculating SP: ", ptf_id + print("calculating SP: ", ptf_id) global human_name - print "CSP -> human_name: ", human_name + print("CSP -> human_name: ", human_name) global base_frame - human_posture = rospy.get_param("/"+human_name+"/actor_posture", "stand") - # set flag 'self-terminate' to terminate DA if in Wait or Init state + node = rclpy.create_node('temporary_node_for_param_get') + human_posture = node.get_parameter("/"+human_name+"/actor_posture").value + if human_posture is None: + human_posture = "stand" flag = None if human_posture == 'stand': flag = 'self-terminate' csp_handler = rcprg_smach.gh_csp.GH_csp(human_name) is_human = False - robot_pose_updater = rcprg_smach.update_pose.UpdatePose(sim_mode, kb_places, "Rico") + robot_pose_updater = rcprg_smach.update_pose.UpdatePose( + sim_mode, kb_places, "Rico") robot_pose_updater.update_pose(base_frame, False, is_human) robot_pose = csp_handler.get_robot_pose(sim_mode, kb_places, "Rico") - human_pose_updater = rcprg_smach.update_pose.UpdatePose(sim_mode, kb_places, human_name) + human_pose_updater = rcprg_smach.update_pose.UpdatePose( + sim_mode, kb_places, human_name) human_frame = "/"+human_name is_human = True - human_pose_updater.update_pose(human_frame, False,is_human) + human_pose_updater.update_pose(human_frame, False, is_human) human_pose = csp_handler.get_robot_pose(sim_mode, kb_places, human_name) human_approach_pose = Pose() - human_yaw = euler_from_quaternion([human_pose.orientation.x,human_pose.orientation.y,human_pose.orientation.z,human_pose.orientation.w])[2] - human_approach_pose.position.x = human_pose.position.x +1*math.cos(human_yaw) - human_approach_pose.position.y = human_pose.position.y +1*math.sin(human_yaw) - (human_approach_pose.orientation.x,human_approach_pose.orientation.y,\ - human_approach_pose.orientation.z,human_approach_pose.orientation.w) = quaternion_from_euler(0,0,human_yaw-math.pi) + human_yaw = euler_from_quaternion( + [human_pose.orientation.x, human_pose.orientation.y, human_pose.orientation.z, human_pose.orientation.w])[2] + human_approach_pose.position.x = human_pose.position.x + \ + 1*math.cos(human_yaw) + human_approach_pose.position.y = human_pose.position.y + \ + 1*math.sin(human_yaw) + (human_approach_pose.orientation.x, human_approach_pose.orientation.y, + human_approach_pose.orientation.z, human_approach_pose.orientation.w) = quaternion_from_euler(0, 0, human_yaw-math.pi) if ptf_id[0] == "scheduleParams": return [flag, csp_handler.get_schedule_params_hf(robot_pose=robot_pose, human_pose=human_approach_pose)] - #ScheduleParams(cost = 10, completion_time=1,cost_per_sec=1,final_resource_state=RobotResource()) elif ptf_id[0] == "suspendCondition": req = SuspendConditionsRequest() req = ptf_id[1] @@ -75,74 +84,80 @@ def ptf_csp(ptf_id): req = CostConditionsRequest() req = ptf_id[1] return [flag, CostConditionsResponse(cost_per_sec=1, cost_to_complete=1)] + + class MyTaskER(TaskER): - def __init__(self,da_state_name, da_name): + def __init__(self, da_state_name, da_name): global kb_places global sim_mode global human_name - self.name = unicode(da_name) - rospy.init_node(self.name) + self.name = da_name + rclpy.init() + self.node = Node(self.name) self.sim_mode = None self.conversation_interface = None self.kb_places = None - TaskER.__init__(self,da_state_name) - rospy.init_node(self.name) - self.sis = smach_ros.IntrospectionServer(unicode(str("/"+self.name+"smach_view_server")), self, unicode(self.name)) - self.sis.start() - places_xml_filename = rospy.get_param('/kb_places_xml') - self.sim_mode = str(rospy.get_param('/sim_mode')) + TaskER.__init__(self, da_state_name) + + places_xml_filename = self.node.get_parameter('kb_places_xml').value + self.sim_mode = self.node.get_parameter('sim_mode').value assert self.sim_mode in ['sim', 'gazebo', 'real'] sim_mode = self.sim_mode - print 'Reading KB for places from file "' + places_xml_filename + '"' + self.node.get_logger().info('Reading KB for places from file "%s"' % + places_xml_filename) self.kb_places = kb_p.PlacesXmlParser(places_xml_filename).getKB() kb_places = self.kb_places self.conversation_interface = rcprg_smach.conversation.ConversationMachine([ - ('ack', 'projects/incare-dialog-agent/agent/intents/ef92199b-d298-470c-8df3-1e1047dd70d1'), - ('ack_i_took', 'ack_i_took'), - ('ack_i_gave', 'ack_i_gave'), - ('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'), - ], self.sim_mode) + ('ack', 'projects/incare-dialog-agent/agent/intents/ef92199b-d298-470c-8df3-1e1047dd70d1'), + ('ack_i_took', 'ack_i_took'), + ('ack_i_gave', 'ack_i_gave'), + ('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'), + ], self.sim_mode) if len(sys.argv) < 2: raise Exception('Too few arguments: ' + str(sys.argv)) - print "ARGS:", sys.argv + print("ARGS:", sys.argv) goods_name = None for idx in range(1, len(sys.argv), 2): if sys.argv[idx] == 'human_name': human_name = sys.argv[idx+1] if human_name is None: - raise Exception('Argument "human_name" is missing in argv: ' + str(sys.argv)) + raise Exception( + 'Argument "human_name" is missing in argv: ' + str(sys.argv)) if isinstance(human_name, str): human_name = human_name.decode('utf-8') human_name = human_name.encode('utf-8').decode('utf-8') - - self.my_fsm = rcprg_smach.human_fell.HumanFell(self.sim_mode, self.conversation_interface, self.kb_places) + + self.my_fsm = rcprg_smach.human_fell.HumanFell( + self.sim_mode, self.conversation_interface, self.kb_places) + def shutdownRequest(self): - print ("my-tasker ----------------------- shutdown") + print("my-tasker ----------------------- shutdown") self.conversation_interface.stop() self.sis.stop() self.sis.clear() + def cleanup_tf(self): - rospy.loginfo('{}: Executing state: {}'.format(rospy.get_name(), self.__class__.__name__)) - print 'Cleanup.execute' + # node.get_logger().info('{}: Executing state: {}'.format(node.get_name(), self.__class__.__name__)) + print('Cleanup.execute') self.conversation_interface.stop() return 'ok' - def get_suspension_tf(self,susp_data): - print "My TASKER -- get_suspension_tf" - print "My TASKER" - print "My TASKER" - rospy.loginfo('{}: Executing state: {}'.format(rospy.get_name(), self.__class__.__name__)) - print 'GetSuspend.execute' - print "susp data: ", susp_data.getData() - print "susp data[0]: ", susp_data.getData()[0] + def get_suspension_tf(self, susp_data): + print("My TASKER -- get_suspension_tf") + print("My TASKER") + print("My TASKER") + # node.get_logger().info('{}: Executing state: {}'.format(node.get_name(), self.__class__.__name__)) + print('GetSuspend.execute') + print("susp data: ", susp_data.getData()) + print("susp data[0]: ", susp_data.getData()[0]) data = susp_data.getData() fsm_executable = None for idx in range(2, len(data), 2): - print data[idx] + print(data[idx]) if data[idx] == 'executable': fsm_executable = data[idx+1] elif data[idx] == 'rosrun': @@ -150,46 +165,53 @@ def get_suspension_tf(self,susp_data): ros_exec = data[idx+2] fsm_executable = "rosrun "+ros_pkg+" "+ros_exec if fsm_executable == None: - print "harmoniser did not specified executable for suspension behaviour, terminating task" + print( + "harmoniser did not specified executable for suspension behaviour, terminating task") fsm_executable = "terminate task" return fsm_executable - def exe_suspension_tf(self,fsm_es_in): - print "My TASKER -- exe_suspension_tf" - print "My TASKER" - print "My TASKER" - rospy.loginfo('{}: Executing state: {}'.format(rospy.get_name(), self.__class__.__name__)) - print 'ExecSuspension.execute' - #srv.shutdown() - print fsm_es_in + def exe_suspension_tf(self, fsm_es_in): + print("My TASKER -- exe_suspension_tf") + print("My TASKER") + print("My TASKER") + rospy.loginfo('{}: Executing state: {}'.format( + rospy.get_name(), self.__class__.__name__)) + print('ExecSuspension.execute') + # srv.shutdown() + print(fsm_es_in) if fsm_es_in == "terminate task": return 'shutdown' else: - p = subprocess.Popen(fsm_es_in , shell=True, stdout=subprocess.PIPE, stderr=subprocess.PIPE) + p = subprocess.Popen(fsm_es_in, shell=True, + stdout=subprocess.PIPE, stderr=subprocess.PIPE) # and you can block util the cmd execute finish p.wait() return 'FINISHED' + def wait_tf(self): - print "My TASKER -- wait_tf" - print "My TASKER" - print "My TASKER" + print("My TASKER -- wait_tf") + print("My TASKER") + print("My TASKER") pass + def update_task_tf(self): - print "My TASKER -- update_task_tf" - print "My TASKER" - print "My TASKER" + print("My TASKER -- update_task_tf") + print("My TASKER") + print("My TASKER") self.sis.stop() self.sis.clear() imp.reload(rcprg_smach.human_fell) - print "SWAPPING" - self.swap_state('ExecFSM', rcprg_smach.human_fell.HumanFell(self.sim_mode, self.conversation_interface, self.kb_places)) + print("SWAPPING") + self.swap_state('ExecFSM', rcprg_smach.human_fell.HumanFell( + self.sim_mode, self.conversation_interface, self.kb_places)) self.sis.start() # self.my_fsm.set_initial_state(['MoveToKitchen']) pass + def initialise_tf(self): - print "My TASKER -- initialise" + print("My TASKER -- initialise") global human_name # if isinstance(goods_name, str): @@ -203,27 +225,33 @@ def initialise_tf(self): self.userdata.human_name = human_name self.userdata.fsm_es = "" + + def main(): + rclpy.init() da_name = None da_type = None da_id = None da_state_name = [] for idx in range(1, len(sys.argv), 2): - if sys.argv[idx] == 'da_name': - da_name = sys.argv[idx+1] - if sys.argv[idx] == 'da_type': - da_type = sys.argv[idx+1] - if sys.argv[idx] == 'da_id': - da_id = sys.argv[idx+1] - if da_name == None or da_type == None or da_id == None: - print "DA: one of the parameters (, , or ) is not specified" + if sys.argv[idx] == 'da_name': + da_name = sys.argv[idx + 1] + if sys.argv[idx] == 'da_type': + da_type = sys.argv[idx + 1] + if sys.argv[idx] == 'da_id': + da_id = sys.argv[idx + 1] + if da_name is None or da_type is None or da_id is None: + print( + "DA: one of the parameters (, , or ) is not specified") return 1 - da = DynAgent( da_name, da_id, da_type, ptf_csp, da_state_name ) - print "RUN BRING" - da.run( MyTaskER(da_state_name,da_name) ) - print "BG ENDED" + da = DynAgent(da_name, da_id, da_type, ptf_csp, da_state_name) + print("RUN BRING") + da.run(MyTaskER(da_state_name, da_name)) + print("BG ENDED") + rclpy.shutdown() return 0 + if __name__ == '__main__': main() - print "ALLLLLLL CLOOOOOSSSSEEEEED" + print("ALLLLLLL CLOOOOOSSSSEEEEED") diff --git a/nodes/idle.py b/nodes/idle.py index fb86d15..00f54a7 100755 --- a/nodes/idle.py +++ b/nodes/idle.py @@ -1,10 +1,12 @@ -#!/usr/bin/env python +#!/usr/bin/env python3 # encoding: utf8 import sys import time -import rospy +import rclpy +from rclpy.node import Node + import smach import smach_ros @@ -20,15 +22,14 @@ class MainState(smach_rcprg.State): def __init__(self, conversation_interface): self.conversation_interface = conversation_interface - smach_rcprg.State.__init__(self, outcomes=['shutdown', 'preemption']) def execute(self, userdata): - rospy.loginfo('{}: Executing state: {}'.format(rospy.get_name(), self.__class__.__name__)) - print 'MainState.execute' + rclpy.logging.get_logger(self.__class__.__name__).info(f'Executing state: {self.__class__.__name__}') + print('MainState.execute') - answer1_id = self.conversation_interface.setAutomaticAnswer( 'q_current_task', u'Nic nie robię' ) - answer2_id = self.conversation_interface.setAutomaticAnswer( 'q_load', u'niekorzystne warunki pogodowe nic nie wiozę' ) + answer1_id = self.conversation_interface.setAutomaticAnswer('q_current_task', u'Nic nie robię') + answer2_id = self.conversation_interface.setAutomaticAnswer('q_load', u'niekorzystne warunki pogodowe nic nie wiozę') while True: if self.preempt_requested(): @@ -45,27 +46,21 @@ def execute(self, userdata): self.service_preempt() return 'shutdown' - rospy.sleep(0.1) + time.sleep(0.1) raise Exception('Unreachable code') -# -# The SM that govenrs the highest-level behaviour. -# - class MainSM(smach_rcprg.StateMachine): def __init__(self): - smach_rcprg.StateMachine.__init__(self, outcomes=['Finished', 'shutdown']) - - places_xml_filename = rospy.get_param('/kb_places_xml') - sim_mode = str(rospy.get_param('/sim_mode')) + super().__init__(outcomes=['Finished', 'shutdown']) + + node = rclpy.create_node('MainSM_node') + places_xml_filename = node.get_parameter('/kb_places_xml').value + sim_mode = str(node.get_parameter('/sim_mode').value) assert sim_mode in ['sim', 'gazebo', 'real'] self.conversation_interface = rcprg_smach.conversation.ConversationMachine([ ('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') - #('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'), ],sim_mode) self.conversation_interface.start() @@ -79,11 +74,13 @@ def shutdownRequest(self): self.conversation_interface.stop() self.request_preempt() -def main(): - da = DynAgent( 'idle' ) - da.run( MainSM() ) +def main(args=None): + rclpy.init(args=args) + da = DynAgent('idle') + da.run(MainSM()) + rclpy.shutdown() return 0 if __name__ == '__main__': - main() + main() \ No newline at end of file diff --git a/nodes/idle_tasker.py b/nodes/idle_tasker.py index 7000bc7..54c4b22 100755 --- a/nodes/idle_tasker.py +++ b/nodes/idle_tasker.py @@ -1,9 +1,9 @@ -#!/usr/bin/env python +#!/usr/bin/env python3 # encoding: utf8 import sys - -import rospy +import rclpy +from rclpy.node import Node import smach import smach_ros @@ -30,54 +30,36 @@ USE_SMACH_INRTOSPECTION_SERVER = False class MainState(TaskER.SuspendableState): - # def __init__(self, conversation_interface): def __init__(self): - # self.conversation_interface = conversation_interface - TaskER.SuspendableState.__init__(self, outcomes=['shutdown', 'PREEMPTED']) def transition_function(self, userdata): - rospy.loginfo('{}: Executing state: {}'.format(rospy.get_name(), self.__class__.__name__)) - print 'MainState.execute' + node = rclpy.create_node(self.__class__.__name__) + node.get_logger().info('{}: Executing state: {}'.format(node.get_name(), self.__class__.__name__)) + print('MainState.execute') # answer1_id = self.conversation_interface.setAutomaticAnswer( 'q_current_task', u'Nic nie robię' ) # answer2_id = self.conversation_interface.setAutomaticAnswer( 'q_load', u'niekorzystne warunki pogodowe nic nie wiozę' ) while True: - if self.is_suspension_flag() != None: - print"" - print"" - print"" - print "IDLE GOT SUSP FLAG" - print"" - print"" - print"" - print"" - # self.conversation_interface.removeAutomaticAnswer(answer1_id) - # self.conversation_interface.removeAutomaticAnswer(answer2_id) - # self.conversation_interface.stop() - # self.request_preempt() - return 'PREEMPTED' - - if self.__shutdown__: - print "IDLE GOT SHUTDOWN FLAG" - # self.conversation_interface.removeAutomaticAnswer(answer1_id) - # self.conversation_interface.removeAutomaticAnswer(answer2_id) - # self.conversation_interface.stop() - self.request_preempt() - return 'shutdown' - - rospy.sleep(0.2) + if self.is_suspension_flag() is not None: + print("\n" * 4 + "IDLE GOT SUSP FLAG" + "\n" * 4) + + if self.__shutdown__: + print("IDLE GOT SHUTDOWN FLAG") + self.request_preempt() + return 'shutdown' + + rclpy.spin_once(node, timeout_sec=0.2) raise Exception('Unreachable code') def ptf_csp(ptf_id): - #print "calculating SP: ", ptf_id global my_name - # set flag 'self-terminate' to terminate DA if in Wait or Init state + node = rclpy.create_node('ptf_csp_node') flag = None - self_terminate_flag = rospy.get_param('/term_ID', 'false') - cost = rospy.get_param("/cost_ID_"+my_name, 5) - print "self_terminate_flag: ", self_terminate_flag + self_terminate_flag = node.get_parameter('/term_ID').value + cost = node.get_parameter("/cost_ID_" + my_name).value + print("self_terminate_flag: ", self_terminate_flag) if self_terminate_flag == True: flag = 'self-terminate' if ptf_id[0] == "scheduleParams": @@ -92,12 +74,13 @@ def ptf_csp(ptf_id): return [flag, CostConditionsResponse(cost_per_sec=0, cost_to_complete=0)] class MyTaskER(TaskER): - def __init__(self,da_state_name, da_name): + def __init__(self, da_state_name, da_name): global USE_SMACH_INRTOSPECTION_SERVER global my_name - my_name = unicode(da_name) + my_name = str(da_name) self.name = my_name - rospy.init_node(self.name) + rclpy.init() + self.node = rclpy.create_node(self.name) self.sim_mode = None self.conversation_interface = None self.kb_places = None @@ -105,113 +88,86 @@ def __init__(self,da_state_name, da_name): if USE_SMACH_INRTOSPECTION_SERVER: self.sis = smach_ros.IntrospectionServer(unicode(str("/"+self.name+"smach_view_server")), self, unicode(self.name)) self.sis.start() - # places_xml_filename = rospy.get_param('/kb_places_xml') - # self.sim_mode = str(rospy.get_param('/sim_mode')) - # assert self.sim_mode in ['sim', 'gazebo', 'real'] - - # print 'Reading KB for places from file "' + places_xml_filename + '"' - # self.kb_places = kb_p.PlacesXmlParser(places_xml_filename).getKB() - - - # 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'), - # ]) - - - # self.my_fsm = MainState(self.conversation_interface) self.my_fsm = MainState() + def shutdownRequest(self): global USE_SMACH_INRTOSPECTION_SERVER - print ("my-tasker ----------------------- shutdown") - # self.conversation_interface.stop() + print("my-tasker ----------------------- shutdown") - if USE_SMACH_INRTOSPECTION_SERVER: - self.sis.stop() - self.sis.clear() - self.request_preempt() - - def cleanup_tf(self): - rospy.loginfo('{}: Executing state: {}'.format(rospy.get_name(), self.__class__.__name__)) - print 'Cleanup.execute' # self.conversation_interface.stop() - return 'ok' - - def get_suspension_tf(self,susp_data): - print "My TASKER -- get_suspension_tf" - print "My TASKER" - print "My TASKER" - rospy.loginfo('{}: Executing state: {}'.format(rospy.get_name(), self.__class__.__name__)) - print 'GetSuspend.execute' - print "susp data: ", susp_data.getData() - print "susp data[0]: ", susp_data.getData()[0] - data = susp_data.getData() - fsm_executable = None - for idx in range(2, len(data), 2): - # print data[idx] - if data[idx] == 'executable': - fsm_executable = data[idx+1] - elif data[idx] == 'rosrun': - ros_pkg = data[idx+1] - ros_exec = data[idx+2] - fsm_executable = "rosrun "+ros_pkg+" "+ros_exec - if fsm_executable == None: - print "harmoniser did not specified executable for suspension behaviour, terminating task" - fsm_executable = "terminate task" - # - # overrite ExeSusp state given by THA - # - # fsm_executable = 'rosrun rcprg_smach suspend_gh' - - return fsm_executable - - def exe_suspension_tf(self,fsm_es_in): - print "My TASKER -- exe_suspension_tf" - print "My TASKER" - print "My TASKER" - rospy.loginfo('{}: Executing state: {}'.format(rospy.get_name(), self.__class__.__name__)) - print 'ExecSuspension.execute' - #srv.shutdown() - print fsm_es_in - if fsm_es_in == "terminate task": - pass - #return 'shutdown' - else: - p = subprocess.Popen(fsm_es_in , shell=True, stdout=subprocess.PIPE, stderr=subprocess.PIPE) - # and you can block util the cmd execute finish - p.wait() - return "FINISHED" - def wait_tf(self): - # print "My TASKER -- wait_tf" - # print "My TASKER" - # print "My TASKER" - pass - def update_task_tf(self): - global USE_SMACH_INRTOSPECTION_SERVER - print "My TASKER -- update_task_tf" - print "My TASKER" - print "My TASKER" if USE_SMACH_INRTOSPECTION_SERVER: self.sis.stop() self.sis.clear() + self.request_preempt() - # imp.reload(rcprg_smach.bring_goods) - print "SWAPPING" - # self.swap_state('ExecFSM', MainState(self.conversation_interface)) - self.swap_state('ExecFSM', MainState()) - - if USE_SMACH_INRTOSPECTION_SERVER: - self.sis.start() - # self.my_fsm.set_initial_state(['MoveToKitchen']) + def cleanup_tf(self): + rclpy.logging.get_logger(self.__class__.__name__).info(f'Executing state: {self.__class__.__name__}') + print('Cleanup.execute') + # self.conversation_interface.stop() + return 'ok' + +def get_suspension_tf(self, susp_data): + print("My TASKER -- get_suspension_tf") + print("My TASKER") + print("My TASKER") + rclpy.logging.get_logger(self.__class__.__name__).info(f'Executing state: {self.__class__.__name__}') + print('GetSuspend.execute') + print(f"susp data: {susp_data.getData()}") + print(f"susp data[0]: {susp_data.getData()[0]}") + data = susp_data.getData() + fsm_executable = None + for idx in range(2, len(data), 2): + if data[idx] == 'executable': + fsm_executable = data[idx+1] + elif data[idx] == 'rosrun': + ros_pkg = data[idx+1] + ros_exec = data[idx+2] + fsm_executable = f"rosrun {ros_pkg} {ros_exec}" + if fsm_executable is None: + print("harmoniser did not specify an executable for suspension behaviour, terminating task") + fsm_executable = "terminate task" + + return fsm_executable + +def exe_suspension_tf(self, fsm_es_in): + print("My TASKER -- exe_suspension_tf") + print("My TASKER") + print("My TASKER") + rclpy.logging.get_logger(self.__class__.__name__).info(f'Executing state: {self.__class__.__name__}') + print('ExecSuspension.execute') + print(fsm_es_in) + if fsm_es_in == "terminate task": pass - def initialise_tf(self): - print "My TASKER -- initialise" - # self.conversation_interface.start() + # return 'shutdown' + else: + p = subprocess.Popen(fsm_es_in, shell=True, stdout=subprocess.PIPE, stderr=subprocess.PIPE) + p.wait() + return "FINISHED" + +def wait_tf(self): + pass + +def update_task_tf(self): + global USE_SMACH_INRTOSPECTION_SERVER + print("My TASKER -- update_task_tf") + print("My TASKER") + print("My TASKER") + + if USE_SMACH_INRTOSPECTION_SERVER: + self.sis.stop() + self.sis.clear() + + print("SWAPPING") + self.swap_state('ExecFSM', MainState()) + + if USE_SMACH_INRTOSPECTION_SERVER: + self.sis.start() + pass + +def initialise_tf(self): + print("My TASKER -- initialise") + # self.conversation_interface.start() def main(): da_name = None diff --git a/nodes/move_to.py b/nodes/move_to.py index 7d85ae9..a630628 100755 --- a/nodes/move_to.py +++ b/nodes/move_to.py @@ -1,4 +1,4 @@ -#!/usr/bin/env python +#!/usr/bin/env python3 # encoding: utf8 import sys diff --git a/nodes/move_to_tasker.py b/nodes/move_to_tasker.py index fe0e241..ca36983 100755 --- a/nodes/move_to_tasker.py +++ b/nodes/move_to_tasker.py @@ -1,4 +1,4 @@ -#!/usr/bin/env python +#!/usr/bin/env python3 # encoding: utf8 import sys diff --git a/nodes/stop.py b/nodes/stop.py index 13f4cb3..b42771f 100755 --- a/nodes/stop.py +++ b/nodes/stop.py @@ -1,4 +1,4 @@ -#!/usr/bin/env python +#!/usr/bin/env python3 # encoding: utf8 import sys diff --git a/nodes/task_harmonizer.py b/nodes/task_harmonizer.py index 9035a27..b09ad7c 100644 --- a/nodes/task_harmonizer.py +++ b/nodes/task_harmonizer.py @@ -1,4 +1,4 @@ -#!/usr/bin/env python3 +#!/usr/bin/env python33 # encoding: utf8 import rospkg diff --git a/nodes/test_bring_goods.py b/nodes/test_bring_goods.py index e5bfea3..03e9742 100755 --- a/nodes/test_bring_goods.py +++ b/nodes/test_bring_goods.py @@ -1,4 +1,4 @@ -#!/usr/bin/env python +#!/usr/bin/env python3 # encoding: utf8 import sys diff --git a/nodes/test_move_to.py b/nodes/test_move_to.py index 34723a5..588e0c7 100755 --- a/nodes/test_move_to.py +++ b/nodes/test_move_to.py @@ -1,4 +1,4 @@ -#!/usr/bin/env python +#!/usr/bin/env python3 # encoding: utf8 import sys diff --git a/nodes/test_wander.py b/nodes/test_wander.py index b1d13a8..a83bfaf 100755 --- a/nodes/test_wander.py +++ b/nodes/test_wander.py @@ -1,4 +1,4 @@ -#!/usr/bin/env python +#!/usr/bin/env python3 # encoding: utf8 import sys diff --git a/nodes/wander.py b/nodes/wander.py index 8f89b1f..28b9c11 100755 --- a/nodes/wander.py +++ b/nodes/wander.py @@ -1,4 +1,4 @@ -#!/usr/bin/env python +#!/usr/bin/env python3 # encoding: utf8 import sys diff --git a/setup.py b/setup.py index c67786a..88caae5 100644 --- a/setup.py +++ b/setup.py @@ -1,4 +1,4 @@ -#!/usr/bin/env python +#!/usr/bin/env python3 from distutils.core import setup from catkin_pkg.python_setup import generate_distutils_setup diff --git a/src/rcprg_smach/bring_goods_tasker.py b/src/rcprg_smach/bring_goods_tasker.py index 9682c70..18ec849 100644 --- a/src/rcprg_smach/bring_goods_tasker.py +++ b/src/rcprg_smach/bring_goods_tasker.py @@ -1,24 +1,25 @@ -#!/usr/bin/env python +#!/usr/bin/env python3 # encoding: utf8 import math import random -import rospy +import rclpy +from rclpy.node import Node import smach import smach_ros -import dynamic_reconfigure.client +import dynamic_reconfigure.client # Note: dynamic_reconfigure will need an ROS 2 equivalent or alternative import actionlib -from move_base_msgs.msg import * +from move_base_msgs.msg import * # You might need to find ROS 2 equivalents for these from actionlib_msgs.msg import GoalStatus -from tf.transformations import quaternion_from_euler +from tf.transformations import quaternion_from_euler # Note: tf2_ros is the package in ROS 2, you might need adjustments here from geometry_msgs.msg import Pose import navigation from TaskER.TaskER import TaskER from rcprg_smach import smach_rcprg -from pl_nouns.dictionary_client import DisctionaryServiceClient +from pl_nouns.dictionary_client import DisctionaryServiceClient # Make sure there's a ROS 2 version of this import task_manager @@ -37,18 +38,16 @@ def makePose(x, y, theta): class SayAskForGoods(TaskER.BlockingState): def __init__(self, sim_mode, conversation_interface): - TaskER.BlockingState.__init__(self, input_keys=['goods_name'], output_keys=['q_load_answer_id'], outcomes=['ok', 'preemption', 'error', 'shutdown','timeout','turn_around']) self.conversation_interface = conversation_interface - self.description = u'Proszę o podanie rzeczy' def transition_function(self, userdata): - rospy.loginfo('{}: Executing state: {}'.format(rospy.get_name(), self.__class__.__name__)) + self.get_logger().info('{}: Executing state: {}'.format(self.get_name(), self.__class__.__name__)) # Using ROS 2 logging - assert isinstance(userdata.goods_name, unicode) + assert isinstance(userdata.goods_name, str) # Note: Changed unicode to str for Python 3 goods_name = userdata.goods_name @@ -107,65 +106,44 @@ def transition_function(self, userdata): raise Exception('Unreachable code') -class SayTakeGoods(TaskER.BlockingState): - def __init__(self, sim_mode, conversation_interface): - TaskER.BlockingState.__init__(self, input_keys=['goods_name', 'q_load_answer_id'], output_keys=[], - outcomes=['ok', 'preemption', 'error', 'shutdown','timeout','turn_around']) - - self.conversation_interface = conversation_interface +class SayTakeGoods: + def __init__(self, conversation_interface): + self.conversation_interface = conversation_interface self.description = u'Proszę o odebranie rzeczy' def transition_function(self, userdata): - rospy.loginfo('{}: Executing state: {}'.format(rospy.get_name(), self.__class__.__name__)) + print(f'Executing state: {self.__class__.__name__}') - assert isinstance(userdata.goods_name, unicode) + if not isinstance(userdata['goods_name'], str): + raise ValueError("Goods name should be a string.") - goods_name = userdata.goods_name + goods_name = userdata['goods_name'] - #self.conversation_interface.addSpeakSentence( u'Odbierz {"' + goods_name + u'", biernik} i potwierdź' ) - self.conversation_interface.speakNowBlocking( u'niekorzystne warunki pogodowe odbierz {"' + goods_name + u'", biernik} i potwierdź' ) + self.conversation_interface.speakNowBlocking(u'niekorzystne warunki pogodowe odbierz {"' + goods_name + u'", biernik} i potwierdź') self.conversation_interface.addExpected('ack') self.conversation_interface.addExpected('ack_i_took') self.conversation_interface.addExpected('turn_around') - answer_id = self.conversation_interface.setAutomaticAnswer( 'q_current_task', u'niekorzystne warunki pogodowe czekam na odebranie {"' + goods_name + u'", dopelniacz}' ) + answer_id = self.conversation_interface.setAutomaticAnswer('q_current_task', u'niekorzystne warunki pogodowe czekam na odebranie {"' + goods_name + u'", dopelniacz}') - start_time = rospy.Time.now() + start_time = time.time() while True: - end_time = rospy.Time.now() - loop_time = end_time - start_time - loop_time_s = loop_time.secs - - if self.__shutdown__: - return 'shutdown' + loop_time_s = time.time() - start_time - if loop_time_s > ACK_WAIT_MAX_TIME_S: + if loop_time_s > 30: # Assuming a 30-second timeout as in the previous conversion. self.conversation_interface.removeExpected('ack') self.conversation_interface.removeExpected('ack_i_took') self.conversation_interface.removeExpected('turn_around') self.conversation_interface.removeAutomaticAnswer(answer_id) - # Do not remove q_load_answer, because we want to enter this state again return 'timeout' - if self.preempt_requested(): - self.conversation_interface.removeExpected('ack') - self.conversation_interface.removeExpected('ack_i_took') - self.conversation_interface.removeExpected('turn_around') - self.conversation_interface.removeAutomaticAnswer(answer_id) - if not userdata.q_load_answer_id is None: - self.conversation_interface.removeAutomaticAnswer(userdata.q_load_answer_id) - self.service_preempt() - return 'preemption' - if self.conversation_interface.consumeExpected('ack') or\ self.conversation_interface.consumeExpected('ack_i_took'): self.conversation_interface.removeExpected('ack') self.conversation_interface.removeExpected('ack_i_took') self.conversation_interface.removeExpected('turn_around') self.conversation_interface.removeAutomaticAnswer(answer_id) - if not userdata.q_load_answer_id is None: - self.conversation_interface.removeAutomaticAnswer(userdata.q_load_answer_id) return 'ok' if self.conversation_interface.consumeExpected('turn_around'): self.conversation_interface.removeExpected('ack') @@ -173,29 +151,21 @@ def transition_function(self, userdata): self.conversation_interface.removeExpected('turn_around') self.conversation_interface.removeAutomaticAnswer(answer_id) return 'turn_around' - rospy.sleep(0.1) + time.sleep(0.1) raise Exception('Unreachable code') -class SayIFinished(TaskER.BlockingState): - def __init__(self, sim_mode, conversation_interface): - TaskER.BlockingState.__init__(self, input_keys=[], output_keys=[], - outcomes=['ok', 'preemption', 'error', 'shutdown']) +class SayIFinished: + def __init__(self, conversation_interface): self.conversation_interface = conversation_interface - self.description = u'Mówię, że zakończyłem' def transition_function(self, userdata): - rospy.loginfo('{}: Executing state: {}'.format(rospy.get_name(), self.__class__.__name__)) - #self.conversation_interface.addSpeakSentence( u'Zakończyłem zadanie' ) - self.conversation_interface.speakNowBlocking( u'niekorzystne warunki pogodowe zakończyłem zadanie' ) - - if self.__shutdown__: - return 'shutdown' + print(f'Executing state: {self.__class__.__name__}') + self.conversation_interface.speakNowBlocking(u'niekorzystne warunki pogodowe zakończyłem zadanie') return 'ok' - class BringGoods(smach_rcprg.StateMachine): def __init__(self, sim_mode, conversation_interface, kb_places): diff --git a/src/rcprg_smach/bring_jar.py b/src/rcprg_smach/bring_jar.py index 55874c2..aadd688 100644 --- a/src/rcprg_smach/bring_jar.py +++ b/src/rcprg_smach/bring_jar.py @@ -1,17 +1,19 @@ -#!/usr/bin/env python2 -# -*- coding: utf-8 -*- +#!/usr/bin/env python32 +# -*- coding: utf-8 -*- -import rospy +import rclpy +from threading import Thread +from rclpy.node import Node import math import PyKDL -from threading import Thread from moveit_msgs.msg import AttachedCollisionObject, CollisionObject from shape_msgs.msg import SolidPrimitive -import tf_conversions.posemath as pm -from rcprg_ros_utils import exitError, MarkerPublisher from visualization_msgs.msg import Marker from geometry_msgs.msg import Vector3 +from tf2_ros import TransformBroadcaster, TransformListener +import tf2_pykdl as tf2_pykdl +from rcprg_ros_utils import exitError, MarkerPublisher from TaskER.TaskER import TaskER from rcprg_smach import smach_rcprg @@ -23,20 +25,21 @@ class MarkerPublisherThread: def threaded_function(self, obj): pub = MarkerPublisher("attached_objects") while not self.stop_thread: - pub.publishSinglePointMarker(PyKDL.Vector(), 1, r=1, g=0, b=0, a=1, namespace='default', frame_id=obj.link_name, m_type=Marker.CYLINDER, scale=Vector3(0.02, 0.02, 1.0), T=pm.fromMsg(obj.object.primitive_poses[0])) + pub.publishSinglePointMarker(PyKDL.Vector(), 1, r=1, g=0, b=0, a=1, namespace='default', frame_id=obj.link_name, + m_type=Marker.CYLINDER, scale=Vector3(0.02, 0.02, 1.0), T=tf2_pykdl.fromMsg(obj.object.primitive_poses[0])) try: - rospy.sleep(0.1) + rclpy.sleep(0.1) except: break try: pub.eraseMarkers(0, 10, namespace='default') - rospy.sleep(0.5) + rclpy.sleep(0.5) except: pass def __init__(self, obj): - self.thread = Thread(target = self.threaded_function, args = (obj, )) + self.thread = Thread(target=self.threaded_function, args=(obj,)) def start(self): self.stop_thread = False @@ -46,30 +49,36 @@ def stop(self): self.stop_thread = True self.thread.join() + class SetBaseDestination(TaskER.BlockingState): def __init__(self, sim_mode, conversation_interface): - TaskER.BlockingState.__init__(self, input_keys=['dest_name'], output_keys=['goal_pose'], - outcomes=['ok', 'preemption', 'error', 'shutdown']) + super().__init__(input_keys=['dest_name'], output_keys=[ + 'goal_pose'], outcomes=['ok', 'preemption', 'error', 'shutdown']) self.conversation_interface = conversation_interface self.description = u'Znajduje cel ruchu' + def transition_function(self, userdata): - rospy.loginfo('{}: Executing state: {}'.format(rospy.get_name(), self.__class__.__name__)) - #self.conversation_interface.addSpeakSentence( u'Zakończyłem zadanie' ) - self.conversation_interface.speakNowBlocking( u'niekorzystne warunki pogodowe Ustalam gdzie jest miejsce przedmiotu' ) + # Replace 'node_name' with appropriate name + node = rclpy.create_node('node_name') + node.get_logger().info(f'Executing state: {self.__class__.__name__}') + self.conversation_interface.speakNowBlocking( + u'niekorzystne warunki pogodowe Ustalam gdzie jest miejsce przedmiotu') if isinstance(userdata.dest_name, str): - dest_name = userdata.dest_name.decode('utf-8') - dest_name = userdata.dest_name.encode('utf-8').decode('utf-8') - userdata.goal_pose = navigation.PoseDescription({'place_name':unicode(dest_name)}) + dest_name = userdata.dest_name + dest_name = userdata.dest_name + userdata.goal_pose = navigation.PoseDescription( + {'place_name': dest_name}) if self.__shutdown__: return 'shutdown' return 'ok' + class BringJar(smach_rcprg.StateMachine): def __init__(self, sim_mode, conversation_interface, kb_places, pulled_out_flag): - smach_rcprg.StateMachine.__init__(self, input_keys=['object_container','bring_destination','end_pose', 'susp_data'], output_keys=['susp_data'], - outcomes=['PREEMPTED', + smach_rcprg.StateMachine.__init__(self, input_keys=['object_container', 'bring_destination', 'end_pose', 'susp_data'], output_keys=['susp_data'], + outcomes=['PREEMPTED', 'FAILED', 'FINISHED', 'shutdown']) self.userdata.max_lin_vel = 0.2 @@ -86,7 +95,8 @@ def __init__(self, sim_mode, conversation_interface, kb_places, pulled_out_flag) object1.object.id = "object1" object1_prim = SolidPrimitive() object1_prim.type = SolidPrimitive.CYLINDER - object1_prim.dimensions=[None, None] # set initial size of the list to 2 + # set initial size of the list to 2 + object1_prim.dimensions = [None, None] object1_prim.dimensions[SolidPrimitive.CYLINDER_HEIGHT] = 0.25 object1_prim.dimensions[SolidPrimitive.CYLINDER_RADIUS] = 0.06 object1_pose = pm.toMsg(PyKDL.Frame(PyKDL.Rotation.RotY(math.pi/2))) @@ -109,36 +119,37 @@ def __init__(self, sim_mode, conversation_interface, kb_places, pulled_out_flag) 'right_HandFingerThreeKnuckleThreeLink'] marker_publisher = MarkerPublisherThread(object1) - - self.userdata.take_out_pose = navigation.PoseDescription({'place_name':unicode('take_out_pose')}) - self.userdata.help_to_open = navigation.PoseDescription({'place_name':unicode('help_to_open')}) + self.userdata.take_out_pose = navigation.PoseDescription( + {'place_name': unicode('take_out_pose')}) + self.userdata.help_to_open = navigation.PoseDescription( + {'place_name': unicode('help_to_open')}) with self: smach_rcprg.StateMachine.add('SetContainerDestination', SetBaseDestination(sim_mode, conversation_interface), - transitions={'ok':'SetNavParams', 'preemption':'PREEMPTED', 'error': 'FAILED', - 'shutdown':'shutdown'}, - remapping={'dest_name':'object_container'}) + transitions={'ok': 'SetNavParams', 'preemption': 'PREEMPTED', 'error': 'FAILED', + 'shutdown': 'shutdown'}, + remapping={'dest_name': 'object_container'}) smach_rcprg.StateMachine.add('SetNavParams', navigation.SetNavParams(sim_mode), - transitions={'ok':'PrepareToMoveBase', 'preemption':'PREEMPTED', 'error': 'FAILED', - 'shutdown':'shutdown'}, - remapping={'max_lin_vel_in':'max_lin_vel', 'max_lin_accel_in':'max_lin_accel'}) + transitions={'ok': 'PrepareToMoveBase', 'preemption': 'PREEMPTED', 'error': 'FAILED', + 'shutdown': 'shutdown'}, + remapping={'max_lin_vel_in': 'max_lin_vel', 'max_lin_accel_in': 'max_lin_accel'}) smach_rcprg.StateMachine.add('PrepareToMoveBase', manipulation.PrepareToMoveBase(sim_mode, conversation_interface, worker), - transitions={'ok':'HideHands', 'preemption':'PREEMPTED', 'error': 'FAILED', - 'shutdown':'shutdown'}, - remapping={}) + transitions={'ok': 'HideHands', 'preemption': 'PREEMPTED', 'error': 'FAILED', + 'shutdown': 'shutdown'}, + remapping={}) smach_rcprg.StateMachine.add('HideHands', manipulation.HideHands(sim_mode, conversation_interface, worker), - transitions={'ok':'MoveToCabinet', 'preemption':'PREEMPTED', 'error': 'FAILED', - 'shutdown':'shutdown'}, - remapping={}) + transitions={'ok': 'MoveToCabinet', 'preemption': 'PREEMPTED', 'error': 'FAILED', + 'shutdown': 'shutdown'}, + remapping={}) smach_rcprg.StateMachine.add('MoveToCabinet', navigation.MoveToComplex(sim_mode, conversation_interface, kb_places), - transitions={'FINISHED':'PrepareTakeOutObjectLeft', 'PREEMPTED':'PREEMPTED', 'FAILED': 'FAILED', - 'shutdown':'shutdown'}, - remapping={'goal':'goal_pose', 'susp_data':'susp_data'}) + transitions={'FINISHED': 'PrepareTakeOutObjectLeft', 'PREEMPTED': 'PREEMPTED', 'FAILED': 'FAILED', + 'shutdown': 'shutdown'}, + remapping={'goal': 'goal_pose', 'susp_data': 'susp_data'}) # smach_rcprg.StateMachine.add('PrepareToGrip', manipulation.PrepareToGrip(sim_mode, conversation_interface, worker), # transitions={'ok':'OpenDoor', 'preemption':'PREEMPTED', 'error': 'FAILED', @@ -171,41 +182,41 @@ def __init__(self, sim_mode, conversation_interface, kb_places, pulled_out_flag) # remapping={}) smach_rcprg.StateMachine.add('PrepareTakeOutObjectLeft', manipulation.PrepareTakeOutObjectLeft(sim_mode, conversation_interface, worker, marker_publisher), - transitions={'ok':'TakeOutObjectLeft', 'preemption':'PREEMPTED', 'error': 'FAILED', - 'shutdown':'shutdown'}, - remapping={'susp_data':'susp_data'}) + transitions={'ok': 'TakeOutObjectLeft', 'preemption': 'PREEMPTED', 'error': 'FAILED', + 'shutdown': 'shutdown'}, + remapping={'susp_data': 'susp_data'}) smach_rcprg.StateMachine.add('TakeOutObjectLeft', manipulation.TakeOutObjectLeft(sim_mode, conversation_interface, worker, marker_publisher, pulled_out_flag), - transitions={'ok':'SetBringDestination', 'preemption':'PREEMPTED', 'error': 'FAILED', - 'shutdown':'shutdown'}, - remapping={'susp_data':'susp_data'}) + transitions={'ok': 'SetBringDestination', 'preemption': 'PREEMPTED', 'error': 'FAILED', + 'shutdown': 'shutdown'}, + remapping={'susp_data': 'susp_data'}) smach_rcprg.StateMachine.add('SetBringDestination', SetBaseDestination(sim_mode, conversation_interface), - transitions={'ok':'PrepareToMoveBase3', 'preemption':'PREEMPTED', 'error': 'FAILED', - 'shutdown':'shutdown'}, - remapping={'dest_name':'bring_destination', 'susp_data':'susp_data'}) + transitions={'ok': 'PrepareToMoveBase3', 'preemption': 'PREEMPTED', 'error': 'FAILED', + 'shutdown': 'shutdown'}, + remapping={'dest_name': 'bring_destination', 'susp_data': 'susp_data'}) smach_rcprg.StateMachine.add('PrepareToMoveBase3', manipulation.PrepareToMoveWithObject(sim_mode, conversation_interface, worker), - transitions={'ok':'MoveToBringDestination', 'preemption':'PREEMPTED', 'error': 'FAILED', - 'shutdown':'shutdown'}, - remapping={'susp_data':'susp_data'}) + transitions={'ok': 'MoveToBringDestination', 'preemption': 'PREEMPTED', 'error': 'FAILED', + 'shutdown': 'shutdown'}, + remapping={'susp_data': 'susp_data'}) smach_rcprg.StateMachine.add('MoveToBringDestination', navigation.MoveToComplex(sim_mode, conversation_interface, kb_places), - transitions={'FINISHED':'PutDownObjectLeft', 'PREEMPTED':'PREEMPTED', 'FAILED': 'FAILED', - 'shutdown':'shutdown'}, - remapping={'goal':'goal_pose', 'susp_data':'susp_data'}) + transitions={'FINISHED': 'PutDownObjectLeft', 'PREEMPTED': 'PREEMPTED', 'FAILED': 'FAILED', + 'shutdown': 'shutdown'}, + remapping={'goal': 'goal_pose', 'susp_data': 'susp_data'}) smach_rcprg.StateMachine.add('PutDownObjectLeft', manipulation.PutDownObjectLeft(sim_mode, conversation_interface, worker, marker_publisher), - transitions={'ok':'SetEndDestination', 'preemption':'PREEMPTED', 'error':'FAILED' - ,'shutdown':'shutdown', }, - remapping={}) + transitions={ + 'ok': 'SetEndDestination', 'preemption': 'PREEMPTED', 'error': 'FAILED', 'shutdown': 'shutdown', }, + remapping={}) smach_rcprg.StateMachine.add('SetEndDestination', SetBaseDestination(sim_mode, conversation_interface), - transitions={'ok':'MoveToEnd', 'preemption':'PREEMPTED', 'error': 'FAILED', - 'shutdown':'shutdown'}, - remapping={'dest_name':'end_pose'}) + transitions={'ok': 'MoveToEnd', 'preemption': 'PREEMPTED', 'error': 'FAILED', + 'shutdown': 'shutdown'}, + remapping={'dest_name': 'end_pose'}) smach_rcprg.StateMachine.add('MoveToEnd', navigation.MoveToComplex(sim_mode, conversation_interface, kb_places), - transitions={'FINISHED':'FINISHED', 'PREEMPTED':'PREEMPTED', 'FAILED': 'FAILED', - 'shutdown':'shutdown'}, - remapping={'goal':'goal_pose', 'susp_data':'susp_data'}) + transitions={'FINISHED': 'FINISHED', 'PREEMPTED': 'PREEMPTED', 'FAILED': 'FAILED', + 'shutdown': 'shutdown'}, + remapping={'goal': 'goal_pose', 'susp_data': 'susp_data'}) diff --git a/src/rcprg_smach/conversation.py b/src/rcprg_smach/conversation.py index 88505af..ad41ce3 100755 --- a/src/rcprg_smach/conversation.py +++ b/src/rcprg_smach/conversation.py @@ -1,44 +1,44 @@ -#!/usr/bin/env python +#!/usr/bin/env python3 # encoding: utf8 import threading import time -import rospy -import smach -import smach_ros -import std_msgs +import rclpy +from rclpy.action import ActionClient +from rclpy.node import Node +from std_msgs.msg import Bool import tiago_msgs.msg -import actionlib - -# -# New, high-level interface for conversations -# -class ConversationMachine: - # item_types is [(query_name, intent_name)] - def __init__(self, item_types,sim_mode): +class ConversationMachine(Node): + def __init__(self, item_types, sim_mode): + super().__init__('conversation_machine') self.__item_types__ = item_types self.__stop__ = False self.__intent_list__ = set() self.__intent_list_lock__ = threading.Lock() self.__sim_mode = sim_mode - print 'ConversationMachine.__init__: waiting for rico_says ActionServer...' + print('ConversationMachine.__init__: waiting for rico_says ActionServer...') if self.__sim_mode == 'real': - self.rico_says_client = actionlib.SimpleActionClient('rico_says', tiago_msgs.msg.SaySentenceAction) - self.rico_says_client.wait_for_server() - print 'ConversationMachine.__init__: connected to rico_says ActionServer' + self.rico_says_client = ActionClient( + self, tiago_msgs.msg.SaySentence, 'rico_says') + # In ROS2, you can use the service_is_ready() method to check if the server is available. + while not self.rico_says_client.wait_for_server(timeout_sec=5.0): + self.get_logger().info("Waiting for the 'rico_says' ActionServer...") + + print('ConversationMachine.__init__: connected to rico_says ActionServer') - self.sub = rospy.Subscriber("rico_filtered_cmd", tiago_msgs.msg.Command, self.__callbackRicoCmd__) - self.pub = rospy.Publisher('/activate_vad', std_msgs.msg.Bool, queue_size=10) + self.sub = self.create_subscription( + tiago_msgs.msg.Command, "rico_filtered_cmd", self.__callbackRicoCmd__, 10) + self.pub = self.create_publisher(Bool, '/activate_vad', 10) # Expected queries self.__expected_query_types__ = set() self.__expected_queries__ = {} - #self.__expected_autoremove_dict__ = {} + # self.__expected_autoremove_dict__ = {} # Automatic answers self.__automatic_answer_id__ = 0 @@ -51,7 +51,7 @@ def __callbackRicoCmd__(self, data): # Data is of type tiago_msgs.msg.Command, i.e. it encapsulates an intent assert isinstance(data, tiago_msgs.msg.Command) self.__intent_list_lock__.acquire() - self.__intent_list__.add( data ) + self.__intent_list__.add(data) self.__intent_list_lock__.release() def __getNameForIntent__(self, intent): @@ -62,36 +62,38 @@ def __getNameForIntent__(self, intent): return None def start(self): - self.__thread_hear__ = threading.Thread(target=self.__spin_hear__, args=(1,)) + self.__thread_hear__ = threading.Thread( + target=self.__spin_hear__, args=(1,)) self.__thread_hear__.start() - self.__thread_speak__ = threading.Thread(target=self.__spin_speak__, args=(1,)) + self.__thread_speak__ = threading.Thread( + target=self.__spin_speak__, args=(1,)) self.__thread_speak__.start() def stop(self): - print 'ConversationMachine stopping...' + print('ConversationMachine stopping...') self.__stop__ = True self.__thread_hear__.join() self.__thread_speak__.join() - print 'ConversationMachine stopping done.' + print('ConversationMachine stopping done.') def __spin_hear__(self, args): while not self.__stop__: self.__intent_list_lock__.acquire() for intent in self.__intent_list__: - query_type = self.__getNameForIntent__( intent ) + query_type = self.__getNameForIntent__(intent) # Manage expected queries if query_type in self.__expected_query_types__: self.__expected_queries__[query_type] = intent # Manage automatic answers - answer_id_list = self.getAutomaticAnswersIds( query_type ) + answer_id_list = self.getAutomaticAnswersIds(query_type) self.__pending_automatic_answers__ = self.__pending_automatic_answers__ + answer_id_list # Manage unexpected queries if not query_type in self.__expected_query_types__ and not bool(answer_id_list): # Add special answer for unexpected intent - self.__pending_automatic_answers__.append( -1 ) + self.__pending_automatic_answers__.append(-1) # Remove all intents self.__intent_list__ = set() @@ -107,7 +109,8 @@ def __spin_speak__(self, args): answer_id = self.__pending_automatic_answers__.pop(0) self.__current_automatic_answer__ = answer_id self.__intent_list_lock__.release() - self.speakNowBlocking( self.__getAutomaticAnswerText__(answer_id) ) + self.speakNowBlocking( + self.__getAutomaticAnswerText__(answer_id)) self.__intent_list_lock__.acquire() self.__current_automatic_answer__ = None self.__intent_list_lock__.release() @@ -116,10 +119,12 @@ def __spin_speak__(self, args): def setAutomaticAnswer(self, query_type, text): assert isinstance(text, unicode) - self.__automatic_answers_id_map__[self.__automatic_answer_id__] = (query_type, text) + self.__automatic_answers_id_map__[ + self.__automatic_answer_id__] = (query_type, text) if not query_type in self.__automatic_answers_name_map__: self.__automatic_answers_name_map__[query_type] = {} - self.__automatic_answers_name_map__[query_type][self.__automatic_answer_id__] = text + self.__automatic_answers_name_map__[ + query_type][self.__automatic_answer_id__] = text self.__automatic_answer_id__ = self.__automatic_answer_id__ + 1 return self.__automatic_answer_id__-1 @@ -163,28 +168,23 @@ def __getAutomaticAnswerText__(self, answer_id): # Speaks a sentence and waits until it is finished def speakNowBlocking(self, text): - print 'Rico says (blocking): "' + text + '"' - goal = tiago_msgs.msg.SaySentenceGoal() - goal.sentence = text + print('Rico says (blocking): "' + text + '"') + goal_msg = tiago_msgs.msg.SaySentence.Goal() + goal_msg.sentence = text if self.__sim_mode == 'real': - print 'sending conversation goal' - self.rico_says_client.send_goal(goal) - self.rico_says_client.wait_for_result() - print 'Rico says (blocking) finished' - - # Starts speaking a sentence, returns id for polling - #def speakNowBlocking(self, text): - # print 'Rico says (non-blocking): "' + sentence + '"' - # goal = tiago_msgs.msg.SaySentenceGoal() - # goal.sentence = sentence - # self.rico_says_client.send_goal(goal) - # self.rico_says_client.wait_for_result() + print('sending conversation goal') + future = self.rico_says_client.send_goal_async(goal_msg) + rclpy.spin_until_future_complete(self, future) + result = future.result() + + def startListening(self): + self.pub.publish(Bool()) def addExpected(self, query_type): self.__intent_list_lock__.acquire() assert not query_type in self.__expected_query_types__ - self.__expected_query_types__.add( query_type ) - #self.__expected_autoremove_dict__[query_type] = autoremove + self.__expected_query_types__.add(query_type) + # self.__expected_autoremove_dict__[query_type] = autoremove self.__intent_list_lock__.release() def removeExpected(self, query_type): @@ -193,21 +193,18 @@ def removeExpected(self, query_type): # Consume, if there is any left if query_type in self.__expected_queries__: del self.__expected_queries__[query_type] - self.__expected_query_types__.remove( query_type ) + self.__expected_query_types__.remove(query_type) self.__intent_list_lock__.release() - def consumeExpected(self, query_type):#, remove): + def consumeExpected(self, query_type): # , remove): self.__intent_list_lock__.acquire() if query_type in self.__expected_queries__: result = self.__expected_queries__[query_type] del self.__expected_queries__[query_type] - #if remove == True: + # if remove == True: # self.__expected_query_types__.remove(query_type) self.__intent_list_lock__.release() return result else: self.__intent_list_lock__.release() return None - - def startListening(self): - self.pub.publish(std_msgs.msg.Bool()) diff --git a/src/rcprg_smach/gh_csp.py b/src/rcprg_smach/gh_csp.py index e060f50..6b13271 100644 --- a/src/rcprg_smach/gh_csp.py +++ b/src/rcprg_smach/gh_csp.py @@ -1,4 +1,4 @@ -#!/usr/bin/env python +#!/usr/bin/env python3 # encoding: utf8 import math diff --git a/src/rcprg_smach/guide_human.py b/src/rcprg_smach/guide_human.py index aad0177..058faf6 100644 --- a/src/rcprg_smach/guide_human.py +++ b/src/rcprg_smach/guide_human.py @@ -1,4 +1,4 @@ -#!/usr/bin/env python +#!/usr/bin/env python3 # encoding: utf8 import math diff --git a/src/rcprg_smach/hazard_detector.py b/src/rcprg_smach/hazard_detector.py index 55fdbe8..ebaab28 100644 --- a/src/rcprg_smach/hazard_detector.py +++ b/src/rcprg_smach/hazard_detector.py @@ -1,4 +1,4 @@ -#!/usr/bin/env python +#!/usr/bin/env python3 # encoding: utf8 import rospy diff --git a/src/rcprg_smach/human_fell.py b/src/rcprg_smach/human_fell.py index 958e61b..8caafb7 100644 --- a/src/rcprg_smach/human_fell.py +++ b/src/rcprg_smach/human_fell.py @@ -1,13 +1,13 @@ -#!/usr/bin/env python +#!/usr/bin/env python3 # encoding: utf8 import math import random -import rospy +import rclpy import smach import smach_ros import dynamic_reconfigure.client -import actionlib +import rclpy_action from move_base_msgs.msg import * from actionlib_msgs.msg import GoalStatus @@ -24,6 +24,7 @@ ACK_WAIT_MAX_TIME_S = 30 + def makePose(x, y, theta): q = quaternion_from_euler(0, 0, theta) result = Pose() @@ -35,53 +36,58 @@ def makePose(x, y, theta): result.orientation.w = q[3] return result + class SetHumanAndDestination(TaskER.BlockingState): def __init__(self, sim_mode, conversation_interface): TaskER.BlockingState.__init__(self, input_keys=['human_name'], output_keys=['human_pose', 'dest_pose'], - outcomes=['ok', 'preemption', 'error', 'shutdown']) + outcomes=['ok', 'preemption', 'error', 'shutdown']) self.conversation_interface = conversation_interface self.description = u'Znajduję człowieka' def transition_function(self, userdata): - rospy.loginfo('{}: Executing state: {}'.format(rospy.get_name(), self.__class__.__name__)) - #self.conversation_interface.addSpeakSentence( u'Zakończyłem zadanie' ) - # self.conversation_interface.speakNowBlocking( u'niekorzystne warunki pogodowe Ustalam gdzie jest człowiek' ) + self.get_logger().info('{}: Executing state: {}'.format( + self.get_name(), self.__class__.__name__)) if isinstance(userdata.human_name, str): - human_name = userdata.human_name.decode('utf-8') - human_name = userdata.human_name.encode('utf-8').decode('utf-8') - userdata.human_pose = navigation.PoseDescription({'place_name':unicode(human_name)}) + human_name = userdata.human_name + else: + human_name = userdata.human_name + userdata.human_pose = navigation.PoseDescription( + {'place_name': human_name}) if self.__shutdown__: return 'shutdown' return 'ok' + class CheckHumanState(TaskER.BlockingState): def __init__(self, sim_mode, conversation_interface): TaskER.BlockingState.__init__(self, input_keys=['human_name'], output_keys=[], - outcomes=['ok', 'preemption', 'error', 'shutdown']) + outcomes=['ok', 'preemption', 'error', 'shutdown']) self.conversation_interface = conversation_interface self.description = u'Sprawdzam stan człowieka' def transition_function(self, userdata): - rospy.loginfo('{}: Executing state: {}'.format(rospy.get_name(), self.__class__.__name__)) - #self.conversation_interface.addSpeakSentence( u'Zakończyłem zadanie' ) + self.get_logger().info('{}: Executing state: {}'.format( + self.get_name(), self.__class__.__name__)) gender = "" if isinstance(userdata.human_name, str): - human_name = userdata.human_name.decode('utf-8') - human_name = userdata.human_name.encode('utf-8').decode('utf-8') + human_name = userdata.human_name + else: + human_name = userdata.human_name if human_name in ["John", "Peter"]: gender = "powinien Pan" else: gender = "powinna Pani" - - self.conversation_interface.speakNowBlocking( u'niekorzystne warunki pogodowe '+human_name+u', jak się czujesz?' ) - rospy.sleep(3) - self.conversation_interface.speakNowBlocking( u'niekorzystne warunki pogodowe Dziękuję za informację.' ) + self.conversation_interface.speakNowBlocking( + u'niekorzystne warunki pogodowe '+human_name+u', jak się czujesz?') + rclpy.sleep(3) + self.conversation_interface.speakNowBlocking( + u'niekorzystne warunki pogodowe Dziękuję za informację.') if self.__shutdown__: return 'shutdown' return 'ok' @@ -90,25 +96,25 @@ def transition_function(self, userdata): class SayIFinished(TaskER.BlockingState): def __init__(self, sim_mode, conversation_interface): TaskER.BlockingState.__init__(self, - outcomes=['ok', 'shutdown']) + outcomes=['ok', 'shutdown']) self.conversation_interface = conversation_interface self.description = u'Mówię, że zakończyłem' def transition_function(self, userdata): - rospy.loginfo('{}: Executing state: {}'.format(rospy.get_name(), self.__class__.__name__)) - #self.conversation_interface.addSpeakSentence( u'Zakończyłem zadanie' ) - # self.conversation_interface.speakNowBlocking( u'niekorzystne warunki pogodowe zakończyłem zadanie' ) + self.get_logger().info('{}: Executing state: {}'.format( + self.get_name(), self.__class__.__name__)) if self.__shutdown__: return 'shutdown' return 'ok' + class HumanFell(smach_rcprg.StateMachine): def __init__(self, sim_mode, conversation_interface, kb_places): - smach_rcprg.StateMachine.__init__(self, input_keys=['human_name','susp_data'], output_keys=['susp_data'], - outcomes=['PREEMPTED', + smach_rcprg.StateMachine.__init__(self, input_keys=['human_name', 'susp_data'], output_keys=['susp_data'], + outcomes=['PREEMPTED', 'FAILED', 'FINISHED', 'shutdown']) self.userdata.max_lin_vel = 0.2 @@ -122,24 +128,24 @@ def __init__(self, sim_mode, conversation_interface, kb_places): with self: smach_rcprg.StateMachine.add('SetHumanAndDestination', SetHumanAndDestination(sim_mode, conversation_interface), - transitions={'ok':'SetNavParams', 'preemption':'PREEMPTED', 'error': 'FAILED', - 'shutdown':'shutdown'}, - remapping={}) + transitions={'ok': 'SetNavParams', 'preemption': 'PREEMPTED', 'error': 'FAILED', + 'shutdown': 'shutdown'}, + remapping={}) smach_rcprg.StateMachine.add('SetNavParams', navigation.SetNavParams(sim_mode), - transitions={'ok':'MoveToHuman', 'preemption':'PREEMPTED', 'error': 'FAILED', - 'shutdown':'shutdown'}, - remapping={'max_lin_vel_in':'max_lin_vel', 'max_lin_accel_in':'max_lin_accel'}) + transitions={'ok': 'MoveToHuman', 'preemption': 'PREEMPTED', 'error': 'FAILED', + 'shutdown': 'shutdown'}, + remapping={'max_lin_vel_in': 'max_lin_vel', 'max_lin_accel_in': 'max_lin_accel'}) smach_rcprg.StateMachine.add('MoveToHuman', navigation.MoveToHumanComplex(sim_mode, conversation_interface, kb_places), - transitions={'FINISHED':'CheckHumanState', 'PREEMPTED':'PREEMPTED', 'FAILED': 'FAILED', - 'shutdown':'shutdown'}, - remapping={'goal':'human_pose', 'susp_data':'susp_data'}) + transitions={'FINISHED': 'CheckHumanState', 'PREEMPTED': 'PREEMPTED', 'FAILED': 'FAILED', + 'shutdown': 'shutdown'}, + remapping={'goal': 'human_pose', 'susp_data': 'susp_data'}) smach_rcprg.StateMachine.add('CheckHumanState', CheckHumanState(sim_mode, conversation_interface), - transitions={'ok':'SayIFinished', 'preemption':'PREEMPTED', 'error':'FAILED' - ,'shutdown':'shutdown', }, - remapping={'human_name':'human_name'}) + transitions={ + 'ok': 'SayIFinished', 'preemption': 'PREEMPTED', 'error': 'FAILED', 'shutdown': 'shutdown', }, + remapping={'human_name': 'human_name'}) smach_rcprg.StateMachine.add('SayIFinished', SayIFinished(sim_mode, conversation_interface), - transitions={'ok':'FINISHED', 'shutdown':'shutdown'}) + transitions={'ok': 'FINISHED', 'shutdown': 'shutdown'}) diff --git a/src/rcprg_smach/human_fell_approach.py b/src/rcprg_smach/human_fell_approach.py index f711239..7d9e316 100644 --- a/src/rcprg_smach/human_fell_approach.py +++ b/src/rcprg_smach/human_fell_approach.py @@ -1,4 +1,4 @@ -#!/usr/bin/env python +#!/usr/bin/env python3 # encoding: utf8 import math diff --git a/src/rcprg_smach/manipulation.py b/src/rcprg_smach/manipulation.py index ae8bc60..d4186a1 100644 --- a/src/rcprg_smach/manipulation.py +++ b/src/rcprg_smach/manipulation.py @@ -1,4 +1,4 @@ -#!/usr/bin/env python2 +#!/usr/bin/env python32 # -*- coding: utf-8 -*- import rospy diff --git a/src/rcprg_smach/navigation.py b/src/rcprg_smach/navigation.py index e544988..666170f 100644 --- a/src/rcprg_smach/navigation.py +++ b/src/rcprg_smach/navigation.py @@ -1,4 +1,4 @@ -#!/usr/bin/env python +#!/usr/bin/env python3 # encoding: utf8 import rospy @@ -1143,68 +1143,3 @@ def __init__(self, sim_mode, conversation_interface, kb_places): smach_rcprg.StateMachine.add('SayIdontKnow', SayIdontKnow(sim_mode, conversation_interface), transitions={'ok':'FAILED', 'shutdown':'shutdown'}, remapping={'move_goal':'move_goal','goal':'move_goal'}) - - -# def transition_function(self, userdata): -# if not 'place_name' in userdata.goal.parameters or userdata.goal.parameters['place_name'] is None: -# self.description = u'Gdzieś jadę' -# else: -# place_name = userdata.goal.parameters['place_name'] -# self.description = u'Jadę do {"' + place_name + u'", dopelniacz}' -# return super(MoveToComplex, self).transition_function(userdata) - -# class MoveToComplexTorsoMid(smach_rcprg.StateMachine): -# def __init__(self, sim_mode, conversation_interface, kb_places): -# smach_rcprg.StateMachine.__init__(self, outcomes=['FINISHED', 'PREEMPTED', 'FAILED', 'shutdown'], -# input_keys=['goal']) - -# self.userdata.default_height = 0.2 - -# self.description = u'Jadę do określonego miejsca' - -# with self: -# smach_rcprg.StateMachine.add('RememberCurrentPose', RememberCurrentPose(sim_mode), -# transitions={'ok':'UnderstandGoal', 'preemption':'PREEMPTED', 'error': 'FAILED', -# 'shutdown':'shutdown'}, -# remapping={'current_pose':'current_pose'}) - -# smach_rcprg.StateMachine.add('UnderstandGoal', UnderstandGoal(sim_mode, conversation_interface, kb_places), -# transitions={'ok':'SayImGoingTo', 'preemption':'PREEMPTED', 'error': 'SayIdontKnow', -# 'shutdown':'shutdown'}, -# remapping={'in_current_pose':'current_pose', 'goal_pose':'goal', 'move_goal':'move_goal'}) - -# smach_rcprg.StateMachine.add('SayImGoingTo', SayImGoingTo(sim_mode, conversation_interface), -# transitions={'ok':'SetHeightMid', 'preemption':'PREEMPTED', 'error': 'FAILED', -# 'shutdown':'shutdown'}, -# remapping={'move_goal':'move_goal'}) - -# smach_rcprg.StateMachine.add('SetHeightMid', SetHeight(sim_mode, conversation_interface), -# transitions={'ok':'MoveTo', 'preemption':'PREEMPTED', 'error': 'FAILED', -# 'shutdown':'shutdown'}, -# remapping={'torso_height':'default_height'}) - -# smach_rcprg.StateMachine.add('MoveTo', MoveTo(sim_mode, conversation_interface), -# transitions={'ok':'SayIArrivedTo', 'preemption':'PREEMPTED', 'error': 'FAILED', 'stall':'ClearCostMaps', -# 'shutdown':'shutdown'}, -# remapping={'move_goal':'move_goal', 'susp_data':'susp_data'}) - -# smach_rcprg.StateMachine.add('ClearCostMaps', ClearCostMaps(sim_mode), -# transitions={'ok':'MoveTo', 'preemption':'PREEMPTED', 'error': 'FAILED', -# 'shutdown':'shutdown'}) - -# smach_rcprg.StateMachine.add('SayIArrivedTo', SayIArrivedTo(sim_mode, conversation_interface), -# transitions={'ok':'FINISHED', 'preemption':'PREEMPTED', 'error': 'FAILED', -# 'shutdown':'shutdown'}, -# remapping={'move_goal':'move_goal'}) - -# smach_rcprg.StateMachine.add('SayIdontKnow', SayIdontKnow(sim_mode, conversation_interface), -# transitions={'ok':'FAILED', 'shutdown':'shutdown'}, -# remapping={'move_goal':'move_goal'}) - -# def transition_function(self, userdata): -# if not 'place_name' in userdata.goal.parameters or userdata.goal.parameters['place_name'] is None: -# self.description = u'Gdzieś jadę' -# else: -# place_name = userdata.goal.parameters['place_name'] -# self.description = u'Jadę do {"' + place_name + u'", dopelniacz}' -# return super(MoveToComplexTorsoMid, self).transition_function(userdata) diff --git a/src/rcprg_smach/ros_node_utils.py b/src/rcprg_smach/ros_node_utils.py index 41160ff..6fddfe1 100755 --- a/src/rcprg_smach/ros_node_utils.py +++ b/src/rcprg_smach/ros_node_utils.py @@ -1,4 +1,4 @@ -#!/usr/bin/env python +#!/usr/bin/env python3 # encoding: utf8 import rospkg diff --git a/src/rcprg_smach/smach_rcprg.py b/src/rcprg_smach/smach_rcprg.py index 2d176f8..159baf7 100644 --- a/src/rcprg_smach/smach_rcprg.py +++ b/src/rcprg_smach/smach_rcprg.py @@ -1,8 +1,8 @@ -#!/usr/bin/env python +#!/usr/bin/env python3 import smach -#!/usr/bin/env python +#!/usr/bin/env python3 import smach import rospy diff --git a/src/rcprg_smach/suspend_bj.py b/src/rcprg_smach/suspend_bj.py index d73cfe0..df9b9af 100644 --- a/src/rcprg_smach/suspend_bj.py +++ b/src/rcprg_smach/suspend_bj.py @@ -1,4 +1,4 @@ -#!/usr/bin/env python2 +#!/usr/bin/env python32 # -*- coding: utf-8 -*- import rospy diff --git a/src/rcprg_smach/suspend_gh.py b/src/rcprg_smach/suspend_gh.py index 556896c..68533d7 100644 --- a/src/rcprg_smach/suspend_gh.py +++ b/src/rcprg_smach/suspend_gh.py @@ -1,4 +1,4 @@ -#!/usr/bin/env python +#!/usr/bin/env python3 # encoding: utf8 import math diff --git a/src/rcprg_smach/task_manager.py b/src/rcprg_smach/task_manager.py index 7d32ddd..a850bc4 100755 --- a/src/rcprg_smach/task_manager.py +++ b/src/rcprg_smach/task_manager.py @@ -1,4 +1,4 @@ -#!/usr/bin/env python +#!/usr/bin/env python3 # encoding: utf8 # Copyright (c) 2019, Robot Control and Pattern Recognition Group, @@ -6,7 +6,7 @@ # Warsaw University of Technology # # All rights reserved. -# +# # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions are met: # * Redistributions of source code must retain the above copyright @@ -17,7 +17,7 @@ # * Neither the name of the Warsaw University of Technology nor the # names of its contributors may be used to endorse or promote products # derived from this software without specific prior written permission. -# +# # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND # ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED # WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE @@ -32,27 +32,28 @@ # Author: Dawid Seredynski # -import rospy +import rclpy +from rclpy.node import Node from tiago_msgs.msg import Command import pl_nouns.odmiana as ro + class PoseDescription: def __init__(self, parameters): self.parameters = parameters + class TaskMoveTo: def __init__(self, pose, place_name): parameters = {} - if not pose is None: + if pose is not None: parameters['pose'] = pose - if not place_name is None: + if place_name is not None: if isinstance(place_name, str): - parameters['place_name'] = place_name.decode('utf-8') - elif isinstance(place_name, unicode): parameters['place_name'] = place_name else: - raise Exception('Wrong type of "place_name": neither str nor unicode') + raise Exception('Wrong type of "place_name": not a str') self.__goal__ = PoseDescription(parameters) @@ -62,12 +63,13 @@ def getName(self): def getGoal(self): return self.__goal__ + class TaskBringGoods: def __init__(self, goods_name): if isinstance(goods_name, str): - self.__goal__ = goods_name.decode('utf-8') - elif isinstance(goods_name, unicode): self.__goal__ = goods_name + else: + raise Exception('Wrong type of "goods_name": not a str') def getName(self): return 'bring_goods' @@ -75,6 +77,7 @@ def getName(self): def getGoal(self): return self.__goal__ + class TaskUniversal: def __init__(self, task_name): self.__name__ = task_name @@ -85,17 +88,19 @@ def getName(self): def getGoal(self): return None -class TaskManager: + +class TaskManager(Node): def __init__(self, callback_list): - raise Exception('Do not use this class') + super().__init__('task_manager') self.__callback_list__ = callback_list self.o = ro.OdmianaRzeczownikow() - rospy.Subscriber("rico_cmd", Command, self.callback) + self.subscription = self.create_subscription( + Command, "rico_cmd", self.callback, 10) def przypadki(self, word): blocks = self.o.getBlocks(word) if len(blocks) == 0: - print u'Nie moge znaleźć nazwy miejsca w słowniku' + print('Nie moge znaleźć nazwy miejsca w słownik') word_m = word else: m_lp = self.o.getMianownikLp(blocks) @@ -124,71 +129,74 @@ def taskMoveTo(self, place_name): pl_name = place_name.strip().lower() if pl_name == '': - print u'Mam gdzieś iść, ale nie podano miejsca' + print('Mam gdzieś iść, ale nie podano miejsca') return False place_name_m, place_name_d, place_name_b = self.przypadki(pl_name) - print u'Poproszono mnie o przejście do ' + place_name_d.decode('utf-8') + u' (' + place_name_m.decode('utf-8') + u')' + print('Poproszono mnie o przejście do ' + place_name_d.decode('utf-8') + + ' (' + place_name_m.decode('utf-8') + ')') - task = TaskMoveTo( None, place_name_m ) + task = TaskMoveTo(None, place_name_m) return task def taskWander(self): - print u'Poproszono mnie, abym zaczął patrolowac' + print('Poproszono mnie, abym zaczął patrolowac') return TaskUniversal('wander') def taskBring(self, object_name): - print object_name + print(object_name) ob_name = object_name.strip().lower() ob_name_m, ob_name_d, ob_name_b = self.przypadki(ob_name) - print u'Poproszono mnie o przyniesienie ' + ob_name_d.decode('utf-8') + u' (' + ob_name_m.decode('utf-8') + u')' + print('Poproszono mnie o przyniesienie ' + + ob_name_d.decode('utf-8') + ' (' + ob_name_m.decode('utf-8') + ')') task = TaskBringGoods(object_name) return task def taskStop(self): - print u'Poproszono mnie o zatrzymanie się.' + print('Poproszono mnie o zatrzymanie się.') return TaskUniversal('stop') def questionLoad(self): - print u'Zapytano mnie: co wiozę?' + print('Zapytano mnie: co wiozę?') return TaskUniversal('q_load') def questionCurrentTask(self): - print u'Zapytano mnie: co robię?' + print('Zapytano mnie: co robię?') return TaskUniversal('q_current_task') def respAck(self): - print u'Usłyszałem ogólne potwierdzenie' + print('Usłyszałem ogólne potwierdzenie') return TaskUniversal('ack') def respAckIgave(self): - print u'Usłyszałem potwierdzenie podania' + print('Usłyszałem potwierdzenie podania') return TaskUniversal('ack_i_gave') def respAckItook(self): - print u'Usłyszałem potwierdzenie odebrania' + print('Usłyszałem potwierdzenie odebrania') return TaskUniversal('ack_i_took') def callback(self, data): param_dict = {} for param_name, param_value in zip(data.param_names, data.param_values): param_dict[param_name] = param_value - print 'param_name, param_value', param_name, param_value + print('param_name, param_value', param_name, param_value) task = None if data.intent_name == 'projects/incare-dialog-agent/agent/intents/176ab2ca-6250-4227-985b-cc82d5497d9f': task = self.taskBring(param_dict['przedmiot']) elif data.intent_name == 'projects/incare-dialog-agent/agent/intents/0165eceb-9621-4a7d-aecc-7a879951da18': - task = self.taskMoveTo( param_dict['miejsce'] ) + task = self.taskMoveTo(param_dict['miejsce']) elif data.intent_name == 'projects/incare-dialog-agent/agent/intents/7acd4325-4cdd-4e15-99be-ad545f4dddd5': task = self.taskStop() elif data.intent_name == 'projects/incare-dialog-agent/agent/intents/2f028022-05b6-467d-bcbe-e861ab449c17': - print u'Niezrozumiałe polecenie: "' + data.query_text.decode('utf-8') + '"' + print('Niezrozumiałe polecenie: "' + + data.query_text.decode('utf-8') + '"') elif data.intent_name == 'projects/incare-dialog-agent/agent/intents/d9e96166-030b-442f-a513-d3fa2e044030': task = self.taskWander() @@ -216,8 +224,9 @@ def callback(self, data): task = self.respAckItook() else: - raise Exception('Unknown intent: "' + data.intent_name + '", query_text: "' + data.query_text + '"') + raise Exception('Unknown intent: "' + data.intent_name + + '", query_text: "' + data.query_text + '"') if not task is None: for cb in self.__callback_list__: - cb( task ) + cb(task) diff --git a/src/rcprg_smach/update_pose.py b/src/rcprg_smach/update_pose.py index f67a2e0..5322904 100644 --- a/src/rcprg_smach/update_pose.py +++ b/src/rcprg_smach/update_pose.py @@ -1,14 +1,19 @@ -#!/usr/bin/env python +#!/usr/bin/env python3 # encoding: utf8 import math -import rospy +import rclpy +from rclpy.node import Node +from rclpy.qos import qos_profile_default +from rclpy.time import Time +from rclpy.duration import Duration -from move_base_msgs.msg import * -from tf.transformations import quaternion_from_euler, euler_from_quaternion from geometry_msgs.msg import Pose -import tf from visualization_msgs.msg import Marker +import tf2_ros +from tf2_ros import LookupException, ConnectivityException, ExtrapolationException +import tf2_geometry_msgs # Needed for transformations +from tf_transformations import quaternion_from_euler, euler_from_quaternion import task_manager @@ -90,22 +95,24 @@ def setMarker(action, publisher,marker_name, x,y,ox,oy,oz,ow): publisher.publish(marker) publisher.publish(name_marker) -class UpdatePose(): +class UpdatePose(Node): def __init__(self, sim_mode, kb_places, pose_id): + super().__init__('update_pose_node') self.sim_mode = sim_mode if sim_mode == 'gazebo': map_context = 'sim' else: map_context = sim_mode self.kb_places = kb_places - self.listener = tf.TransformListener() - self.marker_pub = rospy.Publisher(pose_id+"_markers", Marker, queue_size=10) + self.tf_buffer = tf2_ros.Buffer() + self.tf_listener = tf2_ros.TransformListener(self.tf_buffer, self) + self.marker_pub = self.create_publisher(Marker, pose_id + "_markers", qos_profile_default) self.pose_id = pose_id pl = self.kb_places.getPlaceByName(pose_id, map_context) pt_dest = pl.getPt() norm = pl.getN() - quat = quaternion_from_euler(0,0,math.atan2(norm[1], norm[0])) - setMarker('add',self.marker_pub, pose_id, pt_dest[0], pt_dest[1], quat[0],quat[1],quat[2], quat[3]) + quat = quaternion_from_euler(0, 0, math.atan2(norm[1], norm[0])) + self.set_marker('add', pose_id, pt_dest[0], pt_dest[1], quat[0], quat[1], quat[2], quat[3]) def update_pose(self, transform_name, face_place, is_human): @@ -120,11 +127,13 @@ def update_pose(self, transform_name, face_place, is_human): mc.updatePointPlace(self.pose_id, self.pose_id, [0, 0], [0, 0]) return else: - while not rospy.is_shutdown(): + while rclpy.ok(): try: - ros_time = rospy.Time() - self.listener.waitForTransform('/map', transform_name, ros_time, rospy.Duration(15.0)) - (trans,rot) = self.listener.lookupTransform('/map', transform_name, rospy.Time(0)) + ros_time = Time() + transform_available = self.tf_buffer.can_transform('map', transform_name, ros_time) + if not transform_available: + rclpy.spin_once(self, timeout_sec=15.0) + (trans,rot) = self.listener.lookupTransform('/map', transform_name, Time(0)) if len(trans) != 0: break except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException): @@ -134,7 +143,7 @@ def update_pose(self, transform_name, face_place, is_human): x = math.cos(theta) y = math.sin(theta) self.pose_id = unicode(self.pose_id) - print "UPDATING POSE: ", self.pose_id, "| data: ", trans + print ("UPDATING POSE: ", self.pose_id, "| data: ", trans) if mc.getPlaceById(self.pose_id) ==None: mc.addPointPlace(self.pose_id, self.pose_id, trans, [x, y], face_place, is_human) else: From c61948c1771ee8c6222565f606850f3b7e6f073c Mon Sep 17 00:00:00 2001 From: Adam Krawczyk Date: Sat, 4 Nov 2023 09:45:53 +0100 Subject: [PATCH 3/4] Nav2 poc Signed-off-by: Adam Krawczyk --- src/rcprg_smach/nav2_statuses.py | 113 +++ src/rcprg_smach/navigation2.py | 1291 +++++++++++++++++++++++++ src/rcprg_smach/navigation_helpers.py | 46 + 3 files changed, 1450 insertions(+) create mode 100644 src/rcprg_smach/nav2_statuses.py create mode 100644 src/rcprg_smach/navigation2.py create mode 100644 src/rcprg_smach/navigation_helpers.py diff --git a/src/rcprg_smach/nav2_statuses.py b/src/rcprg_smach/nav2_statuses.py new file mode 100644 index 0000000..2ba81d1 --- /dev/null +++ b/src/rcprg_smach/nav2_statuses.py @@ -0,0 +1,113 @@ +#!/usr/bin/env python3 +# encoding: utf8 + +import rclpy +from rclpy.action import ActionClient +from rclpy.node import Node +from nav2_msgs.action import NavigateToPose +from geometry_msgs.msg import PoseStamped +from action_msgs.msg import GoalStatus +from rclpy.clock import Clock + +class Nav2Client(Node): + + def __init__(self): + super().__init__('nav2_client') + self._action_client = ActionClient(self, NavigateToPose, 'navigate_to_pose') + self.current_goal_status = None # Variable to hold current goal status + + def send_goal(self, pose): + goal_msg = NavigateToPose.Goal() + goal_msg.pose = pose + + self._action_client.wait_for_server() + self._send_goal_future = self._action_client.send_goal_async(goal_msg, feedback_callback=self.feedback_callback) + self._send_goal_future.add_done_callback(self.goal_response_callback) + + def goal_response_callback(self, future): + goal_handle = future.result() + if not goal_handle.accepted: + self.get_logger().info('Goal rejected :(') + return + + self.get_logger().info('Goal accepted :)') + + self._get_result_future = goal_handle.get_result_async() + self._get_result_future.add_done_callback(self.get_result_callback) + + # Also, keep track of the goal handle to check the status later + self._goal_handle = goal_handle + + def get_result_callback(self, future): + status = future.result().status + if status == GoalStatus.STATUS_SUCCEEDED: + self.get_logger().info('Goal succeeded!') + elif status == GoalStatus.STATUS_CANCELED: + self.get_logger().info('Goal was canceled') + elif status == GoalStatus.STATUS_ABORTED: + self.get_logger().info('Goal was aborted') + else: + self.get_logger().info('Goal failed with status: {0}'.format(status)) + + # Update current goal status + self.current_goal_status = status + + def feedback_callback(self, feedback_msg): + # This callback is for getting feedback + feedback = feedback_msg.feedback + self.get_logger().info('Received feedback: {0}'.format(feedback)) + + def cancel_goal(self): + self._goal_handle.cancel_goal_async() + + def cancel_all_goals(self): + self._action_client.cancel_all_goals_async() + + + # Method to get the current status + def get_current_goal_status(self): + if self._goal_handle: + return self._goal_handle.get_status() + else: + return None + + def get_clock(self): + return Clock() + +def main(args=None): + rclpy.init(args=args) + nav2_client = Nav2Client() + + # Create a PoseStamped object with the goal pose + goal_pose = PoseStamped() + goal_pose.header.frame_id = "map" + goal_pose.pose.position.x = 1.0 + goal_pose.pose.position.y = 1.0 + goal_pose.pose.orientation.w = 1.0 + + nav2_client.send_goal(goal_pose) + + # Spin in a separate thread or perform work and query the status periodically + executor = rclpy.executors.MultiThreadedExecutor() + executor.add_node(nav2_client) + try: + executor_thread = threading.Thread(target=executor.spin) + executor_thread.start() + + # Query the current goal status at the desired rate + while rclpy.ok(): + rclpy.spin_once(nav2_client, timeout_sec=1) + current_status = nav2_client.get_current_goal_status() + if current_status is not None: + nav2_client.get_logger().info(f'Current goal status: {current_status}') + time.sleep(1) + + except KeyboardInterrupt: + pass + finally: + executor.shutdown() + nav2_client.destroy_node() + rclpy.shutdown() + +if __name__ == '__main__': + main() \ No newline at end of file diff --git a/src/rcprg_smach/navigation2.py b/src/rcprg_smach/navigation2.py new file mode 100644 index 0000000..d2d24fe --- /dev/null +++ b/src/rcprg_smach/navigation2.py @@ -0,0 +1,1291 @@ +#!/usr/bin/env python3 +# encoding: utf8 + +import smach +import smach_ros +import dynamic_reconfigure.client +import actionlib +from rclpy.action import ActionClient +import math +import threading +import copy +import yaml + +from navigation_helpers import * + +from move_base_msgs.msg import * +# from actionlib_msgs.msg import GoalStatus +import std_srvs.srv as std_srvs + +from tf.transformations import quaternion_from_euler, euler_from_quaternion + +from TaskER.TaskER import TaskER +from task_manager import PoseDescription +import smach_rcprg +from rcprg_smach.hazard_detector import HazardDetector +from tiago_smach import tiago_torso_controller +from pal_common_msgs.msg import DisableAction, DisableActionGoal, DisableGoal +from control_msgs.msg import PointHeadAction, PointHeadActionGoal, PointHeadGoal +from actionlib_msgs.msg import GoalID +from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint + +import rclpy +from rclpy.executors import SingleThreadedExecutor +from rclpy.node import Node +from geometry_msgs.msg import Pose, PoseWithCovarianceStamped +from nav2_msgs.srv import GetParameters +import time +from nav2_statuses import Nav2Client +from geometry_msgs.msg import PoseStamped +from action_msgs.msg import GoalStatus + +NAVIGATION_MAX_TIME_S = 100 + +class RememberCurrentPose(Node, TaskER.BlockingState): + def __init__(self): + super().__init__('remember_current_pose') + + # In ROS 2, parameters can be declared and used to set up configuration such as 'sim_mode' + self.declare_parameter('sim_mode', 'real') # Example default value 'real' + sim_mode = self.get_parameter('sim_mode').get_parameter_value().string_value + assert sim_mode in ['sim', 'gazebo', 'real'] + self.sim_mode = sim_mode + + # Set up a subscriber if running in 'gazebo' or 'real' mode + if self.sim_mode in ['gazebo', 'real']: + self.lock = Lock() + self.current_pose = None + self.subscription = self.create_subscription( + PoseWithCovarianceStamped, '/amcl_pose', self.pose_callback, 10) + self.subscription # Prevent unused variable warning + + def pose_callback(self, msg): + with self.lock: + self.current_pose = copy.deepcopy(msg.pose.pose) + + def execute(self): + self.get_logger().info('Executing state: RememberCurrentPose') + + if self.sim_mode == 'sim': + # Simulation mode logic would go here + pass + else: + for _ in range(10): + rclpy.spin_once(self, timeout_sec=0.1) + with self.lock: + if self.current_pose: + # Logic to handle the current pose would go here + # For example, store it, or pass it as output from a state machine + break + else: + # If the pose is not received, handle the error + self.get_logger().error('No pose received within the allotted time') + + def transition_function(self, userdata): + # self.get_logger().info(f'Executing state: {self.__class__.____name__}') + print("EXECUTING STATE: ", type(self).__name__) + + if self.sim_mode == 'sim': + # Replace 'makePose' with your actual logic to create a Pose object + pose = Pose() + userdata.current_pose = PoseDescription(pose=pose) + + if self.preempt_requested(): + self.service_preempt() + return 'preemption' + + if self.__shutdown__: + return 'shutdown' + + return 'ok' + else: + pose_valid = False + for i in range(10): + if self.preempt_requested(): + self.service_preempt() + return 'preemption' + + self.__lock__.acquire() + if self.current_pose is not None: + pose_valid = True + userdata.current_pose = PoseDescription( + pose=self.current_pose) + self.__lock__.release() + + if self.__shutdown__: + return 'shutdown' + if pose_valid: + return 'ok' + + # Use spin_once for ROS 2 + rclpy.spin_once(self, timeout_sec=0.1) + if self.__shutdown__: + return 'shutdown' + return 'error' + + +class UnderstandGoal(TaskER.BlockingState): + def __init__(self, sim_mode, conversation_interface, kb_places): + TaskER.BlockingState.__init__(self, tf_freq=10, input_keys=['in_current_pose', 'goal_pose'], output_keys=['move_goal'], + outcomes=['ok', 'preemption', 'error', 'shutdown']) + + assert sim_mode in ['sim', 'gazebo', 'real'] + self.sim_mode = sim_mode + self.kb_places = kb_places + + self.description = u'Próbuję zrozumieć zadany cel' + + def transition_function(self, userdata): + # self.get_logger().info('{}: Executing state: {}'.format(self.get_name(), type(self).__name__)) + print("EXECUTING STATE: ", type(self).__name__) + + # assert isinstance( userdata.goal_pose, PoseDescription ) + + print("GOAL_POSE: ", userdata.goal_pose) + if 'place_name' in userdata.goal_pose.parameters: + place_name = userdata.goal_pose.parameters['place_name'].lower() + pose_valid = False + place_name_valid = True + pose = None + print('place_name', place_name) + elif 'pose' in userdata.goal_pose.parameters: + pose = userdata.goal_pose.parameters['pose'] + pose_valid = True + place_name_valid = False + place_name = u'nieznane' + print('pose', pose) + else: + raise Exception('Parameters are missing') + # elif not userdata.nav_goal_pose is None: + # pose = userdata.nav_goal_pose.pose + # pose_valid = userdata.nav_goal_pose.pose_valid + # place_name_valid = userdata.nav_goal_pose.place_name_valid + # if place_name_valid: + # if isinstance(userdata.nav_goal_pose.place_name, str): + # place_name = userdata.nav_goal_pose.place_name.decode('utf-8') + # elif isinstance(userdata.nav_goal_pose.place_name, unicode): + # place_name = userdata.nav_goal_pose.place_name + # else: + # raise Exception('Unexpected type of place_name: "' + str(type(place_name)) + '"') + # else: + # place_name = u'nieznane' + + assert isinstance(place_name, unicode) + + if self.sim_mode == 'sim': + pose = makePose(0, 0, 0) + else: + if self.sim_mode == 'real': + mc_name = 'real' + elif self.sim_mode == 'gazebo': + mc_name = 'sim' + + if not pose_valid: + if not place_name_valid: + print('UnderstandGoal place_name is not valid') + return 'error' + + current_pose = userdata.in_current_pose + pt_start = ( + current_pose.parameters['pose'].position.x, current_pose.parameters['pose'].position.y) + print("pt_start: ", pt_start) + try: + pl = self.kb_places.getPlaceByName(place_name, mc_name) + except: + userdata.move_goal = PoseDescription( + {'pose': pose, 'place_name': place_name}) + print('UnderstandGoal place_name is not valid') + return 'error' + # NORM: [-0.9667981925794608, 0.25554110202683233] +# angle_dest: -2.88318530718 +# NORM: (-1.0, 0.0) +# angle_dest: 0.972464864357 +# pt_dest: (3.6, 2.0) + +# + if pl.getType() == 'point': + if pl.isDestinationHuman(): + human_pose = yaml.load( + rospy.get_param(place_name+'/pose')) + pt_dest = human_pose['x'], human_pose['y'] + norm = math.cos(human_pose['theta']), math.sin( + human_pose['theta']) + else: + pt_dest = pl.getPt() + norm = pl.getN() + print("NORM_0: ", norm[0]) + print("NORM_1: ", norm[1]) + if pl.isDestinationFace(): + print("HUMAN") + print("NORM: ", norm) + print("pt_dest: ", pt_dest) + angle_dest = -math.atan2(norm[1], -norm[0]) + print("angle_dest: ", angle_dest) + pt = pt_dest + pt_dest = (pt_dest[0]+norm[0], pt_dest[1]+norm[1]) + print("pt_dest: ", pt_dest) + else: + print("NO HUMAN") + print("NORM: ", norm) + angle_dest = math.atan2(norm[1], norm[0]) + print("angle_dest: ", angle_dest) + pt = pt_dest + pt_dest = (pt_dest[0], pt_dest[1]) + print("pt_dest: ", pt_dest) + print('UnderstandGoal place type: point') + print('pt: {}, pt_dest: {}, norm: {}, angle_dest: {}'.format( + pt, pt_dest, norm, angle_dest)) + elif pl.getType() == 'volumetric': + pt_dest = self.kb_places.getClosestPointOfPlace( + pt_start, pl.getId(), mc_name, dbg_output_path='/tmp/') + angle_dest = 0.0 + else: + raise Exception('Unknown place type: "' + + pl.getType() + '"') + pose = makePose(pt_dest[0], pt_dest[1], angle_dest) + else: + place_pos = (pose.position.x, pose.position.y) + result_place_id = self.kb_places.whatIsAt(place_pos, mc_name) + if result_place_id is None: + place_name = u'nieznane' + else: + pl = self.kb_places.getPlaceById(result_place_id, mc_name) + place_name = pl.getName() # returns unicode + assert isinstance(place_name, unicode) + + assert isinstance(place_name, unicode) + + result = PoseDescription({'pose': pose, 'place_name': place_name}) + + if self.preempt_requested(): + self.service_preempt() + return 'preemption' + + if self.__shutdown__: + return 'shutdown' + + userdata.move_goal = result + return 'ok' + + +class SetHeight(TaskER.BlockingState): + def __init__(self, sim_mode, conversation_interface): + TaskER.BlockingState.__init__(self, tf_freq=10, input_keys=['torso_height'], + outcomes=['ok', 'preemption', 'error', 'shutdown']) + + self.conversation_interface = conversation_interface + assert sim_mode in ['sim', 'gazebo', 'real'] + self.sim_mode = sim_mode + if self.sim_mode in ['gazebo', 'real']: + self.torso_controller = tiago_torso_controller.TiagoTorsoController() + + self.description = u'Zmieniam wysokość' + + def transition_function(self, userdata): + # self.get_logger().info(f'{self.get_name()}: Executing state: {type(self).__name__}') + print("EXECUTING STATE: ", type(self).__name__) + + if self.sim_mode == 'sim': + return 'ok' + + current_height = self.torso_controller.get_torso_height() + + if current_height is None: + return 'error' + + if abs(current_height - userdata.torso_height) > 0.05: + self.torso_controller.set_torso_height(userdata.torso_height) + for i in range(30): + if self.preempt_requested(): + self.service_preempt() + return 'preemption' + + time.sleep(0.1) + + if self.__shutdown__: + return 'shutdown' + return 'ok' + + +class SayImGoingTo(TaskER.BlockingState): + def __init__(self, sim_mode, conversation_interface): + TaskER.BlockingState.__init__(self, tf_freq=10, input_keys=['move_goal'], + outcomes=['ok', 'preemption', 'error', 'shutdown']) + + self.conversation_interface = conversation_interface + + self.description = u'Mówię dokąd jadę' + + def transition_function(self, userdata): + self.get_logger().info( + f'{self.get_name()}: Executing state: {type(self).__name__}') + + pose = userdata.move_goal.parameters['pose'] + place_name = userdata.move_goal.parameters['place_name'] + + assert isinstance(place_name, unicode) + # self.conversation_interface.addSpeakSentence( u'Jadę do {"' + place_name + u'", dopelniacz}' ) + self.conversation_interface.speakNowBlocking( + u'niekorzystne warunki pogodowe jadę do {"' + place_name + u'", dopelniacz}') + + if self.preempt_requested(): + # self.conversation_interface.removeExpected('q_current_task') + self.service_preempt() + return 'preemption' + + if self.__shutdown__: + return 'shutdown' + + return 'ok' + + +class SayIdontKnow(TaskER.BlockingState): + def __init__(self, sim_mode, conversation_interface): + TaskER.BlockingState.__init__(self, tf_freq=10, input_keys=['move_goal'], + outcomes=['ok', 'shutdown']) + + self.conversation_interface = conversation_interface + + self.description = u'Mówię, że nie wiem o co chodzi' + + def transition_function(self, userdata): + rospy.loginfo('{}: Executing state: {}'.format( + rospy.get_name(), self.__class__.__name__)) + + place_name = userdata.move_goal.parameters['place_name'] + assert isinstance(place_name, unicode) + # self.conversation_interface.addSpeakSentence( u'Nie wiem gdzie jest {"' + place_name + u'", mianownik}' ) + self.conversation_interface.speakNowBlocking( + u'niekorzystne warunki pogodowe nie wiem gdzie jest {"' + place_name + u'", mianownik}') + + if self.preempt_requested(): + # self.conversation_interface.removeExpected('q_current_task') + self.service_preempt() + return 'preemption' + + if self.__shutdown__: + return 'shutdown' + + return 'ok' + + +class SayIArrivedTo(TaskER.BlockingState): + def __init__(self, sim_mode, conversation_interface): + TaskER.BlockingState.__init__(self, tf_freq=10, input_keys=['move_goal'], + outcomes=['ok', 'preemption', 'error', 'shutdown']) + + self.conversation_interface = conversation_interface + + self.description = u'Mówię, że dojechałem do celu' + + def transition_function(self, userdata): + # rospy.loginfo('{}: Executing state: {}'.format(rospy.get_name(), self.__class__.__name__)) + print("EXECUTING STATE: ", type(self).__name__) + + pose = userdata.move_goal.parameters['pose'] + place_name = userdata.move_goal.parameters['place_name'] + assert isinstance(place_name, unicode) + # self.conversation_interface.addSpeakSentence( u'Dojechałem do {"' + place_name + u'", dopelniacz}' ) + self.conversation_interface.speakNowBlocking( + u'niekorzystne warunki pogodowe dojechałem do {"' + place_name + u'", dopelniacz}') + + if self.preempt_requested(): + self.service_preempt() + return 'preemption' + + if self.__shutdown__: + return 'shutdown' + # self.conversation_interface.addSpeakSentence( 'Dojechałem do pozycji ' + str(pose.position.x) + ', ' + str(pose.position.y) ) + return 'ok' + + +class SetNavParams(TaskER.BlockingState): + def __init__(self, sim_mode): + assert sim_mode in ['sim', 'gazebo', 'real'] + self.sim_mode = sim_mode + # if sim_mode in ['gazebo', 'real']: + # base_local_planner = rospy.get_param('/move_base/base_local_planner') + # self.local_planner_name = base_local_planner.split('/')[-1] + + # print ('SetNavParams: detected local planner: ' + self.local_planner_name) + + # self.dynparam_client = dynamic_reconfigure.client.Client('/move_base/' + self.local_planner_name) + + # # Ensure that we know what we are doing. + # # Check if the given parameters are present in the configuration. + # config = self.dynparam_client.get_configuration() + # if self.local_planner_name == 'PalLocalPlanner': + # assert 'max_vel_x' in config + # assert 'acc_lim_x' in config + # elif self.local_planner_name == 'EBandPlannerROS': + # assert 'max_vel_lin' in config + # assert 'max_acceleration' in config + # elif self.local_planner_name == 'TebLocalPlannerROS': + # assert 'max_vel_x' in config + # assert 'acc_lim_x' in config + # else: + # raise Exception('Local planner "' + self.local_planner_name + '" is not supported.') + + TaskER.BlockingState.__init__(self, tf_freq=10, input_keys=['max_lin_vel_in', 'max_lin_accel_in'], + outcomes=['ok', 'preemption', 'error', 'shutdown']) + + self.description = u'Zmieniam parametry ruchu' + + def transition_function(self, userdata): + # rospy.loginfo('{}: Executing state: {}'.format(rospy.get_name(), self.__class__.__name__)) + print("EXECUTING STATE: ", type(self).__name__) + + if self.sim_mode == 'sim': + time.sleep(1.0) + # else: + # max_lin_vel = userdata.max_lin_vel_in + # max_lin_accel = userdata.max_lin_accel_in + + # setting planner params so it the robot moves slowly + # params = userdata.set_nav_params_params + # Differrent planners have different parameters + # if self.local_planner_name == 'PalLocalPlanner': + # params = { + # 'max_vel_x': max_lin_vel, + # 'acc_lim_x': max_lin_accel + # } + # elif self.local_planner_name == 'EBandPlannerROS': + # params = { + # 'max_vel_lin': max_lin_vel, + # 'max_acceleration': max_lin_accel + # } + # elif self.local_planner_name == 'TebLocalPlannerROS': + # params = { + # 'max_vel_x': max_lin_vel, + # 'acc_lim_x': max_lin_accel + # } + # else: + # raise Exception('Local planner "' + self.local_planner_name + '" is not supported.') + + # config = self.dynparam_client.update_configuration(params) + + if self.preempt_requested(): + self.service_preempt() + return 'preemption' + + if self.__shutdown__: + return 'shutdown' + return 'ok' + + +class MoveToBlocking(TaskER.BlockingState): + def __init__(self, sim_mode, conversation_interface): + assert sim_mode in ['sim', 'gazebo', 'real'] + self.current_pose = Pose() + self.is_feedback_received = False + self.move_base_status = GoalStatus.PENDING + self.is_goal_achieved = False + self.sim_mode = sim_mode + self.conversation_interface = conversation_interface + self.nav2_client = Nav2Client() + + TaskER.BlockingState.__init__(self, + outcomes=['ok', 'preemption', + 'error', 'stall', 'shutdown'], + input_keys=['move_goal', 'susp_data']) + + self.description = u'Jadę' + self.suspendable_move_to = MoveTo( + self.sim_mode, self.conversation_interface) + + def transition_function(self, userdata): + return self.suspendable_move_to.transition_function(userdata) + + +class MoveTo(TaskER.SuspendableState): + def __init__(self, sim_mode, conversation_interface): + assert sim_mode in ['sim', 'gazebo', 'real'] + self.current_pose = Pose() + self.is_feedback_received = False + self.move_base_status = GoalStatus.PENDING + self.is_goal_achieved = False + self.sim_mode = sim_mode + self.conversation_interface = conversation_interface + self.nav2_client = Nav2Client() + self.clock = Nav2Client.get_clock() + + TaskER.SuspendableState.__init__(self, + outcomes=['ok', 'preemption', + 'error', 'stall', 'shutdown'], + input_keys=['move_goal', 'susp_data']) + + self.description = u'Jadę' + + def set_destination_pose(self, userdata): + pass + + def update_destination_pose(self, userdata): + return False + + def transition_function(self, userdata): + global HUMAN_POSE_UPDATE_IN_APPROACH + # rospy.loginfo('{}: Executing state: {}'.format(rospy.get_name(), self.__class__.__name__)) + print("EXECUTING STATE: ", type(self).__name__) + + place_name = userdata.move_goal.parameters['place_name'] + + assert isinstance(place_name, unicode) + answer_id = self.conversation_interface.setAutomaticAnswer( + 'q_current_task', u'niekorzystne warunki pogodowe jadę do {"' + place_name + u'", dopelniacz}') + self.set_destination_pose(userdata) + pose = userdata.move_goal.parameters['pose'] + place_name = userdata.move_goal.parameters['place_name'] + + if self.sim_mode == 'sim': + for i in range(50): + if self.is_suspension_flag() != None: + self.conversation_interface.removeAutomaticAnswer( + answer_id) + self.request_preempt() + return 'preemption' + + time.sleep(0.2) + self.conversation_interface.removeAutomaticAnswer(answer_id) + return 'ok' + else: + goal = PoseStamped() + + goal.pose = pose + goal.header.frame_id = 'map' + goal.header.stamp = self.clock.now().to_msg() + + # client = actionlib.SimpleActionClient('move_base', MoveBaseAction) + # client.wait_for_server() + + # # start moving + # client.send_goal(goal, self.move_base_done_cb, self.move_base_active_cb, self.move_base_feedback_cb) + Nav2Client.send_goal(goal) + + # action_feedback = GoActionFeedback() + # action_result = GoActionResult() + # action_result.result.is_goal_accomplished = False + # userdata.nav_result = action_result.result + + start_time = self.clock.now() + last_human_update = self.clock.now() + self.is_goal_achieved = False + while self.is_goal_achieved == False: + # action_feedback.feedback.current_pose = self.current_pose + + # userdata.nav_feedback = action_feedback.feedback + # userdata.nav_actual_pose = self.current_pose + + end_time = self.clock.now() + loop_time = end_time - start_time + loop_time_s = loop_time.nanoseconds * 1e-9 + + if self.__shutdown__: + Nav2Client.cancel_all_goals() + self.conversation_interface.removeAutomaticAnswer( + answer_id) + self.service_preempt() + return 'shutdown' + + if loop_time_s > NAVIGATION_MAX_TIME_S: + # break the loop, end with error state + self.conversation_interface.removeAutomaticAnswer( + answer_id) + rospy.logwarn( + 'State: Navigation took too much time, returning error') + Nav2Client.cancel_all_goals() + return 'stall' + + if self.update_destination_pose(userdata): + goal.pose = userdata.move_goal.parameters['pose'] + # client.send_goal(goal, self.move_base_done_cb, self.move_base_active_cb, self.move_base_feedback_cb) + Nav2Client.send_goal(goal) + # print "\n\n\n" + # print "======================================" + # print "susp flag: ", self.is_suspension_flag() + + # data = userdata.susp_data.req_data + # print "==================================" + # print "data= ", data + # print "==================================" + # print "======================================" + # print "\n\n\n" + if self.is_suspension_flag() != None: + self.conversation_interface.removeAutomaticAnswer( + answer_id) + client.cancel_all_goals() + self.service_preempt() + return 'preemption' + + rospy.sleep(0.1) + + # Manage state of the move_base action server + self.conversation_interface.removeAutomaticAnswer(answer_id) + self.navigation_goal_status = Nav2Client.get_current_goal_status() + + # Here check move_base DONE status + # if self.move_base_status == GoalStatus.PENDING: + # # The goal has yet to be processed by the action server + # raise Exception('Wrong move_base action status: "PENDING"') + # elif self.move_base_status == GoalStatus.ACTIVE: + # # The goal is currently being processed by the action server + # raise Exception('Wrong move_base action status: "ACTIVE"') + # elif self.move_base_status == GoalStatus.PREEMPTED: + # # The goal received a cancel request after it started executing + # # and has since completed its execution (Terminal State) + # return 'preemption' + # elif self.move_base_status == GoalStatus.SUCCEEDED: + # # The goal was achieved successfully by the action server (Terminal State) + # return 'ok' + # elif self.move_base_status == GoalStatus.ABORTED: + # # The goal was aborted during execution by the action server due + # # to some failure (Terminal State) + # return 'stall' + # elif self.move_base_status == GoalStatus.REJECTED: + # # The goal was rejected by the action server without being processed, + # # because the goal was unattainable or invalid (Terminal State) + # return 'error' + # elif self.move_base_status == GoalStatus.PREEMPTING: + # # The goal received a cancel request after it started executing + # # and has not yet completed execution + # raise Exception('Wrong move_base action status: "PREEMPTING"') + # elif self.move_base_status == GoalStatus.RECALLING: + # # The goal received a cancel request before it started executing, + # # but the action server has not yet confirmed that the goal is canceled + # raise Exception('Wrong move_base action status: "RECALLING"') + # elif self.move_base_status == GoalStatus.RECALLED: + # # The goal received a cancel request before it started executing + # # and was successfully cancelled (Terminal State) + # return 'preemption' + # elif self.move_base_status == GoalStatus.LOST: + # # An action client can determine that a goal is LOST. This should not be + # # sent over the wire by an action server + # raise Exception('Wrong move_base action status: "LOST"') + # else: + # raise Exception('Wrong move_base action status value: "' + str(self.move_base_status) + '"') + + # def move_base_feedback_cb(self, feedback): + # self.current_pose = feedback.base_position.pose + # self.is_feedback_received = True + + # def move_base_done_cb(self, status, result): + # self.is_goal_achieved = True + # self.move_base_status = status + + # def move_base_active_cb(self): + # # Do nothing + # return + + +class MoveToHuman(MoveTo): + def __init__(self, sim_mode, conversation_interface): + self.last_human_pose_update = None + self.HUMAN_POSE_UPDATE_IN_APPROACH = 2 + self.Nav2Client = Nav2Client() + self.clock = Nav2Client.get_clock() + self.navigation_goal_status + MoveTo.__init__(self, sim_mode, conversation_interface) + + def get_current_goal_status(self): + self.navigation_goal_status = self.Nav2Client.get_current_goal_status() + return self.navigation_goal_status + + def set_destination_pose(self, userdata): + human_pose = getFromPose(userdata.move_goal.parameters['pose']) + dest_pose = PoseStamped() + dest_pose.position.x = human_pose[0] + 1*math.cos(human_pose[2]) + dest_pose.position.y = human_pose[1] + 1*math.sin(human_pose[2]) + dest_pose.orientation = makePose( + 0, 0, human_pose[2]-math.pi).orientation + userdata.move_goal.parameters['pose'] = dest_pose + + def update_destination_pose(self, userdata): + if self.last_human_pose_update is None: + self.last_human_pose_update = self.clock.time().now() + if self.clock.time().now() < self.last_human_pose_update + rospy.Duration.from_sec(self.HUMAN_POSE_UPDATE_IN_APPROACH): + return False + self.last_human_pose_update = self.clock.time().now() + print("----------- UPDATING HUMAN POSE for MoveTo----------------") + human = userdata.move_goal.parameters['place_name'] + current_human_pose = yaml.load(rospy.get_param(human+'/pose')) + dest_pose = PoseStamped() + dest_pose.position.x = current_human_pose['x'] + \ + 1*math.cos(current_human_pose['theta']) + dest_pose.position.y = current_human_pose['y'] + \ + 1*math.sin(current_human_pose['theta']) + dest_pose.orientation = makePose( + 0, 0, current_human_pose['theta']-math.pi).orientation + userdata.move_goal.parameters['pose'] = dest_pose + return True + + +class MoveToAwareHazards(MoveTo): + def __init__(self, sim_mode, conversation_interface): + self.hazard_detector = HazardDetector() + self.hazard_trigger = False + self.sim_mode = sim_mode + MoveTo.__init__(self, sim_mode, conversation_interface) + + def transition_function(self, userdata): + global HUMAN_POSE_UPDATE_IN_APPROACH + # rospy.loginfo('{}: Executing state: {}'.format(rospy.get_name(), self.__class__.__name__)) + print("EXECUTING STATE: ", type(self).__name__) + + place_name = userdata.move_goal.parameters['place_name'] + + assert isinstance(place_name, unicode) + answer_id = self.conversation_interface.setAutomaticAnswer( + 'q_current_task', u'niekorzystne warunki pogodowe jadę do {"' + place_name + u'", dopelniacz}') + self.set_destination_pose(userdata) + pose = userdata.move_goal.parameters['pose'] + place_name = userdata.move_goal.parameters['place_name'] + + if self.sim_mode == 'sim': + for i in range(50): + if self.is_suspension_flag() != None: + self.conversation_interface.removeAutomaticAnswer( + answer_id) + self.request_preempt() + return 'preemption' + + time.sleep(0.2) + self.conversation_interface.removeAutomaticAnswer(answer_id) + return 'ok' + else: + goal = PoseStamped() + goal.pose = pose + goal.header.frame_id = 'map' + goal.header.stamp = self.clock.time().now().to_msg() + + # client = actionlib.SimpleActionClient('move_base', MoveBaseAction) + # client.wait_for_server() + + # turn off auto head motion + if self.sim_mode not in ['sim', 'gazebo']: + client_autonomous_head = actionlib.SimpleActionClient( + '/pal_head_manager/disable', DisableAction) + client_autonomous_head.wait_for_server() + client_autonomous_head.send_goal(DisableGoal()) + # client_autonomous_head.wait_for_result() + # move head to detect objects on the floor + client_move_head = actionlib.SimpleActionClient( + '/head_controller/point_head_action', PointHeadAction) + client_move_head.wait_for_server() + point_head_goal = PointHeadGoal() + point_head_goal.target.header.frame_id = 'base_link' + point_head_goal.target.point.x = 1.5 + point_head_goal.pointing_axis.z = 1 + point_head_goal.pointing_frame = 'xtion_rgb_optical_frame' + point_head_goal.min_duration.secs = 1 + point_head_goal.max_velocity = 1 + client_move_head.send_goal(point_head_goal) + # client_move_head.wait_for_result() + # start moving + # client.send_goal(goal, self.move_base_done_cb, self.move_base_active_cb, self.move_base_feedback_cb) + Nav2Client.send_goal(goal) + + # action_feedback = GoActionFeedback() + # action_result = GoActionResult() + # action_result.result.is_goal_accomplished = False + # userdata.nav_result = action_result.result + + start_time = clock.time().now() + last_human_update = clock.time().now() + self.is_goal_achieved = False + self.hazard_trigger = False + while ((self.is_goal_achieved == False or self.get_current_goal_status() == GoalStatus.STATUS_ABORTED) or self.hazard_trigger == True): + # action_feedback.feedback.current_pose = self.current_pose + + # userdata.nav_feedback = action_feedback.feedback + # userdata.nav_actual_pose = self.current_pose + + end_time = clock.time().now() + loop_time = end_time - start_time + loop_time_s = loop_time.secs + + if self.__shutdown__: + Nav2Client.cancel_all_goals() + self.conversation_interface.removeAutomaticAnswer( + answer_id) + self.service_preempt() + return 'shutdown' + + if loop_time_s > NAVIGATION_MAX_TIME_S: + # break the loop, end with error state + self.conversation_interface.removeAutomaticAnswer( + answer_id) + # rospy.logwarn('State: Navigation took too much time, returning error') + print('State: Navigation took too much time, returning error') + Nav2Client.cancel_all_goals() + return 'stall' + + if self.update_destination_pose(userdata): + goal.pose = userdata.move_goal.parameters['pose'] + # client.send_goal(goal, self.move_base_done_cb, self.move_base_active_cb, self.move_base_feedback_cb) + Nav2Client.send_goal(goal) + self.hazard_trigger, hazard_object = self.hazard_detector.check_hazard() + + if self.hazard_trigger: + print("HAZARD DETECTED") + print("UWAGA! ZNALAZLEM ", hazard_object, + " NA PODLODZE. PROSZE OMNIN TA PRZESZKODE") + # goal is interrupted by the hazard cancel goal + Nav2Client.cancel_goal() + answer_id = self.conversation_interface.speakNowBlocking( + u'niekorzystne warunki pogodowe Uwaga! Znalazłem {"' + hazard_object + u'", biernik} na podłodze. Proszę omiń tą przeszkodę.') + time.sleep(3) + # goal was interrupted by hazard continue motion + # client.wait_for_server() + # client.send_goal(goal, self.move_base_done_cb, self.move_base_active_cb, self.move_base_feedback_cb) + Nav2Client.send_goal(goal) + # rospy.sleep(5) + # self.is_goal_achieved = False + # print "MOVE STATUS2: complete ", type(self.move_base_status) + + # print "\n\n\n" + # print "======================================" + # print "susp flag: ", self.is_suspension_flag() + + # data = userdata.susp_data.req_data + # print "==================================" + # print "data= ", data + # print "==================================" + # print "======================================" + # print "\n\n\n" + if self.is_suspension_flag() != None: + self.conversation_interface.removeAutomaticAnswer( + answer_id) + Nav2Client.cancel_all_goals() + self.service_preempt() + return 'preemption' + + rospy.sleep(0.1) + + # Manage state of the move_base action server + self.conversation_interface.removeAutomaticAnswer(answer_id) + # move head ahead + client_move_head.wait_for_server() + point_head_goal = PointHeadGoal() + print("point_head_goal:\n", point_head_goal) + point_head_goal.target.header.frame_id = 'torso_lift_link' + point_head_goal.target.point.x = 1 + point_head_goal.target.point.z = 0.18 + point_head_goal.pointing_axis.z = 1 + point_head_goal.pointing_frame = 'xtion_rgb_optical_frame' + point_head_goal.min_duration.secs = 1 + point_head_goal.max_velocity = 1 + client_move_head.send_goal(point_head_goal) + + # turn on auto head motion + + if self.sim_mode not in ['sim', 'gazebo']: + client_autonomous_head = actionlib.SimpleActionClient( + '/pal_head_manager/disable', DisableAction) + client_autonomous_head.cancel_all_goals() + + goal_status = self.get_current_goal_status() + if (goal_status == GoalStatus.STATUS_SUCCEEDED): + # The goal was achieved successfully by the action server (Terminal State) + return 'ok' + elif (goal_status == GoalStatus.STATUS_ABORTED): + # The goal was aborted during execution by the action server due + # to some failure (Terminal State) + return 'stall' + elif (goal_status == GoalStatus.STATUS_EXECUTING): + # The goal was aborted during execution by the action server due + # to some failure (Terminal State) + return 'stall' + elif (goal_status == GoalStatus.STATUS_CANCELED): + # The goal was aborted during execution by the action server due + # to some failure (Terminal State) + return 'stall' + else: + raise Exception( + 'Wrong move_base action status: "' + str(goal_status) + '"') + + # Here check move_base DONE status + # if self.move_base_status == GoalStatus.PENDING: + # # The goal has yet to be processed by the action server + # raise Exception('Wrong move_base action status: "PENDING"') + # elif self.move_base_status == GoalStatus.ACTIVE: + # # The goal is currently being processed by the action server + # raise Exception('Wrong move_base action status: "ACTIVE"') + # elif self.move_base_status == GoalStatus.PREEMPTED: + # # The goal received a cancel request after it started executing + # # and has since completed its execution (Terminal State) + # return 'preemption' + # elif self.move_base_status == GoalStatus.SUCCEEDED: + # # The goal was achieved successfully by the action server (Terminal State) + # return 'ok' + # elif self.move_base_status == GoalStatus.ABORTED: + # # The goal was aborted during execution by the action server due + # # to some failure (Terminal State) + # return 'stall' + # elif self.move_base_status == GoalStatus.REJECTED: + # # The goal was rejected by the action server without being processed, + # # because the goal was unattainable or invalid (Terminal State) + # return 'error' + # elif self.move_base_status == GoalStatus.PREEMPTING: + # # The goal received a cancel request after it started executing + # # and has not yet completed execution + # raise Exception('Wrong move_base action status: "PREEMPTING"') + # elif self.move_base_status == GoalStatus.RECALLING: + # # The goal received a cancel request before it started executing, + # # but the action server has not yet confirmed that the goal is canceled + # raise Exception('Wrong move_base action status: "RECALLING"') + # elif self.move_base_status == GoalStatus.RECALLED: + # # The goal received a cancel request before it started executing + # # and was successfully cancelled (Terminal State) + # return 'preemption' + # elif self.move_base_status == GoalStatus.LOST: + # # An action client can determine that a goal is LOST. This should not be + # # sent over the wire by an action server + # raise Exception('Wrong move_base action status: "LOST"') + # else: + # raise Exception('Wrong move_base action status value: "' + str(self.move_base_status) + '"') + + +class TurnAround(TaskER.BlockingState): + def __init__(self, sim_mode, conversation_interface): + assert sim_mode in ['sim', 'gazebo', 'real'] + self.current_pose = Pose() + self.is_feedback_received = False + self.move_base_status = GoalStatus.PENDING + self.is_goal_achieved = False + self.sim_mode = sim_mode + self.conversation_interface = conversation_interface + self.Nav2Client = Nav2Client() + self.clock = Nav2Client.get_clock() + + TaskER.BlockingState.__init__(self, tf_freq=10, + outcomes=['ok', 'preemption', + 'error', 'stall', 'shutdown'], + input_keys=['current_pose']) + + self.description = u'Odwracam się' + + def transition_function(self, userdata): + # rospy.loginfo('{}: Executing state: {}'.format(rospy.get_name(), self.__class__.__name__)) + print("EXECUTING STATE: ", type(self).__name__) + + pose = userdata.current_pose.parameters['pose'] + + alpha, beta, theta = euler_from_quaternion( + (pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w)) + print('TurnAround current theta: {} {} {}'.format(alpha, beta, theta)) + if theta < 0: + new_pose = makePose( + pose.position.x, pose.position.y, theta+math.pi) + else: + new_pose = makePose( + pose.position.x, pose.position.y, theta-math.pi) + + answer_id = self.conversation_interface.setAutomaticAnswer( + 'q_current_task', u'niekorzystne warunki pogodowe odwracam się') + + if self.sim_mode == 'sim': + for i in range(50): + if self.preempt_requested(): + self.conversation_interface.removeAutomaticAnswer( + answer_id) + self.service_preempt() + return 'preemption' + + time.sleep(0.1) + self.conversation_interface.removeAutomaticAnswer(answer_id) + return 'ok' + else: + goal = PoseStamped() + goal.pose = new_pose + goal.header.frame_id = 'map' + goal.header.stamp = clock.time().now().to_msg() + + # client = actionlib.SimpleActionClient('move_base', MoveBaseAction) + # client.wait_for_server() + + # start moving + # client.send_goal(goal, self.move_base_done_cb, self.move_base_active_cb, self.move_base_feedback_cb) + Nav2Client.send_goal(goal) + + # action_feedback = GoActionFeedback() + # action_result = GoActionResult() + # action_result.result.is_goal_accomplished = False + # userdata.nav_result = action_result.result + + start_time = clock.time().now() + + self.is_goal_achieved = False + while self.is_goal_achieved == False: + # action_feedback.feedback.current_pose = self.current_pose + + # userdata.nav_feedback = action_feedback.feedback + # userdata.nav_actual_pose = self.current_pose + + end_time = clock.time().now() + loop_time = end_time - start_time + loop_time_s = loop_time.nanoseconds * 1e-9 + + if self.__shutdown__: + client.cancel_all_goals() + self.conversation_interface.removeAutomaticAnswer( + answer_id) + self.service_preempt() + return 'shutdown' + + if loop_time_s > NAVIGATION_MAX_TIME_S: + # break the loop, end with error state + self.conversation_interface.removeAutomaticAnswer( + answer_id) + # rospy.logwarn('State: Navigation took too much time, returning error') + print('State: Navigation took too much time, returning error') + client.cancel_all_goals() + return 'stall' + + if self.preempt_requested(): + self.conversation_interface.removeAutomaticAnswer( + answer_id) + client.cancel_all_goals() + self.service_preempt() + return 'preemption' + + time.sleep(0.1) + + # Manage state of the move_base action server + self.conversation_interface.removeAutomaticAnswer(answer_id) + + goal_status = self.get_current_goal_status() + if (goal_status == GoalStatus.STATUS_SUCCEEDED): + # The goal was achieved successfully by the action server (Terminal State) + return 'ok' + elif (goal_status == GoalStatus.STATUS_ABORTED): + # The goal was aborted during execution by the action server due + # to some failure (Terminal State) + return 'stall' + elif (goal_status == GoalStatus.STATUS_EXECUTING): + # The goal was aborted during execution by the action server due + # to some failure (Terminal State) + return 'stall' + elif (goal_status == GoalStatus.STATUS_CANCELED): + # The goal was aborted during execution by the action server due + # to some failure (Terminal State) + return 'stall' + else: + raise Exception( + 'Wrong move_base action status: "' + str(goal_status) + '"') + + # Here check move_base DONE status + # if self.move_base_status == GoalStatus.PENDING: + # # The goal has yet to be processed by the action server + # raise Exception('Wrong move_base action status: "PENDING"') + # elif self.move_base_status == GoalStatus.ACTIVE: + # # The goal is currently being processed by the action server + # raise Exception('Wrong move_base action status: "ACTIVE"') + # elif self.move_base_status == GoalStatus.PREEMPTED: + # # The goal received a cancel request after it started executing + # # and has since completed its execution (Terminal State) + # return 'preemption' + # elif self.move_base_status == GoalStatus.SUCCEEDED: + # # The goal was achieved successfully by the action server (Terminal State) + # return 'ok' + # elif self.move_base_status == GoalStatus.ABORTED: + # # The goal was aborted during execution by the action server due + # # to some failure (Terminal State) + # return 'stall' + # elif self.move_base_status == GoalStatus.REJECTED: + # # The goal was rejected by the action server without being processed, + # # because the goal was unattainable or invalid (Terminal State) + # return 'error' + # elif self.move_base_status == GoalStatus.PREEMPTING: + # # The goal received a cancel request after it started executing + # # and has not yet completed execution + # raise Exception('Wrong move_base action status: "PREEMPTING"') + # elif self.move_base_status == GoalStatus.RECALLING: + # # The goal received a cancel request before it started executing, + # # but the action server has not yet confirmed that the goal is canceled + # raise Exception('Wrong move_base action status: "RECALLING"') + # elif self.move_base_status == GoalStatus.RECALLED: + # # The goal received a cancel request before it started executing + # # and was successfully cancelled (Terminal State) + # return 'preemption' + # elif self.move_base_status == GoalStatus.LOST: + # # An action client can determine that a goal is LOST. This should not be + # # sent over the wire by an action server + # raise Exception('Wrong move_base action status: "LOST"') + # else: + # raise Exception('Wrong move_base action status value: "' + str(self.move_base_status) + '"') + + # def move_base_feedback_cb(self, feedback): + # self.current_pose = feedback.base_position.pose + # self.is_feedback_received = True + + # def move_base_done_cb(self, status, result): + # self.is_goal_achieved = True + # self.move_base_status = status + + # def move_base_active_cb(self): + # # Do nothing + # return + + +class ClearCostMaps(TaskER.BlockingState): + def __init__(self, sim_mode): + assert sim_mode in ['sim', 'gazebo', 'real'] + # if sim_mode == 'sim': + # self.clear_costmaps = None + # else: + # rospy.wait_for_service('/move_base/clear_costmaps') + # # try: + # self.clear_costmaps = rospy.ServiceProxy( + # '/move_base/clear_costmaps', std_srvs.Empty) + # except rospy.ServiceException, e: + # print "Service call failed: %s"%e + # self.clear_costmaps = None + + TaskER.BlockingState.__init__(self, tf_freq=10, + outcomes=['ok', 'preemption', 'error', 'shutdown']) + + self.description = u'Czyszczę mapę kosztów' + + def transition_function(self, userdata): + # rospy.loginfo('{}: Executing state: {}'.format( + # rospy.get_name(), self.__class__.__name__)) + + # if not self.clear_costmaps is None: + # self.clear_costmaps() + # rospy.sleep(0.5) + + # if self.preempt_requested(): + # self.service_preempt() + # return 'preemption' + + # if self.__shutdown__: + # return 'shutdown' + return 'ok' + + +class MoveToComplexBlocking(smach_rcprg.StateMachine): + def __init__(self, sim_mode, conversation_interface, kb_places): + smach_rcprg.StateMachine.__init__(self, outcomes=['FINISHED', 'PREEMPTED', 'FAILED', 'shutdown'], + input_keys=['goal', 'susp_data']) + + self.description = u'Jadę do określonego miejsca' + + with self: + smach_rcprg.StateMachine.add('RememberCurrentPose', RememberCurrentPose(sim_mode), + transitions={'ok': 'UnderstandGoal', 'preemption': 'PREEMPTED', 'error': 'FAILED', + 'shutdown': 'shutdown'}, + remapping={'current_pose': 'current_pose'}) + + smach_rcprg.StateMachine.add('UnderstandGoal', UnderstandGoal(sim_mode, conversation_interface, kb_places), + transitions={'ok': 'SayImGoingTo', 'preemption': 'PREEMPTED', 'error': 'SayIdontKnow', + 'shutdown': 'shutdown'}, + remapping={'in_current_pose': 'current_pose', 'goal_pose': 'goal', 'move_goal': 'move_goal'}) + + smach_rcprg.StateMachine.add('SayImGoingTo', SayImGoingTo(sim_mode, conversation_interface), + transitions={'ok': 'MoveTo', 'preemption': 'PREEMPTED', 'error': 'FAILED', + 'shutdown': 'shutdown'}, + remapping={'move_goal': 'move_goal'}) + + smach_rcprg.StateMachine.add('MoveTo', MoveToBlocking(sim_mode, conversation_interface), + transitions={'ok': 'SayIArrivedTo', 'preemption': 'PREEMPTED', 'error': 'FAILED', 'stall': 'ClearCostMaps', + 'shutdown': 'shutdown'}, + remapping={'move_goal': 'move_goal', 'susp_data': 'susp_data'}) + + smach_rcprg.StateMachine.add('ClearCostMaps', ClearCostMaps(sim_mode), + transitions={'ok': 'MoveTo', 'preemption': 'PREEMPTED', 'error': 'FAILED', + 'shutdown': 'shutdown'}) + + smach_rcprg.StateMachine.add('SayIArrivedTo', SayIArrivedTo(sim_mode, conversation_interface), + transitions={'ok': 'FINISHED', 'preemption': 'PREEMPTED', 'error': 'FAILED', + 'shutdown': 'shutdown'}, + remapping={'move_goal': 'move_goal'}) + + smach_rcprg.StateMachine.add('SayIdontKnow', SayIdontKnow(sim_mode, conversation_interface), + transitions={ + 'ok': 'FAILED', 'shutdown': 'shutdown'}, + remapping={'move_goal': 'move_goal'}) + + +class MoveToComplex(smach_rcprg.StateMachine): + def __init__(self, sim_mode, conversation_interface, kb_places): + smach_rcprg.StateMachine.__init__(self, outcomes=['FINISHED', 'PREEMPTED', 'FAILED', 'shutdown'], + input_keys=['goal', 'susp_data']) + + self.description = u'Jadę do określonego miejsca' + + with self: + smach_rcprg.StateMachine.add('RememberCurrentPose', RememberCurrentPose(sim_mode), + transitions={'ok': 'UnderstandGoal', 'preemption': 'PREEMPTED', 'error': 'FAILED', + 'shutdown': 'shutdown'}, + remapping={'current_pose': 'current_pose'}) + + smach_rcprg.StateMachine.add('UnderstandGoal', UnderstandGoal(sim_mode, conversation_interface, kb_places), + transitions={'ok': 'SayImGoingTo', 'preemption': 'PREEMPTED', 'error': 'SayIdontKnow', + 'shutdown': 'shutdown'}, + remapping={'in_current_pose': 'current_pose', 'goal_pose': 'goal', 'move_goal': 'move_goal'}) + + smach_rcprg.StateMachine.add('SayImGoingTo', SayImGoingTo(sim_mode, conversation_interface), + transitions={'ok': 'MoveTo', 'preemption': 'PREEMPTED', 'error': 'FAILED', + 'shutdown': 'shutdown'}, + remapping={'move_goal': 'move_goal'}) + + smach_rcprg.StateMachine.add('MoveTo', MoveToAwareHazards(sim_mode, conversation_interface), + transitions={'ok': 'SayIArrivedTo', 'preemption': 'PREEMPTED', 'error': 'FAILED', 'stall': 'ClearCostMaps', + 'shutdown': 'shutdown'}, + remapping={'move_goal': 'move_goal', 'susp_data': 'susp_data'}) + + smach_rcprg.StateMachine.add('ClearCostMaps', ClearCostMaps(sim_mode), + transitions={'ok': 'MoveTo', 'preemption': 'PREEMPTED', 'error': 'FAILED', + 'shutdown': 'shutdown'}) + + smach_rcprg.StateMachine.add('SayIArrivedTo', SayIArrivedTo(sim_mode, conversation_interface), + transitions={'ok': 'FINISHED', 'preemption': 'PREEMPTED', 'error': 'FAILED', + 'shutdown': 'shutdown'}, + remapping={'move_goal': 'move_goal'}) + + smach_rcprg.StateMachine.add('SayIdontKnow', SayIdontKnow(sim_mode, conversation_interface), + transitions={ + 'ok': 'FAILED', 'shutdown': 'shutdown'}, + remapping={'move_goal': 'move_goal'}) + + +class MoveToHumanComplex(smach_rcprg.StateMachine): + def __init__(self, sim_mode, conversation_interface, kb_places): + smach_rcprg.StateMachine.__init__(self, outcomes=['FINISHED', 'PREEMPTED', 'FAILED', 'shutdown'], + input_keys=['goal', 'susp_data']) + + self.description = u'Jadę do określonego miejsca' + + with self: + smach_rcprg.StateMachine.add('RememberCurrentPose', RememberCurrentPose(sim_mode), + transitions={'ok': 'UnderstandGoal', 'preemption': 'PREEMPTED', 'error': 'FAILED', + 'shutdown': 'shutdown'}, + remapping={'current_pose': 'current_pose'}) + + smach_rcprg.StateMachine.add('UnderstandGoal', UnderstandGoal(sim_mode, conversation_interface, kb_places), + transitions={'ok': 'SayImGoingTo', 'preemption': 'PREEMPTED', 'error': 'SayIdontKnow', + 'shutdown': 'shutdown'}, + remapping={'in_current_pose': 'current_pose', 'goal_pose': 'goal', 'move_goal': 'move_goal'}) + + smach_rcprg.StateMachine.add('SayImGoingTo', SayImGoingTo(sim_mode, conversation_interface), + transitions={'ok': 'MoveTo', 'preemption': 'PREEMPTED', 'error': 'FAILED', + 'shutdown': 'shutdown'}, + remapping={'move_goal': 'move_goal'}) + + smach_rcprg.StateMachine.add('MoveTo', MoveToHuman(sim_mode, conversation_interface), + transitions={'ok': 'SayIArrivedTo', 'preemption': 'PREEMPTED', 'error': 'FAILED', 'stall': 'ClearCostMaps', + 'shutdown': 'shutdown'}, + remapping={'move_goal': 'move_goal', 'susp_data': 'susp_data'}) + + smach_rcprg.StateMachine.add('ClearCostMaps', ClearCostMaps(sim_mode), + transitions={'ok': 'MoveTo', 'preemption': 'PREEMPTED', 'error': 'FAILED', + 'shutdown': 'shutdown'}) + + smach_rcprg.StateMachine.add('SayIArrivedTo', SayIArrivedTo(sim_mode, conversation_interface), + transitions={'ok': 'FINISHED', 'preemption': 'PREEMPTED', 'error': 'FAILED', + 'shutdown': 'shutdown'}, + remapping={'move_goal': 'move_goal'}) + + smach_rcprg.StateMachine.add('SayIdontKnow', SayIdontKnow(sim_mode, conversation_interface), + transitions={ + 'ok': 'FAILED', 'shutdown': 'shutdown'}, + remapping={'move_goal': 'move_goal', 'goal': 'move_goal'}) diff --git a/src/rcprg_smach/navigation_helpers.py b/src/rcprg_smach/navigation_helpers.py new file mode 100644 index 0000000..c3208de --- /dev/null +++ b/src/rcprg_smach/navigation_helpers.py @@ -0,0 +1,46 @@ +#!/usr/bin/env python3 +# encoding: utf8 + +import rclpy + +from geometry_msgs.msg import Quaternion +from geometry_msgs.msg import Pose +import math + + +def quaternion_from_euler(roll, pitch, yaw): + """ + Converts euler roll, pitch, yaw to quaternion (w in last place) + quat = [x, y, z, w] + """ + cy = math.cos(yaw * 0.5) + sy = math.sin(yaw * 0.5) + cp = math.cos(pitch * 0.5) + sp = math.sin(pitch * 0.5) + cr = math.cos(roll * 0.5) + sr = math.sin(roll * 0.5) + + q = [0] * 4 + q[0] = cy * cp * cr + sy * sp * sr + q[1] = cy * cp * sr - sy * sp * cr + q[2] = sy * cp * sr + cy * sp * cr + q[3] = sy * cp * cr - cy * sp * sr + + return q + +def makePose(x, y, theta): + q = quaternion_from_euler(0, 0, theta) + result = Pose() + result.position.x = x + result.position.y = y + result.orientation.x = q[0] + result.orientation.y = q[1] + result.orientation.z = q[2] + result.orientation.w = q[3] + return result + +def getFromPose(pose): + roll, pitch, yaw = euler_from_quaternion([pose.orientation.x,pose.orientation.y,pose.orientation.z,pose.orientation.w]) + return pose.position.x, pose.position.y, yaw + + From a6c136bd1a1c3019dbb8920370aa25527e80046e Mon Sep 17 00:00:00 2001 From: Adam Krawczyk Date: Fri, 12 Jan 2024 13:11:50 +0100 Subject: [PATCH 4/4] Fixed build/run issues Signed-off-by: Adam Krawczyk --- CMakeLists.txt | 50 ---------- package.xml | 20 ++-- {src/rcprg_smach => rcprg_smach}/__init__.py | 0 .../__pycache__/__init__.cpython-310.pyc | Bin 0 -> 154 bytes .../ros_node_utils.cpython-310.pyc | Bin 0 -> 1402 bytes .../bring_goods_tasker.py | 0 {src/rcprg_smach => rcprg_smach}/bring_jar.py | 0 .../conversation.py | 0 {src/rcprg_smach => rcprg_smach}/gh_csp.py | 0 .../guide_human.py | 0 .../hazard_detector.py | 0 .../rcprg_smach => rcprg_smach}/human_fell.py | 0 .../human_fell_approach.py | 0 .../manipulation.py | 0 .../nav2_statuses.py | 0 .../rcprg_smach => rcprg_smach}/navigation.py | 0 .../navigation2.py | 0 .../navigation_helpers.py | 0 {nodes => rcprg_smach/nodes}/bring_goods.py | 0 .../nodes}/bring_goods_tasker.py | 0 {nodes => rcprg_smach/nodes}/bring_jar.py | 0 .../nodes}/bring_jar_tasker.py | 0 {nodes => rcprg_smach/nodes}/call.py | 0 .../nodes}/guide_human_tasker.py | 0 .../nodes}/human_fell_approach_tasker.py | 0 .../nodes}/human_fell_tasker.py | 0 {nodes => rcprg_smach/nodes}/idle.py | 0 {nodes => rcprg_smach/nodes}/idle_tasker.py | 91 +++++++++++------- {nodes => rcprg_smach/nodes}/move_to.py | 0 .../nodes}/move_to_tasker.py | 0 {nodes => rcprg_smach/nodes}/stop.py | 0 .../nodes}/task_harmonizer.py | 2 +- .../nodes}/test_bring_goods.py | 0 {nodes => rcprg_smach/nodes}/test_move_to.py | 0 {nodes => rcprg_smach/nodes}/test_wander.py | 0 {nodes => rcprg_smach/nodes}/wander.py | 0 rcprg_smach/ros_node_utils.py | 67 +++++++++++++ .../smach_rcprg.py | 0 .../rcprg_smach => rcprg_smach}/suspend_bj.py | 0 .../rcprg_smach => rcprg_smach}/suspend_gh.py | 0 .../task_manager.py | 0 .../update_pose.py | 0 resource/rcprg_smach | 0 setup.cfg | 4 + setup.py | 30 ++++-- src/rcprg_smach/ros_node_utils.py | 37 ------- test/test_copyright.py | 25 +++++ test/test_flake8.py | 25 +++++ test/test_pep257.py | 23 +++++ 49 files changed, 231 insertions(+), 143 deletions(-) delete mode 100644 CMakeLists.txt rename {src/rcprg_smach => rcprg_smach}/__init__.py (100%) mode change 100644 => 100755 create mode 100644 rcprg_smach/__pycache__/__init__.cpython-310.pyc create mode 100644 rcprg_smach/__pycache__/ros_node_utils.cpython-310.pyc rename {src/rcprg_smach => rcprg_smach}/bring_goods_tasker.py (100%) mode change 100644 => 100755 rename {src/rcprg_smach => rcprg_smach}/bring_jar.py (100%) mode change 100644 => 100755 rename {src/rcprg_smach => rcprg_smach}/conversation.py (100%) rename {src/rcprg_smach => rcprg_smach}/gh_csp.py (100%) mode change 100644 => 100755 rename {src/rcprg_smach => rcprg_smach}/guide_human.py (100%) mode change 100644 => 100755 rename {src/rcprg_smach => rcprg_smach}/hazard_detector.py (100%) mode change 100644 => 100755 rename {src/rcprg_smach => rcprg_smach}/human_fell.py (100%) mode change 100644 => 100755 rename {src/rcprg_smach => rcprg_smach}/human_fell_approach.py (100%) mode change 100644 => 100755 rename {src/rcprg_smach => rcprg_smach}/manipulation.py (100%) mode change 100644 => 100755 rename {src/rcprg_smach => rcprg_smach}/nav2_statuses.py (100%) mode change 100644 => 100755 rename {src/rcprg_smach => rcprg_smach}/navigation.py (100%) mode change 100644 => 100755 rename {src/rcprg_smach => rcprg_smach}/navigation2.py (100%) mode change 100644 => 100755 rename {src/rcprg_smach => rcprg_smach}/navigation_helpers.py (100%) mode change 100644 => 100755 rename {nodes => rcprg_smach/nodes}/bring_goods.py (100%) rename {nodes => rcprg_smach/nodes}/bring_goods_tasker.py (100%) rename {nodes => rcprg_smach/nodes}/bring_jar.py (100%) rename {nodes => rcprg_smach/nodes}/bring_jar_tasker.py (100%) rename {nodes => rcprg_smach/nodes}/call.py (100%) rename {nodes => rcprg_smach/nodes}/guide_human_tasker.py (100%) rename {nodes => rcprg_smach/nodes}/human_fell_approach_tasker.py (100%) rename {nodes => rcprg_smach/nodes}/human_fell_tasker.py (100%) rename {nodes => rcprg_smach/nodes}/idle.py (100%) rename {nodes => rcprg_smach/nodes}/idle_tasker.py (71%) rename {nodes => rcprg_smach/nodes}/move_to.py (100%) rename {nodes => rcprg_smach/nodes}/move_to_tasker.py (100%) rename {nodes => rcprg_smach/nodes}/stop.py (100%) rename {nodes => rcprg_smach/nodes}/task_harmonizer.py (99%) mode change 100644 => 100755 rename {nodes => rcprg_smach/nodes}/test_bring_goods.py (100%) rename {nodes => rcprg_smach/nodes}/test_move_to.py (100%) rename {nodes => rcprg_smach/nodes}/test_wander.py (100%) rename {nodes => rcprg_smach/nodes}/wander.py (100%) create mode 100755 rcprg_smach/ros_node_utils.py rename {src/rcprg_smach => rcprg_smach}/smach_rcprg.py (100%) mode change 100644 => 100755 rename {src/rcprg_smach => rcprg_smach}/suspend_bj.py (100%) mode change 100644 => 100755 rename {src/rcprg_smach => rcprg_smach}/suspend_gh.py (100%) mode change 100644 => 100755 rename {src/rcprg_smach => rcprg_smach}/task_manager.py (100%) rename {src/rcprg_smach => rcprg_smach}/update_pose.py (100%) mode change 100644 => 100755 create mode 100644 resource/rcprg_smach create mode 100644 setup.cfg delete mode 100755 src/rcprg_smach/ros_node_utils.py create mode 100644 test/test_copyright.py create mode 100644 test/test_flake8.py create mode 100644 test/test_pep257.py diff --git a/CMakeLists.txt b/CMakeLists.txt deleted file mode 100644 index 83c2df0..0000000 --- a/CMakeLists.txt +++ /dev/null @@ -1,50 +0,0 @@ -cmake_minimum_required(VERSION 3.8) -project(rcprg_smach) - -# 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 -) - -# rosidl_generate_interfaces(${PROJECT_NAME} -# "msgs and services" -# DEPENDENCIES ${dependencies} -# ) - -ament_export_dependencies(rosidl_default_runtime) - -include_directories( - include -) - -install(DIRECTORY launch/ - DESTINATION share/${PROJECT_NAME}/launch -) - -# Uncomment if you have node executables -# install(TARGETS your_node_targets -# DESTINATION lib/${PROJECT_NAME} -# ) - -ament_package() \ No newline at end of file diff --git a/package.xml b/package.xml index 5aeea17..b7ce96c 100644 --- a/package.xml +++ b/package.xml @@ -6,19 +6,13 @@ TODO Dawid Seredynski - Adam Krawczyk - ament_cmake - - action_msgs - diagnostic_msgs - geometry_msgs - rclcpp - nav_msgs - std_msgs - std_srvs - tf2_ros - - + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + + ament_python + diff --git a/src/rcprg_smach/__init__.py b/rcprg_smach/__init__.py old mode 100644 new mode 100755 similarity index 100% rename from src/rcprg_smach/__init__.py rename to rcprg_smach/__init__.py diff --git a/rcprg_smach/__pycache__/__init__.cpython-310.pyc b/rcprg_smach/__pycache__/__init__.cpython-310.pyc new file mode 100644 index 0000000000000000000000000000000000000000..d1b1d8b76ccc67eea04f100e400c41dde48b70ce GIT binary patch literal 154 zcmd1j<>g`kf)eG&DIoeWh(HF6K#l_t7qb9~6oz01O-8?!3`HPe1o6vOKO;XkRX;H$ zF;~ANGci3szPwmJsWdYuMZYMypeQ}QI5#ml1BD$QpP83g5+AQuPPiqrF6n}4aH(g^AO05ct;zZDLcK6m6GwAEZ;vxcR|WDPmGcB6n;Ky z9v0oKnW&`Ow4LOjkVmHZanQ+CQ>T4>AWb((57+IrT8;j(E)lE-;}f)O;>qRJZW?D0 zS}7l&4Zer-zFyI02s4<&NK8RrpDsJp)hYA>g;Tf+U%;~{R4|E$;yH|Q6!bQT%U}X$ zdn;@(48}O-n~GXLvAM=2*NbRgqG@i@azIr-TzoG^I1)oKgDK2Wiy?j$pTr($RuU0c zHa5}L9ChsBY2ZZTo(p?PH*=zAm`=7$KS?w1uP;)NEAQkTW7DGB-+jo%xbvHE(p)3v zAhU~0^BEdDd66c$*(KvF_8;`j+8!s|rp2Q#Ezz%HUI3|)%fjM5JTA>)k7#;8+fWbP zjdc_<=_z^MkioI$H)W>Ta>C=cBe> z3}z8Oha{xG-yM;}DGfCg=``;jyO2LpUwDDMqB3K3aqQ*cVskjKsyOP^15dnW6}ZR~ zztR;G?ZY3FVVL_EODM2{v0$F#UuE>?i{g>KMJ}%F5j6NJHLLUwij~yo0&?-rg7#hh PFBxVfu2*nV)M~#0X&yQW literal 0 HcmV?d00001 diff --git a/src/rcprg_smach/bring_goods_tasker.py b/rcprg_smach/bring_goods_tasker.py old mode 100644 new mode 100755 similarity index 100% rename from src/rcprg_smach/bring_goods_tasker.py rename to rcprg_smach/bring_goods_tasker.py diff --git a/src/rcprg_smach/bring_jar.py b/rcprg_smach/bring_jar.py old mode 100644 new mode 100755 similarity index 100% rename from src/rcprg_smach/bring_jar.py rename to rcprg_smach/bring_jar.py diff --git a/src/rcprg_smach/conversation.py b/rcprg_smach/conversation.py similarity index 100% rename from src/rcprg_smach/conversation.py rename to rcprg_smach/conversation.py diff --git a/src/rcprg_smach/gh_csp.py b/rcprg_smach/gh_csp.py old mode 100644 new mode 100755 similarity index 100% rename from src/rcprg_smach/gh_csp.py rename to rcprg_smach/gh_csp.py diff --git a/src/rcprg_smach/guide_human.py b/rcprg_smach/guide_human.py old mode 100644 new mode 100755 similarity index 100% rename from src/rcprg_smach/guide_human.py rename to rcprg_smach/guide_human.py diff --git a/src/rcprg_smach/hazard_detector.py b/rcprg_smach/hazard_detector.py old mode 100644 new mode 100755 similarity index 100% rename from src/rcprg_smach/hazard_detector.py rename to rcprg_smach/hazard_detector.py diff --git a/src/rcprg_smach/human_fell.py b/rcprg_smach/human_fell.py old mode 100644 new mode 100755 similarity index 100% rename from src/rcprg_smach/human_fell.py rename to rcprg_smach/human_fell.py diff --git a/src/rcprg_smach/human_fell_approach.py b/rcprg_smach/human_fell_approach.py old mode 100644 new mode 100755 similarity index 100% rename from src/rcprg_smach/human_fell_approach.py rename to rcprg_smach/human_fell_approach.py diff --git a/src/rcprg_smach/manipulation.py b/rcprg_smach/manipulation.py old mode 100644 new mode 100755 similarity index 100% rename from src/rcprg_smach/manipulation.py rename to rcprg_smach/manipulation.py diff --git a/src/rcprg_smach/nav2_statuses.py b/rcprg_smach/nav2_statuses.py old mode 100644 new mode 100755 similarity index 100% rename from src/rcprg_smach/nav2_statuses.py rename to rcprg_smach/nav2_statuses.py diff --git a/src/rcprg_smach/navigation.py b/rcprg_smach/navigation.py old mode 100644 new mode 100755 similarity index 100% rename from src/rcprg_smach/navigation.py rename to rcprg_smach/navigation.py diff --git a/src/rcprg_smach/navigation2.py b/rcprg_smach/navigation2.py old mode 100644 new mode 100755 similarity index 100% rename from src/rcprg_smach/navigation2.py rename to rcprg_smach/navigation2.py diff --git a/src/rcprg_smach/navigation_helpers.py b/rcprg_smach/navigation_helpers.py old mode 100644 new mode 100755 similarity index 100% rename from src/rcprg_smach/navigation_helpers.py rename to rcprg_smach/navigation_helpers.py diff --git a/nodes/bring_goods.py b/rcprg_smach/nodes/bring_goods.py similarity index 100% rename from nodes/bring_goods.py rename to rcprg_smach/nodes/bring_goods.py diff --git a/nodes/bring_goods_tasker.py b/rcprg_smach/nodes/bring_goods_tasker.py similarity index 100% rename from nodes/bring_goods_tasker.py rename to rcprg_smach/nodes/bring_goods_tasker.py diff --git a/nodes/bring_jar.py b/rcprg_smach/nodes/bring_jar.py similarity index 100% rename from nodes/bring_jar.py rename to rcprg_smach/nodes/bring_jar.py diff --git a/nodes/bring_jar_tasker.py b/rcprg_smach/nodes/bring_jar_tasker.py similarity index 100% rename from nodes/bring_jar_tasker.py rename to rcprg_smach/nodes/bring_jar_tasker.py diff --git a/nodes/call.py b/rcprg_smach/nodes/call.py similarity index 100% rename from nodes/call.py rename to rcprg_smach/nodes/call.py diff --git a/nodes/guide_human_tasker.py b/rcprg_smach/nodes/guide_human_tasker.py similarity index 100% rename from nodes/guide_human_tasker.py rename to rcprg_smach/nodes/guide_human_tasker.py diff --git a/nodes/human_fell_approach_tasker.py b/rcprg_smach/nodes/human_fell_approach_tasker.py similarity index 100% rename from nodes/human_fell_approach_tasker.py rename to rcprg_smach/nodes/human_fell_approach_tasker.py diff --git a/nodes/human_fell_tasker.py b/rcprg_smach/nodes/human_fell_tasker.py similarity index 100% rename from nodes/human_fell_tasker.py rename to rcprg_smach/nodes/human_fell_tasker.py diff --git a/nodes/idle.py b/rcprg_smach/nodes/idle.py similarity index 100% rename from nodes/idle.py rename to rcprg_smach/nodes/idle.py diff --git a/nodes/idle_tasker.py b/rcprg_smach/nodes/idle_tasker.py similarity index 71% rename from nodes/idle_tasker.py rename to rcprg_smach/nodes/idle_tasker.py index 54c4b22..ebd18ba 100755 --- a/nodes/idle_tasker.py +++ b/rcprg_smach/nodes/idle_tasker.py @@ -29,13 +29,16 @@ global USE_SMACH_INRTOSPECTION_SERVER USE_SMACH_INRTOSPECTION_SERVER = False + class MainState(TaskER.SuspendableState): def __init__(self): - TaskER.SuspendableState.__init__(self, outcomes=['shutdown', 'PREEMPTED']) + TaskER.SuspendableState.__init__( + self, outcomes=['shutdown', 'PREEMPTED']) def transition_function(self, userdata): node = rclpy.create_node(self.__class__.__name__) - node.get_logger().info('{}: Executing state: {}'.format(node.get_name(), self.__class__.__name__)) + node.get_logger().info('{}: Executing state: {}'.format( + node.get_name(), self.__class__.__name__)) print('MainState.execute') # answer1_id = self.conversation_interface.setAutomaticAnswer( 'q_current_task', u'Nic nie robię' ) @@ -53,6 +56,7 @@ def transition_function(self, userdata): rclpy.spin_once(node, timeout_sec=0.2) raise Exception('Unreachable code') + def ptf_csp(ptf_id): global my_name node = rclpy.create_node('ptf_csp_node') @@ -63,7 +67,7 @@ def ptf_csp(ptf_id): if self_terminate_flag == True: flag = 'self-terminate' if ptf_id[0] == "scheduleParams": - return [flag, ScheduleParams(cost = cost, completion_time=1000,cost_per_sec=0,final_resource_state=RobotResource())] + return [flag, ScheduleParams(cost=cost, completion_time=1000, cost_per_sec=0, final_resource_state=RobotResource())] elif ptf_id[0] == "suspendCondition": req = SuspendConditionsRequest() req = ptf_id[1] @@ -73,6 +77,7 @@ def ptf_csp(ptf_id): req = ptf_id[1] return [flag, CostConditionsResponse(cost_per_sec=0, cost_to_complete=0)] + class MyTaskER(TaskER): def __init__(self, da_state_name, da_name): global USE_SMACH_INRTOSPECTION_SERVER @@ -84,11 +89,12 @@ def __init__(self, da_state_name, da_name): self.sim_mode = None self.conversation_interface = None self.kb_places = None - TaskER.__init__(self,da_state_name) + TaskER.__init__(self, da_state_name) if USE_SMACH_INRTOSPECTION_SERVER: - self.sis = smach_ros.IntrospectionServer(unicode(str("/"+self.name+"smach_view_server")), self, unicode(self.name)) + self.sis = smach_ros.IntrospectionServer( + unicode(str("/"+self.name+"smach_view_server")), self, unicode(self.name)) self.sis.start() - self.my_fsm = MainState() + self.my_fsm = MainState() def shutdownRequest(self): global USE_SMACH_INRTOSPECTION_SERVER @@ -101,17 +107,20 @@ def shutdownRequest(self): self.sis.clear() self.request_preempt() - def cleanup_tf(self): - rclpy.logging.get_logger(self.__class__.__name__).info(f'Executing state: {self.__class__.__name__}') - print('Cleanup.execute') - # self.conversation_interface.stop() - return 'ok' + def cleanup_tf(self): + rclpy.logging.get_logger(self.__class__.__name__).info( + f'Executing state: {self.__class__.__name__}') + print('Cleanup.execute') + # self.conversation_interface.stop() + return 'ok' + def get_suspension_tf(self, susp_data): print("My TASKER -- get_suspension_tf") print("My TASKER") print("My TASKER") - rclpy.logging.get_logger(self.__class__.__name__).info(f'Executing state: {self.__class__.__name__}') + rclpy.logging.get_logger(self.__class__.__name__).info( + f'Executing state: {self.__class__.__name__}') print('GetSuspend.execute') print(f"susp data: {susp_data.getData()}") print(f"susp data[0]: {susp_data.getData()[0]}") @@ -127,27 +136,32 @@ def get_suspension_tf(self, susp_data): if fsm_executable is None: print("harmoniser did not specify an executable for suspension behaviour, terminating task") fsm_executable = "terminate task" - + return fsm_executable + def exe_suspension_tf(self, fsm_es_in): print("My TASKER -- exe_suspension_tf") print("My TASKER") print("My TASKER") - rclpy.logging.get_logger(self.__class__.__name__).info(f'Executing state: {self.__class__.__name__}') + rclpy.logging.get_logger(self.__class__.__name__).info( + f'Executing state: {self.__class__.__name__}') print('ExecSuspension.execute') print(fsm_es_in) if fsm_es_in == "terminate task": pass # return 'shutdown' else: - p = subprocess.Popen(fsm_es_in, shell=True, stdout=subprocess.PIPE, stderr=subprocess.PIPE) + p = subprocess.Popen(fsm_es_in, shell=True, + stdout=subprocess.PIPE, stderr=subprocess.PIPE) p.wait() return "FINISHED" + def wait_tf(self): pass + def update_task_tf(self): global USE_SMACH_INRTOSPECTION_SERVER print("My TASKER -- update_task_tf") @@ -165,31 +179,40 @@ def update_task_tf(self): self.sis.start() pass + def initialise_tf(self): print("My TASKER -- initialise") # self.conversation_interface.start() + def main(): - da_name = None - da_type = None - da_id = None - da_state_name = [] - for idx in range(1, len(sys.argv), 2): - if sys.argv[idx] == 'da_name': - da_name = sys.argv[idx+1] - if sys.argv[idx] == 'da_type': - da_type = sys.argv[idx+1] - if sys.argv[idx] == 'da_id': - da_id = sys.argv[idx+1] - if da_name == None or da_type == None or da_id == None: - print "DA: one of the parameters (, , or ) is not specified" - return 1 - da = DynAgent( da_name, da_id, da_type, ptf_csp, da_state_name ) - print "RUN BRING" - da.run( MyTaskER(da_state_name,da_name) ) - print "BG ENDED" + # da_name = None + # da_type = None + # da_id = None + # da_state_name = [] + # for idx in range(1, len(sys.argv), 2): + # if sys.argv[idx] == 'da_name': + # da_name = sys.argv[idx+1] + # if sys.argv[idx] == 'da_type': + # da_type = sys.argv[idx+1] + # if sys.argv[idx] == 'da_id': + # da_id = sys.argv[idx+1] + # if da_name == None or da_type == None or da_id == None: + # print( + # "DA: one of the parameters (, , or ) is not specified") + # return 1 + + da_name = "test_name" + da_type = "test_type" + da_id = "1" + + da = DynAgent(da_name, da_id, da_type, ptf_csp, da_state_name) + print("RUN BRING") + da.run(MyTaskER(da_state_name, da_name)) + print("BG ENDED") return 0 + if __name__ == '__main__': main() - print "ALLLLLLL CLOOOOOSSSSEEEEED" + print("ALLLLLLL CLOOOOOSSSSEEEEED") diff --git a/nodes/move_to.py b/rcprg_smach/nodes/move_to.py similarity index 100% rename from nodes/move_to.py rename to rcprg_smach/nodes/move_to.py diff --git a/nodes/move_to_tasker.py b/rcprg_smach/nodes/move_to_tasker.py similarity index 100% rename from nodes/move_to_tasker.py rename to rcprg_smach/nodes/move_to_tasker.py diff --git a/nodes/stop.py b/rcprg_smach/nodes/stop.py similarity index 100% rename from nodes/stop.py rename to rcprg_smach/nodes/stop.py diff --git a/nodes/task_harmonizer.py b/rcprg_smach/nodes/task_harmonizer.py old mode 100644 new mode 100755 similarity index 99% rename from nodes/task_harmonizer.py rename to rcprg_smach/nodes/task_harmonizer.py index b09ad7c..cb90ee3 --- a/nodes/task_harmonizer.py +++ b/rcprg_smach/nodes/task_harmonizer.py @@ -6,7 +6,7 @@ import subprocess import threading -import rosgraph +# import rosgraph import rospy import actionlib from std_msgs.msg import String diff --git a/nodes/test_bring_goods.py b/rcprg_smach/nodes/test_bring_goods.py similarity index 100% rename from nodes/test_bring_goods.py rename to rcprg_smach/nodes/test_bring_goods.py diff --git a/nodes/test_move_to.py b/rcprg_smach/nodes/test_move_to.py similarity index 100% rename from nodes/test_move_to.py rename to rcprg_smach/nodes/test_move_to.py diff --git a/nodes/test_wander.py b/rcprg_smach/nodes/test_wander.py similarity index 100% rename from nodes/test_wander.py rename to rcprg_smach/nodes/test_wander.py diff --git a/nodes/wander.py b/rcprg_smach/nodes/wander.py similarity index 100% rename from nodes/wander.py rename to rcprg_smach/nodes/wander.py diff --git a/rcprg_smach/ros_node_utils.py b/rcprg_smach/ros_node_utils.py new file mode 100755 index 0000000..48ac87a --- /dev/null +++ b/rcprg_smach/ros_node_utils.py @@ -0,0 +1,67 @@ +#!/usr/bin/env python3 +# encoding: utf8 + +# import rclpy +# from rclpy.node import Node +# from std_msgs.msg import String + +# class NodeNamesGetter(Node): +# def __init__(self): +# super().__init__('node_names_getter') + +# def get_node_names(self, namespace=None): +# """ +# Get names of active nodes in the ROS2 graph. +# @param namespace: optional namespace to scope return values by. Namespace must already be resolved. +# @type namespace: str +# @return: list of node names +# @rtype: [str] +# """ +# node_names = self.get_node_names_and_namespaces() +# if namespace: +# namespace = namespace if namespace.startswith('/') else '/' + namespace +# filtered_names = [name for name, ns in node_names if ns.startswith(namespace)] +# else: +# filtered_names = [name for name, ns in node_names] +# return list(set(filtered_names)) + +# def main(args=None): +# rclpy.init(args=args) +# node_names_getter = NodeNamesGetter() +# try: +# namespace = None # Set your namespace here if needed +# node_names = node_names_getter.get_node_names(namespace) +# print("Active nodes:", node_names) +# finally: +# node_names_getter.destroy_node() +# rclpy.shutdown() + +# if __name__ == '__main__': +# main() + +import rclpy +from rclpy.node import Node + +class ROSNodeUtils(Node): + def __init__(self): + super().__init__('ros_node_utils') + + def get_node_names(self, namespace=None): + node_names = self.get_node_names_and_namespaces() + if namespace: + namespace = namespace if namespace.startswith('/') else '/' + namespace + filtered_names = [name for name, ns in node_names if ns.startswith(namespace)] + else: + filtered_names = [name for name, ns in node_names] + return list(set(filtered_names)) + +# Outside of the class, you can define a function to be used directly +def get_node_names(namespace=None): + rclpy.init() + ros_node_utils = ROSNodeUtils() + try: + names = ros_node_utils.get_node_names(namespace) + finally: + ros_node_utils.destroy_node() + rclpy.shutdown() + return names diff --git a/src/rcprg_smach/smach_rcprg.py b/rcprg_smach/smach_rcprg.py old mode 100644 new mode 100755 similarity index 100% rename from src/rcprg_smach/smach_rcprg.py rename to rcprg_smach/smach_rcprg.py diff --git a/src/rcprg_smach/suspend_bj.py b/rcprg_smach/suspend_bj.py old mode 100644 new mode 100755 similarity index 100% rename from src/rcprg_smach/suspend_bj.py rename to rcprg_smach/suspend_bj.py diff --git a/src/rcprg_smach/suspend_gh.py b/rcprg_smach/suspend_gh.py old mode 100644 new mode 100755 similarity index 100% rename from src/rcprg_smach/suspend_gh.py rename to rcprg_smach/suspend_gh.py diff --git a/src/rcprg_smach/task_manager.py b/rcprg_smach/task_manager.py similarity index 100% rename from src/rcprg_smach/task_manager.py rename to rcprg_smach/task_manager.py diff --git a/src/rcprg_smach/update_pose.py b/rcprg_smach/update_pose.py old mode 100644 new mode 100755 similarity index 100% rename from src/rcprg_smach/update_pose.py rename to rcprg_smach/update_pose.py diff --git a/resource/rcprg_smach b/resource/rcprg_smach new file mode 100644 index 0000000..e69de29 diff --git a/setup.cfg b/setup.cfg new file mode 100644 index 0000000..085a2a2 --- /dev/null +++ b/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/rcprg_smach +[install] +install_scripts=$base/lib/rcprg_smach \ No newline at end of file diff --git a/setup.py b/setup.py index 88caae5..1b72efe 100644 --- a/setup.py +++ b/setup.py @@ -1,11 +1,25 @@ -#!/usr/bin/env python3 +from setuptools import find_packages, setup -from distutils.core import setup -from catkin_pkg.python_setup import generate_distutils_setup +package_name = 'rcprg_smach' -setup_args = generate_distutils_setup( - packages=['rcprg_smach'], - package_dir={'': 'src'} -) +setup( + name=package_name, + version='0.0.0', + packages=find_packages(exclude=['test']), + data_files=[ + ('share/ament_index/resource_index/packages', + ['resource/' + package_name]), + ('share/' + package_name, ['package.xml']), + ], + install_requires=['setuptools'], + zip_safe=True, + maintainer='adam', + maintainer_email='adam.krawczyk@robotec.ai', + description='TODO: Package description', + license='TODO: License declaration', + tests_require=['pytest'], + entry_points={'console_scripts': [ 'bring_goods_tasker = rcprg_smach.rcprg_smach.nodes.bring_goods_tasker:main', + 'bring_goods = rcprg_smach.rcprg_smach.nodes.bring_goods:main', 'bring_jar_tasker = rcprg_smach.rcprg_smach.nodes.bring_jar_tasker:main', 'bring_jar = rcprg_smach.rcprg_smach.nodes.bring_jar:main', 'call = rcprg_smach.rcprg_smach.nodes.call:main', 'guide_human_tasker = rcprg_smach.rcprg_smach.nodes.guide_human_tasker:main', 'human_fell_approach_tasker = rcprg_smach.rcprg_smach.nodes.human_fell_approach_tasker:main', 'human_fell_approach = rcprg_smach.rcprg_smach.nodes.human_fell_approach:main', 'idle_tasker = rcprg_smach.rcprg_smach.nodes.idle_tasker:main', 'idle = rcprg_smach.rcprg_smach.nodes.idle:main', 'move_to_tasker = rcprg_smach.rcprg_smach.nodes.move_to_tasker.main', 'move_to = rcprg_smach.rcprg_smach.nodes.idle:main', 'stop = rcprg_smach.rcprg_smach.nodes.stop:main', 'task_harmonizer = rcprg_smach.rcprg_smach.nodes.task_harmonizer:main', 'test_bring_goods = rcprg_smach.rcprg_smach.nodes.test_bring_goods:main', 'test_move_to = rcprg_smach.rcprg_smach.nodes.test_move_to:main', 'test_wander = rcprg_smach.rcprg_smach.nodes.test_wander:main', 'wander = rcprg_smach.rcprg_smach.nodes.wander:main', 'nav2_statuses = rcprg_smach.nodes.nav2_statuses:main' + ]}, -setup(**setup_args) +) diff --git a/src/rcprg_smach/ros_node_utils.py b/src/rcprg_smach/ros_node_utils.py deleted file mode 100755 index 6fddfe1..0000000 --- a/src/rcprg_smach/ros_node_utils.py +++ /dev/null @@ -1,37 +0,0 @@ -#!/usr/bin/env python3 -# encoding: utf8 - -import rospkg -import sys -import subprocess - -import rosgraph -import rospy -from std_msgs.msg import String - -ID = '/rosnode' -def get_node_names(namespace=None): - """ - @param namespace: optional namespace to scope return values by. Namespace must already be resolved. - @type namespace: str - @return: list of node caller IDs - @rtype: [str] - @raise ROSNodeIOException: if unable to communicate with master - """ - master = rosgraph.Master(ID) - try: - state = master.getSystemState() - except socket.error: - raise ROSNodeIOException("Unable to communicate with master!") - nodes = [] - if namespace: - # canonicalize namespace with leading/trailing slash - g_ns = rosgraph.names.make_global_ns(namespace) - for s in state: - for t, l in s: - nodes.extend([n for n in l if n.startswith(g_ns) or n == namespace]) - else: - for s in state: - for t, l in s: - nodes.extend(l) - return list(set(nodes)) diff --git a/test/test_copyright.py b/test/test_copyright.py new file mode 100644 index 0000000..97a3919 --- /dev/null +++ b/test/test_copyright.py @@ -0,0 +1,25 @@ +# Copyright 2015 Open Source Robotics Foundation, Inc. +# +# 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. + +from ament_copyright.main import main +import pytest + + +# Remove the `skip` decorator once the source file(s) have a copyright header +@pytest.mark.skip(reason='No copyright header has been placed in the generated source file.') +@pytest.mark.copyright +@pytest.mark.linter +def test_copyright(): + rc = main(argv=['.', 'test']) + assert rc == 0, 'Found errors' diff --git a/test/test_flake8.py b/test/test_flake8.py new file mode 100644 index 0000000..27ee107 --- /dev/null +++ b/test/test_flake8.py @@ -0,0 +1,25 @@ +# Copyright 2017 Open Source Robotics Foundation, Inc. +# +# 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. + +from ament_flake8.main import main_with_errors +import pytest + + +@pytest.mark.flake8 +@pytest.mark.linter +def test_flake8(): + rc, errors = main_with_errors(argv=[]) + assert rc == 0, \ + 'Found %d code style errors / warnings:\n' % len(errors) + \ + '\n'.join(errors) diff --git a/test/test_pep257.py b/test/test_pep257.py new file mode 100644 index 0000000..b234a38 --- /dev/null +++ b/test/test_pep257.py @@ -0,0 +1,23 @@ +# Copyright 2015 Open Source Robotics Foundation, Inc. +# +# 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. + +from ament_pep257.main import main +import pytest + + +@pytest.mark.linter +@pytest.mark.pep257 +def test_pep257(): + rc = main(argv=['.', 'test']) + assert rc == 0, 'Found code style errors / warnings'