From 3a61d7cda38a36f72b67de4de50e250c6181d03c Mon Sep 17 00:00:00 2001 From: Henrik Schnor Date: Fri, 16 Nov 2018 20:33:35 +0100 Subject: [PATCH 01/96] Implemented a ros wrapper prototype for rasa nlu --- .gitignore | 1 + mdr_speech/mdr_rasa/CMakeLists.txt | 44 +++++++++++++ mdr_speech/mdr_rasa/models/notes.txt | 13 ++++ .../models/restaurant_sample/config.yml | 3 + .../models/restaurant_sample/training_data.md | 63 +++++++++++++++++++ mdr_speech/mdr_rasa/package.xml | 28 +++++++++ .../mdr_rasa/ros/launch/rasa_nlu.launch | 4 ++ .../ros/launch/rasa_nlu_training.launch | 4 ++ mdr_speech/mdr_rasa/ros/scripts/rasa_nlu_node | 6 ++ .../ros/scripts/rasa_nlu_training_node | 6 ++ .../ros/src/mdr_rasa_nlu_trainer/__init__.py | 0 .../mdr_rasa_nlu_trainer/rasa_nlu_trainer.py | 29 +++++++++ .../ros/src/mdr_rasa_nlu_wrapper/__init__.py | 0 .../mdr_rasa_nlu_wrapper/rasa_nlu_wrapper.py | 42 +++++++++++++ mdr_speech/mdr_rasa/setup.py | 12 ++++ 15 files changed, 255 insertions(+) create mode 100644 mdr_speech/mdr_rasa/CMakeLists.txt create mode 100644 mdr_speech/mdr_rasa/models/notes.txt create mode 100644 mdr_speech/mdr_rasa/models/restaurant_sample/config.yml create mode 100644 mdr_speech/mdr_rasa/models/restaurant_sample/training_data.md create mode 100644 mdr_speech/mdr_rasa/package.xml create mode 100644 mdr_speech/mdr_rasa/ros/launch/rasa_nlu.launch create mode 100644 mdr_speech/mdr_rasa/ros/launch/rasa_nlu_training.launch create mode 100755 mdr_speech/mdr_rasa/ros/scripts/rasa_nlu_node create mode 100755 mdr_speech/mdr_rasa/ros/scripts/rasa_nlu_training_node create mode 100644 mdr_speech/mdr_rasa/ros/src/mdr_rasa_nlu_trainer/__init__.py create mode 100644 mdr_speech/mdr_rasa/ros/src/mdr_rasa_nlu_trainer/rasa_nlu_trainer.py create mode 100644 mdr_speech/mdr_rasa/ros/src/mdr_rasa_nlu_wrapper/__init__.py create mode 100644 mdr_speech/mdr_rasa/ros/src/mdr_rasa_nlu_wrapper/rasa_nlu_wrapper.py create mode 100644 mdr_speech/mdr_rasa/setup.py diff --git a/.gitignore b/.gitignore index 3c4cc6282..17c949d67 100644 --- a/.gitignore +++ b/.gitignore @@ -6,3 +6,4 @@ .idea/ *.*~ *.swp +/mdr_speech/mdr_rasa/models/*/generated/ diff --git a/mdr_speech/mdr_rasa/CMakeLists.txt b/mdr_speech/mdr_rasa/CMakeLists.txt new file mode 100644 index 000000000..2dcb61afd --- /dev/null +++ b/mdr_speech/mdr_rasa/CMakeLists.txt @@ -0,0 +1,44 @@ +cmake_minimum_required(VERSION 2.8.3) +project(mdr_rasa) + +find_package(catkin REQUIRED + COMPONENTS + rospy +) + +catkin_python_setup() + +################################### +## catkin specific configuration ## +################################### + +catkin_package( +# INCLUDE_DIRS include +# LIBRARIES mdr_question_answering +# CATKIN_DEPENDS other_catkin_pkg +# DEPENDS system_lib +) + +############# +## Install ## +############# + +## Mark executable scripts (Python etc.) for installation +## in contrast to setup.py, you can choose the destination +# install(PROGRAMS +# scripts/my_python_script +# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +# ) + +############# +## Testing ## +############# + +## Add gtest based cpp test target and link libraries +# catkin_add_gtest(${PROJECT_NAME}-test test/test_mdr_question_answering.cpp) +# if(TARGET ${PROJECT_NAME}-test) +# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) +# endif() + +## Add folders to be run by python nosetests +# catkin_add_nosetests(test) diff --git a/mdr_speech/mdr_rasa/models/notes.txt b/mdr_speech/mdr_rasa/models/notes.txt new file mode 100644 index 000000000..c95c78698 --- /dev/null +++ b/mdr_speech/mdr_rasa/models/notes.txt @@ -0,0 +1,13 @@ +Python dependencies: +sudo pip2 install sklearn_crfsuite spacy sklearn + +Afterwards run: +sudo python -m spacy download en + +Further dependencies may be needed: +wget https://raw.githubusercontent.com/RasaHQ/starter-pack-rasa-nlu/master/requirements.txt +sudo pip2 install -r requirements.txt + +If you have a really old laptop (like I do) you might need to uninstall tensorflow. +It crashes rasa if required CPU instruction sets are missing and isn't needed +for training with spacy anyway. diff --git a/mdr_speech/mdr_rasa/models/restaurant_sample/config.yml b/mdr_speech/mdr_rasa/models/restaurant_sample/config.yml new file mode 100644 index 000000000..04134c2df --- /dev/null +++ b/mdr_speech/mdr_rasa/models/restaurant_sample/config.yml @@ -0,0 +1,3 @@ +language: "en" + +pipeline: "spacy_sklearn" diff --git a/mdr_speech/mdr_rasa/models/restaurant_sample/training_data.md b/mdr_speech/mdr_rasa/models/restaurant_sample/training_data.md new file mode 100644 index 000000000..6223dce07 --- /dev/null +++ b/mdr_speech/mdr_rasa/models/restaurant_sample/training_data.md @@ -0,0 +1,63 @@ +## intent:affirm +- yes +- yep +- yeah +- indeed +- that's right +- ok +- great +- right, thank you +- correct +- great choice +- sounds really good + +## intent:goodbye +- bye +- goodbye +- good bye +- stop +- end +- farewell +- Bye bye +- have a good one + +## intent:greet +- hey +- howdy +- hey there +- hello +- hi +- good morning +- good evening +- dear sir + +## intent:restaurant_search +- i'm looking for a place to eat +- I want to grab lunch +- I am searching for a dinner spot +- i'm looking for a place in the [north](location) of town +- show me [chinese](cuisine) restaurants +- show me [chines](cuisine:chinese) restaurants in the [north](location) +- show me a [mexican](cuisine) place in the [centre](location) +- i am looking for an [indian](cuisine) spot called olaolaolaolaolaola +- search for restaurants +- anywhere in the [west](location) +- anywhere near [18328](location) +- I am looking for [asian fusion](cuisine) food +- I am looking a restaurant in [29432](location) +- I am looking for [mexican indian fusion](cuisine) +- [central](location) [indian](cuisine) restaurant + +## synonym:chinese ++ Chines +* Chinese + +## synonym:vegetarian +- vegg +- veggie + +## regex:zipcode +- [0-9]{5} + +## regex:greet +- hey[^\s]* \ No newline at end of file diff --git a/mdr_speech/mdr_rasa/package.xml b/mdr_speech/mdr_rasa/package.xml new file mode 100644 index 000000000..af63efe74 --- /dev/null +++ b/mdr_speech/mdr_rasa/package.xml @@ -0,0 +1,28 @@ + + + mdr_rasa + 0.0.1 + A wrapper for the rasa framework + + MAS robotics + + Henrik Schnor + + GPLv3 + + + + + + + + + + + + + catkin + + rospy + + diff --git a/mdr_speech/mdr_rasa/ros/launch/rasa_nlu.launch b/mdr_speech/mdr_rasa/ros/launch/rasa_nlu.launch new file mode 100644 index 000000000..7e8d3d3dd --- /dev/null +++ b/mdr_speech/mdr_rasa/ros/launch/rasa_nlu.launch @@ -0,0 +1,4 @@ + + + + diff --git a/mdr_speech/mdr_rasa/ros/launch/rasa_nlu_training.launch b/mdr_speech/mdr_rasa/ros/launch/rasa_nlu_training.launch new file mode 100644 index 000000000..b8d57b111 --- /dev/null +++ b/mdr_speech/mdr_rasa/ros/launch/rasa_nlu_training.launch @@ -0,0 +1,4 @@ + + + + diff --git a/mdr_speech/mdr_rasa/ros/scripts/rasa_nlu_node b/mdr_speech/mdr_rasa/ros/scripts/rasa_nlu_node new file mode 100755 index 000000000..afbeaa626 --- /dev/null +++ b/mdr_speech/mdr_rasa/ros/scripts/rasa_nlu_node @@ -0,0 +1,6 @@ +#!/usr/bin/env python + +from mdr_rasa_nlu_wrapper.rasa_nlu_wrapper import RasaNluWrapper + +if __name__ == '__main__': + RasaNluWrapper().run() diff --git a/mdr_speech/mdr_rasa/ros/scripts/rasa_nlu_training_node b/mdr_speech/mdr_rasa/ros/scripts/rasa_nlu_training_node new file mode 100755 index 000000000..2e1bb6ce3 --- /dev/null +++ b/mdr_speech/mdr_rasa/ros/scripts/rasa_nlu_training_node @@ -0,0 +1,6 @@ +#!/usr/bin/env python + +from mdr_rasa_nlu_trainer.rasa_nlu_trainer import RasaNluTrainer + +if __name__ == '__main__': + RasaNluTrainer().run() diff --git a/mdr_speech/mdr_rasa/ros/src/mdr_rasa_nlu_trainer/__init__.py b/mdr_speech/mdr_rasa/ros/src/mdr_rasa_nlu_trainer/__init__.py new file mode 100644 index 000000000..e69de29bb diff --git a/mdr_speech/mdr_rasa/ros/src/mdr_rasa_nlu_trainer/rasa_nlu_trainer.py b/mdr_speech/mdr_rasa/ros/src/mdr_rasa_nlu_trainer/rasa_nlu_trainer.py new file mode 100644 index 000000000..a237b0587 --- /dev/null +++ b/mdr_speech/mdr_rasa/ros/src/mdr_rasa_nlu_trainer/rasa_nlu_trainer.py @@ -0,0 +1,29 @@ +#!/usr/bin/env python + +import rospy +import os +from rasa_nlu.training_data import load_data +from rasa_nlu.model import Trainer +from rasa_nlu import config + + +class RasaNluTrainer(object): + def __init__(self): + # Setup rospy and get config + rospy.init_node('rasa_nlu_wrapper') + self.model_name = rospy.get_param('~rasa_nlu_model', 'restaurant_sample') + + # Build paths to rasa models + self.base_path = os.path.abspath(os.path.join(os.path.dirname( __file__ ), '../../../models', self.model_name)) + self.training_filepath = os.path.join(self.base_path, 'training_data.md') + self.config_filepath = os.path.join(self.base_path, 'config.yml') + + def run(self): + rospy.loginfo('Training model {}...'.format(self.model_name)) + + training_data = load_data(self.training_filepath) + trainer = Trainer(config.load(self.config_filepath)) + trainer.train(training_data) + model_directory = trainer.persist(self.base_path, project_name='generated', fixed_model_name='model') + + rospy.loginfo('Training done! Model directory: {}'.format(model_directory)) diff --git a/mdr_speech/mdr_rasa/ros/src/mdr_rasa_nlu_wrapper/__init__.py b/mdr_speech/mdr_rasa/ros/src/mdr_rasa_nlu_wrapper/__init__.py new file mode 100644 index 000000000..e69de29bb diff --git a/mdr_speech/mdr_rasa/ros/src/mdr_rasa_nlu_wrapper/rasa_nlu_wrapper.py b/mdr_speech/mdr_rasa/ros/src/mdr_rasa_nlu_wrapper/rasa_nlu_wrapper.py new file mode 100644 index 000000000..5de13219c --- /dev/null +++ b/mdr_speech/mdr_rasa/ros/src/mdr_rasa_nlu_wrapper/rasa_nlu_wrapper.py @@ -0,0 +1,42 @@ +#!/usr/bin/env python + +import rospy +import os +import json +from std_msgs.msg import String +from rasa_nlu.model import Interpreter + + +class RasaNluWrapper(object): + + def __init__(self): + # Setup rospy and get config + rospy.init_node("rasa_nlu_wrapper") + self.model_name = rospy.get_param('~rasa_nlu_model', 'restaurant_sample') + self.topic_input_text = rospy.get_param('~rasa_nlu_input_topic', '/rasa_nlu_input') + self.topic_output = rospy.get_param('~rasa_nlu_output_topic', '/rasa_nlu_output') + + # Build paths to rasa models + self.base_path = os.path.abspath(os.path.join(os.path.dirname( __file__ ), '../../../models', self.model_name)) + self.model_path = os.path.join(self.base_path, 'generated', 'model') + + def run(self): + # Setup interpreter + self.interpreter = Interpreter.load(self.model_path) + + # Setup topics for incoming/outgoing messages + self.sub = rospy.Subscriber(self.topic_input_text, String, self.process_msg) + self.pub = rospy.Publisher(self.topic_output, String, latch=True, queue_size=1) + + # Wait until killed + rospy.spin() + + def process_msg(self, data): + # Run message through rasa and get returned json + result = json.dumps(self.interpreter.parse(unicode(data.data, 'utf-8'))) + rospy.loginfo("rasa parsed message: {}".format(result)) + + # Publish result + response = String() + response.data = result + self.pub.publish(response) diff --git a/mdr_speech/mdr_rasa/setup.py b/mdr_speech/mdr_rasa/setup.py new file mode 100644 index 000000000..a6db8e482 --- /dev/null +++ b/mdr_speech/mdr_rasa/setup.py @@ -0,0 +1,12 @@ +#!/usr/bin/env python + +from distutils.core import setup +from catkin_pkg.python_setup import generate_distutils_setup + +d = generate_distutils_setup( + packages=['mdr_rasa_nlu_trainer', 'mdr_rasa_nlu_wrapper'], + package_dir={'mdr_rasa_nlu_trainer': 'ros/src/mdr_rasa_nlu_trainer', + 'mdr_rasa_nlu_wrapper': 'ros/src/mdr_rasa_nlu_wrapper'} +) + +setup(**d) From 8d8f03dce6a409c469080d60b5722a92c1bbe604 Mon Sep 17 00:00:00 2001 From: Alex Mitrevski Date: Thu, 22 Nov 2018 17:41:30 +0100 Subject: [PATCH 02/96] [perceive_plane_client] Fixed an incorrect call to 'delete_named' of the message store --- .../ros/scripts/perceive_plane_client | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/mdr_planning/mdr_actions/mdr_perception_actions/mdr_perceive_plane_action/ros/scripts/perceive_plane_client b/mdr_planning/mdr_actions/mdr_perception_actions/mdr_perceive_plane_action/ros/scripts/perceive_plane_client index bf1ef3fac..61fac11b8 100755 --- a/mdr_planning/mdr_actions/mdr_perception_actions/mdr_perceive_plane_action/ros/scripts/perceive_plane_client +++ b/mdr_planning/mdr_actions/mdr_perception_actions/mdr_perceive_plane_action/ros/scripts/perceive_plane_client @@ -2,6 +2,7 @@ import rospy import actionlib +from mcr_perception_msgs.msg import Plane, Object from mdr_rosplan_interface.action_client_base import ActionClientBase from mdr_perceive_plane_action.msg import PerceivePlaneAction, PerceivePlaneGoal import rosplan_dispatch_msgs.msg as plan_dispatch_msgs @@ -136,12 +137,12 @@ class PerceivePlaneClient(ActionClientBase): ''' # we delete the plane from the knowledge base - self.msg_store_client.delete_named(plane_name) + self.msg_store_client.delete_named(plane_name, Plane._type) # we delete all objects on the plane from the knowledge base plane_objects = self.__get_plane_objects(plane_name) for obj in plane_objects: - self.msg_store_client.delete_named(obj) + self.msg_store_client.delete_named(obj, Object._type) def __get_plane_objects(self, plane_name): '''Returns a list with the names of all objects on the plane with the given name From 6b44a88377845e4de82a9a7df45b8dbbe05b8631 Mon Sep 17 00:00:00 2001 From: Alex Mitrevski Date: Thu, 22 Nov 2018 18:13:17 +0100 Subject: [PATCH 03/96] [rosplan_interface] Added a script for clearing the message store on startup --- .../ros/launch/clear_message_store.launch | 12 +++++++++ .../ros/launch/rosplan.launch | 7 +++++ .../ros/scripts/clear_message_store | 27 +++++++++++++++++++ 3 files changed, 46 insertions(+) create mode 100644 mdr_planning/mdr_rosplan_interface/ros/launch/clear_message_store.launch create mode 100755 mdr_planning/mdr_rosplan_interface/ros/scripts/clear_message_store diff --git a/mdr_planning/mdr_rosplan_interface/ros/launch/clear_message_store.launch b/mdr_planning/mdr_rosplan_interface/ros/launch/clear_message_store.launch new file mode 100644 index 000000000..ed0b9b88d --- /dev/null +++ b/mdr_planning/mdr_rosplan_interface/ros/launch/clear_message_store.launch @@ -0,0 +1,12 @@ + + + + + + + + + + + + diff --git a/mdr_planning/mdr_rosplan_interface/ros/launch/rosplan.launch b/mdr_planning/mdr_rosplan_interface/ros/launch/rosplan.launch index 557082229..5a2072ea6 100644 --- a/mdr_planning/mdr_rosplan_interface/ros/launch/rosplan.launch +++ b/mdr_planning/mdr_rosplan_interface/ros/launch/rosplan.launch @@ -40,4 +40,11 @@ + + + + + + + diff --git a/mdr_planning/mdr_rosplan_interface/ros/scripts/clear_message_store b/mdr_planning/mdr_rosplan_interface/ros/scripts/clear_message_store new file mode 100755 index 000000000..660724b9b --- /dev/null +++ b/mdr_planning/mdr_rosplan_interface/ros/scripts/clear_message_store @@ -0,0 +1,27 @@ +#!/usr/bin/env python + +import rospy +import pymongo as pm + +class ClearMessageStore(object): + def __init__(self): + self.db_name = rospy.get_param('~db_name', 'message_store') + self.collection_name = rospy.get_param('~collection_name', 'message_store') + self.db_port = rospy.get_param('~db_port', 27018) + self.__clear_message_store() + + def __clear_message_store(self): + try: + client = pm.MongoClient(port=self.db_port) + database = client[self.db_name] + collection = database[self.collection_name] + collection.delete_many({}) + except Exception as exc: + rospy.logerr(str(exc)) + +if __name__ == '__main__': + rospy.init_node('clear_message_store') + try: + ClearMessageStore() + except rospy.ROSInterruptException: + pass From ce78732c5613a0713d9a651777479e9ef3d24960 Mon Sep 17 00:00:00 2001 From: Alex Mitrevski Date: Fri, 23 Nov 2018 11:26:27 +0100 Subject: [PATCH 04/96] [perceive_plane_client] Objects are now also deleted from the rosplan KB before new knowledge is added there --- .../ros/scripts/perceive_plane_client | 34 +++++++++++++++++-- 1 file changed, 31 insertions(+), 3 deletions(-) diff --git a/mdr_planning/mdr_actions/mdr_perception_actions/mdr_perceive_plane_action/ros/scripts/perceive_plane_client b/mdr_planning/mdr_actions/mdr_perception_actions/mdr_perceive_plane_action/ros/scripts/perceive_plane_client index 61fac11b8..07d5f6fb5 100755 --- a/mdr_planning/mdr_actions/mdr_perception_actions/mdr_perceive_plane_action/ros/scripts/perceive_plane_client +++ b/mdr_planning/mdr_actions/mdr_perception_actions/mdr_perceive_plane_action/ros/scripts/perceive_plane_client @@ -130,19 +130,47 @@ class PerceivePlaneClient(ActionClientBase): self.msg_store_client.insert_named(obj_name, obj) def __delete_plane_from_kb(self, plane_name): - '''Removes the plane with the given name and all objects on it from the message store + '''Removes the plane with the given name and all objects on it from the + knowledge base (both the ROSPlan knowledge base and the message store) Keyword arguments: plane_name -- string indicating the name of a plane ''' + request = rosplan_srvs.KnowledgeUpdateServiceRequest() + request.update_type = 2 + request.knowledge.knowledge_type = 1 + request.knowledge.attribute_name = 'explored' + + arg_msg = diag_msgs.KeyValue() + arg_msg.key = 'plane' + arg_msg.value = plane_name + request.knowledge.values.append(arg_msg) + # we delete the plane from the knowledge base + self.knowledge_update_client(request) self.msg_store_client.delete_named(plane_name, Plane._type) # we delete all objects on the plane from the knowledge base plane_objects = self.__get_plane_objects(plane_name) - for obj in plane_objects: - self.msg_store_client.delete_named(obj, Object._type) + for obj_name in plane_objects: + request = rosplan_srvs.KnowledgeUpdateServiceRequest() + request.update_type = 2 + request.knowledge.knowledge_type = 1 + request.knowledge.attribute_name = 'on' + + arg_msg = diag_msgs.KeyValue() + arg_msg.key = 'obj' + arg_msg.value = obj_name + request.knowledge.values.append(arg_msg) + + arg_msg = diag_msgs.KeyValue() + arg_msg.key = 'plane' + arg_msg.value = plane_name + request.knowledge.values.append(arg_msg) + + self.knowledge_update_client(request) + self.msg_store_client.delete_named(obj_name, Object._type) def __get_plane_objects(self, plane_name): '''Returns a list with the names of all objects on the plane with the given name From 8e7cd490327609aa314469b626ec89f10b4b3341 Mon Sep 17 00:00:00 2001 From: PatrickNa Date: Wed, 28 Nov 2018 21:30:31 +0100 Subject: [PATCH 05/96] Remove outdated question matching approach. This commit removes the outdated question matching approach from 2016. Methods to compare input have been improved since then and can be found in mdr_speech_matching. Further also the content of the README.md file has been removed. Once the new speech recognition and speech processing is set, this needs to be updated. --- mdr_speech/README.md | 53 +-------- .../mdr_question_matching/CHANGELOG.rst | 37 ------ .../mdr_question_matching/CMakeLists.txt | 32 ----- mdr_speech/mdr_question_matching/package.xml | 24 ---- .../ros/config/go2018.txt | 22 ---- .../ros/config/questions.txt | 68 ----------- .../ros/launch/question_matching.launch | 7 -- .../ros/scripts/question_matcher | 7 -- .../ros/src/mdr_question_matching/__init__.py | 0 .../mdr_question_matching/question_matcher.py | 112 ------------------ mdr_speech/mdr_question_matching/setup.py | 11 -- 11 files changed, 2 insertions(+), 371 deletions(-) delete mode 100644 mdr_speech/mdr_question_matching/CHANGELOG.rst delete mode 100644 mdr_speech/mdr_question_matching/CMakeLists.txt delete mode 100644 mdr_speech/mdr_question_matching/package.xml delete mode 100644 mdr_speech/mdr_question_matching/ros/config/go2018.txt delete mode 100644 mdr_speech/mdr_question_matching/ros/config/questions.txt delete mode 100644 mdr_speech/mdr_question_matching/ros/launch/question_matching.launch delete mode 100755 mdr_speech/mdr_question_matching/ros/scripts/question_matcher delete mode 100644 mdr_speech/mdr_question_matching/ros/src/mdr_question_matching/__init__.py delete mode 100644 mdr_speech/mdr_question_matching/ros/src/mdr_question_matching/question_matcher.py delete mode 100644 mdr_speech/mdr_question_matching/setup.py diff --git a/mdr_speech/README.md b/mdr_speech/README.md index 30a0a4d8c..de2912ceb 100644 --- a/mdr_speech/README.md +++ b/mdr_speech/README.md @@ -1,53 +1,4 @@ -# mdr_speech +mdr_speech -This folder contains the speech recognition MS windows components -- SpeechRecognition is the SAPI 5.4 based API -- ros is a RosWindows wrapper for integration of the API -- test allows to run the recognition without ROS e.g. to test grammars +This folder contains speech recognition components. -- it uses the brsu_sppech_common folder for certain string operations and the config file reader - -### Build Requirements -- Windows 7 -- Visual Studio 2008 or greater (Not express edition since COM objects are used) -- ROSWindows 1.1.0 (http://www.servicerobotics.eu/index.php?id=37) - - -### Recompile ROS messages and Services -If you change the ROS Services or Messages, you have to recompile them in Linux since ROSWin has no facilities for doing that yet. The generated files can/must be used in windows -Linux: roscd brsu_srvs -Linux: Type "rosmake" -Linux: roscd brsu_msgs -Linux: Type "rosmake" -Linux: Commit in git -Windows: Pull in git - - -### Services -ChangeGrammar.srv in brsu_common/srvs -rosservice call /changeGrammar "grammarfile" ("Grammar file will be loaded relative to windows server /SpeechRecognition/grammars folder!") - - -### Topics -Publishes "RecognizedSpeech.msg" in brsu_common/msgs -Message contains: -- String: The full speech sentence that has been understood -- String: A keyword related to the sentence for the scheduler -- Int32: Confidence of the speech recognizer (1 = high, 0 = mid, -1 = low) -- String[]: all keywords identified in the last recognized speech - -### Change or Enhance Grammars -Grammars can be tested by dragging and dropping the grammar file over the gc.bat in the grammar folder -All variables used in the programm and grammars are lower case and with underscore -Grammars can be changed accorsign to the official SAPI grammars docu: http://msdn.microsoft.com/en-us/library/ee125672(v=VS.85).aspx - -ATTENTION: -To make it possible to reference other grammar files with the rule ref tag, a custom parser was developed (SpeechRecognition/src/GrammarParser.cpp) since the microsoft parser seems not to work -This parser loads a grammer, checks all ruleref tags for the URI and combines them into one single grammar file (tempGrammar.xml). This tempGrammar will exist after the -programm is excited and can be checked with the grammar compiler if any problems occured. -This has 2 implications: -- Only 1 rule ref per line is allowed -- the URL tag will be checked for files ONLY! - -## Contributors -- Mike - **Original author** diff --git a/mdr_speech/mdr_question_matching/CHANGELOG.rst b/mdr_speech/mdr_question_matching/CHANGELOG.rst deleted file mode 100644 index c3754b2cb..000000000 --- a/mdr_speech/mdr_question_matching/CHANGELOG.rst +++ /dev/null @@ -1,37 +0,0 @@ -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Changelog for package mdr_question_matching -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ - -1.2.0 (2017-11-04) ------------------- - -1.1.2 (2017-10-29) ------------------- -* Add missing scripts to installs in CMakeLists.txt -* Fix lint issues on mdr_question_matcher -* Contributors: Argentina Ortega Sainz - -1.1.1 (2017-09-20) ------------------- -* Change maintainer tags to MAS Robotics -* Generate changelogs for mdr_speech -* Contributors: Argentina Ortega Sainz - -1.1.0 (2017-09-18 18:15) ------------------------- - -1.0.1 (2017-09-18 18:04) ------------------------- -* Merge branch 'refactor/mdr_speech' into 'indigo' - Refactor/mdr speech - See merge request !30 -* Add mcr_question_matching from mas_common_robotics -* Contributors: Argentina Ortega Sainz, Alex Mitrevski - -1.0.0 (2017-04-11) ------------------- -* Added QAgrammar.txt -* Q&A are loaded from a txt file -* Fixed a problem with matching answers -* Add question matching package from Iryna -* Contributors: Iryna Ivanovska, Santosh Thoduka diff --git a/mdr_speech/mdr_question_matching/CMakeLists.txt b/mdr_speech/mdr_question_matching/CMakeLists.txt deleted file mode 100644 index fe203f1ce..000000000 --- a/mdr_speech/mdr_question_matching/CMakeLists.txt +++ /dev/null @@ -1,32 +0,0 @@ -cmake_minimum_required(VERSION 2.8.3) -project(mdr_question_matching) - -find_package(catkin REQUIRED - COMPONENTS - roslint - rospy -) - - - -catkin_python_setup() - -catkin_package( - CATKIN_DEPENDS - std_msgs -) - -roslint_python() - -### TESTS -if(CATKIN_ENABLE_TESTING) - find_package(roslaunch REQUIRED) - roslaunch_add_file_check(ros/launch) -endif() - - -### INSTALLS -install(PROGRAMS - ros/scripts/question_matcher - DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -) diff --git a/mdr_speech/mdr_question_matching/package.xml b/mdr_speech/mdr_question_matching/package.xml deleted file mode 100644 index fd3281b0e..000000000 --- a/mdr_speech/mdr_question_matching/package.xml +++ /dev/null @@ -1,24 +0,0 @@ - - - mdr_question_matching - 1.2.0 - - Matches questions with answers - - - GPLv3 - - Iryna Ivanovska - - MAS robotics - - catkin - - rospy - roslint - - std_msgs - - roslaunch - - diff --git a/mdr_speech/mdr_question_matching/ros/config/go2018.txt b/mdr_speech/mdr_question_matching/ros/config/go2018.txt deleted file mode 100644 index 402571953..000000000 --- a/mdr_speech/mdr_question_matching/ros/config/go2018.txt +++ /dev/null @@ -1,22 +0,0 @@ -What is Robotics: The scientific study of robot. -Who is considered as the father of industrial robot: Joseph Engelberger. -Give one example for a computer programming language that can be used for robot programming:AML (A Manufacturing Language) -What is the major disadvantage of using a robot:Heavy investment. -Name the language used in Expert system:LISP and PROLOG. -Name one of the most important parts in Expert system:Knowledge Base. -Which system was designed for diagnosis and therapy recommendation for infectious disease:MYCIN. -What is the importance of AI:Human intelligence have certain limit in speed and accuracy. It is interrupted due to the lack of presence of mind, mood, etc. Bu AI systems have their own superb abilities. -Weaving The Web was written by:Tim Burners Lee -What is Beta Test:Trial test of a computer or software before the commercial launch -What is the extension of PDF:Portable document format -Expand RDBMS:Relational Data Base Management System -Difference engine was developed by:Charles Babbage -Orkut.com is now owned by:Google -Worlds first microprocessor is:Intel 4004 -What is SQL:Structured Query Language -What is the expansion of SMS:Short Message Service -Which IT company's nickname is The Big Blue:IBM -What is the full form of IEEE:Institute of Electric and Electronic Engineers -Email was developed by:Raymond Samuel Tomlinson Ray Tomlinson -Who is Netizen :Net Citizen Citizen who uses internet -What is Scareware:Fake antivirus softwares diff --git a/mdr_speech/mdr_question_matching/ros/config/questions.txt b/mdr_speech/mdr_question_matching/ros/config/questions.txt deleted file mode 100644 index 5b0581b92..000000000 --- a/mdr_speech/mdr_question_matching/ros/config/questions.txt +++ /dev/null @@ -1,68 +0,0 @@ -who are the inventors of the c programming language:inventors of the c programming language are ken thompson and dennis ritchie -who is the inventor of the python programming language:inventor of the python programming language is guido van rossum -which robot was the star in the movie wall-e:star in the movie wallee is wallee -which robot was the star in the movie wallee:star in the movie wallee is wallee -where does the term computer bug come from:from a moth trapped in a relay -what is the name of the round robot in the new star wars movie:round robot in the new star wars movie is b b 8 -how many curry sausages are eaten in germany each year:about 800 million currywurst every year -who is president of the galaxy in the hitchhikers guide to the galaxy:zaphod beeblebrox -who is president of the galaxy in the hitchhiker guide to the galaxy:zaphod beeblebrox -which robot is the love interest in wallee:eve -which robot is the love interest in wall-e:eve -which company makes asimo:asimo is made by honda -what company makes big dog:big dog is made by boston dynamics -what is the funny clumsy character of the star wars prequels:the funny clumsy character of the star wars prequels is jar jar binks -how many people live in the germany: a little over 80 million -what are the colours of the german flag: black red and yellow -what city is the capital of the germany:the capital of the germany is berlin -how many arms do you have:i have one arm -what is the heaviest element:plutonium when measured by the mass of the element but osmium is densest -what did alan turing create:many things like turing machines and the turing test -who is the helicopter pilot in the a-team:captain howling mad murdock -what apollo was the last to land on the moon:apollo 17 -who was the last man to step on the moon:the last man to step on the moon was gene cernan -in which county is the play of hamlet set:denmark -what are names of donald ducks nephews:huey dewey and louie duck -what are names of donald duck nephews:huey dewey and louie duck -how many metres are in a mile:about 1609 meters -name a dragon in the lord of the rings:name of the dragon in the lord of the rings is smaug -who is the chancellor of germany:angela merkel -who developed the first industrial robot:the american physicist joseph engelberg. he is also considered the father of robotics -what's the difference between a cyborg and an android:cyborgs are biological being with electromechanical enhancements. androids are human-shaped robots -what is the difference between a cyborg and an android:cyborgs are biological being with electromechanical enhancements. androids are human-shaped robots -do you know any cyborg:professor kevin warwick. he implanted a chip in in his left arm to remotely operate doors an artificial hand and an electronic wheelchair -in which city is this years robocup hosted:in leipzig germany -in which city is this year's robocup hosted:in leipzig germany -which city hosted last years robocup:hefei china -which city hosted last year's robocup:hefei china -in which city will next years robocup be hosted:nagoya in japan -in which city will next year's robocup be hosted:nagoya in japan -name the main rivers surrounding leipzig:the parthe pleisse and the white elster -where is the zoo of this city located:near the central station -where did the peaceful revolution of 1989 start:the peaceful revolution started in september 4 1989 in leipzig at the st. nicholas church -where is the world's oldest trade fair hosted:the worlds oldest trade fair is in leipzig -where is the worlds oldest trade fair hosted:the worlds oldest trade fair is in leipzig -where is one of the world's largest dark music festivals hosted:leipzig hosts one of the worlds largest dark music festivals -where is one of the worlds largest dark music festivals hosted:leipzig hosts one of the worlds largest dark music festivals -where is one of the world largest dark music festivals hosted:leipzig hosts one of the world largest dark music festivals -where is europes oldest continuous coffee shop hosted:europes oldest continuous coffee shop is in leipzig -where is europe's oldest continuous coffee shop hosted:europes oldest continuous coffee shop is in leipzig -where is europe oldest continuous coffee shop hosted:europe oldest continuous coffee shop is in leipzig -name one of the greatest german composers:johann sebastian bach -where is johann sebastian bach buried:johann sebastian bach is buried in st. thomas church here in leipzig -where is johann sebastian bach buried:johann sebastian bach is buried in sant thomas church here in leipzig -do you have dreams:i dream of electric sheeps -hey whats up:i dont know since ive never been there -hey whats up:i do not know since i have never been there -hey what up:i dont know since ive never been there -hey what's up:i dont know since ive never been there -there are seven days in a week true or false:true there are seven days in a week -there are eleven days in a week true or false:false there are seven days in a week not eleven -january has 31 days true or false:true january has 31 days -january has 28 days true or false:false january has 31 days not 28 -february has 28 days true or false:true but in leap-years has 29 -february has 31 days true or false:false february has either 28 or 29 days, depend on the year -what city are you from: i am from sankt augustin -what city are you from: im from sankt augustin -who used first the word robot:the word robot was first used by czech writer karel capek -what origin has the word robot:the czech word robota that means forced work or labour diff --git a/mdr_speech/mdr_question_matching/ros/launch/question_matching.launch b/mdr_speech/mdr_question_matching/ros/launch/question_matching.launch deleted file mode 100644 index a17a3e89d..000000000 --- a/mdr_speech/mdr_question_matching/ros/launch/question_matching.launch +++ /dev/null @@ -1,7 +0,0 @@ - - - - - - - diff --git a/mdr_speech/mdr_question_matching/ros/scripts/question_matcher b/mdr_speech/mdr_question_matching/ros/scripts/question_matcher deleted file mode 100755 index 4119ec922..000000000 --- a/mdr_speech/mdr_question_matching/ros/scripts/question_matcher +++ /dev/null @@ -1,7 +0,0 @@ -#!/usr/bin/env python - -import mdr_question_matching.question_matcher - - -if __name__ == '__main__': - mdr_question_matching.question_matcher.main() diff --git a/mdr_speech/mdr_question_matching/ros/src/mdr_question_matching/__init__.py b/mdr_speech/mdr_question_matching/ros/src/mdr_question_matching/__init__.py deleted file mode 100644 index e69de29bb..000000000 diff --git a/mdr_speech/mdr_question_matching/ros/src/mdr_question_matching/question_matcher.py b/mdr_speech/mdr_question_matching/ros/src/mdr_question_matching/question_matcher.py deleted file mode 100644 index a5144eb9b..000000000 --- a/mdr_speech/mdr_question_matching/ros/src/mdr_question_matching/question_matcher.py +++ /dev/null @@ -1,112 +0,0 @@ -#!/usr/bin/env python -import rospy -import std_msgs.msg - -__author__ = 'Iryna' - - -pub_answer = rospy.Publisher("question_matcher/answer", std_msgs.msg.String, - queue_size=1) - -''' -NOTE: Questions and answers are being loaded from a txt file. -RoboCup2016: There are 50 questions given. -Give an answer to 5 out of 50 in 5 min. -''' - - -def search_items(recognized_phrase, array): - x = [] - i = '' - for item in array: - if recognized_phrase.find(item) != -1: - i = item - x.append(item) - return x - - -def sentence_matching(recognized_phrase, questions): - - candidates = [] - numbers = [] - winner = "" - - recognized_phrase = str(recognized_phrase.lower()) - words = recognized_phrase.split() - - for question in questions: - - question = str(question.lower()) - number_question = len(question.split()) - - keys = search_items(question, words) - number_keys = len(keys) - - if number_keys == number_question: # total match - # print number_keys - winner = question - return winner - - elif number_keys != number_question: - numbers.append(number_keys) - candidates.append(question) - winner = "" - - # print numbers - - if winner == "": - if max(numbers) > 0: - winner = candidates[numbers.index(max(numbers))] - else: - winner = "" - - return winner - - -def question_answer_matching(recognized_phrase): - - txtfile = rospy.get_param("~questions_file") - - questions_answers = {} - questions = [] - - with open(txtfile) as f: - for line in f: - line = line.lower() - line = line.strip() - line = line.rstrip(',') - try: - (key, val) = line.split(":") - questions_answers[str(key)] = str(val) - if str(key) != '': - questions.append(str(key)) - except ValueError: - print('Ignoring: malformed line: "{}"'.format(line)) - - winner = sentence_matching(recognized_phrase, questions) - - if not questions_answers[winner]: - # print("Unknown question!") - answer = "Sorry, I wasn't able to recognize your question!" # random answer - - elif questions_answers[winner]: - # print(questions_answers[winner]) - answer = questions_answers[winner] - - else: - print("Something bad happened...") - answer = "Sorry, I wasn't able to recognize your question!" # random answer - - return answer - - -def recognized_text_callback(recognized_phrase): - answer = question_answer_matching(recognized_phrase.data) - pub_answer.publish(answer) - - -def main(): - rospy.init_node('question_matcher', anonymous=True) - sub = rospy.Subscriber('/recognized_speech', std_msgs.msg.String, - recognized_text_callback) - rospy.spin() diff --git a/mdr_speech/mdr_question_matching/setup.py b/mdr_speech/mdr_question_matching/setup.py deleted file mode 100644 index de2b39f92..000000000 --- a/mdr_speech/mdr_question_matching/setup.py +++ /dev/null @@ -1,11 +0,0 @@ -#!/usr/bin/env python - -from distutils.core import setup -from catkin_pkg.python_setup import generate_distutils_setup - -d = generate_distutils_setup( - packages=['mdr_question_matching'], - package_dir={'mdr_question_matching': 'ros/src/mdr_question_matching'} -) - -setup(**d) From 741d794940a894fd1f51faa2212bd1dec72b7fbc Mon Sep 17 00:00:00 2001 From: Alex Mitrevski Date: Sun, 2 Dec 2018 14:25:49 +0100 Subject: [PATCH 06/96] [pickup_action] Implemented a top grasp strategy (implementation still needs some polishing though) --- .../mdr_pickup_action/README.md | 5 ++ .../ros/action/Pickup.action | 4 + .../ros/scripts/pickup_action | 2 + .../src/mdr_pickup_action/action_states.py | 82 +++++++++++++++---- 4 files changed, 78 insertions(+), 15 deletions(-) diff --git a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_pickup_action/README.md b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_pickup_action/README.md index 1611dfa81..34e875a66 100644 --- a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_pickup_action/README.md +++ b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_pickup_action/README.md @@ -9,6 +9,11 @@ The action includes an action server as well as an action client that interacts ### Goal: * ``geometry_msgs/PoseStamped pose``: An end-effector goal pose +* ``uint32 strategy``: Grasping strategy + +The following constants are also defined in the action goal: +* ``uint32 SIDEWAYS_GRASP=0`` +* ``uint32 TOP_GRASP=1`` ### Result: diff --git a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_pickup_action/ros/action/Pickup.action b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_pickup_action/ros/action/Pickup.action index d6001c17a..2d4f8803d 100644 --- a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_pickup_action/ros/action/Pickup.action +++ b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_pickup_action/ros/action/Pickup.action @@ -1,5 +1,9 @@ # goal +uint32 SIDEWAYS_GRASP=0 +uint32 TOP_GRASP=1 + geometry_msgs/PoseStamped pose +uint32 strategy --- # result bool success diff --git a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_pickup_action/ros/scripts/pickup_action b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_pickup_action/ros/scripts/pickup_action index 78ffb221a..3e83d3d14 100755 --- a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_pickup_action/ros/scripts/pickup_action +++ b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_pickup_action/ros/scripts/pickup_action @@ -25,6 +25,7 @@ class PickupSkill(smach.StateMachine): grasp_offset = float(rospy.get_param('~intermediate_grasp_offset', -1.)) safe_arm_joint_config = rospy.get_param('~safe_arm_joint_config', 'folded') base_elbow_offset = float(rospy.get_param('~base_elbow_offset', -1.)) + arm_base_offset = float(rospy.get_param('~arm_base_offset', -1.)) grasping_orientation = rospy.get_param('~grasping_orientation', list()) grasping_dmp = rospy.get_param('~grasping_dmp', '') dmp_tau = float(rospy.get_param('~dmp_tau', 1.)) @@ -42,6 +43,7 @@ class PickupSkill(smach.StateMachine): intermediate_grasp_offset=grasp_offset, safe_arm_joint_config=safe_arm_joint_config, base_elbow_offset=base_elbow_offset, + arm_base_offset=arm_base_offset, grasping_orientation=grasping_orientation, grasping_dmp=grasping_dmp, dmp_tau=dmp_tau, diff --git a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_pickup_action/ros/src/mdr_pickup_action/action_states.py b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_pickup_action/ros/src/mdr_pickup_action/action_states.py index 195183dc9..e245b8075 100644 --- a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_pickup_action/ros/src/mdr_pickup_action/action_states.py +++ b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_pickup_action/ros/src/mdr_pickup_action/action_states.py @@ -1,5 +1,6 @@ #!/usr/bin/python from importlib import import_module +import numpy as np import rospy import smach @@ -7,6 +8,7 @@ import actionlib from geometry_msgs.msg import PoseStamped +from mdr_move_forward_action.msg import MoveForwardAction, MoveForwardGoal from mdr_move_base_action.msg import MoveBaseAction, MoveBaseGoal from mdr_move_arm_action.msg import MoveArmAction, MoveArmGoal from mdr_pickup_action.msg import PickupGoal, PickupFeedback, PickupResult @@ -32,11 +34,14 @@ class Pickup(smach.State): def __init__(self, timeout=120.0, gripper_controller_pkg_name='mdr_gripper_controller', pregrasp_config_name='pregrasp', + pregrasp_top_config_name='pregrasp_top', intermediate_grasp_offset=-1, safe_arm_joint_config='folded', move_arm_server='move_arm_server', move_base_server='move_base_server', + move_forward_server='move_forward_server', base_elbow_offset=-1., + arm_base_offset=-1., grasping_orientation=list(), grasping_dmp='', dmp_tau=1., @@ -52,11 +57,14 @@ def __init__(self, timeout=120.0, self.gripper = GripperControllerClass() self.pregrasp_config_name = pregrasp_config_name + self.pregrasp_top_config_name = pregrasp_top_config_name self.intermediate_grasp_offset = intermediate_grasp_offset self.safe_arm_joint_config = safe_arm_joint_config self.move_arm_server = move_arm_server self.move_base_server = move_base_server + self.move_forward_server = move_forward_server self.base_elbow_offset = base_elbow_offset + self.arm_base_offset = arm_base_offset self.grasping_orientation = grasping_orientation self.grasping_dmp = grasping_dmp self.dmp_tau = dmp_tau @@ -70,6 +78,9 @@ def __init__(self, timeout=120.0, self.move_base_client = actionlib.SimpleActionClient(self.move_base_server, MoveBaseAction) self.move_base_client.wait_for_server() + self.move_forward_client = actionlib.SimpleActionClient(self.move_forward_server, MoveForwardAction) + self.move_forward_client.wait_for_server() + def execute(self, userdata): feedback = PickupFeedback() feedback.current_state = 'PICKUP' @@ -105,29 +116,37 @@ def execute(self, userdata): rospy.loginfo('[PICKUP] Preparing for grasp verification') self.gripper.init_grasp_verification() - rospy.loginfo('[PICKUP] Moving to a pregrasp configuration...') - self.move_arm(MoveArmGoal.NAMED_TARGET, self.pregrasp_config_name) - - if self.intermediate_grasp_offset > 0: - rospy.loginfo('[PICKUP] Moving to intermediate grasping pose...') - pose_base_link.pose.position.x -= self.intermediate_grasp_offset - self.move_arm(MoveArmGoal.END_EFFECTOR_POSE, pose_base_link) - - rospy.loginfo('[PICKUP] Grasping...') - if self.intermediate_grasp_offset > 0: - pose_base_link.pose.position.x += self.intermediate_grasp_offset - arm_motion_success = self.move_arm(MoveArmGoal.END_EFFECTOR_POSE, pose_base_link) - if not arm_motion_success: - rospy.logerr('[PICKUP] Arm motion unsuccessful') + if userdata.pickup_goal.strategy == PickupGoal.SIDEWAYS_GRASP: + rospy.loginfo('[PICKUP] Preparing sideways graps') + pose_base_link = self.prepare_sideways_grasp(pose_base_link) + + rospy.loginfo('[PICKUP] Grasping...') + arm_motion_success = self.move_arm(MoveArmGoal.END_EFFECTOR_POSE, pose_base_link) + if not arm_motion_success: + rospy.logerr('[PICKUP] Arm motion unsuccessful') + return 'failed' + + rospy.loginfo('[PICKUP] Arm motion successful') + elif userdata.pickup_goal.strategy == PickupGoal.TOP_GRASP: + rospy.loginfo('[PICKUP] Preparing top grasp') + pose_base_link, x_align_distance = self.prepare_top_grasp(pose_base_link) + self.gripper.orient_z(pose_base_link.pose.orientation) + self.gripper.move_down(pose_base_link.pose.position.z) + else: + rospy.logerr('[PICKUP] Unknown grasping strategy requested; ignoring request') return 'failed' - rospy.loginfo('[PICKUP] Arm motion successful') rospy.loginfo('[PICKUP] Closing the gripper') self.gripper.close() rospy.loginfo('[PICKUP] Moving the arm back') self.move_arm(MoveArmGoal.NAMED_TARGET, self.safe_arm_joint_config) + if userdata.pickup_goal.strategy == PickupGoal.TOP_GRASP: + rospy.loginfo('[PICKUP] Moving the base back to the original position') + if abs(x_align_distance) > 0: + self.move_base_along_x(-x_align_distance) + rospy.loginfo('[PICKUP] Verifying the grasp...') grasp_successful = self.gripper.verify_grasp() if grasp_successful: @@ -191,6 +210,39 @@ def move_arm(self, goal_type, goal): result = self.move_arm_client.get_result() return result + def prepare_sideways_grasp(self, pose_base_link): + rospy.loginfo('[PICKUP] Moving to a pregrasp configuration...') + self.move_arm(MoveArmGoal.NAMED_TARGET, self.pregrasp_config_name) + + if self.intermediate_grasp_offset > 0: + rospy.loginfo('[PICKUP] Moving to intermediate grasping pose...') + pose_base_link.pose.position.x -= self.intermediate_grasp_offset + self.move_arm(MoveArmGoal.END_EFFECTOR_POSE, pose_base_link) + + if self.intermediate_grasp_offset > 0: + pose_base_link.pose.position.x += self.intermediate_grasp_offset + return pose_base_link + + def prepare_top_grasp(self, pose_base_link): + rospy.loginfo('[PICKUP] Moving to a pregrasp configuration...') + self.move_arm(MoveArmGoal.NAMED_TARGET, self.pregrasp_top_config_name) + x_align_distance = 0 + if self.arm_base_offset > 0: + x_align_distance = pose_base_link.pose.position.x - self.arm_base_offset + self.move_base_along_x(x_align_distance) + pose_base_link.pose.position.x = self.arm_base_offset + return pose_base_link, x_align_distance + + def move_base_along_x(self, distance_to_move): + movement_speed = np.sign(distance_to_move) * 0.1 # m/s + movement_duration = distance_to_move / movement_speed + move_forward_goal = MoveForwardGoal() + move_forward_goal.movement_duration = movement_duration + move_forward_goal.speed = movement_speed + self.move_forward_client.send_goal(move_forward_goal) + self.move_forward_client.wait_for_result() + self.move_forward_client.get_result() + class SetActionLibResult(smach.State): def __init__(self, result): smach.State.__init__(self, outcomes=['succeeded'], From 4a794dd971fcd83207430eba292772ce6fd6e6ff Mon Sep 17 00:00:00 2001 From: Henrik Schnor Date: Wed, 5 Dec 2018 12:35:01 +0100 Subject: [PATCH 07/96] Moved training to a ROS independent bash script --- mdr_speech/mdr_rasa/models/notes.txt | 13 --------- mdr_speech/mdr_rasa/models/training.sh | 13 +++++++++ .../ros/launch/rasa_nlu_training.launch | 4 --- .../ros/scripts/rasa_nlu_training_node | 6 ---- .../ros/src/mdr_rasa_nlu_trainer/__init__.py | 0 .../mdr_rasa_nlu_trainer/rasa_nlu_trainer.py | 29 ------------------- .../mdr_rasa_nlu_wrapper/rasa_nlu_wrapper.py | 4 +-- mdr_speech/mdr_rasa/setup.py | 5 ++-- 8 files changed, 17 insertions(+), 57 deletions(-) delete mode 100644 mdr_speech/mdr_rasa/models/notes.txt create mode 100755 mdr_speech/mdr_rasa/models/training.sh delete mode 100644 mdr_speech/mdr_rasa/ros/launch/rasa_nlu_training.launch delete mode 100755 mdr_speech/mdr_rasa/ros/scripts/rasa_nlu_training_node delete mode 100644 mdr_speech/mdr_rasa/ros/src/mdr_rasa_nlu_trainer/__init__.py delete mode 100644 mdr_speech/mdr_rasa/ros/src/mdr_rasa_nlu_trainer/rasa_nlu_trainer.py diff --git a/mdr_speech/mdr_rasa/models/notes.txt b/mdr_speech/mdr_rasa/models/notes.txt deleted file mode 100644 index c95c78698..000000000 --- a/mdr_speech/mdr_rasa/models/notes.txt +++ /dev/null @@ -1,13 +0,0 @@ -Python dependencies: -sudo pip2 install sklearn_crfsuite spacy sklearn - -Afterwards run: -sudo python -m spacy download en - -Further dependencies may be needed: -wget https://raw.githubusercontent.com/RasaHQ/starter-pack-rasa-nlu/master/requirements.txt -sudo pip2 install -r requirements.txt - -If you have a really old laptop (like I do) you might need to uninstall tensorflow. -It crashes rasa if required CPU instruction sets are missing and isn't needed -for training with spacy anyway. diff --git a/mdr_speech/mdr_rasa/models/training.sh b/mdr_speech/mdr_rasa/models/training.sh new file mode 100755 index 000000000..7bb536e0d --- /dev/null +++ b/mdr_speech/mdr_rasa/models/training.sh @@ -0,0 +1,13 @@ +#!/bin/bash + +# set CWD to the script's directory +cd "$(dirname "${BASH_SOURCE[0]}")" + +# check number of arguments +if [ "$#" -ne 1 ]; then + echo "Format: ./training.sh " + exit 1 +fi + +# train the model +python -m rasa_nlu.train -c "$1/config.yml" --data "$1/training_data.md" -o "$1" --project "generated" --fixed_model_name "model" --verbose diff --git a/mdr_speech/mdr_rasa/ros/launch/rasa_nlu_training.launch b/mdr_speech/mdr_rasa/ros/launch/rasa_nlu_training.launch deleted file mode 100644 index b8d57b111..000000000 --- a/mdr_speech/mdr_rasa/ros/launch/rasa_nlu_training.launch +++ /dev/null @@ -1,4 +0,0 @@ - - - - diff --git a/mdr_speech/mdr_rasa/ros/scripts/rasa_nlu_training_node b/mdr_speech/mdr_rasa/ros/scripts/rasa_nlu_training_node deleted file mode 100755 index 2e1bb6ce3..000000000 --- a/mdr_speech/mdr_rasa/ros/scripts/rasa_nlu_training_node +++ /dev/null @@ -1,6 +0,0 @@ -#!/usr/bin/env python - -from mdr_rasa_nlu_trainer.rasa_nlu_trainer import RasaNluTrainer - -if __name__ == '__main__': - RasaNluTrainer().run() diff --git a/mdr_speech/mdr_rasa/ros/src/mdr_rasa_nlu_trainer/__init__.py b/mdr_speech/mdr_rasa/ros/src/mdr_rasa_nlu_trainer/__init__.py deleted file mode 100644 index e69de29bb..000000000 diff --git a/mdr_speech/mdr_rasa/ros/src/mdr_rasa_nlu_trainer/rasa_nlu_trainer.py b/mdr_speech/mdr_rasa/ros/src/mdr_rasa_nlu_trainer/rasa_nlu_trainer.py deleted file mode 100644 index a237b0587..000000000 --- a/mdr_speech/mdr_rasa/ros/src/mdr_rasa_nlu_trainer/rasa_nlu_trainer.py +++ /dev/null @@ -1,29 +0,0 @@ -#!/usr/bin/env python - -import rospy -import os -from rasa_nlu.training_data import load_data -from rasa_nlu.model import Trainer -from rasa_nlu import config - - -class RasaNluTrainer(object): - def __init__(self): - # Setup rospy and get config - rospy.init_node('rasa_nlu_wrapper') - self.model_name = rospy.get_param('~rasa_nlu_model', 'restaurant_sample') - - # Build paths to rasa models - self.base_path = os.path.abspath(os.path.join(os.path.dirname( __file__ ), '../../../models', self.model_name)) - self.training_filepath = os.path.join(self.base_path, 'training_data.md') - self.config_filepath = os.path.join(self.base_path, 'config.yml') - - def run(self): - rospy.loginfo('Training model {}...'.format(self.model_name)) - - training_data = load_data(self.training_filepath) - trainer = Trainer(config.load(self.config_filepath)) - trainer.train(training_data) - model_directory = trainer.persist(self.base_path, project_name='generated', fixed_model_name='model') - - rospy.loginfo('Training done! Model directory: {}'.format(model_directory)) diff --git a/mdr_speech/mdr_rasa/ros/src/mdr_rasa_nlu_wrapper/rasa_nlu_wrapper.py b/mdr_speech/mdr_rasa/ros/src/mdr_rasa_nlu_wrapper/rasa_nlu_wrapper.py index 5de13219c..989781fa7 100644 --- a/mdr_speech/mdr_rasa/ros/src/mdr_rasa_nlu_wrapper/rasa_nlu_wrapper.py +++ b/mdr_speech/mdr_rasa/ros/src/mdr_rasa_nlu_wrapper/rasa_nlu_wrapper.py @@ -33,8 +33,8 @@ def run(self): def process_msg(self, data): # Run message through rasa and get returned json - result = json.dumps(self.interpreter.parse(unicode(data.data, 'utf-8'))) - rospy.loginfo("rasa parsed message: {}".format(result)) + result = json.dumps(self.interpreter.parse(unicode(data.data, 'utf-8')), indent=4, separators=(',', ': ')) + rospy.loginfo("rasa_nlu output: \n" + result) # Publish result response = String() diff --git a/mdr_speech/mdr_rasa/setup.py b/mdr_speech/mdr_rasa/setup.py index a6db8e482..5369241a2 100644 --- a/mdr_speech/mdr_rasa/setup.py +++ b/mdr_speech/mdr_rasa/setup.py @@ -4,9 +4,8 @@ from catkin_pkg.python_setup import generate_distutils_setup d = generate_distutils_setup( - packages=['mdr_rasa_nlu_trainer', 'mdr_rasa_nlu_wrapper'], - package_dir={'mdr_rasa_nlu_trainer': 'ros/src/mdr_rasa_nlu_trainer', - 'mdr_rasa_nlu_wrapper': 'ros/src/mdr_rasa_nlu_wrapper'} + packages=['mdr_rasa_nlu_wrapper'], + package_dir={'mdr_rasa_nlu_wrapper': 'ros/src/mdr_rasa_nlu_wrapper'} ) setup(**d) From 5456fa8d3145a4095f32720063d2365578af1b68 Mon Sep 17 00:00:00 2001 From: Henrik Schnor Date: Fri, 21 Dec 2018 14:46:46 +0100 Subject: [PATCH 08/96] Added cocktail party model and moved parsing to own function --- .../mdr_rasa/models/cocktail_party/config.yml | 3 + .../models/cocktail_party/training_data.md | 62 +++++++++++++++++++ .../mdr_rasa_nlu_wrapper/rasa_nlu_wrapper.py | 13 ++-- 3 files changed, 74 insertions(+), 4 deletions(-) create mode 100644 mdr_speech/mdr_rasa/models/cocktail_party/config.yml create mode 100644 mdr_speech/mdr_rasa/models/cocktail_party/training_data.md diff --git a/mdr_speech/mdr_rasa/models/cocktail_party/config.yml b/mdr_speech/mdr_rasa/models/cocktail_party/config.yml new file mode 100644 index 000000000..04134c2df --- /dev/null +++ b/mdr_speech/mdr_rasa/models/cocktail_party/config.yml @@ -0,0 +1,3 @@ +language: "en" + +pipeline: "spacy_sklearn" diff --git a/mdr_speech/mdr_rasa/models/cocktail_party/training_data.md b/mdr_speech/mdr_rasa/models/cocktail_party/training_data.md new file mode 100644 index 000000000..7809f2ab7 --- /dev/null +++ b/mdr_speech/mdr_rasa/models/cocktail_party/training_data.md @@ -0,0 +1,62 @@ +## intent:tell_name +- [Henrik](name) +- Hey, my name is [Alex](name) +- Hi, I'm [Erick](name) +- I'm [Barbara](name) +- You can call me [Patricia](name) + +## intent:tell_order +- [Beer](order) +- I'd like to have a [Coke](order) +- Please bring me some [Juice](order) +- A [Water](order) please +- Could you bring me some [Vodka](order)? +- Do you have an [Energy drink](order)? + +## intent:affirm +- yes +- yep +- yeah +- indeed +- sure +- correct +- true +- great +- ok +- that's correct +- that's right +- yes, correct +- right, thank you +- sounds really good + +## intent:deny +- no +- wrong +- false +- nope +- not at all +- sorry, you misunderstood that + +## lookup:names +- Henrik +- Patrick +- Erick +- Alex +- James +- John +- Robert +- Michael +- William +- Mary +- Patricia +- Linda +- Barbara +- Elizabeth + +## lookup:orders +- Coke +- Beer +- Energy drink +- Juice +- Water +- Vodka diff --git a/mdr_speech/mdr_rasa/ros/src/mdr_rasa_nlu_wrapper/rasa_nlu_wrapper.py b/mdr_speech/mdr_rasa/ros/src/mdr_rasa_nlu_wrapper/rasa_nlu_wrapper.py index 989781fa7..29bdea10e 100644 --- a/mdr_speech/mdr_rasa/ros/src/mdr_rasa_nlu_wrapper/rasa_nlu_wrapper.py +++ b/mdr_speech/mdr_rasa/ros/src/mdr_rasa_nlu_wrapper/rasa_nlu_wrapper.py @@ -31,12 +31,17 @@ def run(self): # Wait until killed rospy.spin() + def get_result(self, data): + # Parse message with rasa nlu + return self.interpreter.parse(unicode(data.data, 'utf-8')) + def process_msg(self, data): - # Run message through rasa and get returned json - result = json.dumps(self.interpreter.parse(unicode(data.data, 'utf-8')), indent=4, separators=(',', ': ')) - rospy.loginfo("rasa_nlu output: \n" + result) + # Process message and return result as json + result = self.get_result(data) + result_json = json.dumps(result, indent=4, separators=(',', ': ')) + rospy.loginfo("rasa_nlu output: \n" + result_json) # Publish result response = String() - response.data = result + response.data = result_json self.pub.publish(response) From d5fc1fbb8901637e81b869368caf3e61a7b1e870 Mon Sep 17 00:00:00 2001 From: erickkramer Date: Fri, 21 Dec 2018 17:37:04 +0100 Subject: [PATCH 09/96] Mbot interface implemented to process the interpreted output from mbot and call the correspondent action --- mdr_hri/mdr_mbot_interface/CMakeLists.txt | 13 ++ mdr_hri/mdr_mbot_interface/package.xml | 65 +++++++++ .../ros/launch/mbot_interface.launch | 18 +++ .../ros/scripts/mbot_interface | 9 ++ .../ros/src/mdr_mbot_interface/__init__.py | 0 .../src/mdr_mbot_interface/mbot_planner.py | 125 ++++++++++++++++++ mdr_hri/mdr_mbot_interface/setup.py | 12 ++ 7 files changed, 242 insertions(+) create mode 100644 mdr_hri/mdr_mbot_interface/CMakeLists.txt create mode 100644 mdr_hri/mdr_mbot_interface/package.xml create mode 100644 mdr_hri/mdr_mbot_interface/ros/launch/mbot_interface.launch create mode 100755 mdr_hri/mdr_mbot_interface/ros/scripts/mbot_interface create mode 100644 mdr_hri/mdr_mbot_interface/ros/src/mdr_mbot_interface/__init__.py create mode 100644 mdr_hri/mdr_mbot_interface/ros/src/mdr_mbot_interface/mbot_planner.py create mode 100644 mdr_hri/mdr_mbot_interface/setup.py diff --git a/mdr_hri/mdr_mbot_interface/CMakeLists.txt b/mdr_hri/mdr_mbot_interface/CMakeLists.txt new file mode 100644 index 000000000..5669c95bd --- /dev/null +++ b/mdr_hri/mdr_mbot_interface/CMakeLists.txt @@ -0,0 +1,13 @@ +cmake_minimum_required(VERSION 2.8.3) +project(mdr_mbot_interface) + +find_package(catkin REQUIRED COMPONENTS + rospy + std_msgs +) + +catkin_python_setup() + +catkin_package( + CATKIN_DEPENDS +) diff --git a/mdr_hri/mdr_mbot_interface/package.xml b/mdr_hri/mdr_mbot_interface/package.xml new file mode 100644 index 000000000..0028a09fc --- /dev/null +++ b/mdr_hri/mdr_mbot_interface/package.xml @@ -0,0 +1,65 @@ + + + mdr_mbot_interface + 0.0.0 + The mdr_mbot_interface package + + + + + lucy + + + + + + TODO + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + catkin + rospy + std_msgs + rospy + std_msgs + rospy + std_msgs + + + + + + + + diff --git a/mdr_hri/mdr_mbot_interface/ros/launch/mbot_interface.launch b/mdr_hri/mdr_mbot_interface/ros/launch/mbot_interface.launch new file mode 100644 index 000000000..2056e1d7a --- /dev/null +++ b/mdr_hri/mdr_mbot_interface/ros/launch/mbot_interface.launch @@ -0,0 +1,18 @@ + + + + + + + + + + + + + + + + + + diff --git a/mdr_hri/mdr_mbot_interface/ros/scripts/mbot_interface b/mdr_hri/mdr_mbot_interface/ros/scripts/mbot_interface new file mode 100755 index 000000000..426370b79 --- /dev/null +++ b/mdr_hri/mdr_mbot_interface/ros/scripts/mbot_interface @@ -0,0 +1,9 @@ +#!/usr/bin/env python + +import rospy +from mdr_mbot_interface.mbot_planner import MbotPlanner + +if __name__ == '__main__': + rospy.init_node('mbot_planner', anonymous=False) + planner = MbotPlanner() + planner.wait_for_interpretation() diff --git a/mdr_hri/mdr_mbot_interface/ros/src/mdr_mbot_interface/__init__.py b/mdr_hri/mdr_mbot_interface/ros/src/mdr_mbot_interface/__init__.py new file mode 100644 index 000000000..e69de29bb diff --git a/mdr_hri/mdr_mbot_interface/ros/src/mdr_mbot_interface/mbot_planner.py b/mdr_hri/mdr_mbot_interface/ros/src/mdr_mbot_interface/mbot_planner.py new file mode 100644 index 000000000..b1dcad6b8 --- /dev/null +++ b/mdr_hri/mdr_mbot_interface/ros/src/mdr_mbot_interface/mbot_planner.py @@ -0,0 +1,125 @@ +#!/usr/bin/env python + +import rospy +import copy +import rospkg +from std_msgs.msg import String +import diagnostic_msgs.msg as diag_msgs +import rosplan_dispatch_msgs.msg as plan_dispatch_msgs +from mbot_nlu.msg import Slot, ActionSlot, ActionSlotArray + +class MbotPlanner(object): + ''' + Planner class that publishes the corresponding action depending on the + received interpretation + ''' + def __init__(self): + self.robot_name = rospy.get_param('~robot_name', '') + self.action_completed = False + self.action_failed = False + + self.action_dispatch_pub = rospy.Publisher('/kcl_rosplan/action_dispatch', + plan_dispatch_msgs.ActionDispatch, + queue_size=1) + + # Subscribe to the topic publishing the interpretation of the ocmmand + rospy.Subscriber('~input_interpretation', + ActionSlotArray, + self.interpretationCallback) + rospy.Subscriber('/kcl_rosplan/action_feedback', + plan_dispatch_msgs.ActionFeedback, + self.action_feedback_cb) + + # to publish the recognition + # self.pub_sentence_recog = rospy.Publisher('~output_recognition', ActionSlotArray, queue_size=1) + # flag to indicate that a text was received and needs to be processed + self.interpretation_received = False + # to store the sentece that will be received in the callback + self.received_interpretation = None + # inform the user that the node has initialized + rospy.loginfo("Planner is ready to receive actions") + + def interpretationCallback(self, msg): + self.received_interpretation = msg + self.interpretation_received = True + + def process_interpretation(self): + ''' + Function that publish the right action depending on the interpretation received + ''' + for action in self.received_interpretation.sentence_recognition: + dispatch_msg = self.get_dispatch_msg(action) + rospy.loginfo('\033[1;36m[process_interpretation] Dispatching action {0}\033[0;37m'.format(dispatch_msg.name)) + self.action_dispatch_pub.publish(dispatch_msg) + while not self.action_completed and not self.action_failed: + rospy.sleep(0.1) + + if self.action_failed: + rospy.logerr('[process_interpretation] {0} could not be executed successfully; aborting plan'.format(action.intention)) + self.action_failed = False + break + + self.action_completed = False + rospy.loginfo('\033[1;36m[process_interpretation] Done dispatching actions\033[0;37m') + + def get_dispatch_msg(self, action): + dispatch_msg = plan_dispatch_msgs.ActionDispatch() + + arg_msg = diag_msgs.KeyValue() + arg_msg.key = 'bot' + arg_msg.value = self.robot_name + dispatch_msg.parameters.append(arg_msg) + + if action.intention == 'go': + dispatch_msg.name = 'move_base' + for slot in action.slots: + arg_msg = diag_msgs.KeyValue() + if slot.type == 'destination': + arg_msg.key = 'to' + arg_msg.value = slot.data + dispatch_msg.parameters.append(arg_msg) + elif action.intention == 'take': + for slot in action.slots: + arg_msg = diag_msgs.KeyValue() + if slot.type == 'object': + arg_msg.key = 'obj' + elif slot.type == 'source': + dispatch_msg.name = 'pick' + arg_msg.key = 'surface' + elif slot.type == 'destination': + dispatch_msg.name = 'place' + arg_msg.key = 'surface' + arg_msg.value = slot.data + dispatch_msg.parameters.append(arg_msg) + + return dispatch_msg + + def action_feedback_cb(self, msg): + if msg.status == 'action achieved': + self.action_completed = True + elif msg.status == 'action failed': + self.action_failed = True + + def wait_for_interpretation(self): + while not rospy.is_shutdown(): + if self.interpretation_received == True: + # lower flag + self.interpretation_received = False + + + # recognize intention + self.process_interpretation() + # action_slot_array_msg = ActionSlotArray() + # for phrase in recognized_sentence: + # # create empty msg + # action_slot_msg = ActionSlot() + # # fill msg + # action_slot_msg.intention = phrase[0] + # for slot in phrase[1]: + # action_slot_msg.slots.append( Slot(type=slot[0], data=slot[1]) ) + # # append each single action_slot to from the array + # action_slot_array_msg.sentence_recognition.append(copy.deepcopy(action_slot_msg)) + # # publish recognition + # self.pub_sentence_recog.publish(action_slot_array_msg) + # sleep to control the frequency of this node + rospy.sleep(0.1) diff --git a/mdr_hri/mdr_mbot_interface/setup.py b/mdr_hri/mdr_mbot_interface/setup.py new file mode 100644 index 000000000..564dbbdb6 --- /dev/null +++ b/mdr_hri/mdr_mbot_interface/setup.py @@ -0,0 +1,12 @@ +#!/usr/bin/env python + +from distutils.core import setup +from catkin_pkg.python_setup import generate_distutils_setup + +# for your packages to be recognized by python +d = generate_distutils_setup( + packages=['mdr_mbot_interface'], + package_dir={'mdr_mbot_interface': 'ros/src/mdr_mbot_interface'} +) + +setup(**d) From 3bca24abc01d2b1f0dd5efcf412acdc2fed00098 Mon Sep 17 00:00:00 2001 From: Alex Mitrevski Date: Fri, 17 Aug 2018 16:49:44 +0200 Subject: [PATCH 10/96] [knowledge_base] Started working on an ontology for domestic environments --- .../common/ontology/apartment.owl | 259 ++++++++++++++++++ 1 file changed, 259 insertions(+) create mode 100644 mdr_planning/mdr_knowledge_base/common/ontology/apartment.owl diff --git a/mdr_planning/mdr_knowledge_base/common/ontology/apartment.owl b/mdr_planning/mdr_knowledge_base/common/ontology/apartment.owl new file mode 100644 index 000000000..00b288976 --- /dev/null +++ b/mdr_planning/mdr_knowledge_base/common/ontology/apartment.owl @@ -0,0 +1,259 @@ + + + +]> + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + From 860c8dddcca53501f9fc7d7edcd024e3f3af4e6b Mon Sep 17 00:00:00 2001 From: Alex Mitrevski Date: Fri, 17 Aug 2018 16:58:06 +0200 Subject: [PATCH 11/96] [knowledge_base] Started working on a component that interacts with an OWL ontology --- .../mdr_knowledge_base/CMakeLists.txt | 10 ++++-- .../knowledge_base_manager.py | 36 +++++++++++++++++++ mdr_planning/mdr_knowledge_base/setup.py | 11 ++++++ 3 files changed, 54 insertions(+), 3 deletions(-) create mode 100644 mdr_planning/mdr_knowledge_base/ros/src/mdr_knowledge_base/knowledge_base_manager.py create mode 100644 mdr_planning/mdr_knowledge_base/setup.py diff --git a/mdr_planning/mdr_knowledge_base/CMakeLists.txt b/mdr_planning/mdr_knowledge_base/CMakeLists.txt index 314d0b4a0..a26f77080 100644 --- a/mdr_planning/mdr_knowledge_base/CMakeLists.txt +++ b/mdr_planning/mdr_knowledge_base/CMakeLists.txt @@ -7,10 +7,14 @@ find_package(catkin REQUIRED COMPONENTS roslint ) -catkin_package() +catkin_package( + CATKIN_DEPENDS + rospy +) + +catkin_python_setup() +roslint_python() include_directories( ${catkin_INCLUDE_DIRS} ) - -roslint_python() diff --git a/mdr_planning/mdr_knowledge_base/ros/src/mdr_knowledge_base/knowledge_base_manager.py b/mdr_planning/mdr_knowledge_base/ros/src/mdr_knowledge_base/knowledge_base_manager.py new file mode 100644 index 000000000..53875a6ab --- /dev/null +++ b/mdr_planning/mdr_knowledge_base/ros/src/mdr_knowledge_base/knowledge_base_manager.py @@ -0,0 +1,36 @@ +import rdflib + +class KnowledgeBaseManager(object): + '''Defines an interface for interacting with an OWL knowledge base + ''' + def __init__(self, ontology_file, class_prefix): + self.knowledge_graph = rdflib.Graph() + self.knowledge_graph.load(ontology_file) + self.class_prefix = class_prefix + + def is_instance_of(self, obj_name, class_name): + '''Checks whether 'obj_name' is an instance of 'class_name' + + Keyword arguments: + obj_name -- string representing the name of an object + class_name -- string representing the name of a class + + ''' + pass + + def get_instances_of(self, class_name): + '''Returns a list of names of all instances belonging to 'class_name' + + Keyword arguments: + class_name -- string representing the name of a class + + ''' + rdf_class = self.class_prefix + ':' + class_name + query_result = self.knowledge_graph.query('SELECT ?instance ' + + 'WHERE {?instance rdf:type ' + rdf_class + '}') + instances = list() + for row in query_result: + instance = row[0] + instance_name = instance[instance.rfind('/')+1:] + instances.append(instance_name) + return instances diff --git a/mdr_planning/mdr_knowledge_base/setup.py b/mdr_planning/mdr_knowledge_base/setup.py new file mode 100644 index 000000000..af34496c0 --- /dev/null +++ b/mdr_planning/mdr_knowledge_base/setup.py @@ -0,0 +1,11 @@ +#!/usr/bin/env python + +from distutils.core import setup +from catkin_pkg.python_setup import generate_distutils_setup + +d = generate_distutils_setup( + packages=['mdr_knowledge_base'], + package_dir={'mdr_knowledge_base': 'ros/src/mdr_knowledge_base'} +) + +setup(**d) From 51af9601fa26a148592e6e0b3ec662cd29601ac9 Mon Sep 17 00:00:00 2001 From: Alex Mitrevski Date: Fri, 17 Aug 2018 18:40:30 +0200 Subject: [PATCH 12/96] [knowledge_base_manager.py] Added methods for getting the parent and subclasses of a class --- .../knowledge_base_manager.py | 36 +++++++++++++++++++ 1 file changed, 36 insertions(+) diff --git a/mdr_planning/mdr_knowledge_base/ros/src/mdr_knowledge_base/knowledge_base_manager.py b/mdr_planning/mdr_knowledge_base/ros/src/mdr_knowledge_base/knowledge_base_manager.py index 53875a6ab..c47a1ef56 100644 --- a/mdr_planning/mdr_knowledge_base/ros/src/mdr_knowledge_base/knowledge_base_manager.py +++ b/mdr_planning/mdr_knowledge_base/ros/src/mdr_knowledge_base/knowledge_base_manager.py @@ -34,3 +34,39 @@ def get_instances_of(self, class_name): instance_name = instance[instance.rfind('/')+1:] instances.append(instance_name) return instances + + def get_subclasses_of(self, class_name): + '''Returns a list of all subclasses of 'class_name' + + Keyword arguments: + class_name -- string representing the name of a class + + ''' + rdf_class = self.class_prefix + ':' + class_name + rdf_class_uri = rdflib.URIRef(rdf_class) + query_result = self.knowledge_graph.transitive_subjects(rdflib.RDFS.subClassOf, + rdf_class_uri) + subclasses = list() + for subclass_uri in query_result: + subclass = str(subclass_uri) + subclass = subclass[subclass.find(':')+1:] + subclasses.append(subclass) + return subclasses + + def get_parent_classes_of(self, class_name): + '''Returns a list of all parent classes of 'class_name' + + Keyword arguments: + class_name -- string representing the name of a class + + ''' + rdf_class = self.class_prefix + ':' + class_name + rdf_class_uri = rdflib.URIRef(rdf_class) + query_result = self.knowledge_graph.transitive_objects(rdf_class_uri, + rdflib.RDFS.subClassOf) + parent_classes = list() + for class_uri in query_result: + parent_class = str(class_uri) + parent_class = parent_class[parent_class.find(':')+1:] + parent_classes.append(parent_class) + return parent_classes From 56e54e923d6828b265e8b1ca40764943ffafd13e Mon Sep 17 00:00:00 2001 From: Alex Mitrevski Date: Sun, 28 Oct 2018 08:55:23 +0100 Subject: [PATCH 13/96] [knowledge_base/ontology] Added some generic subclass definitions --- .../common/ontology/apartment.owl | 26 +++++++++++++++++++ 1 file changed, 26 insertions(+) diff --git a/mdr_planning/mdr_knowledge_base/common/ontology/apartment.owl b/mdr_planning/mdr_knowledge_base/common/ontology/apartment.owl index 00b288976..fd9de816f 100644 --- a/mdr_planning/mdr_knowledge_base/common/ontology/apartment.owl +++ b/mdr_planning/mdr_knowledge_base/common/ontology/apartment.owl @@ -80,11 +80,37 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + From 4a1f0b6616e48ca51a745fff96cc024568a4c4f1 Mon Sep 17 00:00:00 2001 From: Alex Mitrevski Date: Sun, 28 Oct 2018 08:56:05 +0100 Subject: [PATCH 14/96] [knowledge_base/ontology] Added a few simple geometric properties --- .../common/ontology/apartment.owl | 21 +++++++++++++++++++ 1 file changed, 21 insertions(+) diff --git a/mdr_planning/mdr_knowledge_base/common/ontology/apartment.owl b/mdr_planning/mdr_knowledge_base/common/ontology/apartment.owl index fd9de816f..bdb3edfd6 100644 --- a/mdr_planning/mdr_knowledge_base/common/ontology/apartment.owl +++ b/mdr_planning/mdr_knowledge_base/common/ontology/apartment.owl @@ -81,6 +81,27 @@ + + + + + + + + + + + + + + + + + + + + + From 6564581a2e6a06a57f27eaea338229dfd272dc1a Mon Sep 17 00:00:00 2001 From: Alex Mitrevski Date: Sun, 23 Dec 2018 18:37:47 +0100 Subject: [PATCH 15/96] [knowledge_base/ontology] Added a drawer class --- .../common/ontology/apartment.owl | 20 +++++++++++++++---- 1 file changed, 16 insertions(+), 4 deletions(-) diff --git a/mdr_planning/mdr_knowledge_base/common/ontology/apartment.owl b/mdr_planning/mdr_knowledge_base/common/ontology/apartment.owl index bdb3edfd6..230bcc0f7 100644 --- a/mdr_planning/mdr_knowledge_base/common/ontology/apartment.owl +++ b/mdr_planning/mdr_knowledge_base/common/ontology/apartment.owl @@ -15,12 +15,14 @@ + + @@ -108,10 +110,6 @@ - - - - @@ -132,6 +130,15 @@ + + + + + + + + + @@ -146,6 +153,9 @@ + + + @@ -234,6 +244,7 @@ + @@ -250,6 +261,7 @@ + From d5623df7decf5900eef296cca7e5eddc43dd882f Mon Sep 17 00:00:00 2001 From: Alex Mitrevski Date: Sun, 23 Dec 2018 18:40:32 +0100 Subject: [PATCH 16/96] [knowledge_base] Moved the ontology query manager to a ROS-independent Python package and modified most of the implementations there --- .../common/knowledge_utils/__init__.py | 0 .../knowledge_utils/ontology_query_manager.py | 100 ++++++++++++++++++ .../knowledge_base_manager.py | 72 ------------- mdr_planning/mdr_knowledge_base/setup.py | 4 +- 4 files changed, 102 insertions(+), 74 deletions(-) create mode 100644 mdr_planning/mdr_knowledge_base/common/knowledge_utils/__init__.py create mode 100644 mdr_planning/mdr_knowledge_base/common/knowledge_utils/ontology_query_manager.py delete mode 100644 mdr_planning/mdr_knowledge_base/ros/src/mdr_knowledge_base/knowledge_base_manager.py diff --git a/mdr_planning/mdr_knowledge_base/common/knowledge_utils/__init__.py b/mdr_planning/mdr_knowledge_base/common/knowledge_utils/__init__.py new file mode 100644 index 000000000..e69de29bb diff --git a/mdr_planning/mdr_knowledge_base/common/knowledge_utils/ontology_query_manager.py b/mdr_planning/mdr_knowledge_base/common/knowledge_utils/ontology_query_manager.py new file mode 100644 index 000000000..a70b9e2a1 --- /dev/null +++ b/mdr_planning/mdr_knowledge_base/common/knowledge_utils/ontology_query_manager.py @@ -0,0 +1,100 @@ +'''Module defining interfaces for interacting with an OWL ontology. +''' + +import rdflib + +class OntologyQueryManager(object): + '''Defines an interface for interacting with an OWL knowledge base. + + Constructor arguments: + @param ontology_file -- full URL of an ontology file (of the form file://) + @param class_prefix -- class prefix of the items in the given ontology + + @author Alex Mitrevski + @contact aleksandar.mitrevski@h-brs.de + + ''' + def __init__(self, ontology_file, class_prefix): + self.knowledge_graph = rdflib.Graph() + self.knowledge_graph.load(ontology_file) + self.class_prefix = class_prefix + + def is_instance_of(self, obj_name, class_name): + '''Checks whether 'obj_name' is an instance of 'class_name'. + + Keyword arguments: + obj_name -- string representing the name of an object + class_name -- string representing the name of a class + + ''' + return obj_name in self.get_instances_of(class_name) + + def get_instances_of(self, class_name): + '''Returns a list of names of all instances belonging to 'class_name'. + + Keyword arguments: + @param class_name -- string representing the name of a class + + ''' + rdf_class = self.__format_class_name(class_name) + query_result = self.knowledge_graph.query('SELECT ?instance ' + + 'WHERE {?instance rdf:type ' + rdf_class + '}') + instances = [self.__extract_obj_name(x[0]) for x in query_result] + return instances + + def get_subclasses_of(self, class_name): + '''Returns a list of all subclasses of 'class_name'. + + Keyword arguments: + @param class_name -- string representing the name of a class + + ''' + rdf_class_uri = rdflib.URIRef(self.__format_class_name(class_name)) + query_result = self.knowledge_graph.transitive_subjects(rdflib.RDFS.subClassOf, + rdf_class_uri) + subclasses = [self.__extract_class_name(subclass) + for subclass in [str(x) for x in query_result]] + return subclasses + + def get_parent_classes_of(self, class_name): + '''Returns a list of all parent classes of 'class_name'. + + Keyword arguments: + @param class_name -- string representing the name of a class + + ''' + rdf_class_uri = rdflib.URIRef(self.__format_class_name(class_name)) + query_result = self.knowledge_graph.transitive_objects(rdf_class_uri, + rdflib.RDFS.subClassOf) + parent_classes = [self.__extract_class_name(parent_class) + for parent_class in [str(x) for x in query_result]] + return parent_classes + + def __format_class_name(self, class_name): + '''Returns a string of the format "self.class_prefix:class_name". + + Keyword arguments: + @param class_name -- string representing the name of a class + + ''' + return '{0}:{1}'.format(self.class_prefix, class_name) + + def __extract_class_name(self, rdf_class): + '''Extracts the name of a class given a string + of the format "self.class_prefix:class_name". + + Keyword arguments: + @param rdf_class -- string of the form "prefix:class" + + ''' + return rdf_class[rdf_class.find(':')+1:] + + def __extract_obj_name(self, obj_url): + '''Extracts the name of an object from the given full URL, + where the name is the last element of the URL. + + Keyword arguments: + @param obj_url -- object URL in string format + + ''' + return obj_url[obj_url.rfind('/')+1:] diff --git a/mdr_planning/mdr_knowledge_base/ros/src/mdr_knowledge_base/knowledge_base_manager.py b/mdr_planning/mdr_knowledge_base/ros/src/mdr_knowledge_base/knowledge_base_manager.py deleted file mode 100644 index c47a1ef56..000000000 --- a/mdr_planning/mdr_knowledge_base/ros/src/mdr_knowledge_base/knowledge_base_manager.py +++ /dev/null @@ -1,72 +0,0 @@ -import rdflib - -class KnowledgeBaseManager(object): - '''Defines an interface for interacting with an OWL knowledge base - ''' - def __init__(self, ontology_file, class_prefix): - self.knowledge_graph = rdflib.Graph() - self.knowledge_graph.load(ontology_file) - self.class_prefix = class_prefix - - def is_instance_of(self, obj_name, class_name): - '''Checks whether 'obj_name' is an instance of 'class_name' - - Keyword arguments: - obj_name -- string representing the name of an object - class_name -- string representing the name of a class - - ''' - pass - - def get_instances_of(self, class_name): - '''Returns a list of names of all instances belonging to 'class_name' - - Keyword arguments: - class_name -- string representing the name of a class - - ''' - rdf_class = self.class_prefix + ':' + class_name - query_result = self.knowledge_graph.query('SELECT ?instance ' + - 'WHERE {?instance rdf:type ' + rdf_class + '}') - instances = list() - for row in query_result: - instance = row[0] - instance_name = instance[instance.rfind('/')+1:] - instances.append(instance_name) - return instances - - def get_subclasses_of(self, class_name): - '''Returns a list of all subclasses of 'class_name' - - Keyword arguments: - class_name -- string representing the name of a class - - ''' - rdf_class = self.class_prefix + ':' + class_name - rdf_class_uri = rdflib.URIRef(rdf_class) - query_result = self.knowledge_graph.transitive_subjects(rdflib.RDFS.subClassOf, - rdf_class_uri) - subclasses = list() - for subclass_uri in query_result: - subclass = str(subclass_uri) - subclass = subclass[subclass.find(':')+1:] - subclasses.append(subclass) - return subclasses - - def get_parent_classes_of(self, class_name): - '''Returns a list of all parent classes of 'class_name' - - Keyword arguments: - class_name -- string representing the name of a class - - ''' - rdf_class = self.class_prefix + ':' + class_name - rdf_class_uri = rdflib.URIRef(rdf_class) - query_result = self.knowledge_graph.transitive_objects(rdf_class_uri, - rdflib.RDFS.subClassOf) - parent_classes = list() - for class_uri in query_result: - parent_class = str(class_uri) - parent_class = parent_class[parent_class.find(':')+1:] - parent_classes.append(parent_class) - return parent_classes diff --git a/mdr_planning/mdr_knowledge_base/setup.py b/mdr_planning/mdr_knowledge_base/setup.py index af34496c0..b8d5433dd 100644 --- a/mdr_planning/mdr_knowledge_base/setup.py +++ b/mdr_planning/mdr_knowledge_base/setup.py @@ -4,8 +4,8 @@ from catkin_pkg.python_setup import generate_distutils_setup d = generate_distutils_setup( - packages=['mdr_knowledge_base'], - package_dir={'mdr_knowledge_base': 'ros/src/mdr_knowledge_base'} + packages=['knowledge_utils'], + package_dir={'knowledge_utils': 'common/knowledge_utils'} ) setup(**d) From 98a53b39259e2750a73b792494cdff892077bea2 Mon Sep 17 00:00:00 2001 From: Alex Mitrevski Date: Sun, 23 Dec 2018 18:43:38 +0100 Subject: [PATCH 17/96] [knowledge_base] mv ontology_query_manager -> ontology_query_interface --- .../{ontology_query_manager.py => ontology_query_interface.py} | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) rename mdr_planning/mdr_knowledge_base/common/knowledge_utils/{ontology_query_manager.py => ontology_query_interface.py} (98%) diff --git a/mdr_planning/mdr_knowledge_base/common/knowledge_utils/ontology_query_manager.py b/mdr_planning/mdr_knowledge_base/common/knowledge_utils/ontology_query_interface.py similarity index 98% rename from mdr_planning/mdr_knowledge_base/common/knowledge_utils/ontology_query_manager.py rename to mdr_planning/mdr_knowledge_base/common/knowledge_utils/ontology_query_interface.py index a70b9e2a1..92fab8e97 100644 --- a/mdr_planning/mdr_knowledge_base/common/knowledge_utils/ontology_query_manager.py +++ b/mdr_planning/mdr_knowledge_base/common/knowledge_utils/ontology_query_interface.py @@ -3,7 +3,7 @@ import rdflib -class OntologyQueryManager(object): +class OntologyQueryInterface(object): '''Defines an interface for interacting with an OWL knowledge base. Constructor arguments: From 5574e434dfbf3deb559e742c6cb4c80ddfba9e58 Mon Sep 17 00:00:00 2001 From: Alex Mitrevski Date: Mon, 24 Dec 2018 13:44:56 +0100 Subject: [PATCH 18/96] [knowledge_base] Added an interface for interacting with the KB --- .../ros/src/mdr_knowledge_base/__init__.py | 0 .../domestic_kb_interface.py | 41 +++ .../knowledge_base_interface.py | 276 ++++++++++++++++++ mdr_planning/mdr_knowledge_base/setup.py | 5 +- 4 files changed, 320 insertions(+), 2 deletions(-) create mode 100644 mdr_planning/mdr_knowledge_base/ros/src/mdr_knowledge_base/__init__.py create mode 100644 mdr_planning/mdr_knowledge_base/ros/src/mdr_knowledge_base/domestic_kb_interface.py create mode 100644 mdr_planning/mdr_knowledge_base/ros/src/mdr_knowledge_base/knowledge_base_interface.py diff --git a/mdr_planning/mdr_knowledge_base/ros/src/mdr_knowledge_base/__init__.py b/mdr_planning/mdr_knowledge_base/ros/src/mdr_knowledge_base/__init__.py new file mode 100644 index 000000000..e69de29bb diff --git a/mdr_planning/mdr_knowledge_base/ros/src/mdr_knowledge_base/domestic_kb_interface.py b/mdr_planning/mdr_knowledge_base/ros/src/mdr_knowledge_base/domestic_kb_interface.py new file mode 100644 index 000000000..df6409995 --- /dev/null +++ b/mdr_planning/mdr_knowledge_base/ros/src/mdr_knowledge_base/domestic_kb_interface.py @@ -0,0 +1,41 @@ +'''Module defining interfaces for knowledge base interactions +specific to domestic applications. +''' + +from mdr_knowledge_base.knowledge_base_interface import KnowledgeBaseInterface + +class DomesticKBInterface(KnowledgeBaseInterface): + '''Defines an interface for performing knowledge base operations common + for a domestic application. + + @author Alex Mitrevski + @contact aleksandar.mitrevski@h-brs.de + + ''' + def __init__(self): + super(DomesticKBInterface, self).__init__() + + ############################################################################ + #--------------------------- Symbolic knowledge ---------------------------# + ############################################################################ + def get_surface_object_names(self, surface_name): + '''Returns a list of names of all objects on the given surface. + + Keyword arguments: + @param surface_name -- string representing the name of a surface + + ''' + surface_objects = list() + on_instances = self.get_all_attributes('on') + for item in on_instances: + object_on_desired_surface = False + object_name = '' + if not item.is_negative: + for param in item.values: + if param.key == 'plane' and param.value == surface_name: + object_on_desired_surface = True + elif param.key == 'obj': + object_name = param.value + if object_on_desired_surface: + surface_objects.append(object_name) + return surface_objects diff --git a/mdr_planning/mdr_knowledge_base/ros/src/mdr_knowledge_base/knowledge_base_interface.py b/mdr_planning/mdr_knowledge_base/ros/src/mdr_knowledge_base/knowledge_base_interface.py new file mode 100644 index 000000000..1a212cad1 --- /dev/null +++ b/mdr_planning/mdr_knowledge_base/ros/src/mdr_knowledge_base/knowledge_base_interface.py @@ -0,0 +1,276 @@ +'''Module defining interfaces for interacting with a knowledge base. +''' + +import rospy +import diagnostic_msgs.msg as diag_msgs +import rosplan_knowledge_msgs.srv as rosplan_srvs +from mongodb_store.message_store import MessageStoreProxy + +from mdr_monitoring_msgs.msg import ExecutionState + +class KnowledgeUpdateTypes(object): + # the values of these constants are set to correspond + # with the values used by the ROSPlan knowledge update service + INSERT = 0 + REMOVE = 2 + +class KnowledgeBaseInterface(object): + '''Defines an interface for interacting with a knowledge base. + + @author Alex Mitrevski + @contact aleksandar.mitrevski@h-brs.de + + ''' + def __init__(self): + # a client for the /rosplan_knowledge_base/update service + self.knowledge_update_client = None + + # a client for the /rosplan_knowledge_base/state/propositions service + self.attribute_retrieval_client = None + + # a mongodb_store.message_store.MessageStoreProxy instance + self.msg_store_client = None + + # name of the host robot; value read from the knowledge base + self.robot_name = '' + + rospy.loginfo('[kb_interface] Creating a knowledge base update client') + try: + rospy.wait_for_service('/rosplan_knowledge_base/update', 5.) + self.knowledge_update_client = rospy.ServiceProxy('/rosplan_knowledge_base/update', + rosplan_srvs.KnowledgeUpdateService) + except (rospy.ServiceException, rospy.ROSException) as exc: + rospy.logwarn('The service /rosplan_knowledge_base/state/propositions does not appear to exist.\n' + + 'Please spawn rosplan_knowledge_base/knowledgeBase') + + rospy.loginfo('[kb_interface] Creating a knowledge base query client') + try: + rospy.wait_for_service('/rosplan_knowledge_base/state/propositions', 5.) + self.attribute_retrieval_client = rospy.ServiceProxy('/rosplan_knowledge_base/state/propositions', + rosplan_srvs.GetAttributeService) + request = rosplan_srvs.GetAttributeServiceRequest() + request.predicate_name = 'robot_name' + result = self.attribute_retrieval_client(request) + for item in result.attributes: + for param in item.values: + if param.key == 'bot': + self.robot_name = param.value + break + break + except (rospy.ServiceException, rospy.ROSException) as exc: + rospy.logwarn('The service /rosplan_knowledge_base/state/propositions does not appear to exist.\n' + + 'Please spawn rosplan_knowledge_base/knowledgeBase') + + rospy.loginfo('[kb_interface] Creating a message store client') + try: + self.msg_store_client = MessageStoreProxy() + except: + print('Could not create a mongodb_store proxy.\n' + + 'Please spawn mongodb_store/message_store_node.py') + + + ############################################################################ + #--------------------------- Symbolic knowledge ---------------------------# + ############################################################################ + def get_all_attributes(self, predicate_name): + '''Returns a list of all instances of the given predicate in the knowledge base. + + @param predicate_name -- string representing the name of a predicate + + ''' + request = rosplan_srvs.GetAttributeServiceRequest() + request.predicate_name = predicate_name + result = self.attribute_retrieval_client(request) + return result.attributes + + def update_kb(self, facts_to_add, facts_to_remove): + '''Updates the knowledge base by adding and removing the facts + from the given lists. In both input lists, each entry is a tuple of the form + (predicate, [(variable, value), ...]), where "predicate" is the predicate name + and the (variable, value) tuples are the variable values. + + Keyword arguments: + @param facts_to_add -- a list of facts to add to the knowledge base + @param facts_to_remove -- a list of facts to remove from the knowledge base + + ''' + rospy.loginfo('[kb_interface] Inserting facts into the knowledge base') + self.insert_facts(facts_to_add) + + rospy.loginfo('[kb_interface] Removing facts from the knowledge base') + self.remove_facts(facts_to_remove) + + def insert_facts(self, fact_list): + '''Inserts the facts in the given list into the knowledge base. + + Keyword arguments: + @param fact_list -- a list in which each entry is a tuple of the form + (predicate, [(variable, value), ...]), where + "predicate" is the predicate name and the + (variable, value) tuples are the variable values + + ''' + self.__update_kb(KnowledgeUpdateTypes.INSERT, fact_list) + + def remove_facts(self, fact_list): + '''Removes the facts in the given list from the knowledge base. + + Keyword arguments: + @param fact_list -- a list in which each entry is a tuple of the form + (predicate, [(variable, value), ...]), where + "predicate" is the predicate name and the + (variable, value) tuples are the variable values + + ''' + self.__update_kb(KnowledgeUpdateTypes.REMOVE, fact_list) + + def __update_kb(self, update_type, fact_list): + '''Updates the knowledge base with the facts in the given list. + + Keyword arguments: + @param update_type -- a KnowledgeUpdateTypes constant describing the update type + (add or remove facts) + @param fact_list -- a list in which each entry is a tuple of the form + (predicate, [(variable, value), ...]), where + "predicate" is the predicate name and the + (variable, value) tuples are the variable values + + ''' + try: + for predicate, var_data in fact_list: + # we set up the knowledge update request with the given attribute data + request = rosplan_srvs.KnowledgeUpdateServiceRequest() + request.update_type = update_type + request.knowledge.knowledge_type = 1 + request.knowledge.attribute_name = predicate + for var_name, var_value in var_data: + arg_msg = diag_msgs.KeyValue() + arg_msg.key = var_name + arg_msg.value = var_value + request.knowledge.values.append(arg_msg) + + # we modify the fact into the knowledge base + self.knowledge_update_client(request) + except Exception as exc: + rospy.logerr('[kb_interface] %s', str(exc)) + raise KBException('The knowledge base could not be updated') + + + ############################################################################ + #------------------------------ Data storage ------------------------------# + ############################################################################ + def insert_objects(self, object_list): + '''Inserts the objects in the given list into the knowledge base. + + Keyword arguments: + @param object_list -- a list of (obj_name, obj) tuples + + ''' + for obj_name, obj in object_list: + self.msg_store_client.insert_named(obj_name, obj) + + def insert_obj_instance(self, obj_name, obj): + '''Inserts a named object instance into the knowledge base. + + Keyword arguments: + @param obj_name -- name of the object instance + @param obj -- object to insert + + ''' + self.msg_store_client.insert_named(obj_name, obj) + + def update_objects(self, object_list): + '''Updates the knowledge about the objects in the given list. + + Keyword arguments: + @param object_list -- a list of (obj_name, obj) tuples + + ''' + for obj_name, obj in object_list: + self.msg_store_client.update_named(obj_name, obj) + + def update_obj_instance(self, obj_name, obj): + '''Updates the knowledge about the given named object. + + Keyword arguments: + @param obj_name -- name of the object instance + @param obj -- object to insert + + ''' + self.msg_store_client.update_named(obj_name, obj) + + def remove_objects(self, object_list): + '''Removes the objects in the given list from the knowledge base. + + Keyword arguments: + @param object_list -- a list of (obj_name, obj_type) tuples + + ''' + for obj_name, obj_type in object_list: + self.msg_store_client.delete_named(obj_name, obj_type) + + def remove_obj_instance(self, obj_name, obj_type): + '''Removes a named object instance from the knowledge base. + + Keyword arguments: + @param obj_name -- name of the object instance + @param obj_type -- instance type (the value of the object's "_type" field) + + ''' + self.msg_store_client.delete_named(obj_name, obj_type) + + def get_objects(self, object_names, obj_type): + '''Returns a list of named object instances from the knowledge base. + + @param object_names -- a list of strings representing object names + @param obj_type -- type of the objects to be retrieved + + ''' + objects = list() + for obj_name in object_names: + obj = self.get_obj_instance(obj_name, obj_type) + if obj is not None: + objects.append(obj) + return objects + + def get_obj_instance(self, obj_name, obj_type): + '''Retrieves a named object instance from the knowledge base. + + Keyword arguments: + @param obj_name -- name of the object instance + @param obj_type -- instance type (the value of the object's "_type" field) + + ''' + try: + obj = self.msg_store_client.query_named(obj_name, obj_type)[0] + return obj + except: + rospy.logerr('[kb_interface] Error retriving knowledge about %s', obj_name) + return None + + def save_current_state(self, sm_id, state_name): + '''Saves the state of the given state machine as a + "current_state" message store entry. + ''' + execution_state_msg = ExecutionState() + execution_state_msg.stamp = rospy.Time.now() + execution_state_msg.state_machine = sm_id + execution_state_msg.state = state_name + try: + self.msg_store_client.insert_named('current_state', execution_state_msg) + except: + print('Error while saving current state') + +class KBException(Exception): + '''A custom knowledge base exception. + + @author Alex Mitrevski + @contact aleksandar.mitrevski@h-brs.de + + ''' + def __init__(self, message): + super(KBException, self).__init__(message) + self.message = message + + def __str__(self): + return self.message diff --git a/mdr_planning/mdr_knowledge_base/setup.py b/mdr_planning/mdr_knowledge_base/setup.py index b8d5433dd..0d6bd91cb 100644 --- a/mdr_planning/mdr_knowledge_base/setup.py +++ b/mdr_planning/mdr_knowledge_base/setup.py @@ -4,8 +4,9 @@ from catkin_pkg.python_setup import generate_distutils_setup d = generate_distutils_setup( - packages=['knowledge_utils'], - package_dir={'knowledge_utils': 'common/knowledge_utils'} + packages=['knowledge_utils', 'mdr_knowledge_base'], + package_dir={'knowledge_utils': 'common/knowledge_utils', + 'mdr_knowledge_base': 'ros/src/mdr_knowledge_base'} ) setup(**d) From 56973ade9144dcd77d73bbf658ca9042df131b2b Mon Sep 17 00:00:00 2001 From: Alex Mitrevski Date: Mon, 24 Dec 2018 13:52:14 +0100 Subject: [PATCH 19/96] Modified the action clients so that they use the KB interface --- .../ros/scripts/pickup_client | 124 +++++-------- .../mdr_place_action/ros/scripts/place_client | 123 +++---------- .../mdr_place_action/ros/scripts/throw_client | 87 +++------- .../ros/scripts/move_base_client | 45 +---- .../ros/scripts/perceive_plane_client | 163 +++++------------- .../action_client_base.py | 54 ++++-- 6 files changed, 178 insertions(+), 418 deletions(-) diff --git a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_pickup_action/ros/scripts/pickup_client b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_pickup_action/ros/scripts/pickup_client index f46d4a264..5517cb5b7 100755 --- a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_pickup_action/ros/scripts/pickup_client +++ b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_pickup_action/ros/scripts/pickup_client @@ -3,16 +3,18 @@ import rospy import actionlib import tf -from geometry_msgs.msg import PoseStamped -import rosplan_dispatch_msgs.msg as plan_dispatch_msgs -import rosplan_knowledge_msgs.srv as rosplan_srvs -import diagnostic_msgs.msg as diag_msgs - from mcr_perception_msgs.msg import Object from mdr_rosplan_interface.action_client_base import ActionClientBase +from mdr_knowledge_base.knowledge_base_interface import KBException from mdr_pickup_action.msg import PickupAction, PickupGoal class PickupClient(ActionClientBase): + '''Defines a client for an object pickup action. + + @author Alex Mitrevski + @contact aleksandar.mitrevski@h-brs.de + + ''' def __init__(self): super(PickupClient, self).__init__() @@ -35,19 +37,21 @@ class PickupClient(ActionClientBase): client = actionlib.SimpleActionClient(self.action_server_name, PickupAction) client.wait_for_server() goal = self.get_action_message(msg) - - # calling the actionlib server and waiting for the execution to end - rospy.loginfo('[PICKUP] Sending actionlib goal to %s' % self.action_server_name) - client.send_goal(goal) - client.wait_for_result(rospy.Duration.from_sec(int(self.action_timeout))) - result = client.get_result() - - if result and result.success: - rospy.loginfo('[PICKUP] Updating the knowledge base') - self.update_knowledge_base() - self.send_action_feedback(True) + if goal: + # calling the actionlib server and waiting for the execution to end + rospy.loginfo('[pickup] Sending actionlib goal to %s' % self.action_server_name) + client.send_goal(goal) + client.wait_for_result(rospy.Duration.from_sec(int(self.action_timeout))) + result = client.get_result() + + if result and result.success: + rospy.loginfo('[PICKUP] Updating the knowledge base') + self.update_knowledge_base() + self.send_action_feedback(True) + else: + self.send_action_feedback(False) else: - self.send_action_feedback(False) + rospy.loginfo('[pickup] The action dispatch message could not be created; ignoring request') def get_action_message(self, rosplan_action_msg): '''Reads the action parameters and uses them to initialise an actionlib message. @@ -61,73 +65,31 @@ class PickupClient(ActionClientBase): elif param.key == 'bot': self.robot_name = param.value - object_pose = self.get_object_pose(self.obj) - object_pose.header.stamp = rospy.Time(0) - - goal.pose.header.frame_id = self.frame_id - goal.pose.header.stamp = rospy.Time.now() - object_pose_in_grasp_frame = self.tf_listener.transformPose(self.frame_id, - object_pose) - goal.pose = object_pose_in_grasp_frame - return goal - - def get_object_pose(self, object_name): try: - obj = self.msg_store_client.query_named(object_name, Object._type)[0] - return obj.pose - except: - rospy.logerr('Error retriving knowledge about %s', object_name) - return PoseStamped() + obj = self.kb_interface.get_obj_instance(self.obj, Object._type) + object_pose = obj.pose + object_pose.header.stamp = rospy.Time(0) + + goal.pose.header.frame_id = self.frame_id + goal.pose.header.stamp = rospy.Time.now() + object_pose_in_grasp_frame = self.tf_listener.transformPose(self.frame_id, + object_pose) + goal.pose = object_pose_in_grasp_frame + return goal + except KBException as exc: + rospy.logerr('[pickup] %s', str(exc)) + return None def update_knowledge_base(self): - # we remove the fact that the object is on the surface from the knowledge base - request = rosplan_srvs.KnowledgeUpdateServiceRequest() - request.update_type = 2 - request.knowledge.knowledge_type = 1 - request.knowledge.attribute_name = 'on' - - arg_msg = diag_msgs.KeyValue() - arg_msg.key = 'obj' - arg_msg.value = self.obj - request.knowledge.values.append(arg_msg) - - arg_msg = diag_msgs.KeyValue() - arg_msg.key = 'plane' - arg_msg.value = self.obj_plane - request.knowledge.values.append(arg_msg) - - self.knowledge_update_client(request) - - # we remove the fact that the robot's gripper is empty from the knowledge base - request = rosplan_srvs.KnowledgeUpdateServiceRequest() - request.update_type = 2 - request.knowledge.knowledge_type = 1 - request.knowledge.attribute_name = 'empty_gripper' - - arg_msg = diag_msgs.KeyValue() - arg_msg.key = 'bot' - arg_msg.value = self.robot_name - request.knowledge.values.append(arg_msg) - - self.knowledge_update_client(request) - - # we add the fact that the robot is holding the object to the knowledge base - request = rosplan_srvs.KnowledgeUpdateServiceRequest() - request.update_type = 0 - request.knowledge.knowledge_type = 1 - request.knowledge.attribute_name = 'holding' - - arg_msg = diag_msgs.KeyValue() - arg_msg.key = 'bot' - arg_msg.value = self.robot_name - request.knowledge.values.append(arg_msg) - - arg_msg = diag_msgs.KeyValue() - arg_msg.key = 'obj' - arg_msg.value = self.obj - request.knowledge.values.append(arg_msg) - - self.knowledge_update_client(request) + '''Updates the knowledge base with the following facts: + * the robot is holding the grasped object + * the object is not on the plane anymore + * the robot's gripper is not empty anymore + ''' + facts_to_add = [('holding', [('bot', self.robot_name), ('obj', self.obj)])] + facts_to_remove = [('on', [('obj', self.obj), ('plane', self.obj_plane)]), + ('empty_gripper', [('bot', self.robot_name)])] + self.kb_interface.update_kb(facts_to_add, facts_to_remove) if __name__ == '__main__': rospy.init_node('pickup_client') diff --git a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_place_action/ros/scripts/place_client b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_place_action/ros/scripts/place_client index 2f311dbf0..a4e4ece18 100755 --- a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_place_action/ros/scripts/place_client +++ b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_place_action/ros/scripts/place_client @@ -20,6 +20,12 @@ from mdr_rosplan_interface.action_client_base import ActionClientBase from mdr_place_action.msg import PlaceAction, PlaceGoal class PlaceClient(ActionClientBase): + '''Defines a client for an object placing action. + + @author Alex Mitrevski + @contact aleksandar.mitrevski@h-brs.de + + ''' def __init__(self): super(PlaceClient, self).__init__() @@ -36,7 +42,7 @@ class PlaceClient(ActionClientBase): if self.action_name != msg.name.lower(): return - rospy.loginfo('[PLACE] Action request received') + rospy.loginfo('[place] Action request received') self.action_id = msg.action_id client = actionlib.SimpleActionClient(self.action_server_name, PlaceAction) @@ -44,13 +50,13 @@ class PlaceClient(ActionClientBase): goal = self.get_action_message(msg) # calling the actionlib server and waiting for the execution to end - rospy.loginfo('[PLACE] Sending actionlib goal to %s' % self.action_server_name) + rospy.loginfo('[place] Sending actionlib goal to %s' % self.action_server_name) client.send_goal(goal) client.wait_for_result(rospy.Duration.from_sec(int(self.action_timeout))) result = client.get_result() if result and result.success: - rospy.loginfo('[PLACE] Updating the knowledge base') + rospy.loginfo('[place] Updating the knowledge base') self.update_knowledge_base(goal.pose) self.send_action_feedback(True) else: @@ -75,18 +81,18 @@ class PlaceClient(ActionClientBase): obj_config = Configuration() obj_config.frame_id = self.frame_id - surface_object_names = self.get_surface_object_names(self.obj_plane) - surface_objects = self.get_surface_object(surface_object_names) + surface_object_names = self.kb_interface.get_surface_object_names(self.obj_plane) + surface_objects = self.kb_interface.get_objects(surface_object_names, Object._type) for obj in surface_objects: obj.pose.header.stamp = rospy.Time(0) obj.pose = self.tf_listener.transformPose(self.frame_id, obj.pose) obj_config.static_objs.append(ConverterFactory.convert_ros_msg(obj)) - manipulated_obj = self.get_object_by_name(self.obj) + manipulated_obj = self.kb_interface.get_obj_instance(self.obj, Object._type) manipulated_obj.pose.header.frame_id = self.frame_id obj_config.manipulated_obj = ConverterFactory.convert_ros_msg(manipulated_obj) - surface_plane = self.get_surface_by_name(self.obj_plane) + surface_plane = self.kb_interface.get_obj_instance(self.obj_plane, Plane._type) # we convert the surface center point to the desired frame surface_plane_point = PointStamped() @@ -142,103 +148,22 @@ class PlaceClient(ActionClientBase): candidate_pose = ConverterFactory.convert_to_ros_msg(candidate_pose) return candidate_pose - def get_surface_object_names(self, surface_name): - surface_objects = list() - request = rosplan_srvs.GetAttributeServiceRequest() - request.predicate_name = 'on' - result = self.attribute_fetching_client(request) - for item in result.attributes: - object_on_desired_surface = False - object_name = '' - if not item.is_negative: - for param in item.values: - if param.key == 'plane' and param.value == surface_name: - object_on_desired_surface = True - elif param.key == 'obj': - object_name = param.value - if object_on_desired_surface: - surface_objects.append(object_name) - return surface_objects - - def get_surface_object(self, surface_objects): - objects = list() - for obj_name in surface_objects: - obj = self.get_object_by_name(obj_name) - if obj is not None: - objects.append(obj) - return objects - - def get_object_by_name(self, obj_name): - try: - obj = self.msg_store_client.query_named(obj_name, Object._type)[0] - return obj - except: - rospy.logerr('Error retriving knowledge about %s', obj_name) - return None - - def get_surface_by_name(self, surface_name): - try: - surface = self.msg_store_client.query_named(surface_name, Plane._type)[0] - return surface - except: - rospy.logerr('Error retriving knowledge about %s', surface_name) - return None - def update_knowledge_base(self, new_object_pose): - # we remove the fact that the robot is holding the object from the knowledge base - request = rosplan_srvs.KnowledgeUpdateServiceRequest() - request.update_type = 2 - request.knowledge.knowledge_type = 1 - request.knowledge.attribute_name = 'holding' - - arg_msg = diag_msgs.KeyValue() - arg_msg.key = 'bot' - arg_msg.value = self.robot_name - request.knowledge.values.append(arg_msg) - - arg_msg = diag_msgs.KeyValue() - arg_msg.key = 'obj' - arg_msg.value = self.obj - request.knowledge.values.append(arg_msg) - - self.knowledge_update_client(request) - - # we add the fact that the robot's gripper is empty to the knowledge base - request = rosplan_srvs.KnowledgeUpdateServiceRequest() - request.update_type = 0 - request.knowledge.knowledge_type = 1 - request.knowledge.attribute_name = 'empty_gripper' - - arg_msg = diag_msgs.KeyValue() - arg_msg.key = 'bot' - arg_msg.value = self.robot_name - request.knowledge.values.append(arg_msg) - - self.knowledge_update_client(request) - - # we add the fact that the object is on the surface to the knowledge base - request = rosplan_srvs.KnowledgeUpdateServiceRequest() - request.update_type = 0 - request.knowledge.knowledge_type = 1 - request.knowledge.attribute_name = 'on' - - arg_msg = diag_msgs.KeyValue() - arg_msg.key = 'obj' - arg_msg.value = self.obj - request.knowledge.values.append(arg_msg) - - arg_msg = diag_msgs.KeyValue() - arg_msg.key = 'plane' - arg_msg.value = self.obj_plane - request.knowledge.values.append(arg_msg) - - self.knowledge_update_client(request) + '''Updates the knowledge base with the following facts: + * the object is on the placing surface + * the robot's gripper is empty + * the robot is not holding the object anymore + ''' + facts_to_add = [('on', [('obj', self.obj), ('plane', self.obj_plane)]), + ('empty_gripper', [('bot', self.robot_name)])] + facts_to_remove = [('holding', [('bot', self.robot_name), ('obj', self.obj)])] + self.kb_interface.update_kb(facts_to_add, facts_to_remove) # we update the location of the object - placed_object = self.get_object_by_name(self.obj) + placed_object = self.kb_interface.get_obj_instance(self.obj, Object._type) new_object_pose.header.stamp = rospy.Time.now() placed_object.pose = new_object_pose - self.msg_store_client.update_named(self.obj, placed_object) + self.kb_interface.update_obj_instance(self.obj, placed_object) if __name__ == '__main__': rospy.init_node('place_client') diff --git a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_place_action/ros/scripts/throw_client b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_place_action/ros/scripts/throw_client index 61becdbf5..ac4f7c3e0 100755 --- a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_place_action/ros/scripts/throw_client +++ b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_place_action/ros/scripts/throw_client @@ -8,8 +8,6 @@ from std_msgs.msg import Header from geometry_msgs.msg import Point, Quaternion, Pose, PoseStamped import rosplan_dispatch_msgs.msg as plan_dispatch_msgs -import rosplan_knowledge_msgs.srv as rosplan_srvs -import diagnostic_msgs.msg as diag_msgs from mcr_perception_msgs.msg import Object from action_execution.action import Action @@ -19,6 +17,12 @@ from mdr_rosplan_interface.action_client_base import ActionClientBase from mdr_place_action.msg import PlaceAction, PlaceGoal class ThrowClient(ActionClientBase): + '''Defines a client for an object throwing action. + + @author Alex Mitrevski + @contact aleksandar.mitrevski@h-brs.de + + ''' def __init__(self): super(ThrowClient, self).__init__() @@ -35,7 +39,7 @@ class ThrowClient(ActionClientBase): if self.action_name != msg.name.lower(): return - rospy.loginfo('[THROW] Action request received') + rospy.loginfo('[throw] Action request received') self.action_id = msg.action_id client = actionlib.SimpleActionClient(self.action_server_name, PlaceAction) @@ -43,13 +47,13 @@ class ThrowClient(ActionClientBase): goal = self.get_action_message(msg) # calling the actionlib server and waiting for the execution to end - rospy.loginfo('[THROW] Sending actionlib goal to %s' % self.action_server_name) + rospy.loginfo('[throw] Sending actionlib goal to %s' % self.action_server_name) client.send_goal(goal) client.wait_for_result(rospy.Duration.from_sec(int(self.action_timeout))) result = client.get_result() if result and result.success: - rospy.loginfo('[THROW] Updating the knowledge base') + rospy.loginfo('[throw] Updating the knowledge base') self.update_knowledge_base(goal.pose) self.send_action_feedback(True) else: @@ -71,12 +75,12 @@ class ThrowClient(ActionClientBase): return goal def get_throwing_pose(self): - throwing_target = self.get_object_by_name(self.throwing_target) + throwing_target = self.kb_interface.get_obj_instance(self.throwing_target, Object._type) throwing_target.pose.header.stamp = rospy.Time(0) throwing_target.pose = self.tf_listener.transformPose(self.frame_id, throwing_target.pose) throwing_target_obj = ConverterFactory.convert_ros_msg(throwing_target) - obj_to_throw = self.get_object_by_name(self.obj) + obj_to_throw = self.kb_interface.get_obj_instance(self.obj, Object._type) obj_to_throw.pose.header.frame_id = self.frame_id manipulated_obj = ConverterFactory.convert_ros_msg(obj_to_throw) @@ -96,70 +100,23 @@ class ThrowClient(ActionClientBase): candidate_pose = ConverterFactory.convert_to_ros_msg(candidate_pose) return candidate_pose - def get_object_by_name(self, obj_name): - try: - obj = self.msg_store_client.query_named(obj_name, Object._type)[0] - return obj - except: - rospy.logerr('Error retriving knowledge about %s', obj_name) - return None - def update_knowledge_base(self, new_object_pose): - # we remove the fact that the robot is holding the object from the knowledge base - request = rosplan_srvs.KnowledgeUpdateServiceRequest() - request.update_type = 2 - request.knowledge.knowledge_type = 1 - request.knowledge.attribute_name = 'holding' - - arg_msg = diag_msgs.KeyValue() - arg_msg.key = 'bot' - arg_msg.value = self.robot_name - request.knowledge.values.append(arg_msg) - - arg_msg = diag_msgs.KeyValue() - arg_msg.key = 'obj' - arg_msg.value = self.obj - request.knowledge.values.append(arg_msg) - - self.knowledge_update_client(request) - - # we add the fact that the robot's gripper is empty to the knowledge base - request = rosplan_srvs.KnowledgeUpdateServiceRequest() - request.update_type = 0 - request.knowledge.knowledge_type = 1 - request.knowledge.attribute_name = 'empty_gripper' - - arg_msg = diag_msgs.KeyValue() - arg_msg.key = 'bot' - arg_msg.value = self.robot_name - request.knowledge.values.append(arg_msg) - - self.knowledge_update_client(request) - - # we add the fact that the object is on the surface to the knowledge base - request = rosplan_srvs.KnowledgeUpdateServiceRequest() - request.update_type = 0 - request.knowledge.knowledge_type = 1 - request.knowledge.attribute_name = 'in' - - arg_msg = diag_msgs.KeyValue() - arg_msg.key = 'obj' - arg_msg.value = self.obj - request.knowledge.values.append(arg_msg) - - arg_msg = diag_msgs.KeyValue() - arg_msg.key = 'storing_obj' - arg_msg.value = self.throwing_target - request.knowledge.values.append(arg_msg) - - self.knowledge_update_client(request) + '''Updates the knowledge base with the following facts: + * the object is in the throwing target + * the robot's gripper is empty + * the robot is not holding the object anymore + ''' + facts_to_add = [('in', [('obj', self.obj), ('storing_obj', self.throwing_target)]), + ('empty_gripper', [('bot', self.robot_name)])] + facts_to_remove = [('holding', [('bot', self.robot_name), ('obj', self.obj)])] + self.kb_interface.update_kb(facts_to_add, facts_to_remove) # we update the location of the object - thrown_object = self.get_object_by_name(self.obj) + thrown_object = self.kb_interface.get_obj_instance(self.obj, Object._type) new_object_pose.header.stamp = rospy.Time.now() new_object_pose.pose.position.z = 0. thrown_object.pose = new_object_pose - self.msg_store_client.update_named(self.obj, thrown_object) + self.kb_interface.update_obj_instance(self.obj, thrown_object) if __name__ == '__main__': rospy.init_node('throw_client') diff --git a/mdr_planning/mdr_actions/mdr_navigation_actions/mdr_move_base_action/ros/scripts/move_base_client b/mdr_planning/mdr_actions/mdr_navigation_actions/mdr_move_base_action/ros/scripts/move_base_client index 47d891e3f..f07df28e5 100755 --- a/mdr_planning/mdr_actions/mdr_navigation_actions/mdr_move_base_action/ros/scripts/move_base_client +++ b/mdr_planning/mdr_actions/mdr_navigation_actions/mdr_move_base_action/ros/scripts/move_base_client @@ -2,9 +2,6 @@ import rospy import actionlib -import rosplan_knowledge_msgs.srv as rosplan_srvs -import diagnostic_msgs.msg as diag_msgs - from mdr_rosplan_interface.action_client_base import ActionClientBase from mdr_move_base_action.msg import MoveBaseAction, MoveBaseGoal @@ -56,41 +53,13 @@ class MoveBaseClient(ActionClientBase): return goal def update_knowledge_base(self, destination_location): - # we add the fact that the robot is not at the source location to the knowledge base - request = rosplan_srvs.KnowledgeUpdateServiceRequest() - request.update_type = 2 - request.knowledge.knowledge_type = 1 - request.knowledge.attribute_name = 'robot_at' - - arg_msg = diag_msgs.KeyValue() - arg_msg.key = 'bot' - arg_msg.value = self.robot_name - request.knowledge.values.append(arg_msg) - - arg_msg = diag_msgs.KeyValue() - arg_msg.key = 'wp' - arg_msg.value = self.original_location - request.knowledge.values.append(arg_msg) - - self.knowledge_update_client(request) - - # we add the fact that the robot is at the destination location to the knowledge base - request = rosplan_srvs.KnowledgeUpdateServiceRequest() - request.update_type = 0 - request.knowledge.knowledge_type = 1 - request.knowledge.attribute_name = 'robot_at' - - arg_msg = diag_msgs.KeyValue() - arg_msg.key = 'bot' - arg_msg.value = self.robot_name - request.knowledge.values.append(arg_msg) - - arg_msg = diag_msgs.KeyValue() - arg_msg.key = 'wp' - arg_msg.value = destination_location - request.knowledge.values.append(arg_msg) - - self.knowledge_update_client(request) + '''Updates the knowledge base with the following facts: + * the robot is not at the original location anymore + * the robot is at the destination location + ''' + facts_to_add = [('robot_at', [('bot', self.robot_name), ('wp', destination_location)])] + facts_to_remove = [('robot_at', [('bot', self.robot_name), ('wp', self.original_location)])] + self.kb_interface.update_kb(facts_to_add, facts_to_remove) if __name__ == '__main__': rospy.init_node('mdr_move_base_client') diff --git a/mdr_planning/mdr_actions/mdr_perception_actions/mdr_perceive_plane_action/ros/scripts/perceive_plane_client b/mdr_planning/mdr_actions/mdr_perception_actions/mdr_perceive_plane_action/ros/scripts/perceive_plane_client index 07d5f6fb5..21c2d447e 100755 --- a/mdr_planning/mdr_actions/mdr_perception_actions/mdr_perceive_plane_action/ros/scripts/perceive_plane_client +++ b/mdr_planning/mdr_actions/mdr_perception_actions/mdr_perceive_plane_action/ros/scripts/perceive_plane_client @@ -2,14 +2,18 @@ import rospy import actionlib +import rosplan_dispatch_msgs.msg as plan_dispatch_msgs from mcr_perception_msgs.msg import Plane, Object from mdr_rosplan_interface.action_client_base import ActionClientBase from mdr_perceive_plane_action.msg import PerceivePlaneAction, PerceivePlaneGoal -import rosplan_dispatch_msgs.msg as plan_dispatch_msgs -import rosplan_knowledge_msgs.srv as rosplan_srvs -import diagnostic_msgs.msg as diag_msgs class PerceivePlaneClient(ActionClientBase): + '''Defines a client for a plane scanning action. + + @author Minh Nguyen, Alex Mitrevski + @contact minh.nguyen@smail.inf.h-brs.de, aleksandar.mitrevski@h-brs.de + + ''' def __init__(self): super(PerceivePlaneClient, self).__init__() self.clear_plane_memory = rospy.get_param('~clear_plane_memory', False) @@ -54,147 +58,58 @@ class PerceivePlaneClient(ActionClientBase): return goal def update_knowledge_base(self, plane_prefix, plane_list): + '''Updates the knowledge base with the following facts: + * each of the detected planes is now explored, so it's not unexplored anymore + * each object on each of the detected plane is on the plane and has + the recognised category + ''' + facts_to_add = [] + facts_to_remove = [] + objects_to_add = [] for i, plane in enumerate(plane_list.planes): if self.clear_plane_memory: self.__delete_plane_from_kb(plane.name) - # we remove the fact that the plane is unexplored from the knowledge base - request = rosplan_srvs.KnowledgeUpdateServiceRequest() - request.update_type = 2 - request.knowledge.knowledge_type = 1 - request.knowledge.attribute_name = 'unexplored' - - arg_msg = diag_msgs.KeyValue() - arg_msg.key = 'plane' - arg_msg.value = plane.name - request.knowledge.values.append(arg_msg) - - self.knowledge_update_client(request) + facts_to_add.append(('explored', [('plane', plane.name)])) + facts_to_remove.append(('unexplored', [('plane', plane.name)])) + objects_to_add.append((plane.name, plane)) - # we add the fact that the plane is explored to the knowledge base - request = rosplan_srvs.KnowledgeUpdateServiceRequest() - request.update_type = 0 - request.knowledge.knowledge_type = 1 - request.knowledge.attribute_name = 'explored' - - arg_msg = diag_msgs.KeyValue() - arg_msg.key = 'plane' - arg_msg.value = plane.name - request.knowledge.values.append(arg_msg) - - self.knowledge_update_client(request) - self.msg_store_client.insert_named(plane.name, plane) - - # we add the detected objects to the knowledge base + # we update the list of facts and objects to add with the detected objects for obj in plane.object_list.objects: # we set a unique name for each object obj_name = obj.name + '_' + str(i) obj.name = obj_name - request = rosplan_srvs.KnowledgeUpdateServiceRequest() - request.update_type = 0 - request.knowledge.knowledge_type = 1 - request.knowledge.attribute_name = 'on' - - arg_msg = diag_msgs.KeyValue() - arg_msg.key = 'obj' - arg_msg.value = obj_name - request.knowledge.values.append(arg_msg) - - arg_msg = diag_msgs.KeyValue() - arg_msg.key = 'plane' - arg_msg.value = plane.name - request.knowledge.values.append(arg_msg) - - self.knowledge_update_client(request) + facts_to_add.append(('on', [('obj', obj_name), ('plane', plane.name)])) + facts_to_add.append(('object_category', [('obj', obj_name), ('cat', obj.category)])) + object_to_add.append((obj_name, obj)) - # we add the object category to the knowledge base - request = rosplan_srvs.KnowledgeUpdateServiceRequest() - request.update_type = 0 - request.knowledge.knowledge_type = 1 - request.knowledge.attribute_name = 'object_category' - - arg_msg = diag_msgs.KeyValue() - arg_msg.key = 'obj' - arg_msg.value = obj_name - request.knowledge.values.append(arg_msg) - - arg_msg = diag_msgs.KeyValue() - arg_msg.key = 'cat' - arg_msg.value = obj.category - request.knowledge.values.append(arg_msg) - - self.knowledge_update_client(request) - - # we add the object instance to the world model - self.msg_store_client.insert_named(obj_name, obj) + # we finally update the knowledge base + self.kb_interface.update_kb(facts_to_add, facts_to_remove) + self.kb_interface.insert_objects(objects_to_add) def __delete_plane_from_kb(self, plane_name): - '''Removes the plane with the given name and all objects on it from the - knowledge base (both the ROSPlan knowledge base and the message store) + '''Removes the plane with the given name and all objects on it + from the knowledge base. Keyword arguments: plane_name -- string indicating the name of a plane ''' - request = rosplan_srvs.KnowledgeUpdateServiceRequest() - request.update_type = 2 - request.knowledge.knowledge_type = 1 - request.knowledge.attribute_name = 'explored' - - arg_msg = diag_msgs.KeyValue() - arg_msg.key = 'plane' - arg_msg.value = plane_name - request.knowledge.values.append(arg_msg) - - # we delete the plane from the knowledge base - self.knowledge_update_client(request) - self.msg_store_client.delete_named(plane_name, Plane._type) - - # we delete all objects on the plane from the knowledge base - plane_objects = self.__get_plane_objects(plane_name) - for obj_name in plane_objects: - request = rosplan_srvs.KnowledgeUpdateServiceRequest() - request.update_type = 2 - request.knowledge.knowledge_type = 1 - request.knowledge.attribute_name = 'on' - - arg_msg = diag_msgs.KeyValue() - arg_msg.key = 'obj' - arg_msg.value = obj_name - request.knowledge.values.append(arg_msg) - - arg_msg = diag_msgs.KeyValue() - arg_msg.key = 'plane' - arg_msg.value = plane_name - request.knowledge.values.append(arg_msg) + facts_to_remove = [('explored', [('plane', plane_name)])] + objects_to_remove = [(plane_name, Plane._type)] - self.knowledge_update_client(request) - self.msg_store_client.delete_named(obj_name, Object._type) - - def __get_plane_objects(self, plane_name): - '''Returns a list with the names of all objects on the plane with the given name - - Keyword arguments: - plane_name -- string indicating the name of a plane + # we update the lists of facts and objects to remove + # with the objects that on the plane + plane_objects = self.kb_interface.get_surface_object_names(plane_name) + for obj_name in plane_objects: + fact = [('on', [('obj', obj_name), ('plane', plane_name)])] + facts_to_remove.append(fact) + objects_to_remove.append((obj_name, Object._type)) - ''' - plane_objects = list() - request = rosplan_srvs.GetAttributeServiceRequest() - request.predicate_name = 'on' - result = self.attribute_fetching_client(request) - for item in result.attributes: - object_on_desired_surface = False - object_name = '' - if not item.is_negative: - for param in item.values: - if param.key == 'plane' and param.value == plane_name: - object_on_desired_surface = True - elif param.key == 'obj': - object_name = param.value - if object_on_desired_surface: - plane_objects.append(object_name) - return plane_objects + # we finally remove the facts and objects from the knowledge base + self.kb_interface.remove_facts(facts_to_remove) + self.kb_interface.remove_objects(objects_to_remove) if __name__ == '__main__': rospy.init_node('perceive_plane_client') diff --git a/mdr_planning/mdr_rosplan_interface/ros/src/mdr_rosplan_interface/action_client_base.py b/mdr_planning/mdr_rosplan_interface/ros/src/mdr_rosplan_interface/action_client_base.py index dd79fc045..f017b6f09 100644 --- a/mdr_planning/mdr_rosplan_interface/ros/src/mdr_rosplan_interface/action_client_base.py +++ b/mdr_planning/mdr_rosplan_interface/ros/src/mdr_rosplan_interface/action_client_base.py @@ -4,54 +4,86 @@ import rospy import rosplan_dispatch_msgs.msg as plan_dispatch_msgs -import rosplan_knowledge_msgs.srv as rosplan_srvs import diagnostic_msgs.msg as diag_msgs -from mongodb_store.message_store import MessageStoreProxy +from mdr_knowledge_base.domestic_kb_interface import DomesticKBInterface class ActionClientBase(object): + '''An abstract base class for knowledge-enabled action clients. + + @author Alex Mitrevski + @contact aleksandar.mitrevski@h-brs.de + + ''' def __init__(self): self.action_success_msg = 'action achieved' self.action_failure_msg = 'action failed' + + # unique action ID self.action_id = -1 + # name of the robot on which the client is spawned self.robot_name = None + # name of the action (converted to lowercase) self.action_name = rospy.get_param('~action_name', '') self.action_name = self.action_name.lower() + # name of the action server self.action_server_name = rospy.get_param('~server_name', '') - self.action_timeout = rospy.get_param('~action_timeout', 15.) - self.knowledge_update_client = rospy.ServiceProxy('knowledge_update_service', - rosplan_srvs.KnowledgeUpdateService) - - self.attribute_fetching_client = rospy.ServiceProxy('attribute_fetching_service', - rosplan_srvs.GetAttributeService) + # timeout for action calls + self.action_timeout = rospy.get_param('~action_timeout', 15.) - self.msg_store_client = MessageStoreProxy() + # knowledge base interface instance + self.kb_interface = DomesticKBInterface() + # subscriber for dispatched actions rospy.Subscriber('action_dispatch_topic', plan_dispatch_msgs.ActionDispatch, self.call_action) + # action feedback publisher self.feedback_pub = rospy.Publisher('action_feedback_topic', plan_dispatch_msgs.ActionFeedback, queue_size=1) @abc.abstractmethod def call_action(self, msg): + '''Abstract callback for the dispatched action subscriber. + Only reacts to request to "self.action_name"; ignores all other requests. + + Keyword arguments: + msg -- a rosplan_dispatch_msgs.msg.ActionDispatch instance + + ''' pass @abc.abstractmethod def get_action_message(self, rosplan_action_msg): + '''Abstract method for converting the message to an action request. + Returns an actionlib goal instance for the action. + + Keyword arguments: + rosplan_action_msg -- a rosplan_dispatch_msgs.msg.ActionDispatch instance + + ''' return None @abc.abstractmethod - def update_knowledge_base(self, source_location, destination_location): + def update_knowledge_base(self): + '''Abstract method for updating the knowledge base after + the successful completion of an action. + ''' pass - @abc.abstractmethod def send_action_feedback(self, success): + '''Publishes a rosplan_dispatch_msgs.msg.ActionFeedback message + based on the result of the action execution. + + Keyword arguments: + success -- a Boolean indicating whether the action was successfully executed + + ''' msg = plan_dispatch_msgs.ActionFeedback() msg.action_id = self.action_id if success: From 2c21ecfdeec7cf2e6e5aba67b960587e738b84fa Mon Sep 17 00:00:00 2001 From: Alex Mitrevski Date: Mon, 24 Dec 2018 21:30:38 +0100 Subject: [PATCH 20/96] [knowledge_base] Moved the ontology and KB interfaces to a dedicated repo --- .../mdr_knowledge_base/CMakeLists.txt | 3 - .../common/knowledge_utils/__init__.py | 0 .../ontology_query_interface.py | 100 ------ .../common/ontology/apartment.owl | 318 ------------------ .../ros/src/mdr_knowledge_base/__init__.py | 0 .../domestic_kb_interface.py | 41 --- .../knowledge_base_interface.py | 276 --------------- mdr_planning/mdr_knowledge_base/setup.py | 12 - .../mdr_rosplan_interface/CMakeLists.txt | 1 + .../mdr_rosplan_interface/package.xml | 6 +- .../action_client_base.py | 2 +- 11 files changed, 6 insertions(+), 753 deletions(-) delete mode 100644 mdr_planning/mdr_knowledge_base/common/knowledge_utils/__init__.py delete mode 100644 mdr_planning/mdr_knowledge_base/common/knowledge_utils/ontology_query_interface.py delete mode 100644 mdr_planning/mdr_knowledge_base/common/ontology/apartment.owl delete mode 100644 mdr_planning/mdr_knowledge_base/ros/src/mdr_knowledge_base/__init__.py delete mode 100644 mdr_planning/mdr_knowledge_base/ros/src/mdr_knowledge_base/domestic_kb_interface.py delete mode 100644 mdr_planning/mdr_knowledge_base/ros/src/mdr_knowledge_base/knowledge_base_interface.py delete mode 100644 mdr_planning/mdr_knowledge_base/setup.py diff --git a/mdr_planning/mdr_knowledge_base/CMakeLists.txt b/mdr_planning/mdr_knowledge_base/CMakeLists.txt index a26f77080..1f87b2771 100644 --- a/mdr_planning/mdr_knowledge_base/CMakeLists.txt +++ b/mdr_planning/mdr_knowledge_base/CMakeLists.txt @@ -12,9 +12,6 @@ catkin_package( rospy ) -catkin_python_setup() -roslint_python() - include_directories( ${catkin_INCLUDE_DIRS} ) diff --git a/mdr_planning/mdr_knowledge_base/common/knowledge_utils/__init__.py b/mdr_planning/mdr_knowledge_base/common/knowledge_utils/__init__.py deleted file mode 100644 index e69de29bb..000000000 diff --git a/mdr_planning/mdr_knowledge_base/common/knowledge_utils/ontology_query_interface.py b/mdr_planning/mdr_knowledge_base/common/knowledge_utils/ontology_query_interface.py deleted file mode 100644 index 92fab8e97..000000000 --- a/mdr_planning/mdr_knowledge_base/common/knowledge_utils/ontology_query_interface.py +++ /dev/null @@ -1,100 +0,0 @@ -'''Module defining interfaces for interacting with an OWL ontology. -''' - -import rdflib - -class OntologyQueryInterface(object): - '''Defines an interface for interacting with an OWL knowledge base. - - Constructor arguments: - @param ontology_file -- full URL of an ontology file (of the form file://) - @param class_prefix -- class prefix of the items in the given ontology - - @author Alex Mitrevski - @contact aleksandar.mitrevski@h-brs.de - - ''' - def __init__(self, ontology_file, class_prefix): - self.knowledge_graph = rdflib.Graph() - self.knowledge_graph.load(ontology_file) - self.class_prefix = class_prefix - - def is_instance_of(self, obj_name, class_name): - '''Checks whether 'obj_name' is an instance of 'class_name'. - - Keyword arguments: - obj_name -- string representing the name of an object - class_name -- string representing the name of a class - - ''' - return obj_name in self.get_instances_of(class_name) - - def get_instances_of(self, class_name): - '''Returns a list of names of all instances belonging to 'class_name'. - - Keyword arguments: - @param class_name -- string representing the name of a class - - ''' - rdf_class = self.__format_class_name(class_name) - query_result = self.knowledge_graph.query('SELECT ?instance ' + - 'WHERE {?instance rdf:type ' + rdf_class + '}') - instances = [self.__extract_obj_name(x[0]) for x in query_result] - return instances - - def get_subclasses_of(self, class_name): - '''Returns a list of all subclasses of 'class_name'. - - Keyword arguments: - @param class_name -- string representing the name of a class - - ''' - rdf_class_uri = rdflib.URIRef(self.__format_class_name(class_name)) - query_result = self.knowledge_graph.transitive_subjects(rdflib.RDFS.subClassOf, - rdf_class_uri) - subclasses = [self.__extract_class_name(subclass) - for subclass in [str(x) for x in query_result]] - return subclasses - - def get_parent_classes_of(self, class_name): - '''Returns a list of all parent classes of 'class_name'. - - Keyword arguments: - @param class_name -- string representing the name of a class - - ''' - rdf_class_uri = rdflib.URIRef(self.__format_class_name(class_name)) - query_result = self.knowledge_graph.transitive_objects(rdf_class_uri, - rdflib.RDFS.subClassOf) - parent_classes = [self.__extract_class_name(parent_class) - for parent_class in [str(x) for x in query_result]] - return parent_classes - - def __format_class_name(self, class_name): - '''Returns a string of the format "self.class_prefix:class_name". - - Keyword arguments: - @param class_name -- string representing the name of a class - - ''' - return '{0}:{1}'.format(self.class_prefix, class_name) - - def __extract_class_name(self, rdf_class): - '''Extracts the name of a class given a string - of the format "self.class_prefix:class_name". - - Keyword arguments: - @param rdf_class -- string of the form "prefix:class" - - ''' - return rdf_class[rdf_class.find(':')+1:] - - def __extract_obj_name(self, obj_url): - '''Extracts the name of an object from the given full URL, - where the name is the last element of the URL. - - Keyword arguments: - @param obj_url -- object URL in string format - - ''' - return obj_url[obj_url.rfind('/')+1:] diff --git a/mdr_planning/mdr_knowledge_base/common/ontology/apartment.owl b/mdr_planning/mdr_knowledge_base/common/ontology/apartment.owl deleted file mode 100644 index 230bcc0f7..000000000 --- a/mdr_planning/mdr_knowledge_base/common/ontology/apartment.owl +++ /dev/null @@ -1,318 +0,0 @@ - - - -]> - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/mdr_planning/mdr_knowledge_base/ros/src/mdr_knowledge_base/__init__.py b/mdr_planning/mdr_knowledge_base/ros/src/mdr_knowledge_base/__init__.py deleted file mode 100644 index e69de29bb..000000000 diff --git a/mdr_planning/mdr_knowledge_base/ros/src/mdr_knowledge_base/domestic_kb_interface.py b/mdr_planning/mdr_knowledge_base/ros/src/mdr_knowledge_base/domestic_kb_interface.py deleted file mode 100644 index df6409995..000000000 --- a/mdr_planning/mdr_knowledge_base/ros/src/mdr_knowledge_base/domestic_kb_interface.py +++ /dev/null @@ -1,41 +0,0 @@ -'''Module defining interfaces for knowledge base interactions -specific to domestic applications. -''' - -from mdr_knowledge_base.knowledge_base_interface import KnowledgeBaseInterface - -class DomesticKBInterface(KnowledgeBaseInterface): - '''Defines an interface for performing knowledge base operations common - for a domestic application. - - @author Alex Mitrevski - @contact aleksandar.mitrevski@h-brs.de - - ''' - def __init__(self): - super(DomesticKBInterface, self).__init__() - - ############################################################################ - #--------------------------- Symbolic knowledge ---------------------------# - ############################################################################ - def get_surface_object_names(self, surface_name): - '''Returns a list of names of all objects on the given surface. - - Keyword arguments: - @param surface_name -- string representing the name of a surface - - ''' - surface_objects = list() - on_instances = self.get_all_attributes('on') - for item in on_instances: - object_on_desired_surface = False - object_name = '' - if not item.is_negative: - for param in item.values: - if param.key == 'plane' and param.value == surface_name: - object_on_desired_surface = True - elif param.key == 'obj': - object_name = param.value - if object_on_desired_surface: - surface_objects.append(object_name) - return surface_objects diff --git a/mdr_planning/mdr_knowledge_base/ros/src/mdr_knowledge_base/knowledge_base_interface.py b/mdr_planning/mdr_knowledge_base/ros/src/mdr_knowledge_base/knowledge_base_interface.py deleted file mode 100644 index 1a212cad1..000000000 --- a/mdr_planning/mdr_knowledge_base/ros/src/mdr_knowledge_base/knowledge_base_interface.py +++ /dev/null @@ -1,276 +0,0 @@ -'''Module defining interfaces for interacting with a knowledge base. -''' - -import rospy -import diagnostic_msgs.msg as diag_msgs -import rosplan_knowledge_msgs.srv as rosplan_srvs -from mongodb_store.message_store import MessageStoreProxy - -from mdr_monitoring_msgs.msg import ExecutionState - -class KnowledgeUpdateTypes(object): - # the values of these constants are set to correspond - # with the values used by the ROSPlan knowledge update service - INSERT = 0 - REMOVE = 2 - -class KnowledgeBaseInterface(object): - '''Defines an interface for interacting with a knowledge base. - - @author Alex Mitrevski - @contact aleksandar.mitrevski@h-brs.de - - ''' - def __init__(self): - # a client for the /rosplan_knowledge_base/update service - self.knowledge_update_client = None - - # a client for the /rosplan_knowledge_base/state/propositions service - self.attribute_retrieval_client = None - - # a mongodb_store.message_store.MessageStoreProxy instance - self.msg_store_client = None - - # name of the host robot; value read from the knowledge base - self.robot_name = '' - - rospy.loginfo('[kb_interface] Creating a knowledge base update client') - try: - rospy.wait_for_service('/rosplan_knowledge_base/update', 5.) - self.knowledge_update_client = rospy.ServiceProxy('/rosplan_knowledge_base/update', - rosplan_srvs.KnowledgeUpdateService) - except (rospy.ServiceException, rospy.ROSException) as exc: - rospy.logwarn('The service /rosplan_knowledge_base/state/propositions does not appear to exist.\n' + - 'Please spawn rosplan_knowledge_base/knowledgeBase') - - rospy.loginfo('[kb_interface] Creating a knowledge base query client') - try: - rospy.wait_for_service('/rosplan_knowledge_base/state/propositions', 5.) - self.attribute_retrieval_client = rospy.ServiceProxy('/rosplan_knowledge_base/state/propositions', - rosplan_srvs.GetAttributeService) - request = rosplan_srvs.GetAttributeServiceRequest() - request.predicate_name = 'robot_name' - result = self.attribute_retrieval_client(request) - for item in result.attributes: - for param in item.values: - if param.key == 'bot': - self.robot_name = param.value - break - break - except (rospy.ServiceException, rospy.ROSException) as exc: - rospy.logwarn('The service /rosplan_knowledge_base/state/propositions does not appear to exist.\n' + - 'Please spawn rosplan_knowledge_base/knowledgeBase') - - rospy.loginfo('[kb_interface] Creating a message store client') - try: - self.msg_store_client = MessageStoreProxy() - except: - print('Could not create a mongodb_store proxy.\n' + - 'Please spawn mongodb_store/message_store_node.py') - - - ############################################################################ - #--------------------------- Symbolic knowledge ---------------------------# - ############################################################################ - def get_all_attributes(self, predicate_name): - '''Returns a list of all instances of the given predicate in the knowledge base. - - @param predicate_name -- string representing the name of a predicate - - ''' - request = rosplan_srvs.GetAttributeServiceRequest() - request.predicate_name = predicate_name - result = self.attribute_retrieval_client(request) - return result.attributes - - def update_kb(self, facts_to_add, facts_to_remove): - '''Updates the knowledge base by adding and removing the facts - from the given lists. In both input lists, each entry is a tuple of the form - (predicate, [(variable, value), ...]), where "predicate" is the predicate name - and the (variable, value) tuples are the variable values. - - Keyword arguments: - @param facts_to_add -- a list of facts to add to the knowledge base - @param facts_to_remove -- a list of facts to remove from the knowledge base - - ''' - rospy.loginfo('[kb_interface] Inserting facts into the knowledge base') - self.insert_facts(facts_to_add) - - rospy.loginfo('[kb_interface] Removing facts from the knowledge base') - self.remove_facts(facts_to_remove) - - def insert_facts(self, fact_list): - '''Inserts the facts in the given list into the knowledge base. - - Keyword arguments: - @param fact_list -- a list in which each entry is a tuple of the form - (predicate, [(variable, value), ...]), where - "predicate" is the predicate name and the - (variable, value) tuples are the variable values - - ''' - self.__update_kb(KnowledgeUpdateTypes.INSERT, fact_list) - - def remove_facts(self, fact_list): - '''Removes the facts in the given list from the knowledge base. - - Keyword arguments: - @param fact_list -- a list in which each entry is a tuple of the form - (predicate, [(variable, value), ...]), where - "predicate" is the predicate name and the - (variable, value) tuples are the variable values - - ''' - self.__update_kb(KnowledgeUpdateTypes.REMOVE, fact_list) - - def __update_kb(self, update_type, fact_list): - '''Updates the knowledge base with the facts in the given list. - - Keyword arguments: - @param update_type -- a KnowledgeUpdateTypes constant describing the update type - (add or remove facts) - @param fact_list -- a list in which each entry is a tuple of the form - (predicate, [(variable, value), ...]), where - "predicate" is the predicate name and the - (variable, value) tuples are the variable values - - ''' - try: - for predicate, var_data in fact_list: - # we set up the knowledge update request with the given attribute data - request = rosplan_srvs.KnowledgeUpdateServiceRequest() - request.update_type = update_type - request.knowledge.knowledge_type = 1 - request.knowledge.attribute_name = predicate - for var_name, var_value in var_data: - arg_msg = diag_msgs.KeyValue() - arg_msg.key = var_name - arg_msg.value = var_value - request.knowledge.values.append(arg_msg) - - # we modify the fact into the knowledge base - self.knowledge_update_client(request) - except Exception as exc: - rospy.logerr('[kb_interface] %s', str(exc)) - raise KBException('The knowledge base could not be updated') - - - ############################################################################ - #------------------------------ Data storage ------------------------------# - ############################################################################ - def insert_objects(self, object_list): - '''Inserts the objects in the given list into the knowledge base. - - Keyword arguments: - @param object_list -- a list of (obj_name, obj) tuples - - ''' - for obj_name, obj in object_list: - self.msg_store_client.insert_named(obj_name, obj) - - def insert_obj_instance(self, obj_name, obj): - '''Inserts a named object instance into the knowledge base. - - Keyword arguments: - @param obj_name -- name of the object instance - @param obj -- object to insert - - ''' - self.msg_store_client.insert_named(obj_name, obj) - - def update_objects(self, object_list): - '''Updates the knowledge about the objects in the given list. - - Keyword arguments: - @param object_list -- a list of (obj_name, obj) tuples - - ''' - for obj_name, obj in object_list: - self.msg_store_client.update_named(obj_name, obj) - - def update_obj_instance(self, obj_name, obj): - '''Updates the knowledge about the given named object. - - Keyword arguments: - @param obj_name -- name of the object instance - @param obj -- object to insert - - ''' - self.msg_store_client.update_named(obj_name, obj) - - def remove_objects(self, object_list): - '''Removes the objects in the given list from the knowledge base. - - Keyword arguments: - @param object_list -- a list of (obj_name, obj_type) tuples - - ''' - for obj_name, obj_type in object_list: - self.msg_store_client.delete_named(obj_name, obj_type) - - def remove_obj_instance(self, obj_name, obj_type): - '''Removes a named object instance from the knowledge base. - - Keyword arguments: - @param obj_name -- name of the object instance - @param obj_type -- instance type (the value of the object's "_type" field) - - ''' - self.msg_store_client.delete_named(obj_name, obj_type) - - def get_objects(self, object_names, obj_type): - '''Returns a list of named object instances from the knowledge base. - - @param object_names -- a list of strings representing object names - @param obj_type -- type of the objects to be retrieved - - ''' - objects = list() - for obj_name in object_names: - obj = self.get_obj_instance(obj_name, obj_type) - if obj is not None: - objects.append(obj) - return objects - - def get_obj_instance(self, obj_name, obj_type): - '''Retrieves a named object instance from the knowledge base. - - Keyword arguments: - @param obj_name -- name of the object instance - @param obj_type -- instance type (the value of the object's "_type" field) - - ''' - try: - obj = self.msg_store_client.query_named(obj_name, obj_type)[0] - return obj - except: - rospy.logerr('[kb_interface] Error retriving knowledge about %s', obj_name) - return None - - def save_current_state(self, sm_id, state_name): - '''Saves the state of the given state machine as a - "current_state" message store entry. - ''' - execution_state_msg = ExecutionState() - execution_state_msg.stamp = rospy.Time.now() - execution_state_msg.state_machine = sm_id - execution_state_msg.state = state_name - try: - self.msg_store_client.insert_named('current_state', execution_state_msg) - except: - print('Error while saving current state') - -class KBException(Exception): - '''A custom knowledge base exception. - - @author Alex Mitrevski - @contact aleksandar.mitrevski@h-brs.de - - ''' - def __init__(self, message): - super(KBException, self).__init__(message) - self.message = message - - def __str__(self): - return self.message diff --git a/mdr_planning/mdr_knowledge_base/setup.py b/mdr_planning/mdr_knowledge_base/setup.py deleted file mode 100644 index 0d6bd91cb..000000000 --- a/mdr_planning/mdr_knowledge_base/setup.py +++ /dev/null @@ -1,12 +0,0 @@ -#!/usr/bin/env python - -from distutils.core import setup -from catkin_pkg.python_setup import generate_distutils_setup - -d = generate_distutils_setup( - packages=['knowledge_utils', 'mdr_knowledge_base'], - package_dir={'knowledge_utils': 'common/knowledge_utils', - 'mdr_knowledge_base': 'ros/src/mdr_knowledge_base'} -) - -setup(**d) diff --git a/mdr_planning/mdr_rosplan_interface/CMakeLists.txt b/mdr_planning/mdr_rosplan_interface/CMakeLists.txt index ebfddac05..eb6e4db44 100644 --- a/mdr_planning/mdr_rosplan_interface/CMakeLists.txt +++ b/mdr_planning/mdr_rosplan_interface/CMakeLists.txt @@ -5,6 +5,7 @@ find_package(catkin REQUIRED COMPONENTS rosplan_knowledge_msgs rospy roslint + mas_knowledge_base ) catkin_python_setup() diff --git a/mdr_planning/mdr_rosplan_interface/package.xml b/mdr_planning/mdr_rosplan_interface/package.xml index 4325c92b2..2532a7ccd 100644 --- a/mdr_planning/mdr_rosplan_interface/package.xml +++ b/mdr_planning/mdr_rosplan_interface/package.xml @@ -9,9 +9,11 @@ MAS robotics catkin - rosplan_knowledge_msgs rospy + rosplan_knowledge_msgs + mas_knowledge_base - rosplan_knowledge_msgs rospy + rosplan_knowledge_msgs + mas_knowledge_base diff --git a/mdr_planning/mdr_rosplan_interface/ros/src/mdr_rosplan_interface/action_client_base.py b/mdr_planning/mdr_rosplan_interface/ros/src/mdr_rosplan_interface/action_client_base.py index f017b6f09..4f49656a4 100644 --- a/mdr_planning/mdr_rosplan_interface/ros/src/mdr_rosplan_interface/action_client_base.py +++ b/mdr_planning/mdr_rosplan_interface/ros/src/mdr_rosplan_interface/action_client_base.py @@ -5,7 +5,7 @@ import rospy import rosplan_dispatch_msgs.msg as plan_dispatch_msgs import diagnostic_msgs.msg as diag_msgs -from mdr_knowledge_base.domestic_kb_interface import DomesticKBInterface +from mas_knowledge_base.domestic_kb_interface import DomesticKBInterface class ActionClientBase(object): '''An abstract base class for knowledge-enabled action clients. From d8d405983d2a1b2d3a20b89075074ff956b9d28e Mon Sep 17 00:00:00 2001 From: Alex Mitrevski Date: Wed, 26 Dec 2018 00:27:49 +0100 Subject: [PATCH 21/96] Added a behaviours package and some common behaviours --- .../mdr_behaviours/CMakeLists.txt | 4 + .../mdr_behaviours/mdr_behaviours/package.xml | 17 +++ .../mdr_knowledge_behaviours/CMakeLists.txt | 23 ++++ .../mdr_knowledge_behaviours/package.xml | 23 ++++ .../src/mdr_knowledge_behaviours/__init__.py | 0 .../check_empty_surface.py | 16 +++ .../mdr_knowledge_behaviours/setup.py | 11 ++ .../CMakeLists.txt | 33 ++++++ .../mdr_manipulation_behaviours/package.xml | 33 ++++++ .../mdr_manipulation_behaviours/__init__.py | 0 .../pick_closest_from_surface.py | 111 ++++++++++++++++++ .../src/mdr_manipulation_behaviours/place.py | 77 ++++++++++++ .../place_based_on_category.py | 109 +++++++++++++++++ .../throw_object_in.py | 75 ++++++++++++ .../mdr_manipulation_behaviours/setup.py | 11 ++ .../mdr_navigation_behaviours/CMakeLists.txt | 27 +++++ .../mdr_navigation_behaviours/package.xml | 27 +++++ .../src/mdr_navigation_behaviours/__init__.py | 0 .../mdr_navigation_behaviours/move_base.py | 79 +++++++++++++ .../mdr_navigation_behaviours/setup.py | 11 ++ .../mdr_perception_behaviours/CMakeLists.txt | 29 +++++ .../mdr_perception_behaviours/package.xml | 29 +++++ .../src/mdr_perception_behaviours/__init__.py | 0 .../perceive_planes.py | 68 +++++++++++ .../mdr_perception_behaviours/setup.py | 11 ++ 25 files changed, 824 insertions(+) create mode 100644 mdr_planning/mdr_behaviours/mdr_behaviours/CMakeLists.txt create mode 100644 mdr_planning/mdr_behaviours/mdr_behaviours/package.xml create mode 100644 mdr_planning/mdr_behaviours/mdr_knowledge_behaviours/CMakeLists.txt create mode 100644 mdr_planning/mdr_behaviours/mdr_knowledge_behaviours/package.xml create mode 100644 mdr_planning/mdr_behaviours/mdr_knowledge_behaviours/ros/src/mdr_knowledge_behaviours/__init__.py create mode 100644 mdr_planning/mdr_behaviours/mdr_knowledge_behaviours/ros/src/mdr_knowledge_behaviours/check_empty_surface.py create mode 100644 mdr_planning/mdr_behaviours/mdr_knowledge_behaviours/setup.py create mode 100644 mdr_planning/mdr_behaviours/mdr_manipulation_behaviours/CMakeLists.txt create mode 100644 mdr_planning/mdr_behaviours/mdr_manipulation_behaviours/package.xml create mode 100644 mdr_planning/mdr_behaviours/mdr_manipulation_behaviours/ros/src/mdr_manipulation_behaviours/__init__.py create mode 100644 mdr_planning/mdr_behaviours/mdr_manipulation_behaviours/ros/src/mdr_manipulation_behaviours/pick_closest_from_surface.py create mode 100644 mdr_planning/mdr_behaviours/mdr_manipulation_behaviours/ros/src/mdr_manipulation_behaviours/place.py create mode 100644 mdr_planning/mdr_behaviours/mdr_manipulation_behaviours/ros/src/mdr_manipulation_behaviours/place_based_on_category.py create mode 100644 mdr_planning/mdr_behaviours/mdr_manipulation_behaviours/ros/src/mdr_manipulation_behaviours/throw_object_in.py create mode 100644 mdr_planning/mdr_behaviours/mdr_manipulation_behaviours/setup.py create mode 100644 mdr_planning/mdr_behaviours/mdr_navigation_behaviours/CMakeLists.txt create mode 100644 mdr_planning/mdr_behaviours/mdr_navigation_behaviours/package.xml create mode 100644 mdr_planning/mdr_behaviours/mdr_navigation_behaviours/ros/src/mdr_navigation_behaviours/__init__.py create mode 100644 mdr_planning/mdr_behaviours/mdr_navigation_behaviours/ros/src/mdr_navigation_behaviours/move_base.py create mode 100644 mdr_planning/mdr_behaviours/mdr_navigation_behaviours/setup.py create mode 100644 mdr_planning/mdr_behaviours/mdr_perception_behaviours/CMakeLists.txt create mode 100644 mdr_planning/mdr_behaviours/mdr_perception_behaviours/package.xml create mode 100644 mdr_planning/mdr_behaviours/mdr_perception_behaviours/ros/src/mdr_perception_behaviours/__init__.py create mode 100644 mdr_planning/mdr_behaviours/mdr_perception_behaviours/ros/src/mdr_perception_behaviours/perceive_planes.py create mode 100644 mdr_planning/mdr_behaviours/mdr_perception_behaviours/setup.py diff --git a/mdr_planning/mdr_behaviours/mdr_behaviours/CMakeLists.txt b/mdr_planning/mdr_behaviours/mdr_behaviours/CMakeLists.txt new file mode 100644 index 000000000..5db79dfe2 --- /dev/null +++ b/mdr_planning/mdr_behaviours/mdr_behaviours/CMakeLists.txt @@ -0,0 +1,4 @@ +cmake_minimum_required(VERSION 2.8.3) +project(mdr_behaviours) +find_package(catkin REQUIRED) +catkin_metapackage() diff --git a/mdr_planning/mdr_behaviours/mdr_behaviours/package.xml b/mdr_planning/mdr_behaviours/mdr_behaviours/package.xml new file mode 100644 index 000000000..910c1c706 --- /dev/null +++ b/mdr_planning/mdr_behaviours/mdr_behaviours/package.xml @@ -0,0 +1,17 @@ + + + mdr_behaviours + 1.2.0 + Metapackage for packages defining common domestic robot behaviours. + + GPLv3 + + Alex Mitrevski + MAS robotics + + catkin + + + + + diff --git a/mdr_planning/mdr_behaviours/mdr_knowledge_behaviours/CMakeLists.txt b/mdr_planning/mdr_behaviours/mdr_knowledge_behaviours/CMakeLists.txt new file mode 100644 index 000000000..64149274e --- /dev/null +++ b/mdr_planning/mdr_behaviours/mdr_knowledge_behaviours/CMakeLists.txt @@ -0,0 +1,23 @@ +cmake_minimum_required(VERSION 2.8.3) +project(mdr_knowledge_behaviours) + +find_package(catkin REQUIRED COMPONENTS + rospy + roslint + smach + mas_execution_manager +) + +roslint_python() +catkin_python_setup() + +catkin_package( + CATKIN_DEPENDS + rospy + smach + mas_execution_manager +) + +include_directories( + ${catkin_INCLUDE_DIRS} +) diff --git a/mdr_planning/mdr_behaviours/mdr_knowledge_behaviours/package.xml b/mdr_planning/mdr_behaviours/mdr_knowledge_behaviours/package.xml new file mode 100644 index 000000000..f7b45e7bb --- /dev/null +++ b/mdr_planning/mdr_behaviours/mdr_knowledge_behaviours/package.xml @@ -0,0 +1,23 @@ + + + mdr_knowledge_behaviours + 1.2.0 + A package for common knowledge retrieval robot behaviours. + + Alex Mitrevski + MAS robotics + + GPLv3 + + catkin + rospy + roslint + smach + mas_execution_manager + + rospy + smach + mas_execution_manager + + + diff --git a/mdr_planning/mdr_behaviours/mdr_knowledge_behaviours/ros/src/mdr_knowledge_behaviours/__init__.py b/mdr_planning/mdr_behaviours/mdr_knowledge_behaviours/ros/src/mdr_knowledge_behaviours/__init__.py new file mode 100644 index 000000000..e69de29bb diff --git a/mdr_planning/mdr_behaviours/mdr_knowledge_behaviours/ros/src/mdr_knowledge_behaviours/check_empty_surface.py b/mdr_planning/mdr_behaviours/mdr_knowledge_behaviours/ros/src/mdr_knowledge_behaviours/check_empty_surface.py new file mode 100644 index 000000000..49f6e46d7 --- /dev/null +++ b/mdr_planning/mdr_behaviours/mdr_knowledge_behaviours/ros/src/mdr_knowledge_behaviours/check_empty_surface.py @@ -0,0 +1,16 @@ +from mas_execution_manager.scenario_state_base import ScenarioStateBase + +class CheckEmptySurface(ScenarioStateBase): + def __init__(self, save_sm_state=False, **kwargs): + ScenarioStateBase.__init__(self, 'check_empty_surface', + save_sm_state=save_sm_state, + outcomes=['empty', 'not_empty']) + self.sm_id = kwargs.get('sm_id', '') + self.state_name = kwargs.get('state_name', 'check_empty_surface') + self.surface_prefix = kwargs.get('surface_prefix', '') + + def execute(self, userdata): + if self.kb_interface.is_surface_empty(self.surface_prefix): + self.say('{0} is empty'.format(self.surface_prefix)) + return 'empty' + return 'not_empty' diff --git a/mdr_planning/mdr_behaviours/mdr_knowledge_behaviours/setup.py b/mdr_planning/mdr_behaviours/mdr_knowledge_behaviours/setup.py new file mode 100644 index 000000000..0b04f3a10 --- /dev/null +++ b/mdr_planning/mdr_behaviours/mdr_knowledge_behaviours/setup.py @@ -0,0 +1,11 @@ +#!/usr/bin/env python + +from distutils.core import setup +from catkin_pkg.python_setup import generate_distutils_setup + +d = generate_distutils_setup( + packages=['mdr_knowledge_behaviours'], + package_dir={'mdr_knowledge_behaviours': 'ros/src/mdr_knowledge_behaviours'} +) + +setup(**d) diff --git a/mdr_planning/mdr_behaviours/mdr_manipulation_behaviours/CMakeLists.txt b/mdr_planning/mdr_behaviours/mdr_manipulation_behaviours/CMakeLists.txt new file mode 100644 index 000000000..5be429abb --- /dev/null +++ b/mdr_planning/mdr_behaviours/mdr_manipulation_behaviours/CMakeLists.txt @@ -0,0 +1,33 @@ +cmake_minimum_required(VERSION 2.8.3) +project(mdr_manipulation_behaviours) + +find_package(catkin REQUIRED COMPONENTS + rospy + roslint + smach + mas_execution_manager + mdr_move_arm_action + mdr_move_base_action + mdr_perceive_plane_action + mdr_pickup_action + mdr_place_action +) + +roslint_python() +catkin_python_setup() + +catkin_package( + CATKIN_DEPENDS + rospy + smach + mas_execution_manager + mdr_move_arm_action + mdr_move_base_action + mdr_perceive_plane_action + mdr_pickup_action + mdr_place_action +) + +include_directories( + ${catkin_INCLUDE_DIRS} +) diff --git a/mdr_planning/mdr_behaviours/mdr_manipulation_behaviours/package.xml b/mdr_planning/mdr_behaviours/mdr_manipulation_behaviours/package.xml new file mode 100644 index 000000000..920b47716 --- /dev/null +++ b/mdr_planning/mdr_behaviours/mdr_manipulation_behaviours/package.xml @@ -0,0 +1,33 @@ + + + mdr_manipulation_behaviours + 1.2.0 + A package for common manipulation-related robot behaviours. + + Alex Mitrevski + MAS robotics + + GPLv3 + + catkin + rospy + roslint + smach + mas_execution_manager + mdr_move_arm_action + mdr_move_base_action + mdr_perceive_plane_action + mdr_pickup_action + mdr_place_action + + rospy + smach + mas_execution_manager + mdr_move_arm_action + mdr_move_base_action + mdr_perceive_plane_action + mdr_pickup_action + mdr_place_action + + + diff --git a/mdr_planning/mdr_behaviours/mdr_manipulation_behaviours/ros/src/mdr_manipulation_behaviours/__init__.py b/mdr_planning/mdr_behaviours/mdr_manipulation_behaviours/ros/src/mdr_manipulation_behaviours/__init__.py new file mode 100644 index 000000000..e69de29bb diff --git a/mdr_planning/mdr_behaviours/mdr_manipulation_behaviours/ros/src/mdr_manipulation_behaviours/pick_closest_from_surface.py b/mdr_planning/mdr_behaviours/mdr_manipulation_behaviours/ros/src/mdr_manipulation_behaviours/pick_closest_from_surface.py new file mode 100644 index 000000000..b87831fe8 --- /dev/null +++ b/mdr_planning/mdr_behaviours/mdr_manipulation_behaviours/ros/src/mdr_manipulation_behaviours/pick_closest_from_surface.py @@ -0,0 +1,111 @@ +import time +import numpy as np +import rospy +from tf import TransformListener + +import rosplan_dispatch_msgs.msg as plan_dispatch_msgs +import diagnostic_msgs.msg as diag_msgs + +from mcr_perception_msgs.msg import Object +from mas_execution_manager.scenario_state_base import ScenarioStateBase + +class PickClosestFromSurface(ScenarioStateBase): + def __init__(self, save_sm_state=False, **kwargs): + ScenarioStateBase.__init__(self, 'pickup', + save_sm_state=save_sm_state, + output_keys=['grasped_object'], + outcomes=['succeeded', 'failed', + 'failed_after_retrying', + 'find_objects_before_picking']) + self.sm_id = kwargs.get('sm_id', '') + self.state_name = kwargs.get('state_name', 'pick') + self.timeout = kwargs.get('timeout', 120.) + self.picking_surface_prefix = kwargs.get('picking_surface_prefix', 120.) + self.tf_listener = TransformListener() + + self.number_of_retries = kwargs.get('number_of_retries', 0) + self.retry_count = 0 + + def execute(self, userdata): + if self.save_sm_state: + self.save_current_state() + + surface_objects = self.kb_interface.get_surface_object_map(self.picking_surface_prefix) + object_poses = self.kb_interface.get_surface_object_pose_map(surface_objects, Object._type) + surface, obj_to_grasp = self.select_object_for_grasping(object_poses) + if not obj_to_grasp: + rospy.logerr('Could not find an object to grasp') + self.say('Could not find an object to grasp') + return 'find_objects_before_picking' + + dispatch_msg = self.get_dispatch_msg(obj_to_grasp, surface) + rospy.loginfo('Picking %s from %s' % (obj_to_grasp, surface)) + self.say('Picking ' + obj_to_grasp + ' from ' + surface) + self.action_dispatch_pub.publish(dispatch_msg) + + self.executing = True + self.succeeded = False + start_time = time.time() + duration = 0. + while self.executing and duration < self.timeout: + rospy.sleep(0.1) + duration = time.time() - start_time + + if self.succeeded: + rospy.loginfo('%s grasped successfully' % obj_to_grasp) + self.say('Successfully grasped ' + obj_to_grasp) + userdata.grasped_object = obj_to_grasp + return 'succeeded' + + rospy.loginfo('Could not grasp %s' % obj_to_grasp) + self.say('Could not grasp ' + obj_to_grasp) + if self.retry_count == self.number_of_retries: + rospy.loginfo('Failed to grasp %s' % obj_to_grasp) + self.say('Aborting operation') + return 'failed_after_retrying' + rospy.loginfo('Retrying to grasp %s' % obj_to_grasp) + self.retry_count += 1 + return 'failed' + + def select_object_for_grasping(self, surface_object_poses): + '''Returns the index of the object whose distance is closest to the robot + ''' + robot_position = np.zeros(3) + min_dist = 1e100 + min_dist_obj = '' + obj_surface = '' + for surface, object_poses in surface_object_poses.items(): + for obj, pose in object_poses.items(): + base_link_pose = self.tf_listener.transformPose('base_link', pose) + dist = self.distance(robot_position, np.array([base_link_pose.pose.position.x, + base_link_pose.pose.position.y, + base_link_pose.pose.position.z])) + if dist < min_dist: + min_dist = dist + min_dist_obj = obj + obj_surface = surface + return obj_surface, min_dist_obj + + def distance(self, point1, point2): + return np.linalg.norm(np.array(point1) - np.array(point2)) + + def get_dispatch_msg(self, obj_name, surface_name): + dispatch_msg = plan_dispatch_msgs.ActionDispatch() + dispatch_msg.name = self.action_name + + arg_msg = diag_msgs.KeyValue() + arg_msg.key = 'bot' + arg_msg.value = self.robot_name + dispatch_msg.parameters.append(arg_msg) + + arg_msg = diag_msgs.KeyValue() + arg_msg.key = 'obj' + arg_msg.value = obj_name + dispatch_msg.parameters.append(arg_msg) + + arg_msg = diag_msgs.KeyValue() + arg_msg.key = 'plane' + arg_msg.value = surface_name + dispatch_msg.parameters.append(arg_msg) + + return dispatch_msg diff --git a/mdr_planning/mdr_behaviours/mdr_manipulation_behaviours/ros/src/mdr_manipulation_behaviours/place.py b/mdr_planning/mdr_behaviours/mdr_manipulation_behaviours/ros/src/mdr_manipulation_behaviours/place.py new file mode 100644 index 000000000..c57f965d8 --- /dev/null +++ b/mdr_planning/mdr_behaviours/mdr_manipulation_behaviours/ros/src/mdr_manipulation_behaviours/place.py @@ -0,0 +1,77 @@ +import time +import rospy + +import rosplan_dispatch_msgs.msg as plan_dispatch_msgs +import diagnostic_msgs.msg as diag_msgs + +from mas_execution_manager.scenario_state_base import ScenarioStateBase + +class Place(ScenarioStateBase): + def __init__(self, save_sm_state=False, **kwargs): + ScenarioStateBase.__init__(self, 'place', + save_sm_state=save_sm_state, + input_keys=['grasped_object'], + outcomes=['succeeded', 'failed', + 'failed_after_retrying']) + self.sm_id = kwargs.get('sm_id', '') + self.state_name = kwargs.get('state_name', 'place') + self.placing_surface_prefix = kwargs.get('placing_surface_prefix', '') + self.timeout = kwargs.get('timeout', 120.) + + self.number_of_retries = kwargs.get('number_of_retries', 0) + self.retry_count = 0 + + def execute(self, userdata): + if self.save_sm_state: + self.save_current_state() + + grasped_object = userdata.grasped_object + surface_name = self.kb_interface.get_surface_name(self.placing_surface_prefix) + dispatch_msg = self.get_dispatch_msg(grasped_object, surface_name) + rospy.loginfo('Placing %s on %s' % (grasped_object, surface_name)) + self.say('Placing ' + grasped_object + ' on ' + surface_name) + self.action_dispatch_pub.publish(dispatch_msg) + + self.executing = True + self.succeeded = False + start_time = time.time() + duration = 0. + while self.executing and duration < self.timeout: + rospy.sleep(0.1) + duration = time.time() - start_time + + if self.succeeded: + rospy.loginfo('Object placed successfully') + self.say('Successfully placed ' + grasped_object) + return 'succeeded' + + rospy.loginfo('Could not place object %s' % grasped_object) + self.say('Could not place ' + grasped_object) + if self.retry_count == self.number_of_retries: + rospy.loginfo('Failed to place object %s' % grasped_object) + self.say('Aborting operation') + return 'failed_after_retrying' + rospy.loginfo('Retrying to place object %s' % grasped_object) + self.retry_count += 1 + return 'failed' + + def get_dispatch_msg(self, obj_name, plane_name): + dispatch_msg = plan_dispatch_msgs.ActionDispatch() + dispatch_msg.name = self.action_name + + arg_msg = diag_msgs.KeyValue() + arg_msg.key = 'bot' + arg_msg.value = self.robot_name + dispatch_msg.parameters.append(arg_msg) + + arg_msg = diag_msgs.KeyValue() + arg_msg.key = 'obj' + arg_msg.value = obj_name + dispatch_msg.parameters.append(arg_msg) + + arg_msg = diag_msgs.KeyValue() + arg_msg.key = 'plane' + arg_msg.value = plane_name + dispatch_msg.parameters.append(arg_msg) + + return dispatch_msg diff --git a/mdr_planning/mdr_behaviours/mdr_manipulation_behaviours/ros/src/mdr_manipulation_behaviours/place_based_on_category.py b/mdr_planning/mdr_behaviours/mdr_manipulation_behaviours/ros/src/mdr_manipulation_behaviours/place_based_on_category.py new file mode 100644 index 000000000..7c2b995c8 --- /dev/null +++ b/mdr_planning/mdr_behaviours/mdr_manipulation_behaviours/ros/src/mdr_manipulation_behaviours/place_based_on_category.py @@ -0,0 +1,109 @@ +import time +import numpy as np +import rospy + +import rosplan_dispatch_msgs.msg as plan_dispatch_msgs +import diagnostic_msgs.msg as diag_msgs + +from mas_execution_manager.scenario_state_base import ScenarioStateBase + +class PlaceBasedOnCategory(ScenarioStateBase): + def __init__(self, save_sm_state=False, **kwargs): + ScenarioStateBase.__init__(self, 'place_based_on_category', + save_sm_state=save_sm_state, + input_keys=['grasped_object'], + outcomes=['succeeded', 'failed', + 'failed_after_retrying']) + self.sm_id = kwargs.get('sm_id', '') + self.state_name = kwargs.get('state_name', 'place_based_on_category') + self.placing_surface_prefix = kwargs.get('placing_surface_prefix', '') + self.timeout = kwargs.get('timeout', 120.) + + self.number_of_retries = kwargs.get('number_of_retries', 0) + self.retry_count = 0 + + def execute(self, userdata): + if self.save_sm_state: + self.save_current_state() + + grasped_object = userdata.grasped_object + surface_name = self.choose_placing_surface(grasped_object) + dispatch_msg = self.get_dispatch_msg(grasped_object, surface_name) + rospy.loginfo('Placing %s on %s' % (grasped_object, surface_name)) + self.say('Placing ' + grasped_object + ' on ' + surface_name) + self.action_dispatch_pub.publish(dispatch_msg) + + self.executing = True + self.succeeded = False + start_time = time.time() + duration = 0. + while self.executing and duration < self.timeout: + rospy.sleep(0.1) + duration = time.time() - start_time + + if self.succeeded: + rospy.loginfo('Object placed successfully') + self.say('Successfully placed ' + grasped_object) + return 'succeeded' + + rospy.loginfo('Could not place object %s' % grasped_object) + self.say('Could not place ' + grasped_object) + if self.retry_count == self.number_of_retries: + rospy.loginfo('Failed to place object %s' % grasped_object) + self.say('Aborting operation') + return 'failed_after_retrying' + rospy.loginfo('Retrying to place object %s' % grasped_object) + self.retry_count += 1 + return 'failed' + + def choose_placing_surface(self, obj_name): + obj_category_map = self.kb_interface.get_obj_category_map() + surface_category_counts = self.kb_interface.get_surface_category_counts(self.placing_surface_prefix, + obj_category_map) + grasped_object_category = obj_category_map[obj_name] + placing_surface = self.get_best_placing_surface(grasped_object_category, + surface_category_counts) + return placing_surface + + def get_best_placing_surface(self, obj_category, surface_category_counts): + '''If none of the surfaces contain objects whose category is + 'obj_category', returns the name of a random surface; otherwise, + returns the name of the surface that has the largest number + of objects of the category 'obj_category'. + ''' + surfaces = list() + obj_surface_category_counts = list() + for surface, category_counts in surface_category_counts.items(): + surfaces.append(surface) + if obj_category in category_counts: + obj_surface_category_counts.append(category_counts[obj_category]) + else: + obj_surface_category_counts.append(0) + + surface_idx = -1 + if np.count_nonzero(obj_surface_category_counts) != 0: + surface_idx = np.argmax(obj_surface_category_counts) + else: + surface_idx = np.random.randint(0, len(surfaces)) + return surfaces[surface_idx] + + def get_dispatch_msg(self, obj_name, plane_name): + dispatch_msg = plan_dispatch_msgs.ActionDispatch() + dispatch_msg.name = self.action_name + + arg_msg = diag_msgs.KeyValue() + arg_msg.key = 'bot' + arg_msg.value = self.robot_name + dispatch_msg.parameters.append(arg_msg) + + arg_msg = diag_msgs.KeyValue() + arg_msg.key = 'obj' + arg_msg.value = obj_name + dispatch_msg.parameters.append(arg_msg) + + arg_msg = diag_msgs.KeyValue() + arg_msg.key = 'plane' + arg_msg.value = plane_name + dispatch_msg.parameters.append(arg_msg) + + return dispatch_msg diff --git a/mdr_planning/mdr_behaviours/mdr_manipulation_behaviours/ros/src/mdr_manipulation_behaviours/throw_object_in.py b/mdr_planning/mdr_behaviours/mdr_manipulation_behaviours/ros/src/mdr_manipulation_behaviours/throw_object_in.py new file mode 100644 index 000000000..1508810fe --- /dev/null +++ b/mdr_planning/mdr_behaviours/mdr_manipulation_behaviours/ros/src/mdr_manipulation_behaviours/throw_object_in.py @@ -0,0 +1,75 @@ +import time +import rospy + +import rosplan_dispatch_msgs.msg as plan_dispatch_msgs +import diagnostic_msgs.msg as diag_msgs + +from mas_execution_manager.scenario_state_base import ScenarioStateBase + +class ThrowObjectIn(ScenarioStateBase): + def __init__(self, save_sm_state=False, **kwargs): + ScenarioStateBase.__init__(self, 'throw', + save_sm_state=save_sm_state, + input_keys=['grasped_object'], + outcomes=['succeeded', 'failed', 'failed_after_retrying']) + self.sm_id = kwargs.get('sm_id', '') + self.state_name = kwargs.get('state_name', 'throw') + self.throwing_target_name = kwargs.get('throwing_target_name', '') + self.timeout = kwargs.get('timeout', 120.) + + self.number_of_retries = kwargs.get('number_of_retries', 0) + self.retry_count = 0 + + def execute(self, userdata): + if self.save_sm_state: + self.save_current_state() + + grasped_object = userdata.grasped_object + dispatch_msg = self.get_dispatch_msg(grasped_object) + rospy.loginfo('Throwing %s in %s' % (grasped_object, self.throwing_target_name)) + self.say('Throwing {0} in {1}'.format(grasped_object, self.throwing_target_name)) + self.action_dispatch_pub.publish(dispatch_msg) + + self.executing = True + self.succeeded = False + start_time = time.time() + duration = 0. + while self.executing and duration < self.timeout: + rospy.sleep(0.1) + duration = time.time() - start_time + + if self.succeeded: + rospy.loginfo('Object discarded successfully') + self.say('Successfully discarded ' + grasped_object) + return 'succeeded' + + rospy.loginfo('Could not discard object %s' % grasped_object) + self.say('Could not discard ' + grasped_object) + if self.retry_count == self.number_of_retries: + rospy.loginfo('Failed to discard object %s' % grasped_object) + self.say('Aborting operation') + return 'failed_after_retrying' + rospy.loginfo('Retrying to discard object %s' % grasped_object) + self.retry_count += 1 + return 'failed' + + def get_dispatch_msg(self, obj_name): + dispatch_msg = plan_dispatch_msgs.ActionDispatch() + dispatch_msg.name = self.action_name + + arg_msg = diag_msgs.KeyValue() + arg_msg.key = 'bot' + arg_msg.value = self.robot_name + dispatch_msg.parameters.append(arg_msg) + + arg_msg = diag_msgs.KeyValue() + arg_msg.key = 'obj' + arg_msg.value = obj_name + dispatch_msg.parameters.append(arg_msg) + + arg_msg = diag_msgs.KeyValue() + arg_msg.key = 'throwing_target' + arg_msg.value = self.throwing_target_name + dispatch_msg.parameters.append(arg_msg) + + return dispatch_msg diff --git a/mdr_planning/mdr_behaviours/mdr_manipulation_behaviours/setup.py b/mdr_planning/mdr_behaviours/mdr_manipulation_behaviours/setup.py new file mode 100644 index 000000000..0fdc3c40d --- /dev/null +++ b/mdr_planning/mdr_behaviours/mdr_manipulation_behaviours/setup.py @@ -0,0 +1,11 @@ +#!/usr/bin/env python + +from distutils.core import setup +from catkin_pkg.python_setup import generate_distutils_setup + +d = generate_distutils_setup( + packages=['mdr_manipulation_behaviours'], + package_dir={'mdr_manipulation_behaviours': 'ros/src/mdr_manipulation_behaviours'} +) + +setup(**d) diff --git a/mdr_planning/mdr_behaviours/mdr_navigation_behaviours/CMakeLists.txt b/mdr_planning/mdr_behaviours/mdr_navigation_behaviours/CMakeLists.txt new file mode 100644 index 000000000..ed5b4ddf3 --- /dev/null +++ b/mdr_planning/mdr_behaviours/mdr_navigation_behaviours/CMakeLists.txt @@ -0,0 +1,27 @@ +cmake_minimum_required(VERSION 2.8.3) +project(mdr_navigation_behaviours) + +find_package(catkin REQUIRED COMPONENTS + rospy + roslint + smach + mas_execution_manager + mdr_move_arm_action + mdr_move_base_action +) + +roslint_python() +catkin_python_setup() + +catkin_package( + CATKIN_DEPENDS + rospy + smach + mas_execution_manager + mdr_move_arm_action + mdr_move_base_action +) + +include_directories( + ${catkin_INCLUDE_DIRS} +) diff --git a/mdr_planning/mdr_behaviours/mdr_navigation_behaviours/package.xml b/mdr_planning/mdr_behaviours/mdr_navigation_behaviours/package.xml new file mode 100644 index 000000000..913b5ba2e --- /dev/null +++ b/mdr_planning/mdr_behaviours/mdr_navigation_behaviours/package.xml @@ -0,0 +1,27 @@ + + + mdr_navigation_behaviours + 1.2.0 + A package for common navigation-related robot behaviours. + + Alex Mitrevski + MAS robotics + + GPLv3 + + catkin + rospy + roslint + smach + mas_execution_manager + mdr_move_arm_action + mdr_move_base_action + + rospy + smach + mas_execution_manager + mdr_move_arm_action + mdr_move_base_action + + + diff --git a/mdr_planning/mdr_behaviours/mdr_navigation_behaviours/ros/src/mdr_navigation_behaviours/__init__.py b/mdr_planning/mdr_behaviours/mdr_navigation_behaviours/ros/src/mdr_navigation_behaviours/__init__.py new file mode 100644 index 000000000..e69de29bb diff --git a/mdr_planning/mdr_behaviours/mdr_navigation_behaviours/ros/src/mdr_navigation_behaviours/move_base.py b/mdr_planning/mdr_behaviours/mdr_navigation_behaviours/ros/src/mdr_navigation_behaviours/move_base.py new file mode 100644 index 000000000..d248a1f27 --- /dev/null +++ b/mdr_planning/mdr_behaviours/mdr_navigation_behaviours/ros/src/mdr_navigation_behaviours/move_base.py @@ -0,0 +1,79 @@ +import time +import rospy +import actionlib + +import rosplan_dispatch_msgs.msg as plan_dispatch_msgs +import diagnostic_msgs.msg as diag_msgs + +from mdr_move_base_action.msg import MoveBaseAction +from mas_execution_manager.scenario_state_base import ScenarioStateBase + +class MoveBase(ScenarioStateBase): + def __init__(self, save_sm_state=False, **kwargs): + ScenarioStateBase.__init__(self, 'move_base', + save_sm_state=save_sm_state, + outcomes=['succeeded', 'failed', 'failed_after_retrying']) + self.sm_id = kwargs.get('sm_id', '') + self.state_name = kwargs.get('state_name', 'move_base') + self.move_base_server = kwargs.get('move_base_server', 'move_base_server') + self.destination_locations = list(kwargs.get('destination_locations', list())) + self.timeout = kwargs.get('timeout', 120.) + + self.number_of_retries = kwargs.get('number_of_retries', 0) + self.retry_count = 0 + + self.move_base_client = actionlib.SimpleActionClient(self.move_base_server, + MoveBaseAction) + self.move_base_client.wait_for_server() + + def execute(self, userdata): + original_location = self.kb_interface.get_robot_location(self.robot_name) + for destination_location in self.destination_locations: + dispatch_msg = self.get_dispatch_msg(original_location, + destination_location) + + rospy.loginfo('Sending the base to %s' % destination_location) + self.say('Going to ' + destination_location) + self.action_dispatch_pub.publish(dispatch_msg) + + self.executing = True + self.succeeded = False + start_time = time.time() + duration = 0. + while self.executing and duration < self.timeout: + rospy.sleep(0.1) + duration = time.time() - start_time + + if self.succeeded: + rospy.loginfo('Successfully reached %s' % destination_location) + original_location = destination_location + else: + rospy.logerr('Could not reach %s' % destination_location) + self.say('Could not reach ' + destination_location) + if self.retry_count == self.number_of_retries: + self.say('Aborting operation') + return 'failed_after_retrying' + self.retry_count += 1 + return 'failed' + return 'succeeded' + + def get_dispatch_msg(self, original_location, destination_location): + dispatch_msg = plan_dispatch_msgs.ActionDispatch() + dispatch_msg.name = self.action_name + + arg_msg = diag_msgs.KeyValue() + arg_msg.key = 'bot' + arg_msg.value = self.robot_name + dispatch_msg.parameters.append(arg_msg) + + arg_msg = diag_msgs.KeyValue() + arg_msg.key = 'from' + arg_msg.value = original_location + dispatch_msg.parameters.append(arg_msg) + + arg_msg = diag_msgs.KeyValue() + arg_msg.key = 'to' + arg_msg.value = destination_location + dispatch_msg.parameters.append(arg_msg) + + return dispatch_msg diff --git a/mdr_planning/mdr_behaviours/mdr_navigation_behaviours/setup.py b/mdr_planning/mdr_behaviours/mdr_navigation_behaviours/setup.py new file mode 100644 index 000000000..a6a28bc37 --- /dev/null +++ b/mdr_planning/mdr_behaviours/mdr_navigation_behaviours/setup.py @@ -0,0 +1,11 @@ +#!/usr/bin/env python + +from distutils.core import setup +from catkin_pkg.python_setup import generate_distutils_setup + +d = generate_distutils_setup( + packages=['mdr_navigation_behaviours'], + package_dir={'mdr_navigation_behaviours': 'ros/src/mdr_navigation_behaviours'} +) + +setup(**d) diff --git a/mdr_planning/mdr_behaviours/mdr_perception_behaviours/CMakeLists.txt b/mdr_planning/mdr_behaviours/mdr_perception_behaviours/CMakeLists.txt new file mode 100644 index 000000000..283fd5b9e --- /dev/null +++ b/mdr_planning/mdr_behaviours/mdr_perception_behaviours/CMakeLists.txt @@ -0,0 +1,29 @@ +cmake_minimum_required(VERSION 2.8.3) +project(mdr_perception_behaviours) + +find_package(catkin REQUIRED COMPONENTS + rospy + roslint + smach + mas_execution_manager + mdr_move_arm_action + mdr_move_base_action + mdr_perceive_plane_action +) + +roslint_python() +catkin_python_setup() + +catkin_package( + CATKIN_DEPENDS + rospy + smach + mas_execution_manager + mdr_move_arm_action + mdr_move_base_action + mdr_perceive_plane_action +) + +include_directories( + ${catkin_INCLUDE_DIRS} +) diff --git a/mdr_planning/mdr_behaviours/mdr_perception_behaviours/package.xml b/mdr_planning/mdr_behaviours/mdr_perception_behaviours/package.xml new file mode 100644 index 000000000..9619b511d --- /dev/null +++ b/mdr_planning/mdr_behaviours/mdr_perception_behaviours/package.xml @@ -0,0 +1,29 @@ + + + mdr_perception_behaviours + 1.2.0 + A package for common perception-related robot behaviours. + + Alex Mitrevski + MAS robotics + + GPLv3 + + catkin + rospy + roslint + smach + mas_execution_manager + mdr_move_arm_action + mdr_move_base_action + mdr_perceive_plane_action + + rospy + smach + mas_execution_manager + mdr_move_arm_action + mdr_move_base_action + mdr_perceive_plane_action + + + diff --git a/mdr_planning/mdr_behaviours/mdr_perception_behaviours/ros/src/mdr_perception_behaviours/__init__.py b/mdr_planning/mdr_behaviours/mdr_perception_behaviours/ros/src/mdr_perception_behaviours/__init__.py new file mode 100644 index 000000000..e69de29bb diff --git a/mdr_planning/mdr_behaviours/mdr_perception_behaviours/ros/src/mdr_perception_behaviours/perceive_planes.py b/mdr_planning/mdr_behaviours/mdr_perception_behaviours/ros/src/mdr_perception_behaviours/perceive_planes.py new file mode 100644 index 000000000..bf4cd1ad3 --- /dev/null +++ b/mdr_planning/mdr_behaviours/mdr_perception_behaviours/ros/src/mdr_perception_behaviours/perceive_planes.py @@ -0,0 +1,68 @@ +import time +import rospy + +import rosplan_dispatch_msgs.msg as plan_dispatch_msgs +import diagnostic_msgs.msg as diag_msgs + +from mas_execution_manager.scenario_state_base import ScenarioStateBase + +class PerceivePlanes(ScenarioStateBase): + def __init__(self, save_sm_state=False, **kwargs): + ScenarioStateBase.__init__(self, 'perceive_plane', + save_sm_state=save_sm_state, + outcomes=['succeeded', 'failed', 'failed_after_retrying']) + self.sm_id = kwargs.get('sm_id', '') + self.state_name = kwargs.get('state_name', 'perceive_planes') + self.timeout = kwargs.get('timeout', 120.) + self.plane_prefix = kwargs.get('plane_prefix', 0) + + self.number_of_retries = kwargs.get('number_of_retries', 0) + self.retry_count = 0 + + def execute(self, userdata): + if self.save_sm_state: + self.save_current_state() + + dispatch_msg = self.get_dispatch_msg(self.plane_prefix) + rospy.loginfo('Perceiving %s' % self.plane_prefix) + self.say('Perceiving ' + self.plane_prefix) + self.action_dispatch_pub.publish(dispatch_msg) + + self.executing = True + self.succeeded = False + start_time = time.time() + duration = 0. + while self.executing and duration < self.timeout: + rospy.sleep(0.1) + duration = time.time() - start_time + + if self.succeeded: + self.say('%s perceived' % self.plane_prefix) + rospy.loginfo('%s perceived successfully' % self.plane_prefix) + return 'succeeded' + + rospy.loginfo('Could not perceive %s' % self.plane_prefix) + self.say('Could not perceive ' + self.plane_prefix) + if self.retry_count == self.number_of_retries: + rospy.loginfo('Failed to perceive %s' % self.plane_prefix) + self.say('Aborting operation') + return 'failed_after_retrying' + rospy.loginfo('Retrying to perceive %s' % self.plane_prefix) + self.retry_count += 1 + return 'failed' + + def get_dispatch_msg(self, plane_name): + dispatch_msg = plan_dispatch_msgs.ActionDispatch() + dispatch_msg.name = self.action_name + + arg_msg = diag_msgs.KeyValue() + arg_msg.key = 'bot' + arg_msg.value = self.robot_name + dispatch_msg.parameters.append(arg_msg) + + arg_msg = diag_msgs.KeyValue() + arg_msg.key = 'plane' + arg_msg.value = plane_name + dispatch_msg.parameters.append(arg_msg) + + return dispatch_msg diff --git a/mdr_planning/mdr_behaviours/mdr_perception_behaviours/setup.py b/mdr_planning/mdr_behaviours/mdr_perception_behaviours/setup.py new file mode 100644 index 000000000..3ab2b293c --- /dev/null +++ b/mdr_planning/mdr_behaviours/mdr_perception_behaviours/setup.py @@ -0,0 +1,11 @@ +#!/usr/bin/env python + +from distutils.core import setup +from catkin_pkg.python_setup import generate_distutils_setup + +d = generate_distutils_setup( + packages=['mdr_perception_behaviours'], + package_dir={'mdr_perception_behaviours': 'ros/src/mdr_perception_behaviours'} +) + +setup(**d) From 0cf04fdd235f7437be949f99e110e6649b647f4b Mon Sep 17 00:00:00 2001 From: Alex Mitrevski Date: Wed, 26 Dec 2018 00:30:44 +0100 Subject: [PATCH 22/96] [store_groceries] Most of the states are now reused from mdr_behaviours --- .../mdr_store_groceries/CMakeLists.txt | 19 +- .../config/store_groceries_sm.yaml | 56 +++-- .../mdr_store_groceries/package.xml | 26 +-- .../scenario_states/move_base.py | 127 ----------- .../scenario_states/perceive_planes.py | 69 ------ .../scenario_states/pick.py | 167 --------------- .../scenario_states/place.py | 202 ------------------ 7 files changed, 51 insertions(+), 615 deletions(-) delete mode 100644 mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_store_groceries/ros/src/mdr_store_groceries/scenario_states/move_base.py delete mode 100644 mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_store_groceries/ros/src/mdr_store_groceries/scenario_states/perceive_planes.py delete mode 100644 mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_store_groceries/ros/src/mdr_store_groceries/scenario_states/pick.py delete mode 100644 mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_store_groceries/ros/src/mdr_store_groceries/scenario_states/place.py diff --git a/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_store_groceries/CMakeLists.txt b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_store_groceries/CMakeLists.txt index e3a48caae..97198a92d 100644 --- a/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_store_groceries/CMakeLists.txt +++ b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_store_groceries/CMakeLists.txt @@ -2,13 +2,8 @@ cmake_minimum_required(VERSION 2.8.3) project(mdr_store_groceries) find_package(catkin REQUIRED COMPONENTS - roslint rospy - smach_ros - smach - actionlib - mongodb_store - mdr_rosplan_interface + roslint mas_execution_manager mdr_listen_action mdr_process_speech_command_action @@ -17,17 +12,14 @@ find_package(catkin REQUIRED COMPONENTS mdr_pickup_action mdr_place_action mdr_perceive_plane_action + mdr_navigation_behaviours + mdr_perception_behaviours + mdr_manipulation_behaviours ) catkin_package( CATKIN_DEPENDS - roslint rospy - smach_ros - smach - actionlib - mongodb_store - mdr_rosplan_interface mas_execution_manager mdr_listen_action mdr_process_speech_command_action @@ -36,6 +28,9 @@ catkin_package( mdr_pickup_action mdr_place_action mdr_perceive_plane_action + mdr_navigation_behaviours + mdr_perception_behaviours + mdr_manipulation_behaviours ) catkin_python_setup() diff --git a/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_store_groceries/config/store_groceries_sm.yaml b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_store_groceries/config/store_groceries_sm.yaml index 853390bb5..95f8aa53f 100644 --- a/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_store_groceries/config/store_groceries_sm.yaml +++ b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_store_groceries/config/store_groceries_sm.yaml @@ -2,7 +2,7 @@ # Author: Alex Mitrevski # Email: aleksandar.mitrevski@h-brs.de sm_id: mdr_store_groceries -states: [LISTEN, PROCESS_SPEECH_COMMAND, ENTER, GO_TO_SCANNING_POSE, FIND_TABLE, GO_TO_CUPBOARD, OPEN_DOOR, SCAN_CUPBOARD, GO_TO_TABLE, PERCEIVE_TABLE, PICK_OBJECT, FIND_OBJECTS_BEFORE_PICKING, GO_BACK_TO_CUPBOARD, PLACE_OBJECT, GO_BACK_TO_TABLE, EXIT] +states: [LISTEN, PROCESS_SPEECH_COMMAND, ENTER, GO_TO_SCANNING_POSE, FIND_TABLE, GO_TO_CUPBOARD, OPEN_DOOR, SCAN_CUPBOARD, GO_TO_TABLE, PERCEIVE_TABLE, PICK_OBJECT, FIND_OBJECTS_BEFORE_PICKING, GO_BACK_TO_CUPBOARD, PLACE_OBJECT, CHECK_EMPTY_TABLE, GO_BACK_TO_TABLE, EXIT] outcomes: [DONE, TIMEOUT] state_descriptions: - state: @@ -30,7 +30,7 @@ state_descriptions: value: store groceries - state: name: ENTER - state_module_name: mdr_store_groceries.scenario_states.move_base + state_module_name: mdr_navigation_behaviours.move_base state_class_name: MoveBase transitions: - transition: @@ -51,7 +51,7 @@ state_descriptions: value: 3 - state: name: GO_TO_SCANNING_POSE - state_module_name: mdr_store_groceries.scenario_states.move_base + state_module_name: mdr_navigation_behaviours.move_base state_class_name: MoveBase transitions: - transition: @@ -93,7 +93,7 @@ state_descriptions: value: 3 - state: name: GO_TO_CUPBOARD - state_module_name: mdr_store_groceries.scenario_states.move_base + state_module_name: mdr_navigation_behaviours.move_base state_class_name: MoveBase transitions: - transition: @@ -150,7 +150,7 @@ state_descriptions: value: 3 - state: name: GO_TO_TABLE - state_module_name: mdr_store_groceries.scenario_states.move_base + state_module_name: mdr_navigation_behaviours.move_base state_class_name: MoveBase transitions: - transition: @@ -171,7 +171,7 @@ state_descriptions: value: 3 - state: name: PERCEIVE_TABLE - state_module_name: mdr_store_groceries.scenario_states.perceive_planes + state_module_name: mdr_perception_behaviours.perceive_planes state_class_name: PerceivePlanes transitions: - transition: @@ -192,8 +192,8 @@ state_descriptions: value: table - state: name: PICK_OBJECT - state_module_name: mdr_store_groceries.scenario_states.pick - state_class_name: Pick + state_module_name: mdr_manipulation_behaviours.pick_closest_from_surface + state_class_name: PickClosestFromSurface transitions: - transition: name: succeeded @@ -211,9 +211,12 @@ state_descriptions: - argument: name: number_of_retries value: 3 + - argument: + name: picking_surface_prefix + value: table - state: name: FIND_OBJECTS_BEFORE_PICKING - state_module_name: mdr_store_groceries.scenario_states.perceive_planes + state_module_name: mdr_perception_behaviours.perceive_planes state_class_name: PerceivePlanes transitions: - transition: @@ -234,7 +237,7 @@ state_descriptions: value: table - state: name: GO_BACK_TO_CUPBOARD - state_module_name: mdr_store_groceries.scenario_states.move_base + state_module_name: mdr_navigation_behaviours.move_base state_class_name: MoveBase transitions: - transition: @@ -255,15 +258,12 @@ state_descriptions: value: 3 - state: name: PLACE_OBJECT - state_module_name: mdr_store_groceries.scenario_states.place - state_class_name: Place + state_module_name: mdr_manipulation_behaviours.place_based_on_category + state_class_name: PlaceBasedOnCategory transitions: - transition: - name: pick_new_object - state: GO_BACK_TO_TABLE - - transition: - name: finished - state: EXIT + name: succeeded + state: CHECK_EMPTY_TABLE - transition: name: failed state: PLACE_OBJECT @@ -274,9 +274,27 @@ state_descriptions: - argument: name: number_of_retries value: 3 + - argument: + name: placing_surface_prefix + value: shelf + - state: + name: CHECK_EMPTY_TABLE + state_module_name: mdr_knowledge_behaviours.check_empty_surface + state_class_name: CheckEmptySurface + transitions: + - transition: + name: empty + state: EXIT + - transition: + name: not_empty + state: GO_BACK_TO_TABLE + arguments: + - argument: + name: surface_prefix + value: table - state: name: GO_BACK_TO_TABLE - state_module_name: mdr_store_groceries.scenario_states.move_base + state_module_name: mdr_navigation_behaviours.move_base state_class_name: MoveBase transitions: - transition: @@ -297,7 +315,7 @@ state_descriptions: value: 3 - state: name: EXIT - state_module_name: mdr_store_groceries.scenario_states.move_base + state_module_name: mdr_navigation_behaviours.move_base state_class_name: MoveBase transitions: - transition: diff --git a/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_store_groceries/package.xml b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_store_groceries/package.xml index 43c0bc593..eb7b4221d 100644 --- a/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_store_groceries/package.xml +++ b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_store_groceries/package.xml @@ -2,7 +2,7 @@ mdr_store_groceries 1.0.0 - State machine and launch files for a store groceries scenario + State machine and launch files for a RoboCup@Home store groceries scenario GPLv3 Alex Mitrevski @@ -12,12 +12,6 @@ rospy roslint - smach - smach_ros - actionlib - mongodb_store - - mdr_rosplan_interface mas_execution_manager mdr_listen_action mdr_process_speech_command_action @@ -26,18 +20,11 @@ mdr_pickup_action mdr_place_action mdr_perceive_plane_action + mdr_navigation_behaviours + mdr_perception_behaviours + mdr_manipulation_behaviours rospy - roslint - smach - smach_ros - actionlib - - mongodb_store - rosplan_knowledge_base - rosplan_planning_system - - mdr_rosplan_interface mas_execution_manager mdr_listen_action mdr_process_speech_command_action @@ -46,6 +33,7 @@ mdr_pickup_action mdr_place_action mdr_perceive_plane_action - - roslaunch + mdr_navigation_behaviours + mdr_perception_behaviours + mdr_manipulation_behaviours diff --git a/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_store_groceries/ros/src/mdr_store_groceries/scenario_states/move_base.py b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_store_groceries/ros/src/mdr_store_groceries/scenario_states/move_base.py deleted file mode 100644 index 69656a4d2..000000000 --- a/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_store_groceries/ros/src/mdr_store_groceries/scenario_states/move_base.py +++ /dev/null @@ -1,127 +0,0 @@ -import time -import rospy -import actionlib -from std_msgs.msg import String -from geometry_msgs.msg import PoseStamped - -import rosplan_dispatch_msgs.msg as plan_dispatch_msgs -import rosplan_knowledge_msgs.srv as rosplan_srvs -import diagnostic_msgs.msg as diag_msgs - -from mdr_move_base_action.msg import MoveBaseAction, MoveBaseGoal -from mas_execution_manager.scenario_state_base import ScenarioStateBase - -class MoveBase(ScenarioStateBase): - def __init__(self, save_sm_state=False, **kwargs): - ScenarioStateBase.__init__(self, 'move_base', - save_sm_state=save_sm_state, - outcomes=['succeeded', 'failed', 'failed_after_retrying']) - self.sm_id = kwargs.get('sm_id', 'mdr_store_groceries') - self.state_name = kwargs.get('state_name', 'move_base') - self.move_base_server = kwargs.get('move_base_server', 'move_base_server') - self.destination_locations = list(kwargs.get('destination_locations', list())) - self.timeout = kwargs.get('timeout', 120.) - - self.number_of_retries = kwargs.get('number_of_retries', 0) - self.retry_count = 0 - - self.move_base_client = actionlib.SimpleActionClient(self.move_base_server, - MoveBaseAction) - self.move_base_client.wait_for_server() - - def execute(self, userdata): - if self.save_sm_state: - self.save_current_state() - - # if the current location of the robot was not specified - # in the request, we query this information from the knowledge base - original_location = '' - request = rosplan_srvs.GetAttributeServiceRequest() - request.predicate_name = 'robot_at' - result = self.attribute_fetching_client(request) - for item in result.attributes: - if not item.is_negative: - for param in item.values: - if param.key == 'bot' and param.value != self.robot_name: - break - - if param.key == 'wp': - original_location = param.value - break - break - - for destination_location in self.destination_locations: - if destination_location.lower().find('table') != -1: - self.say('Moving to table') - - table_pose = self.get_table_pose() - if table_pose is None: - rospy.loginfo('Could not get table pose') - self.say('Could not move to table') - return 'failed' - - move_base_goal = MoveBaseGoal() - move_base_goal.goal_type = MoveBaseGoal.POSE - move_base_goal.pose = table_pose - self.move_base_client.send_goal(move_base_goal) - self.move_base_client.wait_for_result() - result = self.move_base_client.get_result() - if result: - return 'succeeded' - else: - dispatch_msg = self.get_dispatch_msg(original_location, - destination_location) - - rospy.loginfo('Sending the base to %s' % destination_location) - self.say('Going to ' + destination_location) - self.action_dispatch_pub.publish(dispatch_msg) - - self.executing = True - self.succeeded = False - start_time = time.time() - duration = 0. - while self.executing and duration < self.timeout: - rospy.sleep(0.1) - duration = time.time() - start_time - - if self.succeeded: - rospy.loginfo('Successfully reached %s' % destination_location) - original_location = destination_location - else: - rospy.logerr('Could not reach %s' % destination_location) - self.say('Could not reach ' + destination_location) - if self.retry_count == self.number_of_retries: - self.say('Aborting operation') - return 'failed_after_retrying' - self.retry_count += 1 - return 'failed' - return 'succeeded' - - def get_table_pose(self): - try: - pose = self.msg_store_client.query_named('table_pose', PoseStamped._type)[0] - return pose - except: - rospy.logerr('Error retriving knowledge about table_pose') - return None - - def get_dispatch_msg(self, original_location, destination_location): - dispatch_msg = plan_dispatch_msgs.ActionDispatch() - dispatch_msg.name = self.action_name - - arg_msg = diag_msgs.KeyValue() - arg_msg.key = 'bot' - arg_msg.value = self.robot_name - dispatch_msg.parameters.append(arg_msg) - - arg_msg = diag_msgs.KeyValue() - arg_msg.key = 'from' - arg_msg.value = original_location - dispatch_msg.parameters.append(arg_msg) - - arg_msg = diag_msgs.KeyValue() - arg_msg.key = 'to' - arg_msg.value = destination_location - dispatch_msg.parameters.append(arg_msg) - - return dispatch_msg diff --git a/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_store_groceries/ros/src/mdr_store_groceries/scenario_states/perceive_planes.py b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_store_groceries/ros/src/mdr_store_groceries/scenario_states/perceive_planes.py deleted file mode 100644 index 0c0ad0990..000000000 --- a/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_store_groceries/ros/src/mdr_store_groceries/scenario_states/perceive_planes.py +++ /dev/null @@ -1,69 +0,0 @@ -import time -import rospy -from std_msgs.msg import String - -import rosplan_dispatch_msgs.msg as plan_dispatch_msgs -import diagnostic_msgs.msg as diag_msgs - -from mas_execution_manager.scenario_state_base import ScenarioStateBase - -class PerceivePlanes(ScenarioStateBase): - def __init__(self, save_sm_state=False, **kwargs): - ScenarioStateBase.__init__(self, 'perceive_plane', - save_sm_state=save_sm_state, - outcomes=['succeeded', 'failed', 'failed_after_retrying']) - self.sm_id = kwargs.get('sm_id', 'mdr_store_groceries') - self.state_name = kwargs.get('state_name', 'perceive_planes') - self.timeout = kwargs.get('timeout', 120.) - self.plane_prefix = kwargs.get('plane_prefix', 0) - - self.number_of_retries = kwargs.get('number_of_retries', 0) - self.retry_count = 0 - - def execute(self, userdata): - if self.save_sm_state: - self.save_current_state() - - dispatch_msg = self.get_dispatch_msg(self.plane_prefix) - rospy.loginfo('Perceiving %s' % self.plane_prefix) - self.say('Perceiving ' + self.plane_prefix) - self.action_dispatch_pub.publish(dispatch_msg) - - self.executing = True - self.succeeded = False - start_time = time.time() - duration = 0. - while self.executing and duration < self.timeout: - rospy.sleep(0.1) - duration = time.time() - start_time - - if self.succeeded: - self.say('%s perceived' % self.plane_prefix) - rospy.loginfo('%s perceived successfully' % self.plane_prefix) - return 'succeeded' - - rospy.loginfo('Could not perceive %s' % self.plane_prefix) - self.say('Could not perceive ' + self.plane_prefix) - if self.retry_count == self.number_of_retries: - rospy.loginfo('Failed to perceive %s' % self.plane_prefix) - self.say('Aborting operation') - return 'failed_after_retrying' - rospy.loginfo('Retrying to perceive %s' % self.plane_prefix) - self.retry_count += 1 - return 'failed' - - def get_dispatch_msg(self, plane_name): - dispatch_msg = plan_dispatch_msgs.ActionDispatch() - dispatch_msg.name = self.action_name - - arg_msg = diag_msgs.KeyValue() - arg_msg.key = 'bot' - arg_msg.value = self.robot_name - dispatch_msg.parameters.append(arg_msg) - - arg_msg = diag_msgs.KeyValue() - arg_msg.key = 'plane' - arg_msg.value = plane_name - dispatch_msg.parameters.append(arg_msg) - - return dispatch_msg diff --git a/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_store_groceries/ros/src/mdr_store_groceries/scenario_states/pick.py b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_store_groceries/ros/src/mdr_store_groceries/scenario_states/pick.py deleted file mode 100644 index 2eb8fc701..000000000 --- a/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_store_groceries/ros/src/mdr_store_groceries/scenario_states/pick.py +++ /dev/null @@ -1,167 +0,0 @@ -import time -import numpy as np -import rospy -from std_msgs.msg import String -from tf import TransformListener - -from geometry_msgs.msg import PointStamped - -import rosplan_dispatch_msgs.msg as plan_dispatch_msgs -import rosplan_knowledge_msgs.srv as rosplan_srvs -import diagnostic_msgs.msg as diag_msgs - -from mcr_perception_msgs.msg import Object -from mas_execution_manager.scenario_state_base import ScenarioStateBase - -class Pick(ScenarioStateBase): - def __init__(self, save_sm_state=False, **kwargs): - ScenarioStateBase.__init__(self, 'pickup', - save_sm_state=save_sm_state, - output_keys=['grasped_object'], - outcomes=['succeeded', 'failed', - 'failed_after_retrying', - 'find_objects_before_picking']) - self.sm_id = kwargs.get('sm_id', 'mdr_store_groceries') - self.state_name = kwargs.get('state_name', 'pick') - self.timeout = kwargs.get('timeout', 120.) - self.tf_listener = TransformListener() - - self.number_of_retries = kwargs.get('number_of_retries', 0) - self.retry_count = 0 - - def execute(self, userdata): - if self.save_sm_state: - self.save_current_state() - - surface_objects = self.get_surface_objects(surface_prefix='table') - object_poses = self.get_object_poses(surface_objects) - surface, obj_to_grasp_idx = self.select_object_for_grasping(object_poses) - obj_to_grasp = '' - if obj_to_grasp_idx != -1: - obj_to_grasp = surface_objects[surface][obj_to_grasp_idx] - else: - rospy.logerr('Could not find an object to grasp') - self.say('Could not find an object to grasp') - return 'find_objects_before_picking' - - dispatch_msg = self.get_dispatch_msg(obj_to_grasp, surface) - rospy.loginfo('Picking %s from %s' % (obj_to_grasp, surface)) - self.say('Picking ' + obj_to_grasp + ' from ' + surface) - self.action_dispatch_pub.publish(dispatch_msg) - - self.executing = True - self.succeeded = False - start_time = time.time() - duration = 0. - while self.executing and duration < self.timeout: - rospy.sleep(0.1) - duration = time.time() - start_time - - if self.succeeded: - rospy.loginfo('%s grasped successfully' % obj_to_grasp) - self.say('Successfully grasped ' + obj_to_grasp) - userdata.grasped_object = obj_to_grasp - return 'succeeded' - - rospy.loginfo('Could not grasp %s' % obj_to_grasp) - self.say('Could not grasp ' + obj_to_grasp) - if self.retry_count == self.number_of_retries: - rospy.loginfo('Failed to grasp %s' % obj_to_grasp) - self.say('Aborting operation') - return 'failed_after_retrying' - rospy.loginfo('Retrying to grasp %s' % obj_to_grasp) - self.retry_count += 1 - return 'failed' - - def get_surface_objects(self, surface_prefix='table'): - surface_objects = dict() - request = rosplan_srvs.GetAttributeServiceRequest() - request.predicate_name = 'on' - result = self.attribute_fetching_client(request) - for item in result.attributes: - object_on_desired_surface = False - object_name = '' - surface_name = '' - if not item.is_negative: - for param in item.values: - if param.key == 'plane' and param.value.find(surface_prefix) != -1: - object_on_desired_surface = True - surface_name = param.value - if surface_name not in surface_objects: - surface_objects[surface_name] = list() - elif param.key == 'obj': - object_name = param.value - if object_on_desired_surface: - surface_objects[surface_name].append(object_name) - return surface_objects - - def get_object_poses(self, surface_objects): - object_poses = dict() - for surface, objects in surface_objects.items(): - object_poses[surface] = list() - for obj_name in objects: - try: - obj = self.msg_store_client.query_named(obj_name, Object._type)[0] - object_poses[surface].append(obj.pose) - except: - rospy.logerr('Error retriving knowledge about %s', obj_name) - pass - return object_poses - - def select_object_for_grasping(self, object_poses): - '''Returns the index of the object whose distance is closest to the robot - ''' - object_surfaces = list() - distances = dict() - robot_position = np.zeros(3) - for surface, poses in object_poses.items(): - object_surfaces.append(surface) - distances[surface] = list() - for pose in poses: - base_link_pose = self.tf_listener.transformPose('base_link', pose) - distances[surface].append(self.distance(robot_position, np.array([base_link_pose.pose.position.x, - base_link_pose.pose.position.y, - base_link_pose.pose.position.z]))) - - min_dist = 1e100 - min_dist_obj_idx = -1 - obj_surface = '' - for surface, distance_list in distances.items(): - if distance_list: - surface_min_dist = np.min(distance_list) - if surface_min_dist < min_dist: - min_dist = surface_min_dist - min_dist_obj_idx = np.argmin(distance_list) - obj_surface = surface - return obj_surface, min_dist_obj_idx - - def get_robot_pose(self, map_frame='map', base_link_frame='base_link'): - latest_tf_time = self.tf_listener.getLatestCommonTime(base_link_frame, map_frame) - position, quat_orientation = self.tf_listener.lookupTransform(base_link_frame, - map_frame, - latest_tf_time) - return position, quat_orientation - - def distance(self, point1, point2): - return np.linalg.norm(np.array(point1) - np.array(point2)) - - def get_dispatch_msg(self, obj_name, surface_name): - dispatch_msg = plan_dispatch_msgs.ActionDispatch() - dispatch_msg.name = self.action_name - - arg_msg = diag_msgs.KeyValue() - arg_msg.key = 'bot' - arg_msg.value = self.robot_name - dispatch_msg.parameters.append(arg_msg) - - arg_msg = diag_msgs.KeyValue() - arg_msg.key = 'obj' - arg_msg.value = obj_name - dispatch_msg.parameters.append(arg_msg) - - arg_msg = diag_msgs.KeyValue() - arg_msg.key = 'plane' - arg_msg.value = surface_name - dispatch_msg.parameters.append(arg_msg) - - return dispatch_msg diff --git a/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_store_groceries/ros/src/mdr_store_groceries/scenario_states/place.py b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_store_groceries/ros/src/mdr_store_groceries/scenario_states/place.py deleted file mode 100644 index 456811f95..000000000 --- a/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_store_groceries/ros/src/mdr_store_groceries/scenario_states/place.py +++ /dev/null @@ -1,202 +0,0 @@ -import time -import numpy as np -import rospy -from std_msgs.msg import String - -import rosplan_dispatch_msgs.msg as plan_dispatch_msgs -import rosplan_knowledge_msgs.srv as rosplan_srvs -import diagnostic_msgs.msg as diag_msgs - -from mcr_perception_msgs.msg import Object -from mas_execution_manager.scenario_state_base import ScenarioStateBase - -class Place(ScenarioStateBase): - def __init__(self, save_sm_state=False, **kwargs): - ScenarioStateBase.__init__(self, 'place', - save_sm_state=save_sm_state, - input_keys=['grasped_object'], - outcomes=['pick_new_object', 'finished', - 'failed', 'failed_after_retrying']) - self.sm_id = kwargs.get('sm_id', 'mdr_store_groceries') - self.state_name = kwargs.get('state_name', 'place') - self.timeout = kwargs.get('timeout', 120.) - - self.number_of_retries = kwargs.get('number_of_retries', 0) - self.retry_count = 0 - - def execute(self, userdata): - if self.save_sm_state: - self.save_current_state() - - grasped_object = userdata.grasped_object - grasped_obj_category = self.get_object_category(grasped_object) - surface_name = self.choose_placing_surface(grasped_object, grasped_obj_category) - dispatch_msg = self.get_dispatch_msg(grasped_object, surface_name) - rospy.loginfo('Placing %s on %s' % (grasped_object, surface_name)) - self.say('Placing ' + grasped_object + ' on ' + surface_name) - self.action_dispatch_pub.publish(dispatch_msg) - - self.executing = True - self.succeeded = False - start_time = time.time() - duration = 0. - while self.executing and duration < self.timeout: - rospy.sleep(0.1) - duration = time.time() - start_time - - if self.succeeded: - rospy.loginfo('Object placed successfully') - self.say('Successfully placed ' + grasped_object) - if self.surface_empty(surface_prefix='table'): - self.say('I am finally done storing the groceries') - return 'finished' - return 'pick_new_object' - - rospy.loginfo('Could not place object %s' % grasped_object) - self.say('Could not place ' + obj_to_grasp) - if self.retry_count == self.number_of_retries: - rospy.loginfo('Failed to place object %s' % grasped_object) - self.say('Aborting operation') - return 'failed_after_retrying' - rospy.loginfo('Retrying to place object %s' % grasped_object) - self.retry_count += 1 - return 'failed' - - def get_object_category(self, obj_name): - try: - obj = self.msg_store_client.query_named(obj_name, Object._type)[0] - return obj.category - except: - rospy.logerr('Error retriving knowledge about %s', obj_name) - return '' - - def choose_placing_surface(self, obj_name, obj_category): - obj_category_map = self.get_obj_category_map() - surface_category_counts = self.get_surface_category_counts(obj_category_map) - grasped_object_category = obj_category_map[obj_name] - placing_surface = self.get_best_placing_surface(obj_category, - surface_category_counts) - return placing_surface - - def get_obj_category_map(self): - '''Returns a dictionary of objects and object categories in which - each key represents an object and the value is its category - ''' - request = rosplan_srvs.GetAttributeServiceRequest() - request.predicate_name = 'object_category' - category_result = self.attribute_fetching_client(request) - - obj_category_dict = dict() - for item in category_result.attributes: - obj_name = '' - obj_category = '' - for param in item.values: - if param.key == 'obj': - obj_name = param.value - elif param.key == 'cat': - obj_category = param.value - obj_category_dict[obj_name] = obj_category - return obj_category_dict - - def get_surface_category_counts(self, obj_category_map): - '''Returns a dictionary of surfaces and object category counts in which - each key represents a surface in the environment and the value - is a dictionary of object counts for each category - ''' - surface_category_counts = dict() - - # we take all explored surfaces and populate 'surface_category_counts' - # with an empty dictionary for each surface; each such dictionary - # will store the object category count for the respective surface - request = rosplan_srvs.GetAttributeServiceRequest() - request.predicate_name = 'explored' - explored_result = self.attribute_fetching_client(request) - for item in explored_result.attributes: - surface_name = '' - for param in item.values: - if param.key == 'plane': - surface_name = param.value - # we don't want to place items on the table, so we - # don't consider the table as a placing surface - if surface_name not in surface_category_counts and surface_name.find('table') == -1: - surface_category_counts[surface_name] = dict() - - # we collect a dictionary of object category counts for each surface - request = rosplan_srvs.GetAttributeServiceRequest() - request.predicate_name = 'on' - on_result = self.attribute_fetching_client(request) - for item in on_result.attributes: - obj_name = '' - obj_surface = '' - for param in item.values: - if param.key == 'obj': - obj_name = param.value - elif param.key == 'plane': - obj_surface = param.value - - if obj_surface.find('table') == -1: - obj_category = obj_category_map[obj_name] - if obj_category not in surface_category_counts[obj_surface]: - surface_category_counts[obj_surface][obj_category] = 1 - else: - surface_category_counts[obj_surface][obj_category] += 1 - return surface_category_counts - - def get_best_placing_surface(self, obj_category, surface_category_counts): - '''If none of the surfaces contain objects whose category is - 'obj_category', returns the name of a random surface; otherwise, - returns the name of the surface that has the largest number - of objects of the category 'obj_category'. - ''' - surfaces = list() - obj_surface_category_counts = list() - for surface, category_counts in surface_category_counts.items(): - surfaces.append(surface) - if obj_category in category_counts: - obj_surface_category_counts.append(category_counts[obj_category]) - else: - obj_surface_category_counts.append(0) - - surface_idx = -1 - if np.count_nonzero(obj_surface_category_counts) != 0: - surface_idx = np.argmax(obj_surface_category_counts) - else: - surface_idx = np.random.randint(0, len(surfaces)) - return surfaces[surface_idx] - - def surface_empty(self, surface_prefix='table'): - no_objects_on_surface = True - request = rosplan_srvs.GetAttributeServiceRequest() - request.predicate_name = 'on' - result = self.attribute_fetching_client(request) - for item in result.attributes: - object_on_desired_surface = False - if not item.is_negative: - for param in item.values: - if param.key == 'plane' and param.value.find(surface_prefix) != -1: - object_on_desired_surface = True - if object_on_desired_surface: - no_objects_on_surface = False - break - return no_objects_on_surface - - def get_dispatch_msg(self, obj_name, plane_name): - dispatch_msg = plan_dispatch_msgs.ActionDispatch() - dispatch_msg.name = self.action_name - - arg_msg = diag_msgs.KeyValue() - arg_msg.key = 'bot' - arg_msg.value = self.robot_name - dispatch_msg.parameters.append(arg_msg) - - arg_msg = diag_msgs.KeyValue() - arg_msg.key = 'obj' - arg_msg.value = obj_name - dispatch_msg.parameters.append(arg_msg) - - arg_msg = diag_msgs.KeyValue() - arg_msg.key = 'plane' - arg_msg.value = plane_name - dispatch_msg.parameters.append(arg_msg) - - return dispatch_msg From da9dac4a67ed1442262b7d9d8e0019569d4fa3ad Mon Sep 17 00:00:00 2001 From: Alex Mitrevski Date: Wed, 26 Dec 2018 00:32:00 +0100 Subject: [PATCH 23/96] [demo_patrol] The move_base state from mdr_navigation_behaviours is now reused --- .../mdr_demos/mdr_demo_patrol/CMakeLists.txt | 25 +---- .../mdr_demo_patrol/config/patrol_sm.yaml | 2 +- .../mdr_demos/mdr_demo_patrol/package.xml | 24 +---- .../ros/src/mdr_demo_patrol/__init__.py | 0 .../scenario_states/__init__.py | 0 .../scenario_states/move_base.py | 96 ------------------- .../mdr_demos/mdr_demo_patrol/setup.py | 11 --- 7 files changed, 9 insertions(+), 149 deletions(-) delete mode 100644 mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_patrol/ros/src/mdr_demo_patrol/__init__.py delete mode 100644 mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_patrol/ros/src/mdr_demo_patrol/scenario_states/__init__.py delete mode 100644 mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_patrol/ros/src/mdr_demo_patrol/scenario_states/move_base.py delete mode 100644 mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_patrol/setup.py diff --git a/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_patrol/CMakeLists.txt b/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_patrol/CMakeLists.txt index 4e4cda288..6072a122e 100644 --- a/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_patrol/CMakeLists.txt +++ b/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_patrol/CMakeLists.txt @@ -2,35 +2,18 @@ cmake_minimum_required(VERSION 2.8.3) project(mdr_demo_patrol) find_package(catkin REQUIRED COMPONENTS - roslint - rospy - smach_ros - smach - actionlib - mongodb_store - mdr_rosplan_interface - mas_execution_manager - mdr_move_base_action mdr_move_arm_action + mdr_move_base_action + mdr_navigation_behaviours ) catkin_package( CATKIN_DEPENDS - roslint - rospy - smach_ros - smach - actionlib - mongodb_store - mdr_rosplan_interface - mas_execution_manager - mdr_move_base_action mdr_move_arm_action + mdr_move_base_action + mdr_navigation_behaviours ) -catkin_python_setup() -roslint_python() - install(DIRECTORY ros/launch/ DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/ros/launch ) diff --git a/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_patrol/config/patrol_sm.yaml b/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_patrol/config/patrol_sm.yaml index 8b3aa0ec8..57ec6da66 100644 --- a/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_patrol/config/patrol_sm.yaml +++ b/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_patrol/config/patrol_sm.yaml @@ -7,7 +7,7 @@ outcomes: [DONE, TIMEOUT, FAILED] state_descriptions: - state: name: GO_TO - state_module_name: mdr_demo_patrol.scenario_states.move_base + state_module_name: mdr_navigation_behaviours.move_base state_class_name: MoveBase transitions: - transition: diff --git a/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_patrol/package.xml b/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_patrol/package.xml index 765995100..18414a945 100644 --- a/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_patrol/package.xml +++ b/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_patrol/package.xml @@ -10,29 +10,13 @@ catkin - rospy - roslint - smach - smach_ros - actionlib - mongodb_store - mdr_rosplan_interface - mas_execution_manager - mdr_move_base_action mdr_move_arm_action + mdr_move_base_action + mdr_navigation_behaviours - rospy - roslint - smach - smach_ros - actionlib - mongodb_store - rosplan_knowledge_base - rosplan_planning_system - mdr_rosplan_interface - mas_execution_manager - mdr_move_base_action mdr_move_arm_action + mdr_move_base_action + mdr_navigation_behaviours roslaunch diff --git a/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_patrol/ros/src/mdr_demo_patrol/__init__.py b/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_patrol/ros/src/mdr_demo_patrol/__init__.py deleted file mode 100644 index e69de29bb..000000000 diff --git a/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_patrol/ros/src/mdr_demo_patrol/scenario_states/__init__.py b/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_patrol/ros/src/mdr_demo_patrol/scenario_states/__init__.py deleted file mode 100644 index e69de29bb..000000000 diff --git a/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_patrol/ros/src/mdr_demo_patrol/scenario_states/move_base.py b/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_patrol/ros/src/mdr_demo_patrol/scenario_states/move_base.py deleted file mode 100644 index cb53b95eb..000000000 --- a/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_patrol/ros/src/mdr_demo_patrol/scenario_states/move_base.py +++ /dev/null @@ -1,96 +0,0 @@ -import time -import rospy -import actionlib - -import rosplan_dispatch_msgs.msg as plan_dispatch_msgs -import rosplan_knowledge_msgs.srv as rosplan_srvs -import diagnostic_msgs.msg as diag_msgs - -from mdr_move_base_action.msg import MoveBaseAction -from mas_execution_manager.scenario_state_base import ScenarioStateBase - -class MoveBase(ScenarioStateBase): - def __init__(self, save_sm_state=False, **kwargs): - ScenarioStateBase.__init__(self, 'move_base', - save_sm_state=save_sm_state, - outcomes=['succeeded', 'failed', 'failed_after_retrying']) - self.sm_id = kwargs.get('sm_id', 'mdr_demo_patrol') - self.state_name = kwargs.get('state_name', 'move_base') - self.move_base_server = kwargs.get('move_base_server', 'move_base_server') - self.destination_locations = list(kwargs.get('destination_locations', list())) - self.timeout = kwargs.get('timeout', 120.) - - self.number_of_retries = kwargs.get('number_of_retries', 0) - self.retry_count = 0 - - self.move_base_client = actionlib.SimpleActionClient(self.move_base_server, - MoveBaseAction) - self.move_base_client.wait_for_server() - - def execute(self, userdata): - # if the current location of the robot was not specified - # in the request, we query this information from the knowledge base - original_location = '' - request = rosplan_srvs.GetAttributeServiceRequest() - request.predicate_name = 'robot_at' - result = self.attribute_fetching_client(request) - for item in result.attributes: - if not item.is_negative: - for param in item.values: - if param.key == 'bot' and param.value != self.robot_name: - break - - if param.key == 'wp': - original_location = param.value - break - break - - for destination_location in self.destination_locations: - dispatch_msg = self.get_dispatch_msg(original_location, - destination_location) - - rospy.loginfo('Sending the base to %s' % destination_location) - self.say('Going to ' + destination_location) - self.action_dispatch_pub.publish(dispatch_msg) - - self.executing = True - self.succeeded = False - start_time = time.time() - duration = 0. - while self.executing and duration < self.timeout: - rospy.sleep(0.1) - duration = time.time() - start_time - - if self.succeeded: - rospy.loginfo('Successfully reached %s' % destination_location) - original_location = destination_location - else: - rospy.logerr('Could not reach %s' % destination_location) - self.say('Could not reach ' + destination_location) - if self.retry_count == self.number_of_retries: - self.say('Aborting operation') - return 'failed_after_retrying' - self.retry_count += 1 - return 'failed' - return 'succeeded' - - def get_dispatch_msg(self, original_location, destination_location): - dispatch_msg = plan_dispatch_msgs.ActionDispatch() - dispatch_msg.name = self.action_name - - arg_msg = diag_msgs.KeyValue() - arg_msg.key = 'bot' - arg_msg.value = self.robot_name - dispatch_msg.parameters.append(arg_msg) - - arg_msg = diag_msgs.KeyValue() - arg_msg.key = 'from' - arg_msg.value = original_location - dispatch_msg.parameters.append(arg_msg) - - arg_msg = diag_msgs.KeyValue() - arg_msg.key = 'to' - arg_msg.value = destination_location - dispatch_msg.parameters.append(arg_msg) - - return dispatch_msg diff --git a/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_patrol/setup.py b/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_patrol/setup.py deleted file mode 100644 index 6aed34489..000000000 --- a/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_patrol/setup.py +++ /dev/null @@ -1,11 +0,0 @@ -#!/usr/bin/env python - -from distutils.core import setup -from catkin_pkg.python_setup import generate_distutils_setup - -d = generate_distutils_setup( - packages=['mdr_demo_patrol'], - package_dir={'mdr_demo_patrol': 'ros/src/mdr_demo_patrol'} -) - -setup(**d) From ba4a3411e0010a99e5a16fb79eafa77fcd2135ef Mon Sep 17 00:00:00 2001 From: Alex Mitrevski Date: Wed, 26 Dec 2018 00:33:24 +0100 Subject: [PATCH 24/96] [demo_simple_pick_and_place] The states from mdr_behaviours are now reused --- .../CMakeLists.txt | 33 ++-- .../config/pick_and_place_sm.yaml | 20 ++- .../package.xml | 37 ++-- .../__init__.py | 0 .../scenario_states/__init__.py | 0 .../scenario_states/move_base.py | 102 ----------- .../scenario_states/perceive_planes.py | 68 ------- .../scenario_states/pick.py | 167 ------------------ .../scenario_states/place.py | 89 ---------- .../mdr_demo_simple_pick_and_place/setup.py | 11 -- 10 files changed, 37 insertions(+), 490 deletions(-) delete mode 100644 mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_simple_pick_and_place/ros/src/mdr_demo_simple_pick_and_place/__init__.py delete mode 100644 mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_simple_pick_and_place/ros/src/mdr_demo_simple_pick_and_place/scenario_states/__init__.py delete mode 100644 mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_simple_pick_and_place/ros/src/mdr_demo_simple_pick_and_place/scenario_states/move_base.py delete mode 100644 mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_simple_pick_and_place/ros/src/mdr_demo_simple_pick_and_place/scenario_states/perceive_planes.py delete mode 100644 mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_simple_pick_and_place/ros/src/mdr_demo_simple_pick_and_place/scenario_states/pick.py delete mode 100644 mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_simple_pick_and_place/ros/src/mdr_demo_simple_pick_and_place/scenario_states/place.py delete mode 100644 mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_simple_pick_and_place/setup.py diff --git a/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_simple_pick_and_place/CMakeLists.txt b/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_simple_pick_and_place/CMakeLists.txt index fac562043..48a87d8eb 100644 --- a/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_simple_pick_and_place/CMakeLists.txt +++ b/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_simple_pick_and_place/CMakeLists.txt @@ -2,41 +2,32 @@ cmake_minimum_required(VERSION 2.8.3) project(mdr_demo_simple_pick_and_place) find_package(catkin REQUIRED COMPONENTS - roslint rospy - smach_ros - smach - actionlib - mongodb_store - mdr_rosplan_interface - mas_execution_manager - mdr_move_base_action mdr_move_arm_action + mdr_move_base_action + mdr_perceive_plane_action mdr_pickup_action mdr_place_action - mdr_perceive_plane_action + mas_execution_manager + mdr_navigation_behaviours + mdr_perception_behaviours + mdr_manipulation_behaviours ) catkin_package( CATKIN_DEPENDS - roslint rospy - smach_ros - smach - actionlib - mongodb_store - mdr_rosplan_interface - mas_execution_manager - mdr_move_base_action mdr_move_arm_action + mdr_move_base_action + mdr_perceive_plane_action mdr_pickup_action mdr_place_action - mdr_perceive_plane_action + mas_execution_manager + mdr_navigation_behaviours + mdr_perception_behaviours + mdr_manipulation_behaviours ) -catkin_python_setup() -roslint_python() - install(DIRECTORY ros/launch/ DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/ros/launch ) diff --git a/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_simple_pick_and_place/config/pick_and_place_sm.yaml b/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_simple_pick_and_place/config/pick_and_place_sm.yaml index ea7e4f822..fb9e67142 100644 --- a/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_simple_pick_and_place/config/pick_and_place_sm.yaml +++ b/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_simple_pick_and_place/config/pick_and_place_sm.yaml @@ -7,7 +7,7 @@ outcomes: [DONE, FAILED, TIMEOUT] state_descriptions: - state: name: GO_TO_TABLE - state_module_name: mdr_demo_simple_pick_and_place.scenario_states.move_base + state_module_name: mdr_navigation_behaviours.move_base state_class_name: MoveBase transitions: - transition: @@ -28,7 +28,7 @@ state_descriptions: value: 3 - state: name: PERCEIVE_TABLE - state_module_name: mdr_demo_simple_pick_and_place.scenario_states.perceive_planes + state_module_name: mdr_perception_behaviours.perceive_planes state_class_name: PerceivePlanes transitions: - transition: @@ -49,8 +49,8 @@ state_descriptions: value: table - state: name: PICK_OBJECT - state_module_name: mdr_demo_simple_pick_and_place.scenario_states.pick - state_class_name: Pick + state_module_name: mdr_manipulation_behaviours.pick_closest_from_surface + state_class_name: PickClosestFromSurface transitions: - transition: name: succeeded @@ -68,9 +68,12 @@ state_descriptions: - argument: name: number_of_retries value: 3 + - argument: + name: picking_surface_prefix + value: table - state: name: FIND_OBJECTS_BEFORE_PICKING - state_module_name: mdr_demo_simple_pick_and_place.scenario_states.perceive_planes + state_module_name: mdr_perception_behaviours.perceive_planes state_class_name: PerceivePlanes transitions: - transition: @@ -91,11 +94,11 @@ state_descriptions: value: table - state: name: PLACE_OBJECT - state_module_name: mdr_demo_simple_pick_and_place.scenario_states.place + state_module_name: mdr_manipulation_behaviours.place state_class_name: Place transitions: - transition: - name: pick_new_object + name: succeeded state: PERCEIVE_TABLE - transition: name: failed @@ -107,3 +110,6 @@ state_descriptions: - argument: name: number_of_retries value: 3 + - argument: + name: placing_surface_prefix + value: table diff --git a/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_simple_pick_and_place/package.xml b/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_simple_pick_and_place/package.xml index 3fd4b2660..ab5b77c25 100644 --- a/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_simple_pick_and_place/package.xml +++ b/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_simple_pick_and_place/package.xml @@ -11,37 +11,24 @@ catkin rospy - roslint - smach - smach_ros - actionlib - mongodb_store - - mdr_rosplan_interface - mas_execution_manager - mdr_move_base_action mdr_move_arm_action + mdr_move_base_action + mdr_perceive_plane_action mdr_pickup_action mdr_place_action - mdr_perceive_plane_action + mas_execution_manager + mdr_navigation_behaviours + mdr_perception_behaviours + mdr_manipulation_behaviours rospy - roslint - smach - smach_ros - actionlib - - mongodb_store - rosplan_knowledge_base - rosplan_planning_system - - mdr_rosplan_interface - mas_execution_manager - mdr_move_base_action mdr_move_arm_action + mdr_move_base_action + mdr_perceive_plane_action mdr_pickup_action mdr_place_action - mdr_perceive_plane_action - - roslaunch + mas_execution_manager + mdr_navigation_behaviours + mdr_perception_behaviours + mdr_manipulation_behaviours diff --git a/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_simple_pick_and_place/ros/src/mdr_demo_simple_pick_and_place/__init__.py b/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_simple_pick_and_place/ros/src/mdr_demo_simple_pick_and_place/__init__.py deleted file mode 100644 index e69de29bb..000000000 diff --git a/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_simple_pick_and_place/ros/src/mdr_demo_simple_pick_and_place/scenario_states/__init__.py b/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_simple_pick_and_place/ros/src/mdr_demo_simple_pick_and_place/scenario_states/__init__.py deleted file mode 100644 index e69de29bb..000000000 diff --git a/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_simple_pick_and_place/ros/src/mdr_demo_simple_pick_and_place/scenario_states/move_base.py b/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_simple_pick_and_place/ros/src/mdr_demo_simple_pick_and_place/scenario_states/move_base.py deleted file mode 100644 index 15ccd830f..000000000 --- a/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_simple_pick_and_place/ros/src/mdr_demo_simple_pick_and_place/scenario_states/move_base.py +++ /dev/null @@ -1,102 +0,0 @@ -import time -import rospy -import actionlib -from std_msgs.msg import String -from geometry_msgs.msg import PoseStamped - -import rosplan_dispatch_msgs.msg as plan_dispatch_msgs -import rosplan_knowledge_msgs.srv as rosplan_srvs -import diagnostic_msgs.msg as diag_msgs - -from mdr_move_base_action.msg import MoveBaseAction, MoveBaseGoal -from mas_execution_manager.scenario_state_base import ScenarioStateBase - -class MoveBase(ScenarioStateBase): - def __init__(self, save_sm_state=False, **kwargs): - ScenarioStateBase.__init__(self, 'move_base', - save_sm_state=save_sm_state, - outcomes=['succeeded', 'failed', 'failed_after_retrying']) - self.sm_id = kwargs.get('sm_id', 'mdr_demo_simple_pick_and_place') - self.state_name = kwargs.get('state_name', 'move_base') - self.move_base_server = kwargs.get('move_base_server', 'move_base_server') - self.destination_locations = list(kwargs.get('destination_locations', list())) - self.timeout = kwargs.get('timeout', 120.) - self.save_sm_state = save_sm_state - - self.number_of_retries = kwargs.get('number_of_retries', 0) - self.retry_count = 0 - - self.move_base_client = actionlib.SimpleActionClient(self.move_base_server, - MoveBaseAction) - self.move_base_client.wait_for_server() - - def execute(self, userdata): - if self.save_sm_state: - self.save_current_state() - - # if the current location of the robot was not specified - # in the request, we query this information from the knowledge base - original_location = '' - request = rosplan_srvs.GetAttributeServiceRequest() - request.predicate_name = 'robot_at' - result = self.attribute_fetching_client(request) - for item in result.attributes: - if not item.is_negative: - for param in item.values: - if param.key == 'bot' and param.value != self.robot_name: - break - - if param.key == 'wp': - original_location = param.value - break - break - - for destination_location in self.destination_locations: - dispatch_msg = self.get_dispatch_msg(original_location, - destination_location) - - rospy.loginfo('Sending the base to %s' % destination_location) - self.say('Going to ' + destination_location) - self.action_dispatch_pub.publish(dispatch_msg) - - self.executing = True - self.succeeded = False - start_time = time.time() - duration = 0. - while self.executing and duration < self.timeout: - rospy.sleep(0.1) - duration = time.time() - start_time - - if self.succeeded: - rospy.loginfo('Successfully reached %s' % destination_location) - original_location = destination_location - else: - rospy.logerr('Could not reach %s' % destination_location) - self.say('Could not reach ' + destination_location) - if self.retry_count == self.number_of_retries: - self.say('Aborting operation') - return 'failed_after_retrying' - self.retry_count += 1 - return 'failed' - return 'succeeded' - - def get_dispatch_msg(self, original_location, destination_location): - dispatch_msg = plan_dispatch_msgs.ActionDispatch() - dispatch_msg.name = self.action_name - - arg_msg = diag_msgs.KeyValue() - arg_msg.key = 'bot' - arg_msg.value = self.robot_name - dispatch_msg.parameters.append(arg_msg) - - arg_msg = diag_msgs.KeyValue() - arg_msg.key = 'from' - arg_msg.value = original_location - dispatch_msg.parameters.append(arg_msg) - - arg_msg = diag_msgs.KeyValue() - arg_msg.key = 'to' - arg_msg.value = destination_location - dispatch_msg.parameters.append(arg_msg) - - return dispatch_msg diff --git a/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_simple_pick_and_place/ros/src/mdr_demo_simple_pick_and_place/scenario_states/perceive_planes.py b/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_simple_pick_and_place/ros/src/mdr_demo_simple_pick_and_place/scenario_states/perceive_planes.py deleted file mode 100644 index 910388ce8..000000000 --- a/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_simple_pick_and_place/ros/src/mdr_demo_simple_pick_and_place/scenario_states/perceive_planes.py +++ /dev/null @@ -1,68 +0,0 @@ -import time -import rospy -from std_msgs.msg import String - -import rosplan_dispatch_msgs.msg as plan_dispatch_msgs -import diagnostic_msgs.msg as diag_msgs - -from mas_execution_manager.scenario_state_base import ScenarioStateBase - -class PerceivePlanes(ScenarioStateBase): - def __init__(self, save_sm_state=False, **kwargs): - ScenarioStateBase.__init__(self, 'perceive_plane', - save_sm_state=save_sm_state, - outcomes=['succeeded', 'failed', 'failed_after_retrying']) - self.sm_id = kwargs.get('sm_id', 'mdr_demo_simple_pick_and_place') - self.state_name = kwargs.get('state_name', 'perceive_planes') - self.timeout = kwargs.get('timeout', 120.) - self.plane_prefix = kwargs.get('plane_prefix', 0) - - self.number_of_retries = kwargs.get('number_of_retries', 0) - self.retry_count = 0 - - def execute(self, userdata): - if self.save_sm_state: - self.save_current_state() - - dispatch_msg = self.get_dispatch_msg(self.plane_prefix) - rospy.loginfo('Perceiving plane %s' % self.plane_prefix) - self.say('Perceiving plane ' + self.plane_prefix) - self.action_dispatch_pub.publish(dispatch_msg) - - self.executing = True - self.succeeded = False - start_time = time.time() - duration = 0. - while self.executing and duration < self.timeout: - rospy.sleep(0.1) - duration = time.time() - start_time - - if self.succeeded: - rospy.loginfo('%s perceived successfully' % self.plane_prefix) - return 'succeeded' - - rospy.loginfo('Could not perceive %s' % self.plane_prefix) - self.say('Could not perceive ' + self.plane_prefix) - if self.retry_count == self.number_of_retries: - rospy.loginfo('Failed to perceive %s' % self.plane_prefix) - self.say('Aborting operation') - return 'failed_after_retrying' - rospy.loginfo('Retrying to perceive %s' % self.plane_prefix) - self.retry_count += 1 - return 'failed' - - def get_dispatch_msg(self, plane_name): - dispatch_msg = plan_dispatch_msgs.ActionDispatch() - dispatch_msg.name = self.action_name - - arg_msg = diag_msgs.KeyValue() - arg_msg.key = 'bot' - arg_msg.value = self.robot_name - dispatch_msg.parameters.append(arg_msg) - - arg_msg = diag_msgs.KeyValue() - arg_msg.key = 'plane' - arg_msg.value = plane_name - dispatch_msg.parameters.append(arg_msg) - - return dispatch_msg diff --git a/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_simple_pick_and_place/ros/src/mdr_demo_simple_pick_and_place/scenario_states/pick.py b/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_simple_pick_and_place/ros/src/mdr_demo_simple_pick_and_place/scenario_states/pick.py deleted file mode 100644 index 78be7dec1..000000000 --- a/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_simple_pick_and_place/ros/src/mdr_demo_simple_pick_and_place/scenario_states/pick.py +++ /dev/null @@ -1,167 +0,0 @@ -import time -import numpy as np -import rospy -from std_msgs.msg import String -from tf import TransformListener - -from geometry_msgs.msg import PointStamped - -import rosplan_dispatch_msgs.msg as plan_dispatch_msgs -import rosplan_knowledge_msgs.srv as rosplan_srvs -import diagnostic_msgs.msg as diag_msgs - -from mcr_perception_msgs.msg import Object -from mas_execution_manager.scenario_state_base import ScenarioStateBase - -class Pick(ScenarioStateBase): - def __init__(self, save_sm_state=False, **kwargs): - ScenarioStateBase.__init__(self, 'pickup', - save_sm_state=save_sm_state, - output_keys=['grasped_object'], - outcomes=['succeeded', 'failed', - 'failed_after_retrying', - 'find_objects_before_picking']) - self.sm_id = kwargs.get('sm_id', 'mdr_demo_simple_pick_and_place') - self.state_name = kwargs.get('state_name', 'pick') - self.timeout = kwargs.get('timeout', 120.) - self.tf_listener = TransformListener() - - self.number_of_retries = kwargs.get('number_of_retries', 0) - self.retry_count = 0 - - def execute(self, userdata): - if self.save_sm_state: - self.save_current_state() - - surface_objects = self.get_surface_objects(surface_prefix='table') - object_poses = self.get_object_poses(surface_objects) - surface, obj_to_grasp_idx = self.select_object_for_grasping(object_poses) - obj_to_grasp = '' - if obj_to_grasp_idx != -1: - obj_to_grasp = surface_objects[surface][obj_to_grasp_idx] - else: - rospy.logerr('Could not find an object to grasp') - self.say('Could not find an object to grasp') - return 'find_objects_before_picking' - - dispatch_msg = self.get_dispatch_msg(obj_to_grasp, surface) - rospy.loginfo('Picking %s from %s' % (obj_to_grasp, surface)) - self.say('Picking ' + obj_to_grasp + ' from ' + surface) - self.action_dispatch_pub.publish(dispatch_msg) - - self.executing = True - self.succeeded = False - start_time = time.time() - duration = 0. - while self.executing and duration < self.timeout: - rospy.sleep(0.1) - duration = time.time() - start_time - - if self.succeeded: - rospy.loginfo('%s grasped successfully' % obj_to_grasp) - self.say('Successfully grasped ' + obj_to_grasp) - userdata.grasped_object = obj_to_grasp - return 'succeeded' - - rospy.loginfo('Could not grasp %s' % obj_to_grasp) - self.say('Could not grasp ' + obj_to_grasp) - if self.retry_count == self.number_of_retries: - rospy.loginfo('Failed to grasp %s' % obj_to_grasp) - self.say('Aborting operation') - return 'failed_after_retrying' - rospy.loginfo('Retrying to grasp %s' % obj_to_grasp) - self.retry_count += 1 - return 'failed' - - def get_surface_objects(self, surface_prefix='table'): - surface_objects = dict() - request = rosplan_srvs.GetAttributeServiceRequest() - request.predicate_name = 'on' - result = self.attribute_fetching_client(request) - for item in result.attributes: - object_on_desired_surface = False - object_name = '' - surface_name = '' - if not item.is_negative: - for param in item.values: - if param.key == 'plane' and param.value.find(surface_prefix) != -1: - object_on_desired_surface = True - surface_name = param.value - if surface_name not in surface_objects: - surface_objects[surface_name] = list() - elif param.key == 'obj': - object_name = param.value - if object_on_desired_surface: - surface_objects[surface_name].append(object_name) - return surface_objects - - def get_object_poses(self, surface_objects): - object_poses = dict() - for surface, objects in surface_objects.items(): - object_poses[surface] = list() - for obj_name in objects: - try: - obj = self.msg_store_client.query_named(obj_name, Object._type)[0] - object_poses[surface].append(obj.pose) - except: - rospy.logerr('Error retriving knowledge about %s', obj_name) - pass - return object_poses - - def select_object_for_grasping(self, object_poses): - '''Returns the index of the object whose distance is closest to the robot - ''' - object_surfaces = list() - distances = dict() - robot_position = np.zeros(3) - for surface, poses in object_poses.items(): - object_surfaces.append(surface) - distances[surface] = list() - for pose in poses: - base_link_pose = self.tf_listener.transformPose('base_link', pose) - distances[surface].append(self.distance(robot_position, np.array([base_link_pose.pose.position.x, - base_link_pose.pose.position.y, - base_link_pose.pose.position.z]))) - - min_dist = 1e100 - min_dist_obj_idx = -1 - obj_surface = '' - for surface, distance_list in distances.items(): - if distance_list: - surface_min_dist = np.min(distance_list) - if surface_min_dist < min_dist: - min_dist = surface_min_dist - min_dist_obj_idx = np.argmin(distance_list) - obj_surface = surface - return obj_surface, min_dist_obj_idx - - def get_robot_pose(self, map_frame='map', base_link_frame='base_link'): - latest_tf_time = self.tf_listener.getLatestCommonTime(base_link_frame, map_frame) - position, quat_orientation = self.tf_listener.lookupTransform(base_link_frame, - map_frame, - latest_tf_time) - return position, quat_orientation - - def distance(self, point1, point2): - return np.linalg.norm(np.array(point1) - np.array(point2)) - - def get_dispatch_msg(self, obj_name, surface_name): - dispatch_msg = plan_dispatch_msgs.ActionDispatch() - dispatch_msg.name = self.action_name - - arg_msg = diag_msgs.KeyValue() - arg_msg.key = 'bot' - arg_msg.value = self.robot_name - dispatch_msg.parameters.append(arg_msg) - - arg_msg = diag_msgs.KeyValue() - arg_msg.key = 'obj' - arg_msg.value = obj_name - dispatch_msg.parameters.append(arg_msg) - - arg_msg = diag_msgs.KeyValue() - arg_msg.key = 'plane' - arg_msg.value = surface_name - dispatch_msg.parameters.append(arg_msg) - - return dispatch_msg diff --git a/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_simple_pick_and_place/ros/src/mdr_demo_simple_pick_and_place/scenario_states/place.py b/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_simple_pick_and_place/ros/src/mdr_demo_simple_pick_and_place/scenario_states/place.py deleted file mode 100644 index d0c7d4e69..000000000 --- a/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_simple_pick_and_place/ros/src/mdr_demo_simple_pick_and_place/scenario_states/place.py +++ /dev/null @@ -1,89 +0,0 @@ -import time -import numpy as np -import rospy -from std_msgs.msg import String - -import rosplan_dispatch_msgs.msg as plan_dispatch_msgs -import rosplan_knowledge_msgs.srv as rosplan_srvs -import diagnostic_msgs.msg as diag_msgs - -from mas_execution_manager.scenario_state_base import ScenarioStateBase - -class Place(ScenarioStateBase): - def __init__(self, save_sm_state=False, **kwargs): - ScenarioStateBase.__init__(self, 'place', - save_sm_state=save_sm_state, - input_keys=['grasped_object'], - outcomes=['pick_new_object', 'failed', - 'failed_after_retrying']) - self.sm_id = kwargs.get('sm_id', 'mdr_demo_simple_pick_and_place') - self.state_name = kwargs.get('state_name', 'place') - self.timeout = kwargs.get('timeout', 120.) - - self.number_of_retries = kwargs.get('number_of_retries', 0) - self.retry_count = 0 - - def execute(self, userdata): - if self.save_sm_state: - self.save_current_state() - - grasped_object = userdata.grasped_object - surface_name = self.get_placing_surface(grasped_object, surface_prefix='table') - dispatch_msg = self.get_dispatch_msg(grasped_object, surface_name) - rospy.loginfo('Placing %s on %s' % (grasped_object, surface_name)) - self.say('Placing ' + grasped_object + ' on ' + surface_name) - self.action_dispatch_pub.publish(dispatch_msg) - - self.executing = True - self.succeeded = False - start_time = time.time() - duration = 0. - while self.executing and duration < self.timeout: - rospy.sleep(0.1) - duration = time.time() - start_time - - if self.succeeded: - rospy.loginfo('Object placed successfully') - self.say('Successfully placed ' + grasped_object) - return 'pick_new_object' - - rospy.loginfo('Could not place object %s' % grasped_object) - self.say('Could not place ' + obj_to_grasp) - if self.retry_count == self.number_of_retries: - rospy.loginfo('Failed to place object %s' % grasped_object) - self.say('Aborting operation') - return 'failed_after_retrying' - rospy.loginfo('Retrying to place object %s' % grasped_object) - self.retry_count += 1 - return 'failed' - - def get_placing_surface(self, obj_name, surface_prefix='table'): - request = rosplan_srvs.GetAttributeServiceRequest() - request.predicate_name = 'explored' - explored_result = self.attribute_fetching_client(request) - for item in explored_result.attributes: - surface_name = '' - for param in item.values: - if param.key == 'plane' and param.value.find(surface_prefix) != -1: - return param.value - - def get_dispatch_msg(self, obj_name, plane_name): - dispatch_msg = plan_dispatch_msgs.ActionDispatch() - dispatch_msg.name = self.action_name - - arg_msg = diag_msgs.KeyValue() - arg_msg.key = 'bot' - arg_msg.value = self.robot_name - dispatch_msg.parameters.append(arg_msg) - - arg_msg = diag_msgs.KeyValue() - arg_msg.key = 'obj' - arg_msg.value = obj_name - dispatch_msg.parameters.append(arg_msg) - - arg_msg = diag_msgs.KeyValue() - arg_msg.key = 'plane' - arg_msg.value = plane_name - dispatch_msg.parameters.append(arg_msg) - - return dispatch_msg diff --git a/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_simple_pick_and_place/setup.py b/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_simple_pick_and_place/setup.py deleted file mode 100644 index 02478b30f..000000000 --- a/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_simple_pick_and_place/setup.py +++ /dev/null @@ -1,11 +0,0 @@ -#!/usr/bin/env python - -from distutils.core import setup -from catkin_pkg.python_setup import generate_distutils_setup - -d = generate_distutils_setup( - packages=['mdr_demo_simple_pick_and_place'], - package_dir={'mdr_demo_simple_pick_and_place': 'ros/src/mdr_demo_simple_pick_and_place'} -) - -setup(**d) From 9e66664f73e57223189001cfc79b5d2ef78b82b1 Mon Sep 17 00:00:00 2001 From: Alex Mitrevski Date: Wed, 26 Dec 2018 00:34:45 +0100 Subject: [PATCH 25/96] [demo_throw_table_objects] The states from mdr_behaviours are now reused --- .../CMakeLists.txt | 33 ++-- .../config/throw_table_objects_sm.yaml | 21 ++- .../mdr_demo_throw_table_objects/package.xml | 37 ++-- .../mdr_demo_throw_table_objects/__init__.py | 0 .../scenario_states/__init__.py | 0 .../scenario_states/move_base.py | 109 ------------ .../scenario_states/perceive_planes.py | 67 ------- .../scenario_states/pick.py | 167 ------------------ .../scenario_states/throw.py | 79 --------- .../mdr_demo_throw_table_objects/setup.py | 11 -- 10 files changed, 36 insertions(+), 488 deletions(-) delete mode 100644 mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_throw_table_objects/ros/src/mdr_demo_throw_table_objects/__init__.py delete mode 100644 mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_throw_table_objects/ros/src/mdr_demo_throw_table_objects/scenario_states/__init__.py delete mode 100644 mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_throw_table_objects/ros/src/mdr_demo_throw_table_objects/scenario_states/move_base.py delete mode 100644 mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_throw_table_objects/ros/src/mdr_demo_throw_table_objects/scenario_states/perceive_planes.py delete mode 100644 mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_throw_table_objects/ros/src/mdr_demo_throw_table_objects/scenario_states/pick.py delete mode 100644 mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_throw_table_objects/ros/src/mdr_demo_throw_table_objects/scenario_states/throw.py delete mode 100644 mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_throw_table_objects/setup.py diff --git a/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_throw_table_objects/CMakeLists.txt b/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_throw_table_objects/CMakeLists.txt index 65aabed88..3000e1afd 100644 --- a/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_throw_table_objects/CMakeLists.txt +++ b/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_throw_table_objects/CMakeLists.txt @@ -2,41 +2,32 @@ cmake_minimum_required(VERSION 2.8.3) project(mdr_demo_throw_table_objects) find_package(catkin REQUIRED COMPONENTS - roslint rospy - smach_ros - smach - actionlib - mongodb_store - mdr_rosplan_interface - mas_execution_manager - mdr_move_base_action mdr_move_arm_action + mdr_move_base_action + mdr_perceive_plane_action mdr_pickup_action mdr_place_action - mdr_perceive_plane_action + mas_execution_manager + mdr_navigation_behaviours + mdr_perception_behaviours + mdr_manipulation_behaviours ) catkin_package( CATKIN_DEPENDS - roslint rospy - smach_ros - smach - actionlib - mongodb_store - mdr_rosplan_interface - mas_execution_manager - mdr_move_base_action mdr_move_arm_action + mdr_move_base_action + mdr_perceive_plane_action mdr_pickup_action mdr_place_action - mdr_perceive_plane_action + mas_execution_manager + mdr_navigation_behaviours + mdr_perception_behaviours + mdr_manipulation_behaviours ) -catkin_python_setup() -roslint_python() - install(DIRECTORY ros/launch/ DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/ros/launch ) diff --git a/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_throw_table_objects/config/throw_table_objects_sm.yaml b/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_throw_table_objects/config/throw_table_objects_sm.yaml index 37c9ea62f..ce173fbae 100644 --- a/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_throw_table_objects/config/throw_table_objects_sm.yaml +++ b/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_throw_table_objects/config/throw_table_objects_sm.yaml @@ -7,7 +7,7 @@ outcomes: [DONE, TIMEOUT, FAILED] state_descriptions: - state: name: GO_TO_OBSERVATION_POSE - state_module_name: mdr_demo_throw_table_objects.scenario_states.move_base + state_module_name: mdr_navigation_behaviours.move_base state_class_name: MoveBase transitions: - transition: @@ -28,7 +28,7 @@ state_descriptions: value: 3 - state: name: PERCEIVE_TABLE - state_module_name: mdr_demo_throw_table_objects.scenario_states.perceive_planes + state_module_name: mdr_perception_behaviours.perceive_planes state_class_name: PerceivePlanes transitions: - transition: @@ -49,7 +49,7 @@ state_descriptions: value: table - state: name: GO_TO_TABLE - state_module_name: mdr_demo_throw_table_objects.scenario_states.move_base + state_module_name: mdr_navigation_behaviours.move_base state_class_name: MoveBase transitions: - transition: @@ -70,7 +70,7 @@ state_descriptions: value: 3 - state: name: PERCEIVE_TABLE_BEFORE_PICKING - state_module_name: mdr_demo_throw_table_objects.scenario_states.perceive_planes + state_module_name: mdr_perception_behaviours.perceive_planes state_class_name: PerceivePlanes transitions: - transition: @@ -91,8 +91,8 @@ state_descriptions: value: table - state: name: PICK_OBJECT - state_module_name: mdr_demo_throw_table_objects.scenario_states.pick - state_class_name: Pick + state_module_name: mdr_manipulation_behaviours.pick_closest_from_surface + state_class_name: PickClosestFromSurface transitions: - transition: name: succeeded @@ -110,9 +110,12 @@ state_descriptions: - argument: name: number_of_retries value: 3 + - argument: + name: picking_surface_prefix + value: table - state: name: GO_TO_TRASH_CAN - state_module_name: mdr_demo_throw_table_objects.scenario_states.move_base + state_module_name: mdr_navigation_behaviours.move_base state_class_name: MoveBase transitions: - transition: @@ -133,8 +136,8 @@ state_descriptions: value: 3 - state: name: THROW_OBJECT - state_module_name: mdr_demo_throw_table_objects.scenario_states.throw - state_class_name: Throw + state_module_name: mdr_manipulation_behaviours.throw_object_in + state_class_name: ThrowObjectIn transitions: - transition: name: succeeded diff --git a/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_throw_table_objects/package.xml b/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_throw_table_objects/package.xml index 9852c8599..2e7328bdb 100644 --- a/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_throw_table_objects/package.xml +++ b/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_throw_table_objects/package.xml @@ -11,37 +11,24 @@ catkin rospy - roslint - smach - smach_ros - actionlib - mongodb_store - - mdr_rosplan_interface - mas_execution_manager - mdr_move_base_action mdr_move_arm_action + mdr_move_base_action + mdr_perceive_plane_action mdr_pickup_action mdr_place_action - mdr_perceive_plane_action + mas_execution_manager + mdr_navigation_behaviours + mdr_perception_behaviours + mdr_manipulation_behaviours rospy - roslint - smach - smach_ros - actionlib - - mongodb_store - rosplan_knowledge_base - rosplan_planning_system - - mdr_rosplan_interface - mas_execution_manager - mdr_move_base_action mdr_move_arm_action + mdr_move_base_action + mdr_perceive_plane_action mdr_pickup_action mdr_place_action - mdr_perceive_plane_action - - roslaunch + mas_execution_manager + mdr_navigation_behaviours + mdr_perception_behaviours + mdr_manipulation_behaviours diff --git a/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_throw_table_objects/ros/src/mdr_demo_throw_table_objects/__init__.py b/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_throw_table_objects/ros/src/mdr_demo_throw_table_objects/__init__.py deleted file mode 100644 index e69de29bb..000000000 diff --git a/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_throw_table_objects/ros/src/mdr_demo_throw_table_objects/scenario_states/__init__.py b/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_throw_table_objects/ros/src/mdr_demo_throw_table_objects/scenario_states/__init__.py deleted file mode 100644 index e69de29bb..000000000 diff --git a/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_throw_table_objects/ros/src/mdr_demo_throw_table_objects/scenario_states/move_base.py b/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_throw_table_objects/ros/src/mdr_demo_throw_table_objects/scenario_states/move_base.py deleted file mode 100644 index 3afda7b8a..000000000 --- a/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_throw_table_objects/ros/src/mdr_demo_throw_table_objects/scenario_states/move_base.py +++ /dev/null @@ -1,109 +0,0 @@ -import time -import rospy -import actionlib -from std_msgs.msg import String -from geometry_msgs.msg import PoseStamped - -import rosplan_dispatch_msgs.msg as plan_dispatch_msgs -import rosplan_knowledge_msgs.srv as rosplan_srvs -import diagnostic_msgs.msg as diag_msgs - -from mdr_move_base_action.msg import MoveBaseAction, MoveBaseGoal -from mas_execution_manager.scenario_state_base import ScenarioStateBase - -class MoveBase(ScenarioStateBase): - def __init__(self, save_sm_state=False, **kwargs): - ScenarioStateBase.__init__(self, 'move_base', - save_sm_state=save_sm_state, - outcomes=['succeeded', 'failed', 'failed_after_retrying']) - self.sm_id = kwargs.get('sm_id', 'mdr_demo_throw_table_objects') - self.state_name = kwargs.get('state_name', 'move_base') - self.move_base_server = kwargs.get('move_base_server', 'move_base_server') - self.destination_locations = list(kwargs.get('destination_locations', list())) - self.timeout = kwargs.get('timeout', 120.) - - self.number_of_retries = kwargs.get('number_of_retries', 0) - self.retry_count = 0 - - self.move_base_client = actionlib.SimpleActionClient(self.move_base_server, - MoveBaseAction) - self.move_base_client.wait_for_server() - - def execute(self, userdata): - if self.save_sm_state: - self.save_current_state() - - # if the current location of the robot was not specified - # in the request, we query this information from the knowledge base - original_location = '' - request = rosplan_srvs.GetAttributeServiceRequest() - request.predicate_name = 'robot_at' - result = self.attribute_fetching_client(request) - for item in result.attributes: - if not item.is_negative: - for param in item.values: - if param.key == 'bot' and param.value != self.robot_name: - break - - if param.key == 'wp': - original_location = param.value - break - break - - for destination_location in self.destination_locations: - dispatch_msg = self.get_dispatch_msg(original_location, - destination_location) - - rospy.loginfo('Sending the base to %s' % destination_location) - self.say('Going to ' + destination_location) - self.action_dispatch_pub.publish(dispatch_msg) - - self.executing = True - self.succeeded = False - start_time = time.time() - duration = 0. - while self.executing and duration < self.timeout: - rospy.sleep(0.1) - duration = time.time() - start_time - - if self.succeeded: - rospy.loginfo('Successfully reached %s' % destination_location) - original_location = destination_location - else: - rospy.logerr('Could not reach %s' % destination_location) - self.say('Could not reach ' + destination_location) - if self.retry_count == self.number_of_retries: - self.say('Aborting operation') - return 'failed_after_retrying' - self.retry_count += 1 - return 'failed' - return 'succeeded' - - def get_table_pose(self): - try: - pose = self.msg_store_client.query_named('table_pose', PoseStamped._type)[0] - return pose - except: - rospy.logerr('Error retriving knowledge about table_pose') - return None - - def get_dispatch_msg(self, original_location, destination_location): - dispatch_msg = plan_dispatch_msgs.ActionDispatch() - dispatch_msg.name = self.action_name - - arg_msg = diag_msgs.KeyValue() - arg_msg.key = 'bot' - arg_msg.value = self.robot_name - dispatch_msg.parameters.append(arg_msg) - - arg_msg = diag_msgs.KeyValue() - arg_msg.key = 'from' - arg_msg.value = original_location - dispatch_msg.parameters.append(arg_msg) - - arg_msg = diag_msgs.KeyValue() - arg_msg.key = 'to' - arg_msg.value = destination_location - dispatch_msg.parameters.append(arg_msg) - - return dispatch_msg diff --git a/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_throw_table_objects/ros/src/mdr_demo_throw_table_objects/scenario_states/perceive_planes.py b/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_throw_table_objects/ros/src/mdr_demo_throw_table_objects/scenario_states/perceive_planes.py deleted file mode 100644 index 0bc781fd0..000000000 --- a/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_throw_table_objects/ros/src/mdr_demo_throw_table_objects/scenario_states/perceive_planes.py +++ /dev/null @@ -1,67 +0,0 @@ -import time -import rospy - -import rosplan_dispatch_msgs.msg as plan_dispatch_msgs -import diagnostic_msgs.msg as diag_msgs - -from mas_execution_manager.scenario_state_base import ScenarioStateBase - -class PerceivePlanes(ScenarioStateBase): - def __init__(self, save_sm_state=False, **kwargs): - ScenarioStateBase.__init__(self, 'perceive_plane', - save_sm_state=save_sm_state, - outcomes=['succeeded', 'failed', 'failed_after_retrying']) - self.sm_id = kwargs.get('sm_id', 'mdr_demo_throw_table_objects') - self.state_name = kwargs.get('state_name', 'perceive_planes') - self.timeout = kwargs.get('timeout', 120.) - self.plane_prefix = kwargs.get('plane_prefix', 0) - - self.number_of_retries = kwargs.get('number_of_retries', 0) - self.retry_count = 0 - - def execute(self, userdata): - if self.save_sm_state: - self.save_current_state() - - dispatch_msg = self.get_dispatch_msg(self.plane_prefix) - rospy.loginfo('Perceiving plane %s' % self.plane_prefix) - self.say('Perceiving plane ' + self.plane_prefix) - self.action_dispatch_pub.publish(dispatch_msg) - - self.executing = True - self.succeeded = False - start_time = time.time() - duration = 0. - while self.executing and duration < self.timeout: - rospy.sleep(0.1) - duration = time.time() - start_time - - if self.succeeded: - rospy.loginfo('%s perceived successfully' % self.plane_prefix) - return 'succeeded' - - rospy.loginfo('Could not perceive %s' % self.plane_prefix) - self.say('Could not perceive ' + self.plane_prefix) - if self.retry_count == self.number_of_retries: - rospy.loginfo('Failed to perceive %s' % self.plane_prefix) - self.say('Aborting operation') - return 'failed_after_retrying' - rospy.loginfo('Retrying to perceive %s' % self.plane_prefix) - self.retry_count += 1 - return 'failed' - - def get_dispatch_msg(self, plane_name): - dispatch_msg = plan_dispatch_msgs.ActionDispatch() - dispatch_msg.name = self.action_name - - arg_msg = diag_msgs.KeyValue() - arg_msg.key = 'bot' - arg_msg.value = self.robot_name - dispatch_msg.parameters.append(arg_msg) - - arg_msg = diag_msgs.KeyValue() - arg_msg.key = 'plane' - arg_msg.value = plane_name - dispatch_msg.parameters.append(arg_msg) - - return dispatch_msg diff --git a/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_throw_table_objects/ros/src/mdr_demo_throw_table_objects/scenario_states/pick.py b/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_throw_table_objects/ros/src/mdr_demo_throw_table_objects/scenario_states/pick.py deleted file mode 100644 index 433a556d0..000000000 --- a/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_throw_table_objects/ros/src/mdr_demo_throw_table_objects/scenario_states/pick.py +++ /dev/null @@ -1,167 +0,0 @@ -import time -import numpy as np -import rospy -from std_msgs.msg import String -from tf import TransformListener - -from geometry_msgs.msg import PointStamped - -import rosplan_dispatch_msgs.msg as plan_dispatch_msgs -import rosplan_knowledge_msgs.srv as rosplan_srvs -import diagnostic_msgs.msg as diag_msgs - -from mcr_perception_msgs.msg import Object -from mas_execution_manager.scenario_state_base import ScenarioStateBase - -class Pick(ScenarioStateBase): - def __init__(self, save_sm_state=False, **kwargs): - ScenarioStateBase.__init__(self, 'pickup', - save_sm_state=save_sm_state, - output_keys=['grasped_object'], - outcomes=['succeeded', 'failed', - 'failed_after_retrying', - 'find_objects_before_picking']) - self.sm_id = kwargs.get('sm_id', 'mdr_demo_throw_table_objects') - self.state_name = kwargs.get('state_name', 'pick') - self.timeout = kwargs.get('timeout', 120.) - self.tf_listener = TransformListener() - - self.number_of_retries = kwargs.get('number_of_retries', 0) - self.retry_count = 0 - - def execute(self, userdata): - if self.save_sm_state: - self.save_current_state() - - surface_objects = self.get_surface_objects(surface_prefix='table') - object_poses = self.get_object_poses(surface_objects) - surface, obj_to_grasp_idx = self.select_object_for_grasping(object_poses) - obj_to_grasp = '' - if obj_to_grasp_idx != -1: - obj_to_grasp = surface_objects[surface][obj_to_grasp_idx] - else: - rospy.logerr('Could not find an object to grasp') - self.say('Could not find an object to grasp') - return 'find_objects_before_picking' - - dispatch_msg = self.get_dispatch_msg(obj_to_grasp, surface) - rospy.loginfo('Picking %s from %s' % (obj_to_grasp, surface)) - self.say('Picking ' + obj_to_grasp + ' from ' + surface) - self.action_dispatch_pub.publish(dispatch_msg) - - self.executing = True - self.succeeded = False - start_time = time.time() - duration = 0. - while self.executing and duration < self.timeout: - rospy.sleep(0.1) - duration = time.time() - start_time - - if self.succeeded: - rospy.loginfo('%s grasped successfully' % obj_to_grasp) - self.say('Successfully grasped ' + obj_to_grasp) - userdata.grasped_object = obj_to_grasp - return 'succeeded' - - rospy.loginfo('Could not grasp %s' % obj_to_grasp) - self.say('Could not grasp ' + obj_to_grasp) - if self.retry_count == self.number_of_retries: - rospy.loginfo('Failed to grasp %s' % obj_to_grasp) - self.say('Aborting operation') - return 'failed_after_retrying' - rospy.loginfo('Retrying to grasp %s' % obj_to_grasp) - self.retry_count += 1 - return 'failed' - - def get_surface_objects(self, surface_prefix='table'): - surface_objects = dict() - request = rosplan_srvs.GetAttributeServiceRequest() - request.predicate_name = 'on' - result = self.attribute_fetching_client(request) - for item in result.attributes: - object_on_desired_surface = False - object_name = '' - surface_name = '' - if not item.is_negative: - for param in item.values: - if param.key == 'plane' and param.value.find(surface_prefix) != -1: - object_on_desired_surface = True - surface_name = param.value - if surface_name not in surface_objects: - surface_objects[surface_name] = list() - elif param.key == 'obj': - object_name = param.value - if object_on_desired_surface: - surface_objects[surface_name].append(object_name) - return surface_objects - - def get_object_poses(self, surface_objects): - object_poses = dict() - for surface, objects in surface_objects.items(): - object_poses[surface] = list() - for obj_name in objects: - try: - obj = self.msg_store_client.query_named(obj_name, Object._type)[0] - object_poses[surface].append(obj.pose) - except: - rospy.logerr('Error retriving knowledge about %s', obj_name) - pass - return object_poses - - def select_object_for_grasping(self, object_poses): - '''Returns the index of the object whose distance is closest to the robot - ''' - object_surfaces = list() - distances = dict() - robot_position = np.zeros(3) - for surface, poses in object_poses.items(): - object_surfaces.append(surface) - distances[surface] = list() - for pose in poses: - base_link_pose = self.tf_listener.transformPose('base_link', pose) - distances[surface].append(self.distance(robot_position, np.array([base_link_pose.pose.position.x, - base_link_pose.pose.position.y, - base_link_pose.pose.position.z]))) - - min_dist = 1e100 - min_dist_obj_idx = -1 - obj_surface = '' - for surface, distance_list in distances.items(): - if distance_list: - surface_min_dist = np.min(distance_list) - if surface_min_dist < min_dist: - min_dist = surface_min_dist - min_dist_obj_idx = np.argmin(distance_list) - obj_surface = surface - return obj_surface, min_dist_obj_idx - - def get_robot_pose(self, map_frame='map', base_link_frame='base_link'): - latest_tf_time = self.tf_listener.getLatestCommonTime(base_link_frame, map_frame) - position, quat_orientation = self.tf_listener.lookupTransform(base_link_frame, - map_frame, - latest_tf_time) - return position, quat_orientation - - def distance(self, point1, point2): - return np.linalg.norm(np.array(point1) - np.array(point2)) - - def get_dispatch_msg(self, obj_name, surface_name): - dispatch_msg = plan_dispatch_msgs.ActionDispatch() - dispatch_msg.name = self.action_name - - arg_msg = diag_msgs.KeyValue() - arg_msg.key = 'bot' - arg_msg.value = self.robot_name - dispatch_msg.parameters.append(arg_msg) - - arg_msg = diag_msgs.KeyValue() - arg_msg.key = 'obj' - arg_msg.value = obj_name - dispatch_msg.parameters.append(arg_msg) - - arg_msg = diag_msgs.KeyValue() - arg_msg.key = 'plane' - arg_msg.value = surface_name - dispatch_msg.parameters.append(arg_msg) - - return dispatch_msg diff --git a/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_throw_table_objects/ros/src/mdr_demo_throw_table_objects/scenario_states/throw.py b/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_throw_table_objects/ros/src/mdr_demo_throw_table_objects/scenario_states/throw.py deleted file mode 100644 index 93a78edae..000000000 --- a/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_throw_table_objects/ros/src/mdr_demo_throw_table_objects/scenario_states/throw.py +++ /dev/null @@ -1,79 +0,0 @@ -import time -import numpy as np -import rospy -from std_msgs.msg import String - -import rosplan_dispatch_msgs.msg as plan_dispatch_msgs -import rosplan_knowledge_msgs.srv as rosplan_srvs -import diagnostic_msgs.msg as diag_msgs - -from mcr_perception_msgs.msg import Object -from mas_execution_manager.scenario_state_base import ScenarioStateBase - -class Throw(ScenarioStateBase): - def __init__(self, save_sm_state=False, **kwargs): - ScenarioStateBase.__init__(self, 'throw', - save_sm_state=save_sm_state, - input_keys=['grasped_object'], - outcomes=['succeeded', 'failed', 'failed_after_retrying']) - self.sm_id = kwargs.get('sm_id', 'mdr_demo_throw_table_objects') - self.state_name = kwargs.get('state_name', 'throw') - self.throwing_target_name = kwargs.get('throwing_target_name', 'trash_can') - self.timeout = kwargs.get('timeout', 120.) - - self.number_of_retries = kwargs.get('number_of_retries', 0) - self.retry_count = 0 - - def execute(self, userdata): - if self.save_sm_state: - self.save_current_state() - - grasped_object = userdata.grasped_object - dispatch_msg = self.get_dispatch_msg(grasped_object) - rospy.loginfo('Throwing %s in the trash can' % grasped_object) - self.say('Throwing ' + grasped_object + ' in the trash can') - self.action_dispatch_pub.publish(dispatch_msg) - - self.executing = True - self.succeeded = False - start_time = time.time() - duration = 0. - while self.executing and duration < self.timeout: - rospy.sleep(0.1) - duration = time.time() - start_time - - if self.succeeded: - rospy.loginfo('Object discarded successfully') - self.say('Successfully discarded ' + grasped_object) - return 'succeeded' - - rospy.loginfo('Could not discard object %s' % grasped_object) - self.say('Could not discard ' + grasped_object) - if self.retry_count == self.number_of_retries: - rospy.loginfo('Failed to discard object %s' % grasped_object) - self.say('Aborting operation') - return 'failed_after_retrying' - rospy.loginfo('Retrying to discard object %s' % grasped_object) - self.retry_count += 1 - return 'failed' - - def get_dispatch_msg(self, obj_name): - dispatch_msg = plan_dispatch_msgs.ActionDispatch() - dispatch_msg.name = self.action_name - - arg_msg = diag_msgs.KeyValue() - arg_msg.key = 'bot' - arg_msg.value = self.robot_name - dispatch_msg.parameters.append(arg_msg) - - arg_msg = diag_msgs.KeyValue() - arg_msg.key = 'obj' - arg_msg.value = obj_name - dispatch_msg.parameters.append(arg_msg) - - arg_msg = diag_msgs.KeyValue() - arg_msg.key = 'throwing_target' - arg_msg.value = self.throwing_target_name - dispatch_msg.parameters.append(arg_msg) - - return dispatch_msg diff --git a/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_throw_table_objects/setup.py b/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_throw_table_objects/setup.py deleted file mode 100644 index 0789b4ffa..000000000 --- a/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_throw_table_objects/setup.py +++ /dev/null @@ -1,11 +0,0 @@ -#!/usr/bin/env python - -from distutils.core import setup -from catkin_pkg.python_setup import generate_distutils_setup - -d = generate_distutils_setup( - packages=['mdr_demo_throw_table_objects'], - package_dir={'mdr_demo_throw_table_objects': 'ros/src/mdr_demo_throw_table_objects'} -) - -setup(**d) From e99ad80d6595706f521fac89e86c6b18f8d88e85 Mon Sep 17 00:00:00 2001 From: Alex Mitrevski Date: Wed, 26 Dec 2018 00:48:31 +0100 Subject: [PATCH 26/96] Added mas_knowledge_base and mongodb_store to the rosinstall file --- mas-domestic.rosinstall | 10 ++++++++++ 1 file changed, 10 insertions(+) diff --git a/mas-domestic.rosinstall b/mas-domestic.rosinstall index f687f6097..5eebf0f38 100644 --- a/mas-domestic.rosinstall +++ b/mas-domestic.rosinstall @@ -23,6 +23,11 @@ uri: https://github.com/b-it-bots/mas_execution_manager.git version: master +- git: + local-name: mas_knowledge_base + uri: https://github.com/b-it-bots/mas_knowledge_base.git + version: master + - git: local-name: zbar_ros uri: https://github.com/b-it-bots/zbar_ros.git @@ -38,6 +43,11 @@ uri: https://github.com/KCL-Planning/ROSPlan.git version: master +- git: + local-name: mongodb_store + uri: https://github.com/b-it-bots/mongodb_store.git + version: kinetic-devel + - git: local-name: occupancy_grid_utils uri: https://github.com/b-it-bots/occupancy_grid_utils.git From e0659be40f327c55caa5e7c0ef53a29c470e5256 Mon Sep 17 00:00:00 2001 From: Alex Mitrevski Date: Thu, 27 Dec 2018 00:10:14 +0100 Subject: [PATCH 27/96] [mdr_behaviours] Added a README --- mdr_planning/mdr_behaviours/README.md | 65 +++++++++++++++++++++++++++ 1 file changed, 65 insertions(+) create mode 100644 mdr_planning/mdr_behaviours/README.md diff --git a/mdr_planning/mdr_behaviours/README.md b/mdr_planning/mdr_behaviours/README.md new file mode 100644 index 000000000..1b73c3379 --- /dev/null +++ b/mdr_planning/mdr_behaviours/README.md @@ -0,0 +1,65 @@ +# `mdr_behaviours` + +## Table of contents + +1. [Summary](#summary) +2. [Organisation](#organisation) +3. [Dependencies](#dependencies) +4. [Implemented behaviours](#implemented-behaviours) + 1. [mdr_navigation_behaviours](#mdr_navigation_behaviours) + 2. [mdr_perception_behaviours](#mdr_perception_behaviours) + 3. [mdr_manipulation_behaviours](#mdr_manipulation_behaviours) + 4. [mdr_knowledge_behaviours](#mdr_knowledge_behaviours) + +## Summary + +Most domestic scenarios include common components, such as going to certain locations or manipulating objects in a certain way, The `mdr_behaviours` metapackage groups together such functionalities, thereby facilitating the reusability of components when building complex robot scenarios and minimising unnecessary code repetition in the process. + +The name `behaviours` indicates that the components included in this package make robots behave in a certain manner. In this context, we can define an *atomic behaviour* as a specific way of accomplishing something in a certain way. Under this definition, "picking the closest object from a surface" is a specific robot behaviour that differs from "picking a random object from the surface". Atomic behaviours on their own may not make much sense, but when used in conjunction with other atomic behaviours, they give rise to *complex behaviours* that allow robots to perform complex tasks. For instance, combining the atomic behaviours +1. going to the table +2. turning towards the table +3. locating objects on the table +4. picking the closest object from the table +5. going to a cupboard +6. placing the object in the cupboard + +allows us to define a simple pick-and-place behaviour (as done [here](../mdr_scenarios/mdr_demos/mdr_demo_simple_pick_and_place/config/pick_and_place_sm.yaml)). + +As defined here, the term behaviour has a small semantic difference with our use of the term action as defined in [`mdr_actions`](../mdr_actions), which is why actions and behaviours are kept separately. We use the term action in the classical planning sense, namely an action can be thought of as a function with a certain set of preconditions that, if executed successfully, leads to a given set of effects; on the other hand, a behaviour precedes an action and, roughly speaking, controls what the action does. In the pick-and-place example above, the behaviour of "picking the closest object from the table" precedes a grasping action and defines that a robot should try to grasp the closest rather than a randomly selected object on the table, thereby allowing the robot to make more "optimal" decisions. + +All behaviours included here implement the [`scenario state base class`](https://github.com/b-it-bots/mas_execution_manager/blob/master/ros/src/mas_execution_manager/scenario_state_base.py) in `mas_execution_manager` and interact with the action clients in `mdr_actions`. Each behaviour is implemented in a dedicated Python script. + +## Organisation + +The metapackage is split into the following packages: +* [`mdr_navigation_behaviours`](mdr_navigation_behaviours): A collection of navigation-related behaviours +* [`mdr_perception_behaviours`](mdr_perception_behaviours): Behaviours for perceiving the world in a certain way +* [`mdr_manipulation_behaviours`](mdr_manipulation_behaviours): A group of behaviours for manipulating objects in the world +* [`mdr_knowledge_behaviours`](mdr_knowledge_behaviours): Functionalities that let a robot use its knowledge about the world in order to make informed decisions + +## Dependencies + +* `mas_execution_manager` +* `mas_knowledge_base` +* The actions and action clients in the `mdr_actions` metapackage + +## Implemented behaviours + +### `mdr_navigation_behaviours` + +* `move_base`: Moves through a set of predefined navigation waypoints, updating the location of the robot in the knowledge base during the process + +### `mdr_perception_behaviours` + +* `perceive_planes`: Scans a plane (or a set of planes) assuming that the robot's camera is already oriented towards the plane(s) + +### `mdr_manipulation_behaviours` + +* `pick_closest_from_surface`: Given a surface and a set of objects on it, finds the closest object with respect to the base_link frame of the robot +* `place`: Places an object on a surface whose name starts with a given prefix +* `place_based_on_category`: Places an object on a surface that contains objects of the same category (if objects of the same category are present on multiple surfaces, chooses the surfaces with the highest number of objects of that category) +* `throw_object_in`: Throws an object in a given throwing target + +### `mdr_knowledge_behaviours` + +* `check_empty_surface`: Checks whether a given surface is empty based on the most recent knowledge in the knowledge base From 40db0f561f602bc1df917ea22ecc8ea4dc172af7 Mon Sep 17 00:00:00 2001 From: Alex Mitrevski Date: Thu, 27 Dec 2018 12:06:56 +0100 Subject: [PATCH 28/96] Removed the knowledge retrieval parameters from the action client launchers --- .../mdr_manipulation_actions/mdr_pickup_action/README.md | 2 -- .../mdr_pickup_action/ros/launch/pickup_client.launch | 4 ---- .../mdr_manipulation_actions/mdr_place_action/README.md | 2 -- .../mdr_place_action/ros/launch/place_client.launch | 4 ---- .../mdr_navigation_actions/mdr_move_base_action/README.md | 2 -- .../mdr_move_base_action/ros/launch/move_base_client.launch | 4 ---- .../mdr_perceive_plane_action/README.md | 3 --- .../ros/launch/perceive_plane_client.launch | 2 -- 8 files changed, 23 deletions(-) diff --git a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_pickup_action/README.md b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_pickup_action/README.md index 34e875a66..a61fdf432 100644 --- a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_pickup_action/README.md +++ b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_pickup_action/README.md @@ -78,8 +78,6 @@ The following parameters need to be passed when launching the action client: * ``action_timeout``: Maximum time (in seconds) that we are willing to wait for the action to be executed * ``action_dispatch_topic``: Name of the topic at which the plan dispatcher sends action requests * ``action_feedback_topic``: Name of the topic at which the action sends feedback to the plan dispatcher -* ``knowledge_update_service``: Name of a service used for updating the planning problem as the world changes -* ``attribute_fetching_service``: Name of a service used for retrieving attributes representing the current knowledge about the world * ``grasping_pose_frame``: Name of the frame in which grasping is performed (default: 'base_link') ## Action execution summary diff --git a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_pickup_action/ros/launch/pickup_client.launch b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_pickup_action/ros/launch/pickup_client.launch index 995490d51..b00d629a6 100644 --- a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_pickup_action/ros/launch/pickup_client.launch +++ b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_pickup_action/ros/launch/pickup_client.launch @@ -2,8 +2,6 @@ - - @@ -11,8 +9,6 @@ - - diff --git a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_place_action/README.md b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_place_action/README.md index 3e0c135e2..b432607b6 100644 --- a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_place_action/README.md +++ b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_place_action/README.md @@ -71,8 +71,6 @@ The following parameters need to be passed when launching the action client: * ``action_timeout``: Maximum time (in seconds) that we are willing to wait for the action to be executed * ``action_dispatch_topic``: Name of the topic at which the plan dispatcher sends action requests * ``action_feedback_topic``: Name of the topic at which the action sends feedback to the plan dispatcher -* ``knowledge_update_service``: Name of a service used for updating the planning problem as the world changes -* ``attribute_fetching_service``: Name of a service used for retrieving attributes representing the current knowledge about the world * ``placing_pose_frame``: Name of the frame in which placing is performed (default: 'base_link') ## Action execution summary diff --git a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_place_action/ros/launch/place_client.launch b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_place_action/ros/launch/place_client.launch index 098b273b3..610a4555c 100644 --- a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_place_action/ros/launch/place_client.launch +++ b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_place_action/ros/launch/place_client.launch @@ -2,8 +2,6 @@ - - @@ -11,8 +9,6 @@ - - diff --git a/mdr_planning/mdr_actions/mdr_navigation_actions/mdr_move_base_action/README.md b/mdr_planning/mdr_actions/mdr_navigation_actions/mdr_move_base_action/README.md index 32d626da2..7540ca566 100644 --- a/mdr_planning/mdr_actions/mdr_navigation_actions/mdr_move_base_action/README.md +++ b/mdr_planning/mdr_actions/mdr_navigation_actions/mdr_move_base_action/README.md @@ -76,8 +76,6 @@ The following parameters need to be passed when launching the action client: * ``action_timeout``: Maximum time (in seconds) that we are willing to wait for the action to be executed * ``action_dispatch_topic``: Name of the topic at which the plan dispatcher sends action requests * ``action_feedback_topic``: Name of the topic at which the action sends feedback to the plan dispatcher -* ``knowledge_update_service``: Name of a service used for updating the planning problem as the world changes -* ``attribute_fetching_service``: Name of a service used for retrieving attributes representing the current knowledge about the world ## Action execution summary diff --git a/mdr_planning/mdr_actions/mdr_navigation_actions/mdr_move_base_action/ros/launch/move_base_client.launch b/mdr_planning/mdr_actions/mdr_navigation_actions/mdr_move_base_action/ros/launch/move_base_client.launch index 3cd0c3e57..10a356294 100644 --- a/mdr_planning/mdr_actions/mdr_navigation_actions/mdr_move_base_action/ros/launch/move_base_client.launch +++ b/mdr_planning/mdr_actions/mdr_navigation_actions/mdr_move_base_action/ros/launch/move_base_client.launch @@ -2,8 +2,6 @@ - - @@ -11,7 +9,5 @@ - - diff --git a/mdr_planning/mdr_actions/mdr_perception_actions/mdr_perceive_plane_action/README.md b/mdr_planning/mdr_actions/mdr_perception_actions/mdr_perceive_plane_action/README.md index c3bef11e1..f93961d41 100644 --- a/mdr_planning/mdr_actions/mdr_perception_actions/mdr_perceive_plane_action/README.md +++ b/mdr_planning/mdr_actions/mdr_perception_actions/mdr_perceive_plane_action/README.md @@ -78,9 +78,6 @@ The following parameters need to be passed when launching the action client: * ``clear_plane_memory``: If set to true, all objects previously seen on a plane will be deleted from the knowledge base before new objects are added to it (default false) * ``action_dispatch_topic``: Name of the topic at which the plan dispatcher sends action requests * ``action_feedback_topic``: Name of the topic at which the action sends feedback to the plan dispatcher -* ``knowledge_update_service``: Name of a service used for updating the planning problem as the world changes -* ``attribute_fetching_service``: Name of a service used for retrieving attributes representing the current knowledge -about the world ## Dependencies diff --git a/mdr_planning/mdr_actions/mdr_perception_actions/mdr_perceive_plane_action/ros/launch/perceive_plane_client.launch b/mdr_planning/mdr_actions/mdr_perception_actions/mdr_perceive_plane_action/ros/launch/perceive_plane_client.launch index 5dd8e04b0..f8ef50e31 100644 --- a/mdr_planning/mdr_actions/mdr_perception_actions/mdr_perceive_plane_action/ros/launch/perceive_plane_client.launch +++ b/mdr_planning/mdr_actions/mdr_perception_actions/mdr_perceive_plane_action/ros/launch/perceive_plane_client.launch @@ -12,7 +12,5 @@ - - From 9177218b58a97d9baa22135ef3beb01970eee4db Mon Sep 17 00:00:00 2001 From: Alex Mitrevski Date: Fri, 28 Dec 2018 21:46:56 +0100 Subject: [PATCH 29/96] [rosplan_interface] Added a default PDDL domain --- .../config/default_domain.pddl | 114 ++++++++++++++++++ .../ros/launch/rosplan.launch | 2 +- 2 files changed, 115 insertions(+), 1 deletion(-) create mode 100644 mdr_planning/mdr_rosplan_interface/config/default_domain.pddl diff --git a/mdr_planning/mdr_rosplan_interface/config/default_domain.pddl b/mdr_planning/mdr_rosplan_interface/config/default_domain.pddl new file mode 100644 index 000000000..b84d6456b --- /dev/null +++ b/mdr_planning/mdr_rosplan_interface/config/default_domain.pddl @@ -0,0 +1,114 @@ +(define (domain default-domestic-domain) + + (:requirements :strips :typing :fluents :disjunctive-preconditions :durative-actions) + + (:types + waypoint + object + category + robot + door + plane + ) + + (:predicates + (robot_name ?bot - robot) + (object_category ?obj - object ?cat - category) + (robot_at ?bot - robot ?wp - waypoint) + (door_at ?door - door ?wp - waypoint) + (object_at ?obj - object ?wp - waypoint) + (plane_at ?plane - plane ?wp - waypoint) + (door_open ?door - door) + (belongs_to ?plane - plane ?obj - object) + (unexplored ?plane - plane) + (explored ?plane - plane) + (on ?obj - object ?plane - plane) + (holding ?bot - robot ?obj - object) + (empty_gripper ?bot - robot) + ) + + (:durative-action move_base + :parameters (?bot - robot ?from ?to - waypoint) + :duration ( = ?duration 10) + :condition (and + (at start (robot_at ?bot ?from)) + ) + :effect (and + (at start (not (robot_at ?bot ?from))) + (at end (robot_at ?bot ?to)) + ) + ) + + (:durative-action open_cupboard + :parameters (?cupboard - door ?bot - robot ?c - waypoint) + :duration ( = ?duration 10) + :condition (and + (at start (door_at ?cupboard ?c)) + (at start (robot_at ?bot ?c)) + ) + :effect (and + (at end (door_open ?cupboard)) + ) + ) + + (:durative-action perceive_plane + :parameters (?plane - plane ?bot - robot ?wp - waypoint) + :duration ( = ?duration 10) + :condition (and + (at start (robot_at ?bot ?wp)) + (at start (plane_at ?plane ?wp)) + (at start (unexplored ?plane)) + ) + :effect (and + (at start (not (unexplored ?plane))) + (at end (explored ?plane)) + ) + ) + + (:durative-action pickup + :parameters (?obj - object ?plane - plane ?bot - robot ?wp - waypoint) + :duration ( = ?duration 10) + :condition (and + (at start (robot_at ?bot ?wp)) + (at start (plane_at ?plane ?wp)) + (at start (explored ?plane)) + (at start (on ?obj ?plane)) + (at start (empty_gripper ?bot)) + ) + :effect (and + (at start (not (on ?obj ?plane))) + (at start (not (empty_gripper ?bot))) + (at end (holding ?bot ?obj)) + ) + ) + + (:durative-action place + :parameters (?obj - object ?plane - plane ?bot - robot ?wp - waypoint) + :duration ( = ?duration 10) + :condition (and + (at start (robot_at ?bot ?wp)) + (at start (plane_at ?plane ?wp)) + (at start (holding ?bot ?obj)) + ) + :effect (and + (at start (not (holding ?bot ?obj))) + (at start (empty_gripper ?bot)) + (at end (on ?obj ?plane)) + ) + ) + + (:durative-action throw + :parameters (?obj - object ?dest_obj - object ?bot - robot ?wp - waypoint) + :duration ( = ?duration 10) + :condition (and + (at start (robot_at ?bot ?wp)) + (at start (object_at ?dest_obj ?wp)) + (at start (holding ?bot ?obj)) + ) + :effect (and + (at start (not (holding ?bot ?obj))) + (at start (empty_gripper ?bot)) + (at end (in ?obj ?dest_obj)) + ) + ) +) diff --git a/mdr_planning/mdr_rosplan_interface/ros/launch/rosplan.launch b/mdr_planning/mdr_rosplan_interface/ros/launch/rosplan.launch index 5a2072ea6..e2ac4a8d8 100644 --- a/mdr_planning/mdr_rosplan_interface/ros/launch/rosplan.launch +++ b/mdr_planning/mdr_rosplan_interface/ros/launch/rosplan.launch @@ -3,7 +3,7 @@ - + From e0e70381a6e97825bac4d6c1221c4747fb11a8cf Mon Sep 17 00:00:00 2001 From: Alex Mitrevski Date: Fri, 28 Dec 2018 22:42:26 +0100 Subject: [PATCH 30/96] Added an initial implementation of an object finding action --- .../mdr_find_object_action/CMakeLists.txt | 57 ++++++++++++ .../mdr_find_object_action/README.md | 88 +++++++++++++++++++ .../mdr_find_object_action/package.xml | 37 ++++++++ .../ros/action/FindObject.action | 16 ++++ .../ros/launch/find_object.launch | 9 ++ .../ros/launch/find_object_client.launch | 13 +++ .../ros/scripts/find_object_action | 64 ++++++++++++++ .../scripts/find_object_action_client_test | 20 +++++ .../ros/scripts/find_object_client | 79 +++++++++++++++++ .../src/mdr_find_object_action/__init__.py | 0 .../mdr_find_object_action/action_states.py | 65 ++++++++++++++ .../mdr_find_object_action/setup.py | 11 +++ 12 files changed, 459 insertions(+) create mode 100644 mdr_planning/mdr_actions/mdr_perception_actions/mdr_find_object_action/CMakeLists.txt create mode 100644 mdr_planning/mdr_actions/mdr_perception_actions/mdr_find_object_action/README.md create mode 100644 mdr_planning/mdr_actions/mdr_perception_actions/mdr_find_object_action/package.xml create mode 100644 mdr_planning/mdr_actions/mdr_perception_actions/mdr_find_object_action/ros/action/FindObject.action create mode 100644 mdr_planning/mdr_actions/mdr_perception_actions/mdr_find_object_action/ros/launch/find_object.launch create mode 100644 mdr_planning/mdr_actions/mdr_perception_actions/mdr_find_object_action/ros/launch/find_object_client.launch create mode 100755 mdr_planning/mdr_actions/mdr_perception_actions/mdr_find_object_action/ros/scripts/find_object_action create mode 100755 mdr_planning/mdr_actions/mdr_perception_actions/mdr_find_object_action/ros/scripts/find_object_action_client_test create mode 100755 mdr_planning/mdr_actions/mdr_perception_actions/mdr_find_object_action/ros/scripts/find_object_client create mode 100644 mdr_planning/mdr_actions/mdr_perception_actions/mdr_find_object_action/ros/src/mdr_find_object_action/__init__.py create mode 100644 mdr_planning/mdr_actions/mdr_perception_actions/mdr_find_object_action/ros/src/mdr_find_object_action/action_states.py create mode 100644 mdr_planning/mdr_actions/mdr_perception_actions/mdr_find_object_action/setup.py diff --git a/mdr_planning/mdr_actions/mdr_perception_actions/mdr_find_object_action/CMakeLists.txt b/mdr_planning/mdr_actions/mdr_perception_actions/mdr_find_object_action/CMakeLists.txt new file mode 100644 index 000000000..c7a31e344 --- /dev/null +++ b/mdr_planning/mdr_actions/mdr_perception_actions/mdr_find_object_action/CMakeLists.txt @@ -0,0 +1,57 @@ +cmake_minimum_required(VERSION 2.8.3) +project(mdr_find_object_action) + +find_package(catkin REQUIRED COMPONENTS + rospy + roslint + genmsg + message_generation + actionlib + actionlib_msgs + mas_perception_libs + mcr_perception_msgs + mas_knowledge_base + mdr_move_base_action + mdr_perceive_plane_action +) + +catkin_python_setup() +roslint_python() + +add_action_files(DIRECTORY ros/action + FILES + FindObject.action +) + +generate_messages( + DEPENDENCIES + actionlib_msgs + mcr_perception_msgs +) + +catkin_package( + CATKIN_DEPENDS + rospy + actionlib + actionlib_msgs + message_runtime + mas_perception_libs + mcr_perception_msgs + mas_knowledge_base + mdr_move_base_action + mdr_perceive_plane_action +) + +include_directories( + ${catkin_INCLUDE_DIRS} +) + +install(PROGRAMS + ros/scripts/find_object_action + ros/scripts/find_object_client_test + DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}/scripts +) + +install(DIRECTORY ros/launch/ + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch +) diff --git a/mdr_planning/mdr_actions/mdr_perception_actions/mdr_find_object_action/README.md b/mdr_planning/mdr_actions/mdr_perception_actions/mdr_find_object_action/README.md new file mode 100644 index 000000000..f94c4e35e --- /dev/null +++ b/mdr_planning/mdr_actions/mdr_perception_actions/mdr_find_object_action/README.md @@ -0,0 +1,88 @@ +# ``mdr_find_object_action`` + +An action for finding objects in a familiar environment. The action includes an action server as well as an action client that interacts with ROSPlan. + +## Action definition + +### Goal constants: + +#### Goal types: + +* ``int32 NAMED_OBJECT=1`` +* ``int32 OBJECT_CATEGORY=2`` + +### Goal: + +* ``int32 goal_type``: `NAMED_OBJECT` or `OBJECT_CATEGORY` +* ``string object_name``: Name of an object if `goal_type` is `NAMED_OBJECT`; name of an object category if `goal_type` is `OBJECT_CATEGORY` + +### Result: + +* ``bool success``: Indicates whether the action was successfully completed +* ``mcr_perception_msgs/Object obj``: The object (if found) +* ``string object_location``: The location of the object if the object was found +* ``string relation``: The relation of the found object and its location (such as "on" or "in") + +### Feedback: + +* ``string current_state`` +* ``string message`` + +## Directory structure + +``` +├── CMakeLists.txt +├── package.xml +├── README.md +├── setup.py +└── ros +    ├── action +    │   └── FindObject.action +    ├── launch +    │   ├── find_object.launch +    │   └── find_object_client.launch +    ├── scripts +    │   ├── find_object_action +    │   ├── find_object_client +    │   └── find_object_action_client_test +    └── src +    └── mdr_find_object_action +    ├── action_states.py +    └── __init__.py +``` + +## Launch file parameters + +### Action server + +The following arguments may be passed when launching the action server: + +* ``ontology_url``: URL of an ontology file (if a local ontology is loaded, this parameter should be set to the the absolute path of the ontology prefixed by `file://`) +* ``ontology_class_prefix``: Prefix of the entities in the ontology +* ``retry_count_on_failure``: Number of times to retry the action if the execution fails (default 0) +* ``timeout_s``: Seconds to wait for the action to be executed (default 120.0) + +### Action client + +The action client that interacts with ROSPlan inherits from the action client base class defined in ``mdr_rosplan_interface``. + +The following parameters need to be passed when launching the action client: +* ``action_name``: Name of the action as used in the planning domain +* ``server_name``: Name of the ``find_object`` action server +* ``action_timeout``: Maximum time (in seconds) that we are willing to wait for the action to be executed +* ``action_dispatch_topic``: Name of the topic at which the plan dispatcher sends action requests +* ``action_feedback_topic``: Name of the topic at which the action sends feedback to the plan dispatcher + +## Dependencies + +* ``mas_perception_libs`` +* ``mcr_perception_msgs`` +* ``mas_knowledge_base`` +* ``mdr_move_base_action`` +* ``mdr_perceive_plane_action`` + +## Example usage + +1. Run the robot simulation: ``roslaunch mas__bringup_sim robot.launch`` +2. Run the action server: ``roslaunch mas__find_object_action find_object.launch`` +3. Run the client example: ``rosrun mdr_find_object_action find_object_action_client_test`` diff --git a/mdr_planning/mdr_actions/mdr_perception_actions/mdr_find_object_action/package.xml b/mdr_planning/mdr_actions/mdr_perception_actions/mdr_find_object_action/package.xml new file mode 100644 index 000000000..55e647ecf --- /dev/null +++ b/mdr_planning/mdr_actions/mdr_perception_actions/mdr_find_object_action/package.xml @@ -0,0 +1,37 @@ + + + mdr_find_object_action + 1.0.0 + An action for finding objects in a familiar environment + + Alex Mitrevski + MAS robotics + + GPLv3 + + catkin + genmsg + + rospy + roslint + message_generation + actionlib + actionlib_msgs + mas_perception_libs + mcr_perception_msgs + mas_knowledge_base + mdr_move_base_action + mdr_perceive_plane_action + + rospy + actionlib + actionlib_msgs + message_runtime + mas_perception_libs + mcr_perception_msgs + mas_knowledge_base + mdr_move_base_action + mdr_perceive_plane_action + + + diff --git a/mdr_planning/mdr_actions/mdr_perception_actions/mdr_find_object_action/ros/action/FindObject.action b/mdr_planning/mdr_actions/mdr_perception_actions/mdr_find_object_action/ros/action/FindObject.action new file mode 100644 index 000000000..9bf4223a5 --- /dev/null +++ b/mdr_planning/mdr_actions/mdr_perception_actions/mdr_find_object_action/ros/action/FindObject.action @@ -0,0 +1,16 @@ +int32 NAMED_OBJECT=1 +int32 OBJECT_CATEGORY=2 + +# goal definition +int32 goal_type +string object_name +--- +# result definition +bool success +mcr_perception_msgs/Object obj +string object_location +string relation +--- +# feedback +string current_state +string message diff --git a/mdr_planning/mdr_actions/mdr_perception_actions/mdr_find_object_action/ros/launch/find_object.launch b/mdr_planning/mdr_actions/mdr_perception_actions/mdr_find_object_action/ros/launch/find_object.launch new file mode 100644 index 000000000..fd8ed0c7e --- /dev/null +++ b/mdr_planning/mdr_actions/mdr_perception_actions/mdr_find_object_action/ros/launch/find_object.launch @@ -0,0 +1,9 @@ + + + + + + + + + diff --git a/mdr_planning/mdr_actions/mdr_perception_actions/mdr_find_object_action/ros/launch/find_object_client.launch b/mdr_planning/mdr_actions/mdr_perception_actions/mdr_find_object_action/ros/launch/find_object_client.launch new file mode 100644 index 000000000..b70002cb5 --- /dev/null +++ b/mdr_planning/mdr_actions/mdr_perception_actions/mdr_find_object_action/ros/launch/find_object_client.launch @@ -0,0 +1,13 @@ + + + + + + + + + + + + + diff --git a/mdr_planning/mdr_actions/mdr_perception_actions/mdr_find_object_action/ros/scripts/find_object_action b/mdr_planning/mdr_actions/mdr_perception_actions/mdr_find_object_action/ros/scripts/find_object_action new file mode 100755 index 000000000..9ba77f665 --- /dev/null +++ b/mdr_planning/mdr_actions/mdr_perception_actions/mdr_find_object_action/ros/scripts/find_object_action @@ -0,0 +1,64 @@ +#!/usr/bin/env python +import rospy +import smach +from smach_ros import ActionServerWrapper, IntrospectionServer +from mdr_find_object_action.msg import FindObjectAction +from mdr_find_object_action.action_states import FindObject, SetActionLibResult + +class FindObjectSkill(smach.StateMachine): + def __init__(self): + smach.StateMachine.__init__(self, outcomes=['OVERALL_SUCCESS', 'OVERALL_FAILED', 'PREEMPTED'], + input_keys=['find_object_goal'], + output_keys=['find_object_feedback', 'find_object_result']) + + ontology_url = rospy.get_param('~ontology_url', '') + ontology_class_prefix = rospy.get_param('~ontology_class_prefix', '') + retry_count_on_failure = int(rospy.get_param('~retry_count_on_failure', '0')) + timeout_s = float(rospy.get_param('~timeout_s', '120.')) + if not ontology_url: + rospy.logerr('[FIND_OBJECT] Ontology url not specified') + + if not ontology_class_prefix: + rospy.logerr('[FIND_OBJECT] Ontology class prefix not specified') + + with self: + smach.StateMachine.add('FIND_OBJECT', FindObject(ontology_url, + ontology_class_prefix, + retry_count_on_failure, + timeout_s), + transitions={'succeeded': 'SET_ACTION_LIB_SUCCESS', + 'failed': 'SET_ACTION_LIB_FAILURE', + 'timeout': 'SET_ACTION_LIB_FAILURE'}) + + smach.StateMachine.add('SET_ACTION_LIB_SUCCESS', SetActionLibResult(True), + transitions={'succeeded': 'OVERALL_SUCCESS'}) + + smach.StateMachine.add('SET_ACTION_LIB_FAILURE', SetActionLibResult(False), + transitions={'succeeded': 'OVERALL_FAILED'}) + + +if __name__ == '__main__': + rospy.init_node('find_object_server') + + # Construct state machine + sm = FindObjectSkill() + + # smach viewer + sis = IntrospectionServer('find_object_smach_viewer', sm, '/FIND_OBJECT_SMACH_VIEWER') + sis.start() + + # Construct action server wrapper + asw = ActionServerWrapper( + server_name='find_object_server', + action_spec=FindObjectAction, + wrapped_container=sm, + succeeded_outcomes=['OVERALL_SUCCESS'], + aborted_outcomes=['OVERALL_FAILED'], + preempted_outcomes=['PREEMPTED'], + goal_key='find_object_goal', + feedback_key='find_object_feedback', + result_key='find_object_result') + + # Run the server on a background thread + asw.run_server() + rospy.spin() diff --git a/mdr_planning/mdr_actions/mdr_perception_actions/mdr_find_object_action/ros/scripts/find_object_action_client_test b/mdr_planning/mdr_actions/mdr_perception_actions/mdr_find_object_action/ros/scripts/find_object_action_client_test new file mode 100755 index 000000000..4b101d5de --- /dev/null +++ b/mdr_planning/mdr_actions/mdr_perception_actions/mdr_find_object_action/ros/scripts/find_object_action_client_test @@ -0,0 +1,20 @@ +#! /usr/bin/env python +import rospy +import actionlib +from mdr_find_object_action.msg import FindObjectAction, FindObjectGoal + +if __name__ == '__main__': + rospy.init_node('find_object_client_test') + client = actionlib.SimpleActionClient('find_object_server', FindObjectAction) + client.wait_for_server() + goal = FindObjectGoal() + goal.goal_type = FindObjectGoal.NAMED_OBJECT + goal.object_name = 'CoffeeMug' + try: + timeout = 45.0 + rospy.loginfo('Sending action lib goal to find_object_server') + client.send_goal(goal) + client.wait_for_result(rospy.Duration.from_sec(int(timeout))) + rospy.loginfo(client.get_result()) + except: + pass diff --git a/mdr_planning/mdr_actions/mdr_perception_actions/mdr_find_object_action/ros/scripts/find_object_client b/mdr_planning/mdr_actions/mdr_perception_actions/mdr_find_object_action/ros/scripts/find_object_client new file mode 100755 index 000000000..253f49662 --- /dev/null +++ b/mdr_planning/mdr_actions/mdr_perception_actions/mdr_find_object_action/ros/scripts/find_object_client @@ -0,0 +1,79 @@ +#!/usr/bin/env python +import rospy +import actionlib + +from mdr_rosplan_interface.action_client_base import ActionClientBase +from mdr_find_object_action.msg import FindObjectAction, FindObjectGoal + +class FindObjectClient(ActionClientBase): + '''Defines a client for an action for finding objects. + + @author Alex Mitrevski + @contact aleksandar.mitrevski@h-brs.de + + ''' + def __init__(self): + super(FindObjectClient, self).__init__() + self.object_name = '' + while not rospy.is_shutdown(): + rospy.sleep(0.1) + + def call_action(self, msg): + # we only react to calls to this action + if self.action_name != msg.name.lower(): + return + + self.action_id = msg.action_id + + client = actionlib.SimpleActionClient(self.action_server_name, FindObjectAction) + client.wait_for_server() + goal = self.get_action_message(msg) + + # calling the actionlib server and waiting for the execution to end + rospy.loginfo('[FIND_OBJECT] Sending action lib goal to' + self.action_server_name) + client.send_goal(goal) + client.wait_for_result(rospy.Duration.from_sec(int(self.action_timeout))) + result = client.get_result() + + if result and result.success: + rospy.loginfo('[FIND_OBJECT] %s %s', result.object_location, result.relation) + self.update_knowledge_base(self.object_name, result.object_location, result.relation) + self.send_action_feedback(True) + else: + self.send_action_feedback(False) + + def get_action_message(self, rosplan_action_msg): + '''Reads the action parameters and uses them to initialise an actionlib message. + ''' + goal = FindObjectGoal() + for param in rosplan_action_msg.parameters: + if param.key == 'obj_name': + goal.goal_type = FindObjectGoal.NAMED_OBJECT + goal.object_name = param.value + self.object_name = param.value + elif param.key == 'obj_category': + goal.goal_type = FindObjectGoal.OBJECT_CATEGORY + goal.object_name = param.value + self.object_name = param.value # TODO: make the name unique + elif param.key == 'bot': + self.robot_name = param.value + return goal + + def update_knowledge_base(self, object_name, object_location, relation): + '''TODO: Decide what to do with the detected object; this is not so simple + since there are multiple cases that need to be considered: + * the object was already in the knowledge base + * the object's location was taken from the ontology and then verified; + in this case, we want to insert a relation and save the object + * the robot was searching for an object of a given category and found + a specific instance; this is similar to the previous case, but + we need to make sure that the name of the object is unique + ''' + pass + +if __name__ == '__main__': + rospy.init_node('find_object_client') + try: + FindObjectClient() + except rospy.ROSInterruptException: + pass diff --git a/mdr_planning/mdr_actions/mdr_perception_actions/mdr_find_object_action/ros/src/mdr_find_object_action/__init__.py b/mdr_planning/mdr_actions/mdr_perception_actions/mdr_find_object_action/ros/src/mdr_find_object_action/__init__.py new file mode 100644 index 000000000..e69de29bb diff --git a/mdr_planning/mdr_actions/mdr_perception_actions/mdr_find_object_action/ros/src/mdr_find_object_action/action_states.py b/mdr_planning/mdr_actions/mdr_perception_actions/mdr_find_object_action/ros/src/mdr_find_object_action/action_states.py new file mode 100644 index 000000000..68f11f6b3 --- /dev/null +++ b/mdr_planning/mdr_actions/mdr_perception_actions/mdr_find_object_action/ros/src/mdr_find_object_action/action_states.py @@ -0,0 +1,65 @@ +#!/usr/bin/env python +import rospy +import smach +from mas_knowledge_utils.domestic_ontology_interface import DomesticOntologyInterface +from mas_knowledge_base.domestic_kb_interface import DomesticKBInterface +from mdr_find_object_action.msg import FindObjectGoal, FindObjectResult, FindObjectFeedback + +class FindObject(smach.State): + def __init__(self, ontology_url, ontology_class_prefix, + number_of_retries=0, timeout=120.): + smach.State.__init__(self, outcomes=['succeeded', 'failed', 'timeout'], + input_keys=['find_object_goal'], + output_keys=['find_object_feedback', + 'obj_location', 'relation']) + self.ontology_interface = DomesticOntologyInterface(ontology_url, + ontology_class_prefix) + self.kb_interface = DomesticKBInterface() + + self.number_of_retries = number_of_retries + self.timeout = timeout + + def execute(self, userdata): + if userdata.find_object_goal.goal_type == FindObjectGoal.NAMED_OBJECT: + obj_name = userdata.find_object_goal.object_name + location, predicate = self.kb_interface.get_object_location(obj_name) + if location: + rospy.loginfo('[FIND_OBJECT] Found %s %s %s', obj_name, predicate, location) + # TODO: verify that the object is still at that location + userdata.obj_location = location + userdata.relation = predicate + return 'succeeded' + else: + rospy.loginfo('[FIND_OBJECT] %s not found in the knowledge base; querying the ontology', obj_name) + location = self.ontology_interface.get_default_storing_location(obj_name=obj_name) + if location: + predicate = 'in' + rospy.loginfo('[FIND_OBJECT] %s is usually %s %s', obj_name, predicate, location) + # TODO: check if the object is at the default location + userdata.obj_location = location + userdata.relation = predicate + return 'succeeded' + + rospy.loginfo('[FIND_OBJECT] %s not found', obj_name) + elif userdata.find_object_goal.goal_type == FindObjectGoal.OBJECT_CATEGORY: + return 'failed' + +class SetActionLibResult(smach.State): + def __init__(self, result): + smach.State.__init__(self, outcomes=['succeeded'], + input_keys=['find_object_goal', 'find_object_feedback', + 'obj_location', 'relation'], + output_keys=['find_object_feedback', 'find_object_result']) + self.result = result + + def execute(self, userdata): + if 'find_object_feedback' not in userdata: + userdata.find_object_feedback = FindObjectFeedback() + userdata.find_object_feedback.current_state = 'set_action_lib_result' + + result = FindObjectResult() + result.success = self.result + result.object_location = userdata.obj_location + result.relation = userdata.relation + userdata.find_object_result = result + return 'succeeded' diff --git a/mdr_planning/mdr_actions/mdr_perception_actions/mdr_find_object_action/setup.py b/mdr_planning/mdr_actions/mdr_perception_actions/mdr_find_object_action/setup.py new file mode 100644 index 000000000..d098f420c --- /dev/null +++ b/mdr_planning/mdr_actions/mdr_perception_actions/mdr_find_object_action/setup.py @@ -0,0 +1,11 @@ +#!/usr/bin/env python + +from distutils.core import setup +from catkin_pkg.python_setup import generate_distutils_setup + +d = generate_distutils_setup( + packages=['mdr_find_object_action'], + package_dir={'mdr_find_object_action': 'ros/src/mdr_find_object_action'} +) + +setup(**d) From 72932fa630312fd31f03d256f1c9cb0f4b3ddb20 Mon Sep 17 00:00:00 2001 From: Alex Mitrevski Date: Sat, 29 Dec 2018 11:07:52 +0100 Subject: [PATCH 31/96] Added the action execution library to the rosinstall file --- mas-domestic.rosinstall | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/mas-domestic.rosinstall b/mas-domestic.rosinstall index 5eebf0f38..43b71acc7 100644 --- a/mas-domestic.rosinstall +++ b/mas-domestic.rosinstall @@ -28,6 +28,11 @@ uri: https://github.com/b-it-bots/mas_knowledge_base.git version: master +- git: + local-name: action-execution + uri: https://github.com/b-it-bots/action-execution.git + version: master + - git: local-name: zbar_ros uri: https://github.com/b-it-bots/zbar_ros.git From e8e39a94b86d80e5a4bdfcada521ad9fc000042a Mon Sep 17 00:00:00 2001 From: Alex Mitrevski Date: Sat, 29 Dec 2018 12:15:24 +0100 Subject: [PATCH 32/96] Added a dependency diagram to the README and made various small changes there --- README.md | 51 +++++++++++++++++------- docs/images/repo_dependency_diagram.png | Bin 0 -> 27404 bytes 2 files changed, 37 insertions(+), 14 deletions(-) create mode 100644 docs/images/repo_dependency_diagram.png diff --git a/README.md b/README.md index 3dd368420..e1ecaf2b5 100644 --- a/README.md +++ b/README.md @@ -2,12 +2,36 @@ # mas_domestic_robotics +## Table of contents + +1. [Summary](#summary) +2. [Dependencies](#dependencies) +3. [Packages](#packages) + 1. [Planning](#planning) + 2. [Naming conventions](#naming-conventions) +4. [Getting started](#getting-started) +5. [Docker images](#docker-images) + 1. [Building the bitbots-domestic image](#building-the-bitbots-domestic-image) + 2. [Travis](#travis) +6. [License](#license) +6. [Acknowledgments](#acknowledgments) + ## Summary This repository contains various core domestic robotics functionalities developed by the Autonomous Systems group at Hochschule Bonn-Rhein-Sieg. The code in this repository is mostly ROS-based and is developed in a robot-independent manner. Robot-dependent code and/or configuration files are hosted in robot-specific repositories, e.g. [mas_cob](https://github.com/b-it-bots/mas_cob) for our Care-O-bot. +# Dependencies + +`mas_domestic_robotics` is not a standalone repository and depends on many other components. A diagram of the major dependencies is shown below: + +![mas_domestic_robotics repository dependency diagram](docs/images/repo_dependency_diagram.png) + +Most of the major dependencies are also developed in-house and, in the interest of modularity, are hosted in separate repositories. + +A more complete set of dependencies can be found in the [`mas-domestic.rosinstall`](mas-domestic.rosinstall) file. + ## Packages The functionalities in this repository are organised based on the main capabilities a domestic robot needs to possess. Each of these has its own ROS metapackage, which further includes a set of packages related to that particular functionality. A rough summary of the contents of these packages contents can be found below: @@ -23,18 +47,17 @@ The functionalities in this repository are organised based on the main capabilit The planning metapackage is where most of the high-level functionalites of our robots are implemented. We are developing our robots as skill-based agents; the planning metapackage includes *action* and *scenario* metapackages because of that. The action metapackage is further divided into metapackages for actions related to particular capabilities, while the scenario metapackage includes domain files and/or state machines for various scenarios (e.g. RoboCup@Home tasks, lab demos, and so forth). In principle, scenarios are built by integrating actions together, which is the main benefit of the skill-based agent framework. -### Naming Conventions +### Naming conventions All packages in our domestic code base start with the `mdr_` suffix; this stands for `mas_domestic_robotics`, which is the name of the top-level directory/ROS metapackage. - ## Getting started b-it-bots members can use [these instructions](https://github.com/b-it-bots/dev-env#setup) to setup a complete development environment for all our robots. For external users, the following instructions should get you a working system: -1. Setup a catkin workspace +1. Set up a catkin workspace ``` mkdir -p ~/catkin_ws/src && cd ~/catkin_ws @@ -46,17 +69,18 @@ For external users, the following instructions should get you a working system: ``` wstool update -t src rosdep install --from-paths src --ignore-src --rosdistro=kinetic -y - ``` - -3. Building your code +3. Build the code ``` catkin build ``` +If you encounter any problems, please check the list of [issues](https://github.com/b-it-bots/mas_domestic_robotics/issues) and open a new one if you don't see a discussion of the problem there. + ## Docker images -To test locally, you need to install [docker](https://docs.docker.com/install/linux/docker-ce/ubuntu/) and [docker-compose](https://docs.docker.com/compose/install/), + +If you want to test locally without installing our software and its dependencies, you need to install [docker](https://docs.docker.com/install/linux/docker-ce/ubuntu/) and [docker-compose](https://docs.docker.com/compose/install/). You can pull the image for this repository: @@ -64,19 +88,17 @@ You can pull the image for this repository: docker pull bitbots/bitbots-domestic:kinetic ``` - ### Building the bitbots-domestic image -To manually build this image, first cd into this repository, then: +To manually build this image, first cd into this repository, then execute ``` docker build -t bitbots/bitbots-domestic:kinetic . - ``` ### Travis -To check locally if your changes will pass the tests: +We use continuous integration to ensure the quality of our software. To check locally if your changes will pass the tests, execute ``` docker-compose build @@ -85,9 +107,10 @@ docker-compose run travis ## License -This project is licensed under the GPLv3 License - see the [LICENSE.md](LICENSE.md) file for details +This project is licensed under the GPLv3 License - see the [LICENSE.md](LICENSE.md) file for details. ## Acknowledgments -* Thanks to the many generations of b-it-bots@Home members. You can see a list of contributors [here](https://github.com/b-it-bots/mas_domestic_robotics/graphs/contributors). -* MAS staff and professors who have provided their advice and support +This software would be impossible without +* the many generations of b-it-bots@Home members. A list of contributors can be found [here](https://github.com/b-it-bots/mas_domestic_robotics/graphs/contributors) +* the MAS staff and professors who have provided their advice and support diff --git a/docs/images/repo_dependency_diagram.png b/docs/images/repo_dependency_diagram.png new file mode 100644 index 0000000000000000000000000000000000000000..3abb3c5a50ead20da8a9f8ebfbfb6a6380ca84ee GIT binary patch literal 27404 zcmeFZcT`l__U~J23kZ@_M2R8-l2vk6q7o%bPLh*kC@2aA1SN??$vNkoic-lrhayVO zAfd?NuCn{|IemWJgJ`?^=89Ip>=5Gry}oyp)&1$0fssKp^-po{K9% zAXj`Lkjt7lm%(pH^PQ@}4=h6&DRIaJ=3f#bGYkT`2YDg>OvN>BebU84#W>|++myad z7W&+vU+jkLy{9-h$&&Ye^TxSsuWrzglUp^4h%7g#F3{5F!RtV**0{n+os19_#E0zut_)81_W)#p3R{k=}eKAIG!8^>PQ{$@LHPgqo1{#< zl_iP^1!jS!o+@*SI-U5}nGi4Gcfz-R)7;)s;W4OhP(vPm4z zt;ga<$_3-?FBhn6#Bk+ZR8P|hhH?IQ54#U}1%{O*rJVbkp(;2xOmk>KMeOqAnGmNA zoO;dI)e`%-dhhGpHOQVKxP?}6>Z=flm+bFv<{t_`yb{bDqnAntuo|)4e4v&(Tm&TO zW_J0Ax{`&{iJ6-4L$Bm#oAjk6BDFhB_)GHI1r=7aNox5QauYxGUE=*DaqM!EoZU3L z8Km%73oR&y6y&cVY_(td6b5JzFMV8%+-yBRDE=OdHE!OD)XYx$~f z6QEtPGEoxG7fE-fO(CO{X(gcgsruL)9%$ zu;BEb8rSkbH_O#+*KJKCOXueO6WCGO^ynM*a_P7#?b7?Krf1tJ8S1r%bk2{-A+-|J zS9{;}T@hRg!L3qC7x|=Y&dFH(Z4*g$Gx`Q~l3F%f_}~`e<>Rj{ZqUH3=t~J=Q={sy z@<~ObJEdj4#+!ED+p?><)$+>)?H{@Jy^)AJ-7`xjn$17*rj8@>D9$rz_;6pU+(O*n zuTKn{9ZH}47aea$84$xCFTS>kp!wt{X! zu~h^=`!Oo*5*lmwJP(&NNX$M^!9K7DMjdxNGINB+;Y2HxP(s(akuURmhNJ%?&+OdSUzsGnAN5`=2Aj=rrL3)jq*`vYJ*`Wm?s}P&0YaupS#J8}cY7qM=$f0^NL%eS`H1Ldw^TdnO4=fde{c8E%Mkz#5oE1zS$I zW7bogWV^aCky{cTu8;I^9`k<9Z;}({jn*5|58QVl5q&6X&KXl4Y@yzzaFJ;AgqgDa zryd8ku)R#{@tj0R;Yo7PkP@;}@z!Jh&Vijd+AvXHqCy)!TgFsT|3EyFt<5e6JEdt6 zw&yr~`j6la$rB@8=C6`!jeUVF&XOUB45iCLjN1mE)rp*eh9cJ>O<6H-CDNy zh^7&^HL%dYWY0+IElTo-KCb3-)$7zfw<3qh*dlp2;WuTvc`9_xm{Q{p?^(vNWfg}j z(li#78eh~Nl!SS&svBL_(ncM#7i*PVD$89>b;>ChI;<$< z5IwCA=V4cGiTixakEl)z2qO|zY&^lnO^Ch$-AlJ)?zg{i%Sy_tSa`cNJR!hs_Uh9~ zLT8B{;vinbJhxSCm>e$`{Y<2LiIZ(Y2B-MU@x`u_a~vDYUV3!YKbLlAyq>+gAAo% ziu7xin|qndGfAU4wD+lGAgKxBn96Mx_7Wp0J1-VM@OyET1k>~f~6L~i=8)@R%Iy|WjnCVH9_ zakd-7d)M^r2lxk9<#fA)B}5btEsVy8U&)(7tF_OxdRN%{ zZlfThgvSrPY<_^Hrdnipq-zC*{z6U3_Ar;@u!fHkYG9?j#PKD9q}F~~7FngQ@|Bt6 zK9~}`1G|KS&1=ll7YgI0v}fA4<#49X5>uNqsdBPxbH};Vbxm>h`j4vxxC+zW>S}h# zL{j&Vj=&Q2IOy8PRp%bT{B1pr=RR}{3CKrHIh9^iy(u(!wkhf#l%H6gXYt5_l1sQ} zBio8Dg3cT1ml0H>F<`diLnz{arLk#eZWR{gO9cNyYRwmBk!+_T zz^6S~i-f~o;B5P$c;u}1RkAH#*^p{+^DrH@Fzb2Om5m7A%$X!y&62qJZtL#NR|94x z6d%FbF`0nxrDIS>1#ZRWk*9sxPw;`=^}>(mMBR)LeW*5?RvF9cgI3gw_=A=gU`k#` z4CbojC?-?Q)U(z0M3R)dRnMbkO^m-?#0wtn_^6wRuL~;Iu;->s$~h-6>QpbyDt|CA z)s3xU&;5>iAp{~p`VB%4*H!x_g^^^F+d~^2InhL@!jh@O1w!@2wY|2`?-4HeH$**p z^Qv)a2u##tI@{s&?|q>HJr~HZwGdJS{NBaL+`C`|=cdXtYh4NN%u*xmqLU3<>hV}r z8lUGU$?NQTi-K^=L-n3OVV#$;TEjn9)*mr-mL9p+yWG3jRJ;VDN*vVb`}w+bhqWr1 zl+!LV`}o;COK?u3wb&_H(BijCHpKO8SYh@TBFIV)VqmDXbbq8SLbvnzoS2X%GtpnO z`-H*CZt*jn5i!bF%SCb2gBn_-a2cfB2=N?8V#8$?@NKO+ETCBKe%jgnB1dh6)Ivlj zA$rY$n$BNix*r;pEiYS_!U-7q!iB4CWLw_Ec6TvprW8vcVKmu>vYP2D zhbsNFp$XMyBQ^}vv40Qr}b7Y5=O>f_TtnAmBJ2RmTFgGdb-jF zx=IONzAWdn-kr3Uklv_Ax`a>3T=E-YT`Hk!c{P1|^9zZ`}|<$~Lz z9L%A4yLshI8#fm{(iu2L7W}|VH%J)wDVZq)8aDS%)4jZGSYL#-U0(@UASLnaW5H3e z{2QblWK-v>{r!}vqYa_c>36i(lGIZ}E`Q2MYfkc9mq9T?eQrkOZfQH!4a7P;xU^@1 z8R`FfHO-6khacCkg9{evfd%)E;$k^aE|$A37y^O)2D=5}H(KdWH|X0G6_l_2xW!au8D*qD6@K5G5;Bm^Vr-HrQm&&l$7+;`8H%3jrr~4 zh=>UH{dHo|=&nUpt%}y*D-Z+~=K6~P?Df5CXw7`JH&H?mM{LYT1R58e`;zLuadez0 zw^UbEjaV0l^wxr8GU*B#IXUuh7LUfKuyCu$paHzCw4G)XxCPm>0n6yJ*JP~=I6*>O z{B#L^p4s;L#&y({r%}I7W!Z4(jD9TY|Mlxrn#SRLEi@D{{|eF@3;y_3)1=gGcd<*< zzu2e+mquGhXKr?uiJ5s|V1SO{iq}*ixX)pJ*Pr^&o;`ELysC`bCaN8-itR&sxxnWZ zMW(ws;D#DM+>B*U6mqZ7Z>56Nc!3X9D~_Xx2fs*7H8L|>es>kmrh4Ia zpH-`_y)b0CTt$2LH^}fE9-E18-q_&(QI9%Z9D^rcE?>SZEiLVNIOF%s8>{UjHUvSs z1SSu5badoLKxyyfgg_5#6}q*3`(^;f`;NvPvdkidx&%og{fS0TmYFNt*c9aEuAvKs z4}W+YH3t?8o?L_UDh7^Rfk2+pH?Lg+#s5w4@F0*UVDkQW!_UIbZo4s7e)sO(5^u;; zy{FVyy$A$64&O*ix9==;7-L`Zs>j^Y|AG0oDLO+S%P>(fF;bRSMAX#m0yvPpt4tD# zuT8<}UT21NRDb>KzYOFrZ~p&L{eNh#ta$a734O~2dcwyx&-%1F@63{>J76LgWbY;S zdg}C|nh<(LV;ms@4frad2_g-|41W#{OzE$9sO|ko$mOScBxGbY`rqCi9=f8QL6X>$ z!0-HmsDz-v#!HkTPpQAT3V~B!r-Xp(Kc&Z94f1*xq>T`09EZ9EC^NuO+@vdOHx}n) zK@>=8Ioj=Bhf}e9ioaY`4eC7I=LDvIKAfb6ot>TG0-e~{JHf;y3c z|H+_o`VABv+7{-tG8rh?A%jUO`mPj5!l%#o`!+IVsO#zGx9|)Trav8xlucx~yt+eu zI6B`8E{PQe_>}!h@Vaex6c2(-wEl6 z4HJ8hRWrgJ)87+;tUEeIeb=OETz=GZ))UmmXnE({4~E7n#jpkU1hV)?Mbl{{)0-VR zX70NcE~f`B6MH+>#EiMflf%}C3@&_I`Hoz(Kv#5Z2AzEfm@qFcs5mv(mxyRcWxxVH z;4wSSPBCN~yW1%4y zOpVS;rF92*a+^Oze5(`5{Vp;0iDE8{V$NY2JM-M>aCP84fGD6ufh(4ugS#@35MR@_ zrCyD?v%w*Z{?XRKl}`s>ih4{)=W}ahnJ1nlc+cSrk0|)FkOk^nT>mj(P3aj6_0bgP zwHiD^NE&6F?N-+0Fq@t8@y&^hCu7Zv!|SfK52(Pyg)pQ2g_(Tr?A0RVF3!6mII;j_D_whGES|a>*Wz&rFnH z(TIQl>IskrdhU9zdB-S%E7k?pGH#qodu7ipRVqZc)_H@YN09cHlL{@IX8jG6=oostVn?hi z>K|_^EpWCt_Z{IwG_DZEsaG}UxDuxPy;;(=yh7C&QCy@+J*dF!;|k20Hp~g|oIHK<@Lv3biA1(LdJ>jtKNJOqW&w7Mcn=7@95DqCdakQGlK-My*v(~}nL4Y*BbWwk0G?n$0wDHdmYJRzQTIcaIlkA#{J zIxTMk9tw)QsiWimlC{N#3AYT#g@gbFy7%#J*0c3_xS%?!dcz(eE%U_9mAyGdL4$>>xbZcyqm!0MJ{atg3xZ{ssko`hNs`GaOk zo_qn#S1(bc`-|3FhexwD;C@9O2_G&8%++@kGUjsuIK~z4Mk3lO1PlW*44!~55qMhs z5qhO>WPc2rQ3s{g)OEjElb$ZuE?mR)CXfOVmWyGq!W zNR%bvj)yXQWTX->QD-`?_vr#97xt!mmX%V%CHu1Dv6YB9QGY)X{xIeKH$9_YXTDL1 z`j_xGOyzwnqb{gt0^SE32UW~Kf4@mrOhkD20|C7CNf6^qGx7cLcZxbw)D>+{XI0vm zwYqyK3uHbJ-XWW}dBS{B%B(l>X0vg@qJ!RAAt1|6MTC!Tuiow%fD0xPam+bdHMA$w zEx1~8KOJPXT)mNn&bzNT@D@RXfZ=9?pieYw=StNx%7u>lZXA+ED1;^PQD#*Lml?+uieH+u9?75j9#02 z{W9#J99&m`9}mc|BfvczXXst*o`5u|$V5&iwMHrZf(mY(o;mezIma&G-dTogxug>|G%yQ9oEL>?Dr zdHw^r(G7EkWpKp?L}svh@X*?VWX1U;%h-2u;C8_TY+L177aFz~V~z*9Ah8mC$iT#%uJ)OUoOY zVmgmb41gO~T_;s2_*^ zpx)>kHZE2voMfdB<%HLfCngu}Em#_n{==CPD!D2L!Pmfi5FVY?*HBYykDfw{xF1<3 zqX%v~j~o~qCvbBoQY;`|L&Xisg;$VBp|iPO$8RNVn_USr1Donb#g0Np=&Wx$J1!dci?I{YXN)Sp+!ybE@;c{F)jswc8OgPmyesgP0 zaS7rr>W8j|(#w5O&H2we&X$tjttGylB+Fo3pYU^DI4OCeKpv~$zXy1nb6r-D&#*_S z4yHGnr0@%(cgwGu$aIRH?)^YkWJ-9LoS`O4j;H1E^tJ<@!Q*b0Gtq7hbuHzuP{?YH zRugW_9t_zeRPY=)4BU~G9`Ma!E!m0Pz&!@ZuXEa@IkB9HaZ|wr&j35y9g?5zfvuT0 z8otBgmJseKlP)G>qFQDLC#$oITtW1DJPf7`PTt8w%+z^DCbj;cX#**u-H;D3GkC;E zO~rJleEsXf$L4Cgnnk0Qd;@T5(ay>0-r~QBuB~Ag&$tAJViakVn&iJKHXJK62gwiR z@Mf9+pBNXX!wPO~^yeo>)@Da`1>v4=LkI4$=LZb*92?Z?$&EbLC4^9N+sy)^z|by# z$x(8A;6A@V_U<|F%E`N!ywA`GX)dNg+KCb415`;0cOiS3KIW1xWF2fa=TmP_Yo)Hr zlCL|g4qXHB;-s24`1*D)Z%(Wp$=Ig)fD;%aWL<`Q;C_t-MbS&y(SK`=_l(fQ(-oMr>u+~#YG#-i!38Ve$@kfg>19|Rd&yi&_F$m|1GAgPtcg;fD{ z%Bc8vOQ33Ov7W3!uVf}pm0L!w--Gm$_5v8w@Z`x8wE}Io-~>)8)T<%R6^ z(#ez5O8@@qcEI2uP&V@YzCMG5)Yz_N<3xa@5OsAxbpSGn>N|y%`B8eF8qU9gx`ir8 z*g~wjx_YZ_SN_?hn^xe6vX9_bvE0^AV!N~|tU@WlVM$9VA!EJGLFBRLq~HkFS+~Oq zB{jAEE}m%|8t1Jk9aYtZ74xl1=S#4*JdILEejQQKcYh4o+PEW=At^2^BN!!dvmr+LlVcpyVg)E^K+-1qB%4QWbImwz}uK8_b~$x&enk_2?D z!a7#BFp3hOpd<#vx+!|+%?S-HEpiG9kM+@FFE1}<^@6m_Oa=yqj_#|FWlAjUDhA1u zR{2Y#+R-7UNg-qY#zn&X{Ko()J%0RHy+9jPW{#mz`st)$4SKQ!QSsmF?_9?M=c5?c zZ_S}6dtN_*ogXcU`U4tvjmFT>(1!>xN|jp@_8=GN{Rq~dI3ag6XIudZ>J3kTubP^g z6crWg>p{Qf>({UT{{E+@rwx9D9l;>I)~qlepx+dU^c5aEWAR2`Y*SzKmA&3yN6B?u z+PkDxX#+c6$#mHmqZ7dK+5{-B!g{<}&q8B4b+Tt?To9nX47y18uAa`*#L&>tV*7=5 zF+YMfg3C$n9=lyUbaZssgj99*3m?~6_Wgy&y5!?{Ln9&YncI&1odk+q+?NvNks- z=QTr#z+T^Gcx7gAVVMt?Rqor<)Fsue3!naqjadC0eUIQ@5T5^4O+G#}R+O=yz zK|y*R+rI1K&VIsU@!g3c(@6NmT@Dko@-a(YfFI+ztz+af1>om(;lNOnBqA+00EcYQ z&tU278I1q@`7^jC6BCoBhZA~rXliQ8g6jewFcyW-urP<6`4{Z$g^bD>;!;u~yPX{4 z71jf5XtE$lXHEf8>J1Z%^0Ai8g}Z*NAuZUw*qt$4GMNK(4<59FInEV6S?8c>47|(H z-``)RX|1h20m37Ab{w!{M^xWB4u(ft{b|lmd5QwN8R+P`;`j!7dbWyNs02Vj&5(;- zPUX2_)B=Hgm5+#w+#Ih|$Q_c8=N~FbTe+bmp_nFF%D$&au8Dv;85+*Ua9O^_25!JQ zT4o;BXC^Bxz4P;D5zAdZ``6;);+~$uH;X2pgTvd~+ocs0dfM84`DxjYW5#-4qEgeI z+Y`ocBTR(x63WTVm64HA&K+`@u#?(dh=f|W;oP#hj#;XTq{b_43v+XUS!?U->+df7 zVJQxaov2b%W8+dDOvu&`(WJ;D73JmS?Tq77e)Dk;3q-u9et(Tl{9CcM^~56kiuq~+ z0R5T|WN7G?u$&&R71A}AG7G}N6tU3K8ZER(@W-<;GQI$WKi&~MJrEJ_$vB_m%&e?b zTaQ;{L}e%>CWMAQ_B?fUTUp1$(Be_re6`r<=+Rl3f`WohH#^!sDQh!esxMx=0L}nb zI$(j?U@kram-F`rm<-s4)Ep-D_%HEOBO23ZC^hNoUvUKH2F_fjf+B=jS>T{F%E9ie3eV%w;(YECcwG0~pIgW@cs~ zAt43^DQ6-OQWFCHhE}_tgZ{+mpLusvD(>6*XSqY&(T^KCI%J%QKnwxcyQ8)BRkeM( zd?sk-z`Qdvmp~YJomr&!2SM)!K{!Ps02umxT3P_cG0}WEmHQSk=0yXvYmlcQ&Hq3b ze{8M&S)vL?_fcr^ENjtg?L+%t}gBt?U zvHv#9U!NNedNTqyJ;#e?54swc&hob8|59vx>e?r;~;(Ogp1z z78d9P{NsO_lJV=;dD_)VnFD=NZ|lDe3^)MqEKwe1|D$EF^ndWYjs8TU|86qaona>Z zKNqALKixtF8JPi#a=}J9Tp$Z9;lK=5MO!;>U zTFbKsR+dwrPXGAj0o-8mA^ig`@1}B>0YgtqOKWX?4xSojSqEbRT`KSb3mdS%)IUuV z{PkZry2=w1kcRU#fUD8aJOfraDONa=nVxQ5u1PgZ?<=i%3w0ObSPGiD%r1Ih1zye} z&Gb8P83EUYW2_4&;X-{2uA$`0m8+qf$?C6g4#X92R9oM3A$yhA547VsPg*D3Icd zY;}glf3H|KpuRNmC~#!FcXcTD=FOWUc^ZHCSJTnI;=O{j^k>T7%us)p?KtRRkW(|Q z2I<8yxfypHRW~>>ZeJ4fo&a?S+<75@ygR;1+ZxsQ4@6#5cZfG9G42L*x+)1rkF@lD zH^~s|Wa!zJk?*jN-QP(kEekbniGpVP--tGAOQdcS1`>KY+PC&TJs6WzKCa%0;z<~biyJm9H!r&Kt9j%fr?)>6R zOHT*NLtD6E^yF_*0m`v#T=>@Qzt?FJQ1%yW8L4|RLq@M1Kh#G`68?-6VfrB*asE_3 zy)Ug1tM~;oHeMS9&4#4w`=+CB-_xKDSDSk16Zuwz??|&-wh;?(aq*}3On#_ii8PKkeIp{uq9zC8+=a3gbph;Wl15iuwlBzma zG}a-nI@g_i!CSY1+z1d&ga!#vWEnLGiJZwz*ot&$7q4YvnJ=6;8dKNqXkGd$k8$ow zL-pW=TT#u5@!M9Xz{p6s<7DJIh(&>hXyZ!5Rz2!ss`yzX9mtH4rI!% zuF)h?vHpb~@FUtv-g!Imxb{$!V5GL9FJLLv311V5+`e};1jppkc@3adw!$UgK5gX!_i zuA2#V368JRHqywlE2{_tRy}!2@!F4|=>&U+TGw6DmJ^Q#h?1;4TpRoO6v-8CX|$)K zw+2>V>+QBzAt%qM%d2y8a#B+6$IH8%$v_eHMMb{PnDf;-j`OA?(7tc#J>6=5=NpoK zZWa`U!&fDUsi`_KKm{moSZ#K`eLDprppSpiSq9Z)yG3_?cEDIvSWm&qqvg3-oM){!4mUmFJ|=L$Sy+I^ESV^f)uE zUfyRNk8m8SOPaq3W@?*GwPIYK4g?uag_w9Fkb ztf-X&8d6^(1=(u|Pc5E&(8C#AiMN=YTYb&R+{DEim$nkuO^Hk6vp`W46<|aMYp?1Tkkix_*L^(q_K7htHq z8M{JC8i?y^EIAJpaWlf*yHi4an*vcIcP}H#&Z%ND+ygbWT6!k4-Qc);{hGSq@u+g$ zDGw$~T-sL|uAF#3KFTQZifdDQNPp1H%wr;s-_E97B3}3w%~@P*p;6a~BaB+$Ov|8e z_6KCGv_5Fqk7Z&W=d0%1gfY;uPL9g~Y?U={m|V6!f(e$oJyvh{KUmpWK3}vlqffL@ z9lUl;He}nf{t;iZI*#4L%#zfT{`uA>rU+?S^7(;%3)W2)#1xCqPe=15k@XPWoZF%k z!h?kyLwO@BVvqkNlmsC`=VbK5G%jS{TNwANW(2-un8t_xYF!T9=`^>t-q>)U^2AB5 zFvL3kw{kIXePoRP;*eeeXZu6|>-~{7Ld#P0HL~d%4NyFT_#;q4Em8`dHl}IRTa+Ng zv$xd*SRJSzgC?8hH8z!y4nEyGsnX~HV@TK8^pqgzzw#*p^;_(1ZUmgcaq}fh43S4q z^w$_kRKk!t<2#|zQ`>Q*zLxNkdmoxZ@jd?wv%sZ6Bj*OvGv53 zQ<(uLyi3$Az<4fw^#n_qI?*}Q`pv$SAF82Mm-KNB4EkyBtP(X$1~kA`R;K&K zGDDQmM7B*MQaC2NiAZr=e+FIt1YlWYM1)+T@Z|Womh5`a-=wJv2AAts^-iOfq{WRLFk`9&ETqzJOB}l+mGGo_9U2&Qq!yAwW(>Mpj$~sh9npOXjFI+$h>%Qm)q)< zwDd>7R%^Oun*xB`aDQu>UBF++5b%!WqCw0{1;q$S3irfErjx=a)> zl|PFrF~BjH`S^Z8;okbz*4EL{SA>LwU}i&quNIS{G^*Yh0ILD|MfH45IsrV$9`sk! z;m403pmNU2%IfTtMQK2iM*a@H{J9BjT(jhL8xSOxl$QQZFp`&3E6Txo3~FV-69ChM zK*m1D$HR8!+hStK{&=+fEOd0nqeTXcjEop+!yQ1%9vl!bj)WsI=5X`{lGLE2jCmPn zVs_T;f+o?Q)iAKof4`|pY^#W5)6rB@yM6aAzC9P2h;5PQ&#pDGBN8(KdxEhXs)55 z@%nWdupkIT=oW0S;&WJ-u@rc-5%4eP|3ukHm3>PhH2x9<`o$NIG$TE|3xt-T9MzIi z$kRfgP4yzcBOvJR=?VGxvAwf1JpL*KDzs?|KKWn!4F7!I;KE=BVtGEDOeD*zqI&`Y zTL1Pv0q6Bsl{PvW8ecG@Nn%BzPz(wQdD+>aU;w`+SV>9gJwACP0A7HL+myO)&sJ4c zot+$X5EzC;0}tb8;ljLB0SH*8{jZIiV_iVZIt9Nt!y{!YJ6r{Q2LPhC$ti$eYO{-o zT*SLf@j>^@0t)Masuet4$<%GE%o2sYKAZiW4)$qqA~kHU56w<;`?l~-EA34N`MliR z+U;*(dL2N!q&qc4_!uO?Iu}aD#tWrA!WircN(FN2PJ%wET2vgLgAWmCqm-OhIj;1V zTaHLdO5)ORTaD^-$7U)dZmzB}aomSM4!0Hd!7bjBs9sF(5xLMV98pLRM76X?NpjXU zoS`RK7#Z6J26kIRY5Dm0W@ctUypm(rkj`1Cqy{s~Wigo5+uJJ>#YPFp@ZNGCraMqv zOhrZYP6Wk6u_RUdMJ*SxQ8DgA1bR8<^>sBhP>|*l5D;*fc8A}>e_9E=IvI()5F-?N z0H(NGIa{SmiVHZn6YxK~xn?eIZky?P#ER+!1i*Zku1|<2V6Eij4$#0zIxKI4ll=`*e*mUs zfb;`kYX1TUoGoMArnU=(a+|J)fld*z z$a&l1{4%~I=t%*2`r+YWO-;?oX00tynnmKJA1z>kwbRbUB@r|zP*&%syFFJkKRuW^ zO09STS~s9ovwG1$PZ1x*pI-0V3ccXYq4Lx018Mi8+}h895!>B zmH(>yy_%YurYEp7f%N7}z43qbgh%fCR@m5ndwY98etwN+vuch28yg#yydJQ6%vaK@ zgN08P4lX%l0iM(WXjB-t-$H!=hKtdGM+OF750XqBS>v&t@@S!SX_bGi$17qX7O9*D z8Ze-Jg+b$Uz?CC4*zOFip>1qzKx;0HvSqVZ$~tSsVi`OE0e<^zHt^_F28^95C!J#h z0s--F>e@MsGwD@@N2Qf4%m zr5GY|UbTM9`Qwt%rKf(Z+EtHOS*4Ogqj&@wqXwTM7;eFdd4jz z2&iEa>3@Z!0vLMW}Es^NZ{ zC&(M@$y>)HI6MTfGVDKhi9;~SLy-VH4yFv$7k1yMBB@RB*WjBj*Loh{e~N7fEXSQl zodM{<1b%}svuIcKPETJX`%wXQ__Xrwjpfa)WbKAT2-lo4(ya!H%rdATgG13UI38I0ks|2Uoq|YGx~`BXAq5 zPbVBNu23aXKzp#K;n1?5lGI?W_DcA+d7RaxzSQnneJs#{jE^5I?4dY3&7Yy35MWu4 z091m$V+Ye*u4he2nL1n^<4B}p=KMlJBmPEYLvKj}K(V5Y;?UG6$~u)PeGNBMydq%F z)9_Gqa|KIBOt8gJ+QTiQs2r^mZ3gTiAOKZbk|?Y`4tKoZT;=n z0Ei^K2%`RR1h$xpU6~X^loycs{d;%6b!lk@fc@Y9MAb#7_&HfTKB)X& zh>eJjh1}t@a~lQv_d)mv*LUpq9bA~QtUNYLJ2Rj^zaIXOV(`0;?^tNfnV!fDciTNG z^(|M!iu4zTs>++60TGBNn>t~ukkJ)jCW#cRR=Sd`wwTT&6y-xrW z*s4JxvotR@nwU!Or#KZ<5~2Y*zMBaBIlmAiPXVG!9w*dm=~VF&@H}jLrXYGc?$I-_ zcUpP&3iw(9-_OSgQYL_k6+*TtIlemlE{!9AWc;%pz3OYpJ1+Rx`JIXiDu6;k z`2>RT4^H=Z?2uUOzyD#{HD^-b z_3^EF#ryZG!vj>^Z}g#aZrimF&IpU|H8tfpFCjJZyVxwL$Y(=uWlJM9WyE6%muz*Z z5WB_OQE!G0$^kjPAtY^cT zGzBSZi{b@`2NLd@pi#RV%zC%-_@1w<{ppvsukLW%yo!RMv+r-++ZsHLwH zXLeF8oOI^>e5a|ivu28G;Uv!KROK?Jylq=}2R5&1t>HaY@`_w%eD7f*>5`-x=UtPt zjgt=JN`jLJ>$uIC`{ja@3GX)p2sNq5-=t~^ENz)Kv5ofVBI-=nP zcVq>LyU12*O`X=?4`vCGG6^6^HNM*s(*35+eOD4`-*3;Dw3k`AhIFV)8#v#TW9yIia@nxWwAi0%Boz z^%jZ4YEK75w|%u~k@Pch?tO+vcEh5Pe4`V1+rz_+r4ioX6J-mPw#3b`?cUK3$-0z6 z>NH`$xiOlA?3~T2+tFT`+_fk(%qrhND5nSUD?27>NpV;S$FbvZGUhcn1J{ZBravnp ztRpFoi^{K~!`b_mt#qG^WS&j_XhA1ge6Rmp`>_de-!?hgHu0fzgbbA&?uuDkehs%B z4Lfy}!L?Lt)IhJ;hi~G0-xJm%=bmwq8O?0Ci{|ODS>A4VNNhYmSnrS`&bHtgN;HAe z61{v4dUNZ3SQVpZr)FfpagDNv6jfpleTu1z-c4}0wQR=-+0l{FsX%^Y^)sl)Uj1Rx z$t?WI`YCBt4wdIt3j#a1M)1dIWZ*k^!b3NG``(5ZG=y|mBGE*ri^VAyXSTt8r*lz- z*0t*GTnFd7*k>DCP-eC#ZD8V4FHfr4l$=^v2fRIS-uO8ZOIDWgkzUN72aDs%n;L8? z*B?K=dAm%*c-BNA#~&w{Sc#9^AF?RhOvKpdj!S=hN!<6XL*#DDV(&r>SDV3y(464g z<2jbGPh=&;6UXb`4J5p$fA2=Ds7cCROq&vz%VlZm)8ypQ+9pb5CS`(BZcAR$eLnVC z_viUaiKg7rl02n*lz3Q!s(3iR@K=5>vM!IL%#PuL0*{J+C= z!tSenal@CX)cfH}?QNuyDjY0Gd#W8WZ3a8bGK)s$^2VYYCF9Lcif1=kK4u2}Jo*`0 zjI3c+F?v)u4x4-;X6`MVo7L8cC&#a>8?@gTbQ~Br7Q7$%In_fLW_3!G>lEv61yAI} za`+VWz4w`H0q>T83sVloG_GaUL6FQ%yPSUn^R-5di!tL9@HzzSjhU7jwr3iN)&!v}mEBLK z!mO399n7T8olY$Dtk>*se<>NeoeDf_uSWc|25eK?lX7Bh){h84`c3d#UCq^Q^jp_{ zUOKhD#HQX($8U79M6zM^JdUAP*+5{dB7I+NOt0t*X~pYXb=z_?J#*fkBJk0%y9UJd z?F+Vg_wK(1UqLby?~<)(Y9Gqzk$6_~Y16Sx7aNpR)1Szi?Dker_S=ic&I=<`HNrKybRUo^x7QbLq}cPtPahtKzWYcI z8ZqHKWP2I%1p>frmt*A@c!>HYq7$_{co~{Q{NWA zty_&4_Eph{JLY(&DsDW9@gkC%UKmHHqt|`25f!ucgaWdZ}*^g;c53 z@tB_0^yAUphr~LCR&M*dmiCMX`ycOg1lcyBYod^Kj1h0JJxN@88lQG-wPU+63PT+y zgl=BRjlmxllgW#DGI%UrElTfZH}&X#`{v=hwTjg;`un&CPIM_)@ok-WikEEiyD^() z_oT){NN)Bq2l)9NF*PwNzjqT6R?X+y5qKiAq3!QFGT5B1C}yjtNS0!6&Ltlvgl~OP z;qQNL+vYFe=Qe(ELrw1PvA*b=(kUy`Z;_U!A6oTqmq~IEfA}C;#|+i5@;YQ_nXD;F z7TD^qWsvb-?&(NvdGGp)p6T_C>avc?#=ZJy(E9p_N`+g6&v6p?C*0AT=;2g}*+zW7 z9YO81o}LeYTsy2N3b5u_uO0PM)4vT~R)I{=CR>)30{H$nKyXh?Ko&EWnrEPL2e9bT=HhjhE zS90}?Rjrf9yp`!s`YKuinS3h7BZL%xd@m~K7g)Q|k+)sg~05>^Wnqt*Jrr z@-%O?A2YgfjYZ!(;6bvEMMqA@yO@t0wNrjc4|(AG8u2MyCxfwI#EsGT=6fj1XpB=z zm;NdYf9D}_=)K;t9{cV*nbWgV1A%(xa*up?CA*8d+Co@c@<6J`tf&y0{@lu7E&(rM zyq>k99*4hD`lQ!ADpDRNnqo78D0$XjnrzP8|XfMw%G;U zEpLwmj*(v3awrH~aRs&Ws@&8tO`0)m&@3=9K6f-aa%45?tH5l=Cr|qdLuh!1Qe#OZ=u$J65={(%Wl#9F&Kiuy41%7khSDR~IkI4|jFg1g}vu~3)0%l~YH?@4cd z-m?p98(94CRENXeS)WUebsHknQAQ74-5*FDQD_K!a3X=6Mc>1DcklUws`+bJN&l;| z^A2ljOV@Z%K&69H6r>6YNC!cx6hTl?dJjmKUIe6riWKQBAXR#AAygrBl#cWuAU$*` z(jj-nnKN_moH=vv%|ASOLiS#Lt^NMq@5_hm-mB2Fa2Lw`Xho4;LyM_AyX$8=ilc}< zlU;d`+iNYDh_I>S8YI1Y!KqfA1H`aj1$!Glb^bdzy=urjy4^j>i5>Q$v%l+s;rm?> z@@vqsD^8BJtLasTyKq6?K1xRIqK9I9@4V!&9(c_99c>gbdtO5M^!-y!mV5goq|__4 z2jQ#eLAD0>-oC;HooBv*;-Cj0XJD-YV>(#FGn`?SeKxO5v-+3i;Cos3Y~uZC@)0$# zy0_lM_*sN?1c{?qzhEWo0V(}EFLn1v-A6UZ$Tfs%S&@=Gbs_#2LC`Vy6!Q4@KS7!( zcCeHMQuglmeIR#d0T}S(1`^sN2=<3@>2=hRLHzt7?$}CemO>jmep!KC3_?UEVu&?4 zLhdsC=j&lVewE00S-c-!asguG_XAhj$UruNFX44qGboSG1GZW5IGR<$CP)6n9-Dji z-)kHJ)hM#YLA#s#{4l*(cBwP!-MlsxZzL$_gRRI%x85)Uhg(*5c6kX1#+Ye6!n(O2 zMpr-y!Ao0)9=Hl2koZWfMgs9;T>y^M|MqbUV=U+uP`j$Pmq@wwszP0uKo&xr4y+du ze9NH`tI+(fSHa!{%`Z*%I@Q7e57Gqj2aPvi=2WZC5od?04F20SvE_vYUs4d@=hvpb z0O2KL{NGy&-20D?i4JuEH8|@BWhJ2(ud$in9}NKQj+@MQxyTw@NWW&nWo(XwKUebK zw1FS@Rb2mNFcz>~hwXX;HduGI>I(1~|Fdwf5??zAvar1UNBP0U|1mKBqMvV>7&Ji+ z+O9sMyYIBn}0~@W) zOrr!ld;W&3ZgJJrA}%g&)wFaf#Mg9N%cFy2U$iG*9b;X6<$li`jE5t#z^b3XTh zXMba|8t{Os2r4S7s>zXnz(CGx1fUFjzG*4QoE$b`VPR2GRA*=Ih^%Nz3BXeuP9Q(W zh?UD9l@LJE$hxO0&~KQ1$?P)|!}jHa8GrFEg5MzHKa9AuCw(sGr$<2Ruc;C7_4OS^ z02ziHSDFkr7`J{8a#88|^GL?aKzlACy#5M1Z0nwUdQKB2kFELu7`+3mu#d^WCsz`^Ws70;0B>V4}9(gkSo*(Sj2!K^)Y>kotZ%r2hIhI)e4h}N^oCW{7JpE}D z<~bJ|A|-+0Q$QdFjN5^jTNGD19qBrMR|%jNR(uAt3+}_U6%`fVhK4|(0d}}cDCQ1r zi}S#`b+B*Pd3m4HQKY#ry(RgV#qdve`{!C|L>=>Wp-C2W5Bq}3vd3ba7I6Dxg>^7X zA5<3^)Zb%d`~hM|=F;9U{uJjqN0ya2e2HWN3baW5DkR+k7at!V7uU#gb~;cTwcFL* z&6T+V5JOH@*5S#?2VZEdQ@asw87DD>4vT2Dw5kCw*?vY%O`WgKQPKy(E=o#E?QLvK zbr~Tda}5X6dSJ@AxF8O5T}Yv7YH;8K227ln(~eh~e&a=wusyx#D>Ohclag(_r@-P2 za<_zpgm@r%feZky&y1>fv!4uX3!Vc{E6DZlwYy*{pB!x2p9}y&os>?b48Y9kQY$hU za5X@T#QXI~GPJvUdhT*^?rv{iAtv59Yk{PT0f+wl%8EPq1Kg@>D+mOlGyG;u_ad+j zS!mEgM2PA?dgJa`l0H^e-rwG?1rzeu+L)fiEgv|H$FMJh8-k|FR!s!_?s7|`_eoLi z)(mpSlph-!F$q#!*+PB*WUU@77Y)4VgO)|P)(8}cn0b5)SOVh93T&Ta>*`eN1u%=; zaKRAs`b(TW=M8#=W)I?lQapToHdfY`2vlXI;MxjEQ1i`FFgQ54xw*N>idH>R!4q1a zG73^&I6FB3KArONc^1QC3L^+vx{{>wq7xZ(2Ehr1`y4613mrMzJ zFuRqqcR&!_;^Ja_@Em1!ettfGVyMAO$PW>F`z;XMa6PP}wDDLeIXX49WYCg=f@0W8 zXc7f!dNJqnY7QYhcMU#q3V0IP;J56@I-2(q7$f$@K@1Gm3NG#os0e7K+^j59b93Wo zghr4uI)$i5x9B>94;G9Y@X=%Pp)`B|O>R%O0k1jeR)EJfw6r?g+Cu5>N4vPZ#oNhK z(-ti*Dk=g5C^NM4i;gCqhn*VVI4fk(`W}=ymZXfvAG7% z129hU@$iBJBg?D~*cMcB9|3f%lnnsrTr&nBd}*?!-lXNFrH(p5h(oe&G2rahc6O3Z z3tzV9JHd3z$jAVpP}*a7kSS*RhZUhCL24fxNZp6o!3YGA340#i8XA=3DWWbRKYQ_M z$nIK5@xGyL^5X;U4TP@2^XH7`i7*&71x0jZWK&gvG@kpgPMh-M(}{Aht}A7$I?~;N zpvU65^yK9L?rSB#=7;lkzD9FC6u^=>t#!|NM?;IaHV4NXEq!k0Q`2pXq&pR2rZqB~L{#jGue8~|Fr8({#BzO%pA1TnsD3pSW+ ziYrwakc-kzP8A?9AG5W%7?+&fQ<*G%Nl*1Qg?%rmJ6jD6kcmQsx_MYha+{y~p>-~F!DBnYkkF0wi z7Bk-dDEKm3x0suoyF-}Ph%rhj8&rMNQc&d01>VRW5cSmF{*W#$%7qIoUwKy@>0lo} z*14={hUOFUHUVm6C3pku`VXG%fUXy^j%U0aPpJ55j4?t~%gSmM%QZnP7C7Uf6@gg- z|E)hb5)WX_5qRL3)^7Apd{rMHWDqK63JyAZyjs6s*z1w%RhVAs-z_Sn2^CqtB$A|@ ztHu#mR0wehL3$f}F`?x@1kY*-xUvDH7hpJ^IC{$47B~ z_~~SxYCcngml!`+J7;S(l5rD4qcM_t+z;t*9=X&V<1Hpz6Gk4IIa%CTT&{$_QD~3j4-Ep8F3kP?&6%8``;&9ABx((BaJD5;0Ht3Q1})MA>yJm z`QG~PPwJIwHn}e~uspEWMH(Fj%uG3IB?+x%pVjiLH=LiQu01KZKy%CEv$ZUP)zgkT z2)c8WhvvNAJ&N?R=XMNyadgNKm6oLT>FDr@yTUVEKYDf5(wW%!{yG9boRZG^dwZWo zBNaX^e{#cx_~GP?)m1f51Tsfl@Edwcu*x1k0%{6MJk8Qb+SihHG5_x^{6#R1E1XXkk`YQI%xR=OysWZhCDUx`L zu-^iCaj4NAX5HW+1k+o9u8am+DMpO*uI9$|&9#Eq#|24YhK7YxvGE78iq3`iwWj&@ zw1!4Z-be^~>5aKpsk>Qola|aL7wM>rFLC&uR{rwPP<%Fw+Ik%)T*mnxfLLD}gFnk5{}3Y`a&b6imXO;nw=yd`Ql zVMXs5Z(^}EzNs+UTZ*$s9n;bP#BL6ILDMDSgr1g~PxQz~mwk+!BHzv1sarq$o=1A1 z(HFM;`0~Mxvz@OYY!}HB-I>_FG?XbHc)!SuNb)kT+`3)ZLAcjyvCI*KW;h7*u<06T z(Vf~PjN`vTUAV8_wNZCNlBvbIec^|07++!DU>hOOVYkw*y>hc@>Yr(-S#F7#!3b|f zxEmML6-<;J1QxtFFX=Y9XfAisW-lIRn8})PeNYmaoKs@Wm&#*jHJsS$9Tz4VbJ!O4 zI5YKeFz>dEtB$i-43lWnRKI~;KmW$v(4fXfO;5Nk=Q%P@8KgP-{#pDuQD;_{1vAy_ zp%w5-Idm(TW+rf^`yBlwQAE}&VGO8OoJmA8)c1BSJiJuHMKdqOf;tHA%|)>pK_-ut zWzdx-h{2MYQE~VhYE8jsz1D|{x&|N(y$C7^#I9An!Q{@kpO8Sy+;FN=ZDmROYlMvR zoc!6={vv9Z|KWJ+wTTpklP7L6_o@$5>;i7SW+D-9PgSa$U>HY}i`nW{zLI$m*717I zyg`Zmap=$EbzinqFsk(UeNOi$Yw0{aI>RL^Hob_`XC|Vfczy_JJ~y*Bw5&WEnXY%|SuWHDgiBBuma&jLG141GTrOgxYy>^zp@(@4=67F-U43*BHLFE->| zCnXFlHRQEfyfhIXU?UQ1F{MW~7-~7QtjZj+`s%|>PeoBXL1Npv8H)SKUf*`KrdASy z-!pbg0Nz4qc(YN-v;(MD)NaD%oohb*qU za#j)uHu2(CCuSjKf$XcOqvOVblyNXQkd&LNge$9R9>r3YlPsv2z3`rIBbZua-fcNS z>#VKRk#btq%zgd5Yg$Anu%VY*~92En&_SbF>AN;a1uIPnj1(l_`%b;SPbGw>1>o}O9&t0 z*&<-gfbB1bSaXx#M5Xma0G--M+9p|tcnl*Rm?90s;c-&#E!e8o$14v7o#(cNPX9}mZXZXAqoAg zQMk)+vKjL}??V#$4;1xRnXN`r$fCkFvs5gsCR4P{%iv<= z=88POLT-yiNyEM8P3Jj0Fd;q7T7VYZlw-eSf9IiiQ@=}r8Uf9j0@fP0S`A(1orCAv zIp1r99@-_&F~{7EAb#y=w~%9|QkYwyNA$V6FgXa<-_tR9Aoed?iKvgI%L|{zD?6vZH$WlCWo+& z4@g<%wxjNZuBDEy%A)@4TUWHd&-YdtEX!_{l+YMDa}!RRe(d<8E6h?jzS#MZ?GH8- z;NjwYe1KJ4IsgRYfvQ@}5u7h_1X`Jal%&;65G#HPaV9z5|ac z&5;Kv^)TDJc4PngK9Cy8qar4YH$GJth6bf7k=Tmm)_)k5=WMidy2_)SrdRD?lUPgu zkBLb7-bOgmE2Wdv_V$jXT~$SWhY`t+>UaVOj4Gm#(X$mB4M(wN^!I7rO(H9^{2tk= zWgTSs^^i$!c-86I2liHVxjpx!6K>t|!Q>Pgwvg$C*Ysv1*jXl4MY*{bAoBCt`!oO6 zryp|3HA*V%xO2H^8Fcz{2xxe-C^lOaI*)3*(uf+uf=B>83@~ z`(`mG%5}o7Px-AW>5$GrxVGEv=6(hjof!I^mccoX6f^vCu`8IfYEq-4wC2996Q0tL85YqqTZ%(u(+uTi z;(!eug_Sa}=UMX8^vbwrO50nVc?rUb=186THomF?2+1zyWExo(vW~I@Rx}ci&15qb(M-?K_v~k&L!Y?$U7|cAp9kUX6MOKL`&T1jm)9k#d60 zSzGgJBW%z^^j@);#*BoHjGQl&UgBO`>&~@+C(JO{ic{Z|J8T)m`6}C*V>ZG470!uO zgjctO^r>hW8&$)tF@k;jChc;2U30T98kHQoVqZ*0JkziaI}Ptqo#equr{3Ekv4Ekx z4@}v~B@$>(jbwPIpw5#n`9+EImUf|Ot-UWuhd#nPTB|peQ4%WF-rV_*r@kVUul3Vz zR5J;!D%CEnZ}=k?RQoKcsu(z-qD(*H+FK*0@;Hk|$m@%FTb1anJw{?bk+R_mul2@b z`!d_@bUBy;3Isd-#c%QVESPP)Oi6r+jf||nCAy{-*7&ME?_<~$_sJ}gwpQ)RX5yfR zk_A2b3&^605_dG;h{}p=pMMVyb?EF^R{7V8B6d@oYr=MyS6SY>Rr4Q#)A7DS3=aBf-{Z?uHn5<^T4<8qh|^44 z(~Yq58tB+&FGohiGZ?GFk&XV1U*H&6V;+#FuU||8b44Ymcw5eVy{EibB)1@HK9XL12o9Xf4&((81kGhjaWtUy8`U8}nWv}z=?~)|UZ_ zzA4q$SS2pXBtwbL{!(nkzG>vSv*+vFMkmeekF9%Uu4F z-s1b)2Qn#R*!A-u1v}zIZI561%;#Ekn~bCeM{#67QO)G@aV;@6(S>TNsKMe*e19L^ zv8R)EFgfNFO+V}vBBBi$s#2p3GZCp70z1B^cA8`bbqm(q|@IYrq6fZ(!A2%_QQ6ofLZ?h#iBqZ#1ytlj&B&r zR0_j!(hXmG6?mMS@qs7q;aKF?vQXnmp2`zhFK_8It|EhDWdG)#l`akBkB8=mNTwKv zkI23{l_~z?#h4=Nm5&aFX49NQQdc?{%-Ib3PRjQ)PDiW1!PZss%nc^L^EEi99na^n0O?cv?V;8fDX68S1& S;P Date: Mon, 31 Dec 2018 18:01:31 +0100 Subject: [PATCH 33/96] [find_object] Implemented the basic code for finding objects of a given category --- .../mdr_find_object_action/action_states.py | 50 +++++++++++++++---- 1 file changed, 39 insertions(+), 11 deletions(-) diff --git a/mdr_planning/mdr_actions/mdr_perception_actions/mdr_find_object_action/ros/src/mdr_find_object_action/action_states.py b/mdr_planning/mdr_actions/mdr_perception_actions/mdr_find_object_action/ros/src/mdr_find_object_action/action_states.py index 68f11f6b3..3bf257804 100644 --- a/mdr_planning/mdr_actions/mdr_perception_actions/mdr_find_object_action/ros/src/mdr_find_object_action/action_states.py +++ b/mdr_planning/mdr_actions/mdr_perception_actions/mdr_find_object_action/ros/src/mdr_find_object_action/action_states.py @@ -29,19 +29,47 @@ def execute(self, userdata): userdata.obj_location = location userdata.relation = predicate return 'succeeded' - else: - rospy.loginfo('[FIND_OBJECT] %s not found in the knowledge base; querying the ontology', obj_name) - location = self.ontology_interface.get_default_storing_location(obj_name=obj_name) - if location: - predicate = 'in' - rospy.loginfo('[FIND_OBJECT] %s is usually %s %s', obj_name, predicate, location) - # TODO: check if the object is at the default location - userdata.obj_location = location - userdata.relation = predicate - return 'succeeded' - rospy.loginfo('[FIND_OBJECT] %s not found', obj_name) + rospy.loginfo('[FIND_OBJECT] %s not found in the knowledge base; querying the ontology', obj_name) + location = self.ontology_interface.get_default_storing_location(obj_name=obj_name) + if location: + predicate = 'in' + rospy.loginfo('[FIND_OBJECT] %s is usually %s %s', obj_name, predicate, location) + # TODO: check if the object is at the default location + userdata.obj_location = location + userdata.relation = predicate + return 'succeeded' + + rospy.loginfo('[FIND_OBJECT] %s not found', obj_name) + userdata.obj_location = None + userdata.relation = None + return 'failed' elif userdata.find_object_goal.goal_type == FindObjectGoal.OBJECT_CATEGORY: + obj_category = userdata.find_object_goal.object_name + category_objects = self.kb_interface.get_category_objects(obj_category) + if category_objects: + for obj_name in category_objects: + location, predicate = self.kb_interface.get_object_location(obj_name) + if location: + rospy.loginfo('[FIND_OBJECT] Found %s %s %s', obj_name, predicate, location) + # TODO: verify that the object is still at that location + userdata.obj_location = location + userdata.relation = predicate + return 'succeeded' + else: + rospy.loginfo('[FIND_OBJECT] The location of %s is unknown', obj_name) + else: + rospy.loginfo('[FIND_OBJECT] No object of category %s was found in the knowledge base; querying the ontology', obj_category) + + location = self.ontology_interface.get_default_storing_location(obj_category=obj_category) + if location: + predicate = 'in' + rospy.loginfo('[FIND_OBJECT] Objects of %s are usually %s %s', obj_category, predicate, location) + # TODO: check if an object of the desired category is at the default location + userdata.obj_location = location + userdata.relation = predicate + return 'succeeded' + return 'failed' class SetActionLibResult(smach.State): From 87c569fc5a2922d403de84182a554edeeda338c8 Mon Sep 17 00:00:00 2001 From: Alex Mitrevski Date: Fri, 11 Jan 2019 22:11:59 +0100 Subject: [PATCH 34/96] [rosplan_interface] Adapted the rosplan launcher to the newest version of ROSPlan --- .../ros/launch/rosplan.launch | 39 ++++++++++++++----- 1 file changed, 30 insertions(+), 9 deletions(-) diff --git a/mdr_planning/mdr_rosplan_interface/ros/launch/rosplan.launch b/mdr_planning/mdr_rosplan_interface/ros/launch/rosplan.launch index e2ac4a8d8..47a9aeaf1 100644 --- a/mdr_planning/mdr_rosplan_interface/ros/launch/rosplan.launch +++ b/mdr_planning/mdr_rosplan_interface/ros/launch/rosplan.launch @@ -7,16 +7,14 @@ - - - + - + @@ -27,20 +25,43 @@ - + + + + + + + + + - + + + + - - - + + + + + + + + + + + + + + + From 085731d94f78a46c9630dc55b86000c616d6015a Mon Sep 17 00:00:00 2001 From: Alex Mitrevski Date: Sat, 12 Jan 2019 00:22:12 +0100 Subject: [PATCH 35/96] [rosplan_interface] An additional param had to be passed to the rosplan_problem_interface node --- mdr_planning/mdr_rosplan_interface/ros/launch/rosplan.launch | 1 + 1 file changed, 1 insertion(+) diff --git a/mdr_planning/mdr_rosplan_interface/ros/launch/rosplan.launch b/mdr_planning/mdr_rosplan_interface/ros/launch/rosplan.launch index 47a9aeaf1..c926c7f68 100644 --- a/mdr_planning/mdr_rosplan_interface/ros/launch/rosplan.launch +++ b/mdr_planning/mdr_rosplan_interface/ros/launch/rosplan.launch @@ -33,6 +33,7 @@ + From 3bd40bfcadfea4bcb3525544bdf1faaea97841f1 Mon Sep 17 00:00:00 2001 From: Alex Mitrevski Date: Sat, 12 Jan 2019 14:49:57 +0100 Subject: [PATCH 36/96] [rosplan_interface] Added a class for interacting with the planner (goal mgmt, invocation) --- mdr_planning/mdr_rosplan_interface/README.md | 23 +++- .../ros/launch/rosplan.launch | 6 + .../planner_interface.py | 111 ++++++++++++++++++ 3 files changed, 138 insertions(+), 2 deletions(-) create mode 100644 mdr_planning/mdr_rosplan_interface/ros/src/mdr_rosplan_interface/planner_interface.py diff --git a/mdr_planning/mdr_rosplan_interface/README.md b/mdr_planning/mdr_rosplan_interface/README.md index 0920feb8b..7564e7d93 100644 --- a/mdr_planning/mdr_rosplan_interface/README.md +++ b/mdr_planning/mdr_rosplan_interface/README.md @@ -6,7 +6,7 @@ A package for interacting with the ROSPlan planning system. ### action_client_base -A base implementation of an action client class able to (i) respond to action dispatch messages sent by a ROSPlan dispatcher and (ii) update the knowledge base based on an executed action. +A base implementation of an action client class (`ActionClientBase`) able to (i) respond to action dispatch messages sent by a ROSPlan dispatcher and (ii) update the knowledge base based on an executed action. The base class defines the following methods: * `call_action`: Callback method called when an action is dispatched. Only needs to listen to request for a particular action; all other requests should be ignored. @@ -16,6 +16,20 @@ The base class defines the following methods: Action-specific implementations need to override `call_action`, `get_action_message`, and `update_knowledge_base`; `send_action_feedback` is already implemented in the base class. Example implementations of action clients can be found in the `mdr_actions` package (the [pickup action client](../mdr_actions/mdr_manipulation_actions/mdr_pickup_action/ros/scripts/pickup_client) is a representative example). +### planner_interface + +An interface (`PlannerInterface`) for interacting with a planner. The interface exposes the following methods: +* `add_plan_goals`: Registers a list of ground predicates as planning goals.s +* `remove_plan_goals`: Removes a list of ground predicates from the list of planning goals. +* `plan`: Generates a problem from the current state of the knowledge base and invokes the planner. +* `start_plan_dispatch`: Parses a previously generated plan and invokes the plan dispatcher. + +The interface contains an instance of the `DomesticKBInterface` class in [`mas_knowledge_base`](https://github.com/b-it-bots/mas_knowledge_base) for interacting with the knowledge base. In addition, the following parameters need to be exposed on the ROS parameters server: +* `/planner/problem_generation_srv`: Name of a service that generates a planner problem +* `/planner/planner_srv`: Name of a service that invokes a planner +* `/planner/plan_parsing_srv`: Name of a service that parses a generated plan +* `/planner/plan_dispatch_srv`: Name of a service that invokes a plan dispatcher + ## Executable scripts ### init_planning_problem @@ -83,4 +97,9 @@ mdr_rosplan_interface |____mdr_rosplan_interface | __init__.py |____action_client_base.py -``` \ No newline at end of file +``` + +## Dependencies + +* `ROSPlan` +* `mas_knowledge_base` diff --git a/mdr_planning/mdr_rosplan_interface/ros/launch/rosplan.launch b/mdr_planning/mdr_rosplan_interface/ros/launch/rosplan.launch index c926c7f68..147c4fe2d 100644 --- a/mdr_planning/mdr_rosplan_interface/ros/launch/rosplan.launch +++ b/mdr_planning/mdr_rosplan_interface/ros/launch/rosplan.launch @@ -13,6 +13,12 @@ + + + + + + diff --git a/mdr_planning/mdr_rosplan_interface/ros/src/mdr_rosplan_interface/planner_interface.py b/mdr_planning/mdr_rosplan_interface/ros/src/mdr_rosplan_interface/planner_interface.py new file mode 100644 index 000000000..126c59be9 --- /dev/null +++ b/mdr_planning/mdr_rosplan_interface/ros/src/mdr_rosplan_interface/planner_interface.py @@ -0,0 +1,111 @@ +import rospy +from std_srvs.srv import Empty +from mas_knowledge_base.domestic_kb_interface import DomesticKBInterface + +class PlannerInterface(object): + '''An interface providing functionalities for interacting with a planner. + + The interface expects four planner services to be exposed, whose names should be + made available to the parameter server as follows: + * /planner/problem_generation_srv: Name of a service that generates a planner problem + * /planner/planner_srv: Name of a service that invokes a planner + * /planner/plan_parsing_srv: Name of a service that parses a generated plan + * /planner/plan_dispatch_srv: Name of a service that invokes a plan dispatcher + + @author Alex Mitrevski + @contact aleksandar.mitrevski@h-brs.de + + ''' + def __init__(self): + self.problem_generation_proxy = None + self.planner_proxy = None + self.plan_parsing_proxy = None + self.plan_dispatch_proxy = None + self.kb_interface = DomesticKBInterface() + + problem_generation_srv = rospy.get_param('/planner/problem_generation_srv', + '/rosplan_problem_interface/problem_generation_server') + try: + rospy.wait_for_service(problem_generation_srv, 5.) + self.problem_generation_proxy = rospy.ServiceProxy(problem_generation_srv, Empty) + except (rospy.ServiceException, rospy.ROSException): + rospy.logerr('The service %s does not appear to exist.', problem_generation_srv) + + planner_srv = rospy.get_param('/planner/planner_srv', + '/rosplan_planner_interface/planning_server') + try: + rospy.wait_for_service(planner_srv, 5.) + self.planner_proxy = rospy.ServiceProxy(planner_srv, Empty) + except (rospy.ServiceException, rospy.ROSException): + rospy.logerr('The service %s does not appear to exist.', planner_srv) + + plan_parsing_srv = rospy.get_param('/planner/plan_parsing_srv', + '/rosplan_parsing_interface/parse_plan') + try: + rospy.wait_for_service(plan_parsing_srv, 5.) + self.plan_parsing_proxy = rospy.ServiceProxy(plan_parsing_srv, Empty) + except (rospy.ServiceException, rospy.ROSException): + rospy.logerr('The service %s does not appear to exist.', plan_parsing_srv) + + plan_dispatch_srv = rospy.get_param('/planner/plan_dispatch_srv', + '/rosplan_plan_dispatcher/dispatch_plan') + try: + rospy.wait_for_service(plan_dispatch_srv, 5.) + self.plan_dispatch_proxy = rospy.ServiceProxy(plan_dispatch_srv, Empty) + except (rospy.ServiceException, rospy.ROSException): + rospy.logerr('The service %s does not appear to exist.', plan_dispatch_srv) + + def add_plan_goals(self, goals): + '''Registers the ground predicates in the given list as planning goals. + + Keyword arguments: + @param goal_list -- a list in which each entry is a tuple of the form + (predicate, [(variable, value), ...]), where + "predicate" is the predicate name and the + (variable, value) tuples are the variable values + + ''' + self.kb_interface.insert_goals(goals) + + def remove_plan_goals(self, goals): + '''Removes the ground predicates in the given list from the list of planning goals. + + Keyword arguments: + @param goal_list -- a list in which each entry is a tuple of the form + (predicate, [(variable, value), ...]), where + "predicate" is the predicate name and the + (variable, value) tuples are the variable values + + ''' + self.kb_interface.remove_goals(goals) + + def plan(self): + '''Generates a problem file from the current state of the knowledge base + and invokes the planner. + ''' + rospy.loginfo('[planner_interface] Generating a problem file') + try: + self.problem_generation_proxy() + except rospy.ServiceException as exc: + rospy.logerr('An error occured while generating a problem file: ' + str(exc)) + + rospy.loginfo('[planner_interface] Invoking the planner') + try: + self.planner_proxy() + except rospy.ServiceException as exc: + rospy.logerr('An error occured while planning: ' + str(exc)) + + def start_plan_dispatch(self): + '''Parses a previously generated plan and starts dispatching the plan. + ''' + rospy.loginfo('[planner_interface] Parsing the plan') + try: + self.plan_parsing_proxy() + except rospy.ServiceException as exc: + rospy.logerr('An error occured while parsing the plan: ' + str(exc)) + + rospy.loginfo('[planner_interface] Invoking the plan dispatcher') + try: + self.plan_dispatch_proxy() + except rospy.ServiceException as exc: + rospy.logerr('An error occured while dispatching the plan: ' + str(exc)) From 5307d7635b609605297d93014b59a41022d780e6 Mon Sep 17 00:00:00 2001 From: Alex Mitrevski Date: Fri, 18 Jan 2019 15:53:15 +0100 Subject: [PATCH 37/96] [rosplan_interface] Added subscribers for action dispatch and feedback --- .../ros/launch/rosplan.launch | 8 +++-- .../planner_interface.py | 33 +++++++++++++++++++ 2 files changed, 39 insertions(+), 2 deletions(-) diff --git a/mdr_planning/mdr_rosplan_interface/ros/launch/rosplan.launch b/mdr_planning/mdr_rosplan_interface/ros/launch/rosplan.launch index 147c4fe2d..073a01f1e 100644 --- a/mdr_planning/mdr_rosplan_interface/ros/launch/rosplan.launch +++ b/mdr_planning/mdr_rosplan_interface/ros/launch/rosplan.launch @@ -7,6 +7,8 @@ + + @@ -18,6 +20,8 @@ + + @@ -65,8 +69,8 @@ - - + + diff --git a/mdr_planning/mdr_rosplan_interface/ros/src/mdr_rosplan_interface/planner_interface.py b/mdr_planning/mdr_rosplan_interface/ros/src/mdr_rosplan_interface/planner_interface.py index 126c59be9..b9f10dfdf 100644 --- a/mdr_planning/mdr_rosplan_interface/ros/src/mdr_rosplan_interface/planner_interface.py +++ b/mdr_planning/mdr_rosplan_interface/ros/src/mdr_rosplan_interface/planner_interface.py @@ -1,5 +1,6 @@ import rospy from std_srvs.srv import Empty +import rosplan_dispatch_msgs.msg as plan_dispatch_msgs from mas_knowledge_base.domestic_kb_interface import DomesticKBInterface class PlannerInterface(object): @@ -21,6 +22,10 @@ def __init__(self): self.planner_proxy = None self.plan_parsing_proxy = None self.plan_dispatch_proxy = None + self.action_dispatch_topic = rospy.get_param('/planner/action_dispatch_topic', + '/kcl_rosplan/action_dispatch') + self.action_feedback_topic = rospy.get_param('/planner/action_feedback_topic', + '/kcl_rosplan/action_feedback') self.kb_interface = DomesticKBInterface() problem_generation_srv = rospy.get_param('/planner/problem_generation_srv', @@ -55,6 +60,15 @@ def __init__(self): except (rospy.ServiceException, rospy.ROSException): rospy.logerr('The service %s does not appear to exist.', plan_dispatch_srv) + self.current_action = '' + self.executing = False + self.action_dispatch_sub = rospy.Subscriber(self.action_dispatch_topic, + plan_dispatch_msgs.ActionDispatch, + self.get_dispatched_action) + self.action_feedback_sub = rospy.Subscriber(self.action_feedback_topic, + plan_dispatch_msgs.ActionFeedback, + self.get_action_feedback) + def add_plan_goals(self, goals): '''Registers the ground predicates in the given list as planning goals. @@ -88,12 +102,15 @@ def plan(self): self.problem_generation_proxy() except rospy.ServiceException as exc: rospy.logerr('An error occured while generating a problem file: ' + str(exc)) + return False rospy.loginfo('[planner_interface] Invoking the planner') try: self.planner_proxy() except rospy.ServiceException as exc: rospy.logerr('An error occured while planning: ' + str(exc)) + return False + return True def start_plan_dispatch(self): '''Parses a previously generated plan and starts dispatching the plan. @@ -103,9 +120,25 @@ def start_plan_dispatch(self): self.plan_parsing_proxy() except rospy.ServiceException as exc: rospy.logerr('An error occured while parsing the plan: ' + str(exc)) + return False rospy.loginfo('[planner_interface] Invoking the plan dispatcher') try: self.plan_dispatch_proxy() except rospy.ServiceException as exc: rospy.logerr('An error occured while dispatching the plan: ' + str(exc)) + return False + + self.executing = True + return True + + def get_current_action(self): + return self.current_action + + def get_dispatched_action(self, msg): + self.current_action = msg.name + + def get_action_feedback(self, msg): + if msg.information and msg.information[0].key == 'action_name' and \ + msg.information[0].value == self.current_action: + self.executing = msg.status != 'action achieved' From 162e7efc69926a4bf9a0ddb74f6966152aeaeb71 Mon Sep 17 00:00:00 2001 From: Alex Mitrevski Date: Fri, 18 Jan 2019 16:48:40 +0100 Subject: [PATCH 38/96] [behaviours] Added states for interacting with the planner --- .../mdr_planning_behaviours/CMakeLists.txt | 25 +++++++++++++++++++ .../mdr_planning_behaviours/package.xml | 25 +++++++++++++++++++ .../src/mdr_planning_behaviours/__init__.py | 0 .../mdr_planning_behaviours/dispatch_plan.py | 19 ++++++++++++++ .../mdr_planning_behaviours/invoke_planner.py | 17 +++++++++++++ .../mdr_planning_behaviours/setup.py | 11 ++++++++ 6 files changed, 97 insertions(+) create mode 100644 mdr_planning/mdr_behaviours/mdr_planning_behaviours/CMakeLists.txt create mode 100644 mdr_planning/mdr_behaviours/mdr_planning_behaviours/package.xml create mode 100644 mdr_planning/mdr_behaviours/mdr_planning_behaviours/ros/src/mdr_planning_behaviours/__init__.py create mode 100644 mdr_planning/mdr_behaviours/mdr_planning_behaviours/ros/src/mdr_planning_behaviours/dispatch_plan.py create mode 100644 mdr_planning/mdr_behaviours/mdr_planning_behaviours/ros/src/mdr_planning_behaviours/invoke_planner.py create mode 100644 mdr_planning/mdr_behaviours/mdr_planning_behaviours/setup.py diff --git a/mdr_planning/mdr_behaviours/mdr_planning_behaviours/CMakeLists.txt b/mdr_planning/mdr_behaviours/mdr_planning_behaviours/CMakeLists.txt new file mode 100644 index 000000000..d2fa4984a --- /dev/null +++ b/mdr_planning/mdr_behaviours/mdr_planning_behaviours/CMakeLists.txt @@ -0,0 +1,25 @@ +cmake_minimum_required(VERSION 2.8.3) +project(mdr_planning_behaviours) + +find_package(catkin REQUIRED COMPONENTS + rospy + roslint + smach + mas_execution_manager + mdr_rosplan_interface +) + +roslint_python() +catkin_python_setup() + +catkin_package( + CATKIN_DEPENDS + rospy + smach + mas_execution_manager + mdr_rosplan_interface +) + +include_directories( + ${catkin_INCLUDE_DIRS} +) diff --git a/mdr_planning/mdr_behaviours/mdr_planning_behaviours/package.xml b/mdr_planning/mdr_behaviours/mdr_planning_behaviours/package.xml new file mode 100644 index 000000000..bd567a5d2 --- /dev/null +++ b/mdr_planning/mdr_behaviours/mdr_planning_behaviours/package.xml @@ -0,0 +1,25 @@ + + + mdr_planning_behaviours + 1.2.0 + A package for common planning-related robot behaviours + + Alex Mitrevski + MAS robotics + + GPLv3 + + catkin + rospy + roslint + smach + mas_execution_manager + mdr_rosplan_interface + + rospy + smach + mas_execution_manager + mdr_rosplan_interface + + + diff --git a/mdr_planning/mdr_behaviours/mdr_planning_behaviours/ros/src/mdr_planning_behaviours/__init__.py b/mdr_planning/mdr_behaviours/mdr_planning_behaviours/ros/src/mdr_planning_behaviours/__init__.py new file mode 100644 index 000000000..e69de29bb diff --git a/mdr_planning/mdr_behaviours/mdr_planning_behaviours/ros/src/mdr_planning_behaviours/dispatch_plan.py b/mdr_planning/mdr_behaviours/mdr_planning_behaviours/ros/src/mdr_planning_behaviours/dispatch_plan.py new file mode 100644 index 000000000..180055acb --- /dev/null +++ b/mdr_planning/mdr_behaviours/mdr_planning_behaviours/ros/src/mdr_planning_behaviours/dispatch_plan.py @@ -0,0 +1,19 @@ +from mas_execution_manager.scenario_state_base import ScenarioStateBase +from mdr_rosplan_interface.planner_interface import PlannerInterface + +class DispatchPlan(ScenarioStateBase): + def __init__(self, save_sm_state=False, **kwargs): + ScenarioStateBase.__init__(self, 'dispatch_plan', + save_sm_state=save_sm_state, + outcomes=['succeeded', 'failed']) + self.sm_id = kwargs.get('sm_id', '') + self.state_name = kwargs.get('state_name', 'dispatch_plan') + self.planner_interface = PlannerInterface() + + def execute(self, userdata): + dispatching = self.start_plan_dispatch.plan() + if not dispatching: + return 'failed' + + # TODO: monitor the plan + return 'succeeded' diff --git a/mdr_planning/mdr_behaviours/mdr_planning_behaviours/ros/src/mdr_planning_behaviours/invoke_planner.py b/mdr_planning/mdr_behaviours/mdr_planning_behaviours/ros/src/mdr_planning_behaviours/invoke_planner.py new file mode 100644 index 000000000..f7ec65589 --- /dev/null +++ b/mdr_planning/mdr_behaviours/mdr_planning_behaviours/ros/src/mdr_planning_behaviours/invoke_planner.py @@ -0,0 +1,17 @@ +from mas_execution_manager.scenario_state_base import ScenarioStateBase +from mdr_rosplan_interface.planner_interface import PlannerInterface + +class InvokePlanner(ScenarioStateBase): + def __init__(self, save_sm_state=False, **kwargs): + ScenarioStateBase.__init__(self, 'invoke_planner', + save_sm_state=save_sm_state, + outcomes=['succeeded', 'failed']) + self.sm_id = kwargs.get('sm_id', '') + self.state_name = kwargs.get('state_name', 'invoke_planner') + self.planner_interface = PlannerInterface() + + def execute(self, userdata): + planner_called = self.planner_interface.plan() + if planner_called: + return 'succeeded' + return 'failed' diff --git a/mdr_planning/mdr_behaviours/mdr_planning_behaviours/setup.py b/mdr_planning/mdr_behaviours/mdr_planning_behaviours/setup.py new file mode 100644 index 000000000..12161e1ef --- /dev/null +++ b/mdr_planning/mdr_behaviours/mdr_planning_behaviours/setup.py @@ -0,0 +1,11 @@ +#!/usr/bin/env python + +from distutils.core import setup +from catkin_pkg.python_setup import generate_distutils_setup + +d = generate_distutils_setup( + packages=['mdr_planning_behaviours'], + package_dir={'mdr_planning_behaviours': 'ros/src/mdr_planning_behaviours'} +) + +setup(**d) From 3ab4ae32fe9ad1b273f88c895cb25a4f9b690e07 Mon Sep 17 00:00:00 2001 From: Alex Mitrevski Date: Fri, 18 Jan 2019 17:34:44 +0100 Subject: [PATCH 39/96] [pickup_acion] Fixed an import in the client --- .../mdr_pickup_action/ros/scripts/pickup_client | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_pickup_action/ros/scripts/pickup_client b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_pickup_action/ros/scripts/pickup_client index 5517cb5b7..574bf5241 100755 --- a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_pickup_action/ros/scripts/pickup_client +++ b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_pickup_action/ros/scripts/pickup_client @@ -5,7 +5,7 @@ import tf from mcr_perception_msgs.msg import Object from mdr_rosplan_interface.action_client_base import ActionClientBase -from mdr_knowledge_base.knowledge_base_interface import KBException +from mas_knowledge_base.knowledge_base_interface import KBException from mdr_pickup_action.msg import PickupAction, PickupGoal class PickupClient(ActionClientBase): From f0e05c5938a172b48c5acb19d2344cc0a034a13d Mon Sep 17 00:00:00 2001 From: Alex Mitrevski Date: Fri, 18 Jan 2019 18:49:16 +0100 Subject: [PATCH 40/96] [perceive_plane_action] Fixed some errors in the KB update --- .../ros/scripts/perceive_plane_client | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/mdr_planning/mdr_actions/mdr_perception_actions/mdr_perceive_plane_action/ros/scripts/perceive_plane_client b/mdr_planning/mdr_actions/mdr_perception_actions/mdr_perceive_plane_action/ros/scripts/perceive_plane_client index 21c2d447e..c0af27784 100755 --- a/mdr_planning/mdr_actions/mdr_perception_actions/mdr_perceive_plane_action/ros/scripts/perceive_plane_client +++ b/mdr_planning/mdr_actions/mdr_perception_actions/mdr_perceive_plane_action/ros/scripts/perceive_plane_client @@ -82,7 +82,7 @@ class PerceivePlaneClient(ActionClientBase): facts_to_add.append(('on', [('obj', obj_name), ('plane', plane.name)])) facts_to_add.append(('object_category', [('obj', obj_name), ('cat', obj.category)])) - object_to_add.append((obj_name, obj)) + objects_to_add.append((obj_name, obj)) # we finally update the knowledge base self.kb_interface.update_kb(facts_to_add, facts_to_remove) @@ -103,7 +103,7 @@ class PerceivePlaneClient(ActionClientBase): # with the objects that on the plane plane_objects = self.kb_interface.get_surface_object_names(plane_name) for obj_name in plane_objects: - fact = [('on', [('obj', obj_name), ('plane', plane_name)])] + fact = ('on', [('obj', obj_name), ('plane', plane_name)]) facts_to_remove.append(fact) objects_to_remove.append((obj_name, Object._type)) From 3fd1b513960c618cebaac3e249e549f7f719972f Mon Sep 17 00:00:00 2001 From: Alex Mitrevski Date: Fri, 18 Jan 2019 19:17:00 +0100 Subject: [PATCH 41/96] [manip_behaviours/place_based_on_category] Fixed an incorrect action name --- .../src/mdr_manipulation_behaviours/place_based_on_category.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/mdr_planning/mdr_behaviours/mdr_manipulation_behaviours/ros/src/mdr_manipulation_behaviours/place_based_on_category.py b/mdr_planning/mdr_behaviours/mdr_manipulation_behaviours/ros/src/mdr_manipulation_behaviours/place_based_on_category.py index 7c2b995c8..29c551c77 100644 --- a/mdr_planning/mdr_behaviours/mdr_manipulation_behaviours/ros/src/mdr_manipulation_behaviours/place_based_on_category.py +++ b/mdr_planning/mdr_behaviours/mdr_manipulation_behaviours/ros/src/mdr_manipulation_behaviours/place_based_on_category.py @@ -9,7 +9,7 @@ class PlaceBasedOnCategory(ScenarioStateBase): def __init__(self, save_sm_state=False, **kwargs): - ScenarioStateBase.__init__(self, 'place_based_on_category', + ScenarioStateBase.__init__(self, 'place', save_sm_state=save_sm_state, input_keys=['grasped_object'], outcomes=['succeeded', 'failed', From 29733544899e2eecc5de705cdf2c295ad5f2c699 Mon Sep 17 00:00:00 2001 From: Minh Date: Fri, 18 Jan 2019 20:35:22 +0100 Subject: [PATCH 42/96] add planning flowchart (#93) --- docs/planning_flowchart.html | 12 ++++++++++++ docs/planning_flowchart.png | Bin 0 -> 67174 bytes mdr_planning/README.md | 3 +++ 3 files changed, 15 insertions(+) create mode 100644 docs/planning_flowchart.html create mode 100644 docs/planning_flowchart.png create mode 100644 mdr_planning/README.md diff --git a/docs/planning_flowchart.html b/docs/planning_flowchart.html new file mode 100644 index 000000000..43dc59774 --- /dev/null +++ b/docs/planning_flowchart.html @@ -0,0 +1,12 @@ + + + +Draw.io Diagram + + + + +
+ + + diff --git a/docs/planning_flowchart.png b/docs/planning_flowchart.png new file mode 100644 index 0000000000000000000000000000000000000000..197302fc08472d3dcd98a0225b8d92db84f030bb GIT binary patch literal 67174 zcmdSBWmr{fw+4)ff&zksxCl`|T0mfdga}B3lr%^Sh_L7e3F+>Z?(SAnx#y#%)9(a9_5PXP6h=ziK@=#caUm6AF76uB+ z&HMN6g0DPyWx$JqLW&~H&ns&kyB=p_FQZd;l~D9D1N9M0()%W*n-2q7Q&z&$#6{7= zgWpkQeo(5$?td-(G!AVSM+xtS)LUMD9h@B5@OL8xxZXd0Bz?eLJ>X#Oo~UHD#kDYR zdbn_QHupOHeo5lnft0SL3tI;@hR+hF5)$ztoLhTed~bj?{QEyM^ydUxwcy_;S`M&& z{FO(Db1}Qprho(3`U1WV!HN*9L(q75FAo)b=qFcfuINQ=rQ@nV%F?2*_z_7<%VcL{ z9*>q68&loxV9bGYd%xm%W4_(#IoTvbx*)u>TSck z1k)YliYvdkM<8+JwB%?;rP>j?Pv_&pduyaEx({Y(#HyYuUP7lG0pNt=KJa_01%p+JuL{!+YDQ?iUfhPR z_48-L*~wSYs!QMc#xd0ui!#SkV$WNqEGI>Xoh=Wwm^w8)EjiSeXsx&3D}OI-FWJ01 zT&qI}u4aPn_yE~Ex7V6^VVP`{)?!AHU4O%9dCd^1(2Zf{B#<8bJ zXBDHmFDmC}-@2H~{09|Cq~9x(-Fi-!tX*W!$TO8VUfux3#@8FjnOlWEe4bU-t3rMU ziHwA16=HI)V(S|XYTS8YSR@@0em+jq(7wAOJ5vpahq;bM%#~893Ry_Ev9a1 zQjmtWL>TLu@6SNr>wxk%`Sf00B;HcNfr^59W*CW94 zYswBaIRYj~=AX+|W~Kk;d9LcrQmu?@!R;xjULiZ-TfEm**xu@SHp?AQBj%FQ}wm)bpe(juv{ z!7P6KFu_?Ss@lCVCA0`(uQ_=8Fhzg!fQMH!;rL`e*P8^Y=Uxh8;Gt_m`A=8k99-bE ztP4RxFUSgo_{;FaAF;6XuWYQj<9SFPen~Wm3jG*PAUshNmn>^OUu*vU-*>s9G&@*? zBGt`=9Xy~2Pke#uVMQ(ZtVj78xU0cLfp<4iCYh^*=!zF2!pI8jgBRTYFh_|WJTbZ9 zcmD>R{tmUH5H4j;#`pQ1cD{97O3&{#>ai^B@8pR$5hR2AS0WvGa;HSVVzpxA2bRjv zq}x(TY|9j_FPhUhPfsQ&Z-fW!nmZ)1rE}|NXR- zn`cKtP?u9}0;H3uW5F&VW=(C^hK3sEET0{NYwaJ^8e@T{Kf$rn|EhMEJhPEud~?X% z?6Y)i`^o{kfASvXA;-pun#R?}Ja1CkdukSKsWMh6YZZ!}bR6XoHAKCSZ>4s0INAA? z*aC%twT=7sDgM+_waP{6ro}G4k7x849_ksw{%baAq33j7@>VMIO~hNQrMEB;OwQS@ zM#$~8wp$W&*p6H_&g~JM6l!(S7efqmu6=xH3hmo&L-&m?Jixei(8XzuHn=;+&)j1s^C5+V>9+Z`(CnHT#FTx3nGT?la=U2767A1)#DKbro(3AGN zHvbCXg#{W4p*APJQM)c6<0dii^Xb`15Zzr}9X}+q%lp%Zlvl!R0v@#@_IaB-_{D3m z>?aij))tk7$s_uqqs@PsEkqX~6)R0&6YfyWKu9h80-531CCy=#;=TB^2)w2EcR9uf zq!1PTjyz+PLExBM%WHlHAOb~KsASaAJ^7%xNO|MKcQNlab4t~wWA{4xn{S~S-O9%H zRvi}Vyum;o+zoH4J2KB{7Co_?A)~#JBb3>$W|srXB~>@e{Sn7GX|W}QVZ9Fpsls|U z!_eJZ2XN**=isL?xh~jpA{uv7PFX~SL0YJ){FuH0-h!lyweS#Gq$I30NQplADr?Mi z)bR~922YLZmTCRqpin*+i+rR7)pM#}`X9;9SVzb(1_%En9`!$2=|=t}&?*a^FaIJF z+%qnXjsHbDdrO;k=Y5Mg+jiTgRoYiu3q9|~uwjxN**smpz&sVc1)f%;CIU0>!D;p7 z;AKC3QP}%d{Nr>bnTrm~MIPmkaeFd~7MBgfrY)yTEvK)3Ln)KwE{)*X+uI0FW=WD% zs@RO)#{zdUx}+;7%rUhpRYaRJBGS?4$d5}?&$m!GZWMX6p_6_2ZEeo{&|o3hYU0RG|%>wF6>VhDxHFTTU^u$6dL6C=3L$^ z1)JYlJU(9k(sh*`S(%qZeYA9kg_$8#Zq2FhqTcIhSEth&U27~xsp{2%PVQtRDHd3% zJ({O0W|I``opKXzR+0Y$G^UA;$jjkyG%1o6dCwX;2LjQWLT(+T-I+9HU)`W^iIwSY zfL;2FgnWtkg7dbYUX9#YT^U=nAV5x^4-OuV5f2VtR>9jhu5J@YZ0&$L@H>*# zi=U3}x!E3*+la{$2c;##Ge-Bfy?u6KQV#W2ME~l!I{x6QzGK*BdF2~3`QqPVj{sI; zwCIjI|Kp+J&!~EZ*LOI)mRh~k|5YQ983kpV3>2`>^5pVs0VU;S9@!S`D!EIu26RCIRz}F`-{cg%Ub%*GO(03!0d>)Cgj;;N zMp689@J0H!3)RZ+?WstU%O*8j{i)=JF^arHjfjH!VdG>_Nne0e)kySoqOQ9Q!WH=w zQ3wM^xhV$pY*Q!o583JNrM5~Rm_zV}S2IhPj^=CHC#)9b7VBiH-fT6UPgb^eWSLG= zPllM~fPMO`FSi*Zw3I&Y=h3P{rn^iC@V46RreCNH<#4gu0&SJbJF ziL9$h8pF0R=<#jo#`d7e<>{Xq1G}(b!Xh*^q(nxtH8#J`DTs)Si`g8^^@t`E zZbIxYjqTE_^EK0BkjU1EC5%%qGg#ygtkIp>IgN|bAMLbe@|D=keyI};7UJJ~sAvvi zraa#az<%WP(N}xzvq)>hu|N97#FqnjjoFsp6cm_4M68w^l6}kg{re&tZ#{O``tJVp zmaFSSzNmh9WP%Nq-481@=1Io$O4`XdD`a2pD9&;Hf@#fLcpH?wFd;a25BY;CUIX8w z_qNP$&y06oV&M#UrzV1_(~ELm1sIx%?&b3OrP)U-6jBbmO~&d(YzJ)s6dy1DPFjZ0 zZysxF>{y&lN^#Sy$bY{bUar~KIiXGekX{g>Ag7t)n=*{&u{E9byoKPxx?y<1Fr$Q} ze!i17k=#; zC<4t0)CT!>5%l(-eoCmhkqdzFj7E(bjJ{_2t#2WQkupGnaR217rG(Yqk}xB2WBvS> zs$GK-Q}Ja@&uE8}2p-A3ybqG+ILGmTBmR&mc9`RM|4bX-8m0JWN3EJNYuVV#UymN8 z^*u8%%y=jBn1!S0r4m@)YA4>mM+rFwcw-Rc7*3gXao~2K&JC0c8VV&u^Lu4&W7MMB zv2W$D$kNBjtpGIJcG$p#g!>!BJGC!xf8_l5)-=O}nbmzHN7l>T%W zmWU6Zy_4n{-5((oE9XSYx+fHrqOKG{-yq#W5GH~A(Kr3f(=|OmC_WI+2CL~Alg!{4 znn6u!>d+1J-5V&*_`S(Z-#|S+n_}bOG}2KXXUDj^@!Qbc18ymYCZkMwC)2kOG$|(; z@C#R{{}XNWyRr4khQ4J%s13vrf939$k(&s0jxu&k z7#52J*V{1sPyNVi`UT#Wye1F|`bX{VM%`J1K}Wq&AUsO_U4@$ZI34g*IUW8i*q!$v zoAuh>c(I6zP6>TvzStN6@D9weQ)_J!+>S>D3HV0}OY;^zry;-GFY^z;7nXoLPE zt%c9?2AT=}Ec~jR7AY;Kfgo;hu{6U`0hI81&+{&+FW}yXnAFAwzi-cz(8#}ib3bBE zMZB7B|5UGLe!=DBx9_%m%FMdC*gt7Q>8(CM$>?{9ktpZUMeviy9|1-9L+dy=Y;wQm zmBkzn=6VW$m^79LjdbGq2fd*;4ZzEIzmRmKOlN0x#T2{zKrbS`@81ZODR;tILBUM+N4_ zyNI`W6fSW7XlcRbs8&BHKMm?;is{<^R4WT@k3EBaxq$C2DmbsU)_*nCFFk6ZB2HMj zxyHHEQp8o+N7?F z7y~UZ5ubA8h;55Is@Rvm!j%)$?m##2N0Ry!_&x-cE~rfmbKCIsXJbtxE4~Jimd#|V z(fEmS(W&2rs8FN5)x6d;dmN4rz&S^e2JXe$484e~J5DQtU))bVHmMM#`?Z!V3`vLk zh-K+>dSQkZ1rk8ol$V(`4RLU#?U#ViaaQ3|@!_-NX(^|7&btNe_#{gHW7HJ00oUel zjNl#*zrn4EApT@n6ZF5%^y!89_AnNB>$g<0Z9E%tNj0VNuD9_rs$B+ zzVP3oO^KMUa8mBa)o@?0!j#`+K65ugC3gW;{22!EKR4oOQAk)krNX~L%(o?6B95W| zSWop_!8x~PfktJMru{HkT6vXnR3XemN zI);Lyz=;4y^wpGcMS-zQOWCVZ3u%*3VP@JQ_%Sx(EqSRSMAN)w`w2L#rNK?%a4VX0 zPRATggOx=@MO(Uy=_8a&$NX?(&?_}QOe?iuee3N_7?>%o}Nh??#y-$Q{NUCeE?f)fHw=JduDEkt>%;Wd0dIn$hO%&fwp8VIvf|- z76lz&DJoDFA%v+!oyGP_R=S$-I@oZIz41GU2_9=#SnQNQVb*i8!F_&-OY*nCd1?efWAFe>r zflu>TfbZrl^es$Kl{cuMjJO^fxqB|LF+X&}pjZ|($)Q;}&q38%0l4_SdLb*<%*xz) z=WT#)@;^L&7}U1g3}GNll&%F^A@Vlxy2RlB0KuKHKMvwth@NdSS$1&(pFqLdNs^}V z_d&BG*GoW%Q$0SdJ_)~jk!xRG`<0=868Aa;o-1vRQB3{5vR*Vj$J9T3ix=jo93Twv z)WGsqhKPh(9VPpTB8YX*TBbzk<{jck1)vWGe~aZyYvH&DzVm+A%+Z%< zo_8d{SU8IDM{AgRidkb3>h6N}L{|;}+Si#5sdBuPO|0VSh@nh;It*% zmUv3mZ?2x})VSw=0J=n}<|K~RRjeH$SM533rl@2MaTU%rQLW}dqLpJc89MK75fY5Y zoK|02I_@GvqGv7-lK0;t@snm{h#G_WZizR5q;!UapQzu13ZZn(d)HuJgFF1c*SZg_ zssymy4`jWH!&Fji{6Z+Cm8Yl@mTS4UkWhM+N06(jt5`(fW0$p=P^_qRO zCWo%MP5=MJZSqJls-6-!{s5)tdNC;59FSJEmiB@nN4Sr>Ir8!&o(549i05!Bq{k}= z<6s+W`yPB9#RDrpyXM-rF}^5m%-70LgboV`OtuiLDZjBnCPBvwklT-;XKlMx^Ycd4 zEt0h6$3aC_@-CeJ{ZYd6!!)tek-EY8gd++`1+@fD4S|EEd5cmU%FVI#82+-hHgyQR zLD*KS5(dd!^H9M>`@6Hh@+}+OVjZgH6O5?F)Q{2(oW`LK0d@s^u)c;kVV4pL=XZ(q zG46mh?BGNCJ>dBWzyj{yoqY&YIfqqBN~cOmHm}3fGJZC%RSHTcIypr_k=Lh>n4(s0 zr1egBU47E{P7Y2QFh1aLYQC%a7S^!{3!m0fjX7-%?DT)&Q9Nr5N655;G`3Li8~F*s z(}^z$8(yEjAz+?zc@hrxSM=}66528XY7fBZ(@^0nJ$#R zD}2k5g8{V=TIJs-{K*GcL42|lw&{(}6J27-^~v1iC)rhCQv(22UNA88RWb5^lFaB4 z;Q7iAY8)1-sr{-eQ?sRw8Q_k_-nI5Nw_dJ!RwKn1Ch)Jpk37O2bqWLf2GDW+=sjCr2Xw`wPi8rGjrR%u`t}v=>4<=WsJC1@Xj3O+WX|8`-gSN6K7@ zoQ53ar|?u-&H1vUvhk?cmvDBO)R3_pLEk?RHnuNgS2Doma6T0{#-vL^JLOT@E*r6@ z(G_aaTI6r)Z?5ZK%gCLv@SRuP!xZD22@kY6%hcRCVviF_#cQ{18nv$8kG1ix9lawY zzL?$nF6eY@=z758UwAcXqs#!e#9aCkw|PXQv%`0cAQ_9{WS85Zv`gQ**mJy)jOV5l z>gjOW>Ns1Fm1}2mN_^QWJ_vxh!AUoNgGZE^GsKL5&_p{yvNY%?13>}V&cy)A zwXDkIbxN$dSC4}mo%nZ+`PH8~Uv)y50xHvwq};LVo_g%yfu_z@2wEM6k@z$t$1oEw z&}N|k{rJ-R`!?>Mb;k#|@>WnDb*8@wHikuS%>o~-2+j0;s}jY_WV#@zg%wMzA5TOV zlz$zy0lx9C7r=`Tv=7KUdy6mZL=&gmc@QWCAm%C51ViN<=cc($Ij8#H4Y&>V$gq2O zjlQ!zHjE8)SyB-F0~%y|e18>xAeImZ=lj#8-^!Pm&XtWy|4l}#1#x5AOar%|qT}g0 zevNR!p)3wBqy9ZzHLFC@WKwm25P_x|Pbi3hp1o0%&Kr`tu{`Ln!KLc;oxukfn&>f} z1*OS)@CdWP1 zuMrUj!j0$LODS9lJsLLj03ZJDGi)FJ5@g{`paY&-k5B|3`%E6Pt6A57BhUh^<=1-R z^$}IO0H-27ALZ$_(%Z-xsq2&}@D|4L;Tt;;t5%0;4~}=tTaR7fgL!K2fF_!195CG5 zHlm)iGbR-%OSX>wpa{*Qh0R+P(L{K`Ob&4vxVuy0zK!1B7P*+k5U6V zRKFvMJtbIlB|yew8T@*wLXqmNv!}uhNM-#cW}O$0d;qr`8WAkSQ(GJ$@kLeCG?V|& z;+$%X&}z@Lu%Ar3dv%o{YmRAEqf)5>qwx93QPxn#vO202dtC89IyEhJkiv`A59Ha* z-3MWZlT&-^OGE0kWQ7X+3!;m`LNBV2dOJW|2CLA|)JZd$qW5F)^K679^rMlf1J%MW#6Uci3b*7$&@GJaT$Eec`;Sbr8GkId znH+1@D5MJDr)$--(@;zg9qGpMkkazOeupFWEE&0v$pm1lFH|dYj^1%_LDrr-2v3oE zKL}?4xO5#*Cn{1f04~8AIkl^;Ujs68>=?Z5`eZpr>yn^D7jShD)gSN*?Xsu{EvVE z;YH&=}C&0&m)BfW>?FVJK$?$XVUa)*s5P-Ex zT|}o!AWd?0nivJyxEN|${xXI|;)u^%r+Esj0bt+G=O|9A3cZ16LkP)`20R-kHsE+v z?M8AhloY?TCnro#O45k>zK9UwNUc!|fDg)x@kAGE51-d86kp12pNqL7COe-D`&Ckr zD;HCCP6h>S&mRgRm$o_yeazM3CoYA~Cy1Wh*BPqRBbg5334@LfAhzRh;)`55KgBrdk^13iA8VUdQ@DVBqO$zQgQVXJid zA)uaDfL$TB%J$rL7X=7-OOopgG|6ExCvOrpgL)$-hRpDun6~Z_cvvD=DN3Hjl4*!?g0vyOn-2)Cfhzu8o z<(2xv)8NmCgdMpj3KRe9U{WqG-_bvP>R(G2bK2!R$AN9PQo7fp^w zlXYFkCgl1oSr}ZcKL+}X1MpPJ5=5J4L6OLRi2)J?!`fGJBpf(_Y+{zyNzSay6<~Qs zD%E4<*-JY5tpi}MXKfOOLeY^kt2K@_h9Lu&W8NSGH5b|JSLV6^8KigifJS(AOOm0R z4zCO`g#1vcc>_wqkTK-q)agHrA;+c^*nN=uz~N%svbLZ$8LndZ1R?e>PWQ^me|+q* z$6?+U$g;88&e6wsVPiw0D#QtM79&#@b}IxP;Qe)hs~FqV4*vx#oDMyZHn*(M;f7Tz z%tF@iS_ycteuB0c!W(td4u@}3aBx7g_72@x{R7vYNASj|8AOz~11d;C;ZpVd#B!Aa z=(RH(D~}O>3Tr~Ov7I1iMuyi`-KYF}=T--X@3Mbh5&z+(uJmOM4+EKJ3gi|lgu(0; zz9dQ7!1xHd6(ZoPZ9E8pc?P_`>IDY19d>X^)Rq;=mue>*AQd(ExTeU#K$bc6Miyi> z2|W?crNfx6mjhyaqM=tcV9_frOO54w?8q2giMzgV@pz2nKL+xQ&BkwM6?H5R(N;Ju z;}U!6s|v!oK&-qJ_R1C6I&FIW(4r1C9$_L0N&Gdh=rQ8CPXTj!2q=3DKT!2hf{j(z zjT<7YA&E1fXQ#?=tIC2LV?-ImA)v^qhhty(6lQq<#wcKr;1*=YL+_~f0rWRN2YXNy z84wpnTvnraILo+}grR;>I?9YhW_fF|n&yeIhM3hYSBR@j@1;T(e9a334up&SZf+61_ zfdA-@kvew*2zCG!TsxNFhqG7U#ip(djb~c8?~uxqVI2i5SA27F?Hv#cPMz-EufD2A z=|A`Rs%l|>qmQsZokA*!x@OXBcoYQe-!P7|ox{cUg4>p#3!H+y?1w3j1_kpw*chJ2 z!&LH6tj&prPYH6FnooGL``_f}lB>FWe{rdKy5r_|xwZWjsdN|L2^0`+;Oc@rUqCX% zq*kk=MpO(lAtON{AO%hWph@lLyH-PROGPw-x^*qFc5R%yMWYLb4plJ_4Z)tx=Cc=f zBU+cNCo8MIj^lW&(dCcOV6+zsU_#ubpFgcPF`gm}21JoGKwh;%AR`E<^ZsyJq zv(xdw;e^evU*8ka1p2Tbm&>&3eTXlK6HfPR1VvLONrbBNu57(76~I_ZY$GC9>|c{Z z;ia$G9MnWKJx&R>)XQU2u$@4;8#H}#AwPsSOVHn92)tUTBKby-f)3;P#QpZiWX#+Z zRhWXYm(#5d`+x^86aG950L;vK9=SY)nHLzq8P`B6(Rw4LTAtpVJv;c#e+6$ckQsRD z-2nU(Zqn3fiJ@`8!}2I=p6&AMfy_|?e5u=PXB-%Zyjz+2v}TaR|1l=jzWKjZu2LD~74XD${;hdWo^2I*r@0*k>IU+~3k#T7ysJP-3R#(Vlcs^~NeC zt7~mI>^d!|AquSIf`M!n+^fh7A(Fd*eANNroYpt}t>PK@jhzqQr%$sN*XWT%q4h>0 zWa4_{(Y{sKl`TvaX4!6dWR7?{CIK>BX!MW6V}!ZfI`W6m%YHw9DhXLY4yRJD{f3;g zihxnyGS06LOCWo^QnPHV1Jv$AMhp?1ZU^XBE;6v=I+gEjd&2E&3?W-aNmOUb4srhN z<1stOe0w4ul3}U`I!Uh@d@*k^vC2Y$HTmJcGJq0_#dQ=wV=IwN(a##CqJ5RJTovzS!lkS zs4N129Dv`d={59Czu4w{kTVr1u}m!)FkrcWh)i`G@tU9=^q^;PF?GH=M>N@ePCV62 zYY314A^=CE9NC6;9cO^HL>OLoV1HdukZl2CgfH{T4=IMLYzs;HGgq1c(FHB5dvhK8 z?2|sY27c48^uh=~QTZo=9BlqeBaY%O#K7eG_fqrHhDIeffA; zcJiU`6<2x3?b}JHp!=Hu1V0iTC{_ey{157Ldmx2kc(oiZQC{i80dOFaNH2I=9aq;Dmqt?$W`Y zXd3x}ZNY;Qjl>HJugW~W?ZN(se{WiEAhtEShb0}m=^w^lVr++<+nc8Mjukjv?5DPW z7S#rvJvUj_hklL=b4wn{h=()W|Kr0_-Gk(gOA-gaJ;d$0C=*AH=%KUUSqr zoF+;|H+bz@o7Dh*jOorifmFdOvwzR;@y1dI3lk?+gBZz;iSnZPA-f&aM<%hResWd&!7L|Lf7X z&btTGjr7Y>`y!0#caKZAXbQ_&?JM+FD?f=NEKR;h*+RmX!>Y|b6PUhK-^t+_Req`sWFd`0ykJ zhnn%MRhoo62+d^UDaY=)k=p#Gy048pL<17+Wvp%);?zLqDzf-;t8UHy#{amlk6PT2 z_zH5R5rPAm+R`AoztCiSVy<+$*tLqml>jh0unE(bpZsn3^Vi^#b(h#}FD^&U&%5E@ zomjhZs;KVhdozVn?>#>I^BtntqBxc$pz(@h+~n$2HX`eL|KpyXn1n7If zbeBLQ0(_%ose8-DWoR6Q7IYw&3De^B9T^X)?wqKSV*t1>H5r|HeapLNF2-g)4+7jj z(+-3(>TW=0Cdje_WlW6RhZWuSDF5SjsP)ViRd?bTWBP&58Z=oB5D1-S0DV0|gdO;X z)Bnx%eceoBQ*VR6VQhg`1)5fwYcnK1=qXeF_TGP!knyyw$ zZkhLx6KS;A9pI5WcSiRI-kfE025!iHD=&vberw#L#S3zIN#HVLnqBvCAhRA+BM z=B2+5)9q+Vm^+`=zU-Wjn;&muGGio#$2wAR;|>>=O!FJ z?YHb6yJ*Dcbbw8gcOA&k6EZy<{STRZdF${ekvF&A8gU+v49^Bz=mAC(>qdF8 z1?JV}&fGFRn5RLQtTk3QB>M!r%C`K`2x|TXVIeHka^R#RvW?4?+z8!yxVNtu`i82o zt95Gn5I{Z&ERSgk;>XmN$nf@>WH_h1W|D7{+pzZ`WR2_trujJPjl8m$mQ372(84*_j9doUIpVb}%|PZG!(8`GhRxR8yJ zST&SZK1S<1&|j6RsmP~(oA{kPaJ1!GQ!dn;aOmNR;D|Iq7WQnBav_r9n@OB z?LR+vyz&sxtTH8)+cN4g*t9-rYr~|s+n@b0TD+S?631?q#Hcsi=pL|K{9yFEiev(V zv54jDm0gK->ITn7m|5t-@Sl!K3AVLaNWVU`>4Mx@%V%qrz;e;h)-HS1twc8_=fk() zedjTKNdI4lerdx+z{MM|GozI&ui1J|B0=~7{?00^qMtDH0tNUxSl z_Sci|L=f${A3e<&3=|N_bVD{QfESNL9QIEIQmUU1O1(zC{-t6c0G{#y#z|Z-S3|ta zg~I6zYCk40Py-*RAdtP6w+n7YfwDja%ritpGO=*T*z-6l!sL+8sx*^)Y2TprgZp?_*ZuPSMP##t#4!{)zsiN8(yHkaK-9d@M*PA)uDUsr1B0NJWUm*Q&^JFpK{efKlNQzPSEy3ZFxn9on zjDauTyNck&5?{o!Cp?FmQTn~Kp=bk0myfi39XU13ppD)1s(E1hy12A?cKbBU$eS3O+=Yk$@K#vAk@jxT2d* z4j5?q2)Yq`U`y}?ok=)A)bZRB#@+xjynCW0iviqrexPeSZV$cuh-p5(+$>LO%8nOB zg$CrZhd>X2><7@88EDEER?HUYg1`m(s&L!B>tS4ut938zELwxxd;vlO4S01v497uM zT53(7hMVs?P5&hnqVHD3Ji+5Pr35(S=T`DqCi)w#PbFN3iec2@JH^z{yih;+XNU$P zk^IB)0z;kcW8eSL53cS)iBF<-2hEEgR;zqT2F*ORc+iYLMr|#`y3TkJ{W58sP}2l@ zO-EZj5(hBCVNfadS*j8=6b$E!@Q2xZ^OywFGd?A<8K5=-v;=J@l!72zAh7!IJCD%{ zbV`F#%>*D7wVuYssUY5P?n8iXAsfeM7pfPKg%#{6Cuh!EJOceEt%9=}#p( z3LxGyAW?Q}ZzHH(R&*^I%r~AYD*}=Nw0waOo@uu+TwhShRVI?z{?cd#?J+QSV%aHT zLcTKK!yp7Ysh{aoG>GE2QCXjkA=m+V2NOo@Tb%KDTPER009rn@bx_G3AB#e-A00v5 zGgfTnrNx#~6Cu0U`bGqq%pQB}H^D*bho(R#Y49ySe@s0r`_Jy~dx~f?MBbIAFQOGw z>bxNDg(h7{rt;GH!-7G60MlGU`HCW-(Hrh6Rf~ZC(UY}xk{r9bZRo8By`bZ*CU=0D zqbI4~xbgVzbV}+_q5v^)Qb2Dc#E5}(0Gb+Nw*X*8>2@;MLa!8Em4~Jfp*Xzm57gN=LA;^!x0f4Pi{eaIW)eI ziYU0LqbdZflT&W{rN8d^UK;@iASi$?a)LJAOsq&0zg3( z$sC+(wYaP`H2{#cO|?pL{Z{g|z39Et`<-loHRI-kh3b7xdx1QLE3Nceud4=@sfH^Q zGCC_v)NYk3C!NjUTG0hlwHND2KgpN{&L_<;a?i`+xz&ulYGL}urWryOrLX@KcJPs0 z(tG9cFfhGb>W*Ukq8ld zU1}EFoYVH{tGc(O=c=nu2|6~}3`Z~ZcEdN+A-YWuB9Wq~kWN3=tp%+p`xyTR8U?u91>2(em6G8t^NJ%lg;OSl3efFH8*ROahht z_-^fn!^Rc7wr394z)IVukXPX|s6KUk+49wIKyT75*<8i072xomTtn$#*nPSe7w1*W z@jGXDI=)s;fa6JB`d$KeL0};mDrn%y(1}0%R`VK6(8g{tk#+B|ayoF=u+*k=sW}8d zt8t@O1@8Mj9o6OX>4?PGYP4m-bAqN{_{&=OsFLz&4pc7iFYSN0D5j!WrL`P{9w|R& z1+nAuBn)MlpcXA&UxC$Z^xGZZ6=NN#mGT{{PkIk#zz7>E+Wx(Brp3+b7<@20Bak{h zO#Qn>S?Ttu%w!je$!%os-p<)4$>5~oVUxEU@x#s2OqL!H4FGBiI&jzvg2~$R+gJ@H zt0>Nv6)y05RvI^!yWhgzTHUz*s7X&#VO;6=Tm3sF^q1jhvd|!HMxnrc$o~!D`jf)1 zk>0q@G{9I>hthPz9G?_}%ejbaH2P$SUD5Z7(@r&bDm^P0`9=nIX*Tg5%n4N$iX4xt zuayIZDoKv~efh_hd<)w_^er|ltxhfEC-uf>|IEDpR99E6BgR3Q1P;#aJERyts)6b8 z8Er=QRZ)rV;>ccBvDryaXMEGs>B)m$Yb)>pA^>|#IvQpeptIO&{f~cXc<3&?< ztm5HH;`CHd>^wc<7?3J}!Lw}-rR2P9i>HIIOgnb+s{yDDX_%AC#w)K3i~;uj)g?R} zFg^*;F2IA3@WjFr{4qIciG)p+0>ikrDjHCc02rI!Cj@hH@2tOc!F&p?fJO3<&k8#%X2IJ=FQ;L6i0f6WP z2L7agytvkXAu>f-8=3eOb(()o?{%!C6gx>yF3ON_B5z^N3}F6w@{Xq~e!&llBTp@nY}4tQu~4}MBGnIZMKP`o*En3)s)n7Vu+VL_o|is$GqMO^cr3)k4l>- z+FiZ(1OAbP@@;C7SODYz=TimnDE|bR#(DE*(=T)dh3s@ntEDEc%8Q`m$~R2dy2d>) z9zEq2J|fXWzTyru9KHJRFxe`~qXBmoW*hyCC}Eu;oBGhr?lbd_wI@9le6WvU1W;=p zibYRpX6qett>@reXkB6O!gQC?1|~R{^fd&ov2;3kFl@k)j+`332s<7CkEm~75e(FZ z54!!jPkGc%~DWNxJrJ7oJ0_zTcE+I9g| z7D9Qz4n_p#$062Wnf@(|vvYOGi!FR;m0^2H(GG`)p4W-y%3+q^BG-u0jubE(8)n*qpT-Lw}_>kpX^lw*XH@wzI-V636xA4Fo6taE`tJpgp(6=?P_I?=GadG_CN z?)BQ&9(e!@3U<>C=j?x9yV&UBx+569S3j=1M^_UEeh$Munr=Y_)cf@j5otO)r(kYj ziy>!7W@U3iQa87)C#3c9dkDE=_%rEGS#u}_v1ah-@Vav@nl%Xqc9vdrd!=+@Nw~=Fm7w_`ur$0~|e7=i-jxtNVI23|KCxFlg z_#2!q^kRUMAsQ|4zO*pGC(mjCljNXRZxX~_4v5%-~r?8Q?TYw5Zm5eS$trp z)q+xoK9Cy4=v?^ZmU;17ULBmyIy3tr%U}|m%$t=J! zfqm)pWzRqwC=-h*{@%A*;3y_Wa39y&`{66b3tMH&Uq5Ahu)uZ1xDf{#R7Xl$v$tlO zHCh09w5>^s5#P)U1MLWq{eW`q7Jipi3+3v`XJh2jdHw6EOM3v||A3!T0R%#zqXYow z)+gZo)&|oXuihGk59cME3ITTV()7pAxiCxP-~SgX%8{7v;dy$^Vy=$p)qGbBL#bdL zdxUD|PF+nhr%ZHO;NPR}M>-YG&D1_QQqd0?0B&_2-wcK13!IsodKQ%(OUoR)$0 z-ee^7;-8hTayXCx1O#!H90lyf*lW;c?K9ExRFrIw@;Nbc+igc`oO~-^X9T}f_{IEC z(68rpjaLO@59|3r!S1$j0Er-G!0f0irEd>02cg@M1-y1a# z3hI2X>XsMCYEL$b&UO-__~d88N8I4%4rAD;OJT{+Fi=#y)K(;iF*lRo7_z?u)C$5k*22KX-d|KUso)~q4sOa32F!oK^0Ade13rQc_#E6YB_0GgKji#8*Ai2J5uhklp3 zCwyxxL2d_Bq95U+fAqU0C3-VgQAQ5YcgG^y2@B4nAUK+3@` zj0AQ9hdtfW0wB$irBb%szq$f6EqDp(SQqX)of%%Rf~@K^QUigoq{Q8#i@KeC`>`4m z%ZR#>y{#}A5S|hr(^zQa7O9HBbOMUNECDod;R>HAM?t{{6HBKhtmzzLTqAw#*+GV) zqC9p30CVXw~SOiCfIQf8JOEh%?ro| z%#$@V1Y&M^x^=`xj1c-U53fa{3L?jVaSO?AxZJpcn;*!--ISmG+^BPq8($_d0pjTRahr59hHNXTM7by5>n%jTl zadH3$3W^r}j+XC%rP{ON1)p7MjnBs22J>TJ*fT~IW{Nj<`2vi(L2Vz@6kn@e`ml*a z_QoLpxHZv^*D;LF#zt7wH#0nM7=w`{#ffJa5gV>xo~AJ?QxbZT?BotE3JiuGyU=i4 zpqdk|{F{L)=C6T@KIiltP+p#l6gda4-mt|HcZfXqpSxZ8N?bH?LfDHQYJD?11I6>{ zV+VA$jsWHVo4f1`;2aihv62Z>HpHH3P*5P?rsK7WrP(rCV4T9>5eZQi$ zi5E=m+AY}ow6+vsd&{(sPke=pXb~cASE5atSt^CjAkzcXT6?f+0IX25r3QuD<+(ly zz)k{&5IMmQUr{}c2UER}o(X;w&-LW)-|3(X0obXE71hlj9&@ieeGV4t2^iu+ltvOt z+4WdVH~OZUB6sjw^F$z`9-5huzyBhk1oq1gqjcyS9{XI%Y`|wA;SgWu>o0|LcYk! zlqQ}80U|A5z`OddXOmkz5?gDg`fw*r1~2|xrkSkfMAPP*Xc47(^XI3*l~5Z2Y)pX7yj{7C@3R7g z?fY7!c`q;^lnuaU#L=PE=vf)VP5D~-xn`-t#T3YrLOVh58=5=mi9FbF>%m7!NX+F` z@wDrYVVr!JhU?+fsaP}J0NP$4-YOG}|FqIsZ9OKTk|Z&dMEkgWcj`gYuVV8s_`W7+ z8v@qSY8Eo1!su~P<+Whg4Pmd-b$oBJ?9kl-pdF~ZTvrud;P@d+5I>neWM<11-&aB$ zFK#SBik+aE)1NfJoa!)sw1CEnApRq~l*7c~21*S!G`qzMgVEdrDmDi0?FWN5e_BC@ zN&gsi@*&C4vYIjAF8~oRvVeRmZZ8ixKkr?Y_hiWy`X*1nL6*P%W3 z_Vn;#g#auEUy%7ZieYrxGpWaSR4`DBu#q#>xlg~Is>&gSW=!EGbAK=n4YR$|Za%x+ z9X&GY$+y?pA$M!AS`YDhP#I6CKJernb#Vk=vG~mPcOn>3$i#4=I-IxQcVE}?3p?y`r`&^)`>uU5z-=e0rFG|9K! zjp|}fq4Ws#j?NJM3J-3Yj^pssU@^b0D%0Ji-p8iym4GsCL|Zpdj~ph|Xy*tnuk= zDEobXdEfdkwQrS3KbWr6hM34=^8P<;y>(oaTiZ6QB7!1AhypT{sDQM93|&fhmz0D^ z$dJ+!(jiDlcXxM#NSAaElF}*tt&6?y=XXEP_x{1&iZgSqIM*4+ahz+u+IbjV2FJ&o z*c!7M$5KUGeIjR zV8vBOvMr(ueyLk$h};CLD7Wj{HRVwh_kLTz6L*Z00&{5V=cm-PM!QSc08m<5ul8zik*O=*O!0Qax10yzK9QNMIwwWNjG&EN8neodnOD|}0%OdY!i^-13e5N9;YuREG9sESEezoVYBpX?tLd zgvU&_PChlIUeN_MpGwcubjtL;Z^DME@Zi9tJQ=;d7EC%!*|Vf1#W$jr?f((rZiM~% zxLLKpz`oP8HD@#Y<4wdyStY)y(C@w$!>zMmvlXSf=?8iIy9c9}WkdbzLOqY7l zDYf7%zD424j}cekMIm;UghiRAxo2vM@qT6dh6vdK*1ABgE`~vG4kNbz2*#d=Hqpj| zf<wrgGC92#5fjnRg%%W8x zdgqG=RvxXz>el%?Htk2qCX#zH8Tsw*Uo(yqyq5W_>J(-zf$8vXCj2>)u{FM1I=b-s z4QjjFn6BiG*V66zEHq#{Uov1q)4X0;WJ>)r6^ZIoxVQyTCvQl+?+OJ8x8wa_PL!)} zEG*n2c9ihwReEODgq!wGZSqwscd) z8a*+4Au$}s;?nhI$oG`*ua#q8>SbW+1yULny(VFY79E@Q)DN7NNOss>f^_5dR%3F# z7iVEn>u+`4T1#uQ$uDKg^JnbDA;?LqkXG2Cj=Ry z^hgA1n_93&6vyH#;mf_9#R7GU38I41^j$S<&4k0@c3RHaCo{NLL_vi0-`F(u?h4|n z?iij_eH~sFr;}BcR&%NO(6V0m;<6O+S>^})oNqbxdOP>T6Umrj%NmR>!b^3` z5|mtR6#Y+rW{9(ka>(ZJ)o`YlG#Ds!?hg5p$f`(bjLEU+ZLW3QHJ!ewn-XrICs7r! zFG6im9?Yn~{dRB2ga^TR%oED?5|_ov^=b#K@}|B_cKzDMiA*y~o%+I7@fM*Hr{!2@ zgZkmU_q)I7ZPD*f6XVe`AD}6E6{W&x>;2TGjQc0kvZ?8o8d-0-o7C3v9e(MoNQG@^ zfKa5>+}Y)Iz{(zz+qj{5iztwFzj3B?q0C`Oy(YMRUGP)}S2O3)ibLf)nMi6GF2&Rz zb~hIf&OaW*5pN@$gSY1AY-{Hkk?$Ydew`B)vL?q1U`=$*lq0S+e!J(N8r$Kvw$*22 z-06P!0CpluEQC7K3Cr24xn`EWok!m5v2 zX}jMd%$R$OM3i)r`fYI3v$Q0Ht`?pKJnZUEnq6}|O1E50TQIbUO$H2xUnh)P4EQnD zKl67wtrCwzUIy<3ybL_}wkDz%_uz_V5wr1qYtd)RdV0ASlHb5%qu+W)Ca#sd>f4MlV!O4)35LE4AnXt@%;K#WN&?pal<5(ky_$MMhX~3Xs!=O z3ot@{uI#s#nF!n#8TGEJsITF(uVYTC#0=I~fbTu=TEFm24SXUWPOApjyN?yV{>)vW z{`FJVFYmVaP?+z=aib}pBD!c$l2#Ze9q;Q_-%ctza@22QdQ{yYzzFQNS{iUfsE3}m zQmAk3d;HX*Wm^5x48;zcPK^_n+wIBqLl4_;F6Ludu+A{IxjC$YjE$Pg%qwS+8BA=!bSs1C5W4tn|}a znWV$zuKQJNVCZy1PhtGr*>B(DfRu^bTzvLhWMan36GdJTkn=zhxZ7)8JkVO`bq+N+ zH$GnJ$kk(>^h#w~n>yMr6pXjPvr3?61xKY{$`Y7{;e;*m@n6S+vE9`LgNo5t{=#Qy z(5skHx-*PWOtl|pEF(lxw3W<>+t=*JAek8Oc8`bq(HCRd=42cD%uMDi%nw|t#4>OH z^EL{y4>9xPhHy&YC1>AY+%(~qgEKQ;oBM0^J%>Mb?$nAW1B%i-8u0Oz*Jd128x|2{ zM43MvS>H?>5C~3Q@6b>fV)}pHi$w)a|%M7%9|+~&+DC7y^0_xHfPNe*+}WAwuOD0sm<%b+na zVpM6t@}hZ35pbE@!lisNehvEDLYUymRYke?k6g=$yqw7_Y3}_le))H_E|}M7f6Ok8 z&Ff<3>Q`Rn*<(Q$QnNVK8$67J2m*9|-=~C#>F+1B&I`@|nOysE06~i@a=KdgM3pYN7Q-_aZw{I)hw5DVl8g;}2*BQl| z)!9~&ho4nEw#j^bocmN;lJqRn)xuHb z#qW%+i50E0%X98WK9RvVhmVkdv)(@r^ouOgX|(%9Cr5xDg`+g(Vor>-Jb2vu>xBS6 zi~B|Vw^&u7&9i;Y|L5?V15BgI*c5Y%@NQEkVnD+V^A>I0BUT(QylR*;qk@fgSccnL z?~V5{JOIn^Fu-`qw^c{<1Eo$pOrH-YMc50M>D#!k=iy@fR7tZ4eBSCtsrGSJ@T17Z>sg**SPSM#&1x5s!SP zwkRmfg$bc^t@rP_j`9_@$eCX7ZOv!=P{Dz+{5@>=s$+tggZwrR6QZ${ASofCa83oa zBcbx5?z<5&=%aCV?{3q3b4eiP8(E^$h&021Yc|3!`7}(A%MII_2Tj8ZWqjbd^529_ zBU}8>Y4iECO{>w7tH};b=L>7f#5X||BfTcH^JLo!6l%}X#PKmHXYz(3aEC6sQC^+b z5%^JCaz~l%yR(&ubm^y&cFo#TpRc`@chze+WOu-G#M3=$ajOIuS&3tU&5?6E1CHMP z*>Iqxt1&Gw7x#{(79CNuF$8^l0v0|c`6Z7U8vj^?hZ4=__r*o+D;s9Wzo=52&#lSx z`|R7zAyXW!JGjM?Z3`HYecSY3h}Ug!cj)z(#NhCHkQkMNpW0Dw~>%iquzyg8)@J zCk|IqOQUf-8<8pXI!FYEywySpTV3 zkrS5MVam`8=o!8H-;ADN)K_SqsJN<+E}fTb{5aQdcN9WxS3X_T)^Gea?`LD;v&K~~ zluZX}WqGLLddP9lf0zzao>xpv;-e73uZ2TD|EpgOt1KkKDTwK5J4X-1-#5e}{#1c_ zi^c}n3^cv7nraEyZyvt*$<%wwF|(^GchIg3YKy4nNP-q4uLLMdA8It3G=Tm;-Q%L=PDBK61SoI_Lb47Qpv} z2T`--a&%_DH#sM5V)REXpcOOq<$if@PU@GX2|2K^UmhMb){IP~iWTkM#m{s05Mw3J zoVtVl#`l~rw<4LFGR+6IG)TYzBgUDicy=#mP2@IYiN4<~a}jalW%vq<`kGRcKNrX- zVLqeu=rA8N{$Cc}xjq#F`gsIxqpf~>zY?ugjXuW7$Jnwr@|;=e z4Zqzwf4Nj%hu^L`JZjAJ$*}&zLfPR2mbPZ0Gu_8`N3jm={v1XGbjC|j2EFt&ijN!= z{JS2`0l34}e4Cc0yXL!2f^9SzGHH8H1FJ12EPT=a1?=}DiC5e&aC$aq_oPQMEu9yA zK$_rOt@ow1!?11PET5z~p!>_AwQ*5@hj#WN2od0)l^V~UYmF=4$!QDCbFPbH!OKXa zzWGa2Z8LyKut}GW>&fUo4Bnu9U*w-z2{Ky}aLp>d9YZA2e@=$^nhZT%KP+D96 z+L&oO_`9$(H7ilcpLz5ZTHxKTa2@TLBpyY%$P4OW&TD)s8`WQ@-=1ueXw=BPIlN(? zS)km&Y97D0smfu{>t_DV%0jys6?Cq@^oP|yqLhag-LR{1zdL;Fd!NIZ%+a$yGMd1y z6_tug=nwl#HLrG8HXBwdHA*Jr9p~l+MGR$cee|jb6bqi#H z6ed+YTl>X~wsr6Q$$*_qB-U4D-om7`JP}|wQ|F4>B-*diVt!67;Le366P0%dL~@Sc z%_2Mu(ewb)h)g@Bz7vZs%EH)h#-)V!71dBXlZN&OlXK?4jG;PYJy+q1hDK4VnD^?v zUoVsDd8BF{A`JYzd{>IH1e1~ji#;ER-+p->*5eW=P(-V|L6@U+6VLPv^UG-iR2vxN zbX*6=IN91q7=gw<(K!UTh7x@V7cHg^Bm|k!@))OmDJTgu$5A;@S&eI^k+ekphV6w6 ze!2@j;ouAZB4u~o*%8@@r!Y09Plqb+H8>RuDHoXT$$*GyR8 zupYV0%1Oa|H69X7${qa}I0%WQ(!=lJ`Q$Mn9%6k+wpJh{=y7w!%i*rtz)4(Vv>~QS6!YJewpygy%jJRdMNy|xY z;HIXQ@%I~^Zwqi~k-54(l3lwHUIVHOdKwk-pU~_BRCuYue6G>WA;eSqAYqRP*|wj` z7x!K$x%st6%LafS90&{q#a0a8w`-Ryb6pD+p(wh=@g(mro>N()!uP{D9Y{)LmZOYy z7_;Oq4u?7>a?e|(@PrM#akU4qJRzX77_n~xj)30E3nvK=n~*-5?gs55@!f2-xRWx0 zB)3r}jl*e*=+PHj*lrSmJ zxl&8(WA?Ev+4W-YKdylYNkq?@4xq^&XPq{w+FGH93FQKHH7tK584N3eeO(obCK=ml7!_P9ClD@m~40~}=kSgv^OBOjbSZGA$T$?kZSSJfkM z?b@E@TN`=Phzhj7+S(gr*rRicmIwh%z1Ag5-Al`73c=-Ime60gG0b5F>d68`C=4q%o%2AHd(BEqzkWGseU}P z0URxe&!BwWF?d0qXh|9p>T^T^11>}o#A|RkwIyQH<*)z_dMW<{&sE+#6k;nLZm0L! zr6Uk8aCoksVPk?2)vzm!FxAEYx$vyRvyO#D4W{5r6VbaM-E-pC*pP!a6lh@)ylIY1 zw_X6HP?7}vE)rkTFe4?Sc7qN}CnJ$N1OxoDy(Y)>uOq~Zw;NeJ<*6VlD#D`G7k6e4 zGT31gCQZkC9zU2W<_1mOD$}F$hr&B9J)K_*w=F{q)FS*iO==5UVXR@~_=lu&X z;xj%CA$#4JwuQ-se2Rzn<_oDR-Np(T^m&!V=~)6v2-3;>0vUZ%q?#c!?-fel`toLC zGD*!P>Erjo{?yDDUzYh=d_93JcT;P1EOsjK-SRV<2PV!g9@T0yDE374Dm8k1q1)ZgY?kY4v$E_* za}E5@G@Koz2Jgq~y7aOaQRr_NRFpkVxyFwN@$FbRK<+`wdGoJ!WGrUy6d7>Fn-Q7b zjvY&J+hy>*8cv6=5zP4b@7BiTpUjswI%O7K%!C_N1P39!)*f7>22$PD;~abvO{)dK zr!$}ECzr)9tvmTuj2Izs8zq)M&e5|9z(5emre$j%`W)|TdvS8Uhz-znJhx1WVYCA! zuGuciys#%JwIndOVa&M^T{NAPG(y=dA=}@8`6+c1z00Wd;V35@R!e>ds;59%Io>x0 z26p4nRxYXUbBKRN^~J>p+EEnl({5tAK$f;5DlC9e0_gh+;DNdTi*JG)Sr9V}7oeNZ zL3m{#L8x!<4kWN{+!Ru9WCiZ<)~^eU&a&ACydO7R2Xj_lKT+fl6+3*&9fs-y84~rs z7#G+g5~4gjU#fC_y);H8fKXzGFW)i0V75JVtEfNlsqIe=PUutXi+;NZ0t*?wiPTQO zt7~{pu=rE}@K6IFrr;Cg;t&9M3qq&om{aG#W@`f8OkC}8M@chDeZl+JA|i<%_eON8 z5RU=!a#*H`gtEYanDiTmk$T6KI-{lO=nmRif6v};N|Q8C(6O#&(WjF=|-7L(Rh!4Z3FFaI|Bvx6|fUHEw2yd44ruMRQ_r@2HQsTblCCC>(3`} zG?HYif>+2>cOQd5OTd7ZgRy3<=1M-p*EYx0AHlC6mwbxVUHk;#oC$VxFl{YHD9MwP z4M3agewi4lzIxnlOb#mV)R&&YT`#!+On@2~>gRlsJI(_P)g^_u-4PAJA8ipXW_0fuFh>}-v(6qASKx#i@yVn`hm;P(K)@Oz zMGKj}Q9we8JZ4soxW>N2n}5mD{5np*dS%X(8(%!ra1%~RTQ3fBy(gnbdKt_t`yI#m z5vDvQEAM5&y-zDmDaK#nbE2y<_xJ6q>E5obgd&+PYI4a82pu@2Z=Rf& z?n*$Ytv(eDL<4|Pw`qQu$GR<1V3^ia)5F3l(IHWJxRRNhLviV<0atJ={c;abs2Tm6 zRZf`Kb;s}y5%RrB+rrPxZYUX#7E}%JOfB&rJpouGnE0;-esTz_f{Kv-u{tO$0QY=j zT~m8emsIuv#;)A(Z4vmVmk~I4*$HJ*RJs{YiWS51M2UI)0O`6_hA?o2Xy_h~-$q#M zwbf^SuLhsu0A6al)g^L`ocNe*hUeAd^idRJ^%=ayrVcEPZ7oHK2_b4Lx%Hf5>J?jK z^g#O)*ggNRfj;!66#v9%=~@Detjx?87bUguq}h@mdH|@5n^V7cvTievtlk;Jn_`Cr zcipei0EvTgnM|*fwR`u{w9=2S+e__{D)pqu9j|POKm;b0Z8A@Emyd6`%W43Z({Ygl z&+e0L6I4?mj}29YVss6jP>q{<95XRMYyHO`U`W>LvZBU=#${B^W?v;CC@?+BC^uE$ zF-*Z^jCjIBJMWWd?g3C04p2S;ml^4R)XfNDn{tFe{bGr~)X<`jCky97MxkuK#Fqtp z7J-XRKTm4?DnL#A68W!ZM7@&^jHo$0DSKoep-lg) z?8+*SA)vJgL#(4F$ted>_Im8JcX0^7&jmk8eLbnJ*J<)AIHIE@O%5K2X>s-T+TO#~ zDIR?^1OWyC#(nR**V_-iqNM%i`X2}tO)?v#55dLA`gm62|7WF7ckE4BM-l>{*37-0 zsDbB*oGiOsleJ^K+iSneOp#5W?Bw9!dEb`>k(T!uI&YMIjbpI0EHg-3?M79tG6gKA zG>#)ayf-ybS?F{8*1e2Q5VKwAsqan^fBRp&8uhuE4adLL{2dkM8HX3&o*`1}z^~+X zduJE&4fTs3`Fw^le3ms_Ct|`QZRE1i-#&MmSg)tEYL>X2)`(nmb4;-{PIgDf5W4c2 z%hVcN)KcdzH735ybRCpq-`)3?a2WCHVkvE)m;MU?ZDea#BnJOro)RM!VK1Go!``1@ z3NfsIA$hPI84hxA3&>;(d^o5bNehX-Idt)4|FyB56wgq+a?@mQjL&si8Vcby)~T&> zqt)UoyTeGXfi@Nr$-U`P<60KSyVZUnx!_j2qQzh7GD)uUarkl;jN=qAx^V?ECJ5yS z4RxI-s+(^g5b950WxR>rDLcL)`1R4hECSefseJ(6r+4pm?f3x3zW*f3VXsedq*L`$ z`J3y~)FiFj+wn?F=%GO!AEqxa)mQeC*)8?vp8RcK#JM!=t8xT%>&Wnk$ypJLmUD9CLW)#U~l5#*E&JE$bpIga-xL4Tui`)CaOjHwL5KeOaw@ z{8DG+shb_3)r~8cwdeO@8z^HLcY&>Ok!E%*n%@RB7N5SAoIzL`Qwn0reoU^)yFTEqUdJh znFFr@nje49Vl$W*R?$4ltRWI7;4KnUh0h%|P#_3#KY@rcXW`^Ue}D*p-BFdj+-`T- zp0PZ3n%~lzZX|g8g8D6;4xd9H6hs=0Tge0*1il{0jxlw27{$-TSf6w)1buEsuoB|b z;dFE>TyZ>o&SCPE{`iFQrLeC+3b8M0U~5gI(LrmHL-TN)Q8zAO4o2_uz_4@0r|tLi z6Vc_i8N=Fn7Sw=TykiDc!MiFF2ZTENT1#%Vi91vWzs_*XGr7s$1xEj_hK$aS+$qPZ zg#%l;^U5Mif^SSNdhepM>lb-&6!ld|Us_^tRHlB6RhH7or#zkudwc$nqeO9jQl`X% z;gvt%aQvQmBV7l*_1uT1DdsR2Z`4o)p$3B29Lo@{O2W!rW8;j|8WmCLSL<6V^Tf$h zUp!+Kg8`MggNa0_b8@XqNsk)Z+mB9Z_F42^Sa;;4r9f;Y#An}LBQxj1aKv@&g@hG> z1!Xgt2aoO@x?lhf)92@XN=S$RB`5Afal69$R^Qr$r5`Y7+=ZCA&GX40ohOEdH{~;a z-%yv4_)JgzjG`+X+za%bfQD7(KH#it&KxSD?)!C0<3sgu%6%#*g||-t66L+T2rE1sDOe_XIfKC`R&7-^}r?9h8bB@ zhIPsEn8}P#BUmR~FKvN$q{Sm_X7RCqZ|i#T6W99M8ypi-|ebeJj0M za8?0LhrZAG6gGt}3$7K-%%yg@+MlhQo|XUj@x<`z$@{E0OB0|hc?dTozz#F1UprEm z3V@U`Ws;Ci>cNx75HD(eo=-)m*$@Q{WCi?y7RQGwsL&d@Kt`p1+4ISh@}O@4IivT0 zZ05o{M*jr8(ouYqq5E3|vozPYMCb#``ddy}box9hhKRAt>tcf1Sf5Fmlw(aSBeq2dBO z%puBEAP_gS?k$oxVeoCwq=55Q5Lx!{?S25`#PA~qO6Hc~SwX|gi&#Arwxh zNWro83V0Ld71{F(n@siDZ#L7ULfkP3T#rqU*18()WJa3a%lxb-B>r6`1uyr$>crVn z%H>4eZeK^6_`m|e%YAtsR>%8xZgG>0pF9vY#%(v(*U+1RWVR`qK^;WxsR3fUU<)1< zG(sGD&%(+N(Cl=(hO|Lm0lc$YNBG7gaE2hnuBO+ZJvsF)9niTAd>O#z7*<~mE)K4v zhBSKD*97hVOXn@i5VeUh}LL=&i5Q6&IzggbUuzWRg)?%KxYEVN-o}4PrYf< zE+A(xe&!zSZwp=m?UHQm3Rhg@;o_DMLO#J+o#$6c00K&wNxwiJec;<#fey)|LA-|m z#i3zb1xNd&U`neq8_IZ5I2SMLE7{-_Q5*sS0A4rydP21xxskuvCuOQALK>0MbP-`| z^0lNlH>ZQZfz6H)r^X%p!9Jf3>^%qNMUz>*1_^* zKqD4m+Lm&HjPBsBeD{KTWE}`88**d}pDmy+(<|#-Q-MD3Joh=;;}?qkngM74Wq`yf zX;gp+M%-#owjPXl=#znmF~Xf=gG^JQ2O=7OfS%2|-$@3U$ouFVFVy@k9wGSSp3V-} z$qVpRkn&CGo^;!LTbGYjUJw-*_0d{~wK^-uciEkiE&;QRUA(L^%?N^nvF`?71=9g_ z6|nK0;Vq8&PY$Q{fLT+U!hkd`vf#hS+y25UEeZuH8IJUgYA4mpF3Ke!Xt)We$=rZ6 zQU`3$E=p~veP^%5ki2|E%!_HWVyL0kbso0x7ksR<5@i?dj^xj zI0Y84VUq~iZ@15AO*+-wtCFI~nbPK(6nrXt2xzxa0;QB_62gYD2G?%CM-PV}Jm)Bu zPa%*DA|n1l&41&Eu8V6t9a{grQ11l`AXlA>VvQe#8*Mx7E(YRl+;Pw| zfd{bJj7#H8p(RklmhWEi)%JdpRCFK;1qU!9!Ve5zpN_s-^92#7?(Kz+x25AN-Spv? z$gv*H7Yt|DK`uv}5F)$&=Mix>&%6!*u|CUf>#}T#pK#6$@v`>X$~YREGoKa1wLC?A zg*`b>#-W`AW@kSju7gH+#89G+*(-*wXz15@Z0lfln7Um3Nq0gzA`DauCdVtpl)C!$ zNiipnO9jqAaZ>rVcdezNazSTg%`)ZX1x8*D5I>6o;^Nu}(jcKR9Zaf&o#$lXkq8Oy z$|4JDr(-2|o_|Wm`=hCK+$;SiC3XE4Oc6;FZI^6^l|~1~4NU{I|*PobN8~4k9jhl%Ew$-}Wp8PCS zH;2weC#oZo-O$T}IKHT$#0L0;s6ds0*!KML?3A{j?uSn{p^ySRv|aK+e!+sk_4)Vw z=lt&jxey;$9HQ(wK@_aO4B5O7)+|PqIdpXDD?Jb7ocmiyte0`|2=;K3>?_C10^QFXv3p{#RFa3 zu{&zM`5))>--83PEyVTNGx&E)fyiwd!`~Allh;?6Fi^Dpo8__T7vxp-$QSU0qctIL z_UXX#ptU!9F2azjLaR9MRE0Qx%)}0%4HvZ5$HXc34|777{RP!yipUF60^S@RV zdfRXY%CbblwLaPLT!#opD*fCP)XvneU3G?iYA}+SGB$;LOyk zIyEjYOo!#k~`imkmUZ<%%lg>jIT!@W63<_oyp z0m7ZXXLot`EGt(Y`J@s1?W5o z?1;S|US6Ha4SFK{P!e4BJ%`MUt8%NN)%(A#HwXcV_W%~@CrxMG;0judd>>%}T^li= zy}f~&@sZTvt*F+Uj6UzEFh)_9W*1g=ZOP?xQ)h$vxO)yjMEA9$YhH7oD^sgf9?qP< zJQ<3QkdNLA0Ew|9s6HT48XrijWB67#7g8p%+ncS2VjP?gM?izC1~%_nm1HY zrgCd<*gUf@uwHeYj5{Uu_cYE(R+vRtrP;+%{6eq+QhAj`4_tpTsbTin;+RMHzl}!( zzNfU7MD$<`bsX$mb00y_ey<5&f&h?*uxcQmWSbL8o=dU>_+Ks9~*sm2*zcJfWHDp5WQEcQ|gZTY{DLhfB*n} z8GOkVnW^q<>dsBjN48*+70$uOzxPGyKjcE>O&;%SxvN`rf%IUV&t2q)F{0@Xbd(3ppH`Erdc#38&W~|I%y%SeA)CMm`Urqn1;pq!bF| zQP*u=legYb0LI|ylaX(c^{)vCW)_S^$v4t5KvgqD1uGALm>o!~;4aWv9_~td^m&Ao zQEEj3N9g62Ild43s})fouOje)VFOoGr4EOfA@kaGLC4LfwZvJH{AXX%{TJUX9UOY& z`Hqd!5rzy%&$BB%Be!4XgHcMKA%Ly_lmK^00*OMFJ^c+iH)9lVn~=z3e!uQ>3eDGh z=sU`5AO7_ZAi6Df!E@rq5vhoMXIJ$joj1}YO=&%rzBUzX_^lpng5hw zG%!*!5a0&C;112qBsx7uPoEewpY$ln`vF9K-5oE!T?KKA&cUH01p$mNA;%XUHNsTC zmu+S5YI0PbQnHczMcV%Jy)FXPmZ8!ob#H1Yp;o54^g9hu!3{B9k`*~*X-o%`+6>g` z9F)X3CjZv6P)`@PSjOQIkd~}Kj>=a_c6JiUOv-w2BCnQkC^;)~_y=m@f>>DwlD8=c z8(k1$uRF{fl?0~(KzEc7pxnOwQ^l7C@>JzWtH^l)X<%}7$nS^7DnZ-!O zV6>fqt9=Pvt^md0eFETauN8A?PQR?0KT-k-;)CdQ76|v-7^|$mp3U8{U1v}bLk&mV z&(g~mb-9dU`a)Xv@*s_tqcE8CAAt1eNhps6tjc@mp0ul9DKY&V3NXm91PIhdxBu_L|DFf23Oo(q3jgx(@3fut z&guR{XvTVnUIj`h2N3`g9)z<&fW&-+fGTkEiU^5B0cY9g>j}V&&!DEF0XWeqaNQ38 zIo_}Y4I8X&CCD+Avs0ly*BX2w$ALe@tF>1E#|^|TU2awe*?3prD*^)Jct<_Im!@E$ zP@sH-Kszr5;ivw>-N0zd9z(?6J6_a0dGrH9(3a*0B%Yvi^*t(g!f*JMLF`qrM!5o@ z&<{cAfb=AAWE0|<3Kgs{zdwN#h!=hU}^$*=4HXg9vz6b?6GCl{ZaL{|$2BZrsg!zsZJGzI8VD3Fu$@V-xq+dh1E6ZYLL3~_{D&GtM1kXf>Fnob@OY_49s}q;OwCO}$2N&@%e#&Ngyv|gX^SeJ& z|Ix5PXOVs{bSRdLvC{s&REPABdawIVpC9?;MT0;ELA{d}xJ33ig5YvYVB6$$4ivnr z>^SC!)lqYz5ZWy3`W>*C&Mvn7f5B6s&0e0gF0mRC2fA37Mq(gzvwll^Rt_n%K*olZ z`F`pk#*G`|ctO9(5f2wgr~rQ&AvybgxWi7>uF#o;P5j+wT+~Y>5dixh0%R}1CqzY+ zI`A_2IkIP)F1@EOU@xIf!EIt>Z=j$pnW3Nck4T$(h^Z_*&P7HYXo0_C4zGfitsbDq zK!L6$>9`6#kLCQyZ>Q+C%vz;d>)@p7i{1t8P08u(TV$xJQI4&}&nC}^kAZ0T?a!(3 zw?8-N`wE;W@7-%pZuLM7@D!2o~h-heYf@X=gHay24u zs^n%}mDcyDT#J3v%gr>!fL0)+pKo8f1+?71_mf;Ntd5*WRvz8}c{A*dpo$P2O36~% zg($pkM76pDY5e4LD8oP6;bN2_{@?8i=^KX!-j6A7@zH-zffi2-me~%Qm=iS>jAgq6 zvNs@PT~p8AIR5`Ns<;e46XL7W>$%fFc%kJj*|y_48?dp>nDF6q=+M_GzF^zs<;HX_;FsyMR}=zI4;l8^&>c@ccVg#qi*94qts$I8>}yb4hV=@ z+mjjkN~bu-Hh2r_rS;(P+`(kSr`w=irY9YkXI{G?4g2e^tKwIZyoD8lydTdV_STsP zG1XP4zXmvMX}l0{iY~#OPWV%J1k=exWP+Ga|6$;OmXNOP6gaKwlA%atqWi!E$Un z)fytKKblax@o*tO3@k5#T-_JLKQX$!u&bFSMi z2%z40|CT2xIZ@u>@DvZJRFgCn_mj_V}8ba7YH%K7E?e6(RjrM1E^jxfXp6TBT*v` zhreC1FLaLG>~L{9&_hB^n) z8tALk&x`n-to9|cT-+LFU^6MSj$nxm-7ZlH_b9v|!R-c#+r`eSquz?yme@}3>JxBs zo1nT92o0eQDa*rwNB?#gsd3HtRDx3t33R3w8XbQQv7CYKld}t~+Afg5Rh1R4SA;Bn z0GuS(+OdD}?n>|7T{qSiIVy6r<=xam73#LD%9kvcx`z7a%{jO0D*VZnXzqh!NxQ9CWM z5m&#!l?Nh3->*h{rtGvYmLvP4-!EHmW+l`bHac#N)=ySeY!3G1&g93d)^^LY?SHU0 zCEU0%632iA34opf_6BrF0r2lPh;RV!z%}~q9{qe|X0Na=lNSk6Z#}89+n=-;op?WP z#%yKrL#b+oXWyb#^Y^$L=2Q-c;_*mpT znK1f*_iOCtznbW^u#x%p^84tZuPV8{Piv8IN>6!E$x#d_H$k&cm2=YL|5KT z#J*z<#r!LGdGrT5@Sye{??Y{pbtuR47=drCE9!|hOzm)e7ulhcD{dC%)5aplZqaka zK+MJRCw^SK%iddB$r+cM#e7Nb#A~E@5ucCb5&wsa{dXc}L@cK#*i<4_6yOWKn4J%P zf)4L1PGR`uI?ubu(VEx(kr!!cvsHHVeGA`buq&F0)<|W0tZ(X)kO~GJEQj19`xE|A zqoBmSY=z5^a`o!~&SSgS_Iu2~BfU4vxBt63%aeCTBm2Q&+G?*9xZTbYqYGno%&R}Z z{DrCSTvz>{nadDCoe2o~xRfs>9d-g{zPR^0BV!$d3kriz(Lnnf=p@Kr!#od=h_< z5+1aFZyffvfs52C@BTWFEfzg-^9RSNg;qn1)P$vR?nnh(wsMKkZCL7?{(lOx-9eKk zC{(*Hr$MLZ{P^k8&hel|Etcl;QVu1z-cz7&P5vZ^2SiMyq|h#jz#%m|=nC4}6i{5kTz(zD%kp^dQjIs0-Wt*ps2eKXkyTz9R=ws2uqB3UiVYm+r1=v z0&WtJja6*0_ePYBIE-pgy(4vk@&ypRHkIMuUEnE0E3zte+V$V?Fncx2#}qR@#ft&z^sM;J!m3V@LPSIR>6X~|$} z|F54N0&A;-*h1Nq3V>hV2ShpO0T+mst^xT}@x*w$y2<3ph&pOOWo2SB&h&_EECGSw zvbO-Y^+@(sug8@H_ujDt0&++N`u*U<&wtF+Zk6ztyWQ-O&IM=~01fWn!7E0&`6{hk z-#;>IjMLV2tu{n91?&CC;>Q;wA!^3@8thAN{bOo+;SS{E7eGFKC#w*R@;?IdBK~(S zkcrPVo{|E2oLzw$=G9EaUDQ)>OD+NEo^#%vzyJ%ea}skr-?14Bj>h+g7@{2qVo2b@ z@1dUR&&v@21PEFvs1Mo3kr8?m)H{bS1A6EF?(1R;)Eg#?zb*a10W_BzD#|qxebd`# zy+rVtfx+;r!~c%q!8tD`xa~EZ zq?WgJMYObRWz;)n`v#%6eGmMy7+%_|`AuiyoXgr`=4pCxUw+2EaH;N@A-K}>(W}2L zFrgK29RPq1KZ7N0U-(f>3fc@k^`*3O$5cfx++D~S^a!+}OUhWbBNP#I2@j5!p=k?!_PyLdNL-?nr8@e`_+kqKR(6j2*tB5 zuR&4M-|uwoe!tEJuuj0+DUjX8BMV2gVRn5Xfli0}_UEC{jZ;8G3vQee@8-yyxIatM=nRkfjI zJ>uE>%FVj0bbBU)gmN!|29^-J%q_|U;%%<}#wt@zNAP7T_8-*y^LVOoKe%go=_H0*$`Je zRsa}Zci$lL_^akPXUsz%AVq=7VKI>RYA+@^4t!ARdB`1W@2wUPli5iKh-Gk3Nw8g%*-pKJRGa(z!@qs_KP5vzB zN6@Th1j6VzAC56BgfC)l_Ud7AKHqWQe-@`T8S(GcUg$tZeMiW*_wv4!cBcfokneCV z?(7Fp(Rks40`$7wxcR&fesm`Fh33yrecd=OEuWM1oxB$@lc-{dsrXTV0CoNU9f zaQL=!Exvt!UscCp=HPS)=UrHs3M@iajZ=wF?{I)+k)JpcF?v`VYu`f<0*}^a2)V*> zN!Ar1H5Ylzd-Iz5LDR>nMOkj{sC@N9htdWWI06pS3?^S+!_TkF*!%z3dJCwk+U!bE!{2M`O?yYbSo(-EnNcA-SuBvz4yMqZ+wp7 z5Eb^>`-!#YnscuG%q^36P`8lRAWb~Nw-zRjD#)F5_pkJ*)=eNyb;v(U?p;CPD2fmV zfuDLF#Yk^?wO`Mu*&m4k-laYI#N+^Zw5%OWgaGf-n`rfKjSZSAl#G^x?VX66kBCD^ z9|=NvAl-*KV9aiWi&DU08K$fuM;J zPniqo&)JUU_Rlgs=25ohHXSt$W6|LciU%h3;? zYMzhK7tg0{-nXr?(3KCd2$D13aLUoQ z=Sh`7@%fnU@jdDzf2`K}NVXSUmXu97{b?vv*sSax%1|BNg|1XW;!)#>b=dapj_=UY z`k#N|Mk#Kgq%JoER$mgW5gla`& zgL%dny}L-Wk8!XyMwy~fXvZWXzGCl07+y!cJCi{8$tcCTFym)?Xwo?mYB~~?R6MsZ7K=UOyY0W9ISJ(#U|Yxc z&ieVrVpd0B6W3Xps65Xu-xLAO{qn<)d}ra^9r&Ku%GKC*r+*R9aXyeA1^@N@0DuNuZNPI zdh9!7qdDa`^zM}PS2ZV$fG2h`?J10-6}Kd{$Mf`Cjd>=#a;qQcdQM$UkCL?9zqK7RU(S(gZSU_w&?E4j~r_In*3NeckZqQrl5(d zL1+ZO?$j#0Djk+(Y{uqN^cF?2fDbm0I?D|%qLUO0+=I_L7S*1}h{Fim{&X|n9pYce z6Akq0X4<|*2xmec^=me$ao|>$FG%m6Pz$)~Gz4a$O9ZYNu9RkWwY1RG!IF&su};#k zBxtFyKl7TpNU)j=u4R$HFR9?HW_b@q7bhu}l+UA(!+tGTa{X~06UI8rZ+xRrZa_Ua z;!*ozyVx6)$B0dYPx(;uWW$F>2gPimm&?kx%Zy(aOPx;kCmpi9e(h}ciggZRyuBlvwhihCKK~CgYob~+Y z=I0zguOXYvshV(hBHS7WILkq;Gi~X)VC+fy&^JG#@ohd^-Gk2&7x2rq`e#DRg!CJu<-yD+ZNayaCjs2mFVs3j* zl1XEfd+Lifwi!{*hx;&q@i3AluilEa9A=MI92Ll=4dh|PqzNpqELop2Emb>=?|ZPj zZX@=ILBU(;t%RW`@1gY!K{(EM)yW3_?osWVrB0T<+rDVms}xq7n`5VzFBj3a%T$*0 zOWV9`9LDw2XL&xj`Eo5PYL;C8?xF2B9^82L_cW8`?~l@hu1{TJA9z^t-krIZ;g6nIfokYHt)8vcmnrjT3BG$n>qjX2s-Ka` z@(<$Z>O(Fu7J7?Zx^P!Td_<9HMb8F~v%3IKR{i_QewE|uv>g_t9KPJ~+d$T-xpZz7 zJ@u}6s7eu+nwRahxDN`IVXe%@fan$Swxv2Z~ueQH+VKru0?eB@ZPm9_F*~ymV)} zC8`_p$@Kdp_nVtN#JQUC9(2hlzlZKO6B67HL?%B3zD`s|-K-6|5~$zoD^c*;=77g_ zGY&TWb!YC^kD4Uki8Ke*TxR!WfREeUcR3X(3vXJ6GXr)7;OPY1;bQg)Pxdx;9PB6D z&H9e$!`o3c)LW(wYP!8DuV9rhYCo5Kl->}57pWapYi>i=5uRd6CvRPR??ntn(2H># zAGW>D5Eeg35qsxE&rg=q2W54=Z6+bVk@M5zT9gaU>8Xd-I#lr`9|;pQJ+wofeJthY z?L@W09XS_G)53Slrk1Eac6K6%;PX(kMq-7+u?S9bqJ${pfD8G)@rA4S%G;4R#-+DW zmgxs79;!EyTcqtLS065rER2cQXCaaaEp;uCB_nxtX?1TfUA^DYsl-qq5#=6m%wgSg zpPC4kh4a*A0{vRUVT7G#TEyybP%jUx4 z^2_zAVq%ZpU4H~X#RYyi;iYLE-I9BQ1d_isf?{Pib z-S7T2(44bfdg*P5HsEq zU!;oq#2<{iX~PpD^zoR3#EAOC%igJ^#Dd_*vbQ2xn$N7Y1tjZom+w@rEcFWEEvA_(Y#6wmiEHNO3QTONq;C>YA072U)(6U#D`fIo zp^X_bODO%7wlX?+7+ox|&d*9dHsr?n!<#c!;N#(DUbEZ9b3yF*FPS$gU`6r)q^rLc z?Y4a@S8CxBuJ0GmP~5Uay?&v2%Lj+_Zj9x3R7Q5b@8FaThM7dat<-GBv0q>ZJ65x!cPdxP zpkn=r={ITG-j2QP>FeVC>z4_bY%}t?ka6Sa3}SzLGX*c0Jr8E`ne#JTGdqWfcjDOg zqbsb!3hDn@*bH#9X&00+w>fr;Iy2Bldri<5}|9f22KIFXnzE1r3H3cK&^D|GL zYTcC0{1tlZ&9u>Stm=V`-51OFAM(P7J3kwIj&Ben%TF_I*lJHCcz)JDIwX8ZBihnz zN_LC;e*T+N@fkO+>O7wbx+G^08#tIqsoC{g*bj*PcX@k^X$32PY{>OTE|FBKACZ{Y zm4+>G8S#EEIuyVjDT0GxumKq$M?NSeWpZk1#!Ccce7h@t`O;P1hmDUvT)OYg^2W5V zd3V-s!@sbf?D)Xhfbe+Jb9we8PVHc*e#c^}{h~?*2TtYmq)qvnl2LY}gYe6vy;nmO znjQTt)xw3vKx7xYPb`9(yVDPNPE23Fd1KEttWWA7HhDJ+-mOZyx^UY zU+|^!{oPqA36A6X)T1@zzBzx)jGLOfIS8x`4l|bXhu)P_%<2}E31hI&PoZGq0k{yHQkDElR1gM|)g-+AAF<<4F2};&AL9l!9^I8|cx=2)G zDY|-TZH96p#GFhnNk2hXt8;q`U-3r~N!8%K?u66Mg7@)GQrYCm!pHc7DR^kxFMPO` zylqEIxqKrRyDHEHA2jI&+e$sJYkKSBQHAdpesb2NKAn0T~yS+$e1 zbxp`nUSscL)$Zr*vcziI0S)=?P4Ts5cIl8TWr)qnPXZ!mf_ZM3Qp&uTXLXb7Q+6hi z6HopT#Qd1M4)+{ECj_jAZxe4nXD*OTz%K|Jed<8Q)rEys(*}n^=>VWO^lM(~8BupY z`4U}e_QvkE2mBH0BbR#51Me4(_W8|c)(^!U$VE~t88J7*0k;IQl-(0ajHbC{Iul`fE0EtnYPOE@$-m4HPLB>JDJ5 z@CXmc);zxN+sZ2p^UVlbOEm5IY~5ovokq_O-tajz>ShB0q>=jVc+!0BB*_u7yXQp3 zVJ#-I$f?0nMnkAyWq$hjwKqoqJVkrCZrG^Pq6xtmF|6w=fA^fFeDAxA4BeI@)r+KO z7?M$n3aV?{*a8wfL@AWzR?_L|8H#p7mQR*@G*a32tOd>*qFjjWgi1abGD0v-rgQ^L z^l<(<1ZNZY7_O^dYVd=izNR+L<5>&3DqMvL!Do?hzNahBP#4OLjFyPDIDT5zXNKT5 zO5$&+t$57=kFG`jgn`q)mM5!6IMSKhN(xD>^T|vF@`M|fGh>nQ*MlWZxW8Q`mvPVX zd@ZN_I{!8!>@{z|4rf?WNkr6GE#wtOhaw zR~3Q-pP6!2`Z?V${`_^^hw3M6j7_S)nTtAfD7`#Ww}nH2|E;mY)4c_@`IdHe0r~A7 z??LYqMgF&|U3T*X9P{65_D&hQQri8x0N zXX0yl?i(=Mu<+5`kI0!Jo`1R(`dpcL3auZ!%zUBB-VhfHs{}cO3yHP<7c^v)j$bQO z&J<^qg(0MRpf`!Qnbx*Eo34g zBza4A;;aMI(VtL|6+Y$VQ@XjAV^hhYNcQ3<0hXic*}oIY=%Uae-l)*~jw4t9`}llL z*2m0}6071FX^0JH=sTZYB)By!#);MID}t?5Pg7Eg89rZ?8Al=~A%43;taHlM*(LxA z*e4286zTUw+A2>wVG-akvOEQK7(xYne^zVPdUE|q z@9UXa1inN}(Iw1iC=w=TU?9o95>^r*(|GgHiG8`n_l6@E7M8KZslSNtHJ3=Umc+#j zKK9(+LzR(#zOYg8mSp5C21%xb1wOG3x~@V&3aIlD0JF{{!sZFQx>F?qJ?F)0#XPsj%N)RYoPk2<34k!eZaACH@w+NUMU**W*!_8_ zvE=^JcF5=is?lNL7(HVp)p0Xi*H-E{UI8im-ob)Q7WnWM&jng$G5MZ`vT)@3_5c zH?H=B=fT{V4&`;#cxrhCC5yQUbyCg%FYP=e!T_M=ej>fj{1os0N9?zl0RMaK-M0;= zYhrcB8%(OSqTxexR%nSWNge@O3*Th`|FN{T@UGr3!$<#%c@ z>?Bp(Pt@w8ffL#aRut{e+J*|x<$h%&hhjS>+CDZVhv&I!M_NmZjYE2&(lLT)o64ot z;o$2#t)u&WaeKKj!#oZP-vl>`t12y*CDL}-;ss2fm9D9^#atKu7+vXZr(bDOSUl#T zBp0u@K5dU>eAxuDrNet&>f`mEU?SmW2Pyd}zZvHf1`AT5^0eHJMWZ9H?9~m2_>b%R zIyj}C8XwfZ9PzguIeFVpzIq&-s%gW6 z7eLK44_q16V%xnVukVKk#a}R>_Yr7*k?MazgnRN7AuKWr6%_Bcyl>hG8;NzN-l@WIS*Pm0>`<6hT`PutGwG!!WQygWCVX;o z^T;va47M~Km(*qg%mdHq$?n46V^Rb1udY7CPujIzlHdQ@Y^X5%)E!>& zX>{jGXzZLa{xq_VpZPu^icG{$7Nh>j`0T`^<~|Z(T~=p#fXeEV=-E>uuGU_d%=;TX|%81^~4JurES9klDZ2NEkq;PlV$Hmsx z9u~&esk~bGOnfj}5Ax>M%y3CY+=;FnzLjI4RUeD%`TKNOz$|KIB#x zNG(X+w@G9*(ewsaem-L`-+^5)D~Nt}KHP2u^LMS$xC~vKCU*h8lvosnIyki*eOF@z z7mJ?_#ve9x6J=Pb*lzqzs7?gjc{umn!PXYPrl57qMkf(|J-)AW@v4o=}pF_ULG^cmoK^438Q4-r(FOZ zsnA4p^gLO%z4d@dmM}Ae^F({#i+3o(drK{0*PS~3Wyu2Lnb5jnyLy(v_%pHqfAg0x zAyVD=_?j9ra%&Ki2SVrF&#OUN=-ncK3Tw+^6|7E^eymf7t%e;9DVYp!4m zLUL6OWG6NVHuCMC+kbqygj`TEqQh)YYfIOty1nW(d@&zt{-*kdaB_}lvb{5;LeC#j z3|Z*iT6sPqT+hbSlUPVn_rVl-xJ%X`u?b$P25doyygvOuoz-p@a|F(8-s z{#%@Pn~!B?yk@@AQEQOhbTn#O3;84^Wz6%V-Sr34dNlT1>1;lhx#;}YntQ2Y+wbC3 z@^fEyzdC5)hdU04i(eQF_df37;#}}Z!R=i@p6z*=bmc@IHt9kn+;+nHt*YZv?xm!R z)WvuS4JAq>KHvdo4Is@KeUEM%Y9S2>2*SW{t-VIJaysRp?B0VxH=mu1A;lg=we+0K zl8XGQ)45rhyN?dCM6XL0)ipr22=VTc5&_l1dOk>-L{Kob_^oJy)xz7UhAH9j z9DS8K1PMWT$RKCtB$Dvvyp7IYMxzrtZF8CY(R&$0^#DA<@Fe3JJ|IRQ)>m^oLJVoy zbbGI754Fq&La2oQ6Yy~vyAg+rY-(7=O2{hyt4$ybCL`eI0Ux#*!F*}E5HipFv2dCj zCujsoUZtD_L&IsPS2z#|h#v#RjSNsFmDfh^TmeSD>}ewh$MbipCK$}5isE&TA z56B}y6e_nr-BnztDke-7%>IcbzY1dSy|*7HgoHE&6ri5+LcR@R&|(y>QFUKt#YGUR zIC5pGl}n%A(uDuVkl|JjngdxrnfHGNxeS_mazf5QEijpPzCNg*ZS;hAU#KhlI_8db*MGBRuA{2cT z5XW97D+Y26T#Bw%i`KC#wN(IQ@f$~i$_AHlOc&=;cy}+Am5F5dbE2Z%x|2ah>0duSLo;xz^9kZ7d_bkF8aLWb##kg0c3%JDhzAvxO<=t_H7>MK#fXd&g%ee_h zN=>si#v=*@h*?qZhOG`~^A(D40b<=oXc8M8wTvq|xq{>K6ak|`{p^zkc!(aT?W5{c zgC<5oO~McKS(BwMwLsGS%`GhA>eageC?gA-|8qil#-r+8n# z{%G)CIljN?b$38zzLBQ4DZv>bdcx7SI@$^0fjDkEl*qRg|mIkhy#SxQhaKz1{!f8R0@1o2_aGvj=@L z$`D3Cr#C}86GACcY%iKc0s3CVX;Gub?%LpaT5Tur$c?w7eIEt1BoyT2fcdv&=#JD0 z3sJV?x5{leQ!KMSDijMA6yNaA7Nh6(O%Ri~huV)~XCs>c0kL($_!FJ4$IwpY*W)3l zEd@l&C`A!+bVxKJYUrC}EJjyjtZ*M`{rAmthYANhx~=%h{!D3CcgKuSbpVwR;jNKa z%c`3}kZ{}tgB3nBOSr3=E98Ttb+>ie7&OtZCk0FO@SQ8q3(rWL(JI+EdWHN(`{C8v z^mCXVP$;d;ZML^PCu%UgBj$nAwX$G6YUD7saqldMO6NyOa2HXW$j4`?7LB7I>yZ+xFITRYPw72n#na$xPykS$0hs z^ZRw#D{WefNaPovCb0+AKs%g43RY%*%<98v6W>>-&`v!eBtT7*EAtA<6lJ?ciPtKb2Cj4#&yxz zX0`}wmTSUQBd(w-8xF0L9PdA5?_-@Vp;LFq*iaqEm&Hc3OpMD7#n+#FTB^Uqa6!2I z$4|4Wy%(S>i|x}ERU*r7rcHX%NUBfk1H)w<@hL9DG=WZ#z;b&+CpAakeNhW<$ZN^d%oVP#ru&}s%ssno5M@5)GDuA zP8tiY6brAs{|91)mD{nm3+G7Z?vWZ>U&psEY9RdHMjgI=PkdmMvn(4UTtWadwtLPL z$A&wSg?>$ex-ed z_I-SGWlc~?fPK7TH#Gag1@g)&9xLHTIGv#D1*yMXJRI5a@T;S9{r;WQ zoV1ZH)XG0kSuGT}A}O}K z@)QaP+@hDT=!J)?>zCBMH*kxQE#T87w>B3WebdmWmXpJ#W^UQpmt?b|5Y*^#oK9La~4apd@S|sm{ z(*E4->2k6ZH6?K&Bk)^lA@g)Ry?nzQ3!y#Js|Sib3Mkcz+~|b6} zTx^*<^IYo557SshmPRU??_2vNqwUc?E$3ql>n67OAS|t(@GX9g?^&#II(2IC$Rnxv>^yaHnC$+mHzSXytJs>h1 zuPvqO2}v2fvtB-js+pebk5?Pbz1;A-d>h;orfzOKD!46utmpHUgP`sGx~9S2y2GuX zI+a5K@mq#TBF9p4RflFt3=0OT_A}|%9?e7-9r^~xNd_})DBN#s|Bn}-&2P||7FPsi zC@=7e1JCZAUQS$pD7r*XK*>f*35<|sPj4e?bh^6TZh`$#-WV$7esvzfLSLbMrn=2$ zrJFR3U=n;GxD+fl+AH*av%v7Evv`YY6rG|vwtwyN(SBd>NbO;?R_z+CL}yr8OuPtol!qN5}7zUqQTfOl_+`e ze#4{q%1sfx6Wc+Hk=S2IcDfrGa&$mTi)y4@v~jE4TLXuU@$(mS$RJx<9?fMSb0W1e z?2B`^9L;NFzn%d;mLu-|b8u>VSc>rExSl2NoiSneVkOShG#BGitI{G@>aS+z;gg47 z45S*h8R)+nAMyB4QD=s|c*E+3DL*Ku&he}L3C@JIm`HBP)zfQ`#)eidpXHgXNn^$D z|0W(r)82ku7Zz=o_bAXMGF@CZPlXKBVds~U&WINT>`IJ4XO)>MTJzfxb4*>ZJujAM zweU+?-|gfI9;QTQX;Fm(@82^p$@Sf!`DUv{7LRuyr+ju%?8`GE)#(Ha{j>+{*@x-F z2qOw4lWQFnBk+bq<l4Q*e#TP6(f9o^I#Ky06fgbv4Fy8G%kCP(-gk+ zqPuXiE`@)bkxeFnb0#eCG>m`wG6Z+T&$Q{8PKVe^bUOGo3w!UxRv-WW+A=6l^>=$6 zOE1v3PMa8an@W!(3mUvs$haVrqJ(5nC#RCFln+*8Wz&$oYpdh+%ztO|;3#TvsdOya z!BN@Tf^jVLHlDSG6=TO0vrdFSu9EIE5>{D=P34;B=9$u0qswR|&YT%lDQsF&Ki}vV z_o`2|u!Mp0^6a>u(HTK{?n)qsx}Twtz_C6)-0&BAV~ExTA6#Bi=+FHpP~S?R~S zM2xFho|LzCpPI~d4appn4nMYEa{GG3J0y3( z$SP4EpBVgbD^ediM5*$|*6X0S*QvpuaEgAEe~FU8j_)c5@0IpHbrYh6 zpY?vn!;u{EoWb_qxZwA+t)4z_Np#vb_ZP}3e;5BBQA_b! z_4EU>bYSDa0aYfo%;<5&O%ueU1X9czaA?0vBOu$LOa$tL!|oKv)c5OQ1p>H2`{hJk zzE_wrj>EfVxu~b0;z$m?WEB&>q7*S|GyUsp;POz|F^B7Uv3b6NICk63dw0r7GaVGq zZZm;dkn1T6OCSM{cB|;7&fr2{EiFyEsxg~4cHPD)3|7TyAS2zgr|5Xv{J|o!aoJZM zl(8nz6A)3)nQN35%HY?>t)m1MgWZBh7B+5UPQYv9rHsrioi+|w@Gox$MM}2SsJTt( zvw)z^O~I}g6=S6)1-7mSMWOZrBJ0G1AF_4CGjzK^F!KSLf4jM+s{7_9&plLyBfIpm z&1Z&#*+EL4ne9G!S!09}4A$C11P8&%@bQ=%Sl2-|1Xbq_Xmv!cYxYnh(qV4)v*@SY zSZdE9JR^6e{W7KZv;8F?)uKmP#f&0v=l*k(b3qM|4*we+JE|+XC7ejz2$XkB8f6}7 z^GLzPX|AP)J{~4EJ?(#B@e1c$Sd_R>iz_T0Z-#JFC?#50*zOm<->bZkqWNHsy6EnE9iO1l z^W~2sT@3aH$ak)#hSP)bqugeloG=7kQL%VmZD&*W@JSnCxZeEPJ4%_1Q{Qf|Mk1(1 zwXu=vot4E0xxi=Fz|z;0qh+c#G-FuF7!3Hu4^r*&jvn^kbp`%cgoK8QSDv|NSYj-c ztukr$Q>mOzd+-GqSsWM%GhSAKejU=>^tSe6A9l`J^2XCHMQVHgL)y%1c{GM>8@O^6~K0E4*2Y%F3ej;Nh0d3^&|s;hFZK9gNOs9gXD{ z(6MY@j1inWzvomiY$mP5Uri|&9T_Pb9;eZl*tC}}#5o&UHoTZEIlp0|@vgPQHvOjX zRQO%P1fZyfm_>!DC{l?l7W?CW>aGc z!#<9xJWia5p&Ywi{f8cXkB!y7F;X2gL~>OJ$+6 z_`*I9xevu@wjAp($z|G%-1fULHBpwP^a@;$@#^|t@#qhyhMrOs7i-;S`qFV%-^^Ny zk_8GO1j$0#T6#C<)pm}$d&>ql225}>W=pP?$K!n4V`J7^%4Lt)JX}K4Nse{;rHm2A zhTEnFB`q?dasBD+oE-(8ZBx~bdtNpr~81#No%66rqh!Thfq|%gqf9H4# zdse7pWmYB6UW#zWVyWrZ^Q%2FsN*!l>j(|8AVC#t(Pj#n$WQgJ7hNeDybZqt-pu@@ zqv3;50WE+`?<1C;$4hE_%Cb>dhd~;n3Fc_>3RHOn>WJ3IAE+;eTN3XY6079>L{lsU z3>)X=&qx6Q6<_o-eSU^t^}HB}{99>1lwF?&Zn**P`&gQhXH=S~;lNF#B?NIYLB#s! z`PMFt-aoeN$@X8pYop}ClzJW^LO7UENa(vI8Nq*t=);C%3)s0xPDzIV=A-TQ8O_UL zVh@C<&AXPCjqr=E=q8qP_5hq=)bn}+l&D(PQY`()!Kn`hbS;<{#MF#X4x@)xkHI0^ z$ILz8w^Ez30pka{)==?#`>A?@-_!tyhl>EA!OB`iE0mv8ceGaq-vC~VfB(a?S~s)4 zg_kLUoWjvTxZk^4NXDlWMVRwmm-Kj99=nK)1%o1X#|Vr}fNL7E9BKS$X{6oqiqx$V zJD9LnLeb&jf}YiOpW{f~r57wGkc!on=r~#1qgzktQ`Py6)2CO*qPNEpdp!UiAl3jC zSzvbiJF%ey0c%CI7&WzBB;*BmRaLq~nI)OB-+S%9JH-WUwc!d_VRlXfYdJmAEmTrr z9U2IPVnS`4!DblA?s(42aQ~Kq9~or>7tXk=NDE1tTi=LJ)BD7-y|%C%x9=SdukrQ0 zvp5mk+?=t8-2Oj^4Id$JgXEuT&uj1MQUt15luWYMk|{g^m0k?gGq|QTklHUFdIJ46 zY99-f>5m(6(G614^OxM)zJ1OFyKEL-Bun;O{iMRlP6Iai!9nbW3wSk1lR-%=87`dW zC}qBCGu*Sr2$O)BJ!o9*zpgtl zbqhT9fW@y_IqBch8y@9{&mMN5795xVvTyx=L=23=?ftn(oQA>0ML(DVjjPIBbfHPG z=t+PH?|p)bSW*^LfGj0hVZON`^1cAs9+20*dOko57(jqFfC#|NC?F#<_@wFz;?E<; z1=An>)v~z+ubFYAWdLe~RTYG1MWUFT{dRr0POJS@@kD9DzXlnE;hw_Br&-BGBXxDD z8sWjtvL5PKYpBJ$b~68T8p};!9T7gOnxK5OzPs%&N%cjm;vV8bw}RTfSdraCIphNz zP1|UG=m2!&!p%$cP#D&J)>~z@qg4MVwB^kd`pWooyx{l$UFHiK&rHFZ&&k2|;)DZl zYKzDNDfpLqedbu?)i^YA*tOg~P7K_FEk(<$2>eYg*}$R?xY?JXZd#!i@4xaN()_n; z-6%8(=ZU*jW5&-R*^o?VryvG$eBPQ(j9kcs&Ijb@Nb0CWP-tRA+3zaCkm3u@e^QQV zDgL8AfO;C+++(7Zd>=&G*Z9kO#%E4X)scGKN^9i|Cl^CpctG>FHk})i0`|J{=uWZt zdT{i{wC2j=u!Y*Iea7Gkxb#Gkh1e*L9vqWIlj4y}ZqD;mEJ()zQkakI(T?iYLA>91 zM+Lb7vYVQ6at-PG$X%phFV-79@l&YmDAuW)&~1D(sarR(7)QPqM(Sf6C)s?w__ut= zIkC?v?0;16i!QN&I8jc6+0`4-+Xae?3(6!-)QCUR>C;BpDy|p-c0z7hM_}N9j)X(P z_Zc99(e)(;?TE5dsI2f;r_=K(A_^i$2y`@47CV!x#SJ6K z;$q~?svWCR_P^yV8N-q1k0oz8vNcw;t_6C)C&$KGB;sMr9CABaCX82=3<{M5IWB8| zs5O;n-Cej`i0n@!J{sw$t|4SM^bbVG*_?I&_`um8_QF|A!d(DkyfchVPe?`EqjjI+ zqJ!BBHmKf^{;l0TG1i-gmKvy|>9s4s>8~Qx#dRcfL}Z-#-{95uU=UBVg_`C49jT9!&i?zPr6-{!c}K3zVbs#hQ96dXCbjaUapQ&Y8~?I zaLg}j!YZ00^_B%zsko3{ zq}KriAHS7|1IO!RV&%X4C@vahSd<=g4&@5X7To(GA23^l-Et!lc=PO`DeeJ?d_W-p zC$l-#^oi@R(39b_hrNeXqMf%4!)S{!Cl=S(I~aJ z@MSm@3p3waCeDrw)9`3`4lA;xe3DL815fowFtg4?Q6q)Uikgp4DKn7!&~Y{-kYdNt z+WXmrSM_67Bc?*#w>6%7bIbJ%jXGA&rXdJ>V3v+K+No`#I5=4A9d?H*ME?{zQ0u~h z(2vP5srnn0yLUosfA!0vFTpT82Kgq^y79(C2r~Ld_j)kK!+yQtpDmZTvmS?9ADQo3 zE6%?VP^^&#M{Nh%+vQ>$7t|DY5#ND)X;FkE_cyPrnY>^Zp#$?&nQ4dQqnAaed&eG) z**&eM-pXd(d*S;1*@%1Jj=aJaw|1M$kyO6znM8qN{*2|Owi+P3$B#%zANo9)c&y!g zu~ISFx0{Jz#6h?!;tO<{Ak~8ZB72L4A*~>*&%sx{$`8l-Tb8#g`*LpGS*a3I75t7f zUSVf%OitvHIor=;bng_p#-a2@vgljwrkWd4Lw0GrAt2E}dV_ zQxen>D7iOADbE=ER8OW^_`yft+8pv1Vwf%?vF-MvT%OmD)a&!=54@)ZhdfNs`93Md zT8}&;Ir^CRYUhN*M={F*`I{TBE*_(asM`UTG?5u^^g5!|1pm{FaezjoxQ2}|gxR8i-aGDfQq$`jB0YVSn*SMf@ zJ50%UuARsJMZM2fHQ#Dh#cvOOb(l?Fm~{u=Ewn%6glGKpy=s;#J{r!4``b-cfz5bP zy?gb?h5L8}N2|u8&Q6zb@#=2EorHrWYx1QbBA`Fz!%w=p|C;zslQ%IHDKerzFSMw6 zt}ubQ-2=Iipr?`Gd?W?`H_|UTx7P}tBSKnM!3lWZ#OTC%_`u>wfpk4UL64Zt;YSh- z_6!r{vP@sriMn}mzb2j(SB6rb0y-ehJ`?}r$+4U>VC^P?Ae)Bd`g7C9&}hJpIQYP- z37v2HArIqG^gJn#t|_F7U5nkcfgYU427$<(?Vtm*@-k zKW-9+JTtxsdOhwR^Smc~E7cA664i8@uu*1(ZY}=KcQ|VGG#D2oS&J6RuUHjS#Uzcq{m) z3B*g4Q*RX;IHE7yr6a7u1>eXkgsyqiAFH*8XMry)&J#A07Ry3+W-Xfw=QG%74lI16 z$mNaVZ=i};;;=6}BOGAV^29}!N-h5_jnRKhVZUutusHd(?pReCu;8v8p$$LG>0Lk9DDnip zlQZt^|Mxp{CziZp0;=k1*S2-et{DzpaRpE9DR6w90&eFy1%?is+RkV&LH_na{c)Zl z^Sv9N-4|~!(C}a z5E|vtC;A?Uhb)p6?e#!1{+(_oaGud*_OYDGnBv zE7AN%k=o-5S^}fD_ZY8S`R>Um&>4HMlT^OFB4*iZ@F+(0wm?5)YK_m-$XT@0Cb62E z`|*3>EA0iGZ&lmx$LXoM5P{qYJOcbOG`JHrTV%$|A|P-jwE96z*Ir)sxT&>$l8Kd4 z+Bn}&VQTDL)2esdQTR{y9Cm9~x@Hj&KJEu3Opf5MC7lM!mE$Eusf*pErtS{3wiP4pO+;+Q9+dnM~N1NCn z>aF-2Eea?myrE2(lNxd?{1f|-Cmrs(a2E4R5V*f|Lg>~;&_;L6OQVaCqy1` zh@CXv=*D3fIQo&!3?N^X$v#4=wtv^X1|tB{2YPlTYW zR!KV{r1(J|3fd9_AwF|9taKLT6vqCsH~+a^X3qyJf1I!KyLPbYfA^GJQ+@dWIzyqs zVI|QN7tM2~?u$7xl%Id%&x%2o#1Fc#25nj*(r#>1&`()(_({AFe!Knh`&CFr^QQq1 zE?>h=;OXMSHF*z-Es}gXXv^hIOL~A>H-FYf9TLzHRw7QSe|HQc=pC4%cXRfkhL7y( zmDm=V?PUbnsKqQrLu=ubfme49l=LirJ719?RFZNEV@zyO=fvIN^O)~VkeBvN&)Qpv zP38aM%S|VP`9{T_Fo)0iIg4H1W!~*6xC+s~q|_!JTa!3v@6?#>Qab%&cOe#epp1EO zr(~_KlcR{#eUsf~M^643($@~2Ew9P!B#>C#myN?QW#=M`H8Y2HoQSTdD5Tv!ZW_(k zCxSi;tbcMH(It4E531qDg8%wWPfyLbN#}lKImju?IW#eUfTJ<+&liBEEtKx1cM<0Q zPe+4Unv?_i$ZZAioMv=$1lgBcyjqCFQ7B~jYbXU|euvhBy54b0+*d2%OS;R2V8fL=%?w)9hD3B0}FVeY`6?nEr7$ivvC zBCXTbc#*k{m=S!oz$lqb#cnD5S!4_Sdvei!=!dYoK`kSRd`l(C{F4mswU3*I*4~%O-_oSAAXlt}#K&MqHT_d}^3zjR>i(E`nckStP>-D4vX$RKm1`)^`(v{j!{}W` z1PMVzN1BL$^;CMUQ2wwoW9~gLa_7JU3uEJpNOuOp5-a}%+uL-2c`Lxy71?(|8Bx5@ z5Xk^r6RDX~?mLiI+W2L@x@T>l+B_jJH7OE}SkeKYLXhk*3}@r^&FgPkE$-Ni!oP4D zZn2md=C>;vjr7sbPPAQo*VLx+s->gqM@aP1NepL=@tQfL@TU|Q&%gR4(oO(4N8?%V zrlUn|-pLiv=#`_wFX4T)s)j}93{Eg>U75kyk-;dRd(^lWe_qR&a&%`nAh=jEhYb5B z_(RAsmvEZ{6s}Yo_c>Km%o=#dXsWE6*ym4wa}d7Eqbf1>=5?yy=EvoQrSo|j*^>B^ z<9GZf>7dQbJB&spb;#;a0xIp5!e|u-&E21YSCH~WJ)9_$M%tg`lR|tXU!N+{KA$@D zeP82Sa}?#`%jXuSZpiypjat|R%lj>}!);(0E5O{i5_`P&8q8O}-8|Cfy(m#Myy(O3 zjT%mD{ZbT@Wdxl8bR@dVZ!FVGXXdN;O}mVYgtLB}fqYGYi72-^>Bi@;So;9lpg%a` zet@*D*H2p=qMY{>={4I%VSaVL{bJDS`9K=ucv|!5#WTn)*^gLGE+lXhmS?jFY_T0# zoLNB9reib(49f3JoEvYGcawi15wuwW#tZDtsNtaKEvf?@&z2aCq+e(LBu%N>vYFg1 zD!&u$;m7zc7&)pwBmLU_r`y##{7e#hT|ZdhjX&T5LWcWK>Va2lK8@1torsA!$(x=& z{MziofXmOCf3hMSxxwqu9Fz9mu%Tj|T&G>*lUtPFN-v4qOQQ^Ko0HbX)+3(JJUea8 z-%5J(9keZG7T&FAndZ*W{=JiS{jCh#-pngwJ%(>96Nju8^D&p%2?hzWm1a2XN4Sl? z>0PORP|g}6u^rZKW9D#wqU-3hPgvhij*l4~tjfya%4He!NyQ;qSuJYM6yg zSLNxT2D8+t!N;k}f!+{!it(_s5Zfs*<8q;H;5n%lN_i_|U}1sFqy8-{-fl0lKqMGC zVYqCTcpjCxw)QYJo|Y(9fv{$ZOVZ(j-lS=~L+FLi7y6O5xRCpFg1Wyj!7 z>3sY8peztYQdYVqu~poz{zr zR#4a)iuh_QNsPXZDH&XG+@rLh>f@H`-*+D($}%dU0a*xEuIiO|APszz&=0RiZZJS6 z5Yjmsym?iwmmh*!dcCePG(l0b@`<_1?bCtI@M+`Q?~J9NVmT)Jgt-J40CqdIywI!x zX0;a}7zEylZw--)A4B2Cpf5G(iM4E&8q;M6y9nV7wU7UZx>uL)BLw>lk##TsaOn5V zC$}{nbSEu=BKyxh!EXBP_8?*_oB>K*8R_No;ul4l?;tfHvCbtql?%eF*2>w>aw5%W zvpkMHPgPE68kR?|yVN$P-A7sbXEt|A4q3Bge|oJ`ww{MXd@zb5#Kw|pKAB&#K!%-x zX7dE$qp66Onl*)B=Y?OxT}V1t#gfvFl7`M;U=)&FePeSb@AN`9t8#N8_s_ z%CB^9gUh_E)&l?0?F$KA0{{5AENp&6vGYDe5Ho;B$ZP^T7Yt)^$KN*(~iyml6Uf(h(F86ptDE|ih?!D)~H|G#?-t7DC?(8$q z%+Bs+7Qq@&yZ|2vQ^JnUtwp6#HEJEli*q-d+f118VV=6dUCsbk8gC)HQ_a5iCH2H= z6zesDfLZ#-a;5V{*MY%24$hvo8b)cIs|w|@u^JhOQOY4MQKBg=c@@vAgL4kxg<%6m zWBM$#J@1x8&jsjF7*VYz`%|Py{P@+5AiWR2&Gxz%LDVyb(P3340BGQKuKwJMUKJN* zh|rUR$cm(%9`4bTVk^s^r}*nwj3yFS2m9!g>V ztM6cMPDIrSYSM)5>^n14sLG|iY#HDN<$%dR^~IvCa2|v91lcOEN%Y%V;aVRqzuQRO zP+%hYWm3J6Ip2iLc_C~6>xAMxcJ>&~lz9`P?av1vrO%v65PeU51Qiv~S`FUjV%IaNuA1t6o@>;nWeJm0*r{>OV{_Gow<)@A@2N`heQ5pm;Ms4qMa1Cb3!S$$9L&Xb z)8{!#yb{l;u=!>z?ODGT4Zak+{NAo(v%G(aF+3>v`=yL{d~e@Om5U5&cKc2tiZc&TPgDM z*Bpq$-Sp_8*p^9Ho*Z>({>%6IQJC5|$GA1-jx_c~(TBKxmsk6^gGXsEC0(1h?iL9M z&$U-o?ea!!o$|j_zW!`2p`GtO~g6?uo z4(4E0na`779q6I=&}(toqIwF16%QPW$yIhHNL%1wiD21DtCZmD_I}RJGyP}G_Sh%N zxwU8tsz2pac|k-4;~^bSTO&I5HMJ9EJpQ{Y;3#$0pDgfq|Ni}P7YBzYl^;IH-qh55 z53VV_c>a7xN>DHlT~gv?Yi2gm+1uMYUphQ8JZwuf0&Z05E5cS(NXZZKu(0Ums@HzQ z`zj7Dy2)Ac77!Mrx~^2tdC$vN&bfm(PtVRg?Tr25WJ$a06m?d#$;PcQ{$gxgHNsS( z7S;$Fy--ZvL`rN*xm8g1q{&>x=tEb%wV8}a+K05JEyijyWvh#pj|u6r-Q2`*0JZ7N(+x63!d#t54pnKlNaeE`QyHHaLWu zD%AOTL*@V*SEK&2EEGt(I7&MzDx%}Sb@|~B>5)cWlM7;KSJ%>3az%#Hdx4Cv<6XkP z3aON@7g=peD$}*m@Pvhjd)GgKoX*?TpLI|3@&z+t;!&QAeTkz*AwcQA1no;ZS$>ozFJUcV<3F<|sEbv~=?h5tm zN-aM9{*FZTgU$;7Z87V-6FC&f^685FmkyHa%ZY6Ke{QO3-n>~ZKc0PCZBN_aV1M-Y zk5TrSg~c2lmNDz;=~aJUq-}HP`u?5Yg_+3lG})B{XU&6rW$fN)W7pii?ChS*tk_V6 zob^!4d&+tryT09njqW)*KSj2p)s09anU6J-38?mMoG~-qOQBny(b4V?qG{;w^wlJJ zF%4k(kmJeXK1=mDt+(F9YD-7+q;!J~aIn1Q;fC105UvC~AhGLUzzf-ZfduDD;h{?T zOsRRjcJDeS9T$3%ZWXV6(&hhS*&3DMBjlwh%YV^s^wmR?p1`sQJmcJ6=2tz{XKlV3 zw^BA0Q=O)wJ8z^9x<-vs+`NBv^_4KCaHVkd!{c#l8Q~bDEkmW|tJepeuL`}DF56&I*x$e0}gq zmw32$_69(0x*6$H@N7RxAukI4@U3rJcT~WH6)4khZ2JoPSgJ8GNUP8^-^OW2gRLYS zig1O~C^n8Ny|WJ{*Q+Xxm{(o z3!G4n|9 z(o%$-dSI1uZs6SrMw`CzF$3NAwP-glU6)qbb$rp75~evg@X7gw5e1A~x+T!=+N8i) zRwf2irhtHe>}!K?dn*QBrPbZN-Wk|>z*lE8W=5)u_k$8vOBzhLHvG)4i+VgXB$qp~O%;b6hYLQcroXPvuI>Fe$cMkqDdtBS* zMa8uKD{SkN2v8wDf`qiqCy3Mh{8>a6I|KgShUX{GdrH7zOiZz{2!vEDwE3x25i=7L z69e?Hm*7oJ?ZKtP&nf{J5%A65f^v*eJeP%An<&1yia;K|CxJ;)4j)# zTw!5hZT9dMP-|Axq!;y|jbo<|Rz>~h;_7}a8~ppX{q8TBdi>@D z_l*WVHV&V1P>k)cW~&|FNY%Ln+xY#S%LRYRC5b-R2|23JmX_rKS2v~UhD`OmzNQ3+DDu@T71&dx-ONIQ?x zT+>;6n0B?Wun?wl#lXOKm0m7OJGAF5Enl;=o%&U+X1@rp~&Lm;+UTn zZ%^rJ#>dCM!D6{TfByW=_yQy#B!o&sxa=v>5NCs>&PbC!z20H!;5rLG*r}a z?I@FS+lB`c4YO+xJAxagb~ff9WT$%3Xfnu`j*i&a*i0;8^=Ai6<j}~J32G1sz7^|=N6BjVz4*eIL%m+%%gX5W%G8ic-DUEB~3M=Q!aZ2BU zRCRQi6nLU59cR1Kj+#|N2gN>(js+A7WEUA?&Bd>F2_B!2(6F>=4KUNz)zz1X#9FAS zt2-PhJ^2;M%9VK0{5f#KVm5@I2hFrbH_fhH)eMgl(r$E=&aSE9(8_0-8+hBgyS;V{ zW${MLvW`nv^W5{Sgm~uGDDI}w>L+E+V>Pvv>=4!q#sRCVkDZ3ftXnH4Va{o3?x(nX zh&*RrOtMh}6_8+~3Z?1sMy%t%<@4n>XB_O>L8gLLO-xMMFe{>>AhFh)k`e~UBm#ct z&Ygg35v-ZHxs(7zl9DV52?-Dtv9ISvL@pr-j!x!&^}O!x?p5Avf(ks^7)7X9{jP+C z4_?4&D3sT)fHD*@H(y*kc{nhj_qW{T*$?4Q!v>^b^sUTLUmt+2qM`!D02R`D9T3ml z*Vk9&hG)Ke@szIaB3FNJuP#8-y(TJ1K;NTh@yvF(bV`^{;P~Jm0PH)b5oxK*m!}ep zECN5@51d$P9mgU;hf2+BNy?FvlXFG$IArTboHoecKv`Etkv#&Yo?$4&eXbwqEl_5CeSMUpOO?CF zcXPz=1!SZY*9?i4t~X6yc{<$|duH&1LlR+Wtk$#1wj5t|E%wDYD|icE z(7ESie9h+W%aaYq#-PFirFW}?f~grB@0YFPYv}0cz;Hxy%Uek#@3@U!Z#-(56g`wl zP3Oe?U>P}3cs}=;mKVpyqO1`1j~+GlmzWWK$MUr^S~68b*|?6Hb7e;XKI-VwTBhvC zq2K3&&eSG=99M%F7t5p$k@M$cac5kDOMT zT<+c|atRCgfC416>9E{qURYQF58y2}EFz?+hC&-0rO6>L8qGTB=0{Wnf^4(Y;S3EL3@|@RJltPwyB-5FjmJ((PSb)LmU&F&Ov_;hxb^ zE@0N3#~|+>v@@SzWhH}v9#aboJbLtKa+8>Ojuy(RwCnC!?M%^nXoefsy%AQ?O#}Jo zXmjE{4VDdan?e~KCVpoqcr!zm2_r7n{lzB3azOp4gav$+coW4eK?>&*QR1BC!SghE zPrs7MI~EpEk&*NZmy^USLttb|8QOHvfwz{_Fd8y6+GCrimKHkI&j0}|V9F-(Uqrlw zFYQm8EY>_U{I3m!0r-N@Xb~wXDU^OIopt`VaA5luCOVSDLgSgAXW0wnHUkQL=e2VG zBVpLVb1%KSA+xS-|9o&$Z|+SUeSNn90*|sE7cgTzJw2vGFjMt8!PN-H5i~w09`3z* zw$oM;T~svXCRgUS=hK;?th}rB;?BSHh>QDb;~1I(!uxcc36uoJw9Nm&4|3(&HA-7t zdI`2=X*0XD6bh_%^Lv{PK&Ed*1TY9YaIb-t$0pLEfX`;b9+c zP``ZJ6ynXBH;@QcQI(rF=>UdpZ3O|&YsV~OBad{_9uTO8XSs+u*5@S2A`LmV0EDiD zD(@Zx1|L+YW^H{2r1+_H8u|MDJIctyo02tx60r5g#s)|xgWO!~Yk>nlgADXtg=Y7s zn14g=&7?^;xg}cYsf%}Mj{grjCQCEGGJxUX);Be!6&8?o9E3pLnU=)pP6K*;>ItEO z{lg=w6!=A1sKY?%XJA^zJs0mnJgGG6w|}KV$gCkiP&x7MMi#bq*Ki-)_ZL0uRo&dA za^l*e zG+tN>$Q5U2=Qe3hhNIfMzP^0<^7XG@w%E7XVN?lIP3r0O4v31UQ`UzLN*#H&ph<^5 zLc5TzPY6zE{8;xLG;e_92LooHdGUFY$v}ezr0qq7Bf3IfY`jLx!y~!kj zKMW9JnKd&nf^@kXXgj+}B87|FoPOhpwh^iJ|WfJ|6;kI}ibAsMz!a zEgG$2U?2)ZnHd@xsgjz-@Hq<-hb&(h78DJkfUlP{W)OSOU zgYRwVAXcC<;B|cXa1s)em>2@$9GlM6>ad2RR$2l2m-6>MxoGeq7)>~6-Bud%klzzFajYC()FY6q3q!rphp0nar}(41f8OxdMa9uBw8cg}Gsh zpCEoOy8lZLt^q{>ykAQXz48X+;jET@FaiQbr*oSL3nQ*8)7Pr*UIZjh21&eN5(FH6 z$fu9qYrrJjzRduT3N&o0DU|p?S)J@(ZS^(G6&rbLy;18PjQ*H^aihIX4`grBEJ4AH zKYEGp`uVqg-UMMM#LoIdNC0jYIF7L%E8HJ{*Zb|s1$=r|gGO6c7Z*)_?VEf4{Q2aH zSB2Af^%J}3AXRT~SqTY=ih;XzzU(SATfR#AMn-4&_>Mt!CnB$F4A6r#YA@~bPVg<5JY)S3~VpwmiH)DZ@7o=%{?a~QZx6I z7mz6Mh;=nz1503A&JO4s7%<~{KgD0TjEOjN_LcuYxZC9xzJi(0o3%@IbP=C}fb$Re z1WdDxRm0TJB+HhYV%Qu};&Kxs@&wnHFJDxAxJ*q=Pou7wnQ=ls{cm1uT2crGehg64 z?5Sc+rXLo;O~W4?NL~Sf;BmJKam=HZ8_X6UJIZk1)pjAkf5gqD*6p=f!0VtEt=I5l z!UBNiUGcpFq~XpPWw+J)9~`$?!^sc`^qoWj>m9|K>l>pE2w>!|X=>5}l2%b614yJY zFevj_81`d?Lf$!#$${aBYtQLg{GL!6^w*y(VkY^M|0b`CowjR=^NQ9?|Mt~GdEC1^Bs1Z=~xanhX5L#2(JA|7jJ8=t$8PQtuC0 z?q6dHT*afwxD+U>L49;XWJqv!IB**esqM)B%u+>6mK+*y3e_~N**V~-_*kklF^C8& zXXxRMe8aO`T(@(A&!}@qyVFBSQI;c?eo12Ke<&AFE&lI>|EMJtM_nd?pf`|)Dzwl! zpOp)QRICrf2^ycx62jbKR@Y=VW)>)MFSNST?xntTiU6@atD^$a`ZOyCC>WcP?^fZ# zE-s>edpe@!V6Vt>&!-4R7D$Ml#j0!Xjw9Ebmu@E2o30rwsL)W1kgoZi9wA*}apYC< z{o$=RY?q^8#_O07yXuJLSx{88ecD$#TedVd|XiSR1_Ow?^G7HWsTi^V$*II?&#*>07 zQK`R@Ib{@;vDWT=JdSjml4&D5XvNW`#dhRY&4Rg;oo8RhNcMo=*R?@^1EfDMF?~LQ z6pj^y)26=wVW<`m23BOxun7-u-wuEEd%O12fjU4zR-2?+7zl>1?4BXTt&PC(EO4vw}aosm+6gl8;9EE8G`?{!*%GIon$LB%MY6#$Q?9x#$$Z0l3;_Z+8nj4 zH(IiG$+5l!0&{7v<(nkLm+)}){T$Nj#r4}?#7#;Tg-{3v7XF`RKK|!Gu!qUrCJZ)x zmCx2!{Z`v{-RIRT4oO*;9|hry;%BBGw+tP|7!_HjK`e>d;p3PjpRxG|PsiG%flVW% z=ZI9y$T!y^ungf&&p$QjIj+3ll?OHij zRMFq_UEQKzEiXa!?EHa-GF4gBJX3a@xpD+antgx#_+cCtF&HFg$e+DT(vi4<1I&KYTyhFW_09~c?})z>^SuSX8%@)ERU+I=;`) Date: Thu, 24 Jan 2019 10:24:03 +0100 Subject: [PATCH 43/96] Fixed possible race condition --- .../mdr_rasa/ros/src/mdr_rasa_nlu_wrapper/rasa_nlu_wrapper.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/mdr_speech/mdr_rasa/ros/src/mdr_rasa_nlu_wrapper/rasa_nlu_wrapper.py b/mdr_speech/mdr_rasa/ros/src/mdr_rasa_nlu_wrapper/rasa_nlu_wrapper.py index 29bdea10e..899fc62bd 100644 --- a/mdr_speech/mdr_rasa/ros/src/mdr_rasa_nlu_wrapper/rasa_nlu_wrapper.py +++ b/mdr_speech/mdr_rasa/ros/src/mdr_rasa_nlu_wrapper/rasa_nlu_wrapper.py @@ -25,8 +25,8 @@ def run(self): self.interpreter = Interpreter.load(self.model_path) # Setup topics for incoming/outgoing messages - self.sub = rospy.Subscriber(self.topic_input_text, String, self.process_msg) self.pub = rospy.Publisher(self.topic_output, String, latch=True, queue_size=1) + self.sub = rospy.Subscriber(self.topic_input_text, String, self.process_msg) # Wait until killed rospy.spin() From a06e042b42c63f777c6f3df51f18ebc500619694 Mon Sep 17 00:00:00 2001 From: Alex Mitrevski Date: Thu, 24 Jan 2019 18:02:59 +0100 Subject: [PATCH 44/96] The pickup action is now implemented using a fault-tolerant state machine --- mas-domestic.rosinstall | 5 + .../ros/scripts/pickup_action | 120 ++++++------ .../src/mdr_pickup_action/action_states.py | 171 +++++++++--------- 3 files changed, 151 insertions(+), 145 deletions(-) diff --git a/mas-domestic.rosinstall b/mas-domestic.rosinstall index 43b71acc7..2cb176888 100644 --- a/mas-domestic.rosinstall +++ b/mas-domestic.rosinstall @@ -33,6 +33,11 @@ uri: https://github.com/b-it-bots/action-execution.git version: master +- git: + local-name: ftsm + uri: https://github.com/ropod-project/ftsm.git + version: master + - git: local-name: zbar_ros uri: https://github.com/b-it-bots/zbar_ros.git diff --git a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_pickup_action/ros/scripts/pickup_action b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_pickup_action/ros/scripts/pickup_action index 3e83d3d14..e0da8c631 100755 --- a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_pickup_action/ros/scripts/pickup_action +++ b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_pickup_action/ros/scripts/pickup_action @@ -1,22 +1,41 @@ #!/usr/bin/env python import rospy -import smach - -from smach_ros import ActionServerWrapper, IntrospectionServer +import actionlib from mdr_pickup_action.msg import PickupAction -from mdr_pickup_action.action_states import (SetupPickup, Pickup, SetActionLibResult) +from mdr_pickup_action.action_states import PickupSM +class PickupServer(object): + '''A server exposing a pickup action. -class PickupSkill(smach.StateMachine): - def __init__(self): - smach.StateMachine.__init__(self, - outcomes=['OVERALL_SUCCESS', - 'OVERALL_FAILED', 'PREEMPTED'], - input_keys=['pickup_goal'], - output_keys=['pickup_feedback', - 'pickup_result']) + The server expects the following parameters to be made available on the ROS parameter server: + * move_arm_server: Name of the move_arm action server (default: 'move_arm_server') + * move_base_server: Name of the move_base action server (default: 'move_base_server') + * gripper_controller_pkg_name: The name of a package that implements functionalities for + controlling a robot's gripper (default: 'mdr_gripper_controller') + * pregrasp_config_name: The name of the pregrasp configuration (default: 'pregrasp') + * intermediate_grasp_offset: An optional pose offset that creates an intermediate + trajectory goal following the pregrasp configuration (default: -1) + * safe_arm_joint_config: The name of a configuration in which the robot can + safely move around the environment (default: 'folded') + * base_elbow_offset: An optional offset between base_link and the manipulator's elbow; + used for aligning the base with the object to be grasped + so that the manipulator can easily reach the object (default: -1) + * grasping_dmp: Path to a YAML file containing the weights of a dynamic motion primitive + used for grasping (default: '') + * dmp_tau: The value of the temporal dynamic motion primitive parameter (default: 1) + * grasping_orientation: For more constrained manipulators, it might make sense to use + a fixed grasping orientation (expressed as an (x, y, z, w) quaternion) + to ensure easier reachability (default: [], in which case + the argument is ignored) + * number_of_retries: Number of times a grasp should be repeated in case + it fails the first time (default 0) + @author Alex Mitrevski + @contact aleksandar.mitrevski@h-brs.de + + ''' + def __init__(self): move_arm_server = rospy.get_param('~move_arm_server', 'move_arm_server') move_base_server = rospy.get_param('~move_base_server', 'move_base_server') gripper_controller_pkg_name = rospy.get_param('~gripper_controller_pkg_name', @@ -31,52 +50,41 @@ class PickupSkill(smach.StateMachine): dmp_tau = float(rospy.get_param('~dmp_tau', 1.)) number_of_retries = int(rospy.get_param('~number_of_retries', 0)) - with self: - smach.StateMachine.add('SETUP_PICKUP', SetupPickup(), - transitions={'succeeded': 'PICKUP', - 'failed': 'SETUP_PICKUP'}) + rospy.loginfo('[pickup] Initialising state machine') + self.action_sm = PickupSM(move_arm_server=move_arm_server, + move_base_server=move_base_server, + gripper_controller_pkg_name=gripper_controller_pkg_name, + pregrasp_config_name=pregrasp_config_name, + intermediate_grasp_offset=grasp_offset, + safe_arm_joint_config=safe_arm_joint_config, + base_elbow_offset=base_elbow_offset, + arm_base_offset=arm_base_offset, + grasping_orientation=grasping_orientation, + grasping_dmp=grasping_dmp, + dmp_tau=dmp_tau, + number_of_retries=number_of_retries) + rospy.loginfo('[pickup] State machine initialised') - smach.StateMachine.add('PICKUP', Pickup(move_arm_server=move_arm_server, - move_base_server=move_base_server, - gripper_controller_pkg_name=gripper_controller_pkg_name, - pregrasp_config_name=pregrasp_config_name, - intermediate_grasp_offset=grasp_offset, - safe_arm_joint_config=safe_arm_joint_config, - base_elbow_offset=base_elbow_offset, - arm_base_offset=arm_base_offset, - grasping_orientation=grasping_orientation, - grasping_dmp=grasping_dmp, - dmp_tau=dmp_tau, - number_of_retries=number_of_retries), - transitions={'succeeded': 'SET_ACTION_LIB_SUCCESS', - 'failed': 'SET_ACTION_LIB_FAILED'}) - - smach.StateMachine.add('SET_ACTION_LIB_FAILED', - SetActionLibResult(False), - transitions={'succeeded': 'OVERALL_FAILED'}) - - smach.StateMachine.add('SET_ACTION_LIB_SUCCESS', - SetActionLibResult(True), - transitions={'succeeded': 'OVERALL_SUCCESS'}) + self.action_server = actionlib.SimpleActionServer('pickup_server', + PickupAction, self.execute, False) + self.action_server.start() + rospy.loginfo('pickup action server ready') + def execute(self, goal): + rospy.loginfo('[pickup] Received an action request') + self.action_sm.goal = goal + self.action_sm.execution_requested = True + while self.action_sm.result is None: + rospy.sleep(0.05) + self.action_server.set_succeeded(self.action_sm.result) if __name__ == '__main__': rospy.init_node('pickup_server') - sm = PickupSkill() - sis = IntrospectionServer('pickup_smach_viewer', sm, - '/PICKUP_SMACH_VIEWER') - sis.start() - - asw = ActionServerWrapper( - server_name='pickup_server', - action_spec=PickupAction, - wrapped_container=sm, - succeeded_outcomes=['OVERALL_SUCCESS'], - aborted_outcomes=['OVERALL_FAILED'], - preempted_outcomes=['PREEMPTED'], - goal_key='pickup_goal', - feedback_key='pickup_feedback', - result_key='pickup_result') - - asw.run_server() - rospy.spin() + pickup_server = PickupServer() + try: + pickup_server.action_sm.run() + while pickup_server.action_sm.is_running and not rospy.is_shutdown(): + rospy.spin() + except (KeyboardInterrupt, SystemExit): + print('{0} interrupted; exiting...'.format(pickup_server.action_sm.name)) + pickup_server.action_sm.stop() diff --git a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_pickup_action/ros/src/mdr_pickup_action/action_states.py b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_pickup_action/ros/src/mdr_pickup_action/action_states.py index e245b8075..dff083a8a 100644 --- a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_pickup_action/ros/src/mdr_pickup_action/action_states.py +++ b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_pickup_action/ros/src/mdr_pickup_action/action_states.py @@ -3,34 +3,18 @@ import numpy as np import rospy -import smach import tf import actionlib from geometry_msgs.msg import PoseStamped +from pyftsm.ftsm import FTSMTransitions +from mas_execution.action_sm_base import ActionSMBase from mdr_move_forward_action.msg import MoveForwardAction, MoveForwardGoal from mdr_move_base_action.msg import MoveBaseAction, MoveBaseGoal from mdr_move_arm_action.msg import MoveArmAction, MoveArmGoal from mdr_pickup_action.msg import PickupGoal, PickupFeedback, PickupResult -class SetupPickup(smach.State): - def __init__(self): - smach.State.__init__(self, outcomes=['succeeded', 'failed'], - input_keys=['pickup_goal'], - output_keys=['pickup_feedback', 'pickup_result']) - - def execute(self, userdata): - # consider moving the other components of the robot out of the arm's way, though - # this could be dangerous if the robot is for example close to some furniture item - - feedback = PickupFeedback() - feedback.current_state = 'SETUP_PICKUP' - feedback.message = '[SETUP_PICKUP] setting up the arm' - userdata.pickup_feedback = feedback - - return 'succeeded' - -class Pickup(smach.State): +class PickupSM(ActionSMBase): def __init__(self, timeout=120.0, gripper_controller_pkg_name='mdr_gripper_controller', pregrasp_config_name='pregrasp', @@ -45,10 +29,9 @@ def __init__(self, timeout=120.0, grasping_orientation=list(), grasping_dmp='', dmp_tau=1., - number_of_retries=0): - smach.State.__init__(self, input_keys=['pickup_goal'], - output_keys=['pickup_feedback'], - outcomes=['succeeded', 'failed']) + number_of_retries=0, + max_recovery_attempts=1): + super(PickupSM, self).__init__('Pick', [], max_recovery_attempts) self.timeout = timeout gripper_controller_module_name = '{0}.gripper_controller'.format(gripper_controller_pkg_name) @@ -72,27 +55,41 @@ def __init__(self, timeout=120.0, self.tf_listener = tf.TransformListener() - self.move_arm_client = actionlib.SimpleActionClient(self.move_arm_server, MoveArmAction) - self.move_arm_client.wait_for_server() - - self.move_base_client = actionlib.SimpleActionClient(self.move_base_server, MoveBaseAction) - self.move_base_client.wait_for_server() - - self.move_forward_client = actionlib.SimpleActionClient(self.move_forward_server, MoveForwardAction) - self.move_forward_client.wait_for_server() - - def execute(self, userdata): - feedback = PickupFeedback() - feedback.current_state = 'PICKUP' - feedback.message = '[PICKUP] moving the arm' - userdata.pickup_feedback = feedback - - pose = userdata.pickup_goal.pose + self.move_arm_client = None + self.move_base_client = None + self.move_forward_client = None + + def init(self): + try: + self.move_arm_client = actionlib.SimpleActionClient(self.move_arm_server, MoveArmAction) + rospy.loginfo('[pickup] Waiting for % server', self.move_arm_server) + self.move_arm_client.wait_for_server() + except: + rospy.logerr('[pickup] %s server does not seem to respond', self.move_arm_server) + + try: + self.move_base_client = actionlib.SimpleActionClient(self.move_base_server, MoveBaseAction) + rospy.loginfo('[pickup] Waiting for % server', self.move_base_server) + self.move_base_client.wait_for_server() + except: + rospy.logerr('[pickup] %s server does not seem to respond', self.move_base_server) + + try: + self.move_forward_client = actionlib.SimpleActionClient(self.move_forward_server, MoveForwardAction) + rospy.loginfo('[pickup] Waiting for % server', self.move_forward_server) + self.move_forward_client.wait_for_server() + except: + rospy.logerr('[pickup] %s server does not seem to respond', self.move_forward_server) + + return FTSMTransitions.INITIALISED + + def running(self): + pose = self.goal.pose pose.header.stamp = rospy.Time(0) pose_base_link = self.tf_listener.transformPose('base_link', pose) if self.base_elbow_offset > 0: - self.align_base_with_pose(pose_base_link) + self.__align_base_with_pose(pose_base_link) # the base is now correctly aligned with the pose, so we set the # y position of the goal pose to the elbow offset @@ -108,60 +105,64 @@ def execute(self, userdata): retry_count = 0 while (not grasp_successful) and (retry_count <= self.number_of_retries): if retry_count > 0: - rospy.loginfo('[PICKUP] Retrying grasp') + rospy.loginfo('[pickup] Retrying grasp') - rospy.loginfo('[PICKUP] Opening the gripper...') + rospy.loginfo('[pickup] Opening the gripper...') self.gripper.open() - rospy.loginfo('[PICKUP] Preparing for grasp verification') + rospy.loginfo('[pickup] Preparing for grasp verification') self.gripper.init_grasp_verification() - if userdata.pickup_goal.strategy == PickupGoal.SIDEWAYS_GRASP: - rospy.loginfo('[PICKUP] Preparing sideways graps') - pose_base_link = self.prepare_sideways_grasp(pose_base_link) + if self.goal.strategy == PickupGoal.SIDEWAYS_GRASP: + rospy.loginfo('[pickup] Preparing sideways graps') + pose_base_link = self.__prepare_sideways_grasp(pose_base_link) - rospy.loginfo('[PICKUP] Grasping...') - arm_motion_success = self.move_arm(MoveArmGoal.END_EFFECTOR_POSE, pose_base_link) + rospy.loginfo('[pickup] Grasping...') + arm_motion_success = self.__move_arm(MoveArmGoal.END_EFFECTOR_POSE, pose_base_link) if not arm_motion_success: - rospy.logerr('[PICKUP] Arm motion unsuccessful') - return 'failed' - - rospy.loginfo('[PICKUP] Arm motion successful') - elif userdata.pickup_goal.strategy == PickupGoal.TOP_GRASP: - rospy.loginfo('[PICKUP] Preparing top grasp') - pose_base_link, x_align_distance = self.prepare_top_grasp(pose_base_link) + rospy.logerr('[pickup] Arm motion unsuccessful') + self.result = self.set_result(False) + return FTSMTransitions.DONE + + rospy.loginfo('[pickup] Arm motion successful') + elif self.goal.strategy == PickupGoal.TOP_GRASP: + rospy.loginfo('[pickup] Preparing top grasp') + pose_base_link, x_align_distance = self.__prepare_top_grasp(pose_base_link) self.gripper.orient_z(pose_base_link.pose.orientation) self.gripper.move_down(pose_base_link.pose.position.z) else: - rospy.logerr('[PICKUP] Unknown grasping strategy requested; ignoring request') - return 'failed' + rospy.logerr('[pickup] Unknown grasping strategy requested; ignoring request') + self.result = self.set_result(False) + return FTSMTransitions.DONE - rospy.loginfo('[PICKUP] Closing the gripper') + rospy.loginfo('[pickup] Closing the gripper') self.gripper.close() - rospy.loginfo('[PICKUP] Moving the arm back') - self.move_arm(MoveArmGoal.NAMED_TARGET, self.safe_arm_joint_config) + rospy.loginfo('[pickup] Moving the arm back') + self.__move_arm(MoveArmGoal.NAMED_TARGET, self.safe_arm_joint_config) - if userdata.pickup_goal.strategy == PickupGoal.TOP_GRASP: - rospy.loginfo('[PICKUP] Moving the base back to the original position') + if self.goal.strategy == PickupGoal.TOP_GRASP: + rospy.loginfo('[pickup] Moving the base back to the original position') if abs(x_align_distance) > 0: - self.move_base_along_x(-x_align_distance) + self.__move_base_along_x(-x_align_distance) - rospy.loginfo('[PICKUP] Verifying the grasp...') + rospy.loginfo('[pickup] Verifying the grasp...') grasp_successful = self.gripper.verify_grasp() if grasp_successful: - rospy.loginfo('[PICKUP] Successfully grasped object') + rospy.loginfo('[pickup] Successfully grasped object') else: - rospy.loginfo('[PICKUP] Grasp unsuccessful') + rospy.loginfo('[pickup] Grasp unsuccessful') retry_count += 1 if grasp_successful: - return 'succeeded' + self.result = self.set_result(True) + return FTSMTransitions.DONE - rospy.loginfo('[PICKUP] Grasp could not be performed successfully') - return 'failed' + rospy.loginfo('[pickup] Grasp could not be performed successfully') + self.result = self.set_result(False) + return FTSMTransitions.DONE - def align_base_with_pose(self, pose_base_link): + def __align_base_with_pose(self, pose_base_link): '''Moves the base so that the elbow is aligned with the goal pose. Keyword arguments: @@ -187,7 +188,7 @@ def align_base_with_pose(self, pose_base_link): self.move_base_client.wait_for_result() self.move_base_client.get_result() - def move_arm(self, goal_type, goal): + def __move_arm(self, goal_type, goal): '''Sends a request to the 'move_arm' action server and waits for the results of the action execution. @@ -210,30 +211,30 @@ def move_arm(self, goal_type, goal): result = self.move_arm_client.get_result() return result - def prepare_sideways_grasp(self, pose_base_link): + def __prepare_sideways_grasp(self, pose_base_link): rospy.loginfo('[PICKUP] Moving to a pregrasp configuration...') - self.move_arm(MoveArmGoal.NAMED_TARGET, self.pregrasp_config_name) + self.__move_arm(MoveArmGoal.NAMED_TARGET, self.pregrasp_config_name) if self.intermediate_grasp_offset > 0: rospy.loginfo('[PICKUP] Moving to intermediate grasping pose...') pose_base_link.pose.position.x -= self.intermediate_grasp_offset - self.move_arm(MoveArmGoal.END_EFFECTOR_POSE, pose_base_link) + self.__move_arm(MoveArmGoal.END_EFFECTOR_POSE, pose_base_link) if self.intermediate_grasp_offset > 0: pose_base_link.pose.position.x += self.intermediate_grasp_offset return pose_base_link - def prepare_top_grasp(self, pose_base_link): + def __prepare_top_grasp(self, pose_base_link): rospy.loginfo('[PICKUP] Moving to a pregrasp configuration...') - self.move_arm(MoveArmGoal.NAMED_TARGET, self.pregrasp_top_config_name) + self.__move_arm(MoveArmGoal.NAMED_TARGET, self.pregrasp_top_config_name) x_align_distance = 0 if self.arm_base_offset > 0: x_align_distance = pose_base_link.pose.position.x - self.arm_base_offset - self.move_base_along_x(x_align_distance) + self.__move_base_along_x(x_align_distance) pose_base_link.pose.position.x = self.arm_base_offset return pose_base_link, x_align_distance - def move_base_along_x(self, distance_to_move): + def __move_base_along_x(self, distance_to_move): movement_speed = np.sign(distance_to_move) * 0.1 # m/s movement_duration = distance_to_move / movement_speed move_forward_goal = MoveForwardGoal() @@ -243,15 +244,7 @@ def move_base_along_x(self, distance_to_move): self.move_forward_client.wait_for_result() self.move_forward_client.get_result() -class SetActionLibResult(smach.State): - def __init__(self, result): - smach.State.__init__(self, outcomes=['succeeded'], - input_keys=['pickup_goal'], - output_keys=['pickup_feedback', 'pickup_result']) - self.result = result - - def execute(self, userdata): + def set_result(self, success): result = PickupResult() - result.success = self.result - userdata.pickup_result = result - return 'succeeded' + result.success = success + return result From 88ebc188459b71eaf27150337b606825630ad039 Mon Sep 17 00:00:00 2001 From: Alex Mitrevski Date: Thu, 24 Jan 2019 18:10:03 +0100 Subject: [PATCH 45/96] The place action is now implemented using a fault-tolerant state machine --- .../mdr_place_action/ros/scripts/place_action | 116 +++++++++--------- .../ros/src/mdr_place_action/action_states.py | 103 +++++++--------- 2 files changed, 102 insertions(+), 117 deletions(-) diff --git a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_place_action/ros/scripts/place_action b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_place_action/ros/scripts/place_action index 0c14a80b1..882a743b3 100755 --- a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_place_action/ros/scripts/place_action +++ b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_place_action/ros/scripts/place_action @@ -1,22 +1,38 @@ #!/usr/bin/env python import rospy -import smach - -from smach_ros import ActionServerWrapper, IntrospectionServer +import actionlib from mdr_place_action.msg import PlaceAction -from mdr_place_action.action_states import (SetupPlace, Place, SetActionLibResult) +from mdr_place_action.action_states import PlaceSM +class PlaceServer(object): + '''A server exposing a placing action. -class PlaceSkill(smach.StateMachine): - def __init__(self): - smach.StateMachine.__init__(self, - outcomes=['OVERALL_SUCCESS', - 'OVERALL_FAILED', 'PREEMPTED'], - input_keys=['place_goal'], - output_keys=['place_feedback', - 'place_result']) + The server expects the following parameters to be made available on the ROS parameter server: + * move_arm_server: Name of the move_arm action server (default: 'move_arm_server') + * move_base_server: Name of the move_base action server (default: 'move_base_server') + * gripper_controller_pkg_name: The name of a package that implements functionalities + for controlling a robot's gripper (default: 'mdr_gripper_controller') + * preplace_config_name: Name of the preplace configuration (default: 'pregrasp') + * safe_arm_joint_config: Name of a configuration in which the robot can + safely move around the environment (default: 'folded') + * base_elbow_offset: An optional offset between base_link and the manipulator's elbow; + used for aligning the base with the placing pose so that + the manipulator can easily reach it (default: -1) + * placing_dmp: Path to a YAML file containing the weights of a dynamic + motion primitive used for placing (default: '') + * dmp_tau: The value of the temporal dynamic motion primitive parameter (default: 1) + * placing_orientation: For more constrained manipulators, it might make sense to use + a fixed placing orientation (expressed as an (x, y, z, w) quaternion) + to ensure easier reachability; for instance, we might want to keep + the orientation with which an object was grasped instead of allowing + arbitrary orientations (default: [], in which case the argument is ignored) + + @author Alex Mitrevski + @contact aleksandar.mitrevski@h-brs.de + ''' + def __init__(self): move_arm_server = rospy.get_param('~move_arm_server', 'move_arm_server') move_base_server = rospy.get_param('~move_base_server', 'move_base_server') gripper_controller_pkg_name = rospy.get_param('~gripper_controller_pkg_name', @@ -28,54 +44,38 @@ class PlaceSkill(smach.StateMachine): placing_dmp = rospy.get_param('~placing_dmp', '') dmp_tau = float(rospy.get_param('~dmp_tau', 1.)) - with self: - smach.StateMachine.add('SETUP_PLACE', SetupPlace(), - transitions={'succeeded': 'PLACE', - 'failed': 'SETUP_PLACE'}) - - smach.StateMachine.add('PLACE', Place(move_arm_server=move_arm_server, - move_base_server=move_base_server, - gripper_controller_pkg_name=gripper_controller_pkg_name, - preplace_config_name=preplace_config_name, - safe_arm_joint_config=safe_arm_joint_config, - base_elbow_offset=base_elbow_offset, - placing_orientation=placing_orientation, - placing_dmp=placing_dmp, - dmp_tau=dmp_tau), - transitions={'succeeded': 'SET_ACTION_LIB_SUCCESS', - 'failed': 'SET_ACTION_LIB_FAILED'}) + rospy.loginfo('[place] Initialising state machine') + self.action_sm = PlaceSM(move_arm_server=move_arm_server, + move_base_server=move_base_server, + gripper_controller_pkg_name=gripper_controller_pkg_name, + preplace_config_name=preplace_config_name, + safe_arm_joint_config=safe_arm_joint_config, + base_elbow_offset=base_elbow_offset, + placing_orientation=placing_orientation, + placing_dmp=placing_dmp, + dmp_tau=dmp_tau) + rospy.loginfo('[place] State machine initialised') - smach.StateMachine.add('SET_ACTION_LIB_FAILED', - SetActionLibResult(False), - transitions={'succeeded': 'OVERALL_FAILED'}) - - smach.StateMachine.add('SET_ACTION_LIB_SUCCESS', - SetActionLibResult(True), - transitions={'succeeded': 'OVERALL_SUCCESS'}) + self.action_server = actionlib.SimpleActionServer('place_server', + PlaceAction, self.execute, False) + self.action_server.start() + rospy.loginfo('place action server ready') + def execute(self, goal): + rospy.loginfo('[place] Received an action request') + self.action_sm.goal = goal + self.action_sm.execution_requested = True + while self.action_sm.result is None: + rospy.sleep(0.05) + self.action_server.set_succeeded(self.action_sm.result) if __name__ == '__main__': rospy.init_node('place_server') - - # construct state machine - sm = PlaceSkill() - - # smach viewer - sis = IntrospectionServer('place_smach_viewer', sm, - '/PLACE_SMACH_VIEWER') - sis.start() - - asw = ActionServerWrapper( - server_name='place_server', - action_spec=PlaceAction, - wrapped_container=sm, - succeeded_outcomes=['OVERALL_SUCCESS'], - aborted_outcomes=['OVERALL_FAILED'], - preempted_outcomes=['PREEMPTED'], - goal_key='place_goal', - feedback_key='place_feedback', - result_key='place_result') - - # Run the server in a background thread - asw.run_server() - rospy.spin() + place_server = PlaceServer() + try: + place_server.action_sm.run() + while place_server.action_sm.is_running and not rospy.is_shutdown(): + rospy.spin() + except (KeyboardInterrupt, SystemExit): + print('{0} interrupted; exiting...'.format(place_server.action_sm.name)) + place_server.action_sm.stop() diff --git a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_place_action/ros/src/mdr_place_action/action_states.py b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_place_action/ros/src/mdr_place_action/action_states.py index eff8c71bc..b89c8ee40 100644 --- a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_place_action/ros/src/mdr_place_action/action_states.py +++ b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_place_action/ros/src/mdr_place_action/action_states.py @@ -2,33 +2,17 @@ from importlib import import_module import rospy -import smach import tf import actionlib from geometry_msgs.msg import PoseStamped +from pyftsm.ftsm import FTSMTransitions +from mas_execution.action_sm_base import ActionSMBase from mdr_move_base_action.msg import MoveBaseAction, MoveBaseGoal from mdr_move_arm_action.msg import MoveArmAction, MoveArmGoal from mdr_place_action.msg import PlaceGoal, PlaceFeedback, PlaceResult -class SetupPlace(smach.State): - def __init__(self): - smach.State.__init__(self, outcomes=['succeeded', 'failed'], - input_keys=['place_goal'], - output_keys=['place_feedback', 'place_result']) - - def execute(self, userdata): - # consider moving the other components of the robot out of the arm's way, though - # this could be dangerous if the robot is for example close to some furniture item - - feedback = PlaceFeedback() - feedback.current_state = 'SETUP_PLACE' - feedback.message = '[SETUP_PLACE] setting up the arm' - userdata.place_feedback = feedback - - return 'succeeded' - -class Place(smach.State): +class PlaceSM(ActionSMBase): def __init__(self, timeout=120.0, gripper_controller_pkg_name='mdr_gripper_controller', preplace_config_name='pregrasp', @@ -38,10 +22,9 @@ def __init__(self, timeout=120.0, base_elbow_offset=-1., placing_orientation=list(), placing_dmp='', - dmp_tau=1.): - smach.State.__init__(self, input_keys=['place_goal'], - output_keys=['place_feedback'], - outcomes=['succeeded', 'failed']) + dmp_tau=1., + max_recovery_attempts=1): + super(PlaceSM, self).__init__('Place', [], max_recovery_attempts) self.timeout = timeout gripper_controller_module_name = '{0}.gripper_controller'.format(gripper_controller_pkg_name) @@ -60,19 +43,28 @@ def __init__(self, timeout=120.0, self.tf_listener = tf.TransformListener() - self.move_arm_client = actionlib.SimpleActionClient(self.move_arm_server, MoveArmAction) - self.move_arm_client.wait_for_server() + self.move_arm_client = None + self.move_base_client = None - self.move_base_client = actionlib.SimpleActionClient(self.move_base_server, MoveBaseAction) - self.move_base_client.wait_for_server() + def init(self): + try: + self.move_arm_client = actionlib.SimpleActionClient(self.move_arm_server, MoveArmAction) + rospy.loginfo('[place] Waiting for % server', self.move_arm_server) + self.move_arm_client.wait_for_server() + except: + rospy.logerr('[place] %s server does not seem to respond', self.move_arm_server) - def execute(self, userdata): - feedback = PlaceFeedback() - feedback.current_state = 'PLACE' - feedback.message = '[PLACE] moving the arm' - userdata.place_feedback = feedback + try: + self.move_base_client = actionlib.SimpleActionClient(self.move_base_server, MoveBaseAction) + rospy.loginfo('[place] Waiting for % server', self.move_base_server) + self.move_base_client.wait_for_server() + except: + rospy.logerr('[place] %s server does not seem to respond', self.move_base_server) - pose = userdata.place_goal.pose + return FTSMTransitions.INITIALISED + + def running(self): + pose = self.goal.pose pose.header.stamp = rospy.Time(0) pose_base_link = self.tf_listener.transformPose('base_link', pose) if self.placing_orientation: @@ -82,32 +74,33 @@ def execute(self, userdata): pose_base_link.pose.orientation.w = self.placing_orientation[3] if self.base_elbow_offset > 0: - self.align_base_with_pose(pose_base_link) + self.__align_base_with_pose(pose_base_link) # the base is now correctly aligned with the pose, so we set the # y position of the goal pose to the elbow offset pose_base_link.pose.position.y = self.base_elbow_offset - rospy.loginfo('[PLACE] Moving to a preplace configuration...') - self.move_arm(MoveArmGoal.NAMED_TARGET, self.preplace_config_name) + rospy.loginfo('[place] Moving to a preplace configuration...') + self.__move_arm(MoveArmGoal.NAMED_TARGET, self.preplace_config_name) # we set up the arm group for moving - rospy.loginfo('[PLACE] Placing...') - success = self.move_arm(MoveArmGoal.END_EFFECTOR_POSE, pose_base_link) + rospy.loginfo('[place] Placing...') + success = self.__move_arm(MoveArmGoal.END_EFFECTOR_POSE, pose_base_link) if not success: - rospy.logerr('[PLACE] Arm motion unsuccessful') - return 'failed' + rospy.logerr('[place] Arm motion unsuccessful') + self.result = self.set_result(False) + return FTSMTransitions.DONE - rospy.loginfo('[PLACE] Arm motion successful') - rospy.loginfo('[PLACE] Opening the gripper...') + rospy.loginfo('[place] Arm motion successful') + rospy.loginfo('[place] Opening the gripper...') self.gripper.open() - rospy.loginfo('[PLACE] Moving the arm back') - self.move_arm(MoveArmGoal.NAMED_TARGET, self.safe_arm_joint_config) - - return 'succeeded' + rospy.loginfo('[place] Moving the arm back') + self.__move_arm(MoveArmGoal.NAMED_TARGET, self.safe_arm_joint_config) + self.result = self.set_result(True) + return FTSMTransitions.DONE - def align_base_with_pose(self, pose_base_link): + def __align_base_with_pose(self, pose_base_link): '''Moves the base so that the elbow is aligned with the goal pose. Keyword arguments: @@ -133,7 +126,7 @@ def align_base_with_pose(self, pose_base_link): self.move_base_client.wait_for_result() self.move_base_client.get_result() - def move_arm(self, goal_type, goal): + def __move_arm(self, goal_type, goal): '''Sends a request to the 'move_arm' action server and waits for the results of the action execution. @@ -156,15 +149,7 @@ def move_arm(self, goal_type, goal): result = self.move_arm_client.get_result() return result -class SetActionLibResult(smach.State): - def __init__(self, result): - smach.State.__init__(self, outcomes=['succeeded'], - input_keys=['place_goal'], - output_keys=['place_feedback', 'place_result']) - self.result = result - - def execute(self, userdata): + def set_result(self, success): result = PlaceResult() - result.success = self.result - userdata.place_result = result - return 'succeeded' + result.success = success + return result From d038e9b9810b6fc2b7838907d1fab4c124b88054 Mon Sep 17 00:00:00 2001 From: Alex Mitrevski Date: Thu, 24 Jan 2019 18:33:32 +0100 Subject: [PATCH 46/96] The move base action is now implemented using a fault-tolerant state machine --- .../ros/scripts/move_arm_action | 85 ++++++++----------- .../src/mdr_move_arm_action/action_states.py | 80 +++++++---------- 2 files changed, 67 insertions(+), 98 deletions(-) diff --git a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_move_arm_action/ros/scripts/move_arm_action b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_move_arm_action/ros/scripts/move_arm_action index 125a24dbe..1c6c7c746 100755 --- a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_move_arm_action/ros/scripts/move_arm_action +++ b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_move_arm_action/ros/scripts/move_arm_action @@ -1,65 +1,48 @@ #!/usr/bin/env python import rospy -import smach - -from smach_ros import ActionServerWrapper, IntrospectionServer +import actionlib from mdr_move_arm_action.msg import MoveArmAction -from mdr_move_arm_action.action_states import (SetupMoveArm, MoveArm, - SetActionLibResult) - +from mdr_move_arm_action.action_states import MoveArmSM -class MoveArmSkill(smach.StateMachine): - def __init__(self, timeout=10): - smach.StateMachine.__init__(self, - outcomes=['OVERALL_SUCCESS', - 'OVERALL_FAILED', 'PREEMPTED'], - input_keys=['move_arm_goal'], - output_keys=['move_arm_feedback', - 'move_arm_result']) +class MoveArmServer(object): + '''A server exposing an arm moving action. - arm_name = rospy.get_param('~arm_name', 'arm') + The server expects the following parameters to be made available on the ROS parameter server: + * arm_name: Name of an arm MoveIt! group - with self: - smach.StateMachine.add('SETUP_MOVE_ARM', SetupMoveArm(), - transitions={'succeeded': 'MOVE_ARM', - 'failed': 'SETUP_MOVE_ARM'}) + @author Alex Mitrevski + @contact aleksandar.mitrevski@h-brs.de - smach.StateMachine.add('MOVE_ARM', MoveArm(arm_name=arm_name), - transitions={'succeeded': 'SET_ACTION_LIB_SUCCESS', - 'failed': 'SET_ACTION_LIB_FAILED'}) + ''' + def __init__(self): + arm_name = rospy.get_param('~arm_name', 'arm') - smach.StateMachine.add('SET_ACTION_LIB_FAILED', - SetActionLibResult(False), - transitions={'succeeded': 'OVERALL_FAILED'}) + rospy.loginfo('[move_arm] Initialising state machine') + self.action_sm = MoveArmSM(arm_name=arm_name) + rospy.loginfo('[move_arm] State machine initialised') - smach.StateMachine.add('SET_ACTION_LIB_SUCCESS', - SetActionLibResult(True), - transitions={'succeeded': 'OVERALL_SUCCESS'}) + self.action_server = actionlib.SimpleActionServer('move_arm_server', + MoveArmAction, + self.execute, False) + self.action_server.start() + rospy.loginfo('move_arm action server ready') + def execute(self, goal): + rospy.loginfo('[move_arm] Received an action request') + self.action_sm.goal = goal + self.action_sm.execution_requested = True + while self.action_sm.result is None: + rospy.sleep(0.05) + self.action_server.set_succeeded(self.action_sm.result) if __name__ == '__main__': rospy.init_node('move_arm_server') - - # construct state machine - sm = MoveArmSkill() - - # smach viewer - sis = IntrospectionServer('move_arm_smach_viewer', sm, - '/MOVE_ARM_SMACH_VIEWER') - sis.start() - - asw = ActionServerWrapper( - server_name='move_arm_server', - action_spec=MoveArmAction, - wrapped_container=sm, - succeeded_outcomes=['OVERALL_SUCCESS'], - aborted_outcomes=['OVERALL_FAILED'], - preempted_outcomes=['PREEMPTED'], - goal_key='move_arm_goal', - feedback_key='move_arm_feedback', - result_key='move_arm_result') - - # Run the server in a background thread - asw.run_server() - rospy.spin() + move_arm_server = MoveArmServer() + try: + move_arm_server.action_sm.run() + while move_arm_server.action_sm.is_running and not rospy.is_shutdown(): + rospy.spin() + except (KeyboardInterrupt, SystemExit): + print('{0} interrupted; exiting...'.format(move_arm_server.action_sm.name)) + move_arm_server.action_sm.stop() diff --git a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_move_arm_action/ros/src/mdr_move_arm_action/action_states.py b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_move_arm_action/ros/src/mdr_move_arm_action/action_states.py index e35cfb9e9..ecb50344d 100644 --- a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_move_arm_action/ros/src/mdr_move_arm_action/action_states.py +++ b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_move_arm_action/ros/src/mdr_move_arm_action/action_states.py @@ -2,48 +2,38 @@ import numpy as np import rospy -import smach import moveit_commander +from pyftsm.ftsm import FTSMTransitions +from mas_execution.action_sm_base import ActionSMBase from mdr_move_arm_action.msg import MoveArmGoal, MoveArmFeedback, MoveArmResult from mdr_move_arm_action.dmp import DMPExecutor -class SetupMoveArm(smach.State): - def __init__(self): - smach.State.__init__(self, outcomes=['succeeded', 'failed'], - input_keys=['move_arm_goal'], - output_keys=['move_arm_feedback', 'move_arm_result']) - - def execute(self, userdata): - # consider moving the other components of the robot out of the arm's way, though - # this could be dangerous if the robot is for example close to some furniture item - - feedback = MoveArmFeedback() - feedback.current_state = 'MOVE_ARM' - feedback.message = '[move_arm] moving the arm' - userdata.move_arm_feedback = feedback - - return 'succeeded' - -class MoveArm(smach.State): - def __init__(self, timeout=120.0, arm_name='arm'): - smach.State.__init__(self, input_keys=['move_arm_goal'], - outcomes=['succeeded', 'failed']) +class MoveArmSM(ActionSMBase): + def __init__(self, timeout=120.0, arm_name='arm', max_recovery_attempts=1): + super(MoveArmSM, self).__init__('MoveArm', [], max_recovery_attempts) self.timeout = timeout - self.arm = moveit_commander.MoveGroupCommander(arm_name) + self.arm_name = arm_name + self.arm = None - def execute(self, userdata): + def init(self): + try: + self.arm = moveit_commander.MoveGroupCommander(self.arm_name) + except: + rospy.logerr('[move_arm] %s could not be initialised', self.arm_name) + return FTSMTransitions.INITIALISED + + def running(self): self.arm.clear_pose_targets() success = False - if userdata.move_arm_goal.goal_type == MoveArmGoal.NAMED_TARGET: - self.arm.set_named_target(userdata.move_arm_goal.named_target) + if self.goal.goal_type == MoveArmGoal.NAMED_TARGET: + self.arm.set_named_target(self.goal.named_target) rospy.loginfo('[move_arm] Planning motion and trying to move arm...') success = self.arm.go(wait=True) - elif userdata.move_arm_goal.goal_type == MoveArmGoal.END_EFFECTOR_POSE: - pose = userdata.move_arm_goal.end_effector_pose - - dmp_name = userdata.move_arm_goal.dmp_name - tau = userdata.move_arm_goal.dmp_tau + elif self.goal.goal_type == MoveArmGoal.END_EFFECTOR_POSE: + pose = self.goal.end_effector_pose + dmp_name = self.goal.dmp_name + tau = self.goal.dmp_tau rospy.loginfo('[move_arm] Planning motion and trying to move arm...') # we use a dynamic motion primitive for moving the arm if one is specified; @@ -56,30 +46,26 @@ def execute(self, userdata): self.arm.set_pose_reference_frame(pose.header.frame_id) self.arm.set_pose_target(pose.pose) success = self.arm.go(wait=True) - elif userdata.move_arm_goal.goal_type == MoveArmGoal.JOINT_VALUES: - joint_values = userdata.move_arm_goal.joint_values + elif self.goal.goal_type == MoveArmGoal.JOINT_VALUES: + joint_values = self.goal.joint_values self.arm.set_joint_value_target(joint_values) rospy.loginfo('[move_arm] Planning motion and trying to move arm...') success = self.arm.go(wait=True) else: rospy.logerr('[move_arm] Invalid target specified; ignoring request') - return 'failed' + self.result = self.set_result(False) + return FTSMTransitions.DONE if not success: rospy.logerr('[move_arm] Arm motion unsuccessful') - return 'failed' - rospy.loginfo('[move_arm] Arm motion successful') - return 'succeeded' + self.result = self.set_result(False) + return FTSMTransitions.DONE -class SetActionLibResult(smach.State): - def __init__(self, result): - smach.State.__init__(self, outcomes=['succeeded'], - input_keys=['move_arm_goal'], - output_keys=['move_arm_feedback', 'move_arm_result']) - self.result = result + rospy.loginfo('[move_arm] Arm motion successful') + self.result = self.set_result(True) + return FTSMTransitions.DONE - def execute(self, userdata): + def set_result(self, success): result = MoveArmResult() - result.success = self.result - userdata.move_arm_result = result - return 'succeeded' + result.success = success + return result From 59550521420a7bf833c5eee79773d2d44a7a0280 Mon Sep 17 00:00:00 2001 From: Alex Mitrevski Date: Thu, 24 Jan 2019 22:37:24 +0100 Subject: [PATCH 47/96] The navigation actions are now implemented using a fault-tolerant state machine --- .../ros/scripts/enter_door_action | 93 +++++++------- .../mdr_enter_door_action/action_states.py | 102 +++++++--------- .../ros/scripts/move_base_action | 112 +++++++++-------- .../src/mdr_move_base_action/action_states.py | 113 ++++++++---------- .../ros/scripts/move_forward_action | 83 ++++++------- .../mdr_move_forward_action/action_states.py | 59 +++------ .../ros/scripts/turn_base_to_action | 88 +++++++------- .../mdr_turn_base_to_action/action_states.py | 71 +++++------ 8 files changed, 305 insertions(+), 416 deletions(-) diff --git a/mdr_planning/mdr_actions/mdr_navigation_actions/mdr_enter_door_action/ros/scripts/enter_door_action b/mdr_planning/mdr_actions/mdr_navigation_actions/mdr_enter_door_action/ros/scripts/enter_door_action index 2ca7a3525..a367bb3b3 100755 --- a/mdr_planning/mdr_actions/mdr_navigation_actions/mdr_enter_door_action/ros/scripts/enter_door_action +++ b/mdr_planning/mdr_actions/mdr_navigation_actions/mdr_enter_door_action/ros/scripts/enter_door_action @@ -1,65 +1,54 @@ #!/usr/bin/env python import rospy -import smach - -from smach_ros import ActionServerWrapper, IntrospectionServer +import actionlib from mdr_enter_door_action.msg import EnterDoorAction -from mdr_enter_door_action.action_states import SetupEnterDoor, WaitForDoor, EnterDoor, SetActionLibResult +from mdr_enter_door_action.action_states import EnterDoorSM + +class EnterDoorServer(object): + '''A server exposing an action in which a robot waits until a door opens and then enters inside. + + The server expects the following parameters to be made available on the ROS parameter server: + * movement_duration: Time (in seconds) for which the robot should move + * speed: Movement speed + * move_forward_server: Name of a server exposing an action for moving a base forward -class EnterDoorSkill(smach.StateMachine): - def __init__(self, timeout=10): - smach.StateMachine.__init__(self, - outcomes=['OVERALL_SUCCESS', 'OVERALL_FAILED', 'PREEMPTED'], - input_keys=['enter_door_goal'], - output_keys=['enter_door_feedback', 'enter_door_result']) + @author Alex Mitrevski + @contact aleksandar.mitrevski@h-brs.de + ''' + def __init__(self): movement_duration = float(rospy.get_param('~movement_duration', 15.)) speed = float(rospy.get_param('~speed', 0.15)) move_forward_server = rospy.get_param('~move_forward_server', 'move_forward_server') - with self: - smach.StateMachine.add('SETUP_ENTER_DOOR', SetupEnterDoor(), - transitions={'succeeded': 'WAIT_FOR_DOOR', - 'failed': 'SETUP_ENTER_DOOR'}) - - smach.StateMachine.add('WAIT_FOR_DOOR', WaitForDoor(), - transitions={'succeeded': 'ENTER_DOOR', - 'waiting': 'WAIT_FOR_DOOR'}) - - smach.StateMachine.add('ENTER_DOOR', EnterDoor(move_forward_server=move_forward_server, - movement_duration=movement_duration, - speed=speed), - transitions={'succeeded': 'SET_ACTION_LIB_SUCCESS', - 'failed': 'SET_ACTION_LIB_FAILED'}) - - smach.StateMachine.add('SET_ACTION_LIB_FAILED', SetActionLibResult(False), - transitions={'succeeded': 'OVERALL_FAILED'}) - - smach.StateMachine.add('SET_ACTION_LIB_SUCCESS', SetActionLibResult(True), - transitions={'succeeded': 'OVERALL_SUCCESS'}) + rospy.loginfo('[enter_door] Initialising state machine') + self.action_sm = EnterDoorSM(move_forward_server=move_forward_server, + movement_duration=movement_duration, + speed=speed) + rospy.loginfo('[enter_door] State machine initialised') + + self.action_server = actionlib.SimpleActionServer('enter_door_server', + EnterDoorAction, + self.execute, False) + self.action_server.start() + rospy.loginfo('enter_door action server ready') + + def execute(self, goal): + rospy.loginfo('[enter_door] Received an action request') + self.action_sm.goal = goal + self.action_sm.execution_requested = True + while self.action_sm.result is None: + rospy.sleep(0.05) + self.action_server.set_succeeded(self.action_sm.result) if __name__ == '__main__': rospy.init_node('enter_door_server') - - # construct state machine - sm = EnterDoorSkill() - - # smach viewer - sis = IntrospectionServer('enter_door_smach_viewer', sm, '/enter_door_SMACH_VIEWER') - sis.start() - - asw = ActionServerWrapper( - server_name='enter_door_server', - action_spec=EnterDoorAction, - wrapped_container=sm, - succeeded_outcomes=['OVERALL_SUCCESS'], - aborted_outcomes=['OVERALL_FAILED'], - preempted_outcomes=['PREEMPTED'], - goal_key='enter_door_goal', - feedback_key='enter_door_feedback', - result_key='enter_door_result') - - # Run the server in a background thread - asw.run_server() - rospy.spin() + enter_door_server = EnterDoorServer() + try: + enter_door_server.action_sm.run() + while enter_door_server.action_sm.is_running and not rospy.is_shutdown(): + rospy.spin() + except (KeyboardInterrupt, SystemExit): + print('{0} interrupted; exiting...'.format(enter_door_server.action_sm.name)) + enter_door_server.action_sm.stop() diff --git a/mdr_planning/mdr_actions/mdr_navigation_actions/mdr_enter_door_action/ros/src/mdr_enter_door_action/action_states.py b/mdr_planning/mdr_actions/mdr_navigation_actions/mdr_enter_door_action/ros/src/mdr_enter_door_action/action_states.py index f76a46336..d0697b618 100644 --- a/mdr_planning/mdr_actions/mdr_navigation_actions/mdr_enter_door_action/ros/src/mdr_enter_door_action/action_states.py +++ b/mdr_planning/mdr_actions/mdr_navigation_actions/mdr_enter_door_action/ros/src/mdr_enter_door_action/action_states.py @@ -1,66 +1,48 @@ #!/usr/bin/python - import rospy -import smach -from actionlib import SimpleActionClient - +import actionlib from std_msgs.msg import Bool -from mdr_enter_door_action.msg import EnterDoorFeedback, EnterDoorResult -from mdr_move_forward_action.msg import MoveForwardAction, MoveForwardGoal - -class SetupEnterDoor(smach.State): - def __init__(self): - smach.State.__init__(self, outcomes=['succeeded', 'failed'], - output_keys=['enter_door_feedback']) - def execute(self, userdata): - feedback = EnterDoorFeedback() - feedback.current_state = 'setup_enter_door' - feedback.message = '[enter_door] entering door' - userdata.enter_door_feedback = feedback +from pyftsm.ftsm import FTSMTransitions +from mas_execution.action_sm_base import ActionSMBase +from mdr_move_forward_action.msg import MoveForwardAction, MoveForwardGoal +from mdr_enter_door_action.msg import EnterDoorFeedback, EnterDoorResult - return 'succeeded' +class EnterDoorSM(ActionSMBase): + def __init__(self, waiting_sleep_duration=1., + door_status_topic='/mcr_perception/door_status/door_status', + timeout=120.0, + move_forward_server='move_forward_server', + movement_duration=10., speed=0.1, + max_recovery_attempts=1): + super(EnterDoorSM, self).__init__('EnterDoor', [], max_recovery_attempts) -class WaitForDoor(smach.State): - def __init__(self, sleep_duration=1., - door_status_topic='/mcr_perception/door_status/door_status'): - smach.State.__init__(self, outcomes=['succeeded', 'waiting'], - output_keys=['enter_door_feedback']) - self.sleep_duration = sleep_duration - self.door_open = False + self.waiting_sleep_duration = waiting_sleep_duration self.door_status_sub = rospy.Subscriber(door_status_topic, Bool, self.update_door_status) - - def execute(self, userdata): - feedback = EnterDoorFeedback() - feedback.current_state = 'wait_for_door' - feedback.message = '[enter_door] waiting for door to open' - userdata.enter_door_feedback = feedback - - rospy.sleep(self.sleep_duration) - - if self.door_open: - return 'succeeded' - return 'waiting' - - def update_door_status(self, msg): - self.door_open = msg.data - -class EnterDoor(smach.State): - def __init__(self, timeout=120.0, - move_forward_server='move_forward_server', - movement_duration=10., speed=0.1): - smach.State.__init__(self, outcomes=['succeeded', 'failed']) self.timeout = timeout self.movement_duration = movement_duration self.speed = speed - self.entered = False + self.move_forward_server = move_forward_server - self.move_forward_client = SimpleActionClient(move_forward_server, - MoveForwardAction) - self.move_forward_client.wait_for_server() + self.door_open = False + self.move_forward_client = None + + def init(self): + try: + self.move_forward_client = actionlib.SimpleActionClient(self.move_forward_server, MoveForwardAction) + rospy.loginfo('[enter_door] Waiting for % server', self.move_forward_server) + self.move_forward_client.wait_for_server() + except: + rospy.logerr('[enter_door] %s server does not seem to respond', self.move_forward_server) + return FTSMTransitions.INITIALISED + + def running(self): + rospy.loginfo('[enter_door] Waiting for door to open') + while not self.door_open: + rospy.sleep(self.waiting_sleep_duration) + rospy.loginfo('[enter_door] Door open; entering') - def execute(self, userdata): goal = MoveForwardGoal() goal.movement_duration = self.movement_duration goal.speed = self.speed @@ -68,20 +50,16 @@ def execute(self, userdata): self.move_forward_client.send_goal(goal) timeout_duration = rospy.Duration.from_sec(self.timeout) self.move_forward_client.wait_for_result(timeout_duration) - result = self.move_forward_client.get_result() if result and result.success: - return 'succeeded' - return 'failed' + self.result = self.set_result(True) + self.result = self.set_result(False) + return FTSMTransitions.DONE -class SetActionLibResult(smach.State): - def __init__(self, result): - smach.State.__init__(self, outcomes=['succeeded'], - output_keys=['enter_door_result']) - self.result = result + def update_door_status(self, msg): + self.door_open = msg.data - def execute(self, userdata): + def set_result(self, success): result = EnterDoorResult() - result.success = self.result - userdata.enter_door_result = result - return 'succeeded' + result.success = success + return result diff --git a/mdr_planning/mdr_actions/mdr_navigation_actions/mdr_move_base_action/ros/scripts/move_base_action b/mdr_planning/mdr_actions/mdr_navigation_actions/mdr_move_base_action/ros/scripts/move_base_action index f31be9ea1..c602e1f0e 100755 --- a/mdr_planning/mdr_actions/mdr_navigation_actions/mdr_move_base_action/ros/scripts/move_base_action +++ b/mdr_planning/mdr_actions/mdr_navigation_actions/mdr_move_base_action/ros/scripts/move_base_action @@ -1,57 +1,63 @@ #!/usr/bin/env python import rospy -import smach - -import smach_ros -from mdr_move_base_action.msg import MoveBaseAction, MoveBaseResult -from mdr_move_base_action.action_states import SetupMoveBase, ApproachPose, SetActionLibResult - -def main(): - rospy.init_node('mdr_move_base_action_server') - - move_base_server = rospy.get_param('~move_base_server', '') - pose_description_file = rospy.get_param('~pose_description_file', '') - pose_frame = rospy.get_param('~pose_frame', '') - safe_arm_joint_config = rospy.get_param('~safe_arm_joint_config', 'folded') - move_arm_server = rospy.get_param('~move_arm_server', 'move_arm_server') - - sm = smach.StateMachine(outcomes=['OVERALL_SUCCESS', 'OVERALL_FAILED'], - input_keys=['move_base_goal'], - output_keys=['move_base_feedback', 'move_base_result']) - - with sm: - smach.StateMachine.add('SETUP_MOVE_BASE', - SetupMoveBase(safe_arm_joint_config=safe_arm_joint_config, - move_arm_server=move_arm_server), - transitions={'succeeded': 'MOVE_BASE_TO_DESTINATION', - 'failed': 'SETUP_MOVE_BASE'}) - - smach.StateMachine.add('MOVE_BASE_TO_DESTINATION', - ApproachPose(move_base_server=move_base_server, - pose_description_file=pose_description_file, - pose_frame=pose_frame), - transitions={'succeeded': 'SET_ACTION_LIB_SUCCESS', - 'failed': 'SET_ACTION_LIB_FAILED'}) - - smach.StateMachine.add('SET_ACTION_LIB_FAILED', SetActionLibResult(False), - transitions={'succeeded': 'OVERALL_FAILED'}) - - smach.StateMachine.add('SET_ACTION_LIB_SUCCESS', SetActionLibResult(True), - transitions={'succeeded': 'OVERALL_SUCCESS'}) - - asw = smach_ros.ActionServerWrapper( - server_name='move_base_server', - action_spec=MoveBaseAction, - wrapped_container=sm, - succeeded_outcomes=['OVERALL_SUCCESS'], - aborted_outcomes=['OVERALL_FAILED'], - preempted_outcomes=['PREEMPTED'], - goal_key='move_base_goal', - feedback_key='move_base_feedback', - result_key='move_base_result') - - asw.run_server() - rospy.spin() +import actionlib + +from mdr_move_base_action.msg import MoveBaseAction +from mdr_move_base_action.action_states import MoveBaseSM + +class MoveBaseServer(object): + '''A server exposing a go to action. + + The server expects the following parameters to be made available on the ROS parameter server: + * move_base_server: Name of the default move_base server (default: '/move_base') + * pose_description_file: Name of a yaml file in which named goals are mapped + to actual coordinates + * pose_frame: Name of the frame in which the poses in pose_description_file + are given (default: 'map') + * safe_arm_joint_config: Name of a configuration specified in which the robot can + safely move around the environment (default: 'folded') + * move_arm_server: Name of the move_arm action server (default: 'move_arm_server') + + @author Alex Mitrevski + @contact aleksandar.mitrevski@h-brs.de + + ''' + def __init__(self): + safe_arm_joint_config = rospy.get_param('~safe_arm_joint_config', 'folded') + move_arm_server = rospy.get_param('~move_arm_server', 'move_arm_server') + move_base_server = rospy.get_param('~move_base_server', '') + pose_description_file = rospy.get_param('~pose_description_file', '') + pose_frame = rospy.get_param('~pose_frame', '') + + rospy.loginfo('[move_base] Initialising state machine') + self.action_sm = MoveBaseSM(safe_arm_joint_config=safe_arm_joint_config, + move_arm_server=move_arm_server, + move_base_server=move_base_server, + pose_description_file=pose_description_file, + pose_frame=pose_frame) + rospy.loginfo('[move_base] State machine initialised') + + self.action_server = actionlib.SimpleActionServer('move_base_server', + MoveBaseAction, + self.execute, False) + self.action_server.start() + rospy.loginfo('move_base action server ready') + + def execute(self, goal): + rospy.loginfo('[move_base] Received an action request') + self.action_sm.goal = goal + self.action_sm.execution_requested = True + while self.action_sm.result is None: + rospy.sleep(0.05) + self.action_server.set_succeeded(self.action_sm.result) if __name__ == '__main__': - main() + rospy.init_node('move_base_server') + move_base_server = MoveBaseServer() + try: + move_base_server.action_sm.run() + while move_base_server.action_sm.is_running and not rospy.is_shutdown(): + rospy.spin() + except (KeyboardInterrupt, SystemExit): + print('{0} interrupted; exiting...'.format(move_base_server.action_sm.name)) + move_base_server.action_sm.stop() diff --git a/mdr_planning/mdr_actions/mdr_navigation_actions/mdr_move_base_action/ros/src/mdr_move_base_action/action_states.py b/mdr_planning/mdr_actions/mdr_navigation_actions/mdr_move_base_action/ros/src/mdr_move_base_action/action_states.py index fe7851287..20affaa20 100644 --- a/mdr_planning/mdr_actions/mdr_navigation_actions/mdr_move_base_action/ros/src/mdr_move_base_action/action_states.py +++ b/mdr_planning/mdr_actions/mdr_navigation_actions/mdr_move_base_action/ros/src/mdr_move_base_action/action_states.py @@ -1,63 +1,54 @@ -#!/usr/bin/python - +import yaml import rospy -import smach import actionlib -import yaml import tf from geometry_msgs.msg import PoseStamped, Quaternion import move_base_msgs.msg as move_base_msgs +from pyftsm.ftsm import FTSMTransitions +from mas_execution.action_sm_base import ActionSMBase from mdr_move_arm_action.msg import MoveArmAction, MoveArmGoal from mdr_move_base_action.msg import MoveBaseGoal, MoveBaseFeedback, MoveBaseResult -class SetupMoveBase(smach.State): - def __init__(self, safe_arm_joint_config='folded', move_arm_server='move_arm_server'): - smach.State.__init__(self, outcomes=['succeeded', 'failed'], - input_keys=['move_base_goal'], - output_keys=['move_base_feedback', 'move_base_result']) +class MoveBaseSM(ActionSMBase): + def __init__(self, timeout=120., + safe_arm_joint_config='folded', + move_arm_server='move_arm_server', + move_base_server='/move_base', + pose_description_file='', + pose_frame='map', + max_recovery_attempts=1): + super(MoveBaseSM, self).__init__('MoveBase', [], max_recovery_attempts) + self.pose = None self.safe_arm_joint_config = safe_arm_joint_config self.move_arm_server = move_arm_server - self.move_arm_client = actionlib.SimpleActionClient(self.move_arm_server, MoveArmAction) - self.move_arm_client.wait_for_server() + self.move_base_server = move_base_server + self.pose_description_file = pose_description_file + self.pose_frame = pose_frame + self.timeout = timeout + self.move_arm_client = None - def execute(self, userdata): - feedback = MoveBaseFeedback() - feedback.current_state = 'SETUP_MOVE_BASE' - feedback.message = '[MOVE_BASE] Received a move base request' - userdata.move_base_feedback = feedback + def init(self): + try: + self.move_arm_client = actionlib.SimpleActionClient(self.move_arm_server, MoveArmAction) + rospy.loginfo('[move_base] Waiting for % server', self.move_arm_server) + self.move_arm_client.wait_for_server() + except: + rospy.logerr('[move_base] %s server does not seem to respond', self.move_arm_server) + return FTSMTransitions.INITIALISED - rospy.loginfo('[MOVE_BASE] Moving the arm to a safe configuration...') + def running(self): + rospy.loginfo('[move_base] Moving the arm to a safe configuration...') move_arm_goal = MoveArmGoal() move_arm_goal.goal_type = MoveArmGoal.NAMED_TARGET move_arm_goal.named_target = self.safe_arm_joint_config self.move_arm_client.send_goal(move_arm_goal) self.move_arm_client.wait_for_result() - return 'succeeded' -class ApproachPose(smach.State): - def __init__(self, timeout=120., - move_base_server='/move_base', - pose_description_file='', - pose_frame='map'): - smach.State.__init__(self, outcomes=['succeeded', 'failed'], - input_keys=['move_base_goal'], - output_keys=['move_base_feedback', 'move_base_result']) - self.pose = None - self.move_base_server = move_base_server - self.pose_description_file = pose_description_file - self.pose_frame = pose_frame - self.timeout = timeout - - def execute(self, userdata): pose = PoseStamped() - if userdata.move_base_goal.goal_type == MoveBaseGoal.NAMED_TARGET: - destination = userdata.move_base_goal.destination_location - - feedback = MoveBaseFeedback() - feedback.current_state = 'APPROACH_POSE' - feedback.message = '[MOVE_BASE] Moving base to ' + destination - userdata.move_base_feedback = feedback + if self.goal.goal_type == MoveBaseGoal.NAMED_TARGET: + destination = self.goal.destination_location + rospy.loginfo('[move_base] Moving base to %s', destination) self.pose = self.convert_pose_name_to_coordinates(destination) pose.header.stamp = rospy.Time.now() @@ -67,16 +58,13 @@ def execute(self, userdata): quat = tf.transformations.quaternion_from_euler(0, 0, self.pose[2]) pose.pose.orientation = Quaternion(*quat) - elif userdata.move_base_goal.goal_type == MoveBaseGoal.POSE: - pose = userdata.move_base_goal.pose - - feedback = MoveBaseFeedback() - feedback.current_state = 'APPROACH_POSE' - feedback.message = '[MOVE_BASE] Moving base to {0}'.format(pose) - userdata.move_base_feedback = feedback + elif self.goal.goal_type == MoveBaseGoal.POSE: + pose = self.goal.pose + rospy.loginfo('[move_base] Moving base to %s', pose) else: - rospy.logerr('[MOVE_BASE] Received an unknown goal type; ignoring request') - return 'failed' + rospy.logerr('[move_base] Received an unknown goal type; ignoring request') + self.result = self.set_result(False) + return FTSMTransitions.DONE goal = move_base_msgs.MoveBaseGoal() goal.target_pose = pose @@ -88,13 +76,16 @@ def execute(self, userdata): success = move_base_client.wait_for_result() if success: - rospy.loginfo('Pose reached successfully') - return 'succeeded' - rospy.logerr('Pose could not be reached') - return 'failed' + rospy.loginfo('[move_base] Pose reached successfully') + self.result = self.set_result(True) + return FTSMTransitions.DONE + + rospy.logerr('[move_base] Pose could not be reached') + self.result = self.set_result(False) + return FTSMTransitions.DONE def convert_pose_name_to_coordinates(self, pose_name): - stream = file(self.pose_description_file, 'r') + stream = open(self.pose_description_file, 'r') poses = yaml.load(stream) stream.close() try: @@ -104,15 +95,7 @@ def convert_pose_name_to_coordinates(self, pose_name): rospy.logerr('Pose name "%s" does not exist' % (pose_name)) return None -class SetActionLibResult(smach.State): - def __init__(self, result): - smach.State.__init__(self, outcomes=['succeeded'], - input_keys=['move_base_goal'], - output_keys=['move_base_feedback', 'move_base_result']) - self.result = result - - def execute(self, userdata): + def set_result(self, success): result = MoveBaseResult() - result.success = self.result - userdata.move_base_result = result - return 'succeeded' + result.success = success + return result diff --git a/mdr_planning/mdr_actions/mdr_navigation_actions/mdr_move_forward_action/ros/scripts/move_forward_action b/mdr_planning/mdr_actions/mdr_navigation_actions/mdr_move_forward_action/ros/scripts/move_forward_action index 135f87d7e..3c39fa604 100755 --- a/mdr_planning/mdr_actions/mdr_navigation_actions/mdr_move_forward_action/ros/scripts/move_forward_action +++ b/mdr_planning/mdr_actions/mdr_navigation_actions/mdr_move_forward_action/ros/scripts/move_forward_action @@ -1,58 +1,47 @@ #!/usr/bin/env python import rospy -import smach - -from smach_ros import ActionServerWrapper, IntrospectionServer +import actionlib from mdr_move_forward_action.msg import MoveForwardAction -from mdr_move_forward_action.action_states import SetupMoveForward, MoveForward, SetActionLibResult - -class MoveForwardSkill(smach.StateMachine): - def __init__(self, timeout=10): - smach.StateMachine.__init__(self, - outcomes=['OVERALL_SUCCESS', 'OVERALL_FAILED', 'PREEMPTED'], - input_keys=['move_forward_goal'], - output_keys=['move_forward_feedback', 'move_forward_result']) - - velocity_topic = rospy.get_param('~velocity_topic', '/cmd_vel') +from mdr_move_forward_action.action_states import MoveForwardSM - with self: - smach.StateMachine.add('SETUP_MOVE_FORWARD', SetupMoveForward(), - transitions={'succeeded': 'MOVE_FORWARD', - 'failed': 'SETUP_MOVE_FORWARD'}) +class MoveForwardServer(object): + '''A server exposing an action for moving a base forward and backward. - smach.StateMachine.add('MOVE_FORWARD', MoveForward(velocity_topic=velocity_topic), - transitions={'succeeded': 'SET_ACTION_LIB_SUCCESS', - 'failed': 'SET_ACTION_LIB_FAILED'}) + The server expects the following parameters to be made available on the ROS parameter server: + * velocity_topic: Name of a topic on which velocity commands are published - smach.StateMachine.add('SET_ACTION_LIB_FAILED', SetActionLibResult(False), - transitions={'succeeded': 'OVERALL_FAILED'}) + @author Alex Mitrevski + @contact aleksandar.mitrevski@h-brs.de - smach.StateMachine.add('SET_ACTION_LIB_SUCCESS', SetActionLibResult(True), - transitions={'succeeded': 'OVERALL_SUCCESS'}) + ''' + def __init__(self): + velocity_topic = rospy.get_param('~velocity_topic', '/cmd_vel') + rospy.loginfo('[move_forward] Initialising state machine') + self.action_sm = MoveForwardSM(velocity_topic=velocity_topic) + rospy.loginfo('[move_forward] State machine initialised') + + self.action_server = actionlib.SimpleActionServer('move_forward_server', + MoveForwardAction, + self.execute, False) + self.action_server.start() + rospy.loginfo('move_forward action server ready') + + def execute(self, goal): + rospy.loginfo('[move_forward] Received an action request') + self.action_sm.goal = goal + self.action_sm.execution_requested = True + while self.action_sm.result is None: + rospy.sleep(0.05) + self.action_server.set_succeeded(self.action_sm.result) if __name__ == '__main__': rospy.init_node('move_forward_server') - - # construct state machine - sm = MoveForwardSkill() - - # smach viewer - sis = IntrospectionServer('move_forward_smach_viewer', sm, - '/MOVE_FORWARD_SMACH_VIEWER') - sis.start() - - asw = ActionServerWrapper( - server_name='move_forward_server', - action_spec=MoveForwardAction, - wrapped_container=sm, - succeeded_outcomes=['OVERALL_SUCCESS'], - aborted_outcomes=['OVERALL_FAILED'], - preempted_outcomes=['PREEMPTED'], - goal_key='move_forward_goal', - feedback_key='move_forward_feedback', - result_key='move_forward_result') - - # Run the server in a background thread - asw.run_server() - rospy.spin() + move_forward_server = MoveForwardServer() + try: + move_forward_server.action_sm.run() + while move_forward_server.action_sm.is_running and not rospy.is_shutdown(): + rospy.spin() + except (KeyboardInterrupt, SystemExit): + print('{0} interrupted; exiting...'.format(move_forward_server.action_sm.name)) + move_forward_server.action_sm.stop() diff --git a/mdr_planning/mdr_actions/mdr_navigation_actions/mdr_move_forward_action/ros/src/mdr_move_forward_action/action_states.py b/mdr_planning/mdr_actions/mdr_navigation_actions/mdr_move_forward_action/ros/src/mdr_move_forward_action/action_states.py index be7fc7a89..752258f93 100644 --- a/mdr_planning/mdr_actions/mdr_navigation_actions/mdr_move_forward_action/ros/src/mdr_move_forward_action/action_states.py +++ b/mdr_planning/mdr_actions/mdr_navigation_actions/mdr_move_forward_action/ros/src/mdr_move_forward_action/action_states.py @@ -1,48 +1,26 @@ #!/usr/bin/python - import rospy -import smach -import smach_ros -import actionlib +from pyftsm.ftsm import FTSMTransitions +from mas_execution.action_sm_base import ActionSMBase from geometry_msgs.msg import Twist from mdr_move_forward_action.msg import MoveForwardFeedback, MoveForwardResult - -class SetupMoveForward(smach.State): - def __init__(self): - smach.State.__init__(self, outcomes=['succeeded', 'failed'], - input_keys=['move_forward_goal'], - output_keys=['move_forward_feedback', 'move_forward_result']) - - def execute(self, userdata): - duration = userdata.move_forward_goal.movement_duration - speed = userdata.move_forward_goal.speed - - feedback = MoveForwardFeedback() - feedback.current_state = 'MOVE_FORWARD' - feedback.message = '[move_forward] moving the base forward for ' + str(duration) + 's at ' + str(speed) + 'm/s' - userdata.move_forward_feedback = feedback - - return 'succeeded' - - -class MoveForward(smach.State): - def __init__(self, timeout=120.0, - velocity_topic='/cmd_vel'): - smach.State.__init__(self, input_keys=['move_forward_goal'], - outcomes=['succeeded', 'failed']) +class MoveForwardSM(ActionSMBase): + def __init__(self, timeout=120.0, velocity_topic='/cmd_vel', max_recovery_attempts=1): + super(MoveForwardSM, self).__init__('MoveForward', [], max_recovery_attempts) self.timeout = timeout self.velocity_pub = rospy.Publisher(velocity_topic, Twist) - def execute(self, userdata): - duration = rospy.Duration.from_sec(userdata.move_forward_goal.movement_duration) - speed = userdata.move_forward_goal.speed + def running(self): + duration = rospy.Duration.from_sec(self.goal.movement_duration) + speed = self.goal.speed rate = rospy.Rate(5) twist = Twist() twist.linear.x = speed + rospy.loginfo('[move_forward] moving the base forward for %s s at %s m/s', str(duration), str(speed)) start_time = rospy.Time.now() while (rospy.Time.now() - start_time) < duration: self.velocity_pub.publish(twist) @@ -51,19 +29,10 @@ def execute(self, userdata): # we publish a zero twist at the end so that the robot stops moving zero_twist = Twist() self.velocity_pub.publish(zero_twist) + self.result = self.set_result(True) + return FTSMTransitions.DONE - return 'succeeded' - - -class SetActionLibResult(smach.State): - def __init__(self, result): - smach.State.__init__(self, outcomes=['succeeded'], - input_keys=['move_forward_goal'], - output_keys=['move_forward_feedback', 'move_forward_result']) - self.result = result - - def execute(self, userdata): + def set_result(self, success): result = MoveForwardResult() - result.success = self.result - userdata.move_forward_result = result - return 'succeeded' + result.success = success + return result diff --git a/mdr_planning/mdr_actions/mdr_navigation_actions/mdr_turn_base_to_action/ros/scripts/turn_base_to_action b/mdr_planning/mdr_actions/mdr_navigation_actions/mdr_turn_base_to_action/ros/scripts/turn_base_to_action index e08875e54..e6f987323 100755 --- a/mdr_planning/mdr_actions/mdr_navigation_actions/mdr_turn_base_to_action/ros/scripts/turn_base_to_action +++ b/mdr_planning/mdr_actions/mdr_navigation_actions/mdr_turn_base_to_action/ros/scripts/turn_base_to_action @@ -1,59 +1,51 @@ #!/usr/bin/env python import rospy -import smach - -from smach_ros import ActionServerWrapper, IntrospectionServer +import actionlib from mdr_turn_base_to_action.msg import TurnBaseToAction -from mdr_turn_base_to_action.action_states import SetupTurnBaseTo, TurnBaseTo, SetActionLibResult - -class TurnBaseToSkill(smach.StateMachine): - def __init__(self, timeout=10): - smach.StateMachine.__init__(self, - outcomes=['OVERALL_SUCCESS', 'OVERALL_FAILED', 'PREEMPTED'], - input_keys=['turn_base_to_goal'], - output_keys=['turn_base_to_feedback', 'turn_base_to_result']) +from mdr_turn_base_to_action.action_states import TurnBaseSM - move_base_server_name = rospy.get_param('~move_base_server', '/move_base') - rotation_frame = rospy.get_param('~rotation_frame', 'base_link') +class TurnBaseServer(object): + '''A server exposing an action for turning a robot base to a desired angle. - with self: - smach.StateMachine.add('SETUP_TURN_BASE_TO', SetupTurnBaseTo(), - transitions={'succeeded':'TURN_BASE_TO', - 'failed':'SET_ACTION_LIB_FAILED'}) + The server expects the following parameters to be made available on the ROS parameter server: + * move_base_server: Name of an action server for moving the base + * rotation_frame: Name of the frame in which the rotation is performed - smach.StateMachine.add('TURN_BASE_TO', TurnBaseTo(move_base_server=move_base_server_name, - rotation_frame=rotation_frame), - transitions={'succeeded': 'SET_ACTION_LIB_SUCCESS', - 'failed': 'SET_ACTION_LIB_FAILED'}) + @author Alex Mitrevski, Jose Mayoral Banos + @contact aleksandar.mitrevski@h-brs.de - smach.StateMachine.add('SET_ACTION_LIB_FAILED', SetActionLibResult(False), - transitions={'succeeded':'OVERALL_FAILED'}) + ''' + def __init__(self): + move_base_server_name = rospy.get_param('~move_base_server', '/move_base') + rotation_frame = rospy.get_param('~rotation_frame', 'base_link') - smach.StateMachine.add('SET_ACTION_LIB_SUCCESS', SetActionLibResult(True), - transitions={'succeeded':'OVERALL_SUCCESS'}) + rospy.loginfo('[turn_base] Initialising state machine') + self.action_sm = TurnBaseSM(move_base_server=move_base_server_name, + rotation_frame=rotation_frame) + rospy.loginfo('[turn_base] State machine initialised') + + self.action_server = actionlib.SimpleActionServer('turn_base_to_server', + TurnBaseToAction, + self.execute, False) + self.action_server.start() + rospy.loginfo('turn_base action server ready') + + def execute(self, goal): + rospy.loginfo('[turn_base] Received an action request') + self.action_sm.goal = goal + self.action_sm.execution_requested = True + while self.action_sm.result is None: + rospy.sleep(0.05) + self.action_server.set_succeeded(self.action_sm.result) if __name__ == '__main__': - rospy.init_node('turn_base_to_server') - - # construct state machine - sm = TurnBaseToSkill() - - # smach viewer - sis = IntrospectionServer('turn_base_to_smach_viewer', sm, '/TURN_BASE_TO_SMACH_VIEWER') - sis.start() - - asw = ActionServerWrapper( - server_name='turn_base_to_server', - action_spec=TurnBaseToAction, - wrapped_container=sm, - succeeded_outcomes=['OVERALL_SUCCESS'], - aborted_outcomes=['OVERALL_FAILED'], - preempted_outcomes=['PREEMPTED'], - goal_key='turn_base_to_goal', - feedback_key='turn_base_to_feedback', - result_key='turn_base_to_result') - - # Run the server in a background thread - asw.run_server() - rospy.spin() + rospy.init_node('turn_base_server') + turn_base_server = TurnBaseServer() + try: + turn_base_server.action_sm.run() + while turn_base_server.action_sm.is_running and not rospy.is_shutdown(): + rospy.spin() + except (KeyboardInterrupt, SystemExit): + print('{0} interrupted; exiting...'.format(turn_base_server.action_sm.name)) + turn_base_server.action_sm.stop() diff --git a/mdr_planning/mdr_actions/mdr_navigation_actions/mdr_turn_base_to_action/ros/src/mdr_turn_base_to_action/action_states.py b/mdr_planning/mdr_actions/mdr_navigation_actions/mdr_turn_base_to_action/ros/src/mdr_turn_base_to_action/action_states.py index 3c0c1bb9c..111ca3246 100644 --- a/mdr_planning/mdr_actions/mdr_navigation_actions/mdr_turn_base_to_action/ros/src/mdr_turn_base_to_action/action_states.py +++ b/mdr_planning/mdr_actions/mdr_navigation_actions/mdr_turn_base_to_action/ros/src/mdr_turn_base_to_action/action_states.py @@ -1,70 +1,53 @@ #!/usr/bin/python - import rospy -import smach import actionlib from tf.transformations import quaternion_from_euler -from mdr_turn_base_to_action.msg import TurnBaseToFeedback, TurnBaseToResult +from pyftsm.ftsm import FTSMTransitions +from mas_execution.action_sm_base import ActionSMBase from mdr_move_base_action.msg import MoveBaseAction, MoveBaseGoal +from mdr_turn_base_to_action.msg import TurnBaseToFeedback, TurnBaseToResult - -class SetupTurnBaseTo(smach.State): - def __init__(self): - smach.State.__init__(self, outcomes=['succeeded', 'failed'], - output_keys=['turn_base_to_feedback']) - - def execute(self, userdata): - feedback = TurnBaseToFeedback() - feedback.message = '[turn_base_to_goal] sm initialize' - userdata.turn_base_to_feedback = feedback - return 'succeeded' - - -class TurnBaseTo(smach.State): +class TurnBaseSM(ActionSMBase): def __init__(self, timeout=120.0, rotation_frame='base_link', move_base_server='/move_base', - movement_duration=15., speed=0.1): - smach.State.__init__(self, input_keys=['turn_base_to_goal'], - outcomes=['succeeded', 'failed']) - - rospy.loginfo("Using move base server: " + move_base_server) - rospy.loginfo("Rotation Frame set to : " + rotation_frame) + max_recovery_attempts=1): + super(TurnBaseSM, self).__init__('TurnBase', [], max_recovery_attempts) self.move_base_server = move_base_server self.timeout = timeout self.rotation_frame = rotation_frame - self.movement_duration = movement_duration - self.speed = speed - - self.move_base_client = actionlib.SimpleActionClient(self.move_base_server, MoveBaseAction) - self.move_base_client.wait_for_server() - - def execute(self, userdata): + self.move_base_client = None + + def init(self): + try: + self.move_base_client = actionlib.SimpleActionClient(self.move_base_server, MoveBaseAction) + rospy.loginfo('[turn_base_to] Waiting for % server', self.move_base_server) + self.move_base_client.wait_for_server() + except: + rospy.logerr('[turn_base_to] %s server does not seem to respond', self.move_base_server) + return FTSMTransitions.INITIALISED + + def running(self): goal = MoveBaseGoal() goal.goal_type = MoveBaseGoal.POSE goal.pose.header.frame_id = self.rotation_frame - q = quaternion_from_euler(0, 0, userdata.turn_base_to_goal.desired_yaw) + q = quaternion_from_euler(0, 0, self.goal.desired_yaw) goal.pose.pose.orientation.x = q[0] goal.pose.pose.orientation.y = q[1] goal.pose.pose.orientation.z = q[2] goal.pose.pose.orientation.w = q[3] - rospy.loginfo("[mdr_turn_base_to] Goal %s", goal) + + rospy.loginfo('[turn_base_to] Rotating the base to %s', str(self.goal.desired_yaw)) self.move_base_client.send_goal(goal) success = self.move_base_client.wait_for_result() if success: - return 'succeeded' - return 'failed' - -class SetActionLibResult(smach.State): - def __init__(self, result): - smach.State.__init__(self, outcomes=['succeeded'], - output_keys=['turn_base_to_result']) - self.result = result + self.result = self.set_result(True) + self.result = self.set_result(False) + return FTSMTransitions.DONE - def execute(self, userdata): + def set_result(self, success): result = TurnBaseToResult() - result.success = self.result - userdata.turn_base_to_result = result - return 'succeeded' + result.success = success + return result From d047b58d2a22f84c8cf65eb1a78f7bdd9db6c513 Mon Sep 17 00:00:00 2001 From: Alex Mitrevski Date: Fri, 25 Jan 2019 06:38:39 +0100 Subject: [PATCH 48/96] [.rosinstall] Changed the FTSM url to the b-it-bots fork --- mas-domestic.rosinstall | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/mas-domestic.rosinstall b/mas-domestic.rosinstall index 2cb176888..2d9771dbf 100644 --- a/mas-domestic.rosinstall +++ b/mas-domestic.rosinstall @@ -35,7 +35,7 @@ - git: local-name: ftsm - uri: https://github.com/ropod-project/ftsm.git + uri: https://github.com/b-it-bots/ftsm.git version: master - git: From 7b9ba69ffa96bfb893e493974b7b9eb1ebcb382e Mon Sep 17 00:00:00 2001 From: Alex Mitrevski Date: Fri, 25 Jan 2019 15:37:21 +0100 Subject: [PATCH 49/96] The perception actions are now implemented using a fault-tolerant state machine --- .../ros/scripts/detect_person_action | 70 ++++--- .../src/mdr_detect_person/action_states.py | 110 +++++------ .../ros/src/mdr_detect_person/skill.py | 38 ---- .../ros/scripts/gender_recognition_action | 74 +++++--- .../mdr_gender_recognition/action_states.py | 100 ++++------ .../ros/src/mdr_gender_recognition/skill.py | 44 ----- .../ros/scripts/perceive_plane_action | 130 ++++++------- .../action_states.py | 171 +++++------------- .../ros/scripts/recognize_emotion_action | 97 ++++------ .../action_states.py | 108 +++++------ 10 files changed, 360 insertions(+), 582 deletions(-) delete mode 100644 mdr_planning/mdr_actions/mdr_perception_actions/mdr_detect_person/ros/src/mdr_detect_person/skill.py delete mode 100644 mdr_planning/mdr_actions/mdr_perception_actions/mdr_gender_recognition/ros/src/mdr_gender_recognition/skill.py diff --git a/mdr_planning/mdr_actions/mdr_perception_actions/mdr_detect_person/ros/scripts/detect_person_action b/mdr_planning/mdr_actions/mdr_perception_actions/mdr_detect_person/ros/scripts/detect_person_action index a11532abb..4e0c3c909 100755 --- a/mdr_planning/mdr_actions/mdr_perception_actions/mdr_detect_person/ros/scripts/detect_person_action +++ b/mdr_planning/mdr_actions/mdr_perception_actions/mdr_detect_person/ros/scripts/detect_person_action @@ -1,33 +1,51 @@ #!/usr/bin/env python import rospy -import smach -from smach_ros import ActionServerWrapper, IntrospectionServer +import actionlib -from mdr_detect_person.skill import DetectPersonSkill from mdr_detect_person.msg import DetectPersonAction +from mdr_detect_person.action_states import DetectPersonSM + +class DetectPersonServer(object): + '''A server exposing an action for detecting person faces in an image. + + The server expects the following parameters to be made available on the ROS parameter server: + * image_topic: Name of a camera image topic (default /cam3d/rgb/image_raw) + * detection_model_path: Absolute path of a person detection model (default '') + + @author Alex Mitrevski, Gabriela Cortes, Octavio Arriaga + @contact aleksandar.mitrevski@h-brs.de + + ''' + def __init__(self): + image_topic = rospy.get_param('~image_topic', '/cam3d/rgb/image_raw') + detection_model_path = rospy.get_param('~config_file', '') + + rospy.loginfo('[detect_person] Initialising state machine') + self.action_sm = DetectPersonSM(image_topic=image_topic, + detection_model_path=detection_model_path) + rospy.loginfo('[detect_person] State machine initialised') + + self.action_server = actionlib.SimpleActionServer('detect_person_server', + DetectPersonAction, + self.execute, False) + self.action_server.start() + rospy.loginfo('detect_person action server ready') + + def execute(self, goal): + rospy.loginfo('[detect_person] Received an action request') + self.action_sm.goal = goal + self.action_sm.execution_requested = True + while self.action_sm.result is None: + rospy.sleep(0.05) + self.action_server.set_succeeded(self.action_sm.result) if __name__ == '__main__': rospy.init_node('detect_person_server') - - # construct state machine - sm = DetectPersonSkill() - - # smach viewer - sis = IntrospectionServer('detect_person_smach_viewer', sm, - '/DETECT_PERSON_SMACH_VIEWER') - sis.start() - - asw = ActionServerWrapper( - server_name='detect_person_server', - action_spec=DetectPersonAction, - wrapped_container=sm, - succeeded_outcomes=['OVERALL_SUCCESS'], - aborted_outcomes=['OVERALL_FAILED'], - preempted_outcomes=['PREEMPTED'], - goal_key='detect_person_goal', - feedback_key='detect_person_feedback', - result_key='detect_person_result') - - # Run the server in a background thread - asw.run_server() - rospy.spin() + detect_person_server = DetectPersonServer() + try: + detect_person_server.action_sm.run() + while detect_person_server.action_sm.is_running and not rospy.is_shutdown(): + rospy.spin() + except (KeyboardInterrupt, SystemExit): + print('{0} interrupted; exiting...'.format(detect_person_server.action_sm.name)) + detect_person_server.action_sm.stop() diff --git a/mdr_planning/mdr_actions/mdr_perception_actions/mdr_detect_person/ros/src/mdr_detect_person/action_states.py b/mdr_planning/mdr_actions/mdr_perception_actions/mdr_detect_person/ros/src/mdr_detect_person/action_states.py index 7e2aa355c..c9085e0c2 100644 --- a/mdr_planning/mdr_actions/mdr_perception_actions/mdr_detect_person/ros/src/mdr_detect_person/action_states.py +++ b/mdr_planning/mdr_actions/mdr_perception_actions/mdr_detect_person/ros/src/mdr_detect_person/action_states.py @@ -1,97 +1,71 @@ #!/usr/bin/python - import rospy -import smach -import smach_ros -import actionlib import cv2 import numpy as np from sensor_msgs.msg import Image -from cv_bridge import CvBridge, CvBridgeError +from cv_bridge import CvBridge +from pyftsm.ftsm import FTSMTransitions +from mas_execution.action_sm_base import ActionSMBase from mdr_detect_person.msg import DetectPersonFeedback, DetectPersonGoal, DetectPersonResult from mdr_perception_msgs.msg import FaceBoundingBox - -from mdr_detect_person.inference import detect_faces -from mdr_detect_person.inference import load_detection_model - - -class SetupDetectPerson(smach.State): - def __init__(self): - smach.State.__init__(self, outcomes=['succeeded', 'failed'], - input_keys=['detect_person_goal'], - output_keys=['detect_person_feedback', 'detect_person_result']) - - def execute(self, userdata): - start = userdata.detect_person_goal.start - print (start) - if start: - feedback = DetectPersonFeedback() - feedback.current_state = 'DETECT_PERSON' - feedback.text = '[detect_person] detecting faces' - userdata.detect_person_feedback = feedback - return 'succeeded' - else: - return 'failed' - - -class DetectPerson(smach.State): - def __init__(self, timeout=120., image_topic='/cam3d/rgb/image_raw', detection_model_path=''): - smach.State.__init__(self, outcomes=['succeeded', 'failed'], - input_keys=['detect_person_goal', 'number_of_faces', - 'bounding_boxes'], - output_keys=['detect_person_feedback', 'number_of_faces', - 'bounding_boxes']) +from mdr_detect_person.inference import load_detection_model, detect_faces + +class DetectPersonSM(ActionSMBase): + def __init__(self, timeout=120., + image_topic='/cam3d/rgb/image_raw', + detection_model_path='', + max_recovery_attempts=1): + super(DetectPersonSM, self).__init__('DetectPerson', [], max_recovery_attempts) self.timeout = timeout + self.detection_model_path = detection_model_path self.bridge = CvBridge() - self.image_publisher = rospy.Publisher(image_topic, Image, queue_size=1) + self.image_publisher = rospy.Publisher(image_topic, Image, queue_size=1) + self.face_detection = None - # loading model - self.face_detection = load_detection_model(detection_model_path) - - def execute(self, userdata): - userdata.bounding_boxes = [] - self.input_image = self.convert_image(userdata.detect_person_goal.image) + def init(self): + try: + rospy.loginfo('[detect_person] Loading detection model %s', self.detection_model_path) + self.face_detection = load_detection_model(detection_model_path) + except: + rospy.logerr('[detect_person] Model %s could not be loaded', self.detection_model_path) + return FTSMTransitions.INITIALISED + + def running(self): + bounding_boxes = [] + number_of_faces = 0 + detection_successful = True + input_image = self.__convert_image(self.goal.image) try: - bgr_image = self.input_image + bgr_image = input_image gray_image = cv2.cvtColor(bgr_image, cv2.COLOR_BGR2GRAY) rgb_image = cv2.cvtColor(bgr_image, cv2.COLOR_BGR2RGB) faces = detect_faces(self.face_detection, gray_image) - userdata.number_of_faces = np.size(faces, 0) - except: - userdata.number_of_faces = 0 + number_of_faces = np.size(faces, 0) - if userdata.number_of_faces != 0: for face_coordinates in faces: bounding_box = FaceBoundingBox() - print type(face_coordinates) bounding_box.bounding_box_coordinates = face_coordinates.tolist() - userdata.bounding_boxes.append(bounding_box) + bounding_boxes.append(bounding_box) x, y, w, h = face_coordinates - rgb_cv2 = cv2.rectangle(rgb_image, (x, y), (x + w, y + h), (0,0,255), 2) + rgb_cv2 = cv2.rectangle(rgb_image, (x, y), (x + w, y + h), (0, 0, 255), 2) output_ros_image = self.bridge.cv2_to_imgmsg(rgb_image, 'bgr8') self.image_publisher.publish(output_ros_image) - return 'succeeded' - else: - return 'failed' + except: + detection_successful = False + + self.result = self.set_result(detection_successful, number_of_faces, bounding_boxes) + return FTSMTransitions.DONE - def convert_image(self, ros_image): + def __convert_image(self, ros_image): cv_image = self.bridge.imgmsg_to_cv2(ros_image, 'bgr8') return np.array(cv_image, dtype=np.uint8) -class SetActionLibResult(smach.State): - def __init__(self, result): - smach.State.__init__(self, outcomes=['succeeded'], - input_keys=['number_of_faces', 'bounding_boxes'], - output_keys=['detect_person_result','detect_person_feedback']) - self.result = result - - def execute(self, userdata): + def set_result(self, detection_successful, number_of_faces, bounding_boxes): result = DetectPersonResult() - result.success = self.result - result.number_of_faces = userdata.number_of_faces - result.bounding_boxes = userdata.bounding_boxes - userdata.detect_person_result = result - return 'succeeded' + result.success = detection_successful + result.number_of_faces = number_of_faces + result.bounding_boxes = bounding_boxes + return result diff --git a/mdr_planning/mdr_actions/mdr_perception_actions/mdr_detect_person/ros/src/mdr_detect_person/skill.py b/mdr_planning/mdr_actions/mdr_perception_actions/mdr_detect_person/ros/src/mdr_detect_person/skill.py deleted file mode 100644 index c98b07a2d..000000000 --- a/mdr_planning/mdr_actions/mdr_perception_actions/mdr_detect_person/ros/src/mdr_detect_person/skill.py +++ /dev/null @@ -1,38 +0,0 @@ -#!/usr/bin/env python -import rospy -import smach -from smach import StateMachine as sm -from mdr_detect_person.msg import DetectPersonAction -from mdr_detect_person.action_states import (SetupDetectPerson, DetectPerson, - SetActionLibResult) - - -class DetectPersonSkill(smach.StateMachine): - def __init__(self, timeout=10): - sm.__init__(self, - outcomes=['OVERALL_SUCCESS', 'OVERALL_FAILED', 'PREEMPTED'], - input_keys=['detect_person_goal'], - output_keys=['detect_person_feedback', - 'detect_person_result']) - - detection_model_path = rospy.get_param('~config_file', '') - image_topic = rospy.get_param('~image_topic', '/cam3d/rgb/image_raw') - - with self: - sm.add('SETUP_DETECT_PERSON', SetupDetectPerson(), - transitions={'succeeded': 'DETECT_PERSON', - 'failed': 'SETUP_DETECT_PERSON'}) - - sm.add('DETECT_PERSON', - DetectPerson(image_topic=image_topic, - detection_model_path=detection_model_path), - transitions={'succeeded': 'SET_ACTION_LIB_SUCCESS', - 'failed': 'SET_ACTION_LIB_FAILED'}) - - sm.add('SET_ACTION_LIB_FAILED', - SetActionLibResult(False), - transitions={'succeeded': 'OVERALL_FAILED'}) - - sm.add('SET_ACTION_LIB_SUCCESS', - SetActionLibResult(True), - transitions={'succeeded': 'OVERALL_SUCCESS'}) diff --git a/mdr_planning/mdr_actions/mdr_perception_actions/mdr_gender_recognition/ros/scripts/gender_recognition_action b/mdr_planning/mdr_actions/mdr_perception_actions/mdr_gender_recognition/ros/scripts/gender_recognition_action index f21c97e84..64938f2d4 100755 --- a/mdr_planning/mdr_actions/mdr_perception_actions/mdr_gender_recognition/ros/scripts/gender_recognition_action +++ b/mdr_planning/mdr_actions/mdr_perception_actions/mdr_gender_recognition/ros/scripts/gender_recognition_action @@ -1,34 +1,54 @@ #!/usr/bin/env python import rospy -import smach +import actionlib -from smach_ros import ActionServerWrapper, IntrospectionServer -from mdr_gender_recognition.skill import GenderRecognitionSkill from mdr_gender_recognition.msg import GenderRecognitionAction +from mdr_gender_recognition.action_states import RecognizeGenderSM +class RecognizeGenderServer(object): + '''A server exposing an action for recognizing the genders of previously detected people. + + The server expects the following parameters to be made available on the ROS parameter server: + * gender_model_path: Absolute path of a gender recognition model (default '') + * image_topic: Name of a camera image topic (default '/cam3d/rgb/image_raw') + + @author Alex Mitrevski, Octavio Arriaga + @contact aleksandar.mitrevski@h-brs.de + + ''' + def __init__(self): + gender_model_path = rospy.get_param('~gender_model_path', '') + image_topic = rospy.get_param('~image_topic', '/cam3d/rgb/image_raw') + labels = {0: 'woman', 1: 'man'} + image_size = (64, 64, 1) + + rospy.loginfo('[recognize_gender] Initialising state machine') + self.action_sm = RecognizeGenderSM(gender_model_path=gender_model_path, + image_topic=image_topic, + labels=labels, image_size=image_size) + rospy.loginfo('[recognize_gender] State machine initialised') + + self.action_server = actionlib.SimpleActionServer('recognize_gender_server', + GenderRecognitionAction, + self.execute, False) + self.action_server.start() + rospy.loginfo('recognize_gender action server ready') + + def execute(self, goal): + rospy.loginfo('[recognize_gender] Received an action request') + self.action_sm.goal = goal + self.action_sm.execution_requested = True + while self.action_sm.result is None: + rospy.sleep(0.05) + self.action_server.set_succeeded(self.action_sm.result) if __name__ == '__main__': - rospy.init_node('gender_recognition_server') - - # construct state machine - sm = GenderRecognitionSkill() - - # smach viewer - sis = IntrospectionServer('gender_recognition_smach_viewer', sm, - '/gender_recognition_SMACH_VIEWER') - sis.start() - - asw = ActionServerWrapper( - server_name='gender_recognition_server', - action_spec=GenderRecognitionAction, - wrapped_container=sm, - succeeded_outcomes=['OVERALL_SUCCESS'], - aborted_outcomes=['OVERALL_FAILED'], - preempted_outcomes=['PREEMPTED'], - goal_key='gender_recognition_goal', - feedback_key='gender_recognition_feedback', - result_key='gender_recognition_result') - - # Run the server in a background thread - asw.run_server() - rospy.spin() + rospy.init_node('recognize_gender_server') + recognize_gender_server = RecognizeGenderServer() + try: + recognize_gender_server.action_sm.run() + while recognize_gender_server.action_sm.is_running and not rospy.is_shutdown(): + rospy.spin() + except (KeyboardInterrupt, SystemExit): + print('{0} interrupted; exiting...'.format(recognize_gender_server.action_sm.name)) + recognize_gender_server.action_sm.stop() diff --git a/mdr_planning/mdr_actions/mdr_perception_actions/mdr_gender_recognition/ros/src/mdr_gender_recognition/action_states.py b/mdr_planning/mdr_actions/mdr_perception_actions/mdr_gender_recognition/ros/src/mdr_gender_recognition/action_states.py index 10cf2d016..d0fb1e5dd 100644 --- a/mdr_planning/mdr_actions/mdr_perception_actions/mdr_gender_recognition/ros/src/mdr_gender_recognition/action_states.py +++ b/mdr_planning/mdr_actions/mdr_perception_actions/mdr_gender_recognition/ros/src/mdr_gender_recognition/action_states.py @@ -1,111 +1,89 @@ -#!/usr/bin/python - -import rospy -import smach -import smach_ros -import actionlib -from sensor_msgs.msg import Image - import numpy as np import cv2 import tensorflow as tf from keras.models import load_model from cv_bridge import CvBridge, CvBridgeError -from mdr_gender_recognition.msg import GenderRecognitionFeedback, GenderRecognitionResult - - -class SetupGenderRecognition(smach.State): - def __init__(self): - smach.State.__init__(self, outcomes=['succeeded', 'failed'], - input_keys=['gender_recognition_goal'], - output_keys=['gender_recognition_feedback', 'gender_recognition_result']) - - def execute(self, userdata): - feedback = GenderRecognitionFeedback() - feedback.current_state = 'RECOGNIZE_GENDERS' - feedback.message = '[gender_recognition] Recognizing gender' - userdata.gender_recognition_feedback = feedback - return 'succeeded' +import rospy +from sensor_msgs.msg import Image +from pyftsm.ftsm import FTSMTransitions +from mas_execution.action_sm_base import ActionSMBase +from mdr_gender_recognition.msg import GenderRecognitionFeedback, GenderRecognitionResult -class RecognizeGenders(smach.State): +class RecognizeGenderSM(ActionSMBase): def __init__(self, timeout=120.0, image_topic='/cam3d/rgb/image_raw', - gender_model_path=None, labels=dict(), image_size=(0, 0, 0)): - smach.State.__init__(self, input_keys=['gender_recognition_goal', 'genders'], - output_keys=['gender_recognition_feedback', 'genders'], - outcomes=['succeeded', 'failed']) + gender_model_path=None, labels=None, image_size=(0, 0, 0), + max_recovery_attempts=1): + super(RecognizeGenderSM, self).__init__('RecognizeGender', [], max_recovery_attempts) self.timeout = timeout + self.gender_model_path = gender_model_path self.labels = labels self.image_size = image_size - self.nop = np.array([None]) self.image_publisher = rospy.Publisher(image_topic, Image, queue_size=1) self.bridge = CvBridge() + self.gender_model = None + self.computation_graph = None + def init(self): try: - # loading model + rospy.loginfo('[recognize_gender] Loading model %s', self.gender_model_path) self.gender_model = load_model(gender_model_path) - print('Model ' + gender_model_path + ' loaded successfully') # the following two lines are necessary for avoiding https://github.com/keras-team/keras/issues/2397 self.gender_model._make_predict_function() self.computation_graph = tf.get_default_graph() except: - print('Failed to load model ', gender_model_path) + rospy.logerr('[recognize_gender] Failed to load model %s', self.gender_model_path) + return FTSMTransitions.INITIALISED - def execute(self, userdata): - number_of_faces = userdata.gender_recognition_goal.number_of_faces - bounding_boxes = userdata.gender_recognition_goal.bounding_boxes - userdata.genders = list() + def running(self): + number_of_faces = self.goal.number_of_faces + bounding_boxes = self.goal.bounding_boxes + genders = [] - rgb_image = self.ros2cv(userdata.gender_recognition_goal.image) - gray_image = self.rgb2gray(rgb_image) + rospy.loginfo('[recognize_gender] Recognizing genders') + rgb_image = self.__ros2cv(self.goal.image) + gray_image = self.__rgb2gray(rgb_image) for face in bounding_boxes: x, y, w, h = face.bounding_box_coordinates face = gray_image[y: (y + h), x: (x + w)] # check if it is not rgb face = cv2.resize(face, self.image_size[0:2]) face = np.expand_dims(face, 0) face = np.expand_dims(face, -1) - face = self.preprocess_image(face) - recognized_gender = self.recognize_gender(face) - userdata.genders.append(recognized_gender) + face = self.__preprocess_image(face) + recognized_gender = self.__recognize_gender(face) + genders.append(recognized_gender) rgb_cv2 = cv2.rectangle(rgb_image, (x, y), (x + w, y + h), (0, 0, 255), 2) cv2.putText(rgb_image, recognized_gender, (x, y - 30), cv2.FONT_HERSHEY_SIMPLEX, 0.7, (255, 0, 0), 1, cv2.LINE_AA) output_ros_image = self.bridge.cv2_to_imgmsg(rgb_image, 'bgr8') self.image_publisher.publish(output_ros_image) - return 'succeeded' - def preprocess_image(self, image): + self.result = self.set_result(True, genders) + return FTSMTransitions.DONE + + def set_result(self, success, genders): + result = GenderRecognitionResult() + result.success = success + result.genders = genders + return result + + def __preprocess_image(self, image): image = image / 255.0 return image - def recognize_gender(self, face): + def __recognize_gender(self, face): label = -1 with self.computation_graph.as_default(): class_predictions = self.gender_model.predict(face) label = self.labels[np.argmax(class_predictions)] return label - def ros2cv(self, ros_image): + def __ros2cv(self, ros_image): cv_image = self.bridge.imgmsg_to_cv2(ros_image, 'bgr8') return np.array(cv_image, dtype=np.uint8) - def rgb2gray(self, image): + def __rgb2gray(self, image): gray_image = cv2.cvtColor(image, cv2.COLOR_BGR2GRAY) return gray_image - - -class SetActionLibResult(smach.State): - def __init__(self, result): - smach.State.__init__(self, outcomes=['succeeded'], - input_keys=['gender_recognition_goal', 'genders'], - output_keys=['gender_recognition_feedback', 'gender_recognition_result']) - self.result = result - - def execute(self, userdata): - result = GenderRecognitionResult() - result.success = self.result - result.genders = userdata.genders - userdata.gender_recognition_result = result - return 'succeeded' diff --git a/mdr_planning/mdr_actions/mdr_perception_actions/mdr_gender_recognition/ros/src/mdr_gender_recognition/skill.py b/mdr_planning/mdr_actions/mdr_perception_actions/mdr_gender_recognition/ros/src/mdr_gender_recognition/skill.py deleted file mode 100644 index 80a1aa5b9..000000000 --- a/mdr_planning/mdr_actions/mdr_perception_actions/mdr_gender_recognition/ros/src/mdr_gender_recognition/skill.py +++ /dev/null @@ -1,44 +0,0 @@ -#!/usr/bin/env python -import rospy -import smach -from smach import StateMachine as sm - -from mdr_gender_recognition.msg import GenderRecognitionAction -from mdr_gender_recognition.action_states import (SetupGenderRecognition, - RecognizeGenders, - SetActionLibResult) - - -class GenderRecognitionSkill(smach.StateMachine): - def __init__(self, timeout=10): - sm.__init__(self, - outcomes=['OVERALL_SUCCESS', - 'OVERALL_FAILED', 'PREEMPTED'], - input_keys=['gender_recognition_goal'], - output_keys=['gender_recognition_feedback', - 'gender_recognition_result']) - - gender_model_path = rospy.get_param('~gender_model_path', '') - image_topic = rospy.get_param('~image_topic', '/cam3d/rgb/image_raw') - labels = {0: 'woman', 1: 'man'} - image_size = (64, 64, 1) - - with self: - sm.add('SETUP_GENDER_RECOGNITION', SetupGenderRecognition(), - transitions={'succeeded': 'RECOGNIZE_GENDERS', - 'failed': 'SETUP_GENDER_RECOGNITION'}) - - sm.add('RECOGNIZE_GENDERS', - RecognizeGenders(gender_model_path=gender_model_path, - image_topic=image_topic, - labels=labels, image_size=image_size), - transitions={'succeeded': 'SET_ACTION_LIB_SUCCESS', - 'failed': 'SET_ACTION_LIB_FAILED'}) - - sm.add('SET_ACTION_LIB_FAILED', - SetActionLibResult(False), - transitions={'succeeded': 'OVERALL_FAILED'}) - - sm.add('SET_ACTION_LIB_SUCCESS', - SetActionLibResult(True), - transitions={'succeeded': 'OVERALL_SUCCESS'}) diff --git a/mdr_planning/mdr_actions/mdr_perception_actions/mdr_perceive_plane_action/ros/scripts/perceive_plane_action b/mdr_planning/mdr_actions/mdr_perception_actions/mdr_perceive_plane_action/ros/scripts/perceive_plane_action index 101ed8fbe..206e128a9 100755 --- a/mdr_planning/mdr_actions/mdr_perception_actions/mdr_perceive_plane_action/ros/scripts/perceive_plane_action +++ b/mdr_planning/mdr_actions/mdr_perception_actions/mdr_perceive_plane_action/ros/scripts/perceive_plane_action @@ -1,76 +1,64 @@ #!/usr/bin/env python import rospy -import smach -from smach_ros import ActionServerWrapper, IntrospectionServer -from mas_perception_libs import Constant -from mdr_perceive_plane_action.msg import PerceivePlaneAction -from mdr_perceive_plane_action.action_states import SetActionLibResult, DetectObjects, RecognizeObjects - - -class PerceivePlaneSkill(smach.StateMachine): - def __init__(self, detection_action_name, target_frame, recog_service_name, recog_model_name, - preprocess_input_module): - smach.StateMachine.__init__(self, outcomes=['OVERALL_SUCCESS', 'OVERALL_FAILED', 'PREEMPTED'], - input_keys=['perceive_plane_goal'], - output_keys=['perceive_plane_feedback', 'perceive_plane_result']) - - with self: - smach.StateMachine.add('DETECT_OBJECTS', - DetectObjects(detection_action_name, target_frame), - transitions={Constant.SUCCESS: 'RECOGNIZE_OBJECTS', - Constant.TIMEOUT: 'SET_ACTION_LIB_FAILURE', - Constant.FAILURE: 'SET_ACTION_LIB_FAILURE'}) - - smach.StateMachine.add('RECOGNIZE_OBJECTS', RecognizeObjects(recog_service_name, recog_model_name, - preprocess_input_module), - transitions={Constant.SUCCESS: 'SET_ACTION_LIB_SUCCESS', - Constant.FAILURE: 'SET_ACTION_LIB_FAILURE'}) - - smach.StateMachine.add('SET_ACTION_LIB_SUCCESS', SetActionLibResult(True), - transitions={Constant.SUCCESS: 'OVERALL_SUCCESS'}) - - smach.StateMachine.add('SET_ACTION_LIB_FAILURE', SetActionLibResult(False), - transitions={Constant.SUCCESS: 'OVERALL_FAILED'}) - - -def main(): - rospy.init_node('~perceive_plane_server') - # detection - detection_action_name = rospy.get_param('~detection_action_name', '') - if not detection_action_name: - raise RuntimeError('no detection action server specified') - - # recognition - recognition_service_name = rospy.get_param('~recognition_service_name', '/mdr_perception/recognize_image') - recognition_model_name = rospy.get_param('~recognition_model_name', 'athome_xception_v1') - preprocess_input_module = rospy.get_param('~preprocess_input_module', None) - - target_frame = rospy.get_param("~target_frame") - - # Construct state machine - sm = PerceivePlaneSkill(detection_action_name, target_frame, recognition_service_name, recognition_model_name, - preprocess_input_module) - - # smach viewer - sis = IntrospectionServer('perceive_plane_smach_viewer', sm, '/PERCEIVE_PLANE_SMACH_VIEWER') - sis.start() - - # Construct action server wrapper - asw = ActionServerWrapper( - server_name='perceive_plane_server', - action_spec=PerceivePlaneAction, - wrapped_container=sm, - succeeded_outcomes=['OVERALL_SUCCESS'], - aborted_outcomes=['OVERALL_FAILED'], - preempted_outcomes=['PREEMPTED'], - goal_key='perceive_plane_goal', - feedback_key='perceive_plane_feedback', - result_key='perceive_plane_result') - - # Run the server in a background thread - asw.run_server() - rospy.spin() +import actionlib +from mdr_perceive_plane_action.msg import PerceivePlaneAction +from mdr_perceive_plane_action.action_states import PerceivePlaneSM + +class PerceivePlaneServer(object): + '''A server exposing an action for detecting objects on a plane. + + The server expects the following parameters to be made available on the ROS parameter server: + * detection_action_name: Name of an action server used for object detection + * recognition_service_name: Name of an image recognition service for classifying object + * recognition_model_name: Name of an image classification model + * preprocess_input_module: Name of a module containing an image preprocessing function + to be executed on images before inference + * target_frame: Name of the reference frame the object and plane poses will be + transformed to (default: '/base_link') + + @author Minh Nguyen, Alex Mitrevski + @contact minh.nguyen@smail.inf.h-brs.de, aleksandar.mitrevski@h-brs.de + + ''' + def __init__(self): + detection_action_name = rospy.get_param('~detection_action_name', '/base_link') + if not detection_action_name: + raise RuntimeError('[perceive_plane] no detection action server specified') + recognition_service_name = rospy.get_param('~recognition_service_name', '/mdr_perception/recognize_image') + recognition_model_name = rospy.get_param('~recognition_model_name', 'athome_xception_v1') + preprocess_input_module = rospy.get_param('~preprocess_input_module', None) + target_frame = rospy.get_param('~target_frame', '') + + rospy.loginfo('[perceive_plane] Initialising state machine') + self.action_sm = PerceivePlaneSM(detection_service_proxy=detection_action_name, + recog_service_name=recognition_service_name, + recog_model_name=recognition_model_name, + preprocess_input_module=preprocess_input_module, + target_frame=target_frame) + rospy.loginfo('[perceive_plane] State machine initialised') + + self.action_server = actionlib.SimpleActionServer('perceive_plane_server', + PerceivePlaneAction, + self.execute, False) + self.action_server.start() + rospy.loginfo('perceive_plane action server ready') + + def execute(self, goal): + rospy.loginfo('[perceive_plane] Received an action request') + self.action_sm.goal = goal + self.action_sm.execution_requested = True + while self.action_sm.result is None: + rospy.sleep(0.05) + self.action_server.set_succeeded(self.action_sm.result) if __name__ == '__main__': - main() + rospy.init_node('perceive_plane_server') + perceive_plane_server = PerceivePlaneServer() + try: + perceive_plane_server.action_sm.run() + while perceive_plane_server.action_sm.is_running and not rospy.is_shutdown(): + rospy.spin() + except (KeyboardInterrupt, SystemExit): + print('{0} interrupted; exiting...'.format(perceive_plane_server.action_sm.name)) + perceive_plane_server.action_sm.stop() diff --git a/mdr_planning/mdr_actions/mdr_perception_actions/mdr_perceive_plane_action/ros/src/mdr_perceive_plane_action/action_states.py b/mdr_planning/mdr_actions/mdr_perception_actions/mdr_perceive_plane_action/ros/src/mdr_perceive_plane_action/action_states.py index 06c33a72b..3b6b004fd 100644 --- a/mdr_planning/mdr_actions/mdr_perception_actions/mdr_perceive_plane_action/ros/src/mdr_perceive_plane_action/action_states.py +++ b/mdr_planning/mdr_actions/mdr_perception_actions/mdr_perceive_plane_action/ros/src/mdr_perceive_plane_action/action_states.py @@ -1,38 +1,37 @@ #!/usr/bin/python import rospy -import smach -import std_msgs.msg import sensor_msgs.msg -from mdr_perceive_plane_action.msg import PerceivePlaneResult, PerceivePlaneFeedback +from pyftsm.ftsm import FTSMTransitions +from mas_execution.action_sm_base import ActionSMBase from mcr_perception_msgs.msg import PlaneList -from mas_perception_libs import ObjectDetector, Constant, ImageRecognitionServiceProxy - +from mas_perception_libs import ObjectDetector, ImageRecognitionServiceProxy +from mdr_perceive_plane_action.msg import PerceivePlaneResult, PerceivePlaneFeedback -class DetectObjects(smach.State): - def __init__(self, detection_service_proxy, target_frame=None, timeout_duration=1): - smach.State.__init__(self, outcomes=[Constant.SUCCESS, Constant.FAILURE, Constant.TIMEOUT], - input_keys=['perceive_plane_goal', 'perceive_plane_feedback'], - output_keys=['detected_planes', 'perceive_plane_feedback']) +class PerceivePlaneSM(ActionSMBase): + def __init__(self, detection_service_proxy, + recog_service_name, + recog_model_name, + preprocess_input_module, + target_frame=None, + timeout_duration=1, + max_recovery_attempts=1): + super(PerceivePlaneSM, self).__init__('PerceivePlane', [], max_recovery_attempts) self._detector = ObjectDetector(detection_service_proxy) + self._recog_service_proxy = ImageRecognitionServiceProxy(recog_service_name, + recog_model_name, + preprocess_input_module) + self._image_pub = rospy.Publisher('/first_recognized_image', + sensor_msgs.msg.Image, + queue_size=1) self._timeout_duration = timeout_duration - self._detecting_done = False self._target_frame = target_frame - - def _detection_cb(self): - self._detecting_done = True - - def execute(self, ud): - if 'perceive_plane_feedback' not in ud: - ud.perceive_plane_feedback = PerceivePlaneFeedback() - ud.perceive_plane_feedback.current_state = 'detect_objects' - self._detecting_done = False - ud.detected_planes = None - if 'perceive_plane_goal' not in ud: - rospy.logerr('no goal passed into DetectObjects state') - return Constant.FAILURE - self._detector.start_detect_objects(ud.perceive_plane_goal.plane_frame_prefix, self._detection_cb, + def running(self): + detected_planes = None + self._detecting_done = False + self._detector.start_detect_objects(self.goal.plane_frame_prefix, + self._detection_cb, self._target_frame) timeout = rospy.Duration.from_sec(self._timeout_duration) @@ -41,35 +40,19 @@ def execute(self, ud): while (rospy.Time.now() - start_time) < timeout: if self._detecting_done: if self._detector.plane_list is None: - return Constant.FAILURE - - ud.detected_planes = self._detector.plane_list - return Constant.SUCCESS + rospy.logerr('[perceive_plane] No planes found') + self.result = self.set_result(False, detected_planes) + return FTSMTransitions.DONE + detected_planes = self._detector.plane_list rate.sleep() - return Constant.TIMEOUT - - -class RecognizeObjects(smach.State): - def __init__(self, recog_service_name, recog_model_name, preprocess_input_module): - smach.State.__init__(self, outcomes=[Constant.SUCCESS, Constant.FAILURE], - input_keys=['detected_planes', 'perceive_plane_feedback'], - output_keys=['recognized_planes', 'perceive_plane_feedback']) - - self._service_proxy = ImageRecognitionServiceProxy(recog_service_name, recog_model_name, - preprocess_input_module) - self._image_pub = rospy.Publisher('/first_recognized_image', sensor_msgs.msg.Image, queue_size=1) - - def execute(self, ud): - if 'perceive_plane_feedback' not in ud: - ud.perceive_plane_feedback = PerceivePlaneFeedback() - ud.perceive_plane_feedback.current_state = 'recognize_objects' - - if 'detected_planes' not in ud: - ud.recognized_planes = None - return Constant.FAILURE + if not self._detecting_done: + rospy.logerr('[perceive_plane] A plane could not be found within the alloted time') + self.result = self.set_result(False, detected_planes) + return FTSMTransitions.DONE - for plane in ud.detected_planes.planes: + rospy.loginfo('[perceive_plane] Found %d objects', len(detected_planes.planes)) + for plane in detected_planes.planes: image_messages = [] for obj in plane.object_list.objects: image_messages.append(obj.rgb_image) @@ -79,90 +62,28 @@ def execute(self, ud): if len(indices) > 0: obj_index = indices[0] self._image_pub.publish(image_messages[obj_index]) - rospy.loginfo('first object found: {0} (prob: {1})'.format(classes[obj_index], probs[obj_index])) + rospy.loginfo('[perceive_plane] first object found: {0} (prob: {1})'.format(classes[obj_index], + probs[obj_index])) else: - rospy.logwarn('no object recognized for plane ' + plane.name) + rospy.logwarn('[perceive_plane] no objects recognized for plane %s', plane.name) for i in indices: plane.object_list.objects[i].name = classes[i] # TODO: handle categories plane.object_list.objects[i].category = classes[i] - ud.recognized_planes = ud.detected_planes - return Constant.SUCCESS + self.result = self.set_result(True, detected_planes) + return FTSMTransitions.DONE + def _detection_cb(self): + self._detecting_done = True -class SetupPlaneConfig(smach.State): - def __init__(self, sleep_duration=1): - smach.State.__init__(self, outcomes=[Constant.SUCCESS, Constant.WAITING, Constant.FAILURE, Constant.TIMEOUT], - input_keys=['perceive_plane_goal'], - output_keys=['perceive_plane_feedback']) - self.sleep_duration = sleep_duration - - self.config_name_pub = rospy.Publisher("/mcr_common/dynamic_reconfigure_client/configuration_name", - std_msgs.msg.String, queue_size=1) - self.event_in_pub = rospy.Publisher("/mcr_common/dynamic_reconfigure_client/event_in", - std_msgs.msg.String, queue_size=1) - self.event_out_sub = rospy.Subscriber("/mcr_common/dynamic_reconfigure_client/event_out", - std_msgs.msg.String, self.event_cb) - self.event = None - - def event_cb(self, msg): - self.event = msg.data - - def configure_plane(self, config_name): - """ - :param config_name: configurations name listed in - config/perceive_plane_configurations.yaml - """ - self.event = None - self.config_name_pub.publish(config_name) - self.event_in_pub.publish("e_start") - - timeout = rospy.Duration.from_sec(1.0) - rate = rospy.Rate(10) - start_time = rospy.Time.now() - while (rospy.Time.now() - start_time) < timeout: - if self.event: - if self.event == Constant.E_SUCCESS: - return Constant.SUCCESS - return Constant.FAILURE - rate.sleep() - return Constant.TIMEOUT - - def execute(self, ud): - feedback = PerceivePlaneFeedback() - feedback.current_state = 'setup_plane_config' - if 'perceive_plane_goal' in ud: - feedback.message = '[perceive_plane] received plane config goal ' +\ - ud.perceive_plane_goal.plane_config - ud.perceive_plane_feedback = feedback - return self.configure_plane(ud.perceive_plane_goal.plane_config) - - feedback.message = '[perceive_plane] waiting for plane config goal' - ud.perceive_plane_feedback = feedback - rospy.sleep(self.sleep_duration) - return Constant.WAITING - - -class SetActionLibResult(smach.State): - def __init__(self, result): - smach.State.__init__(self, outcomes=[Constant.SUCCESS], - input_keys=['perceive_plane_goal', 'recognized_planes', 'perceive_plane_feedback'], - output_keys=['perceive_plane_feedback', 'perceive_plane_result']) - self.result = result - - def execute(self, ud): - if 'perceive_plane_feedback' not in ud: - ud.perceive_plane_feedback = PerceivePlaneFeedback() - ud.perceive_plane_feedback.current_state = 'set_action_lib_result' - + def set_result(self, success, recognized_planes): result = PerceivePlaneResult() - result.success = self.result - if ud.recognized_planes is None: + result.success = success + if recognized_planes is None: result.recognized_planes = PlaneList() result.recognized_planes.planes = [] else: - result.recognized_planes = ud.recognized_planes - ud.perceive_plane_result = result - return Constant.SUCCESS + result.recognized_planes = recognized_planes + return result diff --git a/mdr_planning/mdr_actions/mdr_perception_actions/mdr_recognize_emotion_action/ros/scripts/recognize_emotion_action b/mdr_planning/mdr_actions/mdr_perception_actions/mdr_recognize_emotion_action/ros/scripts/recognize_emotion_action index f604ad899..c201b2e1b 100755 --- a/mdr_planning/mdr_actions/mdr_perception_actions/mdr_recognize_emotion_action/ros/scripts/recognize_emotion_action +++ b/mdr_planning/mdr_actions/mdr_perception_actions/mdr_recognize_emotion_action/ros/scripts/recognize_emotion_action @@ -1,73 +1,56 @@ #!/usr/bin/env python import rospy -import smach - -from smach_ros import ActionServerWrapper, IntrospectionServer +import actionlib from mdr_recognize_emotion_action.msg import RecognizeEmotionAction -from mdr_recognize_emotion_action.action_states import (SetupRecognizeEmotion, RecognizeEmotion, - SetActionLibResult) +from mdr_recognize_emotion_action.action_states import RecognizeEmotionSM + +class RecognizeEmotionServer(object): + '''A server exposing an action for recognizing the emotions of previously detected people. + The server expects the following parameters to be made available on the ROS parameter server: + * emotion_model_path: Absolute path of an emotion recognition model (default '') + * image_topic: Name of a camera image topic (default '/cam3d/rgb/image_raw') -class RecognizeEmotionSkill(smach.StateMachine): - def __init__(self, timeout=10): - smach.StateMachine.__init__(self, - outcomes=['OVERALL_SUCCESS', - 'OVERALL_FAILED', 'PREEMPTED'], - input_keys=['recognize_emotion_goal'], - output_keys=['recognize_emotion_feedback', - 'recognize_emotion_result']) + @author Alex Mitrevski, Octavio Arriaga + @contact aleksandar.mitrevski@h-brs.de + ''' + def __init__(self): emotion_model_path = rospy.get_param('~emotion_model_path', '') image_topic = rospy.get_param('~image_topic', '/cam3d/rgb/image_raw') labels = {0: 'angry', 1: 'disgusted', 2: 'sad', 3: 'happy', 4: 'sad', 5: 'surprised', 6: 'neutral'} image_size = (48, 48, 1) - with self: - smach.StateMachine.add('SETUP_RECOGNIZE_EMOTION', - SetupRecognizeEmotion(), - transitions={'succeeded': 'RECOGNIZE_EMOTION', - 'failed': 'SETUP_RECOGNIZE_EMOTION'}) - - smach.StateMachine.add('RECOGNIZE_EMOTION', - RecognizeEmotion(emotion_model_path=emotion_model_path, - image_topic=image_topic, - labels=labels, image_size=image_size), - transitions={'succeeded': 'SET_ACTION_LIB_SUCCESS', - 'failed': 'SET_ACTION_LIB_FAILED'}) - - smach.StateMachine.add('SET_ACTION_LIB_FAILED', - SetActionLibResult(False), - transitions={'succeeded': 'OVERALL_FAILED'}) - - smach.StateMachine.add('SET_ACTION_LIB_SUCCESS', - SetActionLibResult(True), - transitions={'succeeded': 'OVERALL_SUCCESS'}) - + rospy.loginfo('[recognize_emotion] Initialising state machine') + self.action_sm = RecognizeEmotionSM(emotion_model_path=emotion_model_path, + image_topic=image_topic, + labels=labels, + image_size=image_size) + rospy.loginfo('[recognize_emotion] State machine initialised') + + self.action_server = actionlib.SimpleActionServer('recognize_emotion_server', + RecognizeEmotionAction, + self.execute, False) + self.action_server.start() + rospy.loginfo('recognize_emotion action server ready') + + def execute(self, goal): + rospy.loginfo('[recognize_emotion] Received an action request') + self.action_sm.goal = goal + self.action_sm.execution_requested = True + while self.action_sm.result is None: + rospy.sleep(0.05) + self.action_server.set_succeeded(self.action_sm.result) if __name__ == '__main__': rospy.init_node('recognize_emotion_server') - - # construct state machine - sm = RecognizeEmotionSkill() - - # smach viewer - sis = IntrospectionServer('recognize_emotion_smach_viewer', sm, - '/recognize_emotion_SMACH_VIEWER') - sis.start() - - asw = ActionServerWrapper( - server_name='recognize_emotion_server', - action_spec=RecognizeEmotionAction, - wrapped_container=sm, - succeeded_outcomes=['OVERALL_SUCCESS'], - aborted_outcomes=['OVERALL_FAILED'], - preempted_outcomes=['PREEMPTED'], - goal_key='recognize_emotion_goal', - feedback_key='recognize_emotion_feedback', - result_key='recognize_emotion_result') - - # Run the server in a background thread - asw.run_server() - rospy.spin() + recognize_emotion_server = RecognizeEmotionServer() + try: + recognize_emotion_server.action_sm.run() + while recognize_emotion_server.action_sm.is_running and not rospy.is_shutdown(): + rospy.spin() + except (KeyboardInterrupt, SystemExit): + print('{0} interrupted; exiting...'.format(recognize_emotion_server.action_sm.name)) + recognize_emotion_server.action_sm.stop() diff --git a/mdr_planning/mdr_actions/mdr_perception_actions/mdr_recognize_emotion_action/ros/src/mdr_recognize_emotion_action/action_states.py b/mdr_planning/mdr_actions/mdr_perception_actions/mdr_recognize_emotion_action/ros/src/mdr_recognize_emotion_action/action_states.py index 280920551..572a6dd16 100644 --- a/mdr_planning/mdr_actions/mdr_perception_actions/mdr_recognize_emotion_action/ros/src/mdr_recognize_emotion_action/action_states.py +++ b/mdr_planning/mdr_actions/mdr_perception_actions/mdr_recognize_emotion_action/ros/src/mdr_recognize_emotion_action/action_states.py @@ -1,9 +1,5 @@ #!/usr/bin/python - import rospy -import smach -import smach_ros -import actionlib from sensor_msgs.msg import Image import numpy as np @@ -12,67 +8,57 @@ from keras.models import load_model from cv_bridge import CvBridge, CvBridgeError -from mdr_recognize_emotion_action.msg import (RecognizeEmotionFeedback, - RecognizeEmotionResult) - - -class SetupRecognizeEmotion(smach.State): - def __init__(self): - smach.State.__init__(self, outcomes=['succeeded', 'failed'], - input_keys=['recognize_emotion_goal'], - output_keys=['recognize_emotion_feedback', - 'recognize_emotion_result']) - - def execute(self, userdata): - feedback = RecognizeEmotionFeedback() - feedback.current_state = 'RECOGNIZE_EMOTION' - feedback.message = '[recognize_emotion] Recongnizing emotion' - userdata.recognize_emotion_feedback = feedback - return 'succeeded' - - -class RecognizeEmotion(smach.State): - def __init__(self, timeout=120.0, image_topic='/cam3d/rgb/image_raw', - emotion_model_path=None, labels=dict(), image_size=(0, 0, 0)): - smach.State.__init__(self, input_keys=['recognize_emotion_goal', - 'emotions'], - output_keys=['recognize_emotion_feedback', - 'emotions'], - outcomes=['succeeded', 'failed']) +from pyftsm.ftsm import FTSMTransitions +from mas_execution.action_sm_base import ActionSMBase +from mdr_recognize_emotion_action.msg import RecognizeEmotionFeedback, RecognizeEmotionResult + +class RecognizeEmotionSM(ActionSMBase): + def __init__(self, timeout=120.0, + image_topic='/cam3d/rgb/image_raw', + emotion_model_path=None, + labels=None, + image_size=(0, 0, 0), + max_recovery_attempts=1): + super(RecognizeEmotionSM, self).__init__('RecognizeEmotion', [], max_recovery_attempts) self.timeout = timeout self.labels = labels self.image_size = image_size self.image_publisher = rospy.Publisher(image_topic, Image, queue_size=1) + self.emotion_model_path = emotion_model_path self.bridge = CvBridge() + self.emotion_model = None + self.computation_graph = None + def init(self): try: + rospy.loginfo('[recognize_emotion] Loading model %s', self.emotion_model_path) self.emotion_model = load_model(emotion_model_path) - print('Model ' + emotion_model_path + ' loaded successfully') - # the following two lines (and the with... line in predict_emotion) - # are necessary for avoiding + # the following two lines are necessary for avoiding # https://github.com/keras-team/keras/issues/2397 self.emotion_model._make_predict_function() self.computation_graph = tf.get_default_graph() except: - print('Failed to load model ', emotion_model_path) + rospy.logerr('[recognize_emotion] Model %s could not be loaded', self.emotion_model_path) + return FTSMTransitions.INITIALISED - def execute(self, userdata): - number_of_faces = userdata.recognize_emotion_goal.number_of_faces - bounding_boxes = userdata.recognize_emotion_goal.bounding_boxes - userdata.emotions = list() + def running(self): + number_of_faces = self.goal.number_of_faces + bounding_boxes = self.goal.bounding_boxes + emotions = [] - rgb_image = self.ros2cv(userdata.recognize_emotion_goal.image) - gray_image = self.rgb2gray(rgb_image) + rospy.loginfo('[recognize_emotion] Recognizing emotions') + rgb_image = self.__ros2cv(self.goal.image) + gray_image = self.__rgb2gray(rgb_image) for face in bounding_boxes: x, y, w, h = face.bounding_box_coordinates face = gray_image[y: (y + h), x: (x + w)] face = cv2.resize(face, self.image_size[0:2]) face = np.expand_dims(face, 0) face = np.expand_dims(face, -1) - face = self.preprocess_image(face) - predicted_emotion = self.predict_emotion(face) - userdata.emotions.append(predicted_emotion) + face = self.__preprocess_image(face) + predicted_emotion = self.__predict_emotion(face) + emotions.append(predicted_emotion) rgb_cv2 = cv2.rectangle(rgb_image, (x, y), (x + w, y + h), (0, 0, 255), 2) @@ -81,39 +67,31 @@ def execute(self, userdata): 1, cv2.LINE_AA) output_ros_image = self.bridge.cv2_to_imgmsg(rgb_image, 'bgr8') self.image_publisher.publish(output_ros_image) - return 'succeeded' - def preprocess_image(self, image): + self.result = self.set_result(True, emotions) + return FTSMTransitions.DONE + + def set_result(self, success, emotions): + result = RecognizeEmotionResult() + result.success = success + result.emotions = emotions + return result + + def __preprocess_image(self, image): image = image / 255.0 return image - def predict_emotion(self, face): + def __predict_emotion(self, face): label = -1 with self.computation_graph.as_default(): class_predictions = self.emotion_model.predict(face) label = self.labels[np.argmax(class_predictions)] return label - def ros2cv(self, ros_image): + def __ros2cv(self, ros_image): cv_image = self.bridge.imgmsg_to_cv2(ros_image, 'bgr8') return np.array(cv_image, dtype=np.uint8) - def rgb2gray(self, image): + def __rgb2gray(self, image): gray_image = cv2.cvtColor(image, cv2.COLOR_BGR2GRAY) return gray_image - - -class SetActionLibResult(smach.State): - def __init__(self, result): - smach.State.__init__(self, outcomes=['succeeded'], - input_keys=['recognize_emotion_goal', 'emotions'], - output_keys=['recognize_emotion_feedback', - 'recognize_emotion_result']) - self.result = result - - def execute(self, userdata): - result = RecognizeEmotionResult() - result.success = self.result - result.emotions = userdata.emotions - userdata.recognize_emotion_result = result - return 'succeeded' From 3f3412ba74ae49535c9c2aaf02dcb3851b6633ee Mon Sep 17 00:00:00 2001 From: Alex Mitrevski Date: Fri, 25 Jan 2019 16:16:56 +0100 Subject: [PATCH 50/96] The find object action is now also implemented using a fault-tolerant state machine --- .../ros/scripts/find_object_action | 101 +++++++------ .../mdr_find_object_action/action_states.py | 133 ++++++++++-------- 2 files changed, 123 insertions(+), 111 deletions(-) diff --git a/mdr_planning/mdr_actions/mdr_perception_actions/mdr_find_object_action/ros/scripts/find_object_action b/mdr_planning/mdr_actions/mdr_perception_actions/mdr_find_object_action/ros/scripts/find_object_action index 9ba77f665..22494a9fa 100755 --- a/mdr_planning/mdr_actions/mdr_perception_actions/mdr_find_object_action/ros/scripts/find_object_action +++ b/mdr_planning/mdr_actions/mdr_perception_actions/mdr_find_object_action/ros/scripts/find_object_action @@ -1,64 +1,63 @@ #!/usr/bin/env python import rospy -import smach -from smach_ros import ActionServerWrapper, IntrospectionServer +import actionlib + from mdr_find_object_action.msg import FindObjectAction -from mdr_find_object_action.action_states import FindObject, SetActionLibResult +from mdr_find_object_action.action_states import FindObjectSM -class FindObjectSkill(smach.StateMachine): - def __init__(self): - smach.StateMachine.__init__(self, outcomes=['OVERALL_SUCCESS', 'OVERALL_FAILED', 'PREEMPTED'], - input_keys=['find_object_goal'], - output_keys=['find_object_feedback', 'find_object_result']) +class FindObjectServer(object): + '''A server exposing an action for locating objects. + + The server expects the following parameters to be made available on the ROS parameter server: + * ontology_url: URL of a domestic ontology file + * ontology_class_prefix: Prefix of the classes in the ontology + * retry_count_on_failure: Number of times the search should be retried + if it fails the first time + * timeout_s: Timeout (in seconds) for the action execution + + @author Alex Mitrevski + @contact aleksandar.mitrevski@h-brs.de + ''' + def __init__(self): ontology_url = rospy.get_param('~ontology_url', '') ontology_class_prefix = rospy.get_param('~ontology_class_prefix', '') - retry_count_on_failure = int(rospy.get_param('~retry_count_on_failure', '0')) - timeout_s = float(rospy.get_param('~timeout_s', '120.')) + retry_count_on_failure = int(rospy.get_param('~retry_count_on_failure', 0)) + timeout_s = float(rospy.get_param('~timeout_s', 120.)) if not ontology_url: - rospy.logerr('[FIND_OBJECT] Ontology url not specified') + rospy.logerr('[find_object] Ontology url not specified') if not ontology_class_prefix: - rospy.logerr('[FIND_OBJECT] Ontology class prefix not specified') - - with self: - smach.StateMachine.add('FIND_OBJECT', FindObject(ontology_url, - ontology_class_prefix, - retry_count_on_failure, - timeout_s), - transitions={'succeeded': 'SET_ACTION_LIB_SUCCESS', - 'failed': 'SET_ACTION_LIB_FAILURE', - 'timeout': 'SET_ACTION_LIB_FAILURE'}) - - smach.StateMachine.add('SET_ACTION_LIB_SUCCESS', SetActionLibResult(True), - transitions={'succeeded': 'OVERALL_SUCCESS'}) - - smach.StateMachine.add('SET_ACTION_LIB_FAILURE', SetActionLibResult(False), - transitions={'succeeded': 'OVERALL_FAILED'}) - + rospy.logerr('[find_object] Ontology class prefix not specified') + + rospy.loginfo('[find_object] Initialising state machine') + self.action_sm = FindObjectSM(ontology_url=ontology_url, + ontology_class_prefix=ontology_class_prefix, + number_of_retries=retry_count_on_failure, + timeout=timeout_s) + rospy.loginfo('[find_object] State machine initialised') + + self.action_server = actionlib.SimpleActionServer('find_object_server', + FindObjectAction, + self.execute, False) + self.action_server.start() + rospy.loginfo('find_object action server ready') + + def execute(self, goal): + rospy.loginfo('[find_object] Received an action request') + self.action_sm.goal = goal + self.action_sm.execution_requested = True + while self.action_sm.result is None: + rospy.sleep(0.05) + self.action_server.set_succeeded(self.action_sm.result) if __name__ == '__main__': rospy.init_node('find_object_server') - - # Construct state machine - sm = FindObjectSkill() - - # smach viewer - sis = IntrospectionServer('find_object_smach_viewer', sm, '/FIND_OBJECT_SMACH_VIEWER') - sis.start() - - # Construct action server wrapper - asw = ActionServerWrapper( - server_name='find_object_server', - action_spec=FindObjectAction, - wrapped_container=sm, - succeeded_outcomes=['OVERALL_SUCCESS'], - aborted_outcomes=['OVERALL_FAILED'], - preempted_outcomes=['PREEMPTED'], - goal_key='find_object_goal', - feedback_key='find_object_feedback', - result_key='find_object_result') - - # Run the server on a background thread - asw.run_server() - rospy.spin() + find_object_server = FindObjectServer() + try: + find_object_server.action_sm.run() + while find_object_server.action_sm.is_running and not rospy.is_shutdown(): + rospy.spin() + except (KeyboardInterrupt, SystemExit): + print('{0} interrupted; exiting...'.format(find_object_server.action_sm.name)) + find_object_server.action_sm.stop() diff --git a/mdr_planning/mdr_actions/mdr_perception_actions/mdr_find_object_action/ros/src/mdr_find_object_action/action_states.py b/mdr_planning/mdr_actions/mdr_perception_actions/mdr_find_object_action/ros/src/mdr_find_object_action/action_states.py index 3bf257804..d4a67adbb 100644 --- a/mdr_planning/mdr_actions/mdr_perception_actions/mdr_find_object_action/ros/src/mdr_find_object_action/action_states.py +++ b/mdr_planning/mdr_actions/mdr_perception_actions/mdr_find_object_action/ros/src/mdr_find_object_action/action_states.py @@ -1,93 +1,106 @@ #!/usr/bin/env python import rospy -import smach +from pyftsm.ftsm import FTSMTransitions +from mas_execution.action_sm_base import ActionSMBase from mas_knowledge_utils.domestic_ontology_interface import DomesticOntologyInterface from mas_knowledge_base.domestic_kb_interface import DomesticKBInterface -from mdr_find_object_action.msg import FindObjectGoal, FindObjectResult, FindObjectFeedback - -class FindObject(smach.State): - def __init__(self, ontology_url, ontology_class_prefix, - number_of_retries=0, timeout=120.): - smach.State.__init__(self, outcomes=['succeeded', 'failed', 'timeout'], - input_keys=['find_object_goal'], - output_keys=['find_object_feedback', - 'obj_location', 'relation']) - self.ontology_interface = DomesticOntologyInterface(ontology_url, - ontology_class_prefix) - self.kb_interface = DomesticKBInterface() +from mdr_find_object_action.msg import FindObjectGoal, FindObjectResult +class FindObjectSM(ActionSMBase): + def __init__(self, ontology_url, + ontology_class_prefix, + number_of_retries=0, + timeout=120., + max_recovery_attempts=1): + super(FindObjectSM, self).__init__('FindObject', [], max_recovery_attempts) + self.ontology_url = ontology_url + self.ontology_class_prefix = ontology_class_prefix self.number_of_retries = number_of_retries self.timeout = timeout + self.ontology_interface = None + self.kb_interface = None + + def init(self): + try: + rospy.loginfo('[find_object] Creating an interface client for ontology %s', self.ontology_url) + self.ontology_interface = DomesticOntologyInterface(self.ontology_url, + self.ontology_class_prefix) + except Exception as exc: + rospy.logerr('[find_object] Could not create an ontology interface client: %s', exc) + + try: + rospy.loginfo('[find_object] Creating a knowledge base interface client') + self.kb_interface = DomesticKBInterface() + except Exception as exc: + rospy.logerr('[find_object] Could not create a knowledge base interface client: %s', exc) + return FTSMTransitions.INITIALISED - def execute(self, userdata): - if userdata.find_object_goal.goal_type == FindObjectGoal.NAMED_OBJECT: - obj_name = userdata.find_object_goal.object_name + def running(self): + obj_name = None + obj_location = None + relation = None + if self.goal.goal_type == FindObjectGoal.NAMED_OBJECT: + obj_name = self.goal.object_name location, predicate = self.kb_interface.get_object_location(obj_name) if location: - rospy.loginfo('[FIND_OBJECT] Found %s %s %s', obj_name, predicate, location) + rospy.loginfo('[find_object] Found %s %s %s', obj_name, predicate, location) # TODO: verify that the object is still at that location - userdata.obj_location = location - userdata.relation = predicate - return 'succeeded' + obj_location = location + relation = predicate + self.result = self.set_result(True, obj_location, relation) + return FTSMTransitions.DONE - rospy.loginfo('[FIND_OBJECT] %s not found in the knowledge base; querying the ontology', obj_name) + rospy.loginfo('[find_object] %s not found in the knowledge base; querying the ontology', obj_name) location = self.ontology_interface.get_default_storing_location(obj_name=obj_name) if location: predicate = 'in' - rospy.loginfo('[FIND_OBJECT] %s is usually %s %s', obj_name, predicate, location) + rospy.loginfo('[find_object] %s is usually %s %s', obj_name, predicate, location) # TODO: check if the object is at the default location - userdata.obj_location = location - userdata.relation = predicate - return 'succeeded' + obj_location = location + relation = predicate + self.result = self.set_result(True, obj_location, relation) + return FTSMTransitions.DONE - rospy.loginfo('[FIND_OBJECT] %s not found', obj_name) - userdata.obj_location = None - userdata.relation = None - return 'failed' - elif userdata.find_object_goal.goal_type == FindObjectGoal.OBJECT_CATEGORY: - obj_category = userdata.find_object_goal.object_name + rospy.loginfo('[find_object] %s not found', obj_name) + obj_location = None + relation = None + self.result = self.set_result(True, obj_location, relation) + return FTSMTransitions.DONE + elif self.goal.goal_type == FindObjectGoal.OBJECT_CATEGORY: + obj_category = self.goal.object_name category_objects = self.kb_interface.get_category_objects(obj_category) if category_objects: for obj_name in category_objects: location, predicate = self.kb_interface.get_object_location(obj_name) if location: - rospy.loginfo('[FIND_OBJECT] Found %s %s %s', obj_name, predicate, location) + rospy.loginfo('[find_object] Found %s %s %s', obj_name, predicate, location) # TODO: verify that the object is still at that location - userdata.obj_location = location - userdata.relation = predicate - return 'succeeded' + obj_location = location + relation = predicate + self.result = self.set_result(True, obj_location, relation) + return FTSMTransitions.DONE else: - rospy.loginfo('[FIND_OBJECT] The location of %s is unknown', obj_name) + rospy.loginfo('[find_object] The location of %s is unknown', obj_name) else: - rospy.loginfo('[FIND_OBJECT] No object of category %s was found in the knowledge base; querying the ontology', obj_category) + rospy.loginfo('[find_object] No object of category %s was found in the knowledge base; querying the ontology', obj_category) location = self.ontology_interface.get_default_storing_location(obj_category=obj_category) if location: predicate = 'in' - rospy.loginfo('[FIND_OBJECT] Objects of %s are usually %s %s', obj_category, predicate, location) + rospy.loginfo('[find_object] Objects of %s are usually %s %s', obj_category, predicate, location) # TODO: check if an object of the desired category is at the default location - userdata.obj_location = location - userdata.relation = predicate - return 'succeeded' - - return 'failed' - -class SetActionLibResult(smach.State): - def __init__(self, result): - smach.State.__init__(self, outcomes=['succeeded'], - input_keys=['find_object_goal', 'find_object_feedback', - 'obj_location', 'relation'], - output_keys=['find_object_feedback', 'find_object_result']) - self.result = result + obj_location = location + relation = predicate + self.result = self.set_result(True, obj_location, relation) + return FTSMTransitions.DONE - def execute(self, userdata): - if 'find_object_feedback' not in userdata: - userdata.find_object_feedback = FindObjectFeedback() - userdata.find_object_feedback.current_state = 'set_action_lib_result' + rospy.logerr('[find_object] Object of category %s could not be found', obj_category) + self.result = self.set_result(False, obj_location, relation) + return FTSMTransitions.DONE + def set_result(self, success, obj_location, relation): result = FindObjectResult() - result.success = self.result - result.object_location = userdata.obj_location - result.relation = userdata.relation - userdata.find_object_result = result - return 'succeeded' + result.success = success + result.object_location = obj_location + result.relation = relation + return result From 879afea7a2cef6c2c87e5d7efc8c0b890dabd9da Mon Sep 17 00:00:00 2001 From: Alex Mitrevski Date: Sat, 26 Jan 2019 18:04:32 +0100 Subject: [PATCH 51/96] [perceive_plane_action] I'd not renamed one instance of a member variable during the FTSM migration --- .../ros/src/mdr_perceive_plane_action/action_states.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/mdr_planning/mdr_actions/mdr_perception_actions/mdr_perceive_plane_action/ros/src/mdr_perceive_plane_action/action_states.py b/mdr_planning/mdr_actions/mdr_perception_actions/mdr_perceive_plane_action/ros/src/mdr_perceive_plane_action/action_states.py index 3b6b004fd..dc5ceaddd 100644 --- a/mdr_planning/mdr_actions/mdr_perception_actions/mdr_perceive_plane_action/ros/src/mdr_perceive_plane_action/action_states.py +++ b/mdr_planning/mdr_actions/mdr_perception_actions/mdr_perceive_plane_action/ros/src/mdr_perceive_plane_action/action_states.py @@ -56,7 +56,7 @@ def running(self): image_messages = [] for obj in plane.object_list.objects: image_messages.append(obj.rgb_image) - indices, classes, probs = self._service_proxy.classify_image_messages(image_messages) + indices, classes, probs = self._recog_service_proxy.classify_image_messages(image_messages) # TODO: debug output if len(indices) > 0: From ead467e1a7fb06bff6bf4f27e03ff4947e2ec50c Mon Sep 17 00:00:00 2001 From: Alex Mitrevski Date: Sat, 26 Jan 2019 20:21:45 +0100 Subject: [PATCH 52/96] [actions] Fixed the handling of new goals to avoid race conditions --- .../mdr_move_arm_action/ros/scripts/move_arm_action | 3 ++- .../mdr_pickup_action/ros/scripts/pickup_action | 3 ++- .../mdr_place_action/ros/scripts/place_action | 3 ++- .../mdr_enter_door_action/ros/scripts/enter_door_action | 3 ++- .../mdr_move_base_action/ros/scripts/move_base_action | 3 ++- .../mdr_move_forward_action/ros/scripts/move_forward_action | 3 ++- .../mdr_turn_base_to_action/ros/scripts/turn_base_to_action | 3 ++- .../mdr_detect_person/ros/scripts/detect_person_action | 3 ++- .../mdr_find_object_action/ros/scripts/find_object_action | 3 ++- .../ros/scripts/gender_recognition_action | 3 ++- .../ros/scripts/perceive_plane_action | 3 ++- .../ros/scripts/recognize_emotion_action | 3 ++- 12 files changed, 24 insertions(+), 12 deletions(-) diff --git a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_move_arm_action/ros/scripts/move_arm_action b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_move_arm_action/ros/scripts/move_arm_action index 1c6c7c746..830a2e55f 100755 --- a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_move_arm_action/ros/scripts/move_arm_action +++ b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_move_arm_action/ros/scripts/move_arm_action @@ -31,8 +31,9 @@ class MoveArmServer(object): def execute(self, goal): rospy.loginfo('[move_arm] Received an action request') self.action_sm.goal = goal + self.action_sm.result = None self.action_sm.execution_requested = True - while self.action_sm.result is None: + while not self.action_sm.result: rospy.sleep(0.05) self.action_server.set_succeeded(self.action_sm.result) diff --git a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_pickup_action/ros/scripts/pickup_action b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_pickup_action/ros/scripts/pickup_action index e0da8c631..2ca28cd23 100755 --- a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_pickup_action/ros/scripts/pickup_action +++ b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_pickup_action/ros/scripts/pickup_action @@ -73,8 +73,9 @@ class PickupServer(object): def execute(self, goal): rospy.loginfo('[pickup] Received an action request') self.action_sm.goal = goal + self.action_sm.result = None self.action_sm.execution_requested = True - while self.action_sm.result is None: + while not self.action_sm.result: rospy.sleep(0.05) self.action_server.set_succeeded(self.action_sm.result) diff --git a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_place_action/ros/scripts/place_action b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_place_action/ros/scripts/place_action index 882a743b3..38d267601 100755 --- a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_place_action/ros/scripts/place_action +++ b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_place_action/ros/scripts/place_action @@ -64,8 +64,9 @@ class PlaceServer(object): def execute(self, goal): rospy.loginfo('[place] Received an action request') self.action_sm.goal = goal + self.action_sm.result = None self.action_sm.execution_requested = True - while self.action_sm.result is None: + while not self.action_sm.result: rospy.sleep(0.05) self.action_server.set_succeeded(self.action_sm.result) diff --git a/mdr_planning/mdr_actions/mdr_navigation_actions/mdr_enter_door_action/ros/scripts/enter_door_action b/mdr_planning/mdr_actions/mdr_navigation_actions/mdr_enter_door_action/ros/scripts/enter_door_action index a367bb3b3..e61df452e 100755 --- a/mdr_planning/mdr_actions/mdr_navigation_actions/mdr_enter_door_action/ros/scripts/enter_door_action +++ b/mdr_planning/mdr_actions/mdr_navigation_actions/mdr_enter_door_action/ros/scripts/enter_door_action @@ -37,8 +37,9 @@ class EnterDoorServer(object): def execute(self, goal): rospy.loginfo('[enter_door] Received an action request') self.action_sm.goal = goal + self.action_sm.result = None self.action_sm.execution_requested = True - while self.action_sm.result is None: + while not self.action_sm.result: rospy.sleep(0.05) self.action_server.set_succeeded(self.action_sm.result) diff --git a/mdr_planning/mdr_actions/mdr_navigation_actions/mdr_move_base_action/ros/scripts/move_base_action b/mdr_planning/mdr_actions/mdr_navigation_actions/mdr_move_base_action/ros/scripts/move_base_action index c602e1f0e..2379ab34f 100755 --- a/mdr_planning/mdr_actions/mdr_navigation_actions/mdr_move_base_action/ros/scripts/move_base_action +++ b/mdr_planning/mdr_actions/mdr_navigation_actions/mdr_move_base_action/ros/scripts/move_base_action @@ -46,8 +46,9 @@ class MoveBaseServer(object): def execute(self, goal): rospy.loginfo('[move_base] Received an action request') self.action_sm.goal = goal + self.action_sm.result = None self.action_sm.execution_requested = True - while self.action_sm.result is None: + while not self.action_sm.result: rospy.sleep(0.05) self.action_server.set_succeeded(self.action_sm.result) diff --git a/mdr_planning/mdr_actions/mdr_navigation_actions/mdr_move_forward_action/ros/scripts/move_forward_action b/mdr_planning/mdr_actions/mdr_navigation_actions/mdr_move_forward_action/ros/scripts/move_forward_action index 3c39fa604..4a7208c7e 100755 --- a/mdr_planning/mdr_actions/mdr_navigation_actions/mdr_move_forward_action/ros/scripts/move_forward_action +++ b/mdr_planning/mdr_actions/mdr_navigation_actions/mdr_move_forward_action/ros/scripts/move_forward_action @@ -30,8 +30,9 @@ class MoveForwardServer(object): def execute(self, goal): rospy.loginfo('[move_forward] Received an action request') self.action_sm.goal = goal + self.action_sm.result = None self.action_sm.execution_requested = True - while self.action_sm.result is None: + while not self.action_sm.result: rospy.sleep(0.05) self.action_server.set_succeeded(self.action_sm.result) diff --git a/mdr_planning/mdr_actions/mdr_navigation_actions/mdr_turn_base_to_action/ros/scripts/turn_base_to_action b/mdr_planning/mdr_actions/mdr_navigation_actions/mdr_turn_base_to_action/ros/scripts/turn_base_to_action index e6f987323..e22fe40e6 100755 --- a/mdr_planning/mdr_actions/mdr_navigation_actions/mdr_turn_base_to_action/ros/scripts/turn_base_to_action +++ b/mdr_planning/mdr_actions/mdr_navigation_actions/mdr_turn_base_to_action/ros/scripts/turn_base_to_action @@ -34,8 +34,9 @@ class TurnBaseServer(object): def execute(self, goal): rospy.loginfo('[turn_base] Received an action request') self.action_sm.goal = goal + self.action_sm.result = None self.action_sm.execution_requested = True - while self.action_sm.result is None: + while not self.action_sm.result: rospy.sleep(0.05) self.action_server.set_succeeded(self.action_sm.result) diff --git a/mdr_planning/mdr_actions/mdr_perception_actions/mdr_detect_person/ros/scripts/detect_person_action b/mdr_planning/mdr_actions/mdr_perception_actions/mdr_detect_person/ros/scripts/detect_person_action index 4e0c3c909..51d333734 100755 --- a/mdr_planning/mdr_actions/mdr_perception_actions/mdr_detect_person/ros/scripts/detect_person_action +++ b/mdr_planning/mdr_actions/mdr_perception_actions/mdr_detect_person/ros/scripts/detect_person_action @@ -34,8 +34,9 @@ class DetectPersonServer(object): def execute(self, goal): rospy.loginfo('[detect_person] Received an action request') self.action_sm.goal = goal + self.action_sm.result = None self.action_sm.execution_requested = True - while self.action_sm.result is None: + while not self.action_sm.result: rospy.sleep(0.05) self.action_server.set_succeeded(self.action_sm.result) diff --git a/mdr_planning/mdr_actions/mdr_perception_actions/mdr_find_object_action/ros/scripts/find_object_action b/mdr_planning/mdr_actions/mdr_perception_actions/mdr_find_object_action/ros/scripts/find_object_action index 22494a9fa..048aa6ad1 100755 --- a/mdr_planning/mdr_actions/mdr_perception_actions/mdr_find_object_action/ros/scripts/find_object_action +++ b/mdr_planning/mdr_actions/mdr_perception_actions/mdr_find_object_action/ros/scripts/find_object_action @@ -46,8 +46,9 @@ class FindObjectServer(object): def execute(self, goal): rospy.loginfo('[find_object] Received an action request') self.action_sm.goal = goal + self.action_sm.result = None self.action_sm.execution_requested = True - while self.action_sm.result is None: + while not self.action_sm.result: rospy.sleep(0.05) self.action_server.set_succeeded(self.action_sm.result) diff --git a/mdr_planning/mdr_actions/mdr_perception_actions/mdr_gender_recognition/ros/scripts/gender_recognition_action b/mdr_planning/mdr_actions/mdr_perception_actions/mdr_gender_recognition/ros/scripts/gender_recognition_action index 64938f2d4..6138922f3 100755 --- a/mdr_planning/mdr_actions/mdr_perception_actions/mdr_gender_recognition/ros/scripts/gender_recognition_action +++ b/mdr_planning/mdr_actions/mdr_perception_actions/mdr_gender_recognition/ros/scripts/gender_recognition_action @@ -37,8 +37,9 @@ class RecognizeGenderServer(object): def execute(self, goal): rospy.loginfo('[recognize_gender] Received an action request') self.action_sm.goal = goal + self.action_sm.result = None self.action_sm.execution_requested = True - while self.action_sm.result is None: + while not self.action_sm.result: rospy.sleep(0.05) self.action_server.set_succeeded(self.action_sm.result) diff --git a/mdr_planning/mdr_actions/mdr_perception_actions/mdr_perceive_plane_action/ros/scripts/perceive_plane_action b/mdr_planning/mdr_actions/mdr_perception_actions/mdr_perceive_plane_action/ros/scripts/perceive_plane_action index 206e128a9..594d4be7d 100755 --- a/mdr_planning/mdr_actions/mdr_perception_actions/mdr_perceive_plane_action/ros/scripts/perceive_plane_action +++ b/mdr_planning/mdr_actions/mdr_perception_actions/mdr_perceive_plane_action/ros/scripts/perceive_plane_action @@ -47,8 +47,9 @@ class PerceivePlaneServer(object): def execute(self, goal): rospy.loginfo('[perceive_plane] Received an action request') self.action_sm.goal = goal + self.action_sm.result = None self.action_sm.execution_requested = True - while self.action_sm.result is None: + while not self.action_sm.result: rospy.sleep(0.05) self.action_server.set_succeeded(self.action_sm.result) diff --git a/mdr_planning/mdr_actions/mdr_perception_actions/mdr_recognize_emotion_action/ros/scripts/recognize_emotion_action b/mdr_planning/mdr_actions/mdr_perception_actions/mdr_recognize_emotion_action/ros/scripts/recognize_emotion_action index c201b2e1b..c3ec5f57e 100755 --- a/mdr_planning/mdr_actions/mdr_perception_actions/mdr_recognize_emotion_action/ros/scripts/recognize_emotion_action +++ b/mdr_planning/mdr_actions/mdr_perception_actions/mdr_recognize_emotion_action/ros/scripts/recognize_emotion_action @@ -39,8 +39,9 @@ class RecognizeEmotionServer(object): def execute(self, goal): rospy.loginfo('[recognize_emotion] Received an action request') self.action_sm.goal = goal + self.action_sm.result = None self.action_sm.execution_requested = True - while self.action_sm.result is None: + while not self.action_sm.result: rospy.sleep(0.05) self.action_server.set_succeeded(self.action_sm.result) From b3e1638a92d441be7b4c619ce55339971fb29eb4 Mon Sep 17 00:00:00 2001 From: Alex Mitrevski Date: Sat, 26 Jan 2019 20:28:42 +0100 Subject: [PATCH 53/96] [actions] Fixed a few faulty print statements --- .../ros/src/mdr_pickup_action/action_states.py | 6 +++--- .../ros/src/mdr_place_action/action_states.py | 4 ++-- .../ros/src/mdr_enter_door_action/action_states.py | 2 +- .../ros/src/mdr_move_base_action/action_states.py | 2 +- .../ros/src/mdr_turn_base_to_action/action_states.py | 2 +- 5 files changed, 8 insertions(+), 8 deletions(-) diff --git a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_pickup_action/ros/src/mdr_pickup_action/action_states.py b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_pickup_action/ros/src/mdr_pickup_action/action_states.py index dff083a8a..0fb11efd2 100644 --- a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_pickup_action/ros/src/mdr_pickup_action/action_states.py +++ b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_pickup_action/ros/src/mdr_pickup_action/action_states.py @@ -62,21 +62,21 @@ def __init__(self, timeout=120.0, def init(self): try: self.move_arm_client = actionlib.SimpleActionClient(self.move_arm_server, MoveArmAction) - rospy.loginfo('[pickup] Waiting for % server', self.move_arm_server) + rospy.loginfo('[pickup] Waiting for %s server', self.move_arm_server) self.move_arm_client.wait_for_server() except: rospy.logerr('[pickup] %s server does not seem to respond', self.move_arm_server) try: self.move_base_client = actionlib.SimpleActionClient(self.move_base_server, MoveBaseAction) - rospy.loginfo('[pickup] Waiting for % server', self.move_base_server) + rospy.loginfo('[pickup] Waiting for %s server', self.move_base_server) self.move_base_client.wait_for_server() except: rospy.logerr('[pickup] %s server does not seem to respond', self.move_base_server) try: self.move_forward_client = actionlib.SimpleActionClient(self.move_forward_server, MoveForwardAction) - rospy.loginfo('[pickup] Waiting for % server', self.move_forward_server) + rospy.loginfo('[pickup] Waiting for %s server', self.move_forward_server) self.move_forward_client.wait_for_server() except: rospy.logerr('[pickup] %s server does not seem to respond', self.move_forward_server) diff --git a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_place_action/ros/src/mdr_place_action/action_states.py b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_place_action/ros/src/mdr_place_action/action_states.py index b89c8ee40..f982f5b14 100644 --- a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_place_action/ros/src/mdr_place_action/action_states.py +++ b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_place_action/ros/src/mdr_place_action/action_states.py @@ -49,14 +49,14 @@ def __init__(self, timeout=120.0, def init(self): try: self.move_arm_client = actionlib.SimpleActionClient(self.move_arm_server, MoveArmAction) - rospy.loginfo('[place] Waiting for % server', self.move_arm_server) + rospy.loginfo('[place] Waiting for %s server', self.move_arm_server) self.move_arm_client.wait_for_server() except: rospy.logerr('[place] %s server does not seem to respond', self.move_arm_server) try: self.move_base_client = actionlib.SimpleActionClient(self.move_base_server, MoveBaseAction) - rospy.loginfo('[place] Waiting for % server', self.move_base_server) + rospy.loginfo('[place] Waiting for %s server', self.move_base_server) self.move_base_client.wait_for_server() except: rospy.logerr('[place] %s server does not seem to respond', self.move_base_server) diff --git a/mdr_planning/mdr_actions/mdr_navigation_actions/mdr_enter_door_action/ros/src/mdr_enter_door_action/action_states.py b/mdr_planning/mdr_actions/mdr_navigation_actions/mdr_enter_door_action/ros/src/mdr_enter_door_action/action_states.py index d0697b618..8c0b5882d 100644 --- a/mdr_planning/mdr_actions/mdr_navigation_actions/mdr_enter_door_action/ros/src/mdr_enter_door_action/action_states.py +++ b/mdr_planning/mdr_actions/mdr_navigation_actions/mdr_enter_door_action/ros/src/mdr_enter_door_action/action_states.py @@ -31,7 +31,7 @@ def __init__(self, waiting_sleep_duration=1., def init(self): try: self.move_forward_client = actionlib.SimpleActionClient(self.move_forward_server, MoveForwardAction) - rospy.loginfo('[enter_door] Waiting for % server', self.move_forward_server) + rospy.loginfo('[enter_door] Waiting for %s server', self.move_forward_server) self.move_forward_client.wait_for_server() except: rospy.logerr('[enter_door] %s server does not seem to respond', self.move_forward_server) diff --git a/mdr_planning/mdr_actions/mdr_navigation_actions/mdr_move_base_action/ros/src/mdr_move_base_action/action_states.py b/mdr_planning/mdr_actions/mdr_navigation_actions/mdr_move_base_action/ros/src/mdr_move_base_action/action_states.py index 20affaa20..f504e3f46 100644 --- a/mdr_planning/mdr_actions/mdr_navigation_actions/mdr_move_base_action/ros/src/mdr_move_base_action/action_states.py +++ b/mdr_planning/mdr_actions/mdr_navigation_actions/mdr_move_base_action/ros/src/mdr_move_base_action/action_states.py @@ -31,7 +31,7 @@ def __init__(self, timeout=120., def init(self): try: self.move_arm_client = actionlib.SimpleActionClient(self.move_arm_server, MoveArmAction) - rospy.loginfo('[move_base] Waiting for % server', self.move_arm_server) + rospy.loginfo('[move_base] Waiting for %s server', self.move_arm_server) self.move_arm_client.wait_for_server() except: rospy.logerr('[move_base] %s server does not seem to respond', self.move_arm_server) diff --git a/mdr_planning/mdr_actions/mdr_navigation_actions/mdr_turn_base_to_action/ros/src/mdr_turn_base_to_action/action_states.py b/mdr_planning/mdr_actions/mdr_navigation_actions/mdr_turn_base_to_action/ros/src/mdr_turn_base_to_action/action_states.py index 111ca3246..b094c38ed 100644 --- a/mdr_planning/mdr_actions/mdr_navigation_actions/mdr_turn_base_to_action/ros/src/mdr_turn_base_to_action/action_states.py +++ b/mdr_planning/mdr_actions/mdr_navigation_actions/mdr_turn_base_to_action/ros/src/mdr_turn_base_to_action/action_states.py @@ -21,7 +21,7 @@ def __init__(self, timeout=120.0, def init(self): try: self.move_base_client = actionlib.SimpleActionClient(self.move_base_server, MoveBaseAction) - rospy.loginfo('[turn_base_to] Waiting for % server', self.move_base_server) + rospy.loginfo('[turn_base_to] Waiting for %s server', self.move_base_server) self.move_base_client.wait_for_server() except: rospy.logerr('[turn_base_to] %s server does not seem to respond', self.move_base_server) From b390deebfbdb76a09bf2bbba732fa131a2dadbdd Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Argentina=20Ortega=20S=C3=A1inz?= Date: Mon, 28 Jan 2019 12:51:11 +0100 Subject: [PATCH 54/96] [travis-ci] Fixing travis builds (#94) * Change Travis deploying conditions * Update base docker image * Reorganize Travis' stages * Remove unnecessary steps --- .travis.yml | 35 +++++++++++++++++++++-------------- .travis/Dockerfile | 2 +- docker-compose.yml | 4 +--- 3 files changed, 23 insertions(+), 18 deletions(-) diff --git a/.travis.yml b/.travis.yml index a6dbe884f..bc85e2e9a 100644 --- a/.travis.yml +++ b/.travis.yml @@ -16,20 +16,27 @@ before_install: - sudo chmod +x /usr/local/bin/docker-compose - docker-compose -v -script: - - docker-compose build travis - -deploy: - provider: script - skip_cleanup: true - script: - - echo "$DOCKER_PASSWORD" | docker login -u "$DOCKER_USERNAME" --password-stdin - - docker build -t $CONTAINER_RELEASE_IMAGE . - - docker images - - docker push $CONTAINER_RELEASE_IMAGE - on: - all_branches: true - condition: $TRAVIS_BRANCH =~ ^kinetic|devel$ OR tag = true + +stages: + - build + - test + - name: deploy + if: (branch IN (devel, indigo, kinetic, melodic) AND type != "pull_request") OR tag = true + + +jobs: + include: + - stage: build + script: + - docker-compose build travis + - stage: deploy + name: "Docker Hub" + skip_cleanup: true + script: + - echo "$DOCKER_PASSWORD" | docker login --username "$DOCKER_USERNAME" --password-stdin + - docker build -t $CONTAINER_RELEASE_IMAGE . + - docker images + - docker push $CONTAINER_RELEASE_IMAGE notifications: slack: diff --git a/.travis/Dockerfile b/.travis/Dockerfile index fcb2a223f..1583f970a 100644 --- a/.travis/Dockerfile +++ b/.travis/Dockerfile @@ -1,4 +1,4 @@ -FROM bitbots/bitbots-domestic:kinetic +FROM bitbots/bitbots-common:kinetic WORKDIR /travis ADD ./ /travis/src/mas_domestic_robotics diff --git a/docker-compose.yml b/docker-compose.yml index 8f5c47660..0e486beaf 100644 --- a/docker-compose.yml +++ b/docker-compose.yml @@ -1,10 +1,8 @@ version: '2' services: travis: - build: - dockerfile: .travis/Dockerfile - context: . image: bitbots/bitbots-domestic:${TRAVIS_COMMIT} + build: . domestic: build: . image: bitbots/bitbots-domestic:kinetic From 6a35538921c2bbf1ce201441ffcc753018346cbf Mon Sep 17 00:00:00 2001 From: Alex Mitrevski Date: Fri, 8 Feb 2019 17:04:11 +0100 Subject: [PATCH 55/96] [manipulation_behaviours/pick_closest] The state is now failing if it can't find objects within the allowed number of retries --- .../pick_closest_from_surface.py | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/mdr_planning/mdr_behaviours/mdr_manipulation_behaviours/ros/src/mdr_manipulation_behaviours/pick_closest_from_surface.py b/mdr_planning/mdr_behaviours/mdr_manipulation_behaviours/ros/src/mdr_manipulation_behaviours/pick_closest_from_surface.py index b87831fe8..b64c38ee3 100644 --- a/mdr_planning/mdr_behaviours/mdr_manipulation_behaviours/ros/src/mdr_manipulation_behaviours/pick_closest_from_surface.py +++ b/mdr_planning/mdr_behaviours/mdr_manipulation_behaviours/ros/src/mdr_manipulation_behaviours/pick_closest_from_surface.py @@ -36,8 +36,14 @@ def execute(self, userdata): if not obj_to_grasp: rospy.logerr('Could not find an object to grasp') self.say('Could not find an object to grasp') + if self.retry_count == self.number_of_retries: + rospy.logerr('No object could be found; giving up') + self.say('Could not find an object to grasp; giving up') + return 'failed_after_retrying' + self.retry_count += 1 return 'find_objects_before_picking' + self.retry_count = 0 dispatch_msg = self.get_dispatch_msg(obj_to_grasp, surface) rospy.loginfo('Picking %s from %s' % (obj_to_grasp, surface)) self.say('Picking ' + obj_to_grasp + ' from ' + surface) From bd4250434355f2b9e7b0bb92ee22e13e1125dc73 Mon Sep 17 00:00:00 2001 From: Alex Mitrevski Date: Sat, 23 Feb 2019 14:49:28 +0100 Subject: [PATCH 56/96] [docs] Added FTSM to the repo dependency diagram --- docs/images/repo_dependency_diagram.png | Bin 27404 -> 28779 bytes 1 file changed, 0 insertions(+), 0 deletions(-) diff --git a/docs/images/repo_dependency_diagram.png b/docs/images/repo_dependency_diagram.png index 3abb3c5a50ead20da8a9f8ebfbfb6a6380ca84ee..6bcfb52c1056cee7e4fcbb967ad4b772a84b4d7f 100644 GIT binary patch literal 28779 zcmeFZcT|)~w?5kHpa@EkBw0|&K_q8H0TBVo8I+un3{7Z8Ku`okB-4O48Je7yAe&0DW=iIgK{qxtlR`b5=4OP2#?b`c!p6WNxR1~fg&=5c%kn4(% zAE`qimx3XXi~9H%!5J!~do}pFY^tR22y%}5msXz@4T0Q(C_Z|i>6N@T<>{+w@%enq z>WSs|hXgBE*;z=SPM=z2tT``Kw!bz~bH?-3Dbz6{jP0ty^Z1r~kUQc63)|?-UFNvO z@!P`?$b&?JCTe&v)F)ra@tH>%zSolQU9K>-1UXZC{H{JI>4hWmgqirjx`j-XSsQq019IyHPpFUYZkB+J}h2y{X z#RPi`?=%O9E1XCtlSr`)eH&BMGPXJ_$lmX7c?)jq#|qmH4rO03ky;aG|EiQjl2wSR zi%&V&$}yeDGdd-yjlaHbRZnzSsAPIxa|b21@TO-BGq{74ZuCTl9&FIQIP-3@WA#Yp zwp%F0G&70^NCio~m9js*)mPh!XPoz)u_I3)k?XeXB#e)b%|bR!{Zo~$K5W^vo zwQ~d66>lOxCPcPAZ_vF2Zs*?}<2q>@YBU$tSKWN_`>t@x>o<=u!r5-f)3;@nqBn3zynmHzo~21lt}^KmWD z&BZtp4FBD|J#1BG-%+_OO@a-`55x0X!51fQ${PSd;T;0V&U4;F>li+ zLy(Eur(yfr9OTi=b@m#*Q^}@hMv*4DwmI(_usc;TgR)Aty9SLsZ?;{lUCUsy`3F_* z3Ok*vNPgcU(|I`J46~aVNX0qyL~i>rEY3jaJTlDDKvMX-bp>5lVQ#+gWgnX?1)oU5 z1N(7>315T|H^YL7ww=gFjhTh~6!Mv)fkJsntm1xHZNWi&fp4f$-|L9Z?!42?AIF>3 z>r}@0#jFN#8c@GV6CNjWq`wFJ(CkWoNWa3sQhgO?AGBExq0QiH|Ml=?AFFVxs&XHh zEF1ak*@-;X-u}x}^ed!B5J?~m)?TqD867Bm>N47C^9fj4orBy{^%_S3iumlIJbc<5 z{$O}ZyxyeRu6qJ+z9k&^2p^sGUE2XC{si1KD&mNwF zQXb}@Q8?00X!z@lKWA2ON}VBmw{xpRKagbh2v5z9QeL|8)@p-iA3oCt4VBlD^?6vv zp5nQyL}k7){tRZZZ0D1+*;hx2H5K{X=h#*InLV?&!5GULJFRsXV&7}& zaZ~O=>8^Y!3A1xiU#u2lgr&`1)W|-M7k)xgdzV&h70nuE-*}{ahHlDJ-n6pqV|=b~ zK6+u7!ch7x_MwwNW1JpaP=9tIEJTlMb_#(f8_M*%V-L@EBmz(Cdg~}ehbc%-mnPg2 zslSq2z2*7hzJb`snMTlh5y{;-jh4o#T_euY4F3eH;!qHmP%tTx+fVVgCMyV0_c?+@ zyozU$4|NQaKOU$MQOXM3?7qSidr~8Mo!F(a_dN+lLmfdS+x>R0Sf$#psE}l~E5F1N zz4g1zC)rT6LvO|YJ*MUwxcA=WozoZoDA=?l`(ymA-PoOKG$T7#o}eLzlXcAvyIv}( zMr%$4+zYWUVxVirjgSpBT5>!0T+WStvK98`K~L`_mbloUq)A0=_sZxSX%9#}b(_LK zp&?rimF!`)oxKK+ajJ`1JTp?i`ehaRJw%vS$KBY*4?ZpNr9~0{MRBNS1i6#|=UVe% zw8_)Hb1TbF1rm@-4>83gn*j}#6ZVQ5*j#yDKCP^;uH%U3s*D(jhOEzcK zG*xZ-dY5jBym%i+!t9+68{DwCmjD(aM;a~Uw1Cj027YmAu$Y~oV5buPmSVJ$sQ8vV zl566F5jsZl1Bi&-gF+dujGrY-KGnMI5*JCr)*#0O$85(jml2Bi1G~_s%LqOEncQlB zS#Rxk2bm`dME2YCEI~ySQ3}#hafNv>UytT6Y3kFg`FLSsWABYY!u3M-D1RPVd>C64Q-0vrHX9!9k?w1!d3H?}FLdQ1jMmO!eK0520Rw@lS%>DSb zuaTRcVwKM-H1bIfK6P3+q1(x!W^vPyFEPW9&y=zfz`Cfo$+BGi`i?F=2LXdz8hsz0 z$ke3P=OtzS&c;bjL!Bku#SFc>F4fp`9XOU`J?i>Y_Cr{J1Gf0m^224s%Uc{7rgQbT zK}=&f5jm+h3$76hEKJf^jE=U4_nBAJRdJrj)W^j}5c+!f58b{xLn_tl+2CFS$DHp( zOtMMGQD{8$IB%=RR2(4d@^>{uVeDOk$uf<)SzId?;5?s&PVNOqRp$HH281)4eovU>>tL zzm{%)FJAmi9!?_mMbEd9vu2xV{5Gvv6LaEuVI~ju!o}sDwI&S@T7$@l0R0&+TK1-ff}3d%n9_b@ns|=bq|ea$F4SsS=%y zP0z8rNgt9>_Oz=zhZ!8fvfT`8gV5JS1x(XOX007F zmd8V^Bkh-ip!L9;R=%HJh*CMn^#V5@G1ujz$8OARLI-nw{ z4%-e`hD7eHuwW8kdywAvOG(`WF>k*#V_3PjB5s5>m?Y5GF#mk=y|Whn)`++nQb8Rl zV4D%WdW<^T#CCz_^VD!RbZBHan$`!JPtC@^-z@aKzW;rC_h~lh7DD^!^`nL6v_$!6 zcOAEdhim)x%XApm8?o>9EB$#0TK1yXjLt9p*012)z(AB<({u=OqSQ!YiAomikCyFD zD55l)V^qnm{s3p@1y%kA?)(mRqz)VT+*b)^Un7syK?={GdaqeZEmqKt*yDtpzTU{m z1g+R8je$jG@zPG?Y1fsgpr83mbNntG$?%WSx9i9WoYA#b1=^EUM1r+dJh!tuI)BUr&;NL!dOcI7!!vvi_>4pZ~Mt&dAU98Rl=0;X(*$j1N>Kv9I$**rUC%1b)JfjI;1gk|<1JC8?g=v_--PtYR z(jH1eZd3dG85x4FK`x}%r`p4v7fI;m<9EIgcVM_0D=h=@BlRPXuxM%Jw<55ORlZgu ziBkBLL6;N^gY1+-w#rtMk&ZW!dQMWJ-N>oLkjKeuCX8-^e^h*4KL$HP+~~8}Kvykm)GkR$X_@=beGg$R zI9qQkZW%uAYdmXOKSEOs7mRu>cBDU^DDI}9?OW?XA>(Q8a}1fi46u7)F^46jSlAOT z(Dk$JI=WpQ(+H;);+f><=|^S`d^9rjJXP0fIykUQ=#CXPmOdJqY_}l0=oGm(Fib7P z!1sG##kXlt^vExGGvBEkTo>u8|jW^ zt8=`*T&-|YO|FG;qM5_{IJg2?+@yMD(EMl9`=B z0SS?1#5+d5diAQi`)R|-z>@t~$wIWMRMcO$pt4f^ZHsl65z7W8rKP1MCEMHE4cKG5 zF+IpG$uV2Mlfzhv9KQ2>d(_vjvSWGy`{Q-qrF&N}y!RjqGT_l!ot>SMKKuAg-IhBQ z4in~W7sk6h9;H4!37AU*EgIZkpV))eYv)n;79uvQJl4kt5DG?yh7yvJswol)sj0l; zqyZ_o2eD$Rds$O`)sfnp3vzR5_${BBo8LiWmN{*0 zZTHv45b8Ppy9h>Z-69#pz}iG*VOg1`2M@$U0kni^;sFa&^M|`iK_sa#Y_)M=937$c z@KP#ly1)OP-?5vtv@|boVNlR-(H*-jR3`1ccwaVvyTQzalBCaeHJF-bTfkaCN=k~5 z56{idpRDyPoHv)rOT>D9T)zSu77+kCA*>L|z|F<=E;KYdKmT21q)9!rygYupK4QB2 z`8{1;sEUe;P{LDr1fQs=-lu14qs54V0u>K2$UgAVV_LgS!xD2vJv}{nd3kN^*gcWK zSZ)I@%CI#znH`VKX(Kf?KIrapd;4RgHfDZ)esfdWdA7Nxre=57ZEOWHO65ns?3Awx zErIWn%j!DWbFi_Ac&x?O8Oqo>is?DEu={*@03ZziyPTYu$!gbzh6Z3}C z`C4eN7J*&ys28x z12-0khfX103si5x?H1O=?U=ujf@$O1xgMiKV zbhx-@Z?b6VlE#H)K7y3XazC<90>q485V%8{Nq8t(>l6QT1v`)Z*{mN5JGQuKjED`VvQUEC7R>E zaHjohNMAQLHm=>0X|pcZkM1zPcXPTyVHUpxsqJ5J15X2E_we<# zC^+6S#LCJFF)%thnysE=ZgrubY#9OC`Th+K-~nG!I5;rj&vSo!Ak$b5JI5X#9uDW} z0MN(nU5EL7n(z`0$g(I1W{^>(|J(TgYx#fqkfKAQ7EDL8mOroCAERKx2g^2j*O&)9 z9g-GTV#avJ(YP8F**oTRw#@lT+j<3|zJ_i*U&%^M5p`WmJwLSqYYox8gr{H)L{`iR zlLr_FH+rO@Ily#mZ~u945J&&p@=c_+-Tj1XmoLSTza%k~{$CyVm!B)xK+aIz3H-zi z63ZA2r#OB;AF54)ot$9o!JjMM(cxipTU(dP1vlM?4+Gcmw=InFxN}k9#Hi87LZzxm zelY6;jF))DUC+JCo_P>_*+8+hNhFXr?fo&M{+K<-R*Xh`g6XR*6FSPoG4$ypFZ#f` zKb`4xpvD?w`l!gKC~iMZ46JId z!fH`2)BbQVaBjrjmBu*1FXJ=ccJ-wSc9(-|z`#DcZ>4PS8_Zsu;)xOVbIS*6d2*<8ddnj844R`6t%O&{+pY`0pXnfH1} z0S-4z+a2P{VJMKBE|LoVFPw|)6q=2s3Q6!x?qr!=dYiWgkm&mckMbKauU`0HEtb+y zkM#E|&&-%FJeP1>DC_?~bA~EZ`h04*LARbtf{}Eb8mKEdF67b5$Xt?+A>TS-oZ4?* zv}bA#9QJ){IS zkQNU;tuO?fPUPi#E8Z2CLSTk&69DK_Ws&3YIZ3kAsRdvZfG}Bkby(|N>he*yg1r16 z3?&8+^znw)Y)3CxRE&7 zYduN8I+vy8Hr=HCVSBdZWcm6kleSZR|E_iNw6)_o0b?>LOOTi21fJc(4OzN6-7VB7 zP9Bv`ACT6UX|Be3Q-dvo>SX2~yjR4~qp}{1F7!7eAFX!LfR6dBLTQ0B>8yH^-B9SUkkuS|imq)Mc`P7P0 z_r!juc{un66+JLrfHaEG%M-?f+6Iv{8ILkw$i$FH;emPOs2{_9-mtR} z$wG8P*6?UXBX-;nXMxOuP3m2>D@1f&vu{`(;ZkO;?;1}Rc)QU9M_Er3KBT6?FuS)E zB7#Cf_+bi5SD@W*bC`I~%zu(4m7+yn%6U+Rurx*jBrW_P;-|YSbZu_!AcA6b^;(0Y zN`k6N$=;{j%ZSF1*0vr+9lI(Uy|ALPCGJklwFk!f(w8U8K~=QL7XnN9*u2KQ_0?GIU-=Xzr!I8KRwLIJ^FTP zNxXk_sOhYcmtJEtZKi|?xnZ3XmQkdedF?l?elR?+IK9acU#f!X|Pd8-6MH^l ztkU&Y#8D44f z=*LOah%0FH<>4w|)ff_~_qS7&e)V1Yg8_R`6+o_Ef79usL`~mNZlB^`zU%AjtG}u` zJw2_F`PAk-H@?xGp586ssMgazZ7}~EWv9yT#{CL;wC@;G*p815zqs1X5*u?B<_n97 z=e(*fv+UY?n9i8W;eYb7sb1q1@wwxy&XLxF`|BSU#%FEMCUsQor8V^R&Ejd7lJ-uA zCQFX1WGY7vOpHCdcu~FiB>)$K-eHo!938$(Ca_{%n8)W0mYqD0xxZ{M| z=j@`A;=L9F^iRUO?kEqR*_;kE`OwE<8sEue%_qU!Nt2S7Adr6l?UQU0)4#`~Y+Ob+ zu`u|Ur||*cmZO*$^vkzhyuWR@iZMOX3UuDcYa2mRi}QWu{G#CsV($||?$y7?qm(y2 zHr=-C$7%r7EFTQLM)Z<9-+mJI>2uvQ2<^9|<|}?Lu)|Rq9vOndhd(_2(gM6Lx2c91 z-8eaYm3;->XY03x5;nvyUails`~KQQg#ggHOhNs_)I7qBPhYGGIXLtha39PZfitNO?{)w!0P zf4e$kCM%f?*92p}`PP=$DBo-Ggf+ABXaIE6M})~&J;j*U*zkljlu@f^S(fr(8iCHwQWEP z08VSc{qjUrAWS8~n}gld58B{9`qiWl-)FCYiFdsh1_JRu7f&K>#ZP3Q4lq!~m;#bt zfWG4A!c<%@IcjoT!HK5grFu{n$>)`#V3ZulRDcVEDfQZ0e1x`povxy2Lb`B&-p0at z>+GJeeCdL&zm`Qk->8x}^JsnV^X5f(;t)D#v#qjc5Do-OuIo1D-CVkZPjhTbuZ7u6%3LQ)cmg$?v$b&&llYUcC@H%5f zQLR*=$D)nJRs!F2-jY1vv&ha*I49`Yajc`T_iNQ8#_-|svdPSl1o3p?h`!$-5HW>+ z^N4Ye!{q9G%4G2Q(u1=~1n4OTn7P8`OSO@1!;{wdsJ5?VZ(b0JZ$%iL)`>+eWws!b zPQNZuCDlug=k`QqVnjevFVpDqDIJiYBjz6y!bY+tZ!V{2yddSK-0F?`8e$m?_kwPR ztsI>Lx7ni}<$f{xwYT0z=g&sxs2R(Ks_Of@WLTqFC**LVlRrqW89=_=Z{Z4hIcz9> zY2vsjG}7Of$3*A+a4nPycK=YdgU^YfZhJC|YJ5=iW<`;!(|Ai?UXKKlcVj7&FP^J{ zP%CM;SG9r@SGkM47YdORNZ{t(=!98pW@OA=OocP^{Jo6dh!#tA;n$FKRa)+ zoo6mZ7`U9O$H+2S5FfB6%V=Q90_=Ud0h~h=CUaV#|R{@E46zq8!2v7>kOTpTmlz#>73> z?3QN1+9rozEF@}a7%lFeUkKKAI+`KhyXDsntfxqVXIsa9X67SC&u1c*Uou)@$XC8m zRvdY|J3C{hG%=KILB6nho1!>W%gU9rqF>{0LJXm=upQ3T%F_n=A~$)=H)b($_QF$} z*2-W((TqKUU{k|*g`mQ&4cD4k?Lyu%(@|L;YuB4yJUrO_s8^}sgvpZ4OWm#-#7o*B zEsMOm*Pk7uTu9)sgIE#6K}Hc7F)v7s1apZasCwRQF)+**2kX?NKY6D3 zn!7ASD5Q9;bC#uaA!0;TN>JEkEa=OGnV*~c>+m*$XoX|+ZvU8&bRv(bFUu%B0F-1D z%3;1NX+AkIZ^z@vcw@JAXrzD_q$@o=-%0MB$L&JWYB_o{R*a1+M^MqxQvId5sNrQZCW9!R3YiS}+%&wkPjRjPSCvTtOZ6l?Ho z3~}IuQxwV2KC!#jiS}g|mvb7SI{3blH>@-*aaSC#j}b2*!l2Z$XSCRCq|k&{93QfK z1;kxnD<#BE(K6_i=*uY@O&AV3}bm5 zz$^)vUSFqW5wRc!@3?=QZ8S78vN>6;@#&eh01ZZ5i~vJN=SOD8Mv)+4$<0jORC`lFy1UGczkR zuBV}*@?0M;2e14NMqzzdQ@vS3k+eiZgfg({$Nc;q@!-H&U%vzXgr})~`w1kb0Kb(D zt*R1XYW@Rso;L?3#smEl_uNQcBPV(*HGW3KAfc*Z<~b6 zxpoIOtj-(&G+v=xjO~WGUpSN3*B|eQW^Fv(Rs#5qTjZrHL?2^g86=^*fC?!o6=6QM z78VDXJ-A0k=1cP%G6yoN?)aIdCC~jeJ)Wl*AZb)MLWAV1y^w3DCIPs*l;Jm{67x6e zIfliiEgMs{u%j-MT+Q5ILMj54u}hGCPKSw#p5sgCA@zQRTD`&Oy{M?Dt5>fEUcO$U zJX1*rd}?thN29vB8Z<>SNAnN<;RC#V23)S;?~l#V$dZtdxN+l#Bh+SNvbvz4Kp}#T zor~+^2jFf3UBDPw(q}7WHiH0Wu5?24h(~?<6zP#gNX2uFPUI!_^hh7MpBZOpWMtKs zvc9-Yf#1ya+uQc{0!*I|Xotlx65foaZo>Mm$A)csG ziPFG4OzY>zMXoQT>%XWLM=*&1-=Lh;7ti;tuaEZ+BBZ25l;&4^AN8cHYiuH6`FYfS8d?Zi~W2sg}1Cz%HQL{dbfCvkrVd0!=C;O=r%Z1w^m^l)W|s$g=-M zR~#=fKE5t+dtl_6kf!I4RaHl<1;EpAQz*E_GztzPolNG34>tgSC@tLx{utAvBVEFP z)3?a{h9ZRQN8i7HU*tR|54sM4KzWa|=@;hb*FXf~XDB{Qt*Xq%@B>VSwY0g@tmz6OS==I;u7hwLmLCaQ)v+ z;42AQ_n$-!F6~(^0zP`>x8Jz*N*;m6PJMfRFFh;EP9U+l`4Q-7)K}Bu_Fi!3Ee%D* zH-Mc7gBS5Jl1QvtKMxVY?ZOeMv|v4S-VBC`p|J79_bCOKX%60}3T zqZju$JKn=hEe4=#n+9RYsxOwl`N5Wi@4uEhB07T<+&TA@34?$}ROsr@zLwhADf^Eue)>FSE62N`+V!9S2>|8@2dhs*OjP1lW;T6Gc4 zrgHyC*kw6*-fjx}$oiR%}e%9)y)YHEJOg^WK2f*D{Zw{Kfy zf#c<25s=IMi>LUL_yYPSh?qSHa2GCIP>SV>qW?|i;NHz^D=RBB;VgW7YM`U(fQK*P zhb&vhbwYD2I$X&gON)Rh(gwY^s{ZVp2#B4W+WC*4K7IH0?T5ciKdxqu`=sM&7)U+MU-S2u z6M%Q@s>Af%3}$K)5V8{HS5}GuAdqWZ=Qq6j>9r*srRP+-4QUF z6;9{|yAd8-`wu{t$|~7->o~zXik-IM=*MAs=3wfC$Bd!cx=&j{dD(NuS^Nond?pSK z4oWUvAmh_fS7#OvyNxRtglqK&4&&h7B^sk~x9x8SFnl|LijNn)cql9TwKbd;qz_=c z>v6XD!wr?6KK)4loAor1>b6-D(~z!_E+XBBv;SAvfiy;aYpYN`{F?K@K|ekdsGqUi zlAGVi*YYp<|AZM;D!w&n!p~bv1_WbUPd|96f`l(2o(q!E0zo8Nw6W(}oXQHCg z08OE-fw=5@RD@2)=gp8!ZqVN`g-*9+y%jH0s%;22IzmEz+*(Ei%+W} z&AatU3hgk3qqt!jfMj`z=CR6o`2#s)B<#c4Yys`{5;$h1Q^;eX@?t17# z9vae=Z~rhOYYp*{sy_0yg{F|mej?dK3vXMQUD`+?w}&$a-8?;cSYff5Kvy>t_f1=c z@ND@}6wvGYm_BnZ_g`kbzq-qUxt6;>^}}zZ>I>+P2^UiJICZYtVow&|L^umh98OE- zG510h2w5d!Mc7P6?qM>_fKZO9xor7@I3vvg3TU@V-GgI5V8Uh;kcVXY)82Kbhd_zDEi0 zYvsrX9@aW_uIbN*PxPQbsv9LTYSooHU>9vxk<3#O+qGYqH8?REgC>9gsgA9umX=ma zOA8i@t!zji!Gyg{6t&41woOPqs=ePx(p#zG6g_S2&s+M^w<=5CubxNum(p9BmKIbi z$T2y<4EH0|Ot7 zQj(BE&B;cs6MSQJwez$a9dmSGMKLW_FUj+9h>+=YQm}%Zs!+As*+nl;n}=>g_|BJ} zE-ko@zDbra@k{3$vcN|0+rh*=1mat}^HhU7=Lve*Ck->@HuhCnB&Jr{dYn5?BYxhH zT@x`FcQYLyD*^jxRNwBd?ouk>oWC3Nm|?|&GW+B9KKIwX*od znA#H1)3zXQ4`<1qN476$Yqt$vP*74@-V-Qo?}dvmbtilSEjQW$2@+uI>7;zUfC8td zfWSkJvHUiWnZ!J?xSs&@fQ`?dA_mwJeMT$g5!LR@JNz)JC8x+gq~6;Ne@MO6^v*>& zTYU!4ZeOW6|G{nBD7~?_+_tpsz8BdS;kCsYc>a-YXR(R1&kx$PZ_8PiJS<<@;^Ayw zT4w|Yl1iH%;s*WvrH}PS26dFBJ=`?^l5JZ|;bhx6gR~ZNbF1b822X4gMIR4}!Y3$X z@iBbQd8yo~MqKl>D~iL!pCPqr!+TCm%XWQwLC30(>g(#bD3z3zdyxEyE1;-Y9&6$GQjT%fr?RcZ?q7~Mo7Y6i}CnJOY|E%<$YWZiScaLD)2=%MFvI`6} z6O!`WJ##p9w=^D3-R%xV& z-7UbU|1s&7vY(%YnHXI`6XXG@mq%|0=*r#^6QQu^MeD*_st!eQLi~lR8B<4c!_dGE z7vvMPbzODg&Hsk&9UHg^6sqGFThD(HT1uc@8gVmKPc9^3<}WNkOqja=qmjg#IoYil~lRH&OL;Fk|)SgoqFwvjyzu z+TTm;j=c#RN$MS$5a6c%Q-94i!FTcOe4~*o=y2}y#4O@+nLr=yl>&Sw>`Mi=s=mG2 z@=W>bUeEQd>zht{qh3YWw-MX_cPJ3LrY+P7jXVA@fJMPL$c1?>S+8z}fTsY3#yi4; zGUYO4Bbitw=f5S@4*xbAh*=<=AJ6+o33Y{T5D7rGIAMuf4lk5dZ0cn{-K1J1oxzlx zBo|aIX#5mfL9bwsEvCWhvM(uywSVEzP|!4}H(WM`kE4+5>z2L)iN9W7o7KFgh&Vf| zf)_jaEZA#fh_uGOiC^E^%9#bq@=1Qt!}-DJK5~?Cj`dGB?MMP=lb_=q4@D`EPD6h$ zV0N#3_xT9iT}ezeDSr3x=O&APj0xC!hR=}3U4ZKITX?i|zY?Y+!N=+{{j5b&Fr=xkdd#Qt!GpmotWzmj+%eVX7wEY zE%{YbVD9Lu12z^oi1|CXWw)h2=Cvv>$w&Phc=Op^+!d^U8~xCls&J!ITyjHBsno`h z9zrx$?V6>Ml_c(2P+3{I#`W@68?Tn~VVBw%dq9C0;Ar@;1tl6^)PQV=RZ_A(LqVab z03?;522?D$4GOvnIRynODk>i@uQEx_8d^W{0KaNGt-@3L$4Z6T!ifynpS3C7gGxmP z02~Szo_#}PCHrx<-`%@+fks(@ zd~yABR`oJYD>W=b9KMGC^47q>09f(?I)i)TpU}Z^JV1dmva)!M>kfN_(I4S>kYfoP zWTFeO9RRaDefrb`2r9TB{Y%c$WS}(p&kFsoMrQjVjq9N+-!lNZ_6NqHzN>cfL`i9) z%m%RjzY5GUAOQZ)F1zVIdD8iF=9`APx;cOo9kdG!MS(c>TL>|zEJJUmUWJHp0RssN zY5_=inH5L${MX{}PgUee!kQfbN`WYpgX77s3fCAYiT=h5+QEbWRBtbv0gyB{C8fb@ zYgQQTtKmTjFgWnM{ufM4On7*Be0+RkV`D&DDwEoF_7uGNAC#E;<NS7EOQg&qSv zG}?DZ8GwyxeF#vsg#-r$k&}}Hn8Mu33cy|vKL$_*{-Ym`kE{;o#qk&=CM5y69=MDb zq!~c4U8bBQwBja)!#QN^0Meq8DrKahar+OimI=v7Prt?}CGm?(f=pR1bjBbQBBgu} zi`H&Mgl;7Ocv4*h_W__lfd8vM5tP3f{HoNB22_Dj-r&mT&!2$@fr>I?8L=Pp@#7tF zFnO#E+}zxpor^<4LcoLngh>LVECHYoz=JRqCvi;WKPYc)P4aXKjceR&932b(05iz_h8n(q|1RV(o<6wpt2Iaf$%+Dq zTUj|A*E8HpVn2Mi?|b<3<41BF5SbLisT08MeKt{XZ{NOU@cr2Yw8J1XNg)QC8WN#rdn1hC zTqWB71QiE#os53F1KvA}TH4zCzW1&?VqCWb9g__Ow1ob7HP|?CfQCtfd52#Nu))$X z3C|5f9UYwn(hHC;2PR_xVXe^R60Y9`iQ3BEaRR^9@$qqI=;F(&#)hQ3EG$qcltJ2G zf|r+0hy>la+6M?Ru#2I) zyBq8@(cQFt8yb33(DvinGYI4d&Pf0*HFmPuxH&fmCe`}VlCb-#=GZ4XLVyUjKm~?` zdPl)#j`Y?G{cxb!cumZh^5Fvx;&d#s9$*IJ=(%Z(u{oZfH;;L8SNx-y+N78~Wlh(Q zra-VCrd;2_%xn(qO0eunyem!$rh6+`PXQpGJzx#0J`~cxn?Z$m?l7BPk2p$5NC;!+D64B~Sisr~f+j@2)N9KW zG?2dFs^q)q_WhXTWJ7iJuTe07zgGi$fxv$PCc&?zU_T~49!US$!~yJgDuOYU%lHVZH^zP(aF~6;KqovFesTGm ztgI{n0RaG5!G0kS*J{Ru@dW2k3}8o*BnNx%L8(b()Xg*#c;QG=ca47F8o8g0d!U-f zZvg$a5I9IR)3PQmT|-1q{tX8E=Q8`R7~KEj|6INx$ASKPb*zwO>YQI6u<<}fy6Jz| z6frQ}@~@xdfTS(;EBHiR$qd%6_c|%L3VxCUIM2&}{}PAp`4M8~Qs24%IO0v^1}G zHjWVBmkgx5pKAtW$>%(a=Z;;ytf69awqZYIcfzw2GmI8HUw#$|bk4PsC|2`;il*ni zHQKdDnp?D23N?jqIwkI$S9+qi)MAdXUmC6SS=_(Jmnq)9L-OP?{vFZ>vR9RFkrbA6 zj(^ee(Bc_+9g~_@OK50VQT2QRGd?0m`kNa5)wD8*tPDQMjqg5A(JQY0FuI#y(Bly!nJ#1yil_KAcnq1zHVejRKLv}ij z#9b=_njVSG6)rg44;r$Yr-$^vd!FyiV!H%U{EZE6-C=~Xq7kbG*WFJ6?f4~+IUg=?rRSHH(>{xc_!xCiQN*Ucva$cQ8$Y3qYft8%rYINj0_7Y013lv$gyaA&a+wW zv=bzHmw6-NWn}m4V()m&+iLeW4-7PzryF5k2QYiX+{eFruY(3a0z2`05U8@tJ3Bru ziwaP=JVUFX>MUdAU1nJDx7L;yEc}h;3X2_em%F+%uGgGk-4&TBrznu8-%9w1{k`QumzBtclUC(>y&*9VA?e*wGi6=4AiAQ(Ec}IT|K6S%7{TLGV(o zrZbc+&+u%i>=nE+B3*^h!^hIN{Ws}28R9Ff2~~PlVhJNZSA~WB%fxI6L11Fcyj(3Q z$?Nr@F|YrMp!GYn#c*96uc!CN?FO3$vQwj{mRL>yxt&^9o>=T`Rd=q9e`FuQJA#*d z@rN2m;Ymb`j9th|30td?*c(VgVR1J31&+A47=!C09K)=R0(E7=BiPrWkC|steOIA} z6#)a&5k2}E`W_Bx3RiSoRT$U(t>K=&p|Z^bE#G4uZ18G%G{=s3I_`XSfAc#r%V7vq zG|{^=$9Br?-g#GdYG0^-1@yaB9_v*Y@3cSmON_qHTSm-WoukJy-_WE+*^PB(Zl@v1 zlVNuy30*&fFPoM*EQi(E?wJVDJ>K=6o&wDYy?YPa5yBKhwx6{MHX?-4?@#8==G6y! zcVbX4G{5YWy*SwW*wGoI;mdG-Y8%%3=ux?C;CC&L6YGOO9lP3LN1MRdPziCGxeoKi z+kWt{)pBV{L2nVorAJ==4J>c7QiGyTiXS+7s&3`jqRw@P%oo{ZSIWFz9jxK^O;n^{ zbILNVY~oxKo_V( zUtB{AZCmy1`((Nxo=<0z2Me^7kXmYO0Y*zWLcBN=If_o&%07yjJl^Wm*1uhj~@nM8vzE!pgX zmq6CEOLbPNto$RPebyk}nVW+yNeUf;z$3|-q9yJ+36s;BSIFU{<~JHjP(SV)g* zIu^rsP8U7-%>0sf>;z@A%h$gRS}5zA1x0FD@!Ly+nIa&VJ?H@npyR-4YwX%nQPPe( z!aJ0GgKb$|q1MTTrS}#pJ%76~z#%^al0{G_kIXY)~xX7`y`Z(&|#kk_oc zd}-jL`8y|~c6vOk809Uvx7+Ktp(#^-M)-(FWuoOWX7_j#e@t1FcZc`SAvn$abXu{d zqeo6WI?{(TeLlo=?ml0N9ae(RCDfp30rVBJQ^wGPTIt?Rq*=@PjzvY&?oM2xU#$-3 z(81{r>lAVmN=tNO08Uolb|k`?dV;ZZQ0OQp+ts9Eh0i!3Wo}muDvDJa;q2>GMd;O zAYeZ3T%<_e7ZAAH9v-S*>-~nFG{Yp%NLrjNbLXK7rGzS@`A6x5n_k%9AP1XZMmT$+ zhNs^~nbqcp=lVsjLO&orqAm%-CO+;_e+LbU; zMeMp$npb3ueG+W;h5x{sum~VMe0~kI%}$f$jid^#^_BHJAIag=3M9S-rr`j|yZfrF`p)KNk%B+Od{+;9JJYAoQ z#?Ri0Xkbo!7bPjqeOq-)_r>PY_w(IENM{VS@}@q&)ps_Fu1QJX${O?qN$S9`{!WKA z#{+P6!)tFL`Pvpq$C1u5%A{^5VJpLyBD_|aA>+p)m#^38PJds*Xw}Wev>XeeHuNa= z9K?{lz(EjLIE{LrMvS_Z=WmIbE(TXcrs)|}%*}_rKzQRt+{ntU_ns2opHzmhX}wdS z&3+$3feF#HVPK6)_CuIP>`-N9X5(G-0?T&-@`cT); zAJKsiv3fiX^CH@$tHa&FL7oXuBg-rF!dc>^$xy3GQPOUmh6|DsFBuvhz$zct_>{k> z4R{O_rg*Q@fQC1DCiYPYI??sgVyyh_0+G-9VbBjRhY!~@MstkUr`6sk(9X);tSs?O zWxMWlaQdZBfzI}blsr6{!O%BbmzAuk*6}`>nVa8NH}9RB zcgQY!i;}CrMlAVp^K!XDztsxC*)`Geq$rGhE3j8vKCHr~8Pl@{X?nnP8LuOqFV64n zZY8kSQ~6oqXepo+Cw>jPpOqAPwn<`P(^&t@HlHs|F)oj03yf=(E>37M4W@cTeuYAx zWRtGH(8f7mmQ^EOK1;1EZ{@uFnpWak;`aQv}`@QfdS$zvWWmLPVKD0TldwTX|I z!viREyCkCA+t_zG64&Epc9?%G{YpkQqDzgHOv`}IN?FQk^ykUm^RK`-o|P2bk*ljO zRl=}RBi-*m>UELLx{CD1L@YTq_hsov5mKCP$|M%9iFETOzLPugKra4DXuNq1=~VW* z__lsjNChTn*)LJ;<&d!^>8hI>diz*g@ly#-N0QP(i5oO#CFaTSr5k1>Ec+JCF)?bz zJXQns-&o=Zbgw(~i=piKcD@pNeh3X+w!%}7TL`xy8ZEak<#uryU=nwi*1O*l!NoW=_?u$+nirqjEf(7GrjPHl1{}6wvNdxIE~f!$cNKP z4H*=wmK-qKv>rbGK2&cVewScR?|9anVi|}0N4iEs6k)Gx z-f~2kOK>;X54}ES^gF1KtxoPa9<1R9q`clbWh3p6{(ALRHjC~!*ay3goyy#-<-H%K zKZGUjaqIz$+WnVa^h0Vq`i2Pu^>hsn3k>7%bdcwOEm{9 zn|&hR+A=c>)2P<(q57XXEQDjKz*Mog@eCnrqq`aH;_wOD_Lkje%Cv5{h^1!1++ik1 zFw>O2Nx#P2>x*eTMw2o3m-IAA-L;*Y-n{*0q396QR<-}B=Ig9E?0cGZCl9>ph#G{* zNUR?x#!EgQ%SIE-6JMopg1$SVHYU%s@ZCS%8}`}l*oIAOls|}@Zes_Feo1pZAo4Ec zQ4L72P^Q^v)_r~CEhK`=n9M@_6D1NAepM>Rm}fdFGye5gqO%!U-)k#Gp#r?TK-8o; zdYK0fnKkY*Fm)Mz}D253fz-UQ}K)Q{p>@wc|@-Hp7HJQ1*`* z&V+PX^Tt-VcdvL#i{FIieQv+ybxexk2qyV66db~J`^>Qkfyn7mif*Zunkifq$4_qFw z6YEdyH3zK?A)=t`$H8q@1b{xGNVu%n_>Y)S5se4Ihv!!wgh7q@lZAk;&8U@dqzbYv z9_P7lN>F+U`92h_d7Uo}yy2HGq+w98el`}CoX?-ji!VY9JAtU_?D>Y5J}F7n}g54F1OtJ0D(+bn;@fr1R1Vb_Qvft1k!r0<4k)9?g9y9&VK$= zDQ6E^5z1KZH5UL|Iwbh=@Aiq4%P}tV=uY51-fOv(nKiDl1#){Q>#n81TBj>wke{PJx2tIEC2??1IHbZnl=Ds^=|Za zDk`_J;ft3pb(`|{L?7Hc{g{rZt`-s&7G`2En|N@*8W*VfkJo$2W5)i~6SR)0VK zs{n#j3iS7{6^IlA)}R`9HeZMsI$`j+-_qg#9%6s@-`^u}s{cEJ2V`T>)1wEHe=N4Q z9$+qkKbIY&LFO&42 zdS(Iv^pi;+K74rp{wo-IEAmWW4v9pefh7*{%`Ygp5*W?@T5JNrFCZ{9I9O0vsC9Nh zws1J5*F0af>-Q}nl(r>iAAa?Q)87^OA5-an*7L8+VXYqt@jqMMUL77D1^^F4r^0Oj z;AzEy8Z(i(ubvC0GAd;wdMvv6=LBJmyFo;luEZAi4Xlt5z8^-*|M3(AZ35=`(I`+e z#sR9s&;PrAvdrX8bVw&UuyTFm1tKe>O%+z~Ee&&Xb8T&HH#foG7p$tZE5^Cv<$l-v z8xko)L!S9p`-5WtlV#128f1(K3<0`bu!!O@ATCB(QLzbIJ$d#PP31u8p7X+Uv5RUR z@q783-vEeA1tu+ixB^=%AP8ulN29^`y;r&U1q>)}Dk`c3lq(Dd12q8w32#pU%~dNI zt>C-{@yY{Cx4GF5=!jbU>FMa&jLS+zfsDT)D=RBMKg{YiWPpOvZ+{**@<9Tppi}L= zxvHuvAk;mW3nWRR5{|tUy;}xQ5#fakwzJJmcqtIC1EMSBa>{FKJ?5?w1k5~BQew?6 zL7dd%rCOjygb*5S5HV8+659A4?N<3)0+a%>0nOIAEPv4uJbxRMwz6VbF>d=H)R_Cu z9av-IRBP)UK0e3aWhF`aH-kJxK%6WuCkM|R9UcaRM}Z@rN4qjq%?>hPGB!-d4EjS8 zXs1I$kbrIgJUJo8>{9_CwC4AVkS3-dh5u2)CwH0VOA?a)6MFdt_Csm4A3^L!@*=kSeA61VmO~ zNP}n;pjj3a9QHc=8hB~g+RewOF*B2~a6+2$2da70DAx_JP>{$7fT7i)T!DhC5HB-O z@{9a!5Kz9ev(rMEMS>Qw2b|s3h5$%vi{}EYd2lpG_YIf&_cG89O=^?v5|c3ru@EuhC(ZS z4xAI>Bbhs4zWwd(W}qKHcL8q>t+2fUfc2f7Uox(L;WK;?#Y7$^5=F88t=;Py->1kM z0_Mm2TP;mtV8nw+6R?37g0T@1LHE#|3-eR@$`6#E(Q4(N6CS_`FqS~_tNRW!Vk0@{ z9`yqr>`1qVt(g_Nf1idZ*a3l0aJ+EQaD`%I0St=?Boa9~sv!~-q~?7KbbmPDL1mh0 zDJdLu$Nin1bDn+xxq&DmxI95X#>;$FFzvayxxs!Q;dkue=H}+%;Qs8ea=@c0IAjH5eEkc zKqv`faBK^2o^^5ffYoKmhwiI`EM=Np5d;?@QWNi}I72PLtPpu~p3l~iZqz&t1zz*` z9sbc11}yKBTwn>nP$D)XT-_yO1YZUOK~Ag9%}oGb8ym$K88i3yJw~X`Jz})1a0t`u zJr^uCVbB5cZW*;)0D)c$3s3gV35f0_prjLd4S2|nIIr;0F|)FAP+pmL5EiZhkr1C3 zBSgkxckj%PG$tfmFaP-kVwhJk+;0WQ7x`6Bq%x`!k2v*;?q^F=%6=;W9U0^X3o*;-QIMMGm0 z%;H~S4s|hjvrl<3z~g6Ztmn)Il4c@4I)_&*9PV0l26u0VBN(~)O(^((!R3i4jq*0h z=A5;{XM!Asj=!~KgF~aOO>MHwhTjI$q$tG8a9yONNNQ3%unqDlN-Eh;%)jyMs z>P|GVzY%Tp@~E|@#yLFC*vpL5LxpO=DKP^C<_Xo=tzWvIcGRCq1Xj2se&;y)k8C0E z{r|)6{r6kymw{??uSY}G4{R(7&X)E8;4r^~iVcG~`$?kn>%QVEeYT?@_{SVetOpJI zoOBZk2=Sij_Qzr_5trf$Akv`~@*lwyNsiC{N5DDEgtR&1J26r6d@;I~M*63^4vB#? zWQMowbm`o;awgX;Ys{O(LvWTg*C==!(s3wK zl1fj>xUf_bkjYTX>s04Zt<^stHfQioFtOMeo0l2sE;&a;^)hf;z2i{* z{;I$gu<7`{24z_^OhRV~4n z<$Y_F6=iF|;RIF0@Z0SNa!iNFE~jV(683_j^=3^LGA9&=M&0+~!g=(QDRo6$;Y(Wn z@YD!%zqDu^@QZA`Q$P8Mm-ps5ImY{AW;&7PdBB}yaH=XkF?o+g`@zGOJMOsEH`>?8 zdZRY?u?lLwh4W8jUE)S4*X}wD-h9huug`Fz`yv$8V-a7YDu}uBp-;c>g?ECLg%Q;C zhnJpyr1XA6t^VkI_f!h`*7lTP>#LHsG3=e+8mtG_16Ck@lHD5fLr;vs?@o39noFn zNTUCYB*R#lrD=`Cb~{v0h+9)aORG>Qt<&atC(G7tdxK;1Ujhbtvk51-n%Ta^v7v|V zyYF|FY#>s7)!XNda2#==z)@-kW=Qb#1D&BOp`qxN*+STQ@&fKrBZBX3XvfJ(&|)2M z*C6~)gFw4Bix}KpsJ*msT#&MZHjo@I5WXxB+TaohVqJZ@gHOV=Ui&wAPsxxd5lwnD z)<0E-zEaxuvkm5`sP#NaUA7)3WSXe;*e-k(4A_O&4pJ-Ot*vO1!?X@53NW(e*eCBW zRr0f4mGE}{u+9^+2_A*bnXN$~jnt}p|HNh_OFlV#qS8z`0{E1d=Z|SVN!MWn&-kg* z`2j<@KlI$;LKBKjFOIgS=EhCL;W$g_FM8~j zuslN*p8w)lE2Zm(58iM^$<}rYJA38T-doqby96VQl^bMOhr_^#@~YG30^yV}%XQ6u zaT}O0OUOo@CJUTqmp8Y2rmP|wzL$SKn7s*gJf8e3|FJ#)x8CT~p5dF!QXz3ssi)BX z^=6(6GE5U`+NFSSWnZ4Drrz0yKVtIQ!sud%A8EFGmGMGnKMXKfwdvz0^G?_!?LpW4O`ANb}I3-xmgX~wv62EkK{}fUU>n_`HA?~ctwM| z4rOrX)p56tbaueh0T10-VsXs_Z76I&K=aeNP!Re9%T%FI=j8J4JAeCQCb=;6=Gt4X z-S0N51XPQ>DjHxPLOn7hI@vWu z_tzEb_?w&1cFX>uMym32n3yW!g1`0x2Z=MQ%={{q=$agCdN}jyW&A+|v)$xvE*WV@t3IqsuV8a{E3% zB8v7$KM(Lg;K~Lj1;a8L?ifq?-EzW}4e*N$|Jz17+Ve6NT^M_MftA=uZa)j1c_1th zs&3hBj9Hh+v54Q6^&hDC9LA2<#sXjOz1Q|lx=NQ=+h!nV2vxj@q!~&@}tE z6>W~ur()!SUDn)pnB+?~zZb8yi1rHLo>b#U$0S)p5H5{B;+yK|cW>;n7mt#^4;kvk zW;MWqLNgSIY$bCm0>`8|Gn8Jyl4~St>`@hF)pvW87|wn#u{vmq9b}p(};%dwP{d zum=zCDNqvRFMEDadu41AK3h@bn8^F-lCsel<94mp{Qc|}mBH@CN(=NI%%tkM)?m#x z-(Q=|0BjC56#KhZHx>`iEpmxh+@AS0Es%Twh@Jxj_n{hmqdQ!>B$J+U;jXgho^gK) z&NY@?C2n_#xTAWG0y(L9~ z)pCv-VJCB<&&Vot5PSyJO+}|DRJRaut~w2IysFy~+vP&`&7eADM$lk~b?~z%KDiPi za8c3!fi7^P3McKqV{Pgi+dVSp+hWYT8$F$-GVxJILdy!B7HsFz;YF@uzST>s(*GBi z8J}KEs{35w+j%`*mTAFu%5vDuWp~4fIM#NkT_9&fbl-C;q9qdkE?dNffOC9b9;QuO z@8RRjpsxOnvv?;zSDRk!z~Zm#Q7|An61OMzP8vD15%a;Ue`=+u8d;95A5K4>4U2t? zZW3Y%F(`VT*&tY&pxSdo;TSwAL0P@I`Tp8i0r)2i#gb$ONB=Z~@IKRI zWX7VV7t%k2T@WXRd=FEdnpjr|ij0OM!SXhfQq_^Z%(;>NGZp_6i*)!dY#rg8KV~p* zxa7$cEB}1DX$+HjIJGfG)l^gY#nz~6H-po@@D_)zxqi9*m8B>&USpJsL9fY z%N<3xHQe>`4sXHMvM!wVP?MK;4YOR+7OpdO4(MLMCIh{PEtmV~+XDY|4)1-T)0h1n zc+D}@S?kSLukx%%cs+29>D`rjW57@r$i5}Jg+(Dv)_h(b*el>!v}+-Wa7@zJi{lFdX4jt(f8(e7uoAQXwOx*W49V` z_o#R`S0S(n0CH+#2$0Dz!?jMXT#_kl&CwIGuwWGRC|5-MED@XVpAL)UKA9T_E!lSR zIaB_Mp2&=j#rfAKJ+;lac5>l*nC2-6+U|Xj}@* ztJU`wY3Mt%SK$8_UXyOC{BM}j1U~5^gaY5xI$q&^QB2u^k<;@ION$!McpoO|FI!G$ooE#awzK@E=hw6vu5V;=`P#`QLED;MWuu364f%1v zzVq>Wyi#D*?7}35(Y}_M#;qLnLYpMsy5Yf^kv7^RgDqQ#p5RN&<_gj9c+GR>lQdB0@VO`a!&}exk-u3m<9qmWN+aup59p z4Ne>fCHlGcUPah>LXMoUjv@t1hEL8@lKKwx^!^=Yi zOpk3F`r{-DS1ZRw6<4;kF|VR3A?wf6kW%3F#L{!%g#muc7*-R#^G;&@6etbq y0!@wc19spTeD8HJyb!!)fPP)_ZwLiR+CPojPu8TC>u3glhsZuvf)+n9eDhzCegJ`?^=89Ip>=5Gry}oyp)&1$0fssKp^-po{K9% zAXj`Lkjt7lm%(pH^PQ@}4=h6&DRIaJ=3f#bGYkT`2YDg>OvN>BebU84#W>|++myad z7W&+vU+jkLy{9-h$&&Ye^TxSsuWrzglUp^4h%7g#F3{5F!RtV**0{n+os19_#E0zut_)81_W)#p3R{k=}eKAIG!8^>PQ{$@LHPgqo1{#< zl_iP^1!jS!o+@*SI-U5}nGi4Gcfz-R)7;)s;W4OhP(vPm4z zt;ga<$_3-?FBhn6#Bk+ZR8P|hhH?IQ54#U}1%{O*rJVbkp(;2xOmk>KMeOqAnGmNA zoO;dI)e`%-dhhGpHOQVKxP?}6>Z=flm+bFv<{t_`yb{bDqnAntuo|)4e4v&(Tm&TO zW_J0Ax{`&{iJ6-4L$Bm#oAjk6BDFhB_)GHI1r=7aNox5QauYxGUE=*DaqM!EoZU3L z8Km%73oR&y6y&cVY_(td6b5JzFMV8%+-yBRDE=OdHE!OD)XYx$~f z6QEtPGEoxG7fE-fO(CO{X(gcgsruL)9%$ zu;BEb8rSkbH_O#+*KJKCOXueO6WCGO^ynM*a_P7#?b7?Krf1tJ8S1r%bk2{-A+-|J zS9{;}T@hRg!L3qC7x|=Y&dFH(Z4*g$Gx`Q~l3F%f_}~`e<>Rj{ZqUH3=t~J=Q={sy z@<~ObJEdj4#+!ED+p?><)$+>)?H{@Jy^)AJ-7`xjn$17*rj8@>D9$rz_;6pU+(O*n zuTKn{9ZH}47aea$84$xCFTS>kp!wt{X! zu~h^=`!Oo*5*lmwJP(&NNX$M^!9K7DMjdxNGINB+;Y2HxP(s(akuURmhNJ%?&+OdSUzsGnAN5`=2Aj=rrL3)jq*`vYJ*`Wm?s}P&0YaupS#J8}cY7qM=$f0^NL%eS`H1Ldw^TdnO4=fde{c8E%Mkz#5oE1zS$I zW7bogWV^aCky{cTu8;I^9`k<9Z;}({jn*5|58QVl5q&6X&KXl4Y@yzzaFJ;AgqgDa zryd8ku)R#{@tj0R;Yo7PkP@;}@z!Jh&Vijd+AvXHqCy)!TgFsT|3EyFt<5e6JEdt6 zw&yr~`j6la$rB@8=C6`!jeUVF&XOUB45iCLjN1mE)rp*eh9cJ>O<6H-CDNy zh^7&^HL%dYWY0+IElTo-KCb3-)$7zfw<3qh*dlp2;WuTvc`9_xm{Q{p?^(vNWfg}j z(li#78eh~Nl!SS&svBL_(ncM#7i*PVD$89>b;>ChI;<$< z5IwCA=V4cGiTixakEl)z2qO|zY&^lnO^Ch$-AlJ)?zg{i%Sy_tSa`cNJR!hs_Uh9~ zLT8B{;vinbJhxSCm>e$`{Y<2LiIZ(Y2B-MU@x`u_a~vDYUV3!YKbLlAyq>+gAAo% ziu7xin|qndGfAU4wD+lGAgKxBn96Mx_7Wp0J1-VM@OyET1k>~f~6L~i=8)@R%Iy|WjnCVH9_ zakd-7d)M^r2lxk9<#fA)B}5btEsVy8U&)(7tF_OxdRN%{ zZlfThgvSrPY<_^Hrdnipq-zC*{z6U3_Ar;@u!fHkYG9?j#PKD9q}F~~7FngQ@|Bt6 zK9~}`1G|KS&1=ll7YgI0v}fA4<#49X5>uNqsdBPxbH};Vbxm>h`j4vxxC+zW>S}h# zL{j&Vj=&Q2IOy8PRp%bT{B1pr=RR}{3CKrHIh9^iy(u(!wkhf#l%H6gXYt5_l1sQ} zBio8Dg3cT1ml0H>F<`diLnz{arLk#eZWR{gO9cNyYRwmBk!+_T zz^6S~i-f~o;B5P$c;u}1RkAH#*^p{+^DrH@Fzb2Om5m7A%$X!y&62qJZtL#NR|94x z6d%FbF`0nxrDIS>1#ZRWk*9sxPw;`=^}>(mMBR)LeW*5?RvF9cgI3gw_=A=gU`k#` z4CbojC?-?Q)U(z0M3R)dRnMbkO^m-?#0wtn_^6wRuL~;Iu;->s$~h-6>QpbyDt|CA z)s3xU&;5>iAp{~p`VB%4*H!x_g^^^F+d~^2InhL@!jh@O1w!@2wY|2`?-4HeH$**p z^Qv)a2u##tI@{s&?|q>HJr~HZwGdJS{NBaL+`C`|=cdXtYh4NN%u*xmqLU3<>hV}r z8lUGU$?NQTi-K^=L-n3OVV#$;TEjn9)*mr-mL9p+yWG3jRJ;VDN*vVb`}w+bhqWr1 zl+!LV`}o;COK?u3wb&_H(BijCHpKO8SYh@TBFIV)VqmDXbbq8SLbvnzoS2X%GtpnO z`-H*CZt*jn5i!bF%SCb2gBn_-a2cfB2=N?8V#8$?@NKO+ETCBKe%jgnB1dh6)Ivlj zA$rY$n$BNix*r;pEiYS_!U-7q!iB4CWLw_Ec6TvprW8vcVKmu>vYP2D zhbsNFp$XMyBQ^}vv40Qr}b7Y5=O>f_TtnAmBJ2RmTFgGdb-jF zx=IONzAWdn-kr3Uklv_Ax`a>3T=E-YT`Hk!c{P1|^9zZ`}|<$~Lz z9L%A4yLshI8#fm{(iu2L7W}|VH%J)wDVZq)8aDS%)4jZGSYL#-U0(@UASLnaW5H3e z{2QblWK-v>{r!}vqYa_c>36i(lGIZ}E`Q2MYfkc9mq9T?eQrkOZfQH!4a7P;xU^@1 z8R`FfHO-6khacCkg9{evfd%)E;$k^aE|$A37y^O)2D=5}H(KdWH|X0G6_l_2xW!au8D*qD6@K5G5;Bm^Vr-HrQm&&l$7+;`8H%3jrr~4 zh=>UH{dHo|=&nUpt%}y*D-Z+~=K6~P?Df5CXw7`JH&H?mM{LYT1R58e`;zLuadez0 zw^UbEjaV0l^wxr8GU*B#IXUuh7LUfKuyCu$paHzCw4G)XxCPm>0n6yJ*JP~=I6*>O z{B#L^p4s;L#&y({r%}I7W!Z4(jD9TY|Mlxrn#SRLEi@D{{|eF@3;y_3)1=gGcd<*< zzu2e+mquGhXKr?uiJ5s|V1SO{iq}*ixX)pJ*Pr^&o;`ELysC`bCaN8-itR&sxxnWZ zMW(ws;D#DM+>B*U6mqZ7Z>56Nc!3X9D~_Xx2fs*7H8L|>es>kmrh4Ia zpH-`_y)b0CTt$2LH^}fE9-E18-q_&(QI9%Z9D^rcE?>SZEiLVNIOF%s8>{UjHUvSs z1SSu5badoLKxyyfgg_5#6}q*3`(^;f`;NvPvdkidx&%og{fS0TmYFNt*c9aEuAvKs z4}W+YH3t?8o?L_UDh7^Rfk2+pH?Lg+#s5w4@F0*UVDkQW!_UIbZo4s7e)sO(5^u;; zy{FVyy$A$64&O*ix9==;7-L`Zs>j^Y|AG0oDLO+S%P>(fF;bRSMAX#m0yvPpt4tD# zuT8<}UT21NRDb>KzYOFrZ~p&L{eNh#ta$a734O~2dcwyx&-%1F@63{>J76LgWbY;S zdg}C|nh<(LV;ms@4frad2_g-|41W#{OzE$9sO|ko$mOScBxGbY`rqCi9=f8QL6X>$ z!0-HmsDz-v#!HkTPpQAT3V~B!r-Xp(Kc&Z94f1*xq>T`09EZ9EC^NuO+@vdOHx}n) zK@>=8Ioj=Bhf}e9ioaY`4eC7I=LDvIKAfb6ot>TG0-e~{JHf;y3c z|H+_o`VABv+7{-tG8rh?A%jUO`mPj5!l%#o`!+IVsO#zGx9|)Trav8xlucx~yt+eu zI6B`8E{PQe_>}!h@Vaex6c2(-wEl6 z4HJ8hRWrgJ)87+;tUEeIeb=OETz=GZ))UmmXnE({4~E7n#jpkU1hV)?Mbl{{)0-VR zX70NcE~f`B6MH+>#EiMflf%}C3@&_I`Hoz(Kv#5Z2AzEfm@qFcs5mv(mxyRcWxxVH z;4wSSPBCN~yW1%4y zOpVS;rF92*a+^Oze5(`5{Vp;0iDE8{V$NY2JM-M>aCP84fGD6ufh(4ugS#@35MR@_ zrCyD?v%w*Z{?XRKl}`s>ih4{)=W}ahnJ1nlc+cSrk0|)FkOk^nT>mj(P3aj6_0bgP zwHiD^NE&6F?N-+0Fq@t8@y&^hCu7Zv!|SfK52(Pyg)pQ2g_(Tr?A0RVF3!6mII;j_D_whGES|a>*Wz&rFnH z(TIQl>IskrdhU9zdB-S%E7k?pGH#qodu7ipRVqZc)_H@YN09cHlL{@IX8jG6=oostVn?hi z>K|_^EpWCt_Z{IwG_DZEsaG}UxDuxPy;;(=yh7C&QCy@+J*dF!;|k20Hp~g|oIHK<@Lv3biA1(LdJ>jtKNJOqW&w7Mcn=7@95DqCdakQGlK-My*v(~}nL4Y*BbWwk0G?n$0wDHdmYJRzQTIcaIlkA#{J zIxTMk9tw)QsiWimlC{N#3AYT#g@gbFy7%#J*0c3_xS%?!dcz(eE%U_9mAyGdL4$>>xbZcyqm!0MJ{atg3xZ{ssko`hNs`GaOk zo_qn#S1(bc`-|3FhexwD;C@9O2_G&8%++@kGUjsuIK~z4Mk3lO1PlW*44!~55qMhs z5qhO>WPc2rQ3s{g)OEjElb$ZuE?mR)CXfOVmWyGq!W zNR%bvj)yXQWTX->QD-`?_vr#97xt!mmX%V%CHu1Dv6YB9QGY)X{xIeKH$9_YXTDL1 z`j_xGOyzwnqb{gt0^SE32UW~Kf4@mrOhkD20|C7CNf6^qGx7cLcZxbw)D>+{XI0vm zwYqyK3uHbJ-XWW}dBS{B%B(l>X0vg@qJ!RAAt1|6MTC!Tuiow%fD0xPam+bdHMA$w zEx1~8KOJPXT)mNn&bzNT@D@RXfZ=9?pieYw=StNx%7u>lZXA+ED1;^PQD#*Lml?+uieH+u9?75j9#02 z{W9#J99&m`9}mc|BfvczXXst*o`5u|$V5&iwMHrZf(mY(o;mezIma&G-dTogxug>|G%yQ9oEL>?Dr zdHw^r(G7EkWpKp?L}svh@X*?VWX1U;%h-2u;C8_TY+L177aFz~V~z*9Ah8mC$iT#%uJ)OUoOY zVmgmb41gO~T_;s2_*^ zpx)>kHZE2voMfdB<%HLfCngu}Em#_n{==CPD!D2L!Pmfi5FVY?*HBYykDfw{xF1<3 zqX%v~j~o~qCvbBoQY;`|L&Xisg;$VBp|iPO$8RNVn_USr1Donb#g0Np=&Wx$J1!dci?I{YXN)Sp+!ybE@;c{F)jswc8OgPmyesgP0 zaS7rr>W8j|(#w5O&H2we&X$tjttGylB+Fo3pYU^DI4OCeKpv~$zXy1nb6r-D&#*_S z4yHGnr0@%(cgwGu$aIRH?)^YkWJ-9LoS`O4j;H1E^tJ<@!Q*b0Gtq7hbuHzuP{?YH zRugW_9t_zeRPY=)4BU~G9`Ma!E!m0Pz&!@ZuXEa@IkB9HaZ|wr&j35y9g?5zfvuT0 z8otBgmJseKlP)G>qFQDLC#$oITtW1DJPf7`PTt8w%+z^DCbj;cX#**u-H;D3GkC;E zO~rJleEsXf$L4Cgnnk0Qd;@T5(ay>0-r~QBuB~Ag&$tAJViakVn&iJKHXJK62gwiR z@Mf9+pBNXX!wPO~^yeo>)@Da`1>v4=LkI4$=LZb*92?Z?$&EbLC4^9N+sy)^z|by# z$x(8A;6A@V_U<|F%E`N!ywA`GX)dNg+KCb415`;0cOiS3KIW1xWF2fa=TmP_Yo)Hr zlCL|g4qXHB;-s24`1*D)Z%(Wp$=Ig)fD;%aWL<`Q;C_t-MbS&y(SK`=_l(fQ(-oMr>u+~#YG#-i!38Ve$@kfg>19|Rd&yi&_F$m|1GAgPtcg;fD{ z%Bc8vOQ33Ov7W3!uVf}pm0L!w--Gm$_5v8w@Z`x8wE}Io-~>)8)T<%R6^ z(#ez5O8@@qcEI2uP&V@YzCMG5)Yz_N<3xa@5OsAxbpSGn>N|y%`B8eF8qU9gx`ir8 z*g~wjx_YZ_SN_?hn^xe6vX9_bvE0^AV!N~|tU@WlVM$9VA!EJGLFBRLq~HkFS+~Oq zB{jAEE}m%|8t1Jk9aYtZ74xl1=S#4*JdILEejQQKcYh4o+PEW=At^2^BN!!dvmr+LlVcpyVg)E^K+-1qB%4QWbImwz}uK8_b~$x&enk_2?D z!a7#BFp3hOpd<#vx+!|+%?S-HEpiG9kM+@FFE1}<^@6m_Oa=yqj_#|FWlAjUDhA1u zR{2Y#+R-7UNg-qY#zn&X{Ko()J%0RHy+9jPW{#mz`st)$4SKQ!QSsmF?_9?M=c5?c zZ_S}6dtN_*ogXcU`U4tvjmFT>(1!>xN|jp@_8=GN{Rq~dI3ag6XIudZ>J3kTubP^g z6crWg>p{Qf>({UT{{E+@rwx9D9l;>I)~qlepx+dU^c5aEWAR2`Y*SzKmA&3yN6B?u z+PkDxX#+c6$#mHmqZ7dK+5{-B!g{<}&q8B4b+Tt?To9nX47y18uAa`*#L&>tV*7=5 zF+YMfg3C$n9=lyUbaZssgj99*3m?~6_Wgy&y5!?{Ln9&YncI&1odk+q+?NvNks- z=QTr#z+T^Gcx7gAVVMt?Rqor<)Fsue3!naqjadC0eUIQ@5T5^4O+G#}R+O=yz zK|y*R+rI1K&VIsU@!g3c(@6NmT@Dko@-a(YfFI+ztz+af1>om(;lNOnBqA+00EcYQ z&tU278I1q@`7^jC6BCoBhZA~rXliQ8g6jewFcyW-urP<6`4{Z$g^bD>;!;u~yPX{4 z71jf5XtE$lXHEf8>J1Z%^0Ai8g}Z*NAuZUw*qt$4GMNK(4<59FInEV6S?8c>47|(H z-``)RX|1h20m37Ab{w!{M^xWB4u(ft{b|lmd5QwN8R+P`;`j!7dbWyNs02Vj&5(;- zPUX2_)B=Hgm5+#w+#Ih|$Q_c8=N~FbTe+bmp_nFF%D$&au8Dv;85+*Ua9O^_25!JQ zT4o;BXC^Bxz4P;D5zAdZ``6;);+~$uH;X2pgTvd~+ocs0dfM84`DxjYW5#-4qEgeI z+Y`ocBTR(x63WTVm64HA&K+`@u#?(dh=f|W;oP#hj#;XTq{b_43v+XUS!?U->+df7 zVJQxaov2b%W8+dDOvu&`(WJ;D73JmS?Tq77e)Dk;3q-u9et(Tl{9CcM^~56kiuq~+ z0R5T|WN7G?u$&&R71A}AG7G}N6tU3K8ZER(@W-<;GQI$WKi&~MJrEJ_$vB_m%&e?b zTaQ;{L}e%>CWMAQ_B?fUTUp1$(Be_re6`r<=+Rl3f`WohH#^!sDQh!esxMx=0L}nb zI$(j?U@kram-F`rm<-s4)Ep-D_%HEOBO23ZC^hNoUvUKH2F_fjf+B=jS>T{F%E9ie3eV%w;(YECcwG0~pIgW@cs~ zAt43^DQ6-OQWFCHhE}_tgZ{+mpLusvD(>6*XSqY&(T^KCI%J%QKnwxcyQ8)BRkeM( zd?sk-z`Qdvmp~YJomr&!2SM)!K{!Ps02umxT3P_cG0}WEmHQSk=0yXvYmlcQ&Hq3b ze{8M&S)vL?_fcr^ENjtg?L+%t}gBt?U zvHv#9U!NNedNTqyJ;#e?54swc&hob8|59vx>e?r;~;(Ogp1z z78d9P{NsO_lJV=;dD_)VnFD=NZ|lDe3^)MqEKwe1|D$EF^ndWYjs8TU|86qaona>Z zKNqALKixtF8JPi#a=}J9Tp$Z9;lK=5MO!;>U zTFbKsR+dwrPXGAj0o-8mA^ig`@1}B>0YgtqOKWX?4xSojSqEbRT`KSb3mdS%)IUuV z{PkZry2=w1kcRU#fUD8aJOfraDONa=nVxQ5u1PgZ?<=i%3w0ObSPGiD%r1Ih1zye} z&Gb8P83EUYW2_4&;X-{2uA$`0m8+qf$?C6g4#X92R9oM3A$yhA547VsPg*D3Icd zY;}glf3H|KpuRNmC~#!FcXcTD=FOWUc^ZHCSJTnI;=O{j^k>T7%us)p?KtRRkW(|Q z2I<8yxfypHRW~>>ZeJ4fo&a?S+<75@ygR;1+ZxsQ4@6#5cZfG9G42L*x+)1rkF@lD zH^~s|Wa!zJk?*jN-QP(kEekbniGpVP--tGAOQdcS1`>KY+PC&TJs6WzKCa%0;z<~biyJm9H!r&Kt9j%fr?)>6R zOHT*NLtD6E^yF_*0m`v#T=>@Qzt?FJQ1%yW8L4|RLq@M1Kh#G`68?-6VfrB*asE_3 zy)Ug1tM~;oHeMS9&4#4w`=+CB-_xKDSDSk16Zuwz??|&-wh;?(aq*}3On#_ii8PKkeIp{uq9zC8+=a3gbph;Wl15iuwlBzma zG}a-nI@g_i!CSY1+z1d&ga!#vWEnLGiJZwz*ot&$7q4YvnJ=6;8dKNqXkGd$k8$ow zL-pW=TT#u5@!M9Xz{p6s<7DJIh(&>hXyZ!5Rz2!ss`yzX9mtH4rI!% zuF)h?vHpb~@FUtv-g!Imxb{$!V5GL9FJLLv311V5+`e};1jppkc@3adw!$UgK5gX!_i zuA2#V368JRHqywlE2{_tRy}!2@!F4|=>&U+TGw6DmJ^Q#h?1;4TpRoO6v-8CX|$)K zw+2>V>+QBzAt%qM%d2y8a#B+6$IH8%$v_eHMMb{PnDf;-j`OA?(7tc#J>6=5=NpoK zZWa`U!&fDUsi`_KKm{moSZ#K`eLDprppSpiSq9Z)yG3_?cEDIvSWm&qqvg3-oM){!4mUmFJ|=L$Sy+I^ESV^f)uE zUfyRNk8m8SOPaq3W@?*GwPIYK4g?uag_w9Fkb ztf-X&8d6^(1=(u|Pc5E&(8C#AiMN=YTYb&R+{DEim$nkuO^Hk6vp`W46<|aMYp?1Tkkix_*L^(q_K7htHq z8M{JC8i?y^EIAJpaWlf*yHi4an*vcIcP}H#&Z%ND+ygbWT6!k4-Qc);{hGSq@u+g$ zDGw$~T-sL|uAF#3KFTQZifdDQNPp1H%wr;s-_E97B3}3w%~@P*p;6a~BaB+$Ov|8e z_6KCGv_5Fqk7Z&W=d0%1gfY;uPL9g~Y?U={m|V6!f(e$oJyvh{KUmpWK3}vlqffL@ z9lUl;He}nf{t;iZI*#4L%#zfT{`uA>rU+?S^7(;%3)W2)#1xCqPe=15k@XPWoZF%k z!h?kyLwO@BVvqkNlmsC`=VbK5G%jS{TNwANW(2-un8t_xYF!T9=`^>t-q>)U^2AB5 zFvL3kw{kIXePoRP;*eeeXZu6|>-~{7Ld#P0HL~d%4NyFT_#;q4Em8`dHl}IRTa+Ng zv$xd*SRJSzgC?8hH8z!y4nEyGsnX~HV@TK8^pqgzzw#*p^;_(1ZUmgcaq}fh43S4q z^w$_kRKk!t<2#|zQ`>Q*zLxNkdmoxZ@jd?wv%sZ6Bj*OvGv53 zQ<(uLyi3$Az<4fw^#n_qI?*}Q`pv$SAF82Mm-KNB4EkyBtP(X$1~kA`R;K&K zGDDQmM7B*MQaC2NiAZr=e+FIt1YlWYM1)+T@Z|Womh5`a-=wJv2AAts^-iOfq{WRLFk`9&ETqzJOB}l+mGGo_9U2&Qq!yAwW(>Mpj$~sh9npOXjFI+$h>%Qm)q)< zwDd>7R%^Oun*xB`aDQu>UBF++5b%!WqCw0{1;q$S3irfErjx=a)> zl|PFrF~BjH`S^Z8;okbz*4EL{SA>LwU}i&quNIS{G^*Yh0ILD|MfH45IsrV$9`sk! z;m403pmNU2%IfTtMQK2iM*a@H{J9BjT(jhL8xSOxl$QQZFp`&3E6Txo3~FV-69ChM zK*m1D$HR8!+hStK{&=+fEOd0nqeTXcjEop+!yQ1%9vl!bj)WsI=5X`{lGLE2jCmPn zVs_T;f+o?Q)iAKof4`|pY^#W5)6rB@yM6aAzC9P2h;5PQ&#pDGBN8(KdxEhXs)55 z@%nWdupkIT=oW0S;&WJ-u@rc-5%4eP|3ukHm3>PhH2x9<`o$NIG$TE|3xt-T9MzIi z$kRfgP4yzcBOvJR=?VGxvAwf1JpL*KDzs?|KKWn!4F7!I;KE=BVtGEDOeD*zqI&`Y zTL1Pv0q6Bsl{PvW8ecG@Nn%BzPz(wQdD+>aU;w`+SV>9gJwACP0A7HL+myO)&sJ4c zot+$X5EzC;0}tb8;ljLB0SH*8{jZIiV_iVZIt9Nt!y{!YJ6r{Q2LPhC$ti$eYO{-o zT*SLf@j>^@0t)Masuet4$<%GE%o2sYKAZiW4)$qqA~kHU56w<;`?l~-EA34N`MliR z+U;*(dL2N!q&qc4_!uO?Iu}aD#tWrA!WircN(FN2PJ%wET2vgLgAWmCqm-OhIj;1V zTaHLdO5)ORTaD^-$7U)dZmzB}aomSM4!0Hd!7bjBs9sF(5xLMV98pLRM76X?NpjXU zoS`RK7#Z6J26kIRY5Dm0W@ctUypm(rkj`1Cqy{s~Wigo5+uJJ>#YPFp@ZNGCraMqv zOhrZYP6Wk6u_RUdMJ*SxQ8DgA1bR8<^>sBhP>|*l5D;*fc8A}>e_9E=IvI()5F-?N z0H(NGIa{SmiVHZn6YxK~xn?eIZky?P#ER+!1i*Zku1|<2V6Eij4$#0zIxKI4ll=`*e*mUs zfb;`kYX1TUoGoMArnU=(a+|J)fld*z z$a&l1{4%~I=t%*2`r+YWO-;?oX00tynnmKJA1z>kwbRbUB@r|zP*&%syFFJkKRuW^ zO09STS~s9ovwG1$PZ1x*pI-0V3ccXYq4Lx018Mi8+}h895!>B zmH(>yy_%YurYEp7f%N7}z43qbgh%fCR@m5ndwY98etwN+vuch28yg#yydJQ6%vaK@ zgN08P4lX%l0iM(WXjB-t-$H!=hKtdGM+OF750XqBS>v&t@@S!SX_bGi$17qX7O9*D z8Ze-Jg+b$Uz?CC4*zOFip>1qzKx;0HvSqVZ$~tSsVi`OE0e<^zHt^_F28^95C!J#h z0s--F>e@MsGwD@@N2Qf4%m zr5GY|UbTM9`Qwt%rKf(Z+EtHOS*4Ogqj&@wqXwTM7;eFdd4jz z2&iEa>3@Z!0vLMW}Es^NZ{ zC&(M@$y>)HI6MTfGVDKhi9;~SLy-VH4yFv$7k1yMBB@RB*WjBj*Loh{e~N7fEXSQl zodM{<1b%}svuIcKPETJX`%wXQ__Xrwjpfa)WbKAT2-lo4(ya!H%rdATgG13UI38I0ks|2Uoq|YGx~`BXAq5 zPbVBNu23aXKzp#K;n1?5lGI?W_DcA+d7RaxzSQnneJs#{jE^5I?4dY3&7Yy35MWu4 z091m$V+Ye*u4he2nL1n^<4B}p=KMlJBmPEYLvKj}K(V5Y;?UG6$~u)PeGNBMydq%F z)9_Gqa|KIBOt8gJ+QTiQs2r^mZ3gTiAOKZbk|?Y`4tKoZT;=n z0Ei^K2%`RR1h$xpU6~X^loycs{d;%6b!lk@fc@Y9MAb#7_&HfTKB)X& zh>eJjh1}t@a~lQv_d)mv*LUpq9bA~QtUNYLJ2Rj^zaIXOV(`0;?^tNfnV!fDciTNG z^(|M!iu4zTs>++60TGBNn>t~ukkJ)jCW#cRR=Sd`wwTT&6y-xrW z*s4JxvotR@nwU!Or#KZ<5~2Y*zMBaBIlmAiPXVG!9w*dm=~VF&@H}jLrXYGc?$I-_ zcUpP&3iw(9-_OSgQYL_k6+*TtIlemlE{!9AWc;%pz3OYpJ1+Rx`JIXiDu6;k z`2>RT4^H=Z?2uUOzyD#{HD^-b z_3^EF#ryZG!vj>^Z}g#aZrimF&IpU|H8tfpFCjJZyVxwL$Y(=uWlJM9WyE6%muz*Z z5WB_OQE!G0$^kjPAtY^cT zGzBSZi{b@`2NLd@pi#RV%zC%-_@1w<{ppvsukLW%yo!RMv+r-++ZsHLwH zXLeF8oOI^>e5a|ivu28G;Uv!KROK?Jylq=}2R5&1t>HaY@`_w%eD7f*>5`-x=UtPt zjgt=JN`jLJ>$uIC`{ja@3GX)p2sNq5-=t~^ENz)Kv5ofVBI-=nP zcVq>LyU12*O`X=?4`vCGG6^6^HNM*s(*35+eOD4`-*3;Dw3k`AhIFV)8#v#TW9yIia@nxWwAi0%Boz z^%jZ4YEK75w|%u~k@Pch?tO+vcEh5Pe4`V1+rz_+r4ioX6J-mPw#3b`?cUK3$-0z6 z>NH`$xiOlA?3~T2+tFT`+_fk(%qrhND5nSUD?27>NpV;S$FbvZGUhcn1J{ZBravnp ztRpFoi^{K~!`b_mt#qG^WS&j_XhA1ge6Rmp`>_de-!?hgHu0fzgbbA&?uuDkehs%B z4Lfy}!L?Lt)IhJ;hi~G0-xJm%=bmwq8O?0Ci{|ODS>A4VNNhYmSnrS`&bHtgN;HAe z61{v4dUNZ3SQVpZr)FfpagDNv6jfpleTu1z-c4}0wQR=-+0l{FsX%^Y^)sl)Uj1Rx z$t?WI`YCBt4wdIt3j#a1M)1dIWZ*k^!b3NG``(5ZG=y|mBGE*ri^VAyXSTt8r*lz- z*0t*GTnFd7*k>DCP-eC#ZD8V4FHfr4l$=^v2fRIS-uO8ZOIDWgkzUN72aDs%n;L8? z*B?K=dAm%*c-BNA#~&w{Sc#9^AF?RhOvKpdj!S=hN!<6XL*#DDV(&r>SDV3y(464g z<2jbGPh=&;6UXb`4J5p$fA2=Ds7cCROq&vz%VlZm)8ypQ+9pb5CS`(BZcAR$eLnVC z_viUaiKg7rl02n*lz3Q!s(3iR@K=5>vM!IL%#PuL0*{J+C= z!tSenal@CX)cfH}?QNuyDjY0Gd#W8WZ3a8bGK)s$^2VYYCF9Lcif1=kK4u2}Jo*`0 zjI3c+F?v)u4x4-;X6`MVo7L8cC&#a>8?@gTbQ~Br7Q7$%In_fLW_3!G>lEv61yAI} za`+VWz4w`H0q>T83sVloG_GaUL6FQ%yPSUn^R-5di!tL9@HzzSjhU7jwr3iN)&!v}mEBLK z!mO399n7T8olY$Dtk>*se<>NeoeDf_uSWc|25eK?lX7Bh){h84`c3d#UCq^Q^jp_{ zUOKhD#HQX($8U79M6zM^JdUAP*+5{dB7I+NOt0t*X~pYXb=z_?J#*fkBJk0%y9UJd z?F+Vg_wK(1UqLby?~<)(Y9Gqzk$6_~Y16Sx7aNpR)1Szi?Dker_S=ic&I=<`HNrKybRUo^x7QbLq}cPtPahtKzWYcI z8ZqHKWP2I%1p>frmt*A@c!>HYq7$_{co~{Q{NWA zty_&4_Eph{JLY(&DsDW9@gkC%UKmHHqt|`25f!ucgaWdZ}*^g;c53 z@tB_0^yAUphr~LCR&M*dmiCMX`ycOg1lcyBYod^Kj1h0JJxN@88lQG-wPU+63PT+y zgl=BRjlmxllgW#DGI%UrElTfZH}&X#`{v=hwTjg;`un&CPIM_)@ok-WikEEiyD^() z_oT){NN)Bq2l)9NF*PwNzjqT6R?X+y5qKiAq3!QFGT5B1C}yjtNS0!6&Ltlvgl~OP z;qQNL+vYFe=Qe(ELrw1PvA*b=(kUy`Z;_U!A6oTqmq~IEfA}C;#|+i5@;YQ_nXD;F z7TD^qWsvb-?&(NvdGGp)p6T_C>avc?#=ZJy(E9p_N`+g6&v6p?C*0AT=;2g}*+zW7 z9YO81o}LeYTsy2N3b5u_uO0PM)4vT~R)I{=CR>)30{H$nKyXh?Ko&EWnrEPL2e9bT=HhjhE zS90}?Rjrf9yp`!s`YKuinS3h7BZL%xd@m~K7g)Q|k+)sg~05>^Wnqt*Jrr z@-%O?A2YgfjYZ!(;6bvEMMqA@yO@t0wNrjc4|(AG8u2MyCxfwI#EsGT=6fj1XpB=z zm;NdYf9D}_=)K;t9{cV*nbWgV1A%(xa*up?CA*8d+Co@c@<6J`tf&y0{@lu7E&(rM zyq>k99*4hD`lQ!ADpDRNnqo78D0$XjnrzP8|XfMw%G;U zEpLwmj*(v3awrH~aRs&Ws@&8tO`0)m&@3=9K6f-aa%45?tH5l=Cr|qdLuh!1Qe#OZ=u$J65={(%Wl#9F&Kiuy41%7khSDR~IkI4|jFg1g}vu~3)0%l~YH?@4cd z-m?p98(94CRENXeS)WUebsHknQAQ74-5*FDQD_K!a3X=6Mc>1DcklUws`+bJN&l;| z^A2ljOV@Z%K&69H6r>6YNC!cx6hTl?dJjmKUIe6riWKQBAXR#AAygrBl#cWuAU$*` z(jj-nnKN_moH=vv%|ASOLiS#Lt^NMq@5_hm-mB2Fa2Lw`Xho4;LyM_AyX$8=ilc}< zlU;d`+iNYDh_I>S8YI1Y!KqfA1H`aj1$!Glb^bdzy=urjy4^j>i5>Q$v%l+s;rm?> z@@vqsD^8BJtLasTyKq6?K1xRIqK9I9@4V!&9(c_99c>gbdtO5M^!-y!mV5goq|__4 z2jQ#eLAD0>-oC;HooBv*;-Cj0XJD-YV>(#FGn`?SeKxO5v-+3i;Cos3Y~uZC@)0$# zy0_lM_*sN?1c{?qzhEWo0V(}EFLn1v-A6UZ$Tfs%S&@=Gbs_#2LC`Vy6!Q4@KS7!( zcCeHMQuglmeIR#d0T}S(1`^sN2=<3@>2=hRLHzt7?$}CemO>jmep!KC3_?UEVu&?4 zLhdsC=j&lVewE00S-c-!asguG_XAhj$UruNFX44qGboSG1GZW5IGR<$CP)6n9-Dji z-)kHJ)hM#YLA#s#{4l*(cBwP!-MlsxZzL$_gRRI%x85)Uhg(*5c6kX1#+Ye6!n(O2 zMpr-y!Ao0)9=Hl2koZWfMgs9;T>y^M|MqbUV=U+uP`j$Pmq@wwszP0uKo&xr4y+du ze9NH`tI+(fSHa!{%`Z*%I@Q7e57Gqj2aPvi=2WZC5od?04F20SvE_vYUs4d@=hvpb z0O2KL{NGy&-20D?i4JuEH8|@BWhJ2(ud$in9}NKQj+@MQxyTw@NWW&nWo(XwKUebK zw1FS@Rb2mNFcz>~hwXX;HduGI>I(1~|Fdwf5??zAvar1UNBP0U|1mKBqMvV>7&Ji+ z+O9sMyYIBn}0~@W) zOrr!ld;W&3ZgJJrA}%g&)wFaf#Mg9N%cFy2U$iG*9b;X6<$li`jE5t#z^b3XTh zXMba|8t{Os2r4S7s>zXnz(CGx1fUFjzG*4QoE$b`VPR2GRA*=Ih^%Nz3BXeuP9Q(W zh?UD9l@LJE$hxO0&~KQ1$?P)|!}jHa8GrFEg5MzHKa9AuCw(sGr$<2Ruc;C7_4OS^ z02ziHSDFkr7`J{8a#88|^GL?aKzlACy#5M1Z0nwUdQKB2kFELu7`+3mu#d^WCsz`^Ws70;0B>V4}9(gkSo*(Sj2!K^)Y>kotZ%r2hIhI)e4h}N^oCW{7JpE}D z<~bJ|A|-+0Q$QdFjN5^jTNGD19qBrMR|%jNR(uAt3+}_U6%`fVhK4|(0d}}cDCQ1r zi}S#`b+B*Pd3m4HQKY#ry(RgV#qdve`{!C|L>=>Wp-C2W5Bq}3vd3ba7I6Dxg>^7X zA5<3^)Zb%d`~hM|=F;9U{uJjqN0ya2e2HWN3baW5DkR+k7at!V7uU#gb~;cTwcFL* z&6T+V5JOH@*5S#?2VZEdQ@asw87DD>4vT2Dw5kCw*?vY%O`WgKQPKy(E=o#E?QLvK zbr~Tda}5X6dSJ@AxF8O5T}Yv7YH;8K227ln(~eh~e&a=wusyx#D>Ohclag(_r@-P2 za<_zpgm@r%feZky&y1>fv!4uX3!Vc{E6DZlwYy*{pB!x2p9}y&os>?b48Y9kQY$hU za5X@T#QXI~GPJvUdhT*^?rv{iAtv59Yk{PT0f+wl%8EPq1Kg@>D+mOlGyG;u_ad+j zS!mEgM2PA?dgJa`l0H^e-rwG?1rzeu+L)fiEgv|H$FMJh8-k|FR!s!_?s7|`_eoLi z)(mpSlph-!F$q#!*+PB*WUU@77Y)4VgO)|P)(8}cn0b5)SOVh93T&Ta>*`eN1u%=; zaKRAs`b(TW=M8#=W)I?lQapToHdfY`2vlXI;MxjEQ1i`FFgQ54xw*N>idH>R!4q1a zG73^&I6FB3KArONc^1QC3L^+vx{{>wq7xZ(2Ehr1`y4613mrMzJ zFuRqqcR&!_;^Ja_@Em1!ettfGVyMAO$PW>F`z;XMa6PP}wDDLeIXX49WYCg=f@0W8 zXc7f!dNJqnY7QYhcMU#q3V0IP;J56@I-2(q7$f$@K@1Gm3NG#os0e7K+^j59b93Wo zghr4uI)$i5x9B>94;G9Y@X=%Pp)`B|O>R%O0k1jeR)EJfw6r?g+Cu5>N4vPZ#oNhK z(-ti*Dk=g5C^NM4i;gCqhn*VVI4fk(`W}=ymZXfvAG7% z129hU@$iBJBg?D~*cMcB9|3f%lnnsrTr&nBd}*?!-lXNFrH(p5h(oe&G2rahc6O3Z z3tzV9JHd3z$jAVpP}*a7kSS*RhZUhCL24fxNZp6o!3YGA340#i8XA=3DWWbRKYQ_M z$nIK5@xGyL^5X;U4TP@2^XH7`i7*&71x0jZWK&gvG@kpgPMh-M(}{Aht}A7$I?~;N zpvU65^yK9L?rSB#=7;lkzD9FC6u^=>t#!|NM?;IaHV4NXEq!k0Q`2pXq&pR2rZqB~L{#jGue8~|Fr8({#BzO%pA1TnsD3pSW+ ziYrwakc-kzP8A?9AG5W%7?+&fQ<*G%Nl*1Qg?%rmJ6jD6kcmQsx_MYha+{y~p>-~F!DBnYkkF0wi z7Bk-dDEKm3x0suoyF-}Ph%rhj8&rMNQc&d01>VRW5cSmF{*W#$%7qIoUwKy@>0lo} z*14={hUOFUHUVm6C3pku`VXG%fUXy^j%U0aPpJ55j4?t~%gSmM%QZnP7C7Uf6@gg- z|E)hb5)WX_5qRL3)^7Apd{rMHWDqK63JyAZyjs6s*z1w%RhVAs-z_Sn2^CqtB$A|@ ztHu#mR0wehL3$f}F`?x@1kY*-xUvDH7hpJ^IC{$47B~ z_~~SxYCcngml!`+J7;S(l5rD4qcM_t+z;t*9=X&V<1Hpz6Gk4IIa%CTT&{$_QD~3j4-Ep8F3kP?&6%8``;&9ABx((BaJD5;0Ht3Q1})MA>yJm z`QG~PPwJIwHn}e~uspEWMH(Fj%uG3IB?+x%pVjiLH=LiQu01KZKy%CEv$ZUP)zgkT z2)c8WhvvNAJ&N?R=XMNyadgNKm6oLT>FDr@yTUVEKYDf5(wW%!{yG9boRZG^dwZWo zBNaX^e{#cx_~GP?)m1f51Tsfl@Edwcu*x1k0%{6MJk8Qb+SihHG5_x^{6#R1E1XXkk`YQI%xR=OysWZhCDUx`L zu-^iCaj4NAX5HW+1k+o9u8am+DMpO*uI9$|&9#Eq#|24YhK7YxvGE78iq3`iwWj&@ zw1!4Z-be^~>5aKpsk>Qola|aL7wM>rFLC&uR{rwPP<%Fw+Ik%)T*mnxfLLD}gFnk5{}3Y`a&b6imXO;nw=yd`Ql zVMXs5Z(^}EzNs+UTZ*$s9n;bP#BL6ILDMDSgr1g~PxQz~mwk+!BHzv1sarq$o=1A1 z(HFM;`0~Mxvz@OYY!}HB-I>_FG?XbHc)!SuNb)kT+`3)ZLAcjyvCI*KW;h7*u<06T z(Vf~PjN`vTUAV8_wNZCNlBvbIec^|07++!DU>hOOVYkw*y>hc@>Yr(-S#F7#!3b|f zxEmML6-<;J1QxtFFX=Y9XfAisW-lIRn8})PeNYmaoKs@Wm&#*jHJsS$9Tz4VbJ!O4 zI5YKeFz>dEtB$i-43lWnRKI~;KmW$v(4fXfO;5Nk=Q%P@8KgP-{#pDuQD;_{1vAy_ zp%w5-Idm(TW+rf^`yBlwQAE}&VGO8OoJmA8)c1BSJiJuHMKdqOf;tHA%|)>pK_-ut zWzdx-h{2MYQE~VhYE8jsz1D|{x&|N(y$C7^#I9An!Q{@kpO8Sy+;FN=ZDmROYlMvR zoc!6={vv9Z|KWJ+wTTpklP7L6_o@$5>;i7SW+D-9PgSa$U>HY}i`nW{zLI$m*717I zyg`Zmap=$EbzinqFsk(UeNOi$Yw0{aI>RL^Hob_`XC|Vfczy_JJ~y*Bw5&WEnXY%|SuWHDgiBBuma&jLG141GTrOgxYy>^zp@(@4=67F-U43*BHLFE->| zCnXFlHRQEfyfhIXU?UQ1F{MW~7-~7QtjZj+`s%|>PeoBXL1Npv8H)SKUf*`KrdASy z-!pbg0Nz4qc(YN-v;(MD)NaD%oohb*qU za#j)uHu2(CCuSjKf$XcOqvOVblyNXQkd&LNge$9R9>r3YlPsv2z3`rIBbZua-fcNS z>#VKRk#btq%zgd5Yg$Anu%VY*~92En&_SbF>AN;a1uIPnj1(l_`%b;SPbGw>1>o}O9&t0 z*&<-gfbB1bSaXx#M5Xma0G--M+9p|tcnl*Rm?90s;c-&#E!e8o$14v7o#(cNPX9}mZXZXAqoAg zQMk)+vKjL}??V#$4;1xRnXN`r$fCkFvs5gsCR4P{%iv<= z=88POLT-yiNyEM8P3Jj0Fd;q7T7VYZlw-eSf9IiiQ@=}r8Uf9j0@fP0S`A(1orCAv zIp1r99@-_&F~{7EAb#y=w~%9|QkYwyNA$V6FgXa<-_tR9Aoed?iKvgI%L|{zD?6vZH$WlCWo+& z4@g<%wxjNZuBDEy%A)@4TUWHd&-YdtEX!_{l+YMDa}!RRe(d<8E6h?jzS#MZ?GH8- z;NjwYe1KJ4IsgRYfvQ@}5u7h_1X`Jal%&;65G#HPaV9z5|ac z&5;Kv^)TDJc4PngK9Cy8qar4YH$GJth6bf7k=Tmm)_)k5=WMidy2_)SrdRD?lUPgu zkBLb7-bOgmE2Wdv_V$jXT~$SWhY`t+>UaVOj4Gm#(X$mB4M(wN^!I7rO(H9^{2tk= zWgTSs^^i$!c-86I2liHVxjpx!6K>t|!Q>Pgwvg$C*Ysv1*jXl4MY*{bAoBCt`!oO6 zryp|3HA*V%xO2H^8Fcz{2xxe-C^lOaI*)3*(uf+uf=B>83@~ z`(`mG%5}o7Px-AW>5$GrxVGEv=6(hjof!I^mccoX6f^vCu`8IfYEq-4wC2996Q0tL85YqqTZ%(u(+uTi z;(!eug_Sa}=UMX8^vbwrO50nVc?rUb=186THomF?2+1zyWExo(vW~I@Rx}ci&15qb(M-?K_v~k&L!Y?$U7|cAp9kUX6MOKL`&T1jm)9k#d60 zSzGgJBW%z^^j@);#*BoHjGQl&UgBO`>&~@+C(JO{ic{Z|J8T)m`6}C*V>ZG470!uO zgjctO^r>hW8&$)tF@k;jChc;2U30T98kHQoVqZ*0JkziaI}Ptqo#equr{3Ekv4Ekx z4@}v~B@$>(jbwPIpw5#n`9+EImUf|Ot-UWuhd#nPTB|peQ4%WF-rV_*r@kVUul3Vz zR5J;!D%CEnZ}=k?RQoKcsu(z-qD(*H+FK*0@;Hk|$m@%FTb1anJw{?bk+R_mul2@b z`!d_@bUBy;3Isd-#c%QVESPP)Oi6r+jf||nCAy{-*7&ME?_<~$_sJ}gwpQ)RX5yfR zk_A2b3&^605_dG;h{}p=pMMVyb?EF^R{7V8B6d@oYr=MyS6SY>Rr4Q#)A7DS3=aBf-{Z?uHn5<^T4<8qh|^44 z(~Yq58tB+&FGohiGZ?GFk&XV1U*H&6V;+#FuU||8b44Ymcw5eVy{EibB)1@HK9XL12o9Xf4&((81kGhjaWtUy8`U8}nWv}z=?~)|UZ_ zzA4q$SS2pXBtwbL{!(nkzG>vSv*+vFMkmeekF9%Uu4F z-s1b)2Qn#R*!A-u1v}zIZI561%;#Ekn~bCeM{#67QO)G@aV;@6(S>TNsKMe*e19L^ zv8R)EFgfNFO+V}vBBBi$s#2p3GZCp70z1B^cA8`bbqm(q|@IYrq6fZ(!A2%_QQ6ofLZ?h#iBqZ#1ytlj&B&r zR0_j!(hXmG6?mMS@qs7q;aKF?vQXnmp2`zhFK_8It|EhDWdG)#l`akBkB8=mNTwKv zkI23{l_~z?#h4=Nm5&aFX49NQQdc?{%-Ib3PRjQ)PDiW1!PZss%nc^L^EEi99na^n0O?cv?V;8fDX68S1& S;P Date: Wed, 27 Feb 2019 17:09:28 +0100 Subject: [PATCH 57/96] * Adds store groceries README for instructions to run the task --- .../mdr_store_groceries/README.md | 51 +++++++++++++++++++ 1 file changed, 51 insertions(+) create mode 100644 mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_store_groceries/README.md diff --git a/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_store_groceries/README.md b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_store_groceries/README.md new file mode 100644 index 000000000..275bb8167 --- /dev/null +++ b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_store_groceries/README.md @@ -0,0 +1,51 @@ +# mdr_store_groceries + +## Goal + +In this task, the robot has to correctly identify and manipulate objects at different heights, grouping them by category and likelihood. This test focuses on the detection and recognition of objects and their features, as well as object +manipulation. + +## Subtasks involved + +- Open door (not yet implemented) +- Listen for speech command to start task (Currently disabled) +- Inspect cupboard and table +- Recognize and classify objects +- Grasping objects +- Placing objects + +### Setup + +1. Place location: Third shelf of the only open cupboard in the lab. +2. Start position: The robot can start in any position and oritentation. (But it has to be localized first) +3. Objects: Objects are placed on the table next to the shelf. + +## Running the task + +The store groceries task can only be run on the lucy account at the moment. The first step is localizing Lucy. You can localize Lucy using rviz. This localization step can be done either from running rviz on lucy or from your computer. + +1. To run rviz in your computer, first export the ROS master from Lucy. On your terminal, write: + + ``` + export ROS_MASTER_URI=http://192.168.50.201:11311/ + rviz + ``` + or on a terminal on Lucy, just run: + + ``` + rviz + ``` + +2. Load the config file with the hsr model. (If you do not have it, ask to one of the team leaders) +3. Use the 2D Pose Estimate to localize the robot in the map. + +4. Now that the robot is localized, you can run the store groceries task. In one terminal, you need to run the SSD Keras Image detector: + ``` + roslaunch ssd_keras_ros coco_image_detection_server.launch cloud_topic:=/hsrb/head_rgbd_sensor/depth_registered/rectified_points + ``` + + In a different terminal: + + ``` + roslaunch mas_hsr_store_groceries store_groceries.launch + ``` From 1a00d10547e2cf5cc36a3233aae9b7fb327e8732 Mon Sep 17 00:00:00 2001 From: robertocaiwu Date: Wed, 27 Feb 2019 17:51:37 +0100 Subject: [PATCH 58/96] Update store groceries readme - Changes to make instructions clearer - Remove Lucy specific instructions --- .../mdr_store_groceries/README.md | 24 ++++++------------- 1 file changed, 7 insertions(+), 17 deletions(-) diff --git a/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_store_groceries/README.md b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_store_groceries/README.md index 275bb8167..6f24cf808 100644 --- a/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_store_groceries/README.md +++ b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_store_groceries/README.md @@ -8,35 +8,25 @@ manipulation. ## Subtasks involved - Open door (not yet implemented) -- Listen for speech command to start task (Currently disabled) +- Listen for speech command to start task (Currently disabled. Note: this is not a requirement for the task and it is only used for demonstration purposes.) - Inspect cupboard and table - Recognize and classify objects -- Grasping objects -- Placing objects - -### Setup - -1. Place location: Third shelf of the only open cupboard in the lab. -2. Start position: The robot can start in any position and oritentation. (But it has to be localized first) -3. Objects: Objects are placed on the table next to the shelf. +- Grasp objects +- Place objects ## Running the task -The store groceries task can only be run on the lucy account at the moment. The first step is localizing Lucy. You can localize Lucy using rviz. This localization step can be done either from running rviz on lucy or from your computer. +We provide and example of what needs to be run for the store groceries task. The first step is localizing the robot. You can localize it using rviz. This localization step can be done from running rviz from your computer. 1. To run rviz in your computer, first export the ROS master from Lucy. On your terminal, write: ``` - export ROS_MASTER_URI=http://192.168.50.201:11311/ + export ROS_MASTER_URI=http://:11311/ rviz ``` - or on a terminal on Lucy, just run: - ``` - rviz - ``` +2. Load the config file with the robot model. -2. Load the config file with the hsr model. (If you do not have it, ask to one of the team leaders) 3. Use the 2D Pose Estimate to localize the robot in the map. 4. Now that the robot is localized, you can run the store groceries task. In one terminal, you need to run the SSD Keras Image detector: @@ -44,7 +34,7 @@ The store groceries task can only be run on the lucy account at the moment. The roslaunch ssd_keras_ros coco_image_detection_server.launch cloud_topic:=/hsrb/head_rgbd_sensor/depth_registered/rectified_points ``` - In a different terminal: + Then, in a different terminal: ``` roslaunch mas_hsr_store_groceries store_groceries.launch From 0daa8d91bbd098466720556a1fa27a9c02294832 Mon Sep 17 00:00:00 2001 From: Roberto Cai Date: Wed, 27 Feb 2019 18:21:42 +0100 Subject: [PATCH 59/96] Remove Lucy specific instructions - changed steps 1 and 4 --- .../mdr_robocup_tasks/mdr_store_groceries/README.md | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_store_groceries/README.md b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_store_groceries/README.md index 6f24cf808..c5df15839 100644 --- a/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_store_groceries/README.md +++ b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_store_groceries/README.md @@ -18,7 +18,7 @@ manipulation. We provide and example of what needs to be run for the store groceries task. The first step is localizing the robot. You can localize it using rviz. This localization step can be done from running rviz from your computer. -1. To run rviz in your computer, first export the ROS master from Lucy. On your terminal, write: +1. To run rviz in your computer, first export the ROS master from the robot. On your terminal, write: ``` export ROS_MASTER_URI=http://:11311/ @@ -31,11 +31,11 @@ We provide and example of what needs to be run for the store groceries task. The 4. Now that the robot is localized, you can run the store groceries task. In one terminal, you need to run the SSD Keras Image detector: ``` - roslaunch ssd_keras_ros coco_image_detection_server.launch cloud_topic:=/hsrb/head_rgbd_sensor/depth_registered/rectified_points + roslaunch ssd_keras_ros coco_image_detection_server.launch cloud_topic:=/point_cloud_topic_name ``` Then, in a different terminal: ``` - roslaunch mas_hsr_store_groceries store_groceries.launch + roslaunch mdr_store_groceries store_groceries.launch ``` From 6177c33beea2f069ded8587065b3fc4a87ffb63a Mon Sep 17 00:00:00 2001 From: Alex Mitrevski Date: Fri, 8 Mar 2019 12:37:31 +0100 Subject: [PATCH 60/96] [monitoring_msgs] Added an event message --- mdr_msgs/mdr_monitoring_msgs/CMakeLists.txt | 1 + mdr_msgs/mdr_monitoring_msgs/msg/Event.msg | 2 ++ 2 files changed, 3 insertions(+) create mode 100644 mdr_msgs/mdr_monitoring_msgs/msg/Event.msg diff --git a/mdr_msgs/mdr_monitoring_msgs/CMakeLists.txt b/mdr_msgs/mdr_monitoring_msgs/CMakeLists.txt index cd5d586bc..3cefff8d2 100644 --- a/mdr_msgs/mdr_monitoring_msgs/CMakeLists.txt +++ b/mdr_msgs/mdr_monitoring_msgs/CMakeLists.txt @@ -13,6 +13,7 @@ add_message_files( AudioEndpoint.msg Endpoints.msg ExecutionState.msg + Event.msg ) generate_messages(DEPENDENCIES std_msgs) diff --git a/mdr_msgs/mdr_monitoring_msgs/msg/Event.msg b/mdr_msgs/mdr_monitoring_msgs/msg/Event.msg new file mode 100644 index 000000000..895091ab9 --- /dev/null +++ b/mdr_msgs/mdr_monitoring_msgs/msg/Event.msg @@ -0,0 +1,2 @@ +time stamp +string description From 21e9ca613b4accd7db681770d74c2123c540171f Mon Sep 17 00:00:00 2001 From: Alex Mitrevski Date: Fri, 8 Mar 2019 16:51:43 +0100 Subject: [PATCH 61/96] [move_arm_action] A DMP is now also used for top grasps --- .../ros/src/mdr_pickup_action/action_states.py | 11 ++++++++++- 1 file changed, 10 insertions(+), 1 deletion(-) diff --git a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_pickup_action/ros/src/mdr_pickup_action/action_states.py b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_pickup_action/ros/src/mdr_pickup_action/action_states.py index 0fb11efd2..efa488a5a 100644 --- a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_pickup_action/ros/src/mdr_pickup_action/action_states.py +++ b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_pickup_action/ros/src/mdr_pickup_action/action_states.py @@ -129,7 +129,16 @@ def running(self): rospy.loginfo('[pickup] Preparing top grasp') pose_base_link, x_align_distance = self.__prepare_top_grasp(pose_base_link) self.gripper.orient_z(pose_base_link.pose.orientation) - self.gripper.move_down(pose_base_link.pose.position.z) + + pose_base_link, _ = self.__prepare_top_grasp(pose_base_link) + rospy.loginfo('[pickup] Grasping...') + arm_motion_success = self.__move_arm(MoveArmGoal.END_EFFECTOR_POSE, pose_base_link) + if not arm_motion_success: + rospy.logerr('[pickup] Arm motion unsuccessful') + self.result = self.set_result(False) + return FTSMTransitions.DONE + + rospy.loginfo('[pickup] Arm motion successful') else: rospy.logerr('[pickup] Unknown grasping strategy requested; ignoring request') self.result = self.set_result(False) From bb50588d78fd094168621b0a79a2a9d31dee43e2 Mon Sep 17 00:00:00 2001 From: Alex Mitrevski Date: Fri, 8 Mar 2019 17:01:01 +0100 Subject: [PATCH 62/96] [move_base_action] Changed the name of the server in the client test script --- .../ros/scripts/move_base_action_client_test | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/mdr_planning/mdr_actions/mdr_navigation_actions/mdr_move_base_action/ros/scripts/move_base_action_client_test b/mdr_planning/mdr_actions/mdr_navigation_actions/mdr_move_base_action/ros/scripts/move_base_action_client_test index e7f8a06da..f08640e86 100755 --- a/mdr_planning/mdr_actions/mdr_navigation_actions/mdr_move_base_action/ros/scripts/move_base_action_client_test +++ b/mdr_planning/mdr_actions/mdr_navigation_actions/mdr_move_base_action/ros/scripts/move_base_action_client_test @@ -8,7 +8,7 @@ from mdr_move_base_action.msg import MoveBaseAction, MoveBaseGoal if __name__ == '__main__': rospy.init_node('mdr_move_base_client_test') - client = actionlib.SimpleActionClient('/mdr_actions/move_base_server', MoveBaseAction) + client = actionlib.SimpleActionClient('move_base_server', MoveBaseAction) client.wait_for_server() goal = MoveBaseGoal() if len(sys.argv) == 2: From 14e21d38e9262f949efe9f4c0ccde3e19e183a6b Mon Sep 17 00:00:00 2001 From: Roberto Cai Date: Fri, 8 Mar 2019 17:06:45 +0100 Subject: [PATCH 63/96] Creating folder for new task: take out garbage --- .../mdr_robocup_tasks/mdr_take_out_garbage/README.md | 5 +++++ 1 file changed, 5 insertions(+) create mode 100644 mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_take_out_garbage/README.md diff --git a/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_take_out_garbage/README.md b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_take_out_garbage/README.md new file mode 100644 index 000000000..6dfc4c2f3 --- /dev/null +++ b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_take_out_garbage/README.md @@ -0,0 +1,5 @@ +# mdr_take_out_garbage + +## Goal + +In this task, the robot has to out the trash bags from the two bins located inside a room. All garbage bins in the apartment are emptied by the robot and the garbage bags are subsequently moved to a specified collection zone. From 83005d6079bb4073c68cbb704a46d15199ee3821 Mon Sep 17 00:00:00 2001 From: Roberto Cai Date: Fri, 8 Mar 2019 17:08:25 +0100 Subject: [PATCH 64/96] State machine definition for take out garbage task --- .../config/take_out_garbage_sm.yaml | 207 ++++++++++++++++++ 1 file changed, 207 insertions(+) create mode 100644 mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_take_out_garbage/config/take_out_garbage_sm.yaml diff --git a/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_take_out_garbage/config/take_out_garbage_sm.yaml b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_take_out_garbage/config/take_out_garbage_sm.yaml new file mode 100644 index 000000000..8f85629e5 --- /dev/null +++ b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_take_out_garbage/config/take_out_garbage_sm.yaml @@ -0,0 +1,207 @@ +# Description: Defines a state machine for a RoboCup@Home take out garbage scenario +# Author: Roberto Cai +# Email: roberto.cai@smail.inf.h-brs.de +sm_id: mdr_take_out_garbage +states: [LISTEN, PROCESS_SPEECH_COMMAND, ENTER, GO_TO_BIN, FIND_BIN, PERCEIVE_INSIDE_BIN, PICK_OBJECT, GO_TO_COLLECTION_ZONE, PLACE_OBJECT, CHECK_BINS_LEFT, EXIT] +outcomes: [DONE, TIMEOUT] +state_descriptions: + - state: + name: LISTEN + state_module_name: mdr_listen_action.action_states + state_class_name: Listen + transitions: + - transition: + name: received_command + state: PROCESS_SPEECH_COMMAND + - state: + name: PROCESS_SPEECH_COMMAND + state_module_name: mdr_process_speech_command_action.action_states + state_class_name: ProcessCommand + transitions: + - transition: + name: start_store_groceries + state: ENTER + - transition: + name: continue_waiting + state: LISTEN + arguments: + - argument: + name: start_command + value: take out garbage + - state: + name: ENTER + state_module_name: mdr_navigation_behaviours.move_base + state_class_name: MoveBase + transitions: + - transition: + name: succeeded + state: GO_TO_BIN + - transition: + name: failed + state: ENTER + - transition: + name: failed_after_retrying + state: FAILED + arguments: + - argument: + name: destination_locations + value: [INSIDE_ARENA] + - argument: + name: number_of_retries + value: 3 + - state: + name: GO_TO_BIN + state_module_name: mdr_navigation_behaviours.move_base + state_class_name: MoveBase + transitions: + - transition: + name: succeeded + state: FIND_BIN + - transition: + name: failed + state: GO_TO_BIN + - transition: + name: failed_after_retrying + state: FAILED + arguments: + - argument: + name: destination_locations + value: [BIN] + - argument: + name: number_of_retries + value: 3 + - state: + name: FIND_BIN + state_module_name: mdr_take_out_garbage.scenario_states + state_class_name: FindBin + transitions: + - transition: + name: succeeded + state: PERCEIVE_INSIDE_BIN + - transition: + name: failed + state: GO_TO_BIN + - transition: + name: failed_after_retrying + state: FAILED + arguments: + - argument: + name: destination_locations + value: [BIN] + - argument: + name: number_of_retries + value: 3 + - state: + name: PERCEIVE_INSIDE_BIN + state_module_name: mdr_perception_behaviours.perceive_inside_bin + state_class_name: PerceiveInsideBin + transitions: + - transition: + name: succeeded + state: PICK_OBJECT + - transition: + name: failed + state: PERCEIVE_INSIDE_BIN + - transition: + name: failed_after_retrying + state: FAILED + arguments: + - argument: + name: number_of_retries + value: 3 + - state: + name: PICK_OBJECT + state_module_name: mdr_manipulation_behaviours.pick_non_rigid_object_from_bin + state_class_name: PickNonRigidObjectFromBin + transitions: + - transition: + name: succeeded + state: GO_TO_COLLECTION_ZONE + - transition: + name: failed + state: PICK_OBJECT + - transition: + name: failed_after_retrying + state: FAILED + arguments: + - argument: + name: number_of_retries + value: 3 + - state: + name: GO_TO_COLLECTION_ZONE + state_module_name: mdr_navigation_behaviours.move_base + state_class_name: MoveBase + transitions: + - transition: + name: succeeded + state: PLACE_OBJECT + - transition: + name: failed + state: GO_TO_COLLECTION_ZONE + - transition: + name: failed_after_retrying + state: FAILED + arguments: + - argument: + name: destination_locations + value: [COLLETION_ZONE] + - argument: + name: number_of_retries + value: 3 + - state: + name: PLACE_OBJECT + state_module_name: mdr_manipulation_behaviours.place + state_class_name: Place + transitions: + - transition: + name: succeeded + state: CHECK_EMPTY_TABLE + - transition: + name: failed + state: PLACE_OBJECT + - transition: + name: failed_after_retrying + state: FAILED + arguments: + - argument: + name: number_of_retries + value: 3 + - argument: + name: placing_surface_prefix + value: collection_zone + - state: + name: CHECK_BINS_LEFT + state_module_name: mdr_knowledge_behaviours.check_bins_left + state_class_name: CheckBinsLeft + transitions: + - transition: + name: no_bins_left + state: EXIT + - transition: + name: bins_still_left + state: GO_TO_BIN + arguments: + - argument: + name: + value: + - state: + name: EXIT + state_module_name: mdr_navigation_behaviours.move_base + state_class_name: MoveBase + transitions: + - transition: + name: succeeded + state: DONE + - transition: + name: failed + state: EXIT + - transition: + name: failed_after_retrying + state: FAILED + arguments: + - argument: + name: destination_locations + value: [OUTSIDE_ARENA] + - argument: + name: number_of_retries + value: 3 From db9611c581f1dc5aff156ab60f224a3f7435e7e1 Mon Sep 17 00:00:00 2001 From: Henrik Schnor Date: Sat, 9 Mar 2019 13:32:27 +0100 Subject: [PATCH 65/96] Update path to renamed file --- .../mdr_find_object_action/CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/mdr_planning/mdr_actions/mdr_perception_actions/mdr_find_object_action/CMakeLists.txt b/mdr_planning/mdr_actions/mdr_perception_actions/mdr_find_object_action/CMakeLists.txt index c7a31e344..c8d3c5a66 100644 --- a/mdr_planning/mdr_actions/mdr_perception_actions/mdr_find_object_action/CMakeLists.txt +++ b/mdr_planning/mdr_actions/mdr_perception_actions/mdr_find_object_action/CMakeLists.txt @@ -48,7 +48,7 @@ include_directories( install(PROGRAMS ros/scripts/find_object_action - ros/scripts/find_object_client_test + ros/scripts/find_object_action_client_test DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}/scripts ) From 01d4bc86edba162d192bcfcf76785002bab5bfd2 Mon Sep 17 00:00:00 2001 From: Roberto Cai Date: Sun, 10 Mar 2019 17:07:59 +0100 Subject: [PATCH 66/96] Add package build files for take out garbage task --- .../mdr_take_out_garbage/CMakeLists.txt | 41 +++++++++++++++++++ .../mdr_take_out_garbage/package.xml | 40 ++++++++++++++++++ .../mdr_take_out_garbage/setup.py | 11 +++++ 3 files changed, 92 insertions(+) create mode 100644 mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_take_out_garbage/CMakeLists.txt create mode 100644 mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_take_out_garbage/package.xml create mode 100644 mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_take_out_garbage/setup.py diff --git a/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_take_out_garbage/CMakeLists.txt b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_take_out_garbage/CMakeLists.txt new file mode 100644 index 000000000..d01a07511 --- /dev/null +++ b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_take_out_garbage/CMakeLists.txt @@ -0,0 +1,41 @@ +cmake_minimum_required(VERSION 2.8.3) +project(mdr_take_out_garbage) + +find_package(catkin REQUIRED COMPONENTS + rospy + roslint + mas_execution_manager + mdr_listen_action + mdr_process_speech_command_action + mdr_move_base_action + mdr_move_arm_action + mdr_pickup_action + mdr_place_action + mdr_perceive_plane_action + mdr_navigation_behaviours + mdr_perception_behaviours + mdr_manipulation_behaviours +) + +catkin_package( + CATKIN_DEPENDS + rospy + mas_execution_manager + mdr_listen_action + mdr_process_speech_command_action + mdr_move_base_action + mdr_move_arm_action + mdr_pickup_action + mdr_place_action + mdr_perceive_plane_action + mdr_navigation_behaviours + mdr_perception_behaviours + mdr_manipulation_behaviours +) + +catkin_python_setup() +roslint_python() + +install(DIRECTORY ros/launch/ + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/ros/launch +) diff --git a/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_take_out_garbage/package.xml b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_take_out_garbage/package.xml new file mode 100644 index 000000000..b44b219c0 --- /dev/null +++ b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_take_out_garbage/package.xml @@ -0,0 +1,40 @@ + + + mdr_take_out_garbage + 1.0.0 + The mdr_take_out_garbage package + GPLv3 + + Alex Mitrevski + MAS robotics + + + catkin + + rospy + roslint + mas_execution_manager + mdr_listen_action + mdr_process_speech_command_action + mdr_move_base_action + mdr_move_arm_action + mdr_pickup_action + mdr_place_action + mdr_perceive_plane_action + mdr_navigation_behaviours + mdr_perception_behaviours + mdr_manipulation_behaviours + + rospy + mas_execution_manager + mdr_listen_action + mdr_process_speech_command_action + mdr_move_base_action + mdr_move_arm_action + mdr_pickup_action + mdr_place_action + mdr_perceive_plane_action + mdr_navigation_behaviours + mdr_perception_behaviours + mdr_manipulation_behaviours + diff --git a/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_take_out_garbage/setup.py b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_take_out_garbage/setup.py new file mode 100644 index 000000000..10ea35811 --- /dev/null +++ b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_take_out_garbage/setup.py @@ -0,0 +1,11 @@ +#!/usr/bin/env python + +from distutils.core import setup +from catkin_pkg.python_setup import generate_distutils_setup + +d = generate_distutils_setup( + packages=['mdr_take_out_garbage'], + package_dir={'mdr_take_out_garbage': 'ros/src/mdr_take_out_garbage'} +) + +setup(**d) From 14d8701e86c4c0cd79bc62840d7a2d0b9d4e3027 Mon Sep 17 00:00:00 2001 From: Minh Nguyen Date: Mon, 11 Mar 2019 13:32:14 +0100 Subject: [PATCH 67/96] rename all ImageRecognition usages to RecognizeImage --- .../mdr_perceive_plane_action/README.md | 2 +- .../ros/src/mdr_perceive_plane_action/action_states.py | 7 +++---- 2 files changed, 4 insertions(+), 5 deletions(-) diff --git a/mdr_planning/mdr_actions/mdr_perception_actions/mdr_perceive_plane_action/README.md b/mdr_planning/mdr_actions/mdr_perception_actions/mdr_perceive_plane_action/README.md index f93961d41..c248d6ed1 100644 --- a/mdr_planning/mdr_actions/mdr_perception_actions/mdr_perceive_plane_action/README.md +++ b/mdr_planning/mdr_actions/mdr_perception_actions/mdr_perceive_plane_action/README.md @@ -60,7 +60,7 @@ The following arguments may be passed when launching the action server: * ``detection_action_name``: name of the action server used for object detection. Action file: `mcr_perception_msgs/DetectScene.action`. * ``recognition_service_name``: the name of the image recognition service for classifying object. - Service file: `mcr_perception_msgs/ImageRecognition.srv`. + Service file: `mcr_perception_msgs/RecognizeImage.srv`. * ``recognition_model_name``: the name of the image classification model located in the `mdr_object_recognition` package. * ``preprocess_input_module``: the name of the module containing the image preprocessing function to be executed on diff --git a/mdr_planning/mdr_actions/mdr_perception_actions/mdr_perceive_plane_action/ros/src/mdr_perceive_plane_action/action_states.py b/mdr_planning/mdr_actions/mdr_perception_actions/mdr_perceive_plane_action/ros/src/mdr_perceive_plane_action/action_states.py index dc5ceaddd..267a9039f 100644 --- a/mdr_planning/mdr_actions/mdr_perception_actions/mdr_perceive_plane_action/ros/src/mdr_perceive_plane_action/action_states.py +++ b/mdr_planning/mdr_actions/mdr_perception_actions/mdr_perceive_plane_action/ros/src/mdr_perceive_plane_action/action_states.py @@ -4,7 +4,7 @@ from pyftsm.ftsm import FTSMTransitions from mas_execution.action_sm_base import ActionSMBase from mcr_perception_msgs.msg import PlaneList -from mas_perception_libs import ObjectDetector, ImageRecognitionServiceProxy +from mas_perception_libs import ObjectDetector, RecognizeImageServiceProxy from mdr_perceive_plane_action.msg import PerceivePlaneResult, PerceivePlaneFeedback class PerceivePlaneSM(ActionSMBase): @@ -17,9 +17,8 @@ def __init__(self, detection_service_proxy, max_recovery_attempts=1): super(PerceivePlaneSM, self).__init__('PerceivePlane', [], max_recovery_attempts) self._detector = ObjectDetector(detection_service_proxy) - self._recog_service_proxy = ImageRecognitionServiceProxy(recog_service_name, - recog_model_name, - preprocess_input_module) + self._recog_service_proxy = RecognizeImageServiceProxy(recog_service_name, recog_model_name, + preprocess_input_module) self._image_pub = rospy.Publisher('/first_recognized_image', sensor_msgs.msg.Image, queue_size=1) From 38d399da5dbe0be53ccbe091fba32d75dfa4369f Mon Sep 17 00:00:00 2001 From: Henrik Schnor Date: Tue, 12 Mar 2019 11:23:50 +0100 Subject: [PATCH 68/96] [mdr_listen_action] Removed reference to nonexistent python package and fixed indentation --- .../mdr_listen_action/ros/scripts/listen_client_test | 2 +- .../mdr_speech_actions/mdr_listen_action/setup.py | 5 ++--- 2 files changed, 3 insertions(+), 4 deletions(-) diff --git a/mdr_planning/mdr_actions/mdr_speech_actions/mdr_listen_action/ros/scripts/listen_client_test b/mdr_planning/mdr_actions/mdr_speech_actions/mdr_listen_action/ros/scripts/listen_client_test index 5c4e97f49..e7088729e 100755 --- a/mdr_planning/mdr_actions/mdr_speech_actions/mdr_listen_action/ros/scripts/listen_client_test +++ b/mdr_planning/mdr_actions/mdr_speech_actions/mdr_listen_action/ros/scripts/listen_client_test @@ -38,7 +38,7 @@ if __name__ == '__main__': timeout = 15.0 client.wait_for_result(rospy.Duration.from_sec(int(timeout))) - rospy.loginfo(client.get_result()) + rospy.loginfo(client.get_result()) except Exception as ex: rospy.logerr(type(ex).__name__ + ": You have failed!") diff --git a/mdr_planning/mdr_actions/mdr_speech_actions/mdr_listen_action/setup.py b/mdr_planning/mdr_actions/mdr_speech_actions/mdr_listen_action/setup.py index 3b48c27ce..3dd315117 100644 --- a/mdr_planning/mdr_actions/mdr_speech_actions/mdr_listen_action/setup.py +++ b/mdr_planning/mdr_actions/mdr_speech_actions/mdr_listen_action/setup.py @@ -4,8 +4,7 @@ from catkin_pkg.python_setup import generate_distutils_setup d = generate_distutils_setup( - packages=['mdr_listen_action', 'mdr_listen_action_ros'], - package_dir={'mdr_listen_action': 'common/src/mdr_listen_action', - 'mdr_listen_action_ros': 'ros/src/mdr_listen_action_ros'}) + packages=['mdr_listen_action_ros'], + package_dir={'mdr_listen_action_ros': 'ros/src/mdr_listen_action_ros'}) setup(**d) From b09107df188b79dddc9fa70ce3a67e0887b09254 Mon Sep 17 00:00:00 2001 From: Henrik Schnor Date: Fri, 8 Mar 2019 16:55:31 +0100 Subject: [PATCH 69/96] Created a state machine for the "serving drinks" scenario --- .../mdr_serve_drinks/CMakeLists.txt | 35 ++ .../mdr_serve_drinks/README.md | 47 +++ .../config/serve_drinks_sm.yaml | 311 ++++++++++++++++++ .../mdr_serve_drinks/package.xml | 33 ++ .../ros/launch/serve_drinks.launch | 22 ++ .../ros/src/mdr_serve_drinks/__init__.py | 0 .../scenario_states/__init__.py | 0 .../mdr_serve_drinks/setup.py | 11 + 8 files changed, 459 insertions(+) create mode 100644 mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_serve_drinks/CMakeLists.txt create mode 100644 mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_serve_drinks/README.md create mode 100644 mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_serve_drinks/config/serve_drinks_sm.yaml create mode 100644 mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_serve_drinks/package.xml create mode 100644 mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_serve_drinks/ros/launch/serve_drinks.launch create mode 100644 mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_serve_drinks/ros/src/mdr_serve_drinks/__init__.py create mode 100644 mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_serve_drinks/ros/src/mdr_serve_drinks/scenario_states/__init__.py create mode 100644 mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_serve_drinks/setup.py diff --git a/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_serve_drinks/CMakeLists.txt b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_serve_drinks/CMakeLists.txt new file mode 100644 index 000000000..2c35cc4af --- /dev/null +++ b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_serve_drinks/CMakeLists.txt @@ -0,0 +1,35 @@ +cmake_minimum_required(VERSION 2.8.3) +project(mdr_serve_drinks) + +find_package(catkin REQUIRED COMPONENTS + rospy + roslint + mas_execution_manager + mdr_move_base_action + mdr_move_arm_action + mdr_pickup_action + mdr_perceive_plane_action + mdr_navigation_behaviours + mdr_perception_behaviours + mdr_manipulation_behaviours +) + +catkin_package( + CATKIN_DEPENDS + rospy + mas_execution_manager + mdr_move_base_action + mdr_move_arm_action + mdr_pickup_action + mdr_perceive_plane_action + mdr_navigation_behaviours + mdr_perception_behaviours + mdr_manipulation_behaviours +) + +catkin_python_setup() +roslint_python() + +install(DIRECTORY ros/launch/ + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/ros/launch +) diff --git a/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_serve_drinks/README.md b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_serve_drinks/README.md new file mode 100644 index 000000000..b8934e692 --- /dev/null +++ b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_serve_drinks/README.md @@ -0,0 +1,47 @@ +# mdr_serve_drinks + +Your favorite drinks delivered cold and fresh. + +## Goal + +The robot has to find all party guest who don't have a drink yet and take their orders. After that the robot goes to the bar, takes the ordered drinks and brings them to the guests. This task focuses mainly on perception and human-robot interaction. +- The robot has to remember people and orders (people change places) +- One of the ordered drinks is missing + - Bonus points for preemptively informing the guest, that the order is not available +- Instead of grasping and delivering the drinks one by one the robot can optionally use an attached tray + - The barkeeper places the drinks on the tray + - The robot must detect if guests take drinks from the tray they didn't order + +## Subtasks involved + +- Perception + - Recognize objects (drinks) on a table (bar) + - Recognize people + - Recognize if people have drinks +- Speech + - Having a small conversation / dialogue +- Manipulation + - Grasping objects + - Handing over objects + +## Running the task + +1. To run rviz on your computer, export the robot's `ROS_MASTER_URI` first: + ``` + export ROS_MASTER_URI=http://:11311/ + rviz + ``` + +2. Load the config file for the robot. + +3. Use "2D Pose Estimate" to localize the robot on the map. + +4. Run the SSD Keras Image detector (make sure to have the `ROS_MASTER_URI` exported if you use a new terminal): + ``` + roslaunch ssd_keras_ros coco_image_detection_server.launch cloud_topic:=/point_cloud_topic_name + ``` + +5. Launch the serving drinks scenario (remember `ROS_MASTER_URI`) + ``` + roslaunch mdr_serve_drinks serve_drinks.launch + ``` diff --git a/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_serve_drinks/config/serve_drinks_sm.yaml b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_serve_drinks/config/serve_drinks_sm.yaml new file mode 100644 index 000000000..798313e46 --- /dev/null +++ b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_serve_drinks/config/serve_drinks_sm.yaml @@ -0,0 +1,311 @@ +# Description: Defines a state machine for the RoboCup@Home "serving drinks" scenario +# Author: Henrik Schnor +# Email: henrik.schnor@smail.inf.h-brs.de +sm_id: mdr_serve_drinks +states: [GO_NEAR_BAR, FIND_BAR, GO_TO_BAR, PERCEIVE_BAR, GO_TO_LIVING_ROOM, FIND_PEOPLE, CHOOSE_ACTION, GET_ORDER_GO_TO_PERSON, GET_ORDER_DIALOGUE, GET_DRINK_GO_TO_BAR, GET_DRINK_PERCEIVE_BAR, GET_DRINK_PICK, DELIVER_DRINK_GO_TO_PERSON, DELIVER_DRINK_HAND_OVER] +outcomes: [DONE, FAILED] +state_descriptions: + # + # This is for initial observation of all available drinks + # + - state: + name: GO_NEAR_BAR + state_module_name: mdr_navigation_behaviours.move_base + state_class_name: MoveBase + transitions: + - transition: + name: succeeded + state: FIND_BAR + - transition: + name: failed + state: GO_NEAR_BAR + - transition: + name: failed_after_retrying + state: FAILED + arguments: + - argument: + name: destination_locations + value: [NEAR_BAR] + - argument: + name: number_of_retries + value: 3 + - state: + name: FIND_BAR + state_module_name: mdr_perception_behaviours.find_table + state_class_name: FindTable + transitions: + - transition: + name: succeeded + state: GO_TO_BAR + - transition: + name: failed + state: GO_NEAR_BAR + - transition: + name: failed_after_retrying + state: FAILED + arguments: + - argument: + name: number_of_retries + value: 3 + - state: + name: GO_TO_BAR + state_module_name: mdr_navigation_behaviours.move_base + state_class_name: MoveBase + transitions: + - transition: + name: succeeded + state: PERCEIVE_BAR + - transition: + name: failed + state: GO_NEAR_BAR + - transition: + name: failed_after_retrying + state: FAILED + arguments: + - argument: + name: destination_locations + value: [BAR] + - argument: + name: number_of_retries + value: 3 + - state: + name: PERCEIVE_BAR + state_module_name: mdr_perception_behaviours.perceive_planes + state_class_name: PerceivePlanes + transitions: + - transition: + name: succeeded + state: GO_TO_LIVING_ROOM + - transition: + name: failed + state: GO_NEAR_BAR + - transition: + name: failed_after_retrying + state: FAILED + arguments: + - argument: + name: plane_prefix + value: bar + - argument: + name: number_of_retries + value: 3 + + # + # Go to the living room, see who's there and decide what to do + # + - state: + name: GO_TO_LIVING_ROOM + state_module_name: mdr_navigation_behaviours.move_base + state_class_name: MoveBase + transitions: + - transition: + name: succeeded + state: FIND_PEOPLE + - transition: + name: failed + state: GO_TO_LIVING_ROOM + - transition: + name: failed_after_retrying + state: FAILED + arguments: + - argument: + name: destination_locations + value: [LIVING_ROOM] + - argument: + name: number_of_retries + value: 3 + - state: + name: FIND_PEOPLE + state_module_name: mdr_perception_behaviours.find_people + state_class_name: FindPeople + transitions: + - transition: + name: succeeded + state: CHOOSE_ACTION + - transition: + name: failed + state: GO_TO_LIVING_ROOM + - transition: + name: failed_after_retrying + state: FAILED + arguments: + - argument: + name: number_of_retries + value: 3 + - state: + name: CHOOSE_ACTION + state_module_name: mdr_serve_drinks.scenario_states.choose_action + state_class_name: ChooseAction + transitions: + - transition: + name: get_order + state: GET_ORDER_GO_TO_PERSON + - transition: + name: deliver_drink + state: DELIVER_DRINK_GO_TO_PERSON + - transition: + name: done + state: DONE + - transition: + name: failed + state: GO_TO_LIVING_ROOM + - transition: + name: failed_after_retrying + state: FAILED + arguments: + - argument: + name: number_of_retries + value: 3 + + # + # Approach a person and ask for their order + # + - state: + name: GET_ORDER_GO_TO_PERSON + state_module_name: mdr_navigation_behaviours.move_base + state_class_name: MoveBase + transitions: + - transition: + name: succeeded + state: GET_ORDER_DIALOGUE + - transition: + name: failed + state: GO_TO_LIVING_ROOM + - transition: + name: failed_after_retrying + state: FAILED + arguments: + - argument: + name: destination_locations + value: [PERSON_WITHOUT_DRINK] + - argument: + name: number_of_retries + value: 3 + - state: + name: GET_ORDER_DIALOGUE + state_module_name: mdr_speech_behaviours.dialogue + state_class_name: Dialogue + transitions: + - transition: + name: succeeded + state: GET_DRINK_GO_TO_BAR + - transition: + name: failed + state: GO_TO_LIVING_ROOM + - transition: + name: failed_after_retrying + state: FAILED + arguments: + - argument: + name: dialogue_model + value: get_drink_order + + # + # Get a drink from the bar (we'll try to pick it ourselves for now) + # + - state: + name: GET_DRINK_GO_TO_BAR + state_module_name: mdr_navigation_behaviours.move_base + state_class_name: MoveBase + transitions: + - transition: + name: succeeded + state: GET_DRINK_PERCEIVE_BAR + - transition: + name: failed + state: GET_DRINK_GO_TO_BAR + - transition: + name: failed_after_retrying + state: FAILED + arguments: + - argument: + name: destination_locations + value: [BAR] + - argument: + name: number_of_retries + value: 3 + - state: + name: GET_DRINK_PERCEIVE_BAR + state_module_name: mdr_perception_behaviours.perceive_planes + state_class_name: PerceivePlanes + transitions: + - transition: + name: succeeded + state: GET_DRINK_PICK + - transition: + name: failed + state: GET_DRINK_GO_TO_BAR + - transition: + name: failed_after_retrying + state: FAILED + arguments: + - argument: + name: plane_prefix + value: bar + - argument: + name: number_of_retries + value: 3 + - state: + name: GET_DRINK_PICK + state_module_name: mdr_manipulation_behaviours.pick_object + state_class_name: PickObject + transitions: + - transition: + name: succeeded + state: GO_TO_LIVING_ROOM + - transition: + name: failed + state: GET_DRINK_GO_TO_BAR + - transition: + name: failed_after_retrying + state: FAILED + arguments: + - argument: + name: picking_surface_prefix + value: bar + - argument: + name: number_of_retries + value: 3 + + # + # Deliver the drink to the person who ordered it + # + - state: + name: DELIVER_DRINK_GO_TO_PERSON + state_module_name: mdr_navigation_behaviours.move_base + state_class_name: MoveBase + transitions: + - transition: + name: succeeded + state: DELIVER_DRINK_HAND_OVER + - transition: + name: failed + state: DELIVER_DRINK_GO_TO_PERSON + - transition: + name: failed_after_retrying + state: FAILED + arguments: + - argument: + name: destination_locations + value: [PERSON_WITH_ORDER] + - argument: + name: number_of_retries + value: 3 + - state: + name: DELIVER_DRINK_HAND_OVER + state_module_name: mdr_manipulation_behaviours.hand_over_object + state_class_name: HandOverObject + transitions: + - transition: + name: succeeded + state: GO_TO_LIVING_ROOM + - transition: + name: failed + state: DELIVER_DRINK_GO_TO_PERSON + - transition: + name: failed_after_retrying + state: FAILED + arguments: + - argument: + name: number_of_retries + value: 3 diff --git a/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_serve_drinks/package.xml b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_serve_drinks/package.xml new file mode 100644 index 000000000..ba3b94926 --- /dev/null +++ b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_serve_drinks/package.xml @@ -0,0 +1,33 @@ + + + mdr_serve_drinks + 1.0.0 + State machine and launch files for the RoboCup@Home serving drinks scenario + GPLv3 + + Henrik Schnor + MAS robotics + + catkin + + rospy + roslint + mas_execution_manager + mdr_move_base_action + mdr_move_arm_action + mdr_pickup_action + mdr_perceive_plane_action + mdr_navigation_behaviours + mdr_perception_behaviours + mdr_manipulation_behaviours + + rospy + mas_execution_manager + mdr_move_base_action + mdr_move_arm_action + mdr_pickup_action + mdr_perceive_plane_action + mdr_navigation_behaviours + mdr_perception_behaviours + mdr_manipulation_behaviours + diff --git a/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_serve_drinks/ros/launch/serve_drinks.launch b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_serve_drinks/ros/launch/serve_drinks.launch new file mode 100644 index 000000000..063fe05ed --- /dev/null +++ b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_serve_drinks/ros/launch/serve_drinks.launch @@ -0,0 +1,22 @@ + + + + + + + + + + + + + + + + + + + + + + diff --git a/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_serve_drinks/ros/src/mdr_serve_drinks/__init__.py b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_serve_drinks/ros/src/mdr_serve_drinks/__init__.py new file mode 100644 index 000000000..e69de29bb diff --git a/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_serve_drinks/ros/src/mdr_serve_drinks/scenario_states/__init__.py b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_serve_drinks/ros/src/mdr_serve_drinks/scenario_states/__init__.py new file mode 100644 index 000000000..e69de29bb diff --git a/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_serve_drinks/setup.py b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_serve_drinks/setup.py new file mode 100644 index 000000000..d613dfe0d --- /dev/null +++ b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_serve_drinks/setup.py @@ -0,0 +1,11 @@ +#!/usr/bin/env python + +from distutils.core import setup +from catkin_pkg.python_setup import generate_distutils_setup + +d = generate_distutils_setup( + packages=['mdr_serve_drinks'], + package_dir={'mdr_serve_drinks': 'ros/src/mdr_serve_drinks'} +) + +setup(**d) From dfe9b68896e03be9fbc3317277e6644d3ac2970a Mon Sep 17 00:00:00 2001 From: Henrik Schnor Date: Tue, 12 Mar 2019 18:29:05 +0100 Subject: [PATCH 70/96] [mdr_listen_action] Simplified setup.py --- .../mdr_actions/mdr_speech_actions/mdr_listen_action/setup.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/mdr_planning/mdr_actions/mdr_speech_actions/mdr_listen_action/setup.py b/mdr_planning/mdr_actions/mdr_speech_actions/mdr_listen_action/setup.py index 3dd315117..e938e3048 100644 --- a/mdr_planning/mdr_actions/mdr_speech_actions/mdr_listen_action/setup.py +++ b/mdr_planning/mdr_actions/mdr_speech_actions/mdr_listen_action/setup.py @@ -5,6 +5,6 @@ d = generate_distutils_setup( packages=['mdr_listen_action_ros'], - package_dir={'mdr_listen_action_ros': 'ros/src/mdr_listen_action_ros'}) + package_dir={'': 'ros/src/'}) setup(**d) From 83bb5c6053aadbd58995f243a5d3a8daf9488cd9 Mon Sep 17 00:00:00 2001 From: Henrik Schnor Date: Tue, 12 Mar 2019 18:35:29 +0100 Subject: [PATCH 71/96] [mdr_listen_action] Added missing dependencies --- .../mdr_speech_actions/mdr_listen_action/CMakeLists.txt | 4 ++++ .../mdr_speech_actions/mdr_listen_action/package.xml | 4 ++++ 2 files changed, 8 insertions(+) diff --git a/mdr_planning/mdr_actions/mdr_speech_actions/mdr_listen_action/CMakeLists.txt b/mdr_planning/mdr_actions/mdr_speech_actions/mdr_listen_action/CMakeLists.txt index 4bfac3c1c..2fa303bb0 100644 --- a/mdr_planning/mdr_actions/mdr_speech_actions/mdr_listen_action/CMakeLists.txt +++ b/mdr_planning/mdr_actions/mdr_speech_actions/mdr_listen_action/CMakeLists.txt @@ -8,6 +8,8 @@ find_package(catkin REQUIRED COMPONENTS message_generation roslint rospy + mdr_speech_matching + mdr_speech_recognition ) catkin_python_setup() @@ -28,6 +30,8 @@ catkin_package( actionlib_msgs message_runtime std_msgs + mdr_speech_matching + mdr_speech_recognition ) include_directories( diff --git a/mdr_planning/mdr_actions/mdr_speech_actions/mdr_listen_action/package.xml b/mdr_planning/mdr_actions/mdr_speech_actions/mdr_listen_action/package.xml index e7f31d20b..19ce45cd9 100644 --- a/mdr_planning/mdr_actions/mdr_speech_actions/mdr_listen_action/package.xml +++ b/mdr_planning/mdr_actions/mdr_speech_actions/mdr_listen_action/package.xml @@ -15,12 +15,16 @@ message_generation roslint rospy + mdr_speech_matching + mdr_speech_recognition rospy actionlib actionlib_msgs message_runtime std_msgs + mdr_speech_matching + mdr_speech_recognition From 8b5e74ada30c0589900c7674d29cd481dc5fc66d Mon Sep 17 00:00:00 2001 From: Roberto Cai Date: Wed, 13 Mar 2019 13:55:14 +0100 Subject: [PATCH 72/96] Add states for perceiving collection_zone --- .../config/take_out_garbage_sm.yaml | 44 ++++++++++++++++++- 1 file changed, 43 insertions(+), 1 deletion(-) diff --git a/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_take_out_garbage/config/take_out_garbage_sm.yaml b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_take_out_garbage/config/take_out_garbage_sm.yaml index 8f85629e5..daeb6831f 100644 --- a/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_take_out_garbage/config/take_out_garbage_sm.yaml +++ b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_take_out_garbage/config/take_out_garbage_sm.yaml @@ -134,7 +134,7 @@ state_descriptions: transitions: - transition: name: succeeded - state: PLACE_OBJECT + state: FIND_COLLECTION_ZONE - transition: name: failed state: GO_TO_COLLECTION_ZONE @@ -148,6 +148,48 @@ state_descriptions: - argument: name: number_of_retries value: 3 + - state: + name: FIND_COLLECTION_ZONE + state_module_name: mdr_take_out_garbage.scenario_states + state_class_name: FindCollectionZone + transitions: + - transition: + name: succeeded + state: PERCEIVE_TABLE + - transition: + name: failed + state: GO_TO_COLLECTION_ZONE + - transition: + name: failed_after_retrying + state: FAILED + arguments: + - argument: + name: number_of_retries + value: 3 + - argument: + name: placing_surface_prefix + value: collection_zone + - state: + name: PERCEIVE_COLLECTION_ZONE + state_module_name: mdr_perception_behaviours.perceive_planes + state_class_name: PerceivePlanes + transitions: + - transition: + name: succeeded + state: PLACE_OBJECT + - transition: + name: failed + state: PERCEIVE_TABLE + - transition: + name: failed_after_retrying + state: FAILED + arguments: + - argument: + name: number_of_retries + value: 3 + - argument: + name: plane_prefix + value: collection_zone - state: name: PLACE_OBJECT state_module_name: mdr_manipulation_behaviours.place From f0f10baf5d11e992f20db4cae51245f1dbc4dd85 Mon Sep 17 00:00:00 2001 From: Roberto Cai Date: Wed, 13 Mar 2019 14:13:22 +0100 Subject: [PATCH 73/96] Fix typos and state names for state machine --- .../config/take_out_garbage_sm.yaml | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_take_out_garbage/config/take_out_garbage_sm.yaml b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_take_out_garbage/config/take_out_garbage_sm.yaml index daeb6831f..eebfb95f7 100644 --- a/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_take_out_garbage/config/take_out_garbage_sm.yaml +++ b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_take_out_garbage/config/take_out_garbage_sm.yaml @@ -19,7 +19,7 @@ state_descriptions: state_class_name: ProcessCommand transitions: - transition: - name: start_store_groceries + name: start_take_out_garbage state: ENTER - transition: name: continue_waiting @@ -98,7 +98,7 @@ state_descriptions: transitions: - transition: name: succeeded - state: PICK_OBJECT + state: PICK_GARBAGE_BAG - transition: name: failed state: PERCEIVE_INSIDE_BIN @@ -110,7 +110,7 @@ state_descriptions: name: number_of_retries value: 3 - state: - name: PICK_OBJECT + name: PICK_GARBAGE_BAG state_module_name: mdr_manipulation_behaviours.pick_non_rigid_object_from_bin state_class_name: PickNonRigidObjectFromBin transitions: @@ -119,7 +119,7 @@ state_descriptions: state: GO_TO_COLLECTION_ZONE - transition: name: failed - state: PICK_OBJECT + state: PICK_GARBAGE_BAG - transition: name: failed_after_retrying state: FAILED @@ -144,7 +144,7 @@ state_descriptions: arguments: - argument: name: destination_locations - value: [COLLETION_ZONE] + value: [COLLECTION_ZONE] - argument: name: number_of_retries value: 3 @@ -197,7 +197,7 @@ state_descriptions: transitions: - transition: name: succeeded - state: CHECK_EMPTY_TABLE + state: CHECK_BINS_LEFT - transition: name: failed state: PLACE_OBJECT From 5b825f7c7d7da002a753bfabf7e5db306a7e63c3 Mon Sep 17 00:00:00 2001 From: Roberto Cai Date: Fri, 15 Mar 2019 18:01:55 +0100 Subject: [PATCH 74/96] Add python package configuration files --- .../mdr_take_out_garbage/ros/src/mdr_take_out_garbage/__init__.py | 0 .../ros/src/mdr_take_out_garbage/scenario_states/__init__.py | 0 2 files changed, 0 insertions(+), 0 deletions(-) create mode 100644 mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_take_out_garbage/ros/src/mdr_take_out_garbage/__init__.py create mode 100644 mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_take_out_garbage/ros/src/mdr_take_out_garbage/scenario_states/__init__.py diff --git a/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_take_out_garbage/ros/src/mdr_take_out_garbage/__init__.py b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_take_out_garbage/ros/src/mdr_take_out_garbage/__init__.py new file mode 100644 index 000000000..e69de29bb diff --git a/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_take_out_garbage/ros/src/mdr_take_out_garbage/scenario_states/__init__.py b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_take_out_garbage/ros/src/mdr_take_out_garbage/scenario_states/__init__.py new file mode 100644 index 000000000..e69de29bb From 6d080225d9afeb48eb9d690d346e25ac58d3305e Mon Sep 17 00:00:00 2001 From: Roberto Cai Date: Fri, 15 Mar 2019 19:04:01 +0100 Subject: [PATCH 75/96] Add launch file for take out garbage task --- .../ros/launch/take_out_garbage.launch | 27 +++++++++++++++++++ 1 file changed, 27 insertions(+) create mode 100644 mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_take_out_garbage/ros/launch/take_out_garbage.launch diff --git a/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_take_out_garbage/ros/launch/take_out_garbage.launch b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_take_out_garbage/ros/launch/take_out_garbage.launch new file mode 100644 index 000000000..3d24b7b01 --- /dev/null +++ b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_take_out_garbage/ros/launch/take_out_garbage.launch @@ -0,0 +1,27 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + From b59f9b14baa5d2159aa94b1100c443eaeaeb1af7 Mon Sep 17 00:00:00 2001 From: Alex Mitrevski Date: Fri, 12 Apr 2019 18:02:35 +0200 Subject: [PATCH 76/96] [rosplan_interface] Added an 'in' predicate to the default domain --- mdr_planning/mdr_rosplan_interface/config/default_domain.pddl | 1 + 1 file changed, 1 insertion(+) diff --git a/mdr_planning/mdr_rosplan_interface/config/default_domain.pddl b/mdr_planning/mdr_rosplan_interface/config/default_domain.pddl index b84d6456b..d33d086f9 100644 --- a/mdr_planning/mdr_rosplan_interface/config/default_domain.pddl +++ b/mdr_planning/mdr_rosplan_interface/config/default_domain.pddl @@ -23,6 +23,7 @@ (unexplored ?plane - plane) (explored ?plane - plane) (on ?obj - object ?plane - plane) + (in ?obj - object ?source - object) (holding ?bot - robot ?obj - object) (empty_gripper ?bot - robot) ) From a7b559c7e3ede8d133f1fc2fec11618692f889c8 Mon Sep 17 00:00:00 2001 From: Alex Mitrevski Date: Fri, 26 Apr 2019 17:49:45 +0200 Subject: [PATCH 77/96] [pickup/place] Updated the table manipulation DMP --- .../weights_table_grasp.yaml | 1874 +++++++++++++++-- .../weights_table_grasp2.yaml | 205 -- .../weights_table_place.yaml | 1874 +++++++++++++++-- 3 files changed, 3334 insertions(+), 619 deletions(-) delete mode 100644 mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_pickup_action/config/trajectory_weights/weights_table_grasp2.yaml diff --git a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_pickup_action/config/trajectory_weights/weights_table_grasp.yaml b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_pickup_action/config/trajectory_weights/weights_table_grasp.yaml index 6a97b7cbb..2bcb6488b 100644 --- a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_pickup_action/config/trajectory_weights/weights_table_grasp.yaml +++ b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_pickup_action/config/trajectory_weights/weights_table_grasp.yaml @@ -1,207 +1,1667 @@ -pitch: [-35.42493997049422, -37.70819524763274, -41.30898221809194, -46.91255259713179, - -54.67543853474187, -63.46993205757798, -70.71710907291556, -73.40838683036453, - -69.88965835948521, -61.23837968207681, -51.46297199294242, -46.651105552304536, - -53.51199582034473, -77.36775616098751, -119.40733708986616, -173.73781284783232, - -226.1456109740379, -256.9255230366232, -248.19097106023983, -192.5413353804086, - -97.86144890114835, 15.403191745759875, 123.77904323809733, 211.94253960392064, - 278.4370219521786, 331.32494006867165, 376.0838394153919, 404.99584445075004, 397.5504226626487, - 333.15522844035416, 207.40468267623098, 40.23505964106349, -129.5637520862881, -259.90824743283804, - -322.2438715398654, -311.38242615291654, -244.58025251479793, -151.69677985653905, - -62.27097378404166, 4.242548228062668, 41.72701342077948, 54.61937386645844, 52.0678236639199, - 42.46420503183177, 30.605603045097798, 17.831640535863865, 3.9115768409267537, -11.035226656357036, - -25.73694492619038, -38.23525698681982, -46.96747089197185, -51.55412317003241, - -52.72523403265943, -51.52230673716135, -48.47287389295395, -43.418549291861005, - -36.12710203132058, -27.217737390364878, -18.73250127108376, -13.940788921463144, - -16.383140380541224, -28.479814446323395, -50.19840934679473, -78.34588385548898, - -106.9796715308115, -129.0771227243504, -138.96143934459482, -134.39916286752, -117.2699501961774, - -92.4438455558197, -65.54260990887046, -40.863874703900365, -20.503006515797157, - -4.857673288780039, 6.139265967008193, 12.034840538128108, 11.78201008858928, 4.554178806755324, - -9.17603539972846, -27.06452970168944, -45.243054184153266, -59.42498559615973, - -66.20027581755308, -63.910105987553706, -52.76476291531106, -34.26469807990957, - -10.31714588108525, 17.428528508170896, 47.650311361280316, 78.83964513599082, 108.6479511556088, - 133.71712237097847, 150.35674122169067, 155.84935265330853, 149.667219171301, 133.86087612595915, - 112.3745444005185, 89.69640055960969, 69.59508619729118, 54.52489160570828, 45.78344309013908, - 44.04929137036943, 49.782033634194036, 63.15855610291111, 83.57913311970796, 109.088551720991, - 136.1606322569329, 160.14074686489496, 176.30498395284482, 181.14929667909337, 173.36150369292395, - 154.05941286281936, 126.24020279597528, 93.77525021113942, 60.472164188134116, 29.603993012435218, - 3.958702688619639, -13.885207810920182, -21.476471710992165, -16.848997943544642, - 0.7832249798221619, 30.385533864461852, 68.93627511091994, 111.90269031244195, 154.16251910598547, - 190.93981622752082, 218.36838869956537, 233.58094610071046, 234.54607891687763, - 220.01454742143613, 189.79459560980567, 145.2497513417973, 89.62834985791211, 27.797829306827328, - -34.782880289602524, -93.6044248049321, -146.58335798475517, -194.84023705533775, - -242.3963649186648, -294.83526437988365, -357.38490486635334, -433.09803303759264, - -521.7578411720901, -619.8377564762594, -721.4265148901123, -819.6866688138135, - -908.3015435028083, -982.4985157361981, -1039.4942245798738, -1078.4361122467722] -roll: [-94.9809024798238, -89.37326678075686, -74.67496646210842, -49.97877940131072, - -17.593122681262724, 16.06645308336598, 41.71318415962005, 50.60713287019311, 38.51880205462394, - 8.097319900299366, -31.598013501826173, -68.24707622586226, -90.81627135482617, - -93.60576994022517, -77.98514286453168, -51.10227044659737, -22.50696815786439, - -0.5347663027544345, 10.124158386418726, 9.150782393854067, -0.3095242652998163, - -13.68185030980892, -27.163152096763604, -39.030814562375696, -49.5151324263574, - -59.485126018140726, -68.80624552086546, -75.428898637592, -75.92665061062036, -67.34610061221686, - -49.32728657858986, -25.192438385029675, -1.2918864998652975, 15.141124521025375, - 18.667930530166085, 7.7185754208041795, -14.77455802286933, -42.4332819435094, -67.77813685589356, - -84.93421974950635, -91.54423345826856, -89.02137901209117, -81.13057995433573, - -71.8508278902564, -63.789491345075696, -57.835970205818235, -53.8231627143478, - -51.463590908332876, -50.94238773923665, -52.942655659661675, -58.189136134695445, - -66.74155669337485, -77.33961889552769, -87.14688594433058, -92.1973263131889, -88.62131828366893, - -74.32902174902947, -50.44322013852119, -21.67409515681044, 4.817148616819843, 21.806399412879404, - 24.68253720151413, 13.196955457999769, -8.512322146430131, -33.57118762493018, -54.88516872102151, - -67.45252093691383, -69.67085932958665, -63.35440572936342, -52.67299311155918, - -42.53041326451286, -36.97101954194654, -38.07581556440504, -45.58076611309898, - -57.22308967053014, -69.64023776501026, -79.50688077198373, -84.51353160768544, - -83.83257156823157, -77.93238694773984, -67.92280694397907, -54.86274891199053, - -39.45901730010299, -22.31036765250445, -4.4655218706363, 12.1844480635262, 25.094101784610512, - 31.88594109540416, 31.426514019400226, 24.561406129229585, 14.071733616471022, 3.8310309701905063, - -2.4667652078742877, -2.4910925874960017, 4.036658820887705, 15.329450275859665, - 28.159760716196264, 38.92003600168367, 44.79013991534497, 44.63012010630281, 39.28967211649084, - 31.232881080163466, 23.636257019694135, 19.304811972720493, 19.785502677120114, - 24.950320982296397, 33.14353632090505, 41.81125510618662, 48.398727074838696, 51.231113704687615, - 50.09924343700417, 46.35777463863367, 42.492653842074425, 41.28546877990567, 44.839129747983, - 53.785840236969825, 66.95276774389771, 81.62171724086008, 94.3240624864676, 101.92183810445113, - 102.60933075138487, 96.47920077162367, 85.44154179319236, 72.51515576452759, 60.73862809903186, - 52.084774877874715, 46.75751956690583, 43.11719157604091, 38.275825894925006, 29.196437849286824, - 13.971039246031658, -7.127902794056211, -31.207065658218177, -53.168507134733815, - -66.97618274894776, -67.45714153993094, -52.067059591180616, -21.977834522009797, - 17.97026793057221, 60.50618410267125, 97.94258360415643, 124.23605347540033, 136.35854432363232, - 134.57499001187503, 121.7126248618848, 101.89089552853724, 79.27247943521832, 57.21560075628847, - 37.928073989631585, 22.51410879818383] -x: [-23.54082186527676, -23.512905258800767, -23.516224768121276, -23.509127341780307, - -23.454121068234553, -23.37527958550504, -23.398062947937724, -23.714937560177475, - -24.478124554177448, -25.68503685748029, -27.12817117252123, -28.444857414576866, - -29.255164972260452, -29.32533200992744, -28.663538268050935, -27.485379784503554, - -26.068328960660104, -24.58356824095513, -23.00962769946186, -21.200239596466837, - -19.096858163949097, -16.95368487091472, -15.374423042071834, -15.062830081970883, - -16.420337211023146, -19.266187917922952, -22.863442272556544, -26.212763756336138, - -28.43414844400656, -29.064052770088523, -28.17268699560032, -26.28311891920491, - -24.142369984008027, -22.446184009232976, -21.626313157732994, -21.768723364314585, - -22.670561864739494, -23.99022574778685, -25.41383154390955, -26.762121364597242, - -27.994402597101168, -29.11804796936013, -30.06316239908554, -30.607870142419564, - -30.41937426536905, -29.21126868808186, -26.94246526112341, -23.943122787757076, - -20.87495709704674, -18.512790864664698, -17.435764167233458, -17.78028043031174, - -19.180496230067334, -20.915425140594678, -22.174498281441092, -22.3240925499991, - -21.103563664135123, -18.729923248158855, -15.895248710835736, -13.624755694929485, - -12.985496396331671, -14.71180186829232, -18.89175793950522, -24.869265613923826, - -31.43499308439889, -37.24493024420751, -41.28732213685973, -43.18410476572519, - -43.194339294574945, -41.951140068617846, -40.10893022819285, -38.10222626963581, - -36.10877330115395, -34.16247721797567, -32.287238408773284, -30.55543065783834, - -29.06037228818418, -27.855098125159977, -26.91524249105149, -26.147620843657243, - -25.42616007720281, -24.62192925623665, -23.60773081960084, -22.241953853405278, - -20.350308251339122, -17.723112367242447, -14.141853873202399, -9.449513334682687, - -3.6720025070654474, 2.8362755406261857, 9.303036300724449, 14.609576588230889, - 17.5737981092729, 17.371008465565403, 13.903729103835676, 7.920315318993102, 0.7993036840940881, - -5.90970951170443, -10.979658455755517, -13.829074454257093, -14.5755935725983, - -13.85183635092414, -12.500079190061692, -11.274722889530162, -10.641362881668742, - -10.704587515925214, -11.250922772773135, -11.867715076437035, -12.089807887886105, - -11.528372274762951, -9.948640513140584, -7.285309022869411, -3.611258670618167, - 0.9044519621289198, 6.012648194756425, 11.361212296824625, 16.45810776843166, 20.673183290896244, - 23.327284211933843, 23.874543031911173, 22.122886022700243, 18.39546579340212, 13.54233035939333, - 8.770094314293303, 5.339138669234294, 4.239493164175406, 5.965509978348073, 10.462318872054993, - 17.240025852914957, 25.58456988711478, 34.768784271394836, 44.19101560731903, 53.42265111802887, - 62.1967348811316, 70.38947366683362, 78.02811866838204, 85.31956397263224, 92.66238947803912, - 100.60176750352969, 109.7128630951549, 120.43768024118057, 132.93038931028732, 146.9707364427417, - 161.98099303697487, 177.14055334845793, 191.554949364158, 204.4214845608734, 215.14598259878588, - 223.39255572340488, 229.07401275593168] -y: [-48.08181651846675, -49.587117112576244, -50.9623587336631, -52.74638540311553, - -55.339316884123335, -58.187634767212785, -59.19858890679832, -55.18823159102758, - -43.45669279583084, -23.680143811777636, 1.0278742629116866, 24.803094657233476, - 41.56642405836774, 48.12670154484163, 45.66964260010242, 38.41392701146586, 30.559274820896626, - 24.01469229055469, 18.37242623227647, 12.461922135369557, 5.780911723761772, -1.2306825260415035, - -7.855647675106398, -13.850897756070554, -19.628125426393403, -25.766439690878475, - -32.25785903733505, -38.106735397658724, -41.700722135824165, -41.78689726002638, - -38.35450818167012, -32.77088479939355, -27.13457755859672, -23.383886152709735, - -22.698519255950604, -25.30103433852694, -30.459665680447525, -36.587875594965475, - -41.56956109814254, -43.41643845983297, -41.04609408947397, -34.71380717009749, - -25.770834909882954, -15.877657193068476, -6.18267876618329, 3.0042222833118606, - 11.840355394622723, 20.142092255935047, 26.74289840637122, 29.592807279096725, 26.707565820644888, - 17.456235617422454, 3.388730612913831, -11.925225381003171, -23.997617973752774, - -28.962264558977488, -24.922343000496628, -12.598312313006955, 4.970869040522485, - 23.575824375424354, 39.379440684218075, 50.19400929294163, 55.947550262636035, 58.225838746081386, - 59.27299512331673, 61.046406924836795, 64.7113242754852, 70.55113102435496, 78.04412796628496, - 85.97088366315737, 92.64894651093383, 96.42012688093448, 96.28287075303587, 92.33099685071431, - 85.70473506502236, 78.06766687875744, 70.92429970225633, 65.1466098402518, 60.88285588515454, - 57.769140305510135, 55.238497697808114, 52.76635428971193, 50.01647387605132, 46.93679691285544, - 43.83967985214271, 41.42191756324371, 40.63190289145288, 42.344396617716775, 46.9347863760049, - 53.95190906255407, 62.07786213730404, 69.43105232791753, 74.09941024378266, 74.68381768815975, - 70.63554962109797, 62.27282910008037, 50.51258663591012, 36.491359876837045, 21.304370351168693, - 6.013296228474457, -8.122423557377378, -19.438718195250082, -25.98893089734145, - -26.128853719153366, -19.38244812067696, -7.151684031278839, 7.173184693921752, - 18.886623201163076, 23.235809508225923, 16.81931017033493, -1.4029158802374324, - -29.8433104623529, -64.82075623228921, -101.63386062733136, -135.70233578562002, - -163.3136997983213, -181.76783190637622, -189.09357664198424, -183.78138421191431, - -164.93275730603818, -132.8760319352321, -89.87628487198717, -40.3823840377136, - 9.545608199456396, 53.52353674119033, 86.31999893025124, 104.89002936158305, 108.59148361413862, - 98.65485183597463, 77.326293537273, 47.16357178564959, 10.74497388276303, -29.265103349701626, - -69.94908118242546, -108.10390173253013, -140.4074927252729, -163.68216316743766, - -175.12064015744392, -172.44337814636657, -154.07896535435387, -119.47006083653432, - -69.47218086412778, -6.639348762292799, 64.85632773989234, 139.77404187801, 212.67787306545785, - 278.82692966691434, 334.7974511897336, 378.7116450336994, 410.118700189302] -yaw: [-53.959788987781124, -53.30371682466102, -50.30427376032306, -45.009643854796366, - -38.23858486542749, -31.445920900106113, -26.17708607783723, -23.44624165703102, - -23.41933783360354, -25.515961378948077, -28.81688930020335, -32.56774753973497, - -36.529787446838284, -40.964730323552274, -46.18201275969121, -51.81029740608197, - -56.21012654557379, -56.53909770605108, -49.73151173357274, -34.11186523750416, - -10.827794825167219, 15.891304347107724, 40.20849702734764, 57.368805169815346, - 65.95177865876515, 67.97159561584449, 66.64040895830699, 63.589047697786704, 57.82287342197275, - 47.11725574000715, 30.46733711609649, 9.533355760921287, -11.897470382363254, -29.830521402437533, - -41.96018651062839, -48.39143034359017, -50.90765467089368, -51.442211403866196, - -50.88101688407953, -48.972314263916196, -45.17504471685727, -39.55329712338891, - -32.95725845941995, -26.49322988683765, -20.878353476637844, -16.223447732915336, - -12.318730267752962, -9.114208916103852, -7.025318075714705, -6.863715409937331, - -9.418953881919258, -14.892711365810932, -22.46579817424915, -30.24344467810545, - -35.69109978319271, -36.477876243338805, -31.444025538076012, -21.288670881670154, - -8.62375068107963, 2.723377241865635, 8.950421398026423, 7.60130696452477, -1.5512568272241891, - -16.415907780851523, -33.32800892857806, -48.34061652494549, -58.544149663334146, - -62.8778987289918, -62.129401745774736, -58.221245342094605, -53.23124870684945, - -48.65125161003768, -45.139206829597285, -42.67849855974542, -40.883183302808774, - -39.24556037081911, -37.279870735621394, -34.610218273166865, -31.036388602506094, - -26.556600636656302, -21.314340237646586, -15.485545852961351, -9.182367311554216, - -2.4539357174518477, 4.608108661974362, 11.748942419186927, 18.53564938062657, 24.488775175064866, - 29.268641679431983, 32.76438623868329, 35.01433031937393, 36.04103332494177, 35.76699835677284, - 34.10630639596401, 31.16848412968319, 27.407761733176166, 23.587197726368412, 20.558872791588364, - 18.97592238631098, 19.07565937416213, 20.61709103744411, 22.979740291136025, 25.377603643755922, - 27.117925844469166, 27.82778995770433, 27.579205757414403, 26.87117155011749, 26.47346470212829, - 27.18502510687263, 29.588305367087866, 33.87866838785326, 39.818047979613915, 46.81614514744827, - 54.094984627640684, 60.861552883388576, 66.41686398223497, 70.17429953524406, 71.62539144890958, - 70.3348055057656, 66.03071453327549, 58.78227299675599, 49.168265349183585, 38.3031310449789, - 27.633806894374317, 18.533974132396466, 11.83869780529428, 7.516135260286569, 4.634765508402249, - 1.6752451183326715, -2.8928198989370846, -9.947988875516257, -19.190646683273787, - -28.95981792942289, -36.52500660659488, -38.85392526474802, -33.68717119866384, - -20.59129682191029, -1.5875034968128299, 18.968950912034025, 35.341880979175166, - 41.95032447278416, 34.96068739312236, 13.34321915944511, -20.982377451382774, -63.886780871889336, - -110.27067151277551, -155.29388790639618, -195.21850221882582, -227.74597004288862, - -251.94049712978926] -z: [-26.674573624633936, -27.114809002109638, -27.80093370552566, -28.836873185567413, - -30.21881883969757, -31.695951164601357, -32.72354364740953, -32.62183352655716, - -30.90258581567612, -27.583495683677555, -23.297201486579876, -19.087260962922908, - -15.936418055004204, -14.273620356202283, -13.80346926204938, -13.802500150724061, - -13.630619842081579, -13.020757497121616, -11.96881998725885, -10.469674381861992, - -8.452160823356074, -5.952178992153526, -3.225706585502589, -0.5697494663204837, - 1.9808009060413083, 4.68376066797161, 7.7212422921196655, 10.792547098296602, 13.147282962472149, - 14.103314291770715, 13.571455393667573, 12.085886522451826, 10.339296245986802, - 8.68602893983164, 7.033777346134439, 5.120448773462501, 2.8513690211156364, 0.4137469421774722, - -1.8591108055200387, -3.7053995669548203, -5.044430532720517, -5.89457353611374, - -6.160820814559367, -5.4596622601026645, -3.15564680899608, 1.3247372084788658, - 8.009198179393971, 15.980184925805682, 23.347897126275733, 27.749074489647064, 27.248379889872915, - 21.249227180863592, 10.952075671055974, -0.9012226404090096, -10.918446805389463, - -16.22596783788798, -15.453611074433008, -9.153655798428526, 0.49699749838585544, - 10.561454823123338, 18.391516558218946, 22.436392554843273, 22.545534532960506, - 19.747590276327738, 15.689361506049153, 11.99803586193934, 9.79057402569709, 9.448977960828584, - 10.667279692473349, 12.692684350192357, 14.645560060408101, 15.811942817737004, - 15.84203100562251, 14.829843387399771, 13.271968183018519, 11.913283989441497, 11.507542003036887, - 12.558104629802221, 15.134221452069113, 18.84592814794398, 22.998757891465335, 26.871030400509593, - 30.007884926015393, 32.42946843702773, 34.691007713089675, 37.77704102447371, 42.83936321417833, - 50.80550353800271, 61.91981744856823, 75.34762579946161, 89.04033240312027, 100.0468270773533, - 105.30303255039908, 102.67548082168979, 91.82574965851784, 74.46578861532257, 53.83412456102248, - 33.60637700701678, 16.731362600762733, 4.69041935031565, -2.5784558424742117, -6.19851466771825, - -7.652486552004426, -8.203701204838094, -8.60951161090784, -9.098233442799044, -9.48718196283113, - -9.346536633833383, -8.18047015987454, -5.61948916274534, -1.5868054321866703, 3.6274373814820575, - 9.426783721975054, 15.078581176949248, 19.909144614716634, 23.43487153892179, 25.38970214772102, - 25.6741236265473, 24.288580218363613, 21.306889896281145, 16.904641282521364, 11.411558969184892, - 5.333027801777911, -0.7017358032446038, -6.065575734065763, -10.287538553248018, - -13.167739669212384, -14.80357049337103, -15.524777375079806, -15.780272101184652, - -16.035519447260974, -16.72035793136546, -18.231003979213103, -20.961756875344914, - -25.33507117144374, -31.809284959656072, -40.85818085498114, -52.925248528929615, - -68.35651409024713, -87.31513845671434, -109.68639958664852, -134.99539426547764, - -162.3735258302758, -190.60811846102203, -218.28355820802886, -243.98292875048506, - -266.49124025702065, -284.9430654469704, -298.88475826361605, -308.25498187704557] +pitch: [-156.65718192265612, -156.82512813476856, -157.03476785128979, -157.2807477120766, + -157.55369295460645, -157.84461759860156, -158.14778763059294, -158.459737944698, + -158.77686003690096, -159.09532479965736, -159.41325989374954, -159.73163715474024, + -160.0521375845057, -160.3745530176387, -160.69700732848457, -161.0185104942475, + -161.3402653767862, -161.66397674111627, -161.98953153448417, -162.315201866925, + -162.64003295077077, -162.96511555690367, -163.29205326830424, -163.62078022025358, + -163.9496971026068, -164.27788257902984, -164.60632279261657, -164.93652376742696, + -165.26846111422094, -165.60065680035294, -165.93222401150882, -166.26405206032558, + -166.5975532366255, -166.9327393757321, -167.2682466574162, -167.60322357479959, + -167.93847000443222, -168.27530835178302, -168.61378183900877, -168.95263403217712, + -169.29104925446183, -169.62974494472823, -169.9699574813401, -170.31175703012775, + -170.6539879618333, -170.99587071205337, -171.33804689251744, -171.68167070253227, + -172.02683518428148, -172.37247918025702, -172.71785930237908, -173.06354756695924, + -173.4106198177865, -173.75918826314913, -174.10828013599635, -174.45718809095524, + -174.8064204116356, -175.1569783712803, -175.50898997238295, -175.86156501042154, + -176.21403187168687, -176.56684061134436, -176.9209216656706, -177.276415779213, + -177.63250973601328, -177.9885671847553, -178.34498510912093, -178.7026267789968, + -179.0616429301755, -179.42129201479344, -179.7809723347144, -180.14103262348885, + -180.50227258176474, -180.86485046896803, -181.2280913368986, -181.59142740879287, + -181.95516366594228, -182.3200397542152, -182.6862192544363, -183.0530889992954, + -183.42011429540196, -183.78756055865932, -184.15611080378343, -184.525931978699, + -184.8964681246396, -185.2672167028437, -185.638407452449, -186.01067008275334, + -186.3841731854121, -186.75841368027824, -187.132920178221, -187.50789034493104, + -187.88390380611247, -188.26112928818085, -188.6391124973969, -189.01741212654568, + -189.39619709895, -189.7760000696095, -190.15698858912273, -190.53875329031354, + -190.92088183004392, -191.30351746122224, -191.68714886802226, -192.07194129758645, + -192.4575266759198, -192.8435204676572, -193.23004308121793, -193.617542113636, + -194.0061795490326, -194.39562519327265, -194.78552113473754, -195.17596753027587, + -195.56737365493905, -195.95989742408074, -196.35324332333818, -196.74707886293638, + -197.14148632095296, -197.53683929553736, -197.933290967728, -198.33057750888972, + -198.7283906402854, -199.12679692660544, -199.52613681329163, -199.92655820874447, + -200.32782617456402, -200.72965543146975, -201.13209880120448, -201.53546597968077, + -201.93989917925003, -202.34518974707686, -202.751074198292, -203.1575933993827, + -203.56502857939287, -203.97351593447695, -204.38287067560304, -204.7928499203278, + -205.20348419671345, -205.6150284301484, -206.0276125727257, -206.44107345232243, + -206.85518761577225, -207.26997671022124, -207.68567140275596, -208.10239525551458, + -208.5200046331361, -208.93829436247796, -209.35727851912242, -209.77716544140384, + -210.19807222793216, -210.61987285855759, -211.04237931918513, -211.4655992857971, + -211.8897205841901, -212.3148538391931, -212.74088887477996, -213.16765374694393, + -213.5951507769902, -214.02354898389035, -214.45295256340523, -214.8832655549256, + -215.31433103073036, -215.74614688524275, -216.1788649289692, -216.61258302054992, + -217.04721792048107, -217.48262670125752, -217.91880365055158, -218.3558848648318, + -218.79396199767987, -219.23296316292127, -219.67275845698202, -220.11333928225932, + -220.5548274153216, -220.99730847034053, -221.44072066552712, -221.88494618630875, + -222.32997418117233, -222.77591340446227, -223.22284362421576, -223.67071202540126, + -224.119411989995, -224.56893096190808, -225.01936587844727, -225.4707908770044, + -225.9231610756851, -226.37638020375584, -226.83043447547186, -227.2854101278763, + -227.7413759005292, -228.1982939079816, -228.65607742107326, -229.11471183206265, + -229.57427371024147, -230.0348266430826, -230.4963388949898, -230.95873251621126, + -231.42199242410942, -231.88618647266296, -232.35137335201267, -232.81752671335113, + -233.2845766674393, -233.7525079495373, -234.22138057487498, -234.69124859655113, + -235.16209036671597, -235.6338433804674, -236.1064924352654, -236.58009051246535, + -237.05468729088872, -237.53026520903265, -238.00676851209414, -238.48418226093696, + -238.9625531403663, -239.44192671749772, -239.92228896806165, -240.40359029407247, + -240.88581618288202, -241.3690076965991, -241.85320655070794, -242.33840176912216, + -242.8245493571952, -243.31163535831513, -243.79969582627467, -244.2887688805353, + -244.77884615907135, -245.26988875560264, -245.7618833697687, -246.2548616058494, + -246.74885823676817, -247.2438671305236, -247.73985399131797, -248.23680624976333, + -248.7347515676374, -249.23372161331298, -249.73371214631064, -250.2346930390105, + -250.73665250571727, -251.23961472458112, -251.74360849280094, -252.2486311641883, + -252.75465637099336, -253.2616731450968, -253.7697025952815, -254.27877087145927, + -254.78887666179244, -255.29999698245618, -255.81212170080903, -256.3252692292884, + -256.8394632842479, -257.3547036618484, -257.87097041693903, -258.3882542568401, + -258.9065712326523, -259.4259428302655, -259.94636975763615, -260.4678347920479, + -260.99032947413946, -261.5138677937398, -262.0384691984253, -262.564135138717, + -263.09085082541867, -263.6186086167544, -264.14742070931345, -264.677304693403, + -265.208262616922, -265.7402818609308, -266.27335557821664, -266.807494410878, -267.3427142618617, + -267.8790176526088, -268.4163938951752, -268.9548369081826, -269.4943559912946, + -270.0349655189502, -270.5766683811865, -271.11945560417905, -271.6633218393314, + -272.20827523166446, -272.75432877508274, -273.3014856399149, -273.84973837039325, + -274.3990823145224, -274.94952462848454, -275.50107706299923, -276.0537429949804, + -276.6075163099427, -277.16239301421695, -277.7183794210768, -278.27548616510694, + -278.8337167688488, -279.39306630014715, -279.95353138416385, -280.5151176192923, + -281.07783464110923, -281.64168606790315, -282.2066680073117, -282.77277766335567, + -283.3400200314943, -283.90840385592054, -284.47793281036434, -285.0486039147941, + -285.6204149122557, -286.19337029282093, -286.7674780078702, -287.3427417545014, + -287.91915935135097, -288.4967290413007, -289.07545489372984, -289.655344157199, + -290.2364005271319, -290.81862251976554, -291.40200883968265, -291.986563208829, + -292.57229225484906, -293.15919965241676, -293.7472845257627, -294.3365460044106, + -294.9269875259936, -295.51861517154896, -296.1114325809504, -296.70543940721166, + -297.3006351696594, -297.89702307577414, -298.49460872719874, -299.0933957191521, + -299.69338416362336, -300.29457393640496, -300.8969680610969, -301.50057172055426, + -302.10538845895877, -302.71141878594295, -303.31866290235337, -303.92712368726046, + -304.53680595921657, -305.14771320782285, -305.7598462866421, -306.3732056921633, + -306.98779419222967, -307.6036162899247, -308.2206754190174, -308.83897273011434, + -309.4585089879684, -310.0792868772328, -310.70131062915857, -311.3245836222539, + -311.94910726337764, -312.5748825602012, -313.20191213766174, -313.830199994053, + -314.45974945461137, -315.0905621470852, -315.72263929872355, -316.3559834942804, + -316.9905985336233, -317.62648769178344, -318.2636527868513, -318.9020952442645, + -319.5418176247443, -320.18282356031045, -320.8251162796443, -321.4686977648914, + -322.11356962017334, -322.7597343954331, -323.4071955818418, -324.0559563661364, + -324.7060188719847, -325.35738486448656, -326.01005689360136, -326.6640383334178, + -327.3193323334846, -327.97594113975754, -328.63386666231656, -329.29311145984894, + -329.95367881022037, -330.6155718307366, -331.2787928732944, -331.94334397856255, + -332.6092277209164, -333.2764473002516, -333.9450058066361, -334.6149056840792, + -335.286149090949, -335.9587386228065, -336.6326774175029, -337.3079685428293, -337.9846145232684, + -338.6626176233944, -339.3419804642377, -340.022706135458, -340.7047976874095, -341.3882577153029, + -342.07308857971486, -342.75929293043066, -343.4468738209348, -344.1358342888007, + -344.826176991857, -345.5179043776647, -346.2110191272336, -346.90552426826605, + -347.601422829986, -348.29871752613354, -348.99741088331945, -349.6975056155889, + -350.3990047338262, -351.1019112630828, -351.80622796750447, -352.51195744580275, + -353.21910244634364, -353.9276659709034, -354.63765104426665, -355.3490604765017, + -356.0618969323631, -356.7761631954104, -357.4918622649251, -358.2089971690505, + -358.927570760162, -359.64758576380353, -360.36904499927863, -361.09195146903556, + -361.8163082079192, -362.5421181077289, -363.2693839502654, -363.99810859088376, + -364.72829504003204, -365.4599463423245, -366.1930654267157, -366.9276551273741, + -367.6637183358344, -368.4012580746632, -369.14027740104433, -369.88077927933233, + -370.6227665927477, -371.3662422690045, -372.11120934629133, -372.8576708969081, + -373.60562991928094, -374.3550893428728, -375.1060521314913, -375.85852134192436, + -376.6125000638935, -377.3679913289216, -378.12499811035156, -378.88352340794614, + -379.64357029961894, -380.4051418945974, -381.1682412568148, -381.9328714015242, + -382.69903536427796, -383.46673624626146, -384.2359771780845, -385.0067612556417, + -385.7790915344696, -386.5529710857383, -387.32840303572766, -388.1053905381184, + -388.8839367205077, -389.66404467738937, -390.4457175153868, -391.2289583874262, + -392.0137704717747, -392.8001569276271, -393.58812088735715, -394.37766549287124, + -395.1687939249686, -395.96150938755676, -396.7558150704628, -397.55171414010636, + -398.3492097647753, -399.1483051294121, -399.94900339695914, -400.7513076097339, + -401.55522051642305, -402.3607442126537, -403.1678793129748, -403.97662313703535, + -404.78696601570533, -405.5988841676912, -406.41232657422364, -407.22719193743313, + -408.04329038092243, -408.86028352164305, -409.67759674852243, -410.49430019593217, + -411.30896127385165, -412.11948227644524, -412.9229503097521, -413.71553969665734, + -414.49251289191534, -415.2483582419702, -415.9770785993685, -416.6726079072575, + -417.329295131207, -417.94237131855783, -418.5083166266741, -419.02506982614034, + -419.4920624207183, -419.91009801422723, -420.28112301456906, -420.60794297702034, + -420.89393277454684] +roll: [-156.65718192265612, -156.82512813476856, -157.03476785128979, -157.2807477120766, + -157.55369295460645, -157.84461759860156, -158.14778763059294, -158.459737944698, + -158.77686003690096, -159.09532479965736, -159.41325989374954, -159.73163715474024, + -160.0521375845057, -160.3745530176387, -160.69700732848457, -161.0185104942475, + -161.3402653767862, -161.66397674111627, -161.98953153448417, -162.315201866925, + -162.64003295077077, -162.96511555690367, -163.29205326830424, -163.62078022025358, + -163.9496971026068, -164.27788257902984, -164.60632279261657, -164.93652376742696, + -165.26846111422094, -165.60065680035294, -165.93222401150882, -166.26405206032558, + -166.5975532366255, -166.9327393757321, -167.2682466574162, -167.60322357479959, + -167.93847000443222, -168.27530835178302, -168.61378183900877, -168.95263403217712, + -169.29104925446183, -169.62974494472823, -169.9699574813401, -170.31175703012775, + -170.6539879618333, -170.99587071205337, -171.33804689251744, -171.68167070253227, + -172.02683518428148, -172.37247918025702, -172.71785930237908, -173.06354756695924, + -173.4106198177865, -173.75918826314913, -174.10828013599635, -174.45718809095524, + -174.8064204116356, -175.1569783712803, -175.50898997238295, -175.86156501042154, + -176.21403187168687, -176.56684061134436, -176.9209216656706, -177.276415779213, + -177.63250973601328, -177.9885671847553, -178.34498510912093, -178.7026267789968, + -179.0616429301755, -179.42129201479344, -179.7809723347144, -180.14103262348885, + -180.50227258176474, -180.86485046896803, -181.2280913368986, -181.59142740879287, + -181.95516366594228, -182.3200397542152, -182.6862192544363, -183.0530889992954, + -183.42011429540196, -183.78756055865932, -184.15611080378343, -184.525931978699, + -184.8964681246396, -185.2672167028437, -185.638407452449, -186.01067008275334, + -186.3841731854121, -186.75841368027824, -187.132920178221, -187.50789034493104, + -187.88390380611247, -188.26112928818085, -188.6391124973969, -189.01741212654568, + -189.39619709895, -189.7760000696095, -190.15698858912273, -190.53875329031354, + -190.92088183004392, -191.30351746122224, -191.68714886802226, -192.07194129758645, + -192.4575266759198, -192.8435204676572, -193.23004308121793, -193.617542113636, + -194.0061795490326, -194.39562519327265, -194.78552113473754, -195.17596753027587, + -195.56737365493905, -195.95989742408074, -196.35324332333818, -196.74707886293638, + -197.14148632095296, -197.53683929553736, -197.933290967728, -198.33057750888972, + -198.7283906402854, -199.12679692660544, -199.52613681329163, -199.92655820874447, + -200.32782617456402, -200.72965543146975, -201.13209880120448, -201.53546597968077, + -201.93989917925003, -202.34518974707686, -202.751074198292, -203.1575933993827, + -203.56502857939287, -203.97351593447695, -204.38287067560304, -204.7928499203278, + -205.20348419671345, -205.6150284301484, -206.0276125727257, -206.44107345232243, + -206.85518761577225, -207.26997671022124, -207.68567140275596, -208.10239525551458, + -208.5200046331361, -208.93829436247796, -209.35727851912242, -209.77716544140384, + -210.19807222793216, -210.61987285855759, -211.04237931918513, -211.4655992857971, + -211.8897205841901, -212.3148538391931, -212.74088887477996, -213.16765374694393, + -213.5951507769902, -214.02354898389035, -214.45295256340523, -214.8832655549256, + -215.31433103073036, -215.74614688524275, -216.1788649289692, -216.61258302054992, + -217.04721792048107, -217.48262670125752, -217.91880365055158, -218.3558848648318, + -218.79396199767987, -219.23296316292127, -219.67275845698202, -220.11333928225932, + -220.5548274153216, -220.99730847034053, -221.44072066552712, -221.88494618630875, + -222.32997418117233, -222.77591340446227, -223.22284362421576, -223.67071202540126, + -224.119411989995, -224.56893096190808, -225.01936587844727, -225.4707908770044, + -225.9231610756851, -226.37638020375584, -226.83043447547186, -227.2854101278763, + -227.7413759005292, -228.1982939079816, -228.65607742107326, -229.11471183206265, + -229.57427371024147, -230.0348266430826, -230.4963388949898, -230.95873251621126, + -231.42199242410942, -231.88618647266296, -232.35137335201267, -232.81752671335113, + -233.2845766674393, -233.7525079495373, -234.22138057487498, -234.69124859655113, + -235.16209036671597, -235.6338433804674, -236.1064924352654, -236.58009051246535, + -237.05468729088872, -237.53026520903265, -238.00676851209414, -238.48418226093696, + -238.9625531403663, -239.44192671749772, -239.92228896806165, -240.40359029407247, + -240.88581618288202, -241.3690076965991, -241.85320655070794, -242.33840176912216, + -242.8245493571952, -243.31163535831513, -243.79969582627467, -244.2887688805353, + -244.77884615907135, -245.26988875560264, -245.7618833697687, -246.2548616058494, + -246.74885823676817, -247.2438671305236, -247.73985399131797, -248.23680624976333, + -248.7347515676374, -249.23372161331298, -249.73371214631064, -250.2346930390105, + -250.73665250571727, -251.23961472458112, -251.74360849280094, -252.2486311641883, + -252.75465637099336, -253.2616731450968, -253.7697025952815, -254.27877087145927, + -254.78887666179244, -255.29999698245618, -255.81212170080903, -256.3252692292884, + -256.8394632842479, -257.3547036618484, -257.87097041693903, -258.3882542568401, + -258.9065712326523, -259.4259428302655, -259.94636975763615, -260.4678347920479, + -260.99032947413946, -261.5138677937398, -262.0384691984253, -262.564135138717, + -263.09085082541867, -263.6186086167544, -264.14742070931345, -264.677304693403, + -265.208262616922, -265.7402818609308, -266.27335557821664, -266.807494410878, -267.3427142618617, + -267.8790176526088, -268.4163938951752, -268.9548369081826, -269.4943559912946, + -270.0349655189502, -270.5766683811865, -271.11945560417905, -271.6633218393314, + -272.20827523166446, -272.75432877508274, -273.3014856399149, -273.84973837039325, + -274.3990823145224, -274.94952462848454, -275.50107706299923, -276.0537429949804, + -276.6075163099427, -277.16239301421695, -277.7183794210768, -278.27548616510694, + -278.8337167688488, -279.39306630014715, -279.95353138416385, -280.5151176192923, + -281.07783464110923, -281.64168606790315, -282.2066680073117, -282.77277766335567, + -283.3400200314943, -283.90840385592054, -284.47793281036434, -285.0486039147941, + -285.6204149122557, -286.19337029282093, -286.7674780078702, -287.3427417545014, + -287.91915935135097, -288.4967290413007, -289.07545489372984, -289.655344157199, + -290.2364005271319, -290.81862251976554, -291.40200883968265, -291.986563208829, + -292.57229225484906, -293.15919965241676, -293.7472845257627, -294.3365460044106, + -294.9269875259936, -295.51861517154896, -296.1114325809504, -296.70543940721166, + -297.3006351696594, -297.89702307577414, -298.49460872719874, -299.0933957191521, + -299.69338416362336, -300.29457393640496, -300.8969680610969, -301.50057172055426, + -302.10538845895877, -302.71141878594295, -303.31866290235337, -303.92712368726046, + -304.53680595921657, -305.14771320782285, -305.7598462866421, -306.3732056921633, + -306.98779419222967, -307.6036162899247, -308.2206754190174, -308.83897273011434, + -309.4585089879684, -310.0792868772328, -310.70131062915857, -311.3245836222539, + -311.94910726337764, -312.5748825602012, -313.20191213766174, -313.830199994053, + -314.45974945461137, -315.0905621470852, -315.72263929872355, -316.3559834942804, + -316.9905985336233, -317.62648769178344, -318.2636527868513, -318.9020952442645, + -319.5418176247443, -320.18282356031045, -320.8251162796443, -321.4686977648914, + -322.11356962017334, -322.7597343954331, -323.4071955818418, -324.0559563661364, + -324.7060188719847, -325.35738486448656, -326.01005689360136, -326.6640383334178, + -327.3193323334846, -327.97594113975754, -328.63386666231656, -329.29311145984894, + -329.95367881022037, -330.6155718307366, -331.2787928732944, -331.94334397856255, + -332.6092277209164, -333.2764473002516, -333.9450058066361, -334.6149056840792, + -335.286149090949, -335.9587386228065, -336.6326774175029, -337.3079685428293, -337.9846145232684, + -338.6626176233944, -339.3419804642377, -340.022706135458, -340.7047976874095, -341.3882577153029, + -342.07308857971486, -342.75929293043066, -343.4468738209348, -344.1358342888007, + -344.826176991857, -345.5179043776647, -346.2110191272336, -346.90552426826605, + -347.601422829986, -348.29871752613354, -348.99741088331945, -349.6975056155889, + -350.3990047338262, -351.1019112630828, -351.80622796750447, -352.51195744580275, + -353.21910244634364, -353.9276659709034, -354.63765104426665, -355.3490604765017, + -356.0618969323631, -356.7761631954104, -357.4918622649251, -358.2089971690505, + -358.927570760162, -359.64758576380353, -360.36904499927863, -361.09195146903556, + -361.8163082079192, -362.5421181077289, -363.2693839502654, -363.99810859088376, + -364.72829504003204, -365.4599463423245, -366.1930654267157, -366.9276551273741, + -367.6637183358344, -368.4012580746632, -369.14027740104433, -369.88077927933233, + -370.6227665927477, -371.3662422690045, -372.11120934629133, -372.8576708969081, + -373.60562991928094, -374.3550893428728, -375.1060521314913, -375.85852134192436, + -376.6125000638935, -377.3679913289216, -378.12499811035156, -378.88352340794614, + -379.64357029961894, -380.4051418945974, -381.1682412568148, -381.9328714015242, + -382.69903536427796, -383.46673624626146, -384.2359771780845, -385.0067612556417, + -385.7790915344696, -386.5529710857383, -387.32840303572766, -388.1053905381184, + -388.8839367205077, -389.66404467738937, -390.4457175153868, -391.2289583874262, + -392.0137704717747, -392.8001569276271, -393.58812088735715, -394.37766549287124, + -395.1687939249686, -395.96150938755676, -396.7558150704628, -397.55171414010636, + -398.3492097647753, -399.1483051294121, -399.94900339695914, -400.7513076097339, + -401.55522051642305, -402.3607442126537, -403.1678793129748, -403.97662313703535, + -404.78696601570533, -405.5988841676912, -406.41232657422364, -407.22719193743313, + -408.04329038092243, -408.86028352164305, -409.67759674852243, -410.49430019593217, + -411.30896127385165, -412.11948227644524, -412.9229503097521, -413.71553969665734, + -414.49251289191534, -415.2483582419702, -415.9770785993685, -416.6726079072575, + -417.329295131207, -417.94237131855783, -418.5083166266741, -419.02506982614034, + -419.4920624207183, -419.91009801422723, -420.28112301456906, -420.60794297702034, + -420.89393277454684] +x: [-131.00261478679988, -121.88886417422353, -111.9050389833139, -102.61368963497223, + -96.05947080078046, -94.12535169815399, -97.8607435820271, -107.14613364957096, + -120.82659085972966, -137.19720078410953, -154.5163549178483, -171.18694825080595, + -185.5568218223094, -195.704691059387, -199.6329868378576, -195.91936207338776, + -184.48915098542318, -167.07793507920195, -147.0574840422674, -128.53128958566316, + -115.04358889413827, -108.56400855374055, -109.19561099927448, -115.57413755530115, + -125.64926844955686, -137.46655539270932, -149.61816709281686, -161.26501144566066, + -171.91636446335687, -181.2216960780747, -188.87007846616464, -194.53289513944736, + -197.79145167358277, -198.0996571089822, -194.8802396222356, -187.78401698176944, + -177.02826515891735, -163.64756841648978, -149.46450567572836, -136.67890898964777, + -127.21395723237792, -122.15440349976664, -121.54367242044958, -124.56838026444774, + -129.9847428341204, -136.57776680869415, -143.4619977851926, -150.1463172881614, + -156.43047062938794, -162.25579819785062, -167.580799896415, -172.2857363783464, + -176.10313080152142, -178.60602094331426, -179.29466139771722, -177.77366590562514, + -173.94835807935246, -168.14763364573156, -161.10576879062413, -153.79437054295704, + -147.18005615509452, -142.03544698559352, -138.8812834694729, -138.0254327793118, + -139.60154077046317, -143.53795283987066, -149.46190564030064, -156.61307764750555, + -163.87292592523232, -169.97827533844975, -173.87402597845175, -175.0512378675456, + -173.7109894894428, -170.68112972530366, -167.11714893622806, -164.10386427943158, + -162.31622495964814, -161.86114761154576, -162.33151047253835, -163.02192881838113, + -163.2095501914243, -162.39248606696069, -160.40951035619491, -157.42206476020834, + -153.79159590063577, -149.91799716532543, -146.1123968434442, -142.55104990302257, + -139.31149673683075, -136.46131741930625, -134.16539879470983, -132.7805579230332, + -132.8976720404706, -135.27842864395015, -140.64645739243105, -149.3508159834204, + -161.00381971456645, -174.2605798886825, -186.9134811438676, -196.38261868432437, + -200.49211090980378, -198.23763830134754, -190.21480773205266, -178.51115567595826, + -166.0801280692791, -155.83164235826004, -149.80028431814193, -148.6986554765198, + -151.96202945833986, -158.18027510739103, -165.68937034330935, -173.07927043758525, + -179.46192687105753, -184.482441275634, -188.1614046703995, -190.6833573748501, + -192.21809683888495, -192.81960540582097, -192.41403431825069, -190.86577715074623, + -188.09301880254696, -184.18878513004086, -179.4955981906823, -174.5892113462465, + -170.1561255168205, -166.79776163996192, -164.8373602238494, -164.2126907946925, + -164.50294621069565, -165.0839384166419, -165.3557930296701, -164.95884643563477, + -163.898412256587, -162.534820649653, -161.44472376807636, -161.20582442689792, + -162.18450627688458, -164.40124437891336, -167.51392693816024, -170.91206169710102, + -173.8749652264376, -175.73083709030058, -175.97063351490164, -174.31004528380663, + -170.72511032331406, -165.48897950595216, -159.20946482285498, -152.82808750163824, + -147.5191667738631, -144.44886967752777, -144.4240273588793, -147.54449659368146, + -153.01617480666846, -159.25176211818498, -164.2877168274527, -166.41140144219656, + -164.78014816559207, -159.78674905727087, -153.00534915065177, -146.70549554069234, + -143.09178186665255, -143.5446658872191, -148.14637102277132, -155.6663204124734, + -164.00416538606794, -170.916565992789, -174.75161613413627, -174.9259359923934, + -171.99663265443107, -167.34336788125873, -162.6147423868518, -159.16048671989986, + -157.6477458947188, -157.96425018036857, -159.39300876091175, -160.95217491299363, + -161.7572333232152, -161.28442609094157, -159.4763125555903, -156.69932273396668, + -153.6090737832136, -150.9888364227075, -149.60413892450302, -150.0809012374839, + -152.79153608790728, -157.7397093479904, -164.46467355593433, -172.0172934256874, + -179.0661158741304, -184.15783677719142, -186.08855140295393, -184.27238379689192, + -178.96525805487465, -171.2372865037682, -162.67650112956372, -154.91277319136154, + -149.12741792008617, -145.72381647659327, -144.27134285886908, -143.72945580899287, + -142.85723879551645, -140.65447875831518, -136.68276519662683, -131.17312406130938, + -124.91209567953047, -118.9751543144055, -114.4168612960534, -112.01955243552163, + -112.15689719095518, -114.77332263747567, -119.44360198084003, -125.471236872446, + -132.00253791160844, -138.1561515203214, -143.17484043177828, -146.58907912429945, + -148.3511438380617, -148.87742279194816, -148.94591893398302, -149.43982973654823, + -150.99302147422435, -153.65215284576814, -156.69307614107936, -158.6972973355033, + -157.91096745292066, -152.8004175957856, -142.62726270976202, -127.83568739093043, + -110.09574251961111, -91.96442744166842, -76.26517339306562, -65.38756088681923, + -60.727455422828086, -62.41769838627875, -69.38103902919622, -79.63028303419432, + -90.6904492501862, -100.0330543381811, -105.46650890222921, -105.47370228165889, + -99.49440285791094, -88.11732926351014, -73.10854974665124, -57.20048636703596, + -43.619919989560685, -35.42935377805215, -34.84789242860901, -42.75227348465364, + -58.50946410168745, -80.17821602052324, -104.99065766337972, -129.94273353227604, + -152.31614487459336, -170.01709776381944, -181.70800379508364, -186.78019457154642, + -185.24347046638363, -177.59665180543396, -164.71393092910603, -147.75511927137686, + -128.09259386421706, -107.2415258381779, -86.77724029789665, -68.22375027449615, + -52.90610923413581, -41.779927174503385, -35.27752870209294, -33.22579478747385, + -34.88146090859914, -39.094144062824924, -44.55933540558859, -50.084953618202576, + -54.78496998250598, -58.13877584276208, -59.90622409641475, -59.945540069741, -58.02266713890794, + -53.710211558038985, -46.44646056518766, -35.76628477961155, -21.64346099537084, + -4.823099943406338, 13.00074336490228, 29.276996809341675, 40.992025758372115, 45.29133769691425, + 40.16014663983825, 24.958013623469462, 0.6430403690297408, -30.37648983818459, -64.70364605521273, + -98.5780766753714, -128.49010205980096, -151.64916889145232, -166.24865624155242, + -171.54102444522545, -167.77666186080413, -156.06265128020192, -138.17902607586348, + -116.36917642692488, -93.10932192371236, -70.85982959536072, -51.803566443571675, + -37.58102962803787, -29.04191085373232, -26.05140387409622, -27.411942444947794, + -30.971298702656785, -33.967139701487234, -33.59780771242022, -27.721946232536823, + -15.50944865087424, 2.165242029053751, 22.75109265450623, 42.51462881068035, 57.39178500711525, + 63.9603626762866, 60.24680348570627, 46.13482203280333, 23.280449691574223, -5.396531697017779, + -36.44546991778746, -66.59572190185905, -93.244393312115, -114.63002544381644, -129.7277803714285, + -137.99626292368302, -139.13919014515983, -133.01230035499455, -119.72497935278233, + -99.88864280213238, -74.88535670756845, -46.99987777401981, -19.290622507018494, + 4.836900082983619, 22.275753850864515, 30.9051736945987, 30.102091149122813, 20.89767984126534, + 5.709426552642763, -12.280539670085691, -29.912602539696113, -44.76409817954616, + -55.587777836517255, -62.37400184312159, -66.06509298635545, -68.05618356119531, + -69.6650415443935, -71.73438308006683, -74.46044693767506, -77.45330286152975, -79.96093287153582, + -81.15272296993173, -80.36351741010003, -77.23600916293604, -71.74756168638527, + -64.14841195780141, -54.85947253693819, -44.377179252428895, -33.21572713174938, + -21.893453397774184, -10.949679527408357, -0.9673128756345823, 7.423219742995925, + 13.573678927669173, 16.882316200266693, 16.880518451590333, 13.321463895553073, + 6.2494544216467425, -3.9662652022608986, -16.64105804227179, -30.827458079441, -45.40427423165941, + -59.19176164715269, -71.07926973763034, -80.1486617898492, -85.77441240031021, -87.6818651318595, + -85.95088814283162, -80.96333944498441, -73.30666637565525, -63.65782924774226, + -52.676828368491805, -40.93506347992221, -28.891766117399765, -16.916080004322772, + -5.3384835550584935, 5.4924058263232025, 15.171162899221802, 23.231051427618734, + 29.178380505050853, 32.559117153019926, 33.042995254450894, 30.506717141912656, + 25.098309221713915, 17.267877434957573, 7.754667766136128, -2.4738852870074073, + -12.328160893560188, -20.73549791063537, -26.79110541102302, -29.892155231726377, + -29.825103487837175, -26.78324113785426, -21.306403690118064, -14.15394397582623, + -6.139545262622449, 2.0335265366227753, 9.89870331065091, 17.286397543623664, 24.307308641748374, + 31.26525601401877, 38.52026696289829, 46.33161129748563, 54.71416332554038, 63.34061174435135, + 71.51679336934421, 78.24704133338132, 82.39018658725793, 82.88576123321361, 79.00811170033668, + 70.59040844420252, 58.15831519340492, 42.928737491281964, 26.660959489767816, 11.387156505595188, + -0.9161323493847441, -8.636955778598946, -10.746100985455664, -6.88302166146115, + 2.6901550272644528, 17.227609362490647, 35.67099777743684, 56.793973316018906, 79.28492204264502, + 101.76799145249652, 122.79490061004348, 140.8566568494178, 154.45830822835748, 162.2730575641024, + 163.35441171879694, 157.35115570417733, 144.6537822686842, 126.41052162787045, 104.38486553091724, + 80.67370587471055, 57.34955312756698, 36.11589971154659, 18.06234868471712, 3.576456180471944, + -7.5771785605298545, -16.047264197592554, -22.625071287068153, -27.99125205987563, + -32.544187050649704, -36.342770320132814, -39.160889188810124, -40.621845381417664, + -40.36566724147911, -38.20206823583871, -34.21329090178294, -28.788359919239177, + -22.587615544563256, -16.449965238654954, -11.263594052219574, -7.82420704968046, + -6.7043106382969215, -8.153633537426817, -12.045315570129013, -17.87558753135798, + -24.817072522162295, -31.818486356751524, -37.73735081273553, -41.48808157884077, + -42.185843095170476, -39.266966337888135, -32.56948414179687, -22.362390213218752, + -9.319317728622229, 5.559271102094557, 21.062973392548795, 35.90860732266084, 48.87612268299202, + 58.92359238196129, 65.26758898268541, 67.42353430763573, 65.20869447959286, 58.716140928984274, + 48.27012084952794, 34.372265068707556, 17.645386644363096, -1.221017147956034, -21.52101235133283, + -42.57613526226664, -63.77089174961588, -84.5781075128901, -104.57331194568057, + -123.43864361815416, -140.9579908724867, -157.005848574077] +y: [-99.19714419940159, -79.16394233266908, -57.87218080909418, -39.28438472983111, + -28.386800564847263, -29.413961436687575, -43.913473953576144, -69.67422837210728, + -101.14133069492664, -131.32148548272554, -154.36897298960562, -167.56000537729983, + -171.8480811942197, -171.05140505400772, -170.15303380273187, -173.34724789149277, + -182.51652550260764, -196.6077612179733, -212.0578803921079, -224.1551729270074, + -228.85174829123696, -224.25993586587964, -211.25206790585622, -193.0143316349947, + -173.74730833399678, -157.0391212367104, -144.63181352345478, -136.04353714588737, + -129.03535351100325, -120.64812612176986, -108.4578159331508, -91.66712058998779, + -71.6845532860305, -51.92454636690193, -36.75664559094005, -29.944229192958293, + -33.26253995101549, -45.8841436370077, -64.72238023555612, -85.55581883977085, -104.43639262659643, + -118.73440682883057, -127.4439469965765, -130.85855117578427, -129.9883150575577, + -126.05068565061471, -120.20491993068747, -113.51752511264922, -107.00751318510588, + -101.6120221067554, -98.037143648059, -96.5901783209796, -97.11117132586354, -99.05464430683851, + -101.69784273360709, -104.39663493912799, -106.78682465771945, -108.86073554241558, + -110.91272059008197, -113.391986702604, -116.71712944928908, -121.10899616745179, + -126.49101079170191, -132.48890828569083, -138.52907052668536, -143.98403266052748, + -148.28386085159676, -150.950353874693, -151.59029328770967, -149.92372750552684, + -145.8893613284024, -139.79752247093995, -132.44040508370935, -125.04855412827735, + -119.03041856644234, -115.55253894189944, -115.13281650745513, -117.42895405278668, + -121.3137432440983, -125.21048574002899, -127.55581585488525, -127.2035779940535, + -123.63075578035654, -116.92546848468061, -107.64003980195744, -96.63277766107122, + -84.99305975953892, -74.04821594432073, -65.34295582835624, -60.46526291514498, + -60.70407650618115, -66.66922658810824, -78.05651925687684, -93.67617143035707, + -111.7360977518272, -130.24058283704147, -147.30871275663227, -161.30727700805863, + -170.86975350241954, -174.96772570963563, -173.1273631811409, -165.72224178554154, + -154.15911230206518, -140.7708520660487, -128.34356994048002, -119.39667357114564, + -115.49538835769575, -116.87807764072566, -122.52767636389038, -130.62314678287214, + -139.1652566178275, -146.52616783065477, -151.74687442934803, -154.55565073083676, + -155.2031643789819, -154.2459690762812, -152.3741747997027, -150.31243759575744, + -148.7646263574894, -148.35253400075652, -149.52454396025652, -152.4546938221956, + -156.97962048361924, -162.61455065772643, -168.65639907954113, -174.33920996159839, + -178.98123335741235, -182.07734590854741, -183.33592461504546, -182.69582244314955, + -180.3576867141039, -176.82992166677565, -172.94908163552213, -169.81480317042735, + -168.6009806288589, -170.26752614676, -175.26636333032138, -183.36316952799345, + -193.6626336687613, -204.8455532302751, -215.5341659113945, -224.642821518713, -231.5841029386231, + -236.27823723741093, -239.00153643062308, -240.1602207969857, -240.0805222435039, + -238.8792739976468, -236.4402664233786, -232.4857796328019, -226.70748164073063, + -218.90832171524943, -209.10849307704876, -197.58443529231656, -184.83728952594203, + -171.51692947117536, -158.34183621150794, -146.03906486693822, -135.2931551378401, + -126.67274641338989, -120.522356840103, -116.8495422307382, -115.26711775119627, + -115.04262249541723, -115.26672709456207, -115.09929773879223, -114.01174175462747, + -111.938154926336, -109.27967994662575, -106.7610444266074, -105.19243755344766, + -105.22191192914899, -107.15903178753426, -110.91294427768851, -116.03909562366141, + -121.85319340040662, -127.56333385695282, -132.3915434702031, -135.68773579313347, + -137.0553530223522, -136.49152106606573, -134.50396087391331, -132.13096362061216, + -130.78914260327244, -131.92152103045797, -136.50685861312238, -144.58292188907654, + -154.9805743147388, -165.42722647501083, -173.05862816904528, -175.2162591822015, + -170.27334476780416, -158.19760009121686, -140.64912083959248, -120.58743118943534, + -101.54354695989151, -86.82037717684916, -78.86963873202836, -78.9730241819731, + -87.208732905478, -102.59534157990022, -123.30579810686955, -146.90854660058451, + -170.66096479222475, -191.89527725965974, -208.48091570828308, -219.25913002134993, + -224.29508133070183, -224.82455615157096, -222.87658961783313, -220.67901822911378, + -220.0419990907876, -221.92371042747453, -226.30932003382122, -232.4142180953472, + -239.1075434889348, -245.3854703853822, -250.72594095429886, -255.21957879524751, + -259.4638629985364, -264.29101495969917, -270.44645552037593, -278.33363978175873, + -287.8997813229622, -298.675957930859, -309.9292117127586, -320.853465714804, -330.7289512679556, + -339.01001631839443, -345.3404207556757, -349.52468900709295, -351.4950882757702, + -351.30843956420114, -349.19099656641225, -345.6266700588403, -341.4559943792539, + -337.9255863077082, -336.6122458269782, -339.1590394133497, -346.81553649131257, + -359.86602049319964, -377.1262582942168, -395.74022625809545, -411.4675647034107, + -419.50864481032374, -415.70559925146244, -397.7689969926942, -366.1070646451192, + -323.9312171058613, -276.5534397525774, -230.0823638333837, -189.94210230887091, + -159.6860391307984, -140.44018425867142, -131.05960164254728, -128.83405351298265, + -130.4331021797765, -132.77378964958245, -133.598664448043, -131.70104308630897, + -126.85857197498154, -119.59605542060795, -110.89414841971599, -101.91886722614183, + -93.80124103359078, -87.46894981676736, -83.52587581326873, -82.18044665496083, + -83.22666982990846, -86.0776105317449, -89.84325002250512, -93.43992023523293, -95.72026823608788, + -95.61789886265437, -92.30211434757715, -85.33026401582156, -74.77072267985237, + -61.2585993757004, -45.950045468071075, -30.363497680157124, -16.130369251753944, + -4.708318964022113, 2.8777155744834513, 6.212983448101315, 5.56140196148832, 1.82269284521927, + -3.6117451657027027, -9.093677781503999, -13.032649828418581, -14.265052388454068, + -12.419628464737789, -8.183075798478319, -3.351394348965721, -0.5798384940055397, + -2.8238203221278537, -12.574618457960362, -31.09374357688851, -57.88946050977006, + -90.62907055552884, -125.54979971296869, -158.26608815757967, -184.7370925829678, + -202.10993695070283, -209.20930494571166, -206.57443001611037, -196.09246662518265, + -180.3847681302668, -162.1383131743949, -143.5430153470272, -125.92848416505213, + -109.62850194687415, -94.06091918539822, -77.99447435013359, -59.96518435506764, + -38.78528395904798, -14.052481549971988, 13.469116037352999, 41.73558027808582, + 67.67668776494482, 87.82105256949613, 99.17127607438123, 100.09686637149439, 90.96891255317551, + 74.31360110106945, 54.401903491602475, 36.37112195454357, 25.122323308838894, 24.299071767105474, + 35.60733796084545, 58.60925251977363, 90.97083953417878, 129.02527666162507, 168.46442192730262, + 204.9931440177265, 234.84578165082002, 255.13458309044964, 264.04869333102295, 260.9405833712635, + 246.33164088648044, 221.85233074958896, 190.1149237954721, 154.50438006044362, 118.87017882929412, + 87.1135432104481, 62.69333693214143, 48.115270426306004, 44.50773230112809, 51.40175250111006, + 66.8050512534824, 87.59010715789734, 110.12403882647259, 130.98843565029367, 147.60355998106695, + 158.5997478392373, 163.85990646713972, 164.25977605801083, 161.2187754414792, 156.2146660131347, + 150.40095323538046, 144.40924932571875, 138.34575838676523, 131.9293358853309, 124.68710400298332, + 116.12724252945577, 105.83914725773546, 93.51279283344225, 78.90515326791086, 61.80006387437529, + 42.00492201327354, 19.40692541387674, -5.917558252890152, -33.573230874271864, -62.74274528444666, + -92.15503567861164, -120.15265932612857, -144.85998708249542, -164.4286388593254, + -177.31873975901365, -182.56747411912127, -179.99849715708353, -170.33317799853793, + -155.174775508723, -136.8502267694815, -118.11425365797368, -101.74838641351045, + -90.11915445215804, -84.78453470268744, -86.24244721296932, -93.89058890376597, + -106.21495538177916, -121.15951154482048, -136.5743658936049, -150.61621971325263, + -161.99385323020581, -170.00781819684602, -174.40729806224658, -175.15102714454724, + -172.18938784282471, -165.3700625780896, -154.51544144123787, -139.64662444269274, + -121.26379807895945, -100.56065686649099, -79.46407820455939, -60.445349586067294, + -46.126337444272174, -38.77528077288329, -39.82686472674805, -49.55634236027296, + -66.99108245478719, -90.07326313903191, -116.01965740549392, -141.78042183347972, + -164.48915325804674, -181.81824580195223, -192.1936050384186, -194.86464874708207, + -189.85637149914302, -177.8444701746055, -159.9936426666511, -137.78899220364173, + -112.8776101254829, -86.92681328173411, -61.4995443419655, -37.946187885812385, + -17.314095829999818, -0.2793256793802397, 12.892666001859144, 22.35087361058214, + 28.623738724825635, 32.547011711050814, 35.16166362154166, 37.59316158505565, 40.9252515429786, + 46.081001294230944, 53.72209705873961, 64.17527284982758, 77.39288792317001, 92.95301794423807, + 110.10246016313017, 127.84307430057432, 145.05732762285365, 160.66258701121782, + 173.77600795348275, 183.86412276724406, 190.84585708339466, 195.11799782738245, + 197.48122046939216, 198.9638867275426, 200.56755651863108, 202.98585640947218, 206.36735753017484, + 210.1940659607301, 213.32513104699035, 214.21331928977128, 211.25089340990704, 203.15829367329167, + 189.30944456733187, 169.9006771128133, 145.91404235801855, 118.88681324803126, 90.55706188617755, + 62.490765591022935, 35.79722544105233, 11.007111877948589, -11.866681217908095, + -33.12202103984155, -53.09189944897362, -71.89093420571022, -89.25892597900295, + -104.53982239765106, -116.79113539836814, -124.98292837106044, -128.22643422109374, + -125.9727544559038, -118.13810580734831, -105.13616058766276, -87.82215430794395, + -67.3716974347737, -45.12687495280552, -22.4432811717548, -0.5658122278379778, 19.44939304727038, + 36.762171246899385, 50.732584441996885, 60.87993008610584, 66.83886074810738, 68.3326809251376, + 65.17495028403165, 57.29773071625812, 44.792950118379224, 27.94724280533802, 7.252695883249127, + -16.615157231293985, -42.849357139418395, -70.58183967568638, -98.95676356556059, + -127.19340520577649, -154.63036780787843, -180.74870760607394] +yaw: [-156.65718192265612, -156.82512813476856, -157.03476785128979, -157.2807477120766, + -157.55369295460645, -157.84461759860156, -158.14778763059294, -158.459737944698, + -158.77686003690096, -159.09532479965736, -159.41325989374954, -159.73163715474024, + -160.0521375845057, -160.3745530176387, -160.69700732848457, -161.0185104942475, + -161.3402653767862, -161.66397674111627, -161.98953153448417, -162.315201866925, + -162.64003295077077, -162.96511555690367, -163.29205326830424, -163.62078022025358, + -163.9496971026068, -164.27788257902984, -164.60632279261657, -164.93652376742696, + -165.26846111422094, -165.60065680035294, -165.93222401150882, -166.26405206032558, + -166.5975532366255, -166.9327393757321, -167.2682466574162, -167.60322357479959, + -167.93847000443222, -168.27530835178302, -168.61378183900877, -168.95263403217712, + -169.29104925446183, -169.62974494472823, -169.9699574813401, -170.31175703012775, + -170.6539879618333, -170.99587071205337, -171.33804689251744, -171.68167070253227, + -172.02683518428148, -172.37247918025702, -172.71785930237908, -173.06354756695924, + -173.4106198177865, -173.75918826314913, -174.10828013599635, -174.45718809095524, + -174.8064204116356, -175.1569783712803, -175.50898997238295, -175.86156501042154, + -176.21403187168687, -176.56684061134436, -176.9209216656706, -177.276415779213, + -177.63250973601328, -177.9885671847553, -178.34498510912093, -178.7026267789968, + -179.0616429301755, -179.42129201479344, -179.7809723347144, -180.14103262348885, + -180.50227258176474, -180.86485046896803, -181.2280913368986, -181.59142740879287, + -181.95516366594228, -182.3200397542152, -182.6862192544363, -183.0530889992954, + -183.42011429540196, -183.78756055865932, -184.15611080378343, -184.525931978699, + -184.8964681246396, -185.2672167028437, -185.638407452449, -186.01067008275334, + -186.3841731854121, -186.75841368027824, -187.132920178221, -187.50789034493104, + -187.88390380611247, -188.26112928818085, -188.6391124973969, -189.01741212654568, + -189.39619709895, -189.7760000696095, -190.15698858912273, -190.53875329031354, + -190.92088183004392, -191.30351746122224, -191.68714886802226, -192.07194129758645, + -192.4575266759198, -192.8435204676572, -193.23004308121793, -193.617542113636, + -194.0061795490326, -194.39562519327265, -194.78552113473754, -195.17596753027587, + -195.56737365493905, -195.95989742408074, -196.35324332333818, -196.74707886293638, + -197.14148632095296, -197.53683929553736, -197.933290967728, -198.33057750888972, + -198.7283906402854, -199.12679692660544, -199.52613681329163, -199.92655820874447, + -200.32782617456402, -200.72965543146975, -201.13209880120448, -201.53546597968077, + -201.93989917925003, -202.34518974707686, -202.751074198292, -203.1575933993827, + -203.56502857939287, -203.97351593447695, -204.38287067560304, -204.7928499203278, + -205.20348419671345, -205.6150284301484, -206.0276125727257, -206.44107345232243, + -206.85518761577225, -207.26997671022124, -207.68567140275596, -208.10239525551458, + -208.5200046331361, -208.93829436247796, -209.35727851912242, -209.77716544140384, + -210.19807222793216, -210.61987285855759, -211.04237931918513, -211.4655992857971, + -211.8897205841901, -212.3148538391931, -212.74088887477996, -213.16765374694393, + -213.5951507769902, -214.02354898389035, -214.45295256340523, -214.8832655549256, + -215.31433103073036, -215.74614688524275, -216.1788649289692, -216.61258302054992, + -217.04721792048107, -217.48262670125752, -217.91880365055158, -218.3558848648318, + -218.79396199767987, -219.23296316292127, -219.67275845698202, -220.11333928225932, + -220.5548274153216, -220.99730847034053, -221.44072066552712, -221.88494618630875, + -222.32997418117233, -222.77591340446227, -223.22284362421576, -223.67071202540126, + -224.119411989995, -224.56893096190808, -225.01936587844727, -225.4707908770044, + -225.9231610756851, -226.37638020375584, -226.83043447547186, -227.2854101278763, + -227.7413759005292, -228.1982939079816, -228.65607742107326, -229.11471183206265, + -229.57427371024147, -230.0348266430826, -230.4963388949898, -230.95873251621126, + -231.42199242410942, -231.88618647266296, -232.35137335201267, -232.81752671335113, + -233.2845766674393, -233.7525079495373, -234.22138057487498, -234.69124859655113, + -235.16209036671597, -235.6338433804674, -236.1064924352654, -236.58009051246535, + -237.05468729088872, -237.53026520903265, -238.00676851209414, -238.48418226093696, + -238.9625531403663, -239.44192671749772, -239.92228896806165, -240.40359029407247, + -240.88581618288202, -241.3690076965991, -241.85320655070794, -242.33840176912216, + -242.8245493571952, -243.31163535831513, -243.79969582627467, -244.2887688805353, + -244.77884615907135, -245.26988875560264, -245.7618833697687, -246.2548616058494, + -246.74885823676817, -247.2438671305236, -247.73985399131797, -248.23680624976333, + -248.7347515676374, -249.23372161331298, -249.73371214631064, -250.2346930390105, + -250.73665250571727, -251.23961472458112, -251.74360849280094, -252.2486311641883, + -252.75465637099336, -253.2616731450968, -253.7697025952815, -254.27877087145927, + -254.78887666179244, -255.29999698245618, -255.81212170080903, -256.3252692292884, + -256.8394632842479, -257.3547036618484, -257.87097041693903, -258.3882542568401, + -258.9065712326523, -259.4259428302655, -259.94636975763615, -260.4678347920479, + -260.99032947413946, -261.5138677937398, -262.0384691984253, -262.564135138717, + -263.09085082541867, -263.6186086167544, -264.14742070931345, -264.677304693403, + -265.208262616922, -265.7402818609308, -266.27335557821664, -266.807494410878, -267.3427142618617, + -267.8790176526088, -268.4163938951752, -268.9548369081826, -269.4943559912946, + -270.0349655189502, -270.5766683811865, -271.11945560417905, -271.6633218393314, + -272.20827523166446, -272.75432877508274, -273.3014856399149, -273.84973837039325, + -274.3990823145224, -274.94952462848454, -275.50107706299923, -276.0537429949804, + -276.6075163099427, -277.16239301421695, -277.7183794210768, -278.27548616510694, + -278.8337167688488, -279.39306630014715, -279.95353138416385, -280.5151176192923, + -281.07783464110923, -281.64168606790315, -282.2066680073117, -282.77277766335567, + -283.3400200314943, -283.90840385592054, -284.47793281036434, -285.0486039147941, + -285.6204149122557, -286.19337029282093, -286.7674780078702, -287.3427417545014, + -287.91915935135097, -288.4967290413007, -289.07545489372984, -289.655344157199, + -290.2364005271319, -290.81862251976554, -291.40200883968265, -291.986563208829, + -292.57229225484906, -293.15919965241676, -293.7472845257627, -294.3365460044106, + -294.9269875259936, -295.51861517154896, -296.1114325809504, -296.70543940721166, + -297.3006351696594, -297.89702307577414, -298.49460872719874, -299.0933957191521, + -299.69338416362336, -300.29457393640496, -300.8969680610969, -301.50057172055426, + -302.10538845895877, -302.71141878594295, -303.31866290235337, -303.92712368726046, + -304.53680595921657, -305.14771320782285, -305.7598462866421, -306.3732056921633, + -306.98779419222967, -307.6036162899247, -308.2206754190174, -308.83897273011434, + -309.4585089879684, -310.0792868772328, -310.70131062915857, -311.3245836222539, + -311.94910726337764, -312.5748825602012, -313.20191213766174, -313.830199994053, + -314.45974945461137, -315.0905621470852, -315.72263929872355, -316.3559834942804, + -316.9905985336233, -317.62648769178344, -318.2636527868513, -318.9020952442645, + -319.5418176247443, -320.18282356031045, -320.8251162796443, -321.4686977648914, + -322.11356962017334, -322.7597343954331, -323.4071955818418, -324.0559563661364, + -324.7060188719847, -325.35738486448656, -326.01005689360136, -326.6640383334178, + -327.3193323334846, -327.97594113975754, -328.63386666231656, -329.29311145984894, + -329.95367881022037, -330.6155718307366, -331.2787928732944, -331.94334397856255, + -332.6092277209164, -333.2764473002516, -333.9450058066361, -334.6149056840792, + -335.286149090949, -335.9587386228065, -336.6326774175029, -337.3079685428293, -337.9846145232684, + -338.6626176233944, -339.3419804642377, -340.022706135458, -340.7047976874095, -341.3882577153029, + -342.07308857971486, -342.75929293043066, -343.4468738209348, -344.1358342888007, + -344.826176991857, -345.5179043776647, -346.2110191272336, -346.90552426826605, + -347.601422829986, -348.29871752613354, -348.99741088331945, -349.6975056155889, + -350.3990047338262, -351.1019112630828, -351.80622796750447, -352.51195744580275, + -353.21910244634364, -353.9276659709034, -354.63765104426665, -355.3490604765017, + -356.0618969323631, -356.7761631954104, -357.4918622649251, -358.2089971690505, + -358.927570760162, -359.64758576380353, -360.36904499927863, -361.09195146903556, + -361.8163082079192, -362.5421181077289, -363.2693839502654, -363.99810859088376, + -364.72829504003204, -365.4599463423245, -366.1930654267157, -366.9276551273741, + -367.6637183358344, -368.4012580746632, -369.14027740104433, -369.88077927933233, + -370.6227665927477, -371.3662422690045, -372.11120934629133, -372.8576708969081, + -373.60562991928094, -374.3550893428728, -375.1060521314913, -375.85852134192436, + -376.6125000638935, -377.3679913289216, -378.12499811035156, -378.88352340794614, + -379.64357029961894, -380.4051418945974, -381.1682412568148, -381.9328714015242, + -382.69903536427796, -383.46673624626146, -384.2359771780845, -385.0067612556417, + -385.7790915344696, -386.5529710857383, -387.32840303572766, -388.1053905381184, + -388.8839367205077, -389.66404467738937, -390.4457175153868, -391.2289583874262, + -392.0137704717747, -392.8001569276271, -393.58812088735715, -394.37766549287124, + -395.1687939249686, -395.96150938755676, -396.7558150704628, -397.55171414010636, + -398.3492097647753, -399.1483051294121, -399.94900339695914, -400.7513076097339, + -401.55522051642305, -402.3607442126537, -403.1678793129748, -403.97662313703535, + -404.78696601570533, -405.5988841676912, -406.41232657422364, -407.22719193743313, + -408.04329038092243, -408.86028352164305, -409.67759674852243, -410.49430019593217, + -411.30896127385165, -412.11948227644524, -412.9229503097521, -413.71553969665734, + -414.49251289191534, -415.2483582419702, -415.9770785993685, -416.6726079072575, + -417.329295131207, -417.94237131855783, -418.5083166266741, -419.02506982614034, + -419.4920624207183, -419.91009801422723, -420.28112301456906, -420.60794297702034, + -420.89393277454684] +z: [8.660695913847082, 66.39862553863676, 127.55801351244567, 180.5416567670594, 210.78937741627567, + 205.98510123498664, 161.75492603580668, 84.86410600259634, -7.938204250916348, -95.46180594158663, + -159.6160710741801, -191.12823315769305, -191.1077383247354, -168.75160790760808, + -136.7976917935674, -106.70965336063549, -85.49283736476575, -74.97102632230975, + -72.98493565768426, -75.44155069558714, -78.2857794410305, -78.76734664689184, -75.79970900381551, + -69.60926545532763, -61.02352026219444, -50.731204460839145, -38.784872157834826, + -24.48063415135472, -6.604904189438815, 16.003062940241094, 43.61116166930451, 74.59937262542272, + 104.69471826704422, 127.10591770220823, 134.08234145821157, 119.62663119956167, + 82.25187144352125, 26.508018246556365, -37.60809657045731, -97.50294343727298, -142.05752447785434, + -165.01189926649766, -166.22428468257075, -150.6580066881632, -125.88440608451583, + -99.27427444087671, -76.01596349902346, -58.51176921704588, -46.89688221845846, + -40.0319509439571, -36.428785825300736, -34.84658879993211, -34.526729879516196, + -35.16461507569286, -36.74598621919215, -39.344955846661044, -42.94609674951552, + -47.33366981479339, -52.08037130315903, -56.64311306447958, -60.522268045019175, + -63.40062822000451, -65.19777275447987, -66.03688501834353, -66.16552673973, -65.87798906520248, + -65.46886057404075, -65.21885341822762, -65.38814742633711, -66.18701058321993, + -67.71310218370519, -69.87490985875417, -72.34304758340024, -74.57596554892733, + -75.9445158717209, -75.92859524663281, -74.30883578638903, -71.26712435663396, -67.34312951723155, + -63.24836193520065, -59.60140045704585, -56.690265423268045, -54.35501112675093, + -52.025856935869335, -48.89272594930154, -44.14172332474001, -37.177555708112656, + -27.766047180552132, -16.071976274482385, -2.617381566487062, 11.77309724198873, + 25.976102073264208, 38.53687298107297, 47.718551492321396, 51.76605423223608, 49.38878979505458, + 40.290429255399275, 25.491354807794263, 7.246430327597227, -11.475853168764028, + -27.772349573580907, -39.515015426681174, -45.713572144488516, -46.46431428981413, + -42.61798316112623, -35.37703866753643, -25.9934677516497, -15.628487382325641, + -5.320192334714672, 4.040136476501501, 11.774699698468378, 17.500102100664304, 21.15959975218581, + 22.97185786614963, 23.31019232970664, 22.558042956454567, 20.99209210295209, 18.729017300170803, + 15.74651164213902, 11.966823265504356, 7.376265857048469, 2.1431266936820315, -3.3126251807763514, + -8.343814311527554, -12.216153231932507, -14.333645421496714, -14.465721023384898, + -12.87582842967554, -10.283561071346963, -7.656458262095206, -5.898235147224569, + -5.548464248913495, -6.607585778672925, -8.55034572367405, -10.517780185318616, + -11.61186892570347, -11.181289206074855, -8.998149061838516, -5.278041737321532, + -0.5603284740797135, 4.485651709130345, 9.246488587633161, 13.28299530174339, 16.36756662020893, + 18.466571978647558, 19.704100343285322, 20.32815531831131, 20.677451138832144, 21.13258932476601, + 22.03831545911599, 23.60334269570648, 25.810407707445663, 28.386041772609907, 30.87468716575818, + 32.83047926730481, 34.087238764628125, 35.01230737770861, 36.621360874602765, 40.45070511179779, + 48.15601919282285, 60.917623297793895, 78.84123893360714, 100.59365670804146, 123.46645783546617, + 143.91789347099578, 158.44750364654655, 164.50260889528977, 161.097954064323, 148.96575759387107, + 130.25873364860564, 107.99215885210386, 85.46056101968273, 65.78797889606493, 51.62771188476158, + 44.92502099696882, 46.66619035735277, 56.63547648764704, 73.29795847361908, 93.94368722527568, + 115.14660187881398, 133.44963208761388, 146.06803902842015, 151.38700573272683, + 149.12923984177337, 140.2151559819424, 126.44367536795221, 110.13915686176617, 93.84830850562516, + 80.0820870481205, 71.04848324787652, 68.34789291609472, 72.6765134206983, 83.63977979446459, + 99.77023650435606, 118.77456269522624, 137.94387073801647, 154.60399035259434, 166.49431469790014, + 172.0327438962138, 170.4928109096748, 162.13218328502208, 148.26316136662118, 131.1890050423139, + 113.90461235170372, 99.51589491572973, 90.46166485727544, 87.7622777984477, 90.58353615476729, + 96.33891906881483, 101.3726007035209, 102.04139409749659, 95.84221536672281, 82.19477689643513, + 62.61034213227201, 40.20548872373059, 18.75852748002458, 1.657957295481827, -8.90300742366735, + -12.266306640477236, -9.163238878856461, -1.233167864960895, 9.564495981683649, + 21.43992022175332, 33.051379335207095, 43.56592729978036, 52.568382283902764, 59.9082904825656, + 65.55991544152265, 69.53869459501789, 71.88526118098649, 72.70284621092118, 72.21843649851635, + 70.83368992994872, 69.13827315439758, 67.87334691224184, 67.8505290321987, 69.8442171675916, + 74.47726967536957, 82.11311705302812, 92.75985141769938, 105.99298201503689, 120.91543676261807, + 136.18772657541675, 150.16300634223884, 161.13869607756567, 167.68973106972928, + 168.99857035076562, 165.07365193284676, 156.7707462316869, 145.59585156800276, 133.34673828809346, + 121.70779304048943, 111.92368383650255, 104.63857613545397, 99.9202392953937, 97.42415696314441, + 96.61636301843436, 96.97307879858319, 98.1025495789197, 99.77349073377508, 101.86901694512392, + 104.30470000982287, 106.95197406963027, 109.5967587731362, 111.94423545513219, 113.66135899930364, + 114.4352894158048, 114.02254875294729, 112.27073376540599, 109.10848671943762, 104.5142402049289, + 98.48427593329139, 91.02225206964965, 82.16482996592228, 72.04340823046537, 60.9644383318998, + 49.47596335726721, 38.38208716267078, 28.675265153703773, 21.380090006656577, 17.336295088765645, + 16.98025827773935, 20.1977232072601, 26.30579134436739, 34.1820237918946, 42.50787279716556, + 50.05364436655743, 55.91974464196498, 59.66868428102189, 61.32427965788826, 61.25993059906409, + 60.02892932141973, 58.19725874007958, 56.22498874652909, 54.41596991142811, 52.9291999133868, + 51.82801400185255, 51.138336589289274, 50.892598938155075, 51.146726044509975, 51.96875114225244, + 53.40564661532219, 55.43870808852528, 57.93823519360839, 60.62739916553108, 63.064899619087015, + 64.65655841637202, 64.7055801259163, 62.50676710339701, 57.47927602641731, 49.31661470793777, + 38.11706927966492, 24.4510077185352, 9.330391041091739, -5.928968195692799, -19.927807358089126, + -31.432334046946572, -39.58564206883777, -44.02742340146397, -44.89862410201391, + -42.74448812517229, -38.35747836190584, -32.61145550528262, -26.32954437918894, + -20.20719977067912, -14.789151242435427, -10.48250380540003, -7.582119063177211, + -6.287826913269372, -6.702621306839947, -8.81253570937736, -12.45851173987955, -17.315467703254903, + -22.89273066518226, -28.563759038476075, -33.62421205979971, -37.369384313159216, + -39.17768128465537, -38.587042278968454, -35.35458955341083, -29.49368865852931, + -21.285110785761752, -11.260045417067115, -0.15386876206445768, 11.167509802528333, + 21.80199162922029, 30.914335137198602, 37.83281681446311, 42.1250528920106, 43.6424129336149, + 42.527536031710504, 39.1838083377227, 34.20896823571095, 28.298619788384883, 22.130902226323823, + 16.250738211119412, 10.978234041445447, 6.366580287176825, 2.2267366046830612, -1.780708650958427, + -6.007319377392964, -10.670867462380551, -15.74701218807577, -20.935394624211764, + -25.71475763924228, -29.473174580935297, -31.674804797246836, -32.01284071766054, + -30.503625724620882, -27.49687795069467, -23.603695718045664, -19.567702223835404, + -16.117560259075034, -13.838347771722535, -13.087536620644972, -13.9646827725496, + -16.328965936199715, -19.84970900489466, -24.07290465216959, -28.48977493682292, + -32.59854442331317, -35.95539956520115, -38.2137508958304, -39.15228104770502, -38.692332219912934, + -36.90456601359803, -34.004068329378526, -30.332692841352, -26.327899158206776, + -22.478863588907455, -19.27298194233058, -17.13832959626637, -16.389291255919126, + -17.18279456348231, -19.4913537172405, -23.09696936219348, -27.60749078959273, -32.494672428897246, + -37.15076766150979, -40.95788530079359, -43.36162493187602, -43.93846520775633, + -42.446152023679346, -38.848786666305784, -33.313331043792914, -26.180492183033934, + -17.918310843908614, -9.069298172978261, -0.20074671809499756, 8.136492955152297, + 15.441031108092213, 21.29036758626846, 25.372508549246056, 27.522718958733332, 27.762289494620244, + 26.32911239456066, 23.684885172768034, 20.483910329661786, 17.495388855951955, 15.484035434486861, + 15.069215691030688, 16.59509470023471, 20.047924906971748, 25.048393934050562, 30.927904484071945, + 36.87324018466698, 42.10253497425529, 46.024752154331864, 48.33939153393884, 49.0517871424882, + 48.40580399084933, 46.76081807159332, 44.45507771693884, 41.69825116535199, 38.52289882103032, + 34.803261444171405, 30.327999320714078, 24.898546537842716, 18.420586405729864, + 10.962638364832946, 2.769145218537321, -5.769675941714455, -14.177570162462116, + -21.96287190097139, -28.682775714701705, -33.98993062535793, -37.65744218731808, + -39.584533342726225, -39.78848208739123, -38.38883996706238, -35.58823507210259, + -31.651659030216656, -26.884305680876444, -21.60752145164577, -16.133301088860883, + -10.739449278450719, -5.649094917111724, -1.018762093669203, 3.061876393936713, + 6.558023261640048, 9.468369818928435, 11.800068844557336, 13.547269650304042, 14.679655966520992, + 15.144569931188034, 14.882211925027542, 13.849398415516339, 12.04474239771272, 9.52770863530006, + 6.425839876890219, 2.9278589763920806, -0.735815798162439, -4.320509611419369, -7.596486438809738, + -10.37389619761122, -12.519116524619744, -13.960610192047202, -14.685038419335728, + -14.726561926961596, -14.153639370214792, -13.057964531219815, -11.549353043113381, + -9.758421987878664, -7.846069969532445, -6.015682302494043, -4.521627491850837, + -3.6670759284008367, -3.78622388922754, -5.210529940531722, -8.224269075041521, + -13.019390902549098, -19.661194514880624, -28.07379403356179, -38.04868886403423, + -49.2733259570762, -61.37184021856482, -73.94857243204568, -86.62633925391154, -99.07453382284537, + -111.02550604149512] +pitch: [-156.65718192265612, -156.82512813476856, -157.03476785128979, -157.2807477120766, + -157.55369295460645, -157.84461759860156, -158.14778763059294, -158.459737944698, + -158.77686003690096, -159.09532479965736, -159.41325989374954, -159.73163715474024, + -160.0521375845057, -160.3745530176387, -160.69700732848457, -161.0185104942475, + -161.3402653767862, -161.66397674111627, -161.98953153448417, -162.315201866925, + -162.64003295077077, -162.96511555690367, -163.29205326830424, -163.62078022025358, + -163.9496971026068, -164.27788257902984, -164.60632279261657, -164.93652376742696, + -165.26846111422094, -165.60065680035294, -165.93222401150882, -166.26405206032558, + -166.5975532366255, -166.9327393757321, -167.2682466574162, -167.60322357479959, + -167.93847000443222, -168.27530835178302, -168.61378183900877, -168.95263403217712, + -169.29104925446183, -169.62974494472823, -169.9699574813401, -170.31175703012775, + -170.6539879618333, -170.99587071205337, -171.33804689251744, -171.68167070253227, + -172.02683518428148, -172.37247918025702, -172.71785930237908, -173.06354756695924, + -173.4106198177865, -173.75918826314913, -174.10828013599635, -174.45718809095524, + -174.8064204116356, -175.1569783712803, -175.50898997238295, -175.86156501042154, + -176.21403187168687, -176.56684061134436, -176.9209216656706, -177.276415779213, + -177.63250973601328, -177.9885671847553, -178.34498510912093, -178.7026267789968, + -179.0616429301755, -179.42129201479344, -179.7809723347144, -180.14103262348885, + -180.50227258176474, -180.86485046896803, -181.2280913368986, -181.59142740879287, + -181.95516366594228, -182.3200397542152, -182.6862192544363, -183.0530889992954, + -183.42011429540196, -183.78756055865932, -184.15611080378343, -184.525931978699, + -184.8964681246396, -185.2672167028437, -185.638407452449, -186.01067008275334, + -186.3841731854121, -186.75841368027824, -187.132920178221, -187.50789034493104, + -187.88390380611247, -188.26112928818085, -188.6391124973969, -189.01741212654568, + -189.39619709895, -189.7760000696095, -190.15698858912273, -190.53875329031354, + -190.92088183004392, -191.30351746122224, -191.68714886802226, -192.07194129758645, + -192.4575266759198, -192.8435204676572, -193.23004308121793, -193.617542113636, + -194.0061795490326, -194.39562519327265, -194.78552113473754, -195.17596753027587, + -195.56737365493905, -195.95989742408074, -196.35324332333818, -196.74707886293638, + -197.14148632095296, -197.53683929553736, -197.933290967728, -198.33057750888972, + -198.7283906402854, -199.12679692660544, -199.52613681329163, -199.92655820874447, + -200.32782617456402, -200.72965543146975, -201.13209880120448, -201.53546597968077, + -201.93989917925003, -202.34518974707686, -202.751074198292, -203.1575933993827, + -203.56502857939287, -203.97351593447695, -204.38287067560304, -204.7928499203278, + -205.20348419671345, -205.6150284301484, -206.0276125727257, -206.44107345232243, + -206.85518761577225, -207.26997671022124, -207.68567140275596, -208.10239525551458, + -208.5200046331361, -208.93829436247796, -209.35727851912242, -209.77716544140384, + -210.19807222793216, -210.61987285855759, -211.04237931918513, -211.4655992857971, + -211.8897205841901, -212.3148538391931, -212.74088887477996, -213.16765374694393, + -213.5951507769902, -214.02354898389035, -214.45295256340523, -214.8832655549256, + -215.31433103073036, -215.74614688524275, -216.1788649289692, -216.61258302054992, + -217.04721792048107, -217.48262670125752, -217.91880365055158, -218.3558848648318, + -218.79396199767987, -219.23296316292127, -219.67275845698202, -220.11333928225932, + -220.5548274153216, -220.99730847034053, -221.44072066552712, -221.88494618630875, + -222.32997418117233, -222.77591340446227, -223.22284362421576, -223.67071202540126, + -224.119411989995, -224.56893096190808, -225.01936587844727, -225.4707908770044, + -225.9231610756851, -226.37638020375584, -226.83043447547186, -227.2854101278763, + -227.7413759005292, -228.1982939079816, -228.65607742107326, -229.11471183206265, + -229.57427371024147, -230.0348266430826, -230.4963388949898, -230.95873251621126, + -231.42199242410942, -231.88618647266296, -232.35137335201267, -232.81752671335113, + -233.2845766674393, -233.7525079495373, -234.22138057487498, -234.69124859655113, + -235.16209036671597, -235.6338433804674, -236.1064924352654, -236.58009051246535, + -237.05468729088872, -237.53026520903265, -238.00676851209414, -238.48418226093696, + -238.9625531403663, -239.44192671749772, -239.92228896806165, -240.40359029407247, + -240.88581618288202, -241.3690076965991, -241.85320655070794, -242.33840176912216, + -242.8245493571952, -243.31163535831513, -243.79969582627467, -244.2887688805353, + -244.77884615907135, -245.26988875560264, -245.7618833697687, -246.2548616058494, + -246.74885823676817, -247.2438671305236, -247.73985399131797, -248.23680624976333, + -248.7347515676374, -249.23372161331298, -249.73371214631064, -250.2346930390105, + -250.73665250571727, -251.23961472458112, -251.74360849280094, -252.2486311641883, + -252.75465637099336, -253.2616731450968, -253.7697025952815, -254.27877087145927, + -254.78887666179244, -255.29999698245618, -255.81212170080903, -256.3252692292884, + -256.8394632842479, -257.3547036618484, -257.87097041693903, -258.3882542568401, + -258.9065712326523, -259.4259428302655, -259.94636975763615, -260.4678347920479, + -260.99032947413946, -261.5138677937398, -262.0384691984253, -262.564135138717, + -263.09085082541867, -263.6186086167544, -264.14742070931345, -264.677304693403, + -265.208262616922, -265.7402818609308, -266.27335557821664, -266.807494410878, -267.3427142618617, + -267.8790176526088, -268.4163938951752, -268.9548369081826, -269.4943559912946, + -270.0349655189502, -270.5766683811865, -271.11945560417905, -271.6633218393314, + -272.20827523166446, -272.75432877508274, -273.3014856399149, -273.84973837039325, + -274.3990823145224, -274.94952462848454, -275.50107706299923, -276.0537429949804, + -276.6075163099427, -277.16239301421695, -277.7183794210768, -278.27548616510694, + -278.8337167688488, -279.39306630014715, -279.95353138416385, -280.5151176192923, + -281.07783464110923, -281.64168606790315, -282.2066680073117, -282.77277766335567, + -283.3400200314943, -283.90840385592054, -284.47793281036434, -285.0486039147941, + -285.6204149122557, -286.19337029282093, -286.7674780078702, -287.3427417545014, + -287.91915935135097, -288.4967290413007, -289.07545489372984, -289.655344157199, + -290.2364005271319, -290.81862251976554, -291.40200883968265, -291.986563208829, + -292.57229225484906, -293.15919965241676, -293.7472845257627, -294.3365460044106, + -294.9269875259936, -295.51861517154896, -296.1114325809504, -296.70543940721166, + -297.3006351696594, -297.89702307577414, -298.49460872719874, -299.0933957191521, + -299.69338416362336, -300.29457393640496, -300.8969680610969, -301.50057172055426, + -302.10538845895877, -302.71141878594295, -303.31866290235337, -303.92712368726046, + -304.53680595921657, -305.14771320782285, -305.7598462866421, -306.3732056921633, + -306.98779419222967, -307.6036162899247, -308.2206754190174, -308.83897273011434, + -309.4585089879684, -310.0792868772328, -310.70131062915857, -311.3245836222539, + -311.94910726337764, -312.5748825602012, -313.20191213766174, -313.830199994053, + -314.45974945461137, -315.0905621470852, -315.72263929872355, -316.3559834942804, + -316.9905985336233, -317.62648769178344, -318.2636527868513, -318.9020952442645, + -319.5418176247443, -320.18282356031045, -320.8251162796443, -321.4686977648914, + -322.11356962017334, -322.7597343954331, -323.4071955818418, -324.0559563661364, + -324.7060188719847, -325.35738486448656, -326.01005689360136, -326.6640383334178, + -327.3193323334846, -327.97594113975754, -328.63386666231656, -329.29311145984894, + -329.95367881022037, -330.6155718307366, -331.2787928732944, -331.94334397856255, + -332.6092277209164, -333.2764473002516, -333.9450058066361, -334.6149056840792, + -335.286149090949, -335.9587386228065, -336.6326774175029, -337.3079685428293, -337.9846145232684, + -338.6626176233944, -339.3419804642377, -340.022706135458, -340.7047976874095, -341.3882577153029, + -342.07308857971486, -342.75929293043066, -343.4468738209348, -344.1358342888007, + -344.826176991857, -345.5179043776647, -346.2110191272336, -346.90552426826605, + -347.601422829986, -348.29871752613354, -348.99741088331945, -349.6975056155889, + -350.3990047338262, -351.1019112630828, -351.80622796750447, -352.51195744580275, + -353.21910244634364, -353.9276659709034, -354.63765104426665, -355.3490604765017, + -356.0618969323631, -356.7761631954104, -357.4918622649251, -358.2089971690505, + -358.927570760162, -359.64758576380353, -360.36904499927863, -361.09195146903556, + -361.8163082079192, -362.5421181077289, -363.2693839502654, -363.99810859088376, + -364.72829504003204, -365.4599463423245, -366.1930654267157, -366.9276551273741, + -367.6637183358344, -368.4012580746632, -369.14027740104433, -369.88077927933233, + -370.6227665927477, -371.3662422690045, -372.11120934629133, -372.8576708969081, + -373.60562991928094, -374.3550893428728, -375.1060521314913, -375.85852134192436, + -376.6125000638935, -377.3679913289216, -378.12499811035156, -378.88352340794614, + -379.64357029961894, -380.4051418945974, -381.1682412568148, -381.9328714015242, + -382.69903536427796, -383.46673624626146, -384.2359771780845, -385.0067612556417, + -385.7790915344696, -386.5529710857383, -387.32840303572766, -388.1053905381184, + -388.8839367205077, -389.66404467738937, -390.4457175153868, -391.2289583874262, + -392.0137704717747, -392.8001569276271, -393.58812088735715, -394.37766549287124, + -395.1687939249686, -395.96150938755676, -396.7558150704628, -397.55171414010636, + -398.3492097647753, -399.1483051294121, -399.94900339695914, -400.7513076097339, + -401.55522051642305, -402.3607442126537, -403.1678793129748, -403.97662313703535, + -404.78696601570533, -405.5988841676912, -406.41232657422364, -407.22719193743313, + -408.04329038092243, -408.86028352164305, -409.67759674852243, -410.49430019593217, + -411.30896127385165, -412.11948227644524, -412.9229503097521, -413.71553969665734, + -414.49251289191534, -415.2483582419702, -415.9770785993685, -416.6726079072575, + -417.329295131207, -417.94237131855783, -418.5083166266741, -419.02506982614034, + -419.4920624207183, -419.91009801422723, -420.28112301456906, -420.60794297702034, + -420.89393277454684] +roll: [-156.65718192265612, -156.82512813476856, -157.03476785128979, -157.2807477120766, + -157.55369295460645, -157.84461759860156, -158.14778763059294, -158.459737944698, + -158.77686003690096, -159.09532479965736, -159.41325989374954, -159.73163715474024, + -160.0521375845057, -160.3745530176387, -160.69700732848457, -161.0185104942475, + -161.3402653767862, -161.66397674111627, -161.98953153448417, -162.315201866925, + -162.64003295077077, -162.96511555690367, -163.29205326830424, -163.62078022025358, + -163.9496971026068, -164.27788257902984, -164.60632279261657, -164.93652376742696, + -165.26846111422094, -165.60065680035294, -165.93222401150882, -166.26405206032558, + -166.5975532366255, -166.9327393757321, -167.2682466574162, -167.60322357479959, + -167.93847000443222, -168.27530835178302, -168.61378183900877, -168.95263403217712, + -169.29104925446183, -169.62974494472823, -169.9699574813401, -170.31175703012775, + -170.6539879618333, -170.99587071205337, -171.33804689251744, -171.68167070253227, + -172.02683518428148, -172.37247918025702, -172.71785930237908, -173.06354756695924, + -173.4106198177865, -173.75918826314913, -174.10828013599635, -174.45718809095524, + -174.8064204116356, -175.1569783712803, -175.50898997238295, -175.86156501042154, + -176.21403187168687, -176.56684061134436, -176.9209216656706, -177.276415779213, + -177.63250973601328, -177.9885671847553, -178.34498510912093, -178.7026267789968, + -179.0616429301755, -179.42129201479344, -179.7809723347144, -180.14103262348885, + -180.50227258176474, -180.86485046896803, -181.2280913368986, -181.59142740879287, + -181.95516366594228, -182.3200397542152, -182.6862192544363, -183.0530889992954, + -183.42011429540196, -183.78756055865932, -184.15611080378343, -184.525931978699, + -184.8964681246396, -185.2672167028437, -185.638407452449, -186.01067008275334, + -186.3841731854121, -186.75841368027824, -187.132920178221, -187.50789034493104, + -187.88390380611247, -188.26112928818085, -188.6391124973969, -189.01741212654568, + -189.39619709895, -189.7760000696095, -190.15698858912273, -190.53875329031354, + -190.92088183004392, -191.30351746122224, -191.68714886802226, -192.07194129758645, + -192.4575266759198, -192.8435204676572, -193.23004308121793, -193.617542113636, + -194.0061795490326, -194.39562519327265, -194.78552113473754, -195.17596753027587, + -195.56737365493905, -195.95989742408074, -196.35324332333818, -196.74707886293638, + -197.14148632095296, -197.53683929553736, -197.933290967728, -198.33057750888972, + -198.7283906402854, -199.12679692660544, -199.52613681329163, -199.92655820874447, + -200.32782617456402, -200.72965543146975, -201.13209880120448, -201.53546597968077, + -201.93989917925003, -202.34518974707686, -202.751074198292, -203.1575933993827, + -203.56502857939287, -203.97351593447695, -204.38287067560304, -204.7928499203278, + -205.20348419671345, -205.6150284301484, -206.0276125727257, -206.44107345232243, + -206.85518761577225, -207.26997671022124, -207.68567140275596, -208.10239525551458, + -208.5200046331361, -208.93829436247796, -209.35727851912242, -209.77716544140384, + -210.19807222793216, -210.61987285855759, -211.04237931918513, -211.4655992857971, + -211.8897205841901, -212.3148538391931, -212.74088887477996, -213.16765374694393, + -213.5951507769902, -214.02354898389035, -214.45295256340523, -214.8832655549256, + -215.31433103073036, -215.74614688524275, -216.1788649289692, -216.61258302054992, + -217.04721792048107, -217.48262670125752, -217.91880365055158, -218.3558848648318, + -218.79396199767987, -219.23296316292127, -219.67275845698202, -220.11333928225932, + -220.5548274153216, -220.99730847034053, -221.44072066552712, -221.88494618630875, + -222.32997418117233, -222.77591340446227, -223.22284362421576, -223.67071202540126, + -224.119411989995, -224.56893096190808, -225.01936587844727, -225.4707908770044, + -225.9231610756851, -226.37638020375584, -226.83043447547186, -227.2854101278763, + -227.7413759005292, -228.1982939079816, -228.65607742107326, -229.11471183206265, + -229.57427371024147, -230.0348266430826, -230.4963388949898, -230.95873251621126, + -231.42199242410942, -231.88618647266296, -232.35137335201267, -232.81752671335113, + -233.2845766674393, -233.7525079495373, -234.22138057487498, -234.69124859655113, + -235.16209036671597, -235.6338433804674, -236.1064924352654, -236.58009051246535, + -237.05468729088872, -237.53026520903265, -238.00676851209414, -238.48418226093696, + -238.9625531403663, -239.44192671749772, -239.92228896806165, -240.40359029407247, + -240.88581618288202, -241.3690076965991, -241.85320655070794, -242.33840176912216, + -242.8245493571952, -243.31163535831513, -243.79969582627467, -244.2887688805353, + -244.77884615907135, -245.26988875560264, -245.7618833697687, -246.2548616058494, + -246.74885823676817, -247.2438671305236, -247.73985399131797, -248.23680624976333, + -248.7347515676374, -249.23372161331298, -249.73371214631064, -250.2346930390105, + -250.73665250571727, -251.23961472458112, -251.74360849280094, -252.2486311641883, + -252.75465637099336, -253.2616731450968, -253.7697025952815, -254.27877087145927, + -254.78887666179244, -255.29999698245618, -255.81212170080903, -256.3252692292884, + -256.8394632842479, -257.3547036618484, -257.87097041693903, -258.3882542568401, + -258.9065712326523, -259.4259428302655, -259.94636975763615, -260.4678347920479, + -260.99032947413946, -261.5138677937398, -262.0384691984253, -262.564135138717, + -263.09085082541867, -263.6186086167544, -264.14742070931345, -264.677304693403, + -265.208262616922, -265.7402818609308, -266.27335557821664, -266.807494410878, -267.3427142618617, + -267.8790176526088, -268.4163938951752, -268.9548369081826, -269.4943559912946, + -270.0349655189502, -270.5766683811865, -271.11945560417905, -271.6633218393314, + -272.20827523166446, -272.75432877508274, -273.3014856399149, -273.84973837039325, + -274.3990823145224, -274.94952462848454, -275.50107706299923, -276.0537429949804, + -276.6075163099427, -277.16239301421695, -277.7183794210768, -278.27548616510694, + -278.8337167688488, -279.39306630014715, -279.95353138416385, -280.5151176192923, + -281.07783464110923, -281.64168606790315, -282.2066680073117, -282.77277766335567, + -283.3400200314943, -283.90840385592054, -284.47793281036434, -285.0486039147941, + -285.6204149122557, -286.19337029282093, -286.7674780078702, -287.3427417545014, + -287.91915935135097, -288.4967290413007, -289.07545489372984, -289.655344157199, + -290.2364005271319, -290.81862251976554, -291.40200883968265, -291.986563208829, + -292.57229225484906, -293.15919965241676, -293.7472845257627, -294.3365460044106, + -294.9269875259936, -295.51861517154896, -296.1114325809504, -296.70543940721166, + -297.3006351696594, -297.89702307577414, -298.49460872719874, -299.0933957191521, + -299.69338416362336, -300.29457393640496, -300.8969680610969, -301.50057172055426, + -302.10538845895877, -302.71141878594295, -303.31866290235337, -303.92712368726046, + -304.53680595921657, -305.14771320782285, -305.7598462866421, -306.3732056921633, + -306.98779419222967, -307.6036162899247, -308.2206754190174, -308.83897273011434, + -309.4585089879684, -310.0792868772328, -310.70131062915857, -311.3245836222539, + -311.94910726337764, -312.5748825602012, -313.20191213766174, -313.830199994053, + -314.45974945461137, -315.0905621470852, -315.72263929872355, -316.3559834942804, + -316.9905985336233, -317.62648769178344, -318.2636527868513, -318.9020952442645, + -319.5418176247443, -320.18282356031045, -320.8251162796443, -321.4686977648914, + -322.11356962017334, -322.7597343954331, -323.4071955818418, -324.0559563661364, + -324.7060188719847, -325.35738486448656, -326.01005689360136, -326.6640383334178, + -327.3193323334846, -327.97594113975754, -328.63386666231656, -329.29311145984894, + -329.95367881022037, -330.6155718307366, -331.2787928732944, -331.94334397856255, + -332.6092277209164, -333.2764473002516, -333.9450058066361, -334.6149056840792, + -335.286149090949, -335.9587386228065, -336.6326774175029, -337.3079685428293, -337.9846145232684, + -338.6626176233944, -339.3419804642377, -340.022706135458, -340.7047976874095, -341.3882577153029, + -342.07308857971486, -342.75929293043066, -343.4468738209348, -344.1358342888007, + -344.826176991857, -345.5179043776647, -346.2110191272336, -346.90552426826605, + -347.601422829986, -348.29871752613354, -348.99741088331945, -349.6975056155889, + -350.3990047338262, -351.1019112630828, -351.80622796750447, -352.51195744580275, + -353.21910244634364, -353.9276659709034, -354.63765104426665, -355.3490604765017, + -356.0618969323631, -356.7761631954104, -357.4918622649251, -358.2089971690505, + -358.927570760162, -359.64758576380353, -360.36904499927863, -361.09195146903556, + -361.8163082079192, -362.5421181077289, -363.2693839502654, -363.99810859088376, + -364.72829504003204, -365.4599463423245, -366.1930654267157, -366.9276551273741, + -367.6637183358344, -368.4012580746632, -369.14027740104433, -369.88077927933233, + -370.6227665927477, -371.3662422690045, -372.11120934629133, -372.8576708969081, + -373.60562991928094, -374.3550893428728, -375.1060521314913, -375.85852134192436, + -376.6125000638935, -377.3679913289216, -378.12499811035156, -378.88352340794614, + -379.64357029961894, -380.4051418945974, -381.1682412568148, -381.9328714015242, + -382.69903536427796, -383.46673624626146, -384.2359771780845, -385.0067612556417, + -385.7790915344696, -386.5529710857383, -387.32840303572766, -388.1053905381184, + -388.8839367205077, -389.66404467738937, -390.4457175153868, -391.2289583874262, + -392.0137704717747, -392.8001569276271, -393.58812088735715, -394.37766549287124, + -395.1687939249686, -395.96150938755676, -396.7558150704628, -397.55171414010636, + -398.3492097647753, -399.1483051294121, -399.94900339695914, -400.7513076097339, + -401.55522051642305, -402.3607442126537, -403.1678793129748, -403.97662313703535, + -404.78696601570533, -405.5988841676912, -406.41232657422364, -407.22719193743313, + -408.04329038092243, -408.86028352164305, -409.67759674852243, -410.49430019593217, + -411.30896127385165, -412.11948227644524, -412.9229503097521, -413.71553969665734, + -414.49251289191534, -415.2483582419702, -415.9770785993685, -416.6726079072575, + -417.329295131207, -417.94237131855783, -418.5083166266741, -419.02506982614034, + -419.4920624207183, -419.91009801422723, -420.28112301456906, -420.60794297702034, + -420.89393277454684] +x: [-131.00261478679988, -121.88886417422353, -111.9050389833139, -102.61368963497223, + -96.05947080078046, -94.12535169815399, -97.8607435820271, -107.14613364957096, + -120.82659085972966, -137.19720078410953, -154.5163549178483, -171.18694825080595, + -185.5568218223094, -195.704691059387, -199.6329868378576, -195.91936207338776, + -184.48915098542318, -167.07793507920195, -147.0574840422674, -128.53128958566316, + -115.04358889413827, -108.56400855374055, -109.19561099927448, -115.57413755530115, + -125.64926844955686, -137.46655539270932, -149.61816709281686, -161.26501144566066, + -171.91636446335687, -181.2216960780747, -188.87007846616464, -194.53289513944736, + -197.79145167358277, -198.0996571089822, -194.8802396222356, -187.78401698176944, + -177.02826515891735, -163.64756841648978, -149.46450567572836, -136.67890898964777, + -127.21395723237792, -122.15440349976664, -121.54367242044958, -124.56838026444774, + -129.9847428341204, -136.57776680869415, -143.4619977851926, -150.1463172881614, + -156.43047062938794, -162.25579819785062, -167.580799896415, -172.2857363783464, + -176.10313080152142, -178.60602094331426, -179.29466139771722, -177.77366590562514, + -173.94835807935246, -168.14763364573156, -161.10576879062413, -153.79437054295704, + -147.18005615509452, -142.03544698559352, -138.8812834694729, -138.0254327793118, + -139.60154077046317, -143.53795283987066, -149.46190564030064, -156.61307764750555, + -163.87292592523232, -169.97827533844975, -173.87402597845175, -175.0512378675456, + -173.7109894894428, -170.68112972530366, -167.11714893622806, -164.10386427943158, + -162.31622495964814, -161.86114761154576, -162.33151047253835, -163.02192881838113, + -163.2095501914243, -162.39248606696069, -160.40951035619491, -157.42206476020834, + -153.79159590063577, -149.91799716532543, -146.1123968434442, -142.55104990302257, + -139.31149673683075, -136.46131741930625, -134.16539879470983, -132.7805579230332, + -132.8976720404706, -135.27842864395015, -140.64645739243105, -149.3508159834204, + -161.00381971456645, -174.2605798886825, -186.9134811438676, -196.38261868432437, + -200.49211090980378, -198.23763830134754, -190.21480773205266, -178.51115567595826, + -166.0801280692791, -155.83164235826004, -149.80028431814193, -148.6986554765198, + -151.96202945833986, -158.18027510739103, -165.68937034330935, -173.07927043758525, + -179.46192687105753, -184.482441275634, -188.1614046703995, -190.6833573748501, + -192.21809683888495, -192.81960540582097, -192.41403431825069, -190.86577715074623, + -188.09301880254696, -184.18878513004086, -179.4955981906823, -174.5892113462465, + -170.1561255168205, -166.79776163996192, -164.8373602238494, -164.2126907946925, + -164.50294621069565, -165.0839384166419, -165.3557930296701, -164.95884643563477, + -163.898412256587, -162.534820649653, -161.44472376807636, -161.20582442689792, + -162.18450627688458, -164.40124437891336, -167.51392693816024, -170.91206169710102, + -173.8749652264376, -175.73083709030058, -175.97063351490164, -174.31004528380663, + -170.72511032331406, -165.48897950595216, -159.20946482285498, -152.82808750163824, + -147.5191667738631, -144.44886967752777, -144.4240273588793, -147.54449659368146, + -153.01617480666846, -159.25176211818498, -164.2877168274527, -166.41140144219656, + -164.78014816559207, -159.78674905727087, -153.00534915065177, -146.70549554069234, + -143.09178186665255, -143.5446658872191, -148.14637102277132, -155.6663204124734, + -164.00416538606794, -170.916565992789, -174.75161613413627, -174.9259359923934, + -171.99663265443107, -167.34336788125873, -162.6147423868518, -159.16048671989986, + -157.6477458947188, -157.96425018036857, -159.39300876091175, -160.95217491299363, + -161.7572333232152, -161.28442609094157, -159.4763125555903, -156.69932273396668, + -153.6090737832136, -150.9888364227075, -149.60413892450302, -150.0809012374839, + -152.79153608790728, -157.7397093479904, -164.46467355593433, -172.0172934256874, + -179.0661158741304, -184.15783677719142, -186.08855140295393, -184.27238379689192, + -178.96525805487465, -171.2372865037682, -162.67650112956372, -154.91277319136154, + -149.12741792008617, -145.72381647659327, -144.27134285886908, -143.72945580899287, + -142.85723879551645, -140.65447875831518, -136.68276519662683, -131.17312406130938, + -124.91209567953047, -118.9751543144055, -114.4168612960534, -112.01955243552163, + -112.15689719095518, -114.77332263747567, -119.44360198084003, -125.471236872446, + -132.00253791160844, -138.1561515203214, -143.17484043177828, -146.58907912429945, + -148.3511438380617, -148.87742279194816, -148.94591893398302, -149.43982973654823, + -150.99302147422435, -153.65215284576814, -156.69307614107936, -158.6972973355033, + -157.91096745292066, -152.8004175957856, -142.62726270976202, -127.83568739093043, + -110.09574251961111, -91.96442744166842, -76.26517339306562, -65.38756088681923, + -60.727455422828086, -62.41769838627875, -69.38103902919622, -79.63028303419432, + -90.6904492501862, -100.0330543381811, -105.46650890222921, -105.47370228165889, + -99.49440285791094, -88.11732926351014, -73.10854974665124, -57.20048636703596, + -43.619919989560685, -35.42935377805215, -34.84789242860901, -42.75227348465364, + -58.50946410168745, -80.17821602052324, -104.99065766337972, -129.94273353227604, + -152.31614487459336, -170.01709776381944, -181.70800379508364, -186.78019457154642, + -185.24347046638363, -177.59665180543396, -164.71393092910603, -147.75511927137686, + -128.09259386421706, -107.2415258381779, -86.77724029789665, -68.22375027449615, + -52.90610923413581, -41.779927174503385, -35.27752870209294, -33.22579478747385, + -34.88146090859914, -39.094144062824924, -44.55933540558859, -50.084953618202576, + -54.78496998250598, -58.13877584276208, -59.90622409641475, -59.945540069741, -58.02266713890794, + -53.710211558038985, -46.44646056518766, -35.76628477961155, -21.64346099537084, + -4.823099943406338, 13.00074336490228, 29.276996809341675, 40.992025758372115, 45.29133769691425, + 40.16014663983825, 24.958013623469462, 0.6430403690297408, -30.37648983818459, -64.70364605521273, + -98.5780766753714, -128.49010205980096, -151.64916889145232, -166.24865624155242, + -171.54102444522545, -167.77666186080413, -156.06265128020192, -138.17902607586348, + -116.36917642692488, -93.10932192371236, -70.85982959536072, -51.803566443571675, + -37.58102962803787, -29.04191085373232, -26.05140387409622, -27.411942444947794, + -30.971298702656785, -33.967139701487234, -33.59780771242022, -27.721946232536823, + -15.50944865087424, 2.165242029053751, 22.75109265450623, 42.51462881068035, 57.39178500711525, + 63.9603626762866, 60.24680348570627, 46.13482203280333, 23.280449691574223, -5.396531697017779, + -36.44546991778746, -66.59572190185905, -93.244393312115, -114.63002544381644, -129.7277803714285, + -137.99626292368302, -139.13919014515983, -133.01230035499455, -119.72497935278233, + -99.88864280213238, -74.88535670756845, -46.99987777401981, -19.290622507018494, + 4.836900082983619, 22.275753850864515, 30.9051736945987, 30.102091149122813, 20.89767984126534, + 5.709426552642763, -12.280539670085691, -29.912602539696113, -44.76409817954616, + -55.587777836517255, -62.37400184312159, -66.06509298635545, -68.05618356119531, + -69.6650415443935, -71.73438308006683, -74.46044693767506, -77.45330286152975, -79.96093287153582, + -81.15272296993173, -80.36351741010003, -77.23600916293604, -71.74756168638527, + -64.14841195780141, -54.85947253693819, -44.377179252428895, -33.21572713174938, + -21.893453397774184, -10.949679527408357, -0.9673128756345823, 7.423219742995925, + 13.573678927669173, 16.882316200266693, 16.880518451590333, 13.321463895553073, + 6.2494544216467425, -3.9662652022608986, -16.64105804227179, -30.827458079441, -45.40427423165941, + -59.19176164715269, -71.07926973763034, -80.1486617898492, -85.77441240031021, -87.6818651318595, + -85.95088814283162, -80.96333944498441, -73.30666637565525, -63.65782924774226, + -52.676828368491805, -40.93506347992221, -28.891766117399765, -16.916080004322772, + -5.3384835550584935, 5.4924058263232025, 15.171162899221802, 23.231051427618734, + 29.178380505050853, 32.559117153019926, 33.042995254450894, 30.506717141912656, + 25.098309221713915, 17.267877434957573, 7.754667766136128, -2.4738852870074073, + -12.328160893560188, -20.73549791063537, -26.79110541102302, -29.892155231726377, + -29.825103487837175, -26.78324113785426, -21.306403690118064, -14.15394397582623, + -6.139545262622449, 2.0335265366227753, 9.89870331065091, 17.286397543623664, 24.307308641748374, + 31.26525601401877, 38.52026696289829, 46.33161129748563, 54.71416332554038, 63.34061174435135, + 71.51679336934421, 78.24704133338132, 82.39018658725793, 82.88576123321361, 79.00811170033668, + 70.59040844420252, 58.15831519340492, 42.928737491281964, 26.660959489767816, 11.387156505595188, + -0.9161323493847441, -8.636955778598946, -10.746100985455664, -6.88302166146115, + 2.6901550272644528, 17.227609362490647, 35.67099777743684, 56.793973316018906, 79.28492204264502, + 101.76799145249652, 122.79490061004348, 140.8566568494178, 154.45830822835748, 162.2730575641024, + 163.35441171879694, 157.35115570417733, 144.6537822686842, 126.41052162787045, 104.38486553091724, + 80.67370587471055, 57.34955312756698, 36.11589971154659, 18.06234868471712, 3.576456180471944, + -7.5771785605298545, -16.047264197592554, -22.625071287068153, -27.99125205987563, + -32.544187050649704, -36.342770320132814, -39.160889188810124, -40.621845381417664, + -40.36566724147911, -38.20206823583871, -34.21329090178294, -28.788359919239177, + -22.587615544563256, -16.449965238654954, -11.263594052219574, -7.82420704968046, + -6.7043106382969215, -8.153633537426817, -12.045315570129013, -17.87558753135798, + -24.817072522162295, -31.818486356751524, -37.73735081273553, -41.48808157884077, + -42.185843095170476, -39.266966337888135, -32.56948414179687, -22.362390213218752, + -9.319317728622229, 5.559271102094557, 21.062973392548795, 35.90860732266084, 48.87612268299202, + 58.92359238196129, 65.26758898268541, 67.42353430763573, 65.20869447959286, 58.716140928984274, + 48.27012084952794, 34.372265068707556, 17.645386644363096, -1.221017147956034, -21.52101235133283, + -42.57613526226664, -63.77089174961588, -84.5781075128901, -104.57331194568057, + -123.43864361815416, -140.9579908724867, -157.005848574077] +y: [-99.19714419940159, -79.16394233266908, -57.87218080909418, -39.28438472983111, + -28.386800564847263, -29.413961436687575, -43.913473953576144, -69.67422837210728, + -101.14133069492664, -131.32148548272554, -154.36897298960562, -167.56000537729983, + -171.8480811942197, -171.05140505400772, -170.15303380273187, -173.34724789149277, + -182.51652550260764, -196.6077612179733, -212.0578803921079, -224.1551729270074, + -228.85174829123696, -224.25993586587964, -211.25206790585622, -193.0143316349947, + -173.74730833399678, -157.0391212367104, -144.63181352345478, -136.04353714588737, + -129.03535351100325, -120.64812612176986, -108.4578159331508, -91.66712058998779, + -71.6845532860305, -51.92454636690193, -36.75664559094005, -29.944229192958293, + -33.26253995101549, -45.8841436370077, -64.72238023555612, -85.55581883977085, -104.43639262659643, + -118.73440682883057, -127.4439469965765, -130.85855117578427, -129.9883150575577, + -126.05068565061471, -120.20491993068747, -113.51752511264922, -107.00751318510588, + -101.6120221067554, -98.037143648059, -96.5901783209796, -97.11117132586354, -99.05464430683851, + -101.69784273360709, -104.39663493912799, -106.78682465771945, -108.86073554241558, + -110.91272059008197, -113.391986702604, -116.71712944928908, -121.10899616745179, + -126.49101079170191, -132.48890828569083, -138.52907052668536, -143.98403266052748, + -148.28386085159676, -150.950353874693, -151.59029328770967, -149.92372750552684, + -145.8893613284024, -139.79752247093995, -132.44040508370935, -125.04855412827735, + -119.03041856644234, -115.55253894189944, -115.13281650745513, -117.42895405278668, + -121.3137432440983, -125.21048574002899, -127.55581585488525, -127.2035779940535, + -123.63075578035654, -116.92546848468061, -107.64003980195744, -96.63277766107122, + -84.99305975953892, -74.04821594432073, -65.34295582835624, -60.46526291514498, + -60.70407650618115, -66.66922658810824, -78.05651925687684, -93.67617143035707, + -111.7360977518272, -130.24058283704147, -147.30871275663227, -161.30727700805863, + -170.86975350241954, -174.96772570963563, -173.1273631811409, -165.72224178554154, + -154.15911230206518, -140.7708520660487, -128.34356994048002, -119.39667357114564, + -115.49538835769575, -116.87807764072566, -122.52767636389038, -130.62314678287214, + -139.1652566178275, -146.52616783065477, -151.74687442934803, -154.55565073083676, + -155.2031643789819, -154.2459690762812, -152.3741747997027, -150.31243759575744, + -148.7646263574894, -148.35253400075652, -149.52454396025652, -152.4546938221956, + -156.97962048361924, -162.61455065772643, -168.65639907954113, -174.33920996159839, + -178.98123335741235, -182.07734590854741, -183.33592461504546, -182.69582244314955, + -180.3576867141039, -176.82992166677565, -172.94908163552213, -169.81480317042735, + -168.6009806288589, -170.26752614676, -175.26636333032138, -183.36316952799345, + -193.6626336687613, -204.8455532302751, -215.5341659113945, -224.642821518713, -231.5841029386231, + -236.27823723741093, -239.00153643062308, -240.1602207969857, -240.0805222435039, + -238.8792739976468, -236.4402664233786, -232.4857796328019, -226.70748164073063, + -218.90832171524943, -209.10849307704876, -197.58443529231656, -184.83728952594203, + -171.51692947117536, -158.34183621150794, -146.03906486693822, -135.2931551378401, + -126.67274641338989, -120.522356840103, -116.8495422307382, -115.26711775119627, + -115.04262249541723, -115.26672709456207, -115.09929773879223, -114.01174175462747, + -111.938154926336, -109.27967994662575, -106.7610444266074, -105.19243755344766, + -105.22191192914899, -107.15903178753426, -110.91294427768851, -116.03909562366141, + -121.85319340040662, -127.56333385695282, -132.3915434702031, -135.68773579313347, + -137.0553530223522, -136.49152106606573, -134.50396087391331, -132.13096362061216, + -130.78914260327244, -131.92152103045797, -136.50685861312238, -144.58292188907654, + -154.9805743147388, -165.42722647501083, -173.05862816904528, -175.2162591822015, + -170.27334476780416, -158.19760009121686, -140.64912083959248, -120.58743118943534, + -101.54354695989151, -86.82037717684916, -78.86963873202836, -78.9730241819731, + -87.208732905478, -102.59534157990022, -123.30579810686955, -146.90854660058451, + -170.66096479222475, -191.89527725965974, -208.48091570828308, -219.25913002134993, + -224.29508133070183, -224.82455615157096, -222.87658961783313, -220.67901822911378, + -220.0419990907876, -221.92371042747453, -226.30932003382122, -232.4142180953472, + -239.1075434889348, -245.3854703853822, -250.72594095429886, -255.21957879524751, + -259.4638629985364, -264.29101495969917, -270.44645552037593, -278.33363978175873, + -287.8997813229622, -298.675957930859, -309.9292117127586, -320.853465714804, -330.7289512679556, + -339.01001631839443, -345.3404207556757, -349.52468900709295, -351.4950882757702, + -351.30843956420114, -349.19099656641225, -345.6266700588403, -341.4559943792539, + -337.9255863077082, -336.6122458269782, -339.1590394133497, -346.81553649131257, + -359.86602049319964, -377.1262582942168, -395.74022625809545, -411.4675647034107, + -419.50864481032374, -415.70559925146244, -397.7689969926942, -366.1070646451192, + -323.9312171058613, -276.5534397525774, -230.0823638333837, -189.94210230887091, + -159.6860391307984, -140.44018425867142, -131.05960164254728, -128.83405351298265, + -130.4331021797765, -132.77378964958245, -133.598664448043, -131.70104308630897, + -126.85857197498154, -119.59605542060795, -110.89414841971599, -101.91886722614183, + -93.80124103359078, -87.46894981676736, -83.52587581326873, -82.18044665496083, + -83.22666982990846, -86.0776105317449, -89.84325002250512, -93.43992023523293, -95.72026823608788, + -95.61789886265437, -92.30211434757715, -85.33026401582156, -74.77072267985237, + -61.2585993757004, -45.950045468071075, -30.363497680157124, -16.130369251753944, + -4.708318964022113, 2.8777155744834513, 6.212983448101315, 5.56140196148832, 1.82269284521927, + -3.6117451657027027, -9.093677781503999, -13.032649828418581, -14.265052388454068, + -12.419628464737789, -8.183075798478319, -3.351394348965721, -0.5798384940055397, + -2.8238203221278537, -12.574618457960362, -31.09374357688851, -57.88946050977006, + -90.62907055552884, -125.54979971296869, -158.26608815757967, -184.7370925829678, + -202.10993695070283, -209.20930494571166, -206.57443001611037, -196.09246662518265, + -180.3847681302668, -162.1383131743949, -143.5430153470272, -125.92848416505213, + -109.62850194687415, -94.06091918539822, -77.99447435013359, -59.96518435506764, + -38.78528395904798, -14.052481549971988, 13.469116037352999, 41.73558027808582, + 67.67668776494482, 87.82105256949613, 99.17127607438123, 100.09686637149439, 90.96891255317551, + 74.31360110106945, 54.401903491602475, 36.37112195454357, 25.122323308838894, 24.299071767105474, + 35.60733796084545, 58.60925251977363, 90.97083953417878, 129.02527666162507, 168.46442192730262, + 204.9931440177265, 234.84578165082002, 255.13458309044964, 264.04869333102295, 260.9405833712635, + 246.33164088648044, 221.85233074958896, 190.1149237954721, 154.50438006044362, 118.87017882929412, + 87.1135432104481, 62.69333693214143, 48.115270426306004, 44.50773230112809, 51.40175250111006, + 66.8050512534824, 87.59010715789734, 110.12403882647259, 130.98843565029367, 147.60355998106695, + 158.5997478392373, 163.85990646713972, 164.25977605801083, 161.2187754414792, 156.2146660131347, + 150.40095323538046, 144.40924932571875, 138.34575838676523, 131.9293358853309, 124.68710400298332, + 116.12724252945577, 105.83914725773546, 93.51279283344225, 78.90515326791086, 61.80006387437529, + 42.00492201327354, 19.40692541387674, -5.917558252890152, -33.573230874271864, -62.74274528444666, + -92.15503567861164, -120.15265932612857, -144.85998708249542, -164.4286388593254, + -177.31873975901365, -182.56747411912127, -179.99849715708353, -170.33317799853793, + -155.174775508723, -136.8502267694815, -118.11425365797368, -101.74838641351045, + -90.11915445215804, -84.78453470268744, -86.24244721296932, -93.89058890376597, + -106.21495538177916, -121.15951154482048, -136.5743658936049, -150.61621971325263, + -161.99385323020581, -170.00781819684602, -174.40729806224658, -175.15102714454724, + -172.18938784282471, -165.3700625780896, -154.51544144123787, -139.64662444269274, + -121.26379807895945, -100.56065686649099, -79.46407820455939, -60.445349586067294, + -46.126337444272174, -38.77528077288329, -39.82686472674805, -49.55634236027296, + -66.99108245478719, -90.07326313903191, -116.01965740549392, -141.78042183347972, + -164.48915325804674, -181.81824580195223, -192.1936050384186, -194.86464874708207, + -189.85637149914302, -177.8444701746055, -159.9936426666511, -137.78899220364173, + -112.8776101254829, -86.92681328173411, -61.4995443419655, -37.946187885812385, + -17.314095829999818, -0.2793256793802397, 12.892666001859144, 22.35087361058214, + 28.623738724825635, 32.547011711050814, 35.16166362154166, 37.59316158505565, 40.9252515429786, + 46.081001294230944, 53.72209705873961, 64.17527284982758, 77.39288792317001, 92.95301794423807, + 110.10246016313017, 127.84307430057432, 145.05732762285365, 160.66258701121782, + 173.77600795348275, 183.86412276724406, 190.84585708339466, 195.11799782738245, + 197.48122046939216, 198.9638867275426, 200.56755651863108, 202.98585640947218, 206.36735753017484, + 210.1940659607301, 213.32513104699035, 214.21331928977128, 211.25089340990704, 203.15829367329167, + 189.30944456733187, 169.9006771128133, 145.91404235801855, 118.88681324803126, 90.55706188617755, + 62.490765591022935, 35.79722544105233, 11.007111877948589, -11.866681217908095, + -33.12202103984155, -53.09189944897362, -71.89093420571022, -89.25892597900295, + -104.53982239765106, -116.79113539836814, -124.98292837106044, -128.22643422109374, + -125.9727544559038, -118.13810580734831, -105.13616058766276, -87.82215430794395, + -67.3716974347737, -45.12687495280552, -22.4432811717548, -0.5658122278379778, 19.44939304727038, + 36.762171246899385, 50.732584441996885, 60.87993008610584, 66.83886074810738, 68.3326809251376, + 65.17495028403165, 57.29773071625812, 44.792950118379224, 27.94724280533802, 7.252695883249127, + -16.615157231293985, -42.849357139418395, -70.58183967568638, -98.95676356556059, + -127.19340520577649, -154.63036780787843, -180.74870760607394] +yaw: [-156.65718192265612, -156.82512813476856, -157.03476785128979, -157.2807477120766, + -157.55369295460645, -157.84461759860156, -158.14778763059294, -158.459737944698, + -158.77686003690096, -159.09532479965736, -159.41325989374954, -159.73163715474024, + -160.0521375845057, -160.3745530176387, -160.69700732848457, -161.0185104942475, + -161.3402653767862, -161.66397674111627, -161.98953153448417, -162.315201866925, + -162.64003295077077, -162.96511555690367, -163.29205326830424, -163.62078022025358, + -163.9496971026068, -164.27788257902984, -164.60632279261657, -164.93652376742696, + -165.26846111422094, -165.60065680035294, -165.93222401150882, -166.26405206032558, + -166.5975532366255, -166.9327393757321, -167.2682466574162, -167.60322357479959, + -167.93847000443222, -168.27530835178302, -168.61378183900877, -168.95263403217712, + -169.29104925446183, -169.62974494472823, -169.9699574813401, -170.31175703012775, + -170.6539879618333, -170.99587071205337, -171.33804689251744, -171.68167070253227, + -172.02683518428148, -172.37247918025702, -172.71785930237908, -173.06354756695924, + -173.4106198177865, -173.75918826314913, -174.10828013599635, -174.45718809095524, + -174.8064204116356, -175.1569783712803, -175.50898997238295, -175.86156501042154, + -176.21403187168687, -176.56684061134436, -176.9209216656706, -177.276415779213, + -177.63250973601328, -177.9885671847553, -178.34498510912093, -178.7026267789968, + -179.0616429301755, -179.42129201479344, -179.7809723347144, -180.14103262348885, + -180.50227258176474, -180.86485046896803, -181.2280913368986, -181.59142740879287, + -181.95516366594228, -182.3200397542152, -182.6862192544363, -183.0530889992954, + -183.42011429540196, -183.78756055865932, -184.15611080378343, -184.525931978699, + -184.8964681246396, -185.2672167028437, -185.638407452449, -186.01067008275334, + -186.3841731854121, -186.75841368027824, -187.132920178221, -187.50789034493104, + -187.88390380611247, -188.26112928818085, -188.6391124973969, -189.01741212654568, + -189.39619709895, -189.7760000696095, -190.15698858912273, -190.53875329031354, + -190.92088183004392, -191.30351746122224, -191.68714886802226, -192.07194129758645, + -192.4575266759198, -192.8435204676572, -193.23004308121793, -193.617542113636, + -194.0061795490326, -194.39562519327265, -194.78552113473754, -195.17596753027587, + -195.56737365493905, -195.95989742408074, -196.35324332333818, -196.74707886293638, + -197.14148632095296, -197.53683929553736, -197.933290967728, -198.33057750888972, + -198.7283906402854, -199.12679692660544, -199.52613681329163, -199.92655820874447, + -200.32782617456402, -200.72965543146975, -201.13209880120448, -201.53546597968077, + -201.93989917925003, -202.34518974707686, -202.751074198292, -203.1575933993827, + -203.56502857939287, -203.97351593447695, -204.38287067560304, -204.7928499203278, + -205.20348419671345, -205.6150284301484, -206.0276125727257, -206.44107345232243, + -206.85518761577225, -207.26997671022124, -207.68567140275596, -208.10239525551458, + -208.5200046331361, -208.93829436247796, -209.35727851912242, -209.77716544140384, + -210.19807222793216, -210.61987285855759, -211.04237931918513, -211.4655992857971, + -211.8897205841901, -212.3148538391931, -212.74088887477996, -213.16765374694393, + -213.5951507769902, -214.02354898389035, -214.45295256340523, -214.8832655549256, + -215.31433103073036, -215.74614688524275, -216.1788649289692, -216.61258302054992, + -217.04721792048107, -217.48262670125752, -217.91880365055158, -218.3558848648318, + -218.79396199767987, -219.23296316292127, -219.67275845698202, -220.11333928225932, + -220.5548274153216, -220.99730847034053, -221.44072066552712, -221.88494618630875, + -222.32997418117233, -222.77591340446227, -223.22284362421576, -223.67071202540126, + -224.119411989995, -224.56893096190808, -225.01936587844727, -225.4707908770044, + -225.9231610756851, -226.37638020375584, -226.83043447547186, -227.2854101278763, + -227.7413759005292, -228.1982939079816, -228.65607742107326, -229.11471183206265, + -229.57427371024147, -230.0348266430826, -230.4963388949898, -230.95873251621126, + -231.42199242410942, -231.88618647266296, -232.35137335201267, -232.81752671335113, + -233.2845766674393, -233.7525079495373, -234.22138057487498, -234.69124859655113, + -235.16209036671597, -235.6338433804674, -236.1064924352654, -236.58009051246535, + -237.05468729088872, -237.53026520903265, -238.00676851209414, -238.48418226093696, + -238.9625531403663, -239.44192671749772, -239.92228896806165, -240.40359029407247, + -240.88581618288202, -241.3690076965991, -241.85320655070794, -242.33840176912216, + -242.8245493571952, -243.31163535831513, -243.79969582627467, -244.2887688805353, + -244.77884615907135, -245.26988875560264, -245.7618833697687, -246.2548616058494, + -246.74885823676817, -247.2438671305236, -247.73985399131797, -248.23680624976333, + -248.7347515676374, -249.23372161331298, -249.73371214631064, -250.2346930390105, + -250.73665250571727, -251.23961472458112, -251.74360849280094, -252.2486311641883, + -252.75465637099336, -253.2616731450968, -253.7697025952815, -254.27877087145927, + -254.78887666179244, -255.29999698245618, -255.81212170080903, -256.3252692292884, + -256.8394632842479, -257.3547036618484, -257.87097041693903, -258.3882542568401, + -258.9065712326523, -259.4259428302655, -259.94636975763615, -260.4678347920479, + -260.99032947413946, -261.5138677937398, -262.0384691984253, -262.564135138717, + -263.09085082541867, -263.6186086167544, -264.14742070931345, -264.677304693403, + -265.208262616922, -265.7402818609308, -266.27335557821664, -266.807494410878, -267.3427142618617, + -267.8790176526088, -268.4163938951752, -268.9548369081826, -269.4943559912946, + -270.0349655189502, -270.5766683811865, -271.11945560417905, -271.6633218393314, + -272.20827523166446, -272.75432877508274, -273.3014856399149, -273.84973837039325, + -274.3990823145224, -274.94952462848454, -275.50107706299923, -276.0537429949804, + -276.6075163099427, -277.16239301421695, -277.7183794210768, -278.27548616510694, + -278.8337167688488, -279.39306630014715, -279.95353138416385, -280.5151176192923, + -281.07783464110923, -281.64168606790315, -282.2066680073117, -282.77277766335567, + -283.3400200314943, -283.90840385592054, -284.47793281036434, -285.0486039147941, + -285.6204149122557, -286.19337029282093, -286.7674780078702, -287.3427417545014, + -287.91915935135097, -288.4967290413007, -289.07545489372984, -289.655344157199, + -290.2364005271319, -290.81862251976554, -291.40200883968265, -291.986563208829, + -292.57229225484906, -293.15919965241676, -293.7472845257627, -294.3365460044106, + -294.9269875259936, -295.51861517154896, -296.1114325809504, -296.70543940721166, + -297.3006351696594, -297.89702307577414, -298.49460872719874, -299.0933957191521, + -299.69338416362336, -300.29457393640496, -300.8969680610969, -301.50057172055426, + -302.10538845895877, -302.71141878594295, -303.31866290235337, -303.92712368726046, + -304.53680595921657, -305.14771320782285, -305.7598462866421, -306.3732056921633, + -306.98779419222967, -307.6036162899247, -308.2206754190174, -308.83897273011434, + -309.4585089879684, -310.0792868772328, -310.70131062915857, -311.3245836222539, + -311.94910726337764, -312.5748825602012, -313.20191213766174, -313.830199994053, + -314.45974945461137, -315.0905621470852, -315.72263929872355, -316.3559834942804, + -316.9905985336233, -317.62648769178344, -318.2636527868513, -318.9020952442645, + -319.5418176247443, -320.18282356031045, -320.8251162796443, -321.4686977648914, + -322.11356962017334, -322.7597343954331, -323.4071955818418, -324.0559563661364, + -324.7060188719847, -325.35738486448656, -326.01005689360136, -326.6640383334178, + -327.3193323334846, -327.97594113975754, -328.63386666231656, -329.29311145984894, + -329.95367881022037, -330.6155718307366, -331.2787928732944, -331.94334397856255, + -332.6092277209164, -333.2764473002516, -333.9450058066361, -334.6149056840792, + -335.286149090949, -335.9587386228065, -336.6326774175029, -337.3079685428293, -337.9846145232684, + -338.6626176233944, -339.3419804642377, -340.022706135458, -340.7047976874095, -341.3882577153029, + -342.07308857971486, -342.75929293043066, -343.4468738209348, -344.1358342888007, + -344.826176991857, -345.5179043776647, -346.2110191272336, -346.90552426826605, + -347.601422829986, -348.29871752613354, -348.99741088331945, -349.6975056155889, + -350.3990047338262, -351.1019112630828, -351.80622796750447, -352.51195744580275, + -353.21910244634364, -353.9276659709034, -354.63765104426665, -355.3490604765017, + -356.0618969323631, -356.7761631954104, -357.4918622649251, -358.2089971690505, + -358.927570760162, -359.64758576380353, -360.36904499927863, -361.09195146903556, + -361.8163082079192, -362.5421181077289, -363.2693839502654, -363.99810859088376, + -364.72829504003204, -365.4599463423245, -366.1930654267157, -366.9276551273741, + -367.6637183358344, -368.4012580746632, -369.14027740104433, -369.88077927933233, + -370.6227665927477, -371.3662422690045, -372.11120934629133, -372.8576708969081, + -373.60562991928094, -374.3550893428728, -375.1060521314913, -375.85852134192436, + -376.6125000638935, -377.3679913289216, -378.12499811035156, -378.88352340794614, + -379.64357029961894, -380.4051418945974, -381.1682412568148, -381.9328714015242, + -382.69903536427796, -383.46673624626146, -384.2359771780845, -385.0067612556417, + -385.7790915344696, -386.5529710857383, -387.32840303572766, -388.1053905381184, + -388.8839367205077, -389.66404467738937, -390.4457175153868, -391.2289583874262, + -392.0137704717747, -392.8001569276271, -393.58812088735715, -394.37766549287124, + -395.1687939249686, -395.96150938755676, -396.7558150704628, -397.55171414010636, + -398.3492097647753, -399.1483051294121, -399.94900339695914, -400.7513076097339, + -401.55522051642305, -402.3607442126537, -403.1678793129748, -403.97662313703535, + -404.78696601570533, -405.5988841676912, -406.41232657422364, -407.22719193743313, + -408.04329038092243, -408.86028352164305, -409.67759674852243, -410.49430019593217, + -411.30896127385165, -412.11948227644524, -412.9229503097521, -413.71553969665734, + -414.49251289191534, -415.2483582419702, -415.9770785993685, -416.6726079072575, + -417.329295131207, -417.94237131855783, -418.5083166266741, -419.02506982614034, + -419.4920624207183, -419.91009801422723, -420.28112301456906, -420.60794297702034, + -420.89393277454684] +z: [8.660695913847082, 66.39862553863676, 127.55801351244567, 180.5416567670594, 210.78937741627567, + 205.98510123498664, 161.75492603580668, 84.86410600259634, -7.938204250916348, -95.46180594158663, + -159.6160710741801, -191.12823315769305, -191.1077383247354, -168.75160790760808, + -136.7976917935674, -106.70965336063549, -85.49283736476575, -74.97102632230975, + -72.98493565768426, -75.44155069558714, -78.2857794410305, -78.76734664689184, -75.79970900381551, + -69.60926545532763, -61.02352026219444, -50.731204460839145, -38.784872157834826, + -24.48063415135472, -6.604904189438815, 16.003062940241094, 43.61116166930451, 74.59937262542272, + 104.69471826704422, 127.10591770220823, 134.08234145821157, 119.62663119956167, + 82.25187144352125, 26.508018246556365, -37.60809657045731, -97.50294343727298, -142.05752447785434, + -165.01189926649766, -166.22428468257075, -150.6580066881632, -125.88440608451583, + -99.27427444087671, -76.01596349902346, -58.51176921704588, -46.89688221845846, + -40.0319509439571, -36.428785825300736, -34.84658879993211, -34.526729879516196, + -35.16461507569286, -36.74598621919215, -39.344955846661044, -42.94609674951552, + -47.33366981479339, -52.08037130315903, -56.64311306447958, -60.522268045019175, + -63.40062822000451, -65.19777275447987, -66.03688501834353, -66.16552673973, -65.87798906520248, + -65.46886057404075, -65.21885341822762, -65.38814742633711, -66.18701058321993, + -67.71310218370519, -69.87490985875417, -72.34304758340024, -74.57596554892733, + -75.9445158717209, -75.92859524663281, -74.30883578638903, -71.26712435663396, -67.34312951723155, + -63.24836193520065, -59.60140045704585, -56.690265423268045, -54.35501112675093, + -52.025856935869335, -48.89272594930154, -44.14172332474001, -37.177555708112656, + -27.766047180552132, -16.071976274482385, -2.617381566487062, 11.77309724198873, + 25.976102073264208, 38.53687298107297, 47.718551492321396, 51.76605423223608, 49.38878979505458, + 40.290429255399275, 25.491354807794263, 7.246430327597227, -11.475853168764028, + -27.772349573580907, -39.515015426681174, -45.713572144488516, -46.46431428981413, + -42.61798316112623, -35.37703866753643, -25.9934677516497, -15.628487382325641, + -5.320192334714672, 4.040136476501501, 11.774699698468378, 17.500102100664304, 21.15959975218581, + 22.97185786614963, 23.31019232970664, 22.558042956454567, 20.99209210295209, 18.729017300170803, + 15.74651164213902, 11.966823265504356, 7.376265857048469, 2.1431266936820315, -3.3126251807763514, + -8.343814311527554, -12.216153231932507, -14.333645421496714, -14.465721023384898, + -12.87582842967554, -10.283561071346963, -7.656458262095206, -5.898235147224569, + -5.548464248913495, -6.607585778672925, -8.55034572367405, -10.517780185318616, + -11.61186892570347, -11.181289206074855, -8.998149061838516, -5.278041737321532, + -0.5603284740797135, 4.485651709130345, 9.246488587633161, 13.28299530174339, 16.36756662020893, + 18.466571978647558, 19.704100343285322, 20.32815531831131, 20.677451138832144, 21.13258932476601, + 22.03831545911599, 23.60334269570648, 25.810407707445663, 28.386041772609907, 30.87468716575818, + 32.83047926730481, 34.087238764628125, 35.01230737770861, 36.621360874602765, 40.45070511179779, + 48.15601919282285, 60.917623297793895, 78.84123893360714, 100.59365670804146, 123.46645783546617, + 143.91789347099578, 158.44750364654655, 164.50260889528977, 161.097954064323, 148.96575759387107, + 130.25873364860564, 107.99215885210386, 85.46056101968273, 65.78797889606493, 51.62771188476158, + 44.92502099696882, 46.66619035735277, 56.63547648764704, 73.29795847361908, 93.94368722527568, + 115.14660187881398, 133.44963208761388, 146.06803902842015, 151.38700573272683, + 149.12923984177337, 140.2151559819424, 126.44367536795221, 110.13915686176617, 93.84830850562516, + 80.0820870481205, 71.04848324787652, 68.34789291609472, 72.6765134206983, 83.63977979446459, + 99.77023650435606, 118.77456269522624, 137.94387073801647, 154.60399035259434, 166.49431469790014, + 172.0327438962138, 170.4928109096748, 162.13218328502208, 148.26316136662118, 131.1890050423139, + 113.90461235170372, 99.51589491572973, 90.46166485727544, 87.7622777984477, 90.58353615476729, + 96.33891906881483, 101.3726007035209, 102.04139409749659, 95.84221536672281, 82.19477689643513, + 62.61034213227201, 40.20548872373059, 18.75852748002458, 1.657957295481827, -8.90300742366735, + -12.266306640477236, -9.163238878856461, -1.233167864960895, 9.564495981683649, + 21.43992022175332, 33.051379335207095, 43.56592729978036, 52.568382283902764, 59.9082904825656, + 65.55991544152265, 69.53869459501789, 71.88526118098649, 72.70284621092118, 72.21843649851635, + 70.83368992994872, 69.13827315439758, 67.87334691224184, 67.8505290321987, 69.8442171675916, + 74.47726967536957, 82.11311705302812, 92.75985141769938, 105.99298201503689, 120.91543676261807, + 136.18772657541675, 150.16300634223884, 161.13869607756567, 167.68973106972928, + 168.99857035076562, 165.07365193284676, 156.7707462316869, 145.59585156800276, 133.34673828809346, + 121.70779304048943, 111.92368383650255, 104.63857613545397, 99.9202392953937, 97.42415696314441, + 96.61636301843436, 96.97307879858319, 98.1025495789197, 99.77349073377508, 101.86901694512392, + 104.30470000982287, 106.95197406963027, 109.5967587731362, 111.94423545513219, 113.66135899930364, + 114.4352894158048, 114.02254875294729, 112.27073376540599, 109.10848671943762, 104.5142402049289, + 98.48427593329139, 91.02225206964965, 82.16482996592228, 72.04340823046537, 60.9644383318998, + 49.47596335726721, 38.38208716267078, 28.675265153703773, 21.380090006656577, 17.336295088765645, + 16.98025827773935, 20.1977232072601, 26.30579134436739, 34.1820237918946, 42.50787279716556, + 50.05364436655743, 55.91974464196498, 59.66868428102189, 61.32427965788826, 61.25993059906409, + 60.02892932141973, 58.19725874007958, 56.22498874652909, 54.41596991142811, 52.9291999133868, + 51.82801400185255, 51.138336589289274, 50.892598938155075, 51.146726044509975, 51.96875114225244, + 53.40564661532219, 55.43870808852528, 57.93823519360839, 60.62739916553108, 63.064899619087015, + 64.65655841637202, 64.7055801259163, 62.50676710339701, 57.47927602641731, 49.31661470793777, + 38.11706927966492, 24.4510077185352, 9.330391041091739, -5.928968195692799, -19.927807358089126, + -31.432334046946572, -39.58564206883777, -44.02742340146397, -44.89862410201391, + -42.74448812517229, -38.35747836190584, -32.61145550528262, -26.32954437918894, + -20.20719977067912, -14.789151242435427, -10.48250380540003, -7.582119063177211, + -6.287826913269372, -6.702621306839947, -8.81253570937736, -12.45851173987955, -17.315467703254903, + -22.89273066518226, -28.563759038476075, -33.62421205979971, -37.369384313159216, + -39.17768128465537, -38.587042278968454, -35.35458955341083, -29.49368865852931, + -21.285110785761752, -11.260045417067115, -0.15386876206445768, 11.167509802528333, + 21.80199162922029, 30.914335137198602, 37.83281681446311, 42.1250528920106, 43.6424129336149, + 42.527536031710504, 39.1838083377227, 34.20896823571095, 28.298619788384883, 22.130902226323823, + 16.250738211119412, 10.978234041445447, 6.366580287176825, 2.2267366046830612, -1.780708650958427, + -6.007319377392964, -10.670867462380551, -15.74701218807577, -20.935394624211764, + -25.71475763924228, -29.473174580935297, -31.674804797246836, -32.01284071766054, + -30.503625724620882, -27.49687795069467, -23.603695718045664, -19.567702223835404, + -16.117560259075034, -13.838347771722535, -13.087536620644972, -13.9646827725496, + -16.328965936199715, -19.84970900489466, -24.07290465216959, -28.48977493682292, + -32.59854442331317, -35.95539956520115, -38.2137508958304, -39.15228104770502, -38.692332219912934, + -36.90456601359803, -34.004068329378526, -30.332692841352, -26.327899158206776, + -22.478863588907455, -19.27298194233058, -17.13832959626637, -16.389291255919126, + -17.18279456348231, -19.4913537172405, -23.09696936219348, -27.60749078959273, -32.494672428897246, + -37.15076766150979, -40.95788530079359, -43.36162493187602, -43.93846520775633, + -42.446152023679346, -38.848786666305784, -33.313331043792914, -26.180492183033934, + -17.918310843908614, -9.069298172978261, -0.20074671809499756, 8.136492955152297, + 15.441031108092213, 21.29036758626846, 25.372508549246056, 27.522718958733332, 27.762289494620244, + 26.32911239456066, 23.684885172768034, 20.483910329661786, 17.495388855951955, 15.484035434486861, + 15.069215691030688, 16.59509470023471, 20.047924906971748, 25.048393934050562, 30.927904484071945, + 36.87324018466698, 42.10253497425529, 46.024752154331864, 48.33939153393884, 49.0517871424882, + 48.40580399084933, 46.76081807159332, 44.45507771693884, 41.69825116535199, 38.52289882103032, + 34.803261444171405, 30.327999320714078, 24.898546537842716, 18.420586405729864, + 10.962638364832946, 2.769145218537321, -5.769675941714455, -14.177570162462116, + -21.96287190097139, -28.682775714701705, -33.98993062535793, -37.65744218731808, + -39.584533342726225, -39.78848208739123, -38.38883996706238, -35.58823507210259, + -31.651659030216656, -26.884305680876444, -21.60752145164577, -16.133301088860883, + -10.739449278450719, -5.649094917111724, -1.018762093669203, 3.061876393936713, + 6.558023261640048, 9.468369818928435, 11.800068844557336, 13.547269650304042, 14.679655966520992, + 15.144569931188034, 14.882211925027542, 13.849398415516339, 12.04474239771272, 9.52770863530006, + 6.425839876890219, 2.9278589763920806, -0.735815798162439, -4.320509611419369, -7.596486438809738, + -10.37389619761122, -12.519116524619744, -13.960610192047202, -14.685038419335728, + -14.726561926961596, -14.153639370214792, -13.057964531219815, -11.549353043113381, + -9.758421987878664, -7.846069969532445, -6.015682302494043, -4.521627491850837, + -3.6670759284008367, -3.78622388922754, -5.210529940531722, -8.224269075041521, + -13.019390902549098, -19.661194514880624, -28.07379403356179, -38.04868886403423, + -49.2733259570762, -61.37184021856482, -73.94857243204568, -86.62633925391154, -99.07453382284537, + -111.02550604149512] +pitch: [-157.8684778032057, -158.3698356698477, -159.00269434786534, -159.7713293302798, + -160.66230277415343, -161.6470704856313, -162.69256942436138, -163.77220996055956, + -164.87013794172887, -165.9791087337121, -167.09653844180943, -168.22171491746042, + -169.35450952912657, -170.49493872451055, -171.6430483690157, -172.79888945081638, + -173.96251395289818, -175.13397428130384, -176.31332320177768, -177.50061383591552, + -178.69589966306725, -179.89923452271375, -181.1106726168898, -182.33026851262574, + -183.55807714440536, -184.7941538166402, -186.0385542061607, -187.29133436472392, + -188.5525507215386, -189.8222600858066, -191.10051964928178, -192.38738698884652, + -193.6829200691045, -194.98717724499227, -196.3002172644072, -197.62209927085402, + -198.95288280610868, -200.29262781290038, -201.64139463761154, -202.99924403299593, + -204.36623716091532, -205.7424355950942, -207.12790132389352, -208.52269675310262, + -209.92688470875024, -211.3405284399345, -212.76369162167163, -214.19643835776446, + -215.6388331836894, -217.09094106950369, -218.55282742277168, -220.02455809151112, + -221.50619936715893, -222.99781798755748, -224.49948113996047, -226.0112564640592, + -227.53321205502968, -229.0654164665993, -230.60793871413497, -232.16084827775205, + -233.72421510544342, -235.2981096162307, -236.88260270333552, -238.47776573737366, + -240.08367056956885, -241.70038953498985, -243.3279954558082, -244.96656164457897, + -246.6161619075422, -248.27687054794822, -249.9487623694034, -251.63191267924097, + -253.32639729191183, -255.03229253240016, -256.7496752396611, -258.4786227700819, + -260.2192130009664, -261.9715243340426, -263.73563569899414, -265.5116265570161, + -267.2995769043935, -269.0995672761047, -270.9116787494495, -272.7359929477005, + -274.57259204377965, -276.42155876396043, -278.28297639159297, -280.15692877085615, + -282.04350031053394, -283.9427759878175, -285.85484135213284, -287.77978252899436, + -289.71768622388356, -291.66863972615556, -293.6327309129702, -295.61004825325045, + -297.6006808116674, -299.60471825265194, -301.62225084443344, -303.65336946310623, + -305.69816559672216, -307.7567313494122, -309.82915944553446, -311.91554323385157, + -314.0159766917348, -316.1305544293973, -318.2593716941557, -320.40252437472, -322.5601090055133, + -324.7322227710197, -326.91896351016163, -329.12042972070725, -331.33672056370636, + -333.56793586795794, -335.81417613450594, -338.0755425411669, -340.3521369470865, + -342.64406189732756, -344.9514206274901, -347.27431706835876, -349.61285585057675, + -351.9671423093153, -354.337282488798, -356.72338314601444, -359.1255517507706, + -361.5438964706455, -363.97852609840754, -366.4295497760087, -368.8970760521407, + -371.3812099206837, -373.88204421775976, -376.39963653729615, -378.9339521671109, + -381.48473467467466, -384.0512382159208, -386.631726774158, -389.2226378464643, + -391.81736161291474, -394.40474356794095, -396.9676796187925, -399.48243974549047, + -401.9194238634856, -404.2457230176494, -406.4291524303254, -408.4426925007373, + -410.267999574875, -411.89703489626, -413.33164749245606, -414.5816483484149, -415.66220963815863] +roll: [-157.8684778032057, -158.3698356698477, -159.00269434786534, -159.7713293302798, + -160.66230277415343, -161.6470704856313, -162.69256942436138, -163.77220996055956, + -164.87013794172887, -165.9791087337121, -167.09653844180943, -168.22171491746042, + -169.35450952912657, -170.49493872451055, -171.6430483690157, -172.79888945081638, + -173.96251395289818, -175.13397428130384, -176.31332320177768, -177.50061383591552, + -178.69589966306725, -179.89923452271375, -181.1106726168898, -182.33026851262574, + -183.55807714440536, -184.7941538166402, -186.0385542061607, -187.29133436472392, + -188.5525507215386, -189.8222600858066, -191.10051964928178, -192.38738698884652, + -193.6829200691045, -194.98717724499227, -196.3002172644072, -197.62209927085402, + -198.95288280610868, -200.29262781290038, -201.64139463761154, -202.99924403299593, + -204.36623716091532, -205.7424355950942, -207.12790132389352, -208.52269675310262, + -209.92688470875024, -211.3405284399345, -212.76369162167163, -214.19643835776446, + -215.6388331836894, -217.09094106950369, -218.55282742277168, -220.02455809151112, + -221.50619936715893, -222.99781798755748, -224.49948113996047, -226.0112564640592, + -227.53321205502968, -229.0654164665993, -230.60793871413497, -232.16084827775205, + -233.72421510544342, -235.2981096162307, -236.88260270333552, -238.47776573737366, + -240.08367056956885, -241.70038953498985, -243.3279954558082, -244.96656164457897, + -246.6161619075422, -248.27687054794822, -249.9487623694034, -251.63191267924097, + -253.32639729191183, -255.03229253240016, -256.7496752396611, -258.4786227700819, + -260.2192130009664, -261.9715243340426, -263.73563569899414, -265.5116265570161, + -267.2995769043935, -269.0995672761047, -270.9116787494495, -272.7359929477005, + -274.57259204377965, -276.42155876396043, -278.28297639159297, -280.15692877085615, + -282.04350031053394, -283.9427759878175, -285.85484135213284, -287.77978252899436, + -289.71768622388356, -291.66863972615556, -293.6327309129702, -295.61004825325045, + -297.6006808116674, -299.60471825265194, -301.62225084443344, -303.65336946310623, + -305.69816559672216, -307.7567313494122, -309.82915944553446, -311.91554323385157, + -314.0159766917348, -316.1305544293973, -318.2593716941557, -320.40252437472, -322.5601090055133, + -324.7322227710197, -326.91896351016163, -329.12042972070725, -331.33672056370636, + -333.56793586795794, -335.81417613450594, -338.0755425411669, -340.3521369470865, + -342.64406189732756, -344.9514206274901, -347.27431706835876, -349.61285585057675, + -351.9671423093153, -354.337282488798, -356.72338314601444, -359.1255517507706, + -361.5438964706455, -363.97852609840754, -366.4295497760087, -368.8970760521407, + -371.3812099206837, -373.88204421775976, -376.39963653729615, -378.9339521671109, + -381.48473467467466, -384.0512382159208, -386.631726774158, -389.2226378464643, + -391.81736161291474, -394.40474356794095, -396.9676796187925, -399.48243974549047, + -401.9194238634856, -404.2457230176494, -406.4291524303254, -408.4426925007373, + -410.267999574875, -411.89703489626, -413.33164749245606, -414.5816483484149, -415.66220963815863] +x: [-127.90269211939827, -131.2678097441197, -136.70543077518892, -142.80201308388703, + -147.68698584712374, -150.43090024970653, -151.93731066347436, -153.97975151042488, + -157.1611591307891, -160.11613483598717, -160.9024782819122, -159.04126159161888, + -156.07990100822514, -154.23554261200883, -154.56584426858808, -156.35317090044515, + -158.04548754985666, -158.68146511423873, -158.50567928984265, -158.41585507282485, + -158.95307629614172, -159.79740407636362, -160.1014847915644, -159.23338066405643, + -157.30653996810443, -155.20316918530557, -154.14843270998995, -155.05581305813232, + -157.9836096719787, -162.09393936591388, -166.22381774921027, -169.63236007909416, + -172.26672230472926, -174.36451975460542, -175.90099493885046, -176.49219286857985, + -175.798054128794, -173.93808909631377, -171.4925114649683, -169.12343773356466, + -167.17664041477232, -165.5708829941525, -164.00639105353508, -162.2887264315374, + -160.51670606493755, -159.0215878949382, -158.1431878867084, -158.0223901064516, + -158.5297306199403, -159.33939301424593, -160.09571223386598, -160.59095964674498, + -160.8477855836447, -161.02451272164438, -161.17795428146462, -161.05434333340307, + -160.10141346846075, -157.75281536914142, -153.82556545048772, -148.76090920354517, + -143.52247341799338, -139.17693928388738, -136.36789818536403, -134.95124204564573, + -133.9874074915166, -132.1254263634103, -128.22234525346727, -121.90425650473114, + -113.78832718004412, -105.27070727944219, -98.02475400195476, -93.46091461275282, + -92.30826635434332, -94.3390728032595, -98.26494379864691, -101.94885284908618, + -103.04849210213153, -99.9163743951314, -92.28166421723154, -81.29521224087058, + -68.95793224897206, -57.37532906713539, -48.286218251495455, -42.9311115531321, + -41.97664928415379, -45.25819014572354, -51.473054204178524, -58.23430997331244, + -62.75013392767508, -62.9223262047309, -58.280188016636316, -50.21077109811404, + -41.35681129895958, -34.4990081709638, -31.449867093567427, -32.42754723012978, + -36.15011519371447, -40.58089839438473, -43.94788650904792, -45.50317129106411, + -45.62757380883436, -45.28654859482272, -45.24609917358149, -45.57109091310874, + -45.68440108090419, -44.8673235962936, -42.817117026428, -39.89366389594422, -36.92267974205595, + -34.701751190785245, -33.51756732176649, -32.957648526197254, -32.118390119251, + -30.08038179415433, -26.373159258276946, -21.169894158050823, -15.12315035159766, + -8.966988036828536, -3.135614678740315, 2.3860480980408405, 7.925467603882083, 13.822714648008436, + 20.175371018999993, 26.7822639200393, 33.293134470121245, 39.42975802508738, 45.09936774714966, + 50.30205732126428, 54.88772677310383, 58.342229552134924, 59.78844212222738, 58.2636935332989, + 53.15779898393836, 44.5830880558185, 33.47443622424592, 21.363593342632985, 9.940566867866028, + 0.6026213709298839, -5.8399344345675575, -9.240708782689756, -10.022124423046609, + -8.998133886903553, -7.186216889606415, -5.63851099666342, -5.297145100741871, -6.879790086756265, + -10.809823525239507, -17.202765649327844, -25.90589109754088, -36.57282515330877] +y: [-95.1053320679519, -102.57942707703143, -115.79349366134936, -132.91150346877552, + -149.8018623891795, -161.15737205071278, -162.80068928833558, -153.8389080682052, + -137.30710232263985, -118.90086421096659, -104.39325930921773, -97.04253564148495, + -96.49753527612377, -99.89130313663716, -104.21834166352956, -108.11727712225031, + -111.8967411258529, -116.28298547066109, -121.23895629376239, -125.73427933608644, + -128.36883519321356, -128.19087124969568, -125.13859527382397, -120.00974008025909, + -114.2430140709469, -109.69248942902398, -108.19817332485465, -110.78624497979699, + -116.93512079376003, -124.6998747734192, -131.86718572831617, -137.27226089357666, + -141.20696974324156, -144.759479700215, -148.87001560457742, -153.88261513985736, + -159.68734476449134, -166.07420632507709, -172.96469725505096, -180.3920700150901, + -188.21937883314945, -195.72926730208002, -201.41736840430042, -203.31624017701841, + -199.82553623123383, -190.59338598173065, -176.88869378565937, -161.2238841338048, + -146.45822831996625, -134.87673586346156, -127.65308031796329, -124.80090469016707, + -125.45619356179108, -128.27549428216608, -131.84101844992867, -135.07530215082386, + -137.65337624491679, -140.25913711418366, -144.43087684329745, -151.8938368423789, + -163.67339640117606, -179.56328628213708, -198.32489175456183, -218.42489246839165, + -238.7164724172193, -258.60743976458997, -277.7429960783446, -295.56813001110913, + -311.0788097160963, -322.80675460113724, -328.93908316116415, -327.56037608593266, + -317.12330108413994, -297.14900905724227, -268.84769440086353, -235.16971112565426, + -200.04553112593794, -167.12032560737575, -138.66238790035453, -115.19890389580594, + -95.95279511481895, -79.72820892106228, -65.76755588438334, -54.22790801461051, + -46.13190044244114, -42.84004879066222, -45.24539383402826, -52.99497625151277, + -64.07018690090207, -74.97804538426473, -81.59594832574722, -80.41038856548796, + -69.63949990787273, -49.72242741663517, -22.966995663147852, 7.39427794510237, 38.19309913101399, + 66.89173432062842, 91.64289937473961, 111.13226208974095, 124.5518791391896, 131.77418236143038, + 133.46586276149228, 130.83435645169016, 124.99732180672979, 116.333135612117, 104.26680906118598, + 87.68178840678189, 65.74204569072383, 38.662165827940036, 8.014089907704136, -23.571276834584964, + -53.142563231420375, -78.2522773792973, -97.51345531907658, -110.70725605031265, + -118.5016237829998, -122.00842291412158, -122.404338258332, -120.6995916626668, + -117.59445385917022, -113.3407631971568, -107.62417911491204, -99.58313488352168, + -88.06247336126238, -72.06061544362858, -51.18535335279474, -25.910478578073977, + 2.4559772454057507, 32.01779445834423, 60.591762572708525, 85.94477375005923, 105.9576058260377, + 118.78964738570927, 123.11631379953998, 118.43389190043557, 105.33379726489348, + 85.60032102947753, 62.014359671431286, 37.84509121681435, 16.144470009647492, -0.9347463893425387, + -12.559784399157243, -19.23707231899498, -22.45859920077777, -24.150622679079273, + -26.143960951682608, -29.820707661306084, -35.98068482695456, -44.88212815398885] +yaw: [-157.8684778032057, -158.3698356698477, -159.00269434786534, -159.7713293302798, + -160.66230277415343, -161.6470704856313, -162.69256942436138, -163.77220996055956, + -164.87013794172887, -165.9791087337121, -167.09653844180943, -168.22171491746042, + -169.35450952912657, -170.49493872451055, -171.6430483690157, -172.79888945081638, + -173.96251395289818, -175.13397428130384, -176.31332320177768, -177.50061383591552, + -178.69589966306725, -179.89923452271375, -181.1106726168898, -182.33026851262574, + -183.55807714440536, -184.7941538166402, -186.0385542061607, -187.29133436472392, + -188.5525507215386, -189.8222600858066, -191.10051964928178, -192.38738698884652, + -193.6829200691045, -194.98717724499227, -196.3002172644072, -197.62209927085402, + -198.95288280610868, -200.29262781290038, -201.64139463761154, -202.99924403299593, + -204.36623716091532, -205.7424355950942, -207.12790132389352, -208.52269675310262, + -209.92688470875024, -211.3405284399345, -212.76369162167163, -214.19643835776446, + -215.6388331836894, -217.09094106950369, -218.55282742277168, -220.02455809151112, + -221.50619936715893, -222.99781798755748, -224.49948113996047, -226.0112564640592, + -227.53321205502968, -229.0654164665993, -230.60793871413497, -232.16084827775205, + -233.72421510544342, -235.2981096162307, -236.88260270333552, -238.47776573737366, + -240.08367056956885, -241.70038953498985, -243.3279954558082, -244.96656164457897, + -246.6161619075422, -248.27687054794822, -249.9487623694034, -251.63191267924097, + -253.32639729191183, -255.03229253240016, -256.7496752396611, -258.4786227700819, + -260.2192130009664, -261.9715243340426, -263.73563569899414, -265.5116265570161, + -267.2995769043935, -269.0995672761047, -270.9116787494495, -272.7359929477005, + -274.57259204377965, -276.42155876396043, -278.28297639159297, -280.15692877085615, + -282.04350031053394, -283.9427759878175, -285.85484135213284, -287.77978252899436, + -289.71768622388356, -291.66863972615556, -293.6327309129702, -295.61004825325045, + -297.6006808116674, -299.60471825265194, -301.62225084443344, -303.65336946310623, + -305.69816559672216, -307.7567313494122, -309.82915944553446, -311.91554323385157, + -314.0159766917348, -316.1305544293973, -318.2593716941557, -320.40252437472, -322.5601090055133, + -324.7322227710197, -326.91896351016163, -329.12042972070725, -331.33672056370636, + -333.56793586795794, -335.81417613450594, -338.0755425411669, -340.3521369470865, + -342.64406189732756, -344.9514206274901, -347.27431706835876, -349.61285585057675, + -351.9671423093153, -354.337282488798, -356.72338314601444, -359.1255517507706, + -361.5438964706455, -363.97852609840754, -366.4295497760087, -368.8970760521407, + -371.3812099206837, -373.88204421775976, -376.39963653729615, -378.9339521671109, + -381.48473467467466, -384.0512382159208, -386.631726774158, -389.2226378464643, + -391.81736161291474, -394.40474356794095, -396.9676796187925, -399.48243974549047, + -401.9194238634856, -404.2457230176494, -406.4291524303254, -408.4426925007373, + -410.267999574875, -411.89703489626, -413.33164749245606, -414.5816483484149, -415.66220963815863] +z: [27.487690876753174, 15.337246293823744, -6.519501713577381, -32.17378390020878, + -52.95176670190418, -61.55800766961198, -55.94481745228971, -40.02071700799717, + -21.554360535385083, -9.07142702001839, -8.346860847184745, -19.368396082981224, + -36.19083663458397, -50.83073806660286, -58.44885149238343, -59.53297302408236, + -57.86810718901493, -56.968343215030046, -58.092142359329614, -60.51600905693699, + -62.7444480158786, -63.35926464089802, -61.25799874825209, -55.70616111817719, -46.57263622352219, + -34.71402685096095, -22.12817933348742, -11.450779533332875, -4.771195360275572, + -2.4341668275669868, -2.815083023486069, -3.436788885507724, -2.639407740293576, + -0.49257916688823505, 1.6634057300540754, 2.4787157173774994, 1.5816568083905458, + -0.3081254457303777, -1.9465496236895794, -2.235946638392112, -0.587599350222585, + 3.0830448889107243, 8.585148043725034, 15.741871127579394, 24.588070885472288, 35.317967966866895, + 47.92047635319786, 61.70329222579224, 75.14570192551099, 86.40468533274036, 94.26512130476762, + 98.82097106678889, 101.29642767567256, 103.1652314081855, 105.33907861244916, 107.99131029859556, + 110.83900010952557, 113.33429248514499, 114.58664344276714, 113.36413234926317, + 108.50758731712286, 99.61208997647628, 87.50344465477376, 74.1768095655181, 62.22532988939497, + 54.03291376712356, 51.07089312083894, 53.5800909155043, 60.73095402033792, 71.08141473731635, + 83.00300927492155, 94.88248494409528, 105.19436856609353, 112.66234904016706, 116.5532902732588, + 116.89064945934301, 114.34290812223043, 109.7932211178687, 103.88036334169092, 96.83894755858785, + 88.72518958612778, 79.81089200833654, 70.81116373157523, 62.753896533262505, 56.569823287756314, + 52.671108932076606, 50.767905449313446, 49.98887184490788, 49.18533829409316, 47.24706022687302, + 43.33944398884515, 37.069957898660974, 28.60206393388892, 18.673743747050306, 8.448948216774712, + -0.7972607166725888, -8.036081276408424, -12.709833208187701, -14.735329114490181, + -14.3542414036342, -11.989651298267246, -8.214474065551915, -3.796221322329577, + 0.3127831108565613, 3.1349675066051397, 3.941404446156438, 2.4944463514526865, -0.8862482050049294, + -5.474872423279538, -10.422540767592642, -15.02656078493345, -18.872907062153832, + -21.8268990816907, -23.925876718830928, -25.25373825675896, -25.85191903924253, + -25.679848344814015, -24.61517234703336, -22.486711991951278, -19.140322650202826, + -14.528471985404261, -8.792284878821462, -2.2948582536620714, 4.415692468816276, + 10.697174253055142, 15.917112597767035, 19.536456023535326, 21.170365945226813, + 20.644086856810723, 18.052446724627877, 13.800892715061188, 8.584501211461651, 3.2736044258695483, + -1.2815934000814575, -4.466744501254597, -6.057544821897366, -6.251111895303721, + -5.565299343444047, -4.652409384326363, -4.10510631771834, -4.32579622671807, -5.493001584407323, + -7.61115583178264, -10.59834849765868, -14.363422130581966, -18.8448260745628, -24.011142744024184, + -29.84051753345265, -36.298155831550744, -43.32322171459229] diff --git a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_pickup_action/config/trajectory_weights/weights_table_grasp2.yaml b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_pickup_action/config/trajectory_weights/weights_table_grasp2.yaml deleted file mode 100644 index eb2ff09af..000000000 --- a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_pickup_action/config/trajectory_weights/weights_table_grasp2.yaml +++ /dev/null @@ -1,205 +0,0 @@ -pitch: [307.4878359078107, 268.6905664663287, 184.8643455969408, 58.545770552115364, - -89.56664765402549, -224.0840865697819, -311.65407200869106, -340.4518465053893, - -326.36988658361236, -301.0968996723944, -291.5230956032813, -302.448851382407, - -308.3914210364421, -257.0395550286017, -88.99193040972278, 224.31037039483945, - 637.5643785752965, 1014.2632003322818, 1164.60022439979, 937.0932511424448, 318.12976917975465, - -527.5153035415188, -1314.8532609032436, -1771.4497973469677, -1766.3703806713406, - -1356.8367650999844, -731.5592528971431, -99.4581862321175, 399.86183644411614, - 718.1307829590074, 864.2303274362567, 860.5931414552338, 727.3067660910908, 493.74315683086445, - 211.0550064061407, -55.45330435442286, -250.34727987639022, -348.6866563291638, - -358.565639352564, -307.32677689486184, -224.36311412190383, -131.82405445887937, - -44.96270554146841, 23.964988907092717, 64.72417513759937, 72.30507366754365, 50.61763777504188, - 12.362262387723732, -26.009558772221443, -51.563046820900915, -60.25116047362398, - -57.63900635128191, -54.959634616138075, -62.76006262040929, -85.28131693354047, - -118.1619302012079, -150.44250732835803, -169.7791822109458, -168.1441788914054, - -145.07303803275605, -107.02233887068444, -63.74432140905734, -24.160657132225335, - 6.0201500194586295, 24.353211429945322, 30.418517152171734, 24.744807885237314, - 8.54681746977837, -16.072008864403312, -46.073491254692094, -77.77427601735346, - -107.2115322777933, -130.4200732695773, -143.8634527292192, -145.35651564226546, - -135.32974372376032, -117.6277916325565, -98.8975621416649, -86.30310639836785, - -84.41064317127606, -92.8345065874572, -106.05233844759589, -115.72596830250339, - -114.51044215834928, -99.48168494803396, -73.46438430348987, -43.629996980997795, - -18.156763095107696, -2.6983372116195183, 1.6251365457887872, -2.0597324324561095, - -8.49567377463034, -12.953091059582318, -13.113183257935408, -9.481331616425296, - -4.45900774984968, -0.7848834778791399, -0.18676565696596104, -2.808904223949428, - -7.5024500572913935, -12.643567728048698, -16.95955800706378, -19.94186321219478, - -21.708274735805453, -22.481173409023892, -22.036303473716234, -19.483798588997214, - -13.578418452496079, -3.4826781595599914, 10.366639194007917, 25.77702234203514, - 39.091379046783125, 46.29788026101588, 44.56751406543836, 33.53582040720307, 15.694765808949526, - -4.334486259581232, -21.323797450096354, -31.041584270219868, -31.22069213125181, - -21.54964734625588, -2.8776138683103407, 23.7844367985862, 57.82513782104277, 98.79780481615855, - 145.4992270312695, 194.90101264523364, 241.65465388393397, 278.7147855690786, 299.1252817673519, - 298.39336513620555, 276.45014679106646, 238.23150408778133, 192.46066874693108, - 149.0289783101666, 116.02858010020579, 97.63386102982291, 93.58901641305495, 100.29361472348529, - 112.7990857264626, 126.76046188006744, 139.60001029106323, 150.64811215552695, 160.52595461631785, - 170.27880196415424, 180.70091624441704, 192.04307822110903, 204.05305965949202, - 216.18527381768172, 227.82687947071088] -roll: [209.26310319076802, 185.35708614868105, 132.6897487870579, 54.74474038999782, - -31.519454510078013, -98.13006804695982, -118.77669951323375, -85.06693497694505, - -14.235704420919252, 58.16814298294952, 96.67724083087974, 85.15954851222429, 36.873829392237596, - -10.145406945028034, -11.481257116039632, 58.52851156001803, 186.3263326311764, - 315.50269174000914, 366.84348407733404, 276.95027122405827, 36.73874124450958, -294.0894554520116, - -609.7059109083152, -807.7918688909466, -836.3079077346105, -708.6679360172418, - -481.9615748339739, -219.52651695828882, 33.523250870445345, 249.79894425175672, - 405.80675450006413, 475.8414790211664, 443.1339326191075, 317.9892223406249, 143.97679712422791, - -16.929647559130625, -111.72437322217338, -118.54462373871019, -51.16597816395034, - 53.87450880811503, 157.2424781910211, 231.9761739730581, 267.7723038745797, 266.3475125713114, - 234.5635176694967, 180.88439377517432, 115.78806223060508, 52.645730705883835, 5.590726742336357, - -15.6733964645854, -10.816947889478474, 10.163304297425586, 31.218593642664974, - 37.70081170101127, 23.116695718718013, -8.20667536030345, -43.686190491276086, -68.69649557045352, - -73.06837593120727, -54.64085691395713, -18.63246623935183, 25.850445993994438, - 69.51072010097609, 104.80830847584399, 126.12959487616133, 129.57146050260752, 113.55161699310824, - 80.07737649246528, 35.35586720125649, -11.387698416925671, -50.68753776186014, -75.75959843259228, - -84.02293625033323, -76.73566097281713, -57.494444226847854, -30.760409155652727, - -1.0276326529813191, 27.562102578316143, 51.662531378506465, 69.37983248353137, - 80.7684289277017, 87.628045221175, 92.41011548202059, 96.61452101767676, 99.47051698157318, - 97.71676906600825, 86.88901059775175, 63.80375285151239, 29.164901084806242, -11.160013096117748, - -47.262641207400634, -68.63171822218054, -68.46417700105185, -46.71376113942381, - -10.27127461618544, 29.81541870972953, 62.61275158048216, 80.95721075519452, 82.98199830641752, - 71.3139477529786, 50.89179151297815, 26.739883740545775, 2.6378917734857015, -19.10301168403571, - -37.1278851161751, -50.36456315285917, -57.64841632980128, -57.843181803673154, - -50.38294932729228, -35.94466489294901, -16.857642838149165, 3.0719474036396037, - 19.39001696071014, 28.21647015844554, 27.38317791547756, 17.046417831149483, -0.4042222480479882, - -21.00352901558579, -40.12681572356702, -53.39478061329125, -57.446634329655396, - -50.56130059021794, -33.08140703270382, -7.510664287348054, 21.85416673618458, 49.76342142065035, - 71.12837322282269, 82.07761137185807, 80.65960322544773, 67.06414330741778, 43.41422704198978, - 13.263606774566217, -19.081626061668885, -49.34020699117489, -74.0478926740234, - -91.22824282120375, -100.7717211846692, -104.3978188758738, -105.19551733743343, - -106.87164934337899, -112.91706829452616, -125.90116577727926, -147.04455533292247, - -176.13073203863172, -211.72608421627854, -251.60421759783222, -293.23533244875233, - -334.21463552288907, -372.55390356472765, -406.81959736322625] -x: [-45.06172651446696, -44.50528599867761, -42.368640652908475, -38.830535456218065, - -34.65801579069439, -30.983082543826082, -28.72202872009684, -28.015528300878216, - -28.085991021330404, -27.58455176054732, -25.262499727416778, -20.75564499359652, - -15.19536087941695, -11.21671003700053, -11.993639360021191, -19.437422277745792, - -32.447943386643736, -46.478480540755164, -55.149776956285436, -53.3934966463505, - -40.445788116615965, -20.825459731577343, -2.477606887905092, 7.035867486206661, - 4.155856374674821, -9.303862131425433, -27.657692673971805, -44.546343814865175, - -55.7689455322009, -60.25261140134869, -59.13712408519084, -54.160813823309184, - -46.62111236408496, -37.305646841577804, -26.893927075314252, -16.183920442030807, - -5.999674997697829, 2.910908698387282, 9.755343424645364, 13.538283066156557, 13.241472976673812, - 8.396625694314823, -0.38079952591690547, -11.397119171484686, -22.45507255244106, - -31.52321272828289, -37.03035385669701, -37.80041338037171, -33.0712566283519, -22.95158885195406, - -9.124585729331447, 4.883204007262881, 14.480520566178551, 15.770177571956838, 7.41293334710724, - -8.552445864978143, -27.5313642341668, -44.41663657860791, -55.68388034904888, -60.427741680611774, - -59.9937930459423, -56.719364192122605, -52.65005273653141, -48.83977228724342, - -45.33404050933557, -41.59016220917724, -37.02088588471639, -31.426464347551594, - -25.173924942450824, -19.08264482088237, -14.101403266516412, -10.961627261981635, - -9.981518914691089, -11.072260440425637, -13.861239803818183, -17.80909929942429, - -22.270666373919425, -26.54450321960095, -29.975253902604027, -32.107259961129884, - -32.812067748569234, -32.30360716444636, -31.0201553767969, -29.433007073990666, - -27.87503588394257, -26.453081217503584, -25.050397747158176, -23.38476177482476, - -21.085517394485183, -17.77776385282289, -13.185562167890449, -7.263320555902968, - -0.3294708406457369, 6.869419554481178, 13.250421173333713, 17.6760018421937, 19.351189241920277, - 18.163394476378677, 14.785784235499513, 10.47501844204813, 6.655439819182812, 4.485718819454019, - 4.58549847944645, 6.986206011304477, 11.250711135388523, 16.65542375999597, 22.355100377099344, - 27.508920811058278, 31.383724878043846, 33.44881037637206, 33.45614748643667, 31.48873627610872, - 27.96619536639039, 23.605724547672324, 19.33357060504991, 16.133879748033763, 14.832217030260278, - 15.853115457495218, 19.04410095139672, 23.67625043538951, 28.677693300936625, 33.04210297910169, - 36.239788348955784, 38.42043366054024, 40.27448445421512, 42.58813523027747, 45.700060492278425, - 49.14846003137164, 51.72880324371295, 51.98688602021415, 48.941383357875814, 42.68581046653293, - 34.54746197388396, 26.68093213691205, 21.252995525236017, 19.590579912456967, 21.696830762957248, - 26.367017650691324, 31.841655780072134, 36.67100305173607, 40.37149025572581, 43.57957874812686, - 47.68005182843892, 54.14666611922619, 63.93627298937479, 77.18452867591833, 93.25272676218941, - 111.008895420734, 129.17116244093623, 146.58353986331068] -y: [-25.632812653878084, -25.667026850630098, -25.65332690812688, -25.558627745804298, - -25.33186876288174, -24.904897968381164, -24.226218075506107, -23.346949242002584, - -22.525712735071995, -22.251250918985015, -23.085902930377973, -25.359151452629384, - -28.90513928701911, -33.066260971831575, -36.99637792168402, -40.03826105838443, - -41.880735628177405, -42.42050800341945, -41.5567807229203, -39.21066023803674, - -35.599482619495845, -31.47667698045251, -28.009473562646896, -26.25678941493762, - -26.55439421679798, -28.216585252720197, -29.77202618579056, -29.64711497611054, - -26.986915393790145, -22.222325752189317, -17.04789312298538, -13.702191827969843, - -13.833811699034209, -17.575801405395516, -23.413990272727666, -28.95024708198037, - -32.0831960812367, -31.909070477121002, -28.903841440489884, -24.4268833368915, - -19.944130223749177, -16.425839500414593, -14.161649030460561, -12.929616029397886, - -12.266096677317192, -11.61826685907862, -10.35404729487928, -7.780517167794712, - -3.3477180847008396, 2.9236071062312, 9.976564391358705, 15.58103577341059, 16.85974444144582, - 11.469939519433398, -1.079396478715883, -18.786351219778194, -37.73281968864431, - -53.67606253742803, -63.7665596338745, -67.43044993864187, -66.05735789009529, -61.91288732437796, - -57.04711768075798, -52.72981087096425, -49.45062439775582, -47.21155679269143, - -45.83821983372657, -45.182047343603166, -45.1943468982154, -45.8897019743006, -47.23411283167097, - -49.02109598560663, -50.81643091894815, -52.03137069957861, -52.121037130400474, - -50.825141958817056, -48.31933867824606, -45.17065043113971, -42.090592529998936, - -39.598662297423516, -37.768709649386004, -36.19087119160269, -34.16819186445556, - -31.048004963271847, -26.526682203483297, -20.789139245486016, -14.435796157768074, - -8.259559885376913, -2.999964631715642, 0.8186650166284414, 2.9422761080995, 3.387415256575487, - 2.4420763596642288, 0.6635327469620856, -1.179934294458653, -2.2590851149220046, - -1.8911451779088735, 0.28561294065438286, 4.251437691967593, 9.675710506169587, - 16.06681022211528, 22.921733737168566, 29.81746909367072, 36.43167509245798, 42.51653506067146, - 47.859365580737574, 52.25240203792988, 55.479350852962426, 57.319328935856134, 57.56909136480433, - 56.08566795047459, 52.849164358239534, 48.03715133047759, 42.08664925098448, 35.702580137057076, - 29.76916997469084, 25.151441007074254, 22.436448222940378, 21.725577938714736, 22.6016034969853, - 24.329513221147273, 26.229211741871882, 28.045518140109163, 30.10798162753032, 33.15425242231318, - 37.858495287271715, 44.27968744192287, 51.52751426199087, 57.87543900348522, 61.34545269040426, - 60.539929869686446, 55.33441340375058, 47.07099830279992, 38.11135947152029, 30.919291820345794, - 27.083951997907352, 26.735370322450763, 28.616832132079118, 30.75971816520821, 31.42066844205043, - 29.834764123115985, 26.46114693907252, 22.67392658476609, 20.12175806668451, 20.095614133885213, - 23.164629534288462, 29.149928104993194, 37.34013985555722, 46.789986502918026, 56.57574237593195] -yaw: [-85.82284184234118, -81.26044783480269, -70.07101489342415, -52.73568112770571, - -32.312887926707255, -13.78839906939816, -1.6672223628687894, 2.62855477579123, - 1.4637997318473883, -0.7122178494319122, -0.2890683255906193, 2.770600737662801, - 3.2517524724015923, -9.01552308198881, -45.45607518744383, -111.05972990835949, - -195.39715744012742, -268.8330368320655, -290.4412975641661, -227.7732085861161, - -78.9317597006005, 117.9201686381322, 297.2293991423197, 396.33374712240726, 385.71000473625224, - 280.50998160917317, 127.53082352544902, -21.591635655970986, -132.8294537852815, - -196.17596093309243, -217.6704538759289, -207.65930075811795, -174.68571913075345, - -126.36479441762415, -72.15015673522322, -23.100178225311307, 11.58771751528709, - 27.978929943856027, 27.906619220099845, 16.62722814185456, -0.16994623726551647, - -17.947750597398596, -33.17934811581, -42.66989953652374, -43.643970253947295, -34.82553827018641, - -17.59531893754674, 3.929234637048963, 24.263557047593597, 38.64014574629869, 44.552947689726544, - 41.97514116975504, 32.47694516434011, 18.147714201342975, 1.0543883277121482, -16.755211464052067, - -33.21729388374368, -46.57313523562773, -55.90724331833623, -61.43109318991602, - -64.21217824353955, -65.48793688068338, -65.9705207973369, -65.47926190899054, -63.023571617961, - -57.305023318747764, -47.494327619576445, -33.97700167313159, -18.641436579684857, - -4.416663663991963, 5.814384906636482, 10.373847070406663, 9.38411923429142, 4.508860698209896, - -1.9356865197155904, -7.9606412898217584, -12.499124266041337, -15.456913061839213, - -17.37269370009648, -18.966954306399614, -20.79261413744074, -23.053534020790096, - -25.54353654187318, -27.634858828262228, -28.303928178285414, -26.267104900850885, - -20.325567487838626, -9.923911719253681, 4.266053071819271, 20.08528752116726, 34.2619072645698, - 43.435630478396426, 45.44194217842096, 40.22939648971534, 29.921002335698567, 17.985046686760153, - 7.955705382085571, 2.329433852600685, 2.0711572030452037, 6.771528965996401, 15.179522806472088, - 25.75856576877883, 37.04824578537782, 47.805604932006396, 57.016629050400816, 63.87734670108889, - 67.79620197389316, 68.42669261663922, 65.72118040843198, 59.987052835925, 51.912611203184056, - 42.52413442506399, 33.05511088091003, 24.747749762642485, 18.636166646439452, 15.358295662629935, - 15.020899007568389, 17.13149788589931, 20.62788661787804, 24.055160585938665, 25.916901012504752, - 25.143507855405588, 21.51583036210249, 15.833945868198665, 9.689157721402468, 4.8723748326537475, - 2.647898968595296, 3.2228179980864446, 5.6760220781836415, 8.397691363657867, 9.837649261200632, - 9.20442163232231, 6.78138527862297, 3.719218222006543, 1.4229609534044665, 0.843888753429656, - 2.0177255310178945, 4.054148799738449, 5.555057476845689, 5.23807333755755, 2.464326517244721, - -2.5545874557537833, -8.917245787438091, -15.429489147674806, -21.024424026656018, - -25.033276121438494, -27.24968793325762, -27.83529269443636, -27.158644596793252, - -25.645380324756296] -z: [-22.62081372627218, -22.458849389589915, -22.340158902554457, -22.281688493304806, - -22.365061136407927, -22.783008243658642, -23.821075585958067, -25.770596214499594, - -28.86135296108498, -33.26544631579463, -39.04450507004138, -45.82952846645735, - -52.269913587374774, -55.75656315934748, -53.11089966848031, -42.385828263327085, - -24.86439162240813, -5.690258819702291, 7.8631504484666515, 9.877877168383415, -0.999054547295284, - -20.69272715480139, -41.777828295400504, -57.16709867072815, -63.12578232701807, - -59.99447638330691, -50.70475895168664, -38.57061194894658, -25.942421961251238, - -14.240779412656495, -4.664750489376498, 1.5018154527950844, 3.4004229646525492, - 1.3796678726375649, -2.808105554001277, -6.687767274410187, -8.13567855498191, -6.12884439030572, - -0.8262043451300531, 6.859440309828449, 15.802448405523107, 24.98580870233531, 33.52084350515457, - 40.57108640254275, 45.49546788702716, 48.30222273074625, 50.098423905112, 53.01632467678541, - 59.32535943323446, 69.9550778783712, 83.1403276665768, 94.09900368394518, 96.37900956455387, - 84.68921419948347, 57.95847304344338, 20.802149053089696, -17.803763987698026, -48.196705532248124, - -64.05334258054177, -64.52539583557882, -53.530984240916666, -37.054880133149666, - -20.28588637790632, -6.037502459403527, 5.236621851488619, 14.429454803492744, 22.583630638227632, - 30.059855067194608, 36.28274948376065, 40.048521484853396, 40.21129183479851, 36.41448004839188, - 29.48282895280681, 21.241935629479723, 13.846174199316279, 8.976443064217197, 7.319972032589732, - 8.539450931482968, 11.640541057875641, 15.464016816725664, 19.052580077895975, 21.801831228771135, - 23.45521829127891, 24.04836839944462, 23.85682801458122, 23.328355738533755, 22.95662251530825, - 23.093880115620454, 23.768324190695665, 24.609911144877767, 24.959281436177797, - 24.14588012687881, 21.821813902415503, 18.18841686850797, 13.989837373210776, 10.260083272073038, - 7.937434923165615, 7.5282945420541525, 8.969263320855546, 11.723481371537021, 15.02821752987984, - 18.1572033715646, 20.59278751893711, 22.079809495801744, 22.595320729800406, 22.283397926577717, - 21.382969371261755, 20.15189707836302, 18.78636053302327, 17.349778804688402, 15.741082327600221, - 13.729365865020657, 11.056538184639097, 7.574073974976007, 3.3554640075065287, -1.2707331581300847, - -5.789331729992047, -9.643806298237173, -12.399137384313326, -13.859913192763814, - -14.109234743701673, -13.471860394852694, -12.428508393514761, -11.508568062439753, - -11.177408307261295, -11.730428541239489, -13.215270504704932, -15.414360669766424, - -17.913581022513053, -20.25175587304091, -22.102247846902927, -23.40855647511527, - -24.403679701111763, -25.492052525849765, -27.042150597618175, -29.190856906328417, - -31.765243272451322, -34.37489458295107, -36.64080857906956, -38.44975246960033, - -40.10005022850334, -42.254614527413, -45.716546956729246, -51.13480418179141, -58.77622666461046, - -68.45192752900248, -79.59962098381037, -91.456567994907, -103.24154647862528, -114.28940218794558] diff --git a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_place_action/config/trajectory_weights/weights_table_place.yaml b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_place_action/config/trajectory_weights/weights_table_place.yaml index 6a97b7cbb..2bcb6488b 100644 --- a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_place_action/config/trajectory_weights/weights_table_place.yaml +++ b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_place_action/config/trajectory_weights/weights_table_place.yaml @@ -1,207 +1,1667 @@ -pitch: [-35.42493997049422, -37.70819524763274, -41.30898221809194, -46.91255259713179, - -54.67543853474187, -63.46993205757798, -70.71710907291556, -73.40838683036453, - -69.88965835948521, -61.23837968207681, -51.46297199294242, -46.651105552304536, - -53.51199582034473, -77.36775616098751, -119.40733708986616, -173.73781284783232, - -226.1456109740379, -256.9255230366232, -248.19097106023983, -192.5413353804086, - -97.86144890114835, 15.403191745759875, 123.77904323809733, 211.94253960392064, - 278.4370219521786, 331.32494006867165, 376.0838394153919, 404.99584445075004, 397.5504226626487, - 333.15522844035416, 207.40468267623098, 40.23505964106349, -129.5637520862881, -259.90824743283804, - -322.2438715398654, -311.38242615291654, -244.58025251479793, -151.69677985653905, - -62.27097378404166, 4.242548228062668, 41.72701342077948, 54.61937386645844, 52.0678236639199, - 42.46420503183177, 30.605603045097798, 17.831640535863865, 3.9115768409267537, -11.035226656357036, - -25.73694492619038, -38.23525698681982, -46.96747089197185, -51.55412317003241, - -52.72523403265943, -51.52230673716135, -48.47287389295395, -43.418549291861005, - -36.12710203132058, -27.217737390364878, -18.73250127108376, -13.940788921463144, - -16.383140380541224, -28.479814446323395, -50.19840934679473, -78.34588385548898, - -106.9796715308115, -129.0771227243504, -138.96143934459482, -134.39916286752, -117.2699501961774, - -92.4438455558197, -65.54260990887046, -40.863874703900365, -20.503006515797157, - -4.857673288780039, 6.139265967008193, 12.034840538128108, 11.78201008858928, 4.554178806755324, - -9.17603539972846, -27.06452970168944, -45.243054184153266, -59.42498559615973, - -66.20027581755308, -63.910105987553706, -52.76476291531106, -34.26469807990957, - -10.31714588108525, 17.428528508170896, 47.650311361280316, 78.83964513599082, 108.6479511556088, - 133.71712237097847, 150.35674122169067, 155.84935265330853, 149.667219171301, 133.86087612595915, - 112.3745444005185, 89.69640055960969, 69.59508619729118, 54.52489160570828, 45.78344309013908, - 44.04929137036943, 49.782033634194036, 63.15855610291111, 83.57913311970796, 109.088551720991, - 136.1606322569329, 160.14074686489496, 176.30498395284482, 181.14929667909337, 173.36150369292395, - 154.05941286281936, 126.24020279597528, 93.77525021113942, 60.472164188134116, 29.603993012435218, - 3.958702688619639, -13.885207810920182, -21.476471710992165, -16.848997943544642, - 0.7832249798221619, 30.385533864461852, 68.93627511091994, 111.90269031244195, 154.16251910598547, - 190.93981622752082, 218.36838869956537, 233.58094610071046, 234.54607891687763, - 220.01454742143613, 189.79459560980567, 145.2497513417973, 89.62834985791211, 27.797829306827328, - -34.782880289602524, -93.6044248049321, -146.58335798475517, -194.84023705533775, - -242.3963649186648, -294.83526437988365, -357.38490486635334, -433.09803303759264, - -521.7578411720901, -619.8377564762594, -721.4265148901123, -819.6866688138135, - -908.3015435028083, -982.4985157361981, -1039.4942245798738, -1078.4361122467722] -roll: [-94.9809024798238, -89.37326678075686, -74.67496646210842, -49.97877940131072, - -17.593122681262724, 16.06645308336598, 41.71318415962005, 50.60713287019311, 38.51880205462394, - 8.097319900299366, -31.598013501826173, -68.24707622586226, -90.81627135482617, - -93.60576994022517, -77.98514286453168, -51.10227044659737, -22.50696815786439, - -0.5347663027544345, 10.124158386418726, 9.150782393854067, -0.3095242652998163, - -13.68185030980892, -27.163152096763604, -39.030814562375696, -49.5151324263574, - -59.485126018140726, -68.80624552086546, -75.428898637592, -75.92665061062036, -67.34610061221686, - -49.32728657858986, -25.192438385029675, -1.2918864998652975, 15.141124521025375, - 18.667930530166085, 7.7185754208041795, -14.77455802286933, -42.4332819435094, -67.77813685589356, - -84.93421974950635, -91.54423345826856, -89.02137901209117, -81.13057995433573, - -71.8508278902564, -63.789491345075696, -57.835970205818235, -53.8231627143478, - -51.463590908332876, -50.94238773923665, -52.942655659661675, -58.189136134695445, - -66.74155669337485, -77.33961889552769, -87.14688594433058, -92.1973263131889, -88.62131828366893, - -74.32902174902947, -50.44322013852119, -21.67409515681044, 4.817148616819843, 21.806399412879404, - 24.68253720151413, 13.196955457999769, -8.512322146430131, -33.57118762493018, -54.88516872102151, - -67.45252093691383, -69.67085932958665, -63.35440572936342, -52.67299311155918, - -42.53041326451286, -36.97101954194654, -38.07581556440504, -45.58076611309898, - -57.22308967053014, -69.64023776501026, -79.50688077198373, -84.51353160768544, - -83.83257156823157, -77.93238694773984, -67.92280694397907, -54.86274891199053, - -39.45901730010299, -22.31036765250445, -4.4655218706363, 12.1844480635262, 25.094101784610512, - 31.88594109540416, 31.426514019400226, 24.561406129229585, 14.071733616471022, 3.8310309701905063, - -2.4667652078742877, -2.4910925874960017, 4.036658820887705, 15.329450275859665, - 28.159760716196264, 38.92003600168367, 44.79013991534497, 44.63012010630281, 39.28967211649084, - 31.232881080163466, 23.636257019694135, 19.304811972720493, 19.785502677120114, - 24.950320982296397, 33.14353632090505, 41.81125510618662, 48.398727074838696, 51.231113704687615, - 50.09924343700417, 46.35777463863367, 42.492653842074425, 41.28546877990567, 44.839129747983, - 53.785840236969825, 66.95276774389771, 81.62171724086008, 94.3240624864676, 101.92183810445113, - 102.60933075138487, 96.47920077162367, 85.44154179319236, 72.51515576452759, 60.73862809903186, - 52.084774877874715, 46.75751956690583, 43.11719157604091, 38.275825894925006, 29.196437849286824, - 13.971039246031658, -7.127902794056211, -31.207065658218177, -53.168507134733815, - -66.97618274894776, -67.45714153993094, -52.067059591180616, -21.977834522009797, - 17.97026793057221, 60.50618410267125, 97.94258360415643, 124.23605347540033, 136.35854432363232, - 134.57499001187503, 121.7126248618848, 101.89089552853724, 79.27247943521832, 57.21560075628847, - 37.928073989631585, 22.51410879818383] -x: [-23.54082186527676, -23.512905258800767, -23.516224768121276, -23.509127341780307, - -23.454121068234553, -23.37527958550504, -23.398062947937724, -23.714937560177475, - -24.478124554177448, -25.68503685748029, -27.12817117252123, -28.444857414576866, - -29.255164972260452, -29.32533200992744, -28.663538268050935, -27.485379784503554, - -26.068328960660104, -24.58356824095513, -23.00962769946186, -21.200239596466837, - -19.096858163949097, -16.95368487091472, -15.374423042071834, -15.062830081970883, - -16.420337211023146, -19.266187917922952, -22.863442272556544, -26.212763756336138, - -28.43414844400656, -29.064052770088523, -28.17268699560032, -26.28311891920491, - -24.142369984008027, -22.446184009232976, -21.626313157732994, -21.768723364314585, - -22.670561864739494, -23.99022574778685, -25.41383154390955, -26.762121364597242, - -27.994402597101168, -29.11804796936013, -30.06316239908554, -30.607870142419564, - -30.41937426536905, -29.21126868808186, -26.94246526112341, -23.943122787757076, - -20.87495709704674, -18.512790864664698, -17.435764167233458, -17.78028043031174, - -19.180496230067334, -20.915425140594678, -22.174498281441092, -22.3240925499991, - -21.103563664135123, -18.729923248158855, -15.895248710835736, -13.624755694929485, - -12.985496396331671, -14.71180186829232, -18.89175793950522, -24.869265613923826, - -31.43499308439889, -37.24493024420751, -41.28732213685973, -43.18410476572519, - -43.194339294574945, -41.951140068617846, -40.10893022819285, -38.10222626963581, - -36.10877330115395, -34.16247721797567, -32.287238408773284, -30.55543065783834, - -29.06037228818418, -27.855098125159977, -26.91524249105149, -26.147620843657243, - -25.42616007720281, -24.62192925623665, -23.60773081960084, -22.241953853405278, - -20.350308251339122, -17.723112367242447, -14.141853873202399, -9.449513334682687, - -3.6720025070654474, 2.8362755406261857, 9.303036300724449, 14.609576588230889, - 17.5737981092729, 17.371008465565403, 13.903729103835676, 7.920315318993102, 0.7993036840940881, - -5.90970951170443, -10.979658455755517, -13.829074454257093, -14.5755935725983, - -13.85183635092414, -12.500079190061692, -11.274722889530162, -10.641362881668742, - -10.704587515925214, -11.250922772773135, -11.867715076437035, -12.089807887886105, - -11.528372274762951, -9.948640513140584, -7.285309022869411, -3.611258670618167, - 0.9044519621289198, 6.012648194756425, 11.361212296824625, 16.45810776843166, 20.673183290896244, - 23.327284211933843, 23.874543031911173, 22.122886022700243, 18.39546579340212, 13.54233035939333, - 8.770094314293303, 5.339138669234294, 4.239493164175406, 5.965509978348073, 10.462318872054993, - 17.240025852914957, 25.58456988711478, 34.768784271394836, 44.19101560731903, 53.42265111802887, - 62.1967348811316, 70.38947366683362, 78.02811866838204, 85.31956397263224, 92.66238947803912, - 100.60176750352969, 109.7128630951549, 120.43768024118057, 132.93038931028732, 146.9707364427417, - 161.98099303697487, 177.14055334845793, 191.554949364158, 204.4214845608734, 215.14598259878588, - 223.39255572340488, 229.07401275593168] -y: [-48.08181651846675, -49.587117112576244, -50.9623587336631, -52.74638540311553, - -55.339316884123335, -58.187634767212785, -59.19858890679832, -55.18823159102758, - -43.45669279583084, -23.680143811777636, 1.0278742629116866, 24.803094657233476, - 41.56642405836774, 48.12670154484163, 45.66964260010242, 38.41392701146586, 30.559274820896626, - 24.01469229055469, 18.37242623227647, 12.461922135369557, 5.780911723761772, -1.2306825260415035, - -7.855647675106398, -13.850897756070554, -19.628125426393403, -25.766439690878475, - -32.25785903733505, -38.106735397658724, -41.700722135824165, -41.78689726002638, - -38.35450818167012, -32.77088479939355, -27.13457755859672, -23.383886152709735, - -22.698519255950604, -25.30103433852694, -30.459665680447525, -36.587875594965475, - -41.56956109814254, -43.41643845983297, -41.04609408947397, -34.71380717009749, - -25.770834909882954, -15.877657193068476, -6.18267876618329, 3.0042222833118606, - 11.840355394622723, 20.142092255935047, 26.74289840637122, 29.592807279096725, 26.707565820644888, - 17.456235617422454, 3.388730612913831, -11.925225381003171, -23.997617973752774, - -28.962264558977488, -24.922343000496628, -12.598312313006955, 4.970869040522485, - 23.575824375424354, 39.379440684218075, 50.19400929294163, 55.947550262636035, 58.225838746081386, - 59.27299512331673, 61.046406924836795, 64.7113242754852, 70.55113102435496, 78.04412796628496, - 85.97088366315737, 92.64894651093383, 96.42012688093448, 96.28287075303587, 92.33099685071431, - 85.70473506502236, 78.06766687875744, 70.92429970225633, 65.1466098402518, 60.88285588515454, - 57.769140305510135, 55.238497697808114, 52.76635428971193, 50.01647387605132, 46.93679691285544, - 43.83967985214271, 41.42191756324371, 40.63190289145288, 42.344396617716775, 46.9347863760049, - 53.95190906255407, 62.07786213730404, 69.43105232791753, 74.09941024378266, 74.68381768815975, - 70.63554962109797, 62.27282910008037, 50.51258663591012, 36.491359876837045, 21.304370351168693, - 6.013296228474457, -8.122423557377378, -19.438718195250082, -25.98893089734145, - -26.128853719153366, -19.38244812067696, -7.151684031278839, 7.173184693921752, - 18.886623201163076, 23.235809508225923, 16.81931017033493, -1.4029158802374324, - -29.8433104623529, -64.82075623228921, -101.63386062733136, -135.70233578562002, - -163.3136997983213, -181.76783190637622, -189.09357664198424, -183.78138421191431, - -164.93275730603818, -132.8760319352321, -89.87628487198717, -40.3823840377136, - 9.545608199456396, 53.52353674119033, 86.31999893025124, 104.89002936158305, 108.59148361413862, - 98.65485183597463, 77.326293537273, 47.16357178564959, 10.74497388276303, -29.265103349701626, - -69.94908118242546, -108.10390173253013, -140.4074927252729, -163.68216316743766, - -175.12064015744392, -172.44337814636657, -154.07896535435387, -119.47006083653432, - -69.47218086412778, -6.639348762292799, 64.85632773989234, 139.77404187801, 212.67787306545785, - 278.82692966691434, 334.7974511897336, 378.7116450336994, 410.118700189302] -yaw: [-53.959788987781124, -53.30371682466102, -50.30427376032306, -45.009643854796366, - -38.23858486542749, -31.445920900106113, -26.17708607783723, -23.44624165703102, - -23.41933783360354, -25.515961378948077, -28.81688930020335, -32.56774753973497, - -36.529787446838284, -40.964730323552274, -46.18201275969121, -51.81029740608197, - -56.21012654557379, -56.53909770605108, -49.73151173357274, -34.11186523750416, - -10.827794825167219, 15.891304347107724, 40.20849702734764, 57.368805169815346, - 65.95177865876515, 67.97159561584449, 66.64040895830699, 63.589047697786704, 57.82287342197275, - 47.11725574000715, 30.46733711609649, 9.533355760921287, -11.897470382363254, -29.830521402437533, - -41.96018651062839, -48.39143034359017, -50.90765467089368, -51.442211403866196, - -50.88101688407953, -48.972314263916196, -45.17504471685727, -39.55329712338891, - -32.95725845941995, -26.49322988683765, -20.878353476637844, -16.223447732915336, - -12.318730267752962, -9.114208916103852, -7.025318075714705, -6.863715409937331, - -9.418953881919258, -14.892711365810932, -22.46579817424915, -30.24344467810545, - -35.69109978319271, -36.477876243338805, -31.444025538076012, -21.288670881670154, - -8.62375068107963, 2.723377241865635, 8.950421398026423, 7.60130696452477, -1.5512568272241891, - -16.415907780851523, -33.32800892857806, -48.34061652494549, -58.544149663334146, - -62.8778987289918, -62.129401745774736, -58.221245342094605, -53.23124870684945, - -48.65125161003768, -45.139206829597285, -42.67849855974542, -40.883183302808774, - -39.24556037081911, -37.279870735621394, -34.610218273166865, -31.036388602506094, - -26.556600636656302, -21.314340237646586, -15.485545852961351, -9.182367311554216, - -2.4539357174518477, 4.608108661974362, 11.748942419186927, 18.53564938062657, 24.488775175064866, - 29.268641679431983, 32.76438623868329, 35.01433031937393, 36.04103332494177, 35.76699835677284, - 34.10630639596401, 31.16848412968319, 27.407761733176166, 23.587197726368412, 20.558872791588364, - 18.97592238631098, 19.07565937416213, 20.61709103744411, 22.979740291136025, 25.377603643755922, - 27.117925844469166, 27.82778995770433, 27.579205757414403, 26.87117155011749, 26.47346470212829, - 27.18502510687263, 29.588305367087866, 33.87866838785326, 39.818047979613915, 46.81614514744827, - 54.094984627640684, 60.861552883388576, 66.41686398223497, 70.17429953524406, 71.62539144890958, - 70.3348055057656, 66.03071453327549, 58.78227299675599, 49.168265349183585, 38.3031310449789, - 27.633806894374317, 18.533974132396466, 11.83869780529428, 7.516135260286569, 4.634765508402249, - 1.6752451183326715, -2.8928198989370846, -9.947988875516257, -19.190646683273787, - -28.95981792942289, -36.52500660659488, -38.85392526474802, -33.68717119866384, - -20.59129682191029, -1.5875034968128299, 18.968950912034025, 35.341880979175166, - 41.95032447278416, 34.96068739312236, 13.34321915944511, -20.982377451382774, -63.886780871889336, - -110.27067151277551, -155.29388790639618, -195.21850221882582, -227.74597004288862, - -251.94049712978926] -z: [-26.674573624633936, -27.114809002109638, -27.80093370552566, -28.836873185567413, - -30.21881883969757, -31.695951164601357, -32.72354364740953, -32.62183352655716, - -30.90258581567612, -27.583495683677555, -23.297201486579876, -19.087260962922908, - -15.936418055004204, -14.273620356202283, -13.80346926204938, -13.802500150724061, - -13.630619842081579, -13.020757497121616, -11.96881998725885, -10.469674381861992, - -8.452160823356074, -5.952178992153526, -3.225706585502589, -0.5697494663204837, - 1.9808009060413083, 4.68376066797161, 7.7212422921196655, 10.792547098296602, 13.147282962472149, - 14.103314291770715, 13.571455393667573, 12.085886522451826, 10.339296245986802, - 8.68602893983164, 7.033777346134439, 5.120448773462501, 2.8513690211156364, 0.4137469421774722, - -1.8591108055200387, -3.7053995669548203, -5.044430532720517, -5.89457353611374, - -6.160820814559367, -5.4596622601026645, -3.15564680899608, 1.3247372084788658, - 8.009198179393971, 15.980184925805682, 23.347897126275733, 27.749074489647064, 27.248379889872915, - 21.249227180863592, 10.952075671055974, -0.9012226404090096, -10.918446805389463, - -16.22596783788798, -15.453611074433008, -9.153655798428526, 0.49699749838585544, - 10.561454823123338, 18.391516558218946, 22.436392554843273, 22.545534532960506, - 19.747590276327738, 15.689361506049153, 11.99803586193934, 9.79057402569709, 9.448977960828584, - 10.667279692473349, 12.692684350192357, 14.645560060408101, 15.811942817737004, - 15.84203100562251, 14.829843387399771, 13.271968183018519, 11.913283989441497, 11.507542003036887, - 12.558104629802221, 15.134221452069113, 18.84592814794398, 22.998757891465335, 26.871030400509593, - 30.007884926015393, 32.42946843702773, 34.691007713089675, 37.77704102447371, 42.83936321417833, - 50.80550353800271, 61.91981744856823, 75.34762579946161, 89.04033240312027, 100.0468270773533, - 105.30303255039908, 102.67548082168979, 91.82574965851784, 74.46578861532257, 53.83412456102248, - 33.60637700701678, 16.731362600762733, 4.69041935031565, -2.5784558424742117, -6.19851466771825, - -7.652486552004426, -8.203701204838094, -8.60951161090784, -9.098233442799044, -9.48718196283113, - -9.346536633833383, -8.18047015987454, -5.61948916274534, -1.5868054321866703, 3.6274373814820575, - 9.426783721975054, 15.078581176949248, 19.909144614716634, 23.43487153892179, 25.38970214772102, - 25.6741236265473, 24.288580218363613, 21.306889896281145, 16.904641282521364, 11.411558969184892, - 5.333027801777911, -0.7017358032446038, -6.065575734065763, -10.287538553248018, - -13.167739669212384, -14.80357049337103, -15.524777375079806, -15.780272101184652, - -16.035519447260974, -16.72035793136546, -18.231003979213103, -20.961756875344914, - -25.33507117144374, -31.809284959656072, -40.85818085498114, -52.925248528929615, - -68.35651409024713, -87.31513845671434, -109.68639958664852, -134.99539426547764, - -162.3735258302758, -190.60811846102203, -218.28355820802886, -243.98292875048506, - -266.49124025702065, -284.9430654469704, -298.88475826361605, -308.25498187704557] +pitch: [-156.65718192265612, -156.82512813476856, -157.03476785128979, -157.2807477120766, + -157.55369295460645, -157.84461759860156, -158.14778763059294, -158.459737944698, + -158.77686003690096, -159.09532479965736, -159.41325989374954, -159.73163715474024, + -160.0521375845057, -160.3745530176387, -160.69700732848457, -161.0185104942475, + -161.3402653767862, -161.66397674111627, -161.98953153448417, -162.315201866925, + -162.64003295077077, -162.96511555690367, -163.29205326830424, -163.62078022025358, + -163.9496971026068, -164.27788257902984, -164.60632279261657, -164.93652376742696, + -165.26846111422094, -165.60065680035294, -165.93222401150882, -166.26405206032558, + -166.5975532366255, -166.9327393757321, -167.2682466574162, -167.60322357479959, + -167.93847000443222, -168.27530835178302, -168.61378183900877, -168.95263403217712, + -169.29104925446183, -169.62974494472823, -169.9699574813401, -170.31175703012775, + -170.6539879618333, -170.99587071205337, -171.33804689251744, -171.68167070253227, + -172.02683518428148, -172.37247918025702, -172.71785930237908, -173.06354756695924, + -173.4106198177865, -173.75918826314913, -174.10828013599635, -174.45718809095524, + -174.8064204116356, -175.1569783712803, -175.50898997238295, -175.86156501042154, + -176.21403187168687, -176.56684061134436, -176.9209216656706, -177.276415779213, + -177.63250973601328, -177.9885671847553, -178.34498510912093, -178.7026267789968, + -179.0616429301755, -179.42129201479344, -179.7809723347144, -180.14103262348885, + -180.50227258176474, -180.86485046896803, -181.2280913368986, -181.59142740879287, + -181.95516366594228, -182.3200397542152, -182.6862192544363, -183.0530889992954, + -183.42011429540196, -183.78756055865932, -184.15611080378343, -184.525931978699, + -184.8964681246396, -185.2672167028437, -185.638407452449, -186.01067008275334, + -186.3841731854121, -186.75841368027824, -187.132920178221, -187.50789034493104, + -187.88390380611247, -188.26112928818085, -188.6391124973969, -189.01741212654568, + -189.39619709895, -189.7760000696095, -190.15698858912273, -190.53875329031354, + -190.92088183004392, -191.30351746122224, -191.68714886802226, -192.07194129758645, + -192.4575266759198, -192.8435204676572, -193.23004308121793, -193.617542113636, + -194.0061795490326, -194.39562519327265, -194.78552113473754, -195.17596753027587, + -195.56737365493905, -195.95989742408074, -196.35324332333818, -196.74707886293638, + -197.14148632095296, -197.53683929553736, -197.933290967728, -198.33057750888972, + -198.7283906402854, -199.12679692660544, -199.52613681329163, -199.92655820874447, + -200.32782617456402, -200.72965543146975, -201.13209880120448, -201.53546597968077, + -201.93989917925003, -202.34518974707686, -202.751074198292, -203.1575933993827, + -203.56502857939287, -203.97351593447695, -204.38287067560304, -204.7928499203278, + -205.20348419671345, -205.6150284301484, -206.0276125727257, -206.44107345232243, + -206.85518761577225, -207.26997671022124, -207.68567140275596, -208.10239525551458, + -208.5200046331361, -208.93829436247796, -209.35727851912242, -209.77716544140384, + -210.19807222793216, -210.61987285855759, -211.04237931918513, -211.4655992857971, + -211.8897205841901, -212.3148538391931, -212.74088887477996, -213.16765374694393, + -213.5951507769902, -214.02354898389035, -214.45295256340523, -214.8832655549256, + -215.31433103073036, -215.74614688524275, -216.1788649289692, -216.61258302054992, + -217.04721792048107, -217.48262670125752, -217.91880365055158, -218.3558848648318, + -218.79396199767987, -219.23296316292127, -219.67275845698202, -220.11333928225932, + -220.5548274153216, -220.99730847034053, -221.44072066552712, -221.88494618630875, + -222.32997418117233, -222.77591340446227, -223.22284362421576, -223.67071202540126, + -224.119411989995, -224.56893096190808, -225.01936587844727, -225.4707908770044, + -225.9231610756851, -226.37638020375584, -226.83043447547186, -227.2854101278763, + -227.7413759005292, -228.1982939079816, -228.65607742107326, -229.11471183206265, + -229.57427371024147, -230.0348266430826, -230.4963388949898, -230.95873251621126, + -231.42199242410942, -231.88618647266296, -232.35137335201267, -232.81752671335113, + -233.2845766674393, -233.7525079495373, -234.22138057487498, -234.69124859655113, + -235.16209036671597, -235.6338433804674, -236.1064924352654, -236.58009051246535, + -237.05468729088872, -237.53026520903265, -238.00676851209414, -238.48418226093696, + -238.9625531403663, -239.44192671749772, -239.92228896806165, -240.40359029407247, + -240.88581618288202, -241.3690076965991, -241.85320655070794, -242.33840176912216, + -242.8245493571952, -243.31163535831513, -243.79969582627467, -244.2887688805353, + -244.77884615907135, -245.26988875560264, -245.7618833697687, -246.2548616058494, + -246.74885823676817, -247.2438671305236, -247.73985399131797, -248.23680624976333, + -248.7347515676374, -249.23372161331298, -249.73371214631064, -250.2346930390105, + -250.73665250571727, -251.23961472458112, -251.74360849280094, -252.2486311641883, + -252.75465637099336, -253.2616731450968, -253.7697025952815, -254.27877087145927, + -254.78887666179244, -255.29999698245618, -255.81212170080903, -256.3252692292884, + -256.8394632842479, -257.3547036618484, -257.87097041693903, -258.3882542568401, + -258.9065712326523, -259.4259428302655, -259.94636975763615, -260.4678347920479, + -260.99032947413946, -261.5138677937398, -262.0384691984253, -262.564135138717, + -263.09085082541867, -263.6186086167544, -264.14742070931345, -264.677304693403, + -265.208262616922, -265.7402818609308, -266.27335557821664, -266.807494410878, -267.3427142618617, + -267.8790176526088, -268.4163938951752, -268.9548369081826, -269.4943559912946, + -270.0349655189502, -270.5766683811865, -271.11945560417905, -271.6633218393314, + -272.20827523166446, -272.75432877508274, -273.3014856399149, -273.84973837039325, + -274.3990823145224, -274.94952462848454, -275.50107706299923, -276.0537429949804, + -276.6075163099427, -277.16239301421695, -277.7183794210768, -278.27548616510694, + -278.8337167688488, -279.39306630014715, -279.95353138416385, -280.5151176192923, + -281.07783464110923, -281.64168606790315, -282.2066680073117, -282.77277766335567, + -283.3400200314943, -283.90840385592054, -284.47793281036434, -285.0486039147941, + -285.6204149122557, -286.19337029282093, -286.7674780078702, -287.3427417545014, + -287.91915935135097, -288.4967290413007, -289.07545489372984, -289.655344157199, + -290.2364005271319, -290.81862251976554, -291.40200883968265, -291.986563208829, + -292.57229225484906, -293.15919965241676, -293.7472845257627, -294.3365460044106, + -294.9269875259936, -295.51861517154896, -296.1114325809504, -296.70543940721166, + -297.3006351696594, -297.89702307577414, -298.49460872719874, -299.0933957191521, + -299.69338416362336, -300.29457393640496, -300.8969680610969, -301.50057172055426, + -302.10538845895877, -302.71141878594295, -303.31866290235337, -303.92712368726046, + -304.53680595921657, -305.14771320782285, -305.7598462866421, -306.3732056921633, + -306.98779419222967, -307.6036162899247, -308.2206754190174, -308.83897273011434, + -309.4585089879684, -310.0792868772328, -310.70131062915857, -311.3245836222539, + -311.94910726337764, -312.5748825602012, -313.20191213766174, -313.830199994053, + -314.45974945461137, -315.0905621470852, -315.72263929872355, -316.3559834942804, + -316.9905985336233, -317.62648769178344, -318.2636527868513, -318.9020952442645, + -319.5418176247443, -320.18282356031045, -320.8251162796443, -321.4686977648914, + -322.11356962017334, -322.7597343954331, -323.4071955818418, -324.0559563661364, + -324.7060188719847, -325.35738486448656, -326.01005689360136, -326.6640383334178, + -327.3193323334846, -327.97594113975754, -328.63386666231656, -329.29311145984894, + -329.95367881022037, -330.6155718307366, -331.2787928732944, -331.94334397856255, + -332.6092277209164, -333.2764473002516, -333.9450058066361, -334.6149056840792, + -335.286149090949, -335.9587386228065, -336.6326774175029, -337.3079685428293, -337.9846145232684, + -338.6626176233944, -339.3419804642377, -340.022706135458, -340.7047976874095, -341.3882577153029, + -342.07308857971486, -342.75929293043066, -343.4468738209348, -344.1358342888007, + -344.826176991857, -345.5179043776647, -346.2110191272336, -346.90552426826605, + -347.601422829986, -348.29871752613354, -348.99741088331945, -349.6975056155889, + -350.3990047338262, -351.1019112630828, -351.80622796750447, -352.51195744580275, + -353.21910244634364, -353.9276659709034, -354.63765104426665, -355.3490604765017, + -356.0618969323631, -356.7761631954104, -357.4918622649251, -358.2089971690505, + -358.927570760162, -359.64758576380353, -360.36904499927863, -361.09195146903556, + -361.8163082079192, -362.5421181077289, -363.2693839502654, -363.99810859088376, + -364.72829504003204, -365.4599463423245, -366.1930654267157, -366.9276551273741, + -367.6637183358344, -368.4012580746632, -369.14027740104433, -369.88077927933233, + -370.6227665927477, -371.3662422690045, -372.11120934629133, -372.8576708969081, + -373.60562991928094, -374.3550893428728, -375.1060521314913, -375.85852134192436, + -376.6125000638935, -377.3679913289216, -378.12499811035156, -378.88352340794614, + -379.64357029961894, -380.4051418945974, -381.1682412568148, -381.9328714015242, + -382.69903536427796, -383.46673624626146, -384.2359771780845, -385.0067612556417, + -385.7790915344696, -386.5529710857383, -387.32840303572766, -388.1053905381184, + -388.8839367205077, -389.66404467738937, -390.4457175153868, -391.2289583874262, + -392.0137704717747, -392.8001569276271, -393.58812088735715, -394.37766549287124, + -395.1687939249686, -395.96150938755676, -396.7558150704628, -397.55171414010636, + -398.3492097647753, -399.1483051294121, -399.94900339695914, -400.7513076097339, + -401.55522051642305, -402.3607442126537, -403.1678793129748, -403.97662313703535, + -404.78696601570533, -405.5988841676912, -406.41232657422364, -407.22719193743313, + -408.04329038092243, -408.86028352164305, -409.67759674852243, -410.49430019593217, + -411.30896127385165, -412.11948227644524, -412.9229503097521, -413.71553969665734, + -414.49251289191534, -415.2483582419702, -415.9770785993685, -416.6726079072575, + -417.329295131207, -417.94237131855783, -418.5083166266741, -419.02506982614034, + -419.4920624207183, -419.91009801422723, -420.28112301456906, -420.60794297702034, + -420.89393277454684] +roll: [-156.65718192265612, -156.82512813476856, -157.03476785128979, -157.2807477120766, + -157.55369295460645, -157.84461759860156, -158.14778763059294, -158.459737944698, + -158.77686003690096, -159.09532479965736, -159.41325989374954, -159.73163715474024, + -160.0521375845057, -160.3745530176387, -160.69700732848457, -161.0185104942475, + -161.3402653767862, -161.66397674111627, -161.98953153448417, -162.315201866925, + -162.64003295077077, -162.96511555690367, -163.29205326830424, -163.62078022025358, + -163.9496971026068, -164.27788257902984, -164.60632279261657, -164.93652376742696, + -165.26846111422094, -165.60065680035294, -165.93222401150882, -166.26405206032558, + -166.5975532366255, -166.9327393757321, -167.2682466574162, -167.60322357479959, + -167.93847000443222, -168.27530835178302, -168.61378183900877, -168.95263403217712, + -169.29104925446183, -169.62974494472823, -169.9699574813401, -170.31175703012775, + -170.6539879618333, -170.99587071205337, -171.33804689251744, -171.68167070253227, + -172.02683518428148, -172.37247918025702, -172.71785930237908, -173.06354756695924, + -173.4106198177865, -173.75918826314913, -174.10828013599635, -174.45718809095524, + -174.8064204116356, -175.1569783712803, -175.50898997238295, -175.86156501042154, + -176.21403187168687, -176.56684061134436, -176.9209216656706, -177.276415779213, + -177.63250973601328, -177.9885671847553, -178.34498510912093, -178.7026267789968, + -179.0616429301755, -179.42129201479344, -179.7809723347144, -180.14103262348885, + -180.50227258176474, -180.86485046896803, -181.2280913368986, -181.59142740879287, + -181.95516366594228, -182.3200397542152, -182.6862192544363, -183.0530889992954, + -183.42011429540196, -183.78756055865932, -184.15611080378343, -184.525931978699, + -184.8964681246396, -185.2672167028437, -185.638407452449, -186.01067008275334, + -186.3841731854121, -186.75841368027824, -187.132920178221, -187.50789034493104, + -187.88390380611247, -188.26112928818085, -188.6391124973969, -189.01741212654568, + -189.39619709895, -189.7760000696095, -190.15698858912273, -190.53875329031354, + -190.92088183004392, -191.30351746122224, -191.68714886802226, -192.07194129758645, + -192.4575266759198, -192.8435204676572, -193.23004308121793, -193.617542113636, + -194.0061795490326, -194.39562519327265, -194.78552113473754, -195.17596753027587, + -195.56737365493905, -195.95989742408074, -196.35324332333818, -196.74707886293638, + -197.14148632095296, -197.53683929553736, -197.933290967728, -198.33057750888972, + -198.7283906402854, -199.12679692660544, -199.52613681329163, -199.92655820874447, + -200.32782617456402, -200.72965543146975, -201.13209880120448, -201.53546597968077, + -201.93989917925003, -202.34518974707686, -202.751074198292, -203.1575933993827, + -203.56502857939287, -203.97351593447695, -204.38287067560304, -204.7928499203278, + -205.20348419671345, -205.6150284301484, -206.0276125727257, -206.44107345232243, + -206.85518761577225, -207.26997671022124, -207.68567140275596, -208.10239525551458, + -208.5200046331361, -208.93829436247796, -209.35727851912242, -209.77716544140384, + -210.19807222793216, -210.61987285855759, -211.04237931918513, -211.4655992857971, + -211.8897205841901, -212.3148538391931, -212.74088887477996, -213.16765374694393, + -213.5951507769902, -214.02354898389035, -214.45295256340523, -214.8832655549256, + -215.31433103073036, -215.74614688524275, -216.1788649289692, -216.61258302054992, + -217.04721792048107, -217.48262670125752, -217.91880365055158, -218.3558848648318, + -218.79396199767987, -219.23296316292127, -219.67275845698202, -220.11333928225932, + -220.5548274153216, -220.99730847034053, -221.44072066552712, -221.88494618630875, + -222.32997418117233, -222.77591340446227, -223.22284362421576, -223.67071202540126, + -224.119411989995, -224.56893096190808, -225.01936587844727, -225.4707908770044, + -225.9231610756851, -226.37638020375584, -226.83043447547186, -227.2854101278763, + -227.7413759005292, -228.1982939079816, -228.65607742107326, -229.11471183206265, + -229.57427371024147, -230.0348266430826, -230.4963388949898, -230.95873251621126, + -231.42199242410942, -231.88618647266296, -232.35137335201267, -232.81752671335113, + -233.2845766674393, -233.7525079495373, -234.22138057487498, -234.69124859655113, + -235.16209036671597, -235.6338433804674, -236.1064924352654, -236.58009051246535, + -237.05468729088872, -237.53026520903265, -238.00676851209414, -238.48418226093696, + -238.9625531403663, -239.44192671749772, -239.92228896806165, -240.40359029407247, + -240.88581618288202, -241.3690076965991, -241.85320655070794, -242.33840176912216, + -242.8245493571952, -243.31163535831513, -243.79969582627467, -244.2887688805353, + -244.77884615907135, -245.26988875560264, -245.7618833697687, -246.2548616058494, + -246.74885823676817, -247.2438671305236, -247.73985399131797, -248.23680624976333, + -248.7347515676374, -249.23372161331298, -249.73371214631064, -250.2346930390105, + -250.73665250571727, -251.23961472458112, -251.74360849280094, -252.2486311641883, + -252.75465637099336, -253.2616731450968, -253.7697025952815, -254.27877087145927, + -254.78887666179244, -255.29999698245618, -255.81212170080903, -256.3252692292884, + -256.8394632842479, -257.3547036618484, -257.87097041693903, -258.3882542568401, + -258.9065712326523, -259.4259428302655, -259.94636975763615, -260.4678347920479, + -260.99032947413946, -261.5138677937398, -262.0384691984253, -262.564135138717, + -263.09085082541867, -263.6186086167544, -264.14742070931345, -264.677304693403, + -265.208262616922, -265.7402818609308, -266.27335557821664, -266.807494410878, -267.3427142618617, + -267.8790176526088, -268.4163938951752, -268.9548369081826, -269.4943559912946, + -270.0349655189502, -270.5766683811865, -271.11945560417905, -271.6633218393314, + -272.20827523166446, -272.75432877508274, -273.3014856399149, -273.84973837039325, + -274.3990823145224, -274.94952462848454, -275.50107706299923, -276.0537429949804, + -276.6075163099427, -277.16239301421695, -277.7183794210768, -278.27548616510694, + -278.8337167688488, -279.39306630014715, -279.95353138416385, -280.5151176192923, + -281.07783464110923, -281.64168606790315, -282.2066680073117, -282.77277766335567, + -283.3400200314943, -283.90840385592054, -284.47793281036434, -285.0486039147941, + -285.6204149122557, -286.19337029282093, -286.7674780078702, -287.3427417545014, + -287.91915935135097, -288.4967290413007, -289.07545489372984, -289.655344157199, + -290.2364005271319, -290.81862251976554, -291.40200883968265, -291.986563208829, + -292.57229225484906, -293.15919965241676, -293.7472845257627, -294.3365460044106, + -294.9269875259936, -295.51861517154896, -296.1114325809504, -296.70543940721166, + -297.3006351696594, -297.89702307577414, -298.49460872719874, -299.0933957191521, + -299.69338416362336, -300.29457393640496, -300.8969680610969, -301.50057172055426, + -302.10538845895877, -302.71141878594295, -303.31866290235337, -303.92712368726046, + -304.53680595921657, -305.14771320782285, -305.7598462866421, -306.3732056921633, + -306.98779419222967, -307.6036162899247, -308.2206754190174, -308.83897273011434, + -309.4585089879684, -310.0792868772328, -310.70131062915857, -311.3245836222539, + -311.94910726337764, -312.5748825602012, -313.20191213766174, -313.830199994053, + -314.45974945461137, -315.0905621470852, -315.72263929872355, -316.3559834942804, + -316.9905985336233, -317.62648769178344, -318.2636527868513, -318.9020952442645, + -319.5418176247443, -320.18282356031045, -320.8251162796443, -321.4686977648914, + -322.11356962017334, -322.7597343954331, -323.4071955818418, -324.0559563661364, + -324.7060188719847, -325.35738486448656, -326.01005689360136, -326.6640383334178, + -327.3193323334846, -327.97594113975754, -328.63386666231656, -329.29311145984894, + -329.95367881022037, -330.6155718307366, -331.2787928732944, -331.94334397856255, + -332.6092277209164, -333.2764473002516, -333.9450058066361, -334.6149056840792, + -335.286149090949, -335.9587386228065, -336.6326774175029, -337.3079685428293, -337.9846145232684, + -338.6626176233944, -339.3419804642377, -340.022706135458, -340.7047976874095, -341.3882577153029, + -342.07308857971486, -342.75929293043066, -343.4468738209348, -344.1358342888007, + -344.826176991857, -345.5179043776647, -346.2110191272336, -346.90552426826605, + -347.601422829986, -348.29871752613354, -348.99741088331945, -349.6975056155889, + -350.3990047338262, -351.1019112630828, -351.80622796750447, -352.51195744580275, + -353.21910244634364, -353.9276659709034, -354.63765104426665, -355.3490604765017, + -356.0618969323631, -356.7761631954104, -357.4918622649251, -358.2089971690505, + -358.927570760162, -359.64758576380353, -360.36904499927863, -361.09195146903556, + -361.8163082079192, -362.5421181077289, -363.2693839502654, -363.99810859088376, + -364.72829504003204, -365.4599463423245, -366.1930654267157, -366.9276551273741, + -367.6637183358344, -368.4012580746632, -369.14027740104433, -369.88077927933233, + -370.6227665927477, -371.3662422690045, -372.11120934629133, -372.8576708969081, + -373.60562991928094, -374.3550893428728, -375.1060521314913, -375.85852134192436, + -376.6125000638935, -377.3679913289216, -378.12499811035156, -378.88352340794614, + -379.64357029961894, -380.4051418945974, -381.1682412568148, -381.9328714015242, + -382.69903536427796, -383.46673624626146, -384.2359771780845, -385.0067612556417, + -385.7790915344696, -386.5529710857383, -387.32840303572766, -388.1053905381184, + -388.8839367205077, -389.66404467738937, -390.4457175153868, -391.2289583874262, + -392.0137704717747, -392.8001569276271, -393.58812088735715, -394.37766549287124, + -395.1687939249686, -395.96150938755676, -396.7558150704628, -397.55171414010636, + -398.3492097647753, -399.1483051294121, -399.94900339695914, -400.7513076097339, + -401.55522051642305, -402.3607442126537, -403.1678793129748, -403.97662313703535, + -404.78696601570533, -405.5988841676912, -406.41232657422364, -407.22719193743313, + -408.04329038092243, -408.86028352164305, -409.67759674852243, -410.49430019593217, + -411.30896127385165, -412.11948227644524, -412.9229503097521, -413.71553969665734, + -414.49251289191534, -415.2483582419702, -415.9770785993685, -416.6726079072575, + -417.329295131207, -417.94237131855783, -418.5083166266741, -419.02506982614034, + -419.4920624207183, -419.91009801422723, -420.28112301456906, -420.60794297702034, + -420.89393277454684] +x: [-131.00261478679988, -121.88886417422353, -111.9050389833139, -102.61368963497223, + -96.05947080078046, -94.12535169815399, -97.8607435820271, -107.14613364957096, + -120.82659085972966, -137.19720078410953, -154.5163549178483, -171.18694825080595, + -185.5568218223094, -195.704691059387, -199.6329868378576, -195.91936207338776, + -184.48915098542318, -167.07793507920195, -147.0574840422674, -128.53128958566316, + -115.04358889413827, -108.56400855374055, -109.19561099927448, -115.57413755530115, + -125.64926844955686, -137.46655539270932, -149.61816709281686, -161.26501144566066, + -171.91636446335687, -181.2216960780747, -188.87007846616464, -194.53289513944736, + -197.79145167358277, -198.0996571089822, -194.8802396222356, -187.78401698176944, + -177.02826515891735, -163.64756841648978, -149.46450567572836, -136.67890898964777, + -127.21395723237792, -122.15440349976664, -121.54367242044958, -124.56838026444774, + -129.9847428341204, -136.57776680869415, -143.4619977851926, -150.1463172881614, + -156.43047062938794, -162.25579819785062, -167.580799896415, -172.2857363783464, + -176.10313080152142, -178.60602094331426, -179.29466139771722, -177.77366590562514, + -173.94835807935246, -168.14763364573156, -161.10576879062413, -153.79437054295704, + -147.18005615509452, -142.03544698559352, -138.8812834694729, -138.0254327793118, + -139.60154077046317, -143.53795283987066, -149.46190564030064, -156.61307764750555, + -163.87292592523232, -169.97827533844975, -173.87402597845175, -175.0512378675456, + -173.7109894894428, -170.68112972530366, -167.11714893622806, -164.10386427943158, + -162.31622495964814, -161.86114761154576, -162.33151047253835, -163.02192881838113, + -163.2095501914243, -162.39248606696069, -160.40951035619491, -157.42206476020834, + -153.79159590063577, -149.91799716532543, -146.1123968434442, -142.55104990302257, + -139.31149673683075, -136.46131741930625, -134.16539879470983, -132.7805579230332, + -132.8976720404706, -135.27842864395015, -140.64645739243105, -149.3508159834204, + -161.00381971456645, -174.2605798886825, -186.9134811438676, -196.38261868432437, + -200.49211090980378, -198.23763830134754, -190.21480773205266, -178.51115567595826, + -166.0801280692791, -155.83164235826004, -149.80028431814193, -148.6986554765198, + -151.96202945833986, -158.18027510739103, -165.68937034330935, -173.07927043758525, + -179.46192687105753, -184.482441275634, -188.1614046703995, -190.6833573748501, + -192.21809683888495, -192.81960540582097, -192.41403431825069, -190.86577715074623, + -188.09301880254696, -184.18878513004086, -179.4955981906823, -174.5892113462465, + -170.1561255168205, -166.79776163996192, -164.8373602238494, -164.2126907946925, + -164.50294621069565, -165.0839384166419, -165.3557930296701, -164.95884643563477, + -163.898412256587, -162.534820649653, -161.44472376807636, -161.20582442689792, + -162.18450627688458, -164.40124437891336, -167.51392693816024, -170.91206169710102, + -173.8749652264376, -175.73083709030058, -175.97063351490164, -174.31004528380663, + -170.72511032331406, -165.48897950595216, -159.20946482285498, -152.82808750163824, + -147.5191667738631, -144.44886967752777, -144.4240273588793, -147.54449659368146, + -153.01617480666846, -159.25176211818498, -164.2877168274527, -166.41140144219656, + -164.78014816559207, -159.78674905727087, -153.00534915065177, -146.70549554069234, + -143.09178186665255, -143.5446658872191, -148.14637102277132, -155.6663204124734, + -164.00416538606794, -170.916565992789, -174.75161613413627, -174.9259359923934, + -171.99663265443107, -167.34336788125873, -162.6147423868518, -159.16048671989986, + -157.6477458947188, -157.96425018036857, -159.39300876091175, -160.95217491299363, + -161.7572333232152, -161.28442609094157, -159.4763125555903, -156.69932273396668, + -153.6090737832136, -150.9888364227075, -149.60413892450302, -150.0809012374839, + -152.79153608790728, -157.7397093479904, -164.46467355593433, -172.0172934256874, + -179.0661158741304, -184.15783677719142, -186.08855140295393, -184.27238379689192, + -178.96525805487465, -171.2372865037682, -162.67650112956372, -154.91277319136154, + -149.12741792008617, -145.72381647659327, -144.27134285886908, -143.72945580899287, + -142.85723879551645, -140.65447875831518, -136.68276519662683, -131.17312406130938, + -124.91209567953047, -118.9751543144055, -114.4168612960534, -112.01955243552163, + -112.15689719095518, -114.77332263747567, -119.44360198084003, -125.471236872446, + -132.00253791160844, -138.1561515203214, -143.17484043177828, -146.58907912429945, + -148.3511438380617, -148.87742279194816, -148.94591893398302, -149.43982973654823, + -150.99302147422435, -153.65215284576814, -156.69307614107936, -158.6972973355033, + -157.91096745292066, -152.8004175957856, -142.62726270976202, -127.83568739093043, + -110.09574251961111, -91.96442744166842, -76.26517339306562, -65.38756088681923, + -60.727455422828086, -62.41769838627875, -69.38103902919622, -79.63028303419432, + -90.6904492501862, -100.0330543381811, -105.46650890222921, -105.47370228165889, + -99.49440285791094, -88.11732926351014, -73.10854974665124, -57.20048636703596, + -43.619919989560685, -35.42935377805215, -34.84789242860901, -42.75227348465364, + -58.50946410168745, -80.17821602052324, -104.99065766337972, -129.94273353227604, + -152.31614487459336, -170.01709776381944, -181.70800379508364, -186.78019457154642, + -185.24347046638363, -177.59665180543396, -164.71393092910603, -147.75511927137686, + -128.09259386421706, -107.2415258381779, -86.77724029789665, -68.22375027449615, + -52.90610923413581, -41.779927174503385, -35.27752870209294, -33.22579478747385, + -34.88146090859914, -39.094144062824924, -44.55933540558859, -50.084953618202576, + -54.78496998250598, -58.13877584276208, -59.90622409641475, -59.945540069741, -58.02266713890794, + -53.710211558038985, -46.44646056518766, -35.76628477961155, -21.64346099537084, + -4.823099943406338, 13.00074336490228, 29.276996809341675, 40.992025758372115, 45.29133769691425, + 40.16014663983825, 24.958013623469462, 0.6430403690297408, -30.37648983818459, -64.70364605521273, + -98.5780766753714, -128.49010205980096, -151.64916889145232, -166.24865624155242, + -171.54102444522545, -167.77666186080413, -156.06265128020192, -138.17902607586348, + -116.36917642692488, -93.10932192371236, -70.85982959536072, -51.803566443571675, + -37.58102962803787, -29.04191085373232, -26.05140387409622, -27.411942444947794, + -30.971298702656785, -33.967139701487234, -33.59780771242022, -27.721946232536823, + -15.50944865087424, 2.165242029053751, 22.75109265450623, 42.51462881068035, 57.39178500711525, + 63.9603626762866, 60.24680348570627, 46.13482203280333, 23.280449691574223, -5.396531697017779, + -36.44546991778746, -66.59572190185905, -93.244393312115, -114.63002544381644, -129.7277803714285, + -137.99626292368302, -139.13919014515983, -133.01230035499455, -119.72497935278233, + -99.88864280213238, -74.88535670756845, -46.99987777401981, -19.290622507018494, + 4.836900082983619, 22.275753850864515, 30.9051736945987, 30.102091149122813, 20.89767984126534, + 5.709426552642763, -12.280539670085691, -29.912602539696113, -44.76409817954616, + -55.587777836517255, -62.37400184312159, -66.06509298635545, -68.05618356119531, + -69.6650415443935, -71.73438308006683, -74.46044693767506, -77.45330286152975, -79.96093287153582, + -81.15272296993173, -80.36351741010003, -77.23600916293604, -71.74756168638527, + -64.14841195780141, -54.85947253693819, -44.377179252428895, -33.21572713174938, + -21.893453397774184, -10.949679527408357, -0.9673128756345823, 7.423219742995925, + 13.573678927669173, 16.882316200266693, 16.880518451590333, 13.321463895553073, + 6.2494544216467425, -3.9662652022608986, -16.64105804227179, -30.827458079441, -45.40427423165941, + -59.19176164715269, -71.07926973763034, -80.1486617898492, -85.77441240031021, -87.6818651318595, + -85.95088814283162, -80.96333944498441, -73.30666637565525, -63.65782924774226, + -52.676828368491805, -40.93506347992221, -28.891766117399765, -16.916080004322772, + -5.3384835550584935, 5.4924058263232025, 15.171162899221802, 23.231051427618734, + 29.178380505050853, 32.559117153019926, 33.042995254450894, 30.506717141912656, + 25.098309221713915, 17.267877434957573, 7.754667766136128, -2.4738852870074073, + -12.328160893560188, -20.73549791063537, -26.79110541102302, -29.892155231726377, + -29.825103487837175, -26.78324113785426, -21.306403690118064, -14.15394397582623, + -6.139545262622449, 2.0335265366227753, 9.89870331065091, 17.286397543623664, 24.307308641748374, + 31.26525601401877, 38.52026696289829, 46.33161129748563, 54.71416332554038, 63.34061174435135, + 71.51679336934421, 78.24704133338132, 82.39018658725793, 82.88576123321361, 79.00811170033668, + 70.59040844420252, 58.15831519340492, 42.928737491281964, 26.660959489767816, 11.387156505595188, + -0.9161323493847441, -8.636955778598946, -10.746100985455664, -6.88302166146115, + 2.6901550272644528, 17.227609362490647, 35.67099777743684, 56.793973316018906, 79.28492204264502, + 101.76799145249652, 122.79490061004348, 140.8566568494178, 154.45830822835748, 162.2730575641024, + 163.35441171879694, 157.35115570417733, 144.6537822686842, 126.41052162787045, 104.38486553091724, + 80.67370587471055, 57.34955312756698, 36.11589971154659, 18.06234868471712, 3.576456180471944, + -7.5771785605298545, -16.047264197592554, -22.625071287068153, -27.99125205987563, + -32.544187050649704, -36.342770320132814, -39.160889188810124, -40.621845381417664, + -40.36566724147911, -38.20206823583871, -34.21329090178294, -28.788359919239177, + -22.587615544563256, -16.449965238654954, -11.263594052219574, -7.82420704968046, + -6.7043106382969215, -8.153633537426817, -12.045315570129013, -17.87558753135798, + -24.817072522162295, -31.818486356751524, -37.73735081273553, -41.48808157884077, + -42.185843095170476, -39.266966337888135, -32.56948414179687, -22.362390213218752, + -9.319317728622229, 5.559271102094557, 21.062973392548795, 35.90860732266084, 48.87612268299202, + 58.92359238196129, 65.26758898268541, 67.42353430763573, 65.20869447959286, 58.716140928984274, + 48.27012084952794, 34.372265068707556, 17.645386644363096, -1.221017147956034, -21.52101235133283, + -42.57613526226664, -63.77089174961588, -84.5781075128901, -104.57331194568057, + -123.43864361815416, -140.9579908724867, -157.005848574077] +y: [-99.19714419940159, -79.16394233266908, -57.87218080909418, -39.28438472983111, + -28.386800564847263, -29.413961436687575, -43.913473953576144, -69.67422837210728, + -101.14133069492664, -131.32148548272554, -154.36897298960562, -167.56000537729983, + -171.8480811942197, -171.05140505400772, -170.15303380273187, -173.34724789149277, + -182.51652550260764, -196.6077612179733, -212.0578803921079, -224.1551729270074, + -228.85174829123696, -224.25993586587964, -211.25206790585622, -193.0143316349947, + -173.74730833399678, -157.0391212367104, -144.63181352345478, -136.04353714588737, + -129.03535351100325, -120.64812612176986, -108.4578159331508, -91.66712058998779, + -71.6845532860305, -51.92454636690193, -36.75664559094005, -29.944229192958293, + -33.26253995101549, -45.8841436370077, -64.72238023555612, -85.55581883977085, -104.43639262659643, + -118.73440682883057, -127.4439469965765, -130.85855117578427, -129.9883150575577, + -126.05068565061471, -120.20491993068747, -113.51752511264922, -107.00751318510588, + -101.6120221067554, -98.037143648059, -96.5901783209796, -97.11117132586354, -99.05464430683851, + -101.69784273360709, -104.39663493912799, -106.78682465771945, -108.86073554241558, + -110.91272059008197, -113.391986702604, -116.71712944928908, -121.10899616745179, + -126.49101079170191, -132.48890828569083, -138.52907052668536, -143.98403266052748, + -148.28386085159676, -150.950353874693, -151.59029328770967, -149.92372750552684, + -145.8893613284024, -139.79752247093995, -132.44040508370935, -125.04855412827735, + -119.03041856644234, -115.55253894189944, -115.13281650745513, -117.42895405278668, + -121.3137432440983, -125.21048574002899, -127.55581585488525, -127.2035779940535, + -123.63075578035654, -116.92546848468061, -107.64003980195744, -96.63277766107122, + -84.99305975953892, -74.04821594432073, -65.34295582835624, -60.46526291514498, + -60.70407650618115, -66.66922658810824, -78.05651925687684, -93.67617143035707, + -111.7360977518272, -130.24058283704147, -147.30871275663227, -161.30727700805863, + -170.86975350241954, -174.96772570963563, -173.1273631811409, -165.72224178554154, + -154.15911230206518, -140.7708520660487, -128.34356994048002, -119.39667357114564, + -115.49538835769575, -116.87807764072566, -122.52767636389038, -130.62314678287214, + -139.1652566178275, -146.52616783065477, -151.74687442934803, -154.55565073083676, + -155.2031643789819, -154.2459690762812, -152.3741747997027, -150.31243759575744, + -148.7646263574894, -148.35253400075652, -149.52454396025652, -152.4546938221956, + -156.97962048361924, -162.61455065772643, -168.65639907954113, -174.33920996159839, + -178.98123335741235, -182.07734590854741, -183.33592461504546, -182.69582244314955, + -180.3576867141039, -176.82992166677565, -172.94908163552213, -169.81480317042735, + -168.6009806288589, -170.26752614676, -175.26636333032138, -183.36316952799345, + -193.6626336687613, -204.8455532302751, -215.5341659113945, -224.642821518713, -231.5841029386231, + -236.27823723741093, -239.00153643062308, -240.1602207969857, -240.0805222435039, + -238.8792739976468, -236.4402664233786, -232.4857796328019, -226.70748164073063, + -218.90832171524943, -209.10849307704876, -197.58443529231656, -184.83728952594203, + -171.51692947117536, -158.34183621150794, -146.03906486693822, -135.2931551378401, + -126.67274641338989, -120.522356840103, -116.8495422307382, -115.26711775119627, + -115.04262249541723, -115.26672709456207, -115.09929773879223, -114.01174175462747, + -111.938154926336, -109.27967994662575, -106.7610444266074, -105.19243755344766, + -105.22191192914899, -107.15903178753426, -110.91294427768851, -116.03909562366141, + -121.85319340040662, -127.56333385695282, -132.3915434702031, -135.68773579313347, + -137.0553530223522, -136.49152106606573, -134.50396087391331, -132.13096362061216, + -130.78914260327244, -131.92152103045797, -136.50685861312238, -144.58292188907654, + -154.9805743147388, -165.42722647501083, -173.05862816904528, -175.2162591822015, + -170.27334476780416, -158.19760009121686, -140.64912083959248, -120.58743118943534, + -101.54354695989151, -86.82037717684916, -78.86963873202836, -78.9730241819731, + -87.208732905478, -102.59534157990022, -123.30579810686955, -146.90854660058451, + -170.66096479222475, -191.89527725965974, -208.48091570828308, -219.25913002134993, + -224.29508133070183, -224.82455615157096, -222.87658961783313, -220.67901822911378, + -220.0419990907876, -221.92371042747453, -226.30932003382122, -232.4142180953472, + -239.1075434889348, -245.3854703853822, -250.72594095429886, -255.21957879524751, + -259.4638629985364, -264.29101495969917, -270.44645552037593, -278.33363978175873, + -287.8997813229622, -298.675957930859, -309.9292117127586, -320.853465714804, -330.7289512679556, + -339.01001631839443, -345.3404207556757, -349.52468900709295, -351.4950882757702, + -351.30843956420114, -349.19099656641225, -345.6266700588403, -341.4559943792539, + -337.9255863077082, -336.6122458269782, -339.1590394133497, -346.81553649131257, + -359.86602049319964, -377.1262582942168, -395.74022625809545, -411.4675647034107, + -419.50864481032374, -415.70559925146244, -397.7689969926942, -366.1070646451192, + -323.9312171058613, -276.5534397525774, -230.0823638333837, -189.94210230887091, + -159.6860391307984, -140.44018425867142, -131.05960164254728, -128.83405351298265, + -130.4331021797765, -132.77378964958245, -133.598664448043, -131.70104308630897, + -126.85857197498154, -119.59605542060795, -110.89414841971599, -101.91886722614183, + -93.80124103359078, -87.46894981676736, -83.52587581326873, -82.18044665496083, + -83.22666982990846, -86.0776105317449, -89.84325002250512, -93.43992023523293, -95.72026823608788, + -95.61789886265437, -92.30211434757715, -85.33026401582156, -74.77072267985237, + -61.2585993757004, -45.950045468071075, -30.363497680157124, -16.130369251753944, + -4.708318964022113, 2.8777155744834513, 6.212983448101315, 5.56140196148832, 1.82269284521927, + -3.6117451657027027, -9.093677781503999, -13.032649828418581, -14.265052388454068, + -12.419628464737789, -8.183075798478319, -3.351394348965721, -0.5798384940055397, + -2.8238203221278537, -12.574618457960362, -31.09374357688851, -57.88946050977006, + -90.62907055552884, -125.54979971296869, -158.26608815757967, -184.7370925829678, + -202.10993695070283, -209.20930494571166, -206.57443001611037, -196.09246662518265, + -180.3847681302668, -162.1383131743949, -143.5430153470272, -125.92848416505213, + -109.62850194687415, -94.06091918539822, -77.99447435013359, -59.96518435506764, + -38.78528395904798, -14.052481549971988, 13.469116037352999, 41.73558027808582, + 67.67668776494482, 87.82105256949613, 99.17127607438123, 100.09686637149439, 90.96891255317551, + 74.31360110106945, 54.401903491602475, 36.37112195454357, 25.122323308838894, 24.299071767105474, + 35.60733796084545, 58.60925251977363, 90.97083953417878, 129.02527666162507, 168.46442192730262, + 204.9931440177265, 234.84578165082002, 255.13458309044964, 264.04869333102295, 260.9405833712635, + 246.33164088648044, 221.85233074958896, 190.1149237954721, 154.50438006044362, 118.87017882929412, + 87.1135432104481, 62.69333693214143, 48.115270426306004, 44.50773230112809, 51.40175250111006, + 66.8050512534824, 87.59010715789734, 110.12403882647259, 130.98843565029367, 147.60355998106695, + 158.5997478392373, 163.85990646713972, 164.25977605801083, 161.2187754414792, 156.2146660131347, + 150.40095323538046, 144.40924932571875, 138.34575838676523, 131.9293358853309, 124.68710400298332, + 116.12724252945577, 105.83914725773546, 93.51279283344225, 78.90515326791086, 61.80006387437529, + 42.00492201327354, 19.40692541387674, -5.917558252890152, -33.573230874271864, -62.74274528444666, + -92.15503567861164, -120.15265932612857, -144.85998708249542, -164.4286388593254, + -177.31873975901365, -182.56747411912127, -179.99849715708353, -170.33317799853793, + -155.174775508723, -136.8502267694815, -118.11425365797368, -101.74838641351045, + -90.11915445215804, -84.78453470268744, -86.24244721296932, -93.89058890376597, + -106.21495538177916, -121.15951154482048, -136.5743658936049, -150.61621971325263, + -161.99385323020581, -170.00781819684602, -174.40729806224658, -175.15102714454724, + -172.18938784282471, -165.3700625780896, -154.51544144123787, -139.64662444269274, + -121.26379807895945, -100.56065686649099, -79.46407820455939, -60.445349586067294, + -46.126337444272174, -38.77528077288329, -39.82686472674805, -49.55634236027296, + -66.99108245478719, -90.07326313903191, -116.01965740549392, -141.78042183347972, + -164.48915325804674, -181.81824580195223, -192.1936050384186, -194.86464874708207, + -189.85637149914302, -177.8444701746055, -159.9936426666511, -137.78899220364173, + -112.8776101254829, -86.92681328173411, -61.4995443419655, -37.946187885812385, + -17.314095829999818, -0.2793256793802397, 12.892666001859144, 22.35087361058214, + 28.623738724825635, 32.547011711050814, 35.16166362154166, 37.59316158505565, 40.9252515429786, + 46.081001294230944, 53.72209705873961, 64.17527284982758, 77.39288792317001, 92.95301794423807, + 110.10246016313017, 127.84307430057432, 145.05732762285365, 160.66258701121782, + 173.77600795348275, 183.86412276724406, 190.84585708339466, 195.11799782738245, + 197.48122046939216, 198.9638867275426, 200.56755651863108, 202.98585640947218, 206.36735753017484, + 210.1940659607301, 213.32513104699035, 214.21331928977128, 211.25089340990704, 203.15829367329167, + 189.30944456733187, 169.9006771128133, 145.91404235801855, 118.88681324803126, 90.55706188617755, + 62.490765591022935, 35.79722544105233, 11.007111877948589, -11.866681217908095, + -33.12202103984155, -53.09189944897362, -71.89093420571022, -89.25892597900295, + -104.53982239765106, -116.79113539836814, -124.98292837106044, -128.22643422109374, + -125.9727544559038, -118.13810580734831, -105.13616058766276, -87.82215430794395, + -67.3716974347737, -45.12687495280552, -22.4432811717548, -0.5658122278379778, 19.44939304727038, + 36.762171246899385, 50.732584441996885, 60.87993008610584, 66.83886074810738, 68.3326809251376, + 65.17495028403165, 57.29773071625812, 44.792950118379224, 27.94724280533802, 7.252695883249127, + -16.615157231293985, -42.849357139418395, -70.58183967568638, -98.95676356556059, + -127.19340520577649, -154.63036780787843, -180.74870760607394] +yaw: [-156.65718192265612, -156.82512813476856, -157.03476785128979, -157.2807477120766, + -157.55369295460645, -157.84461759860156, -158.14778763059294, -158.459737944698, + -158.77686003690096, -159.09532479965736, -159.41325989374954, -159.73163715474024, + -160.0521375845057, -160.3745530176387, -160.69700732848457, -161.0185104942475, + -161.3402653767862, -161.66397674111627, -161.98953153448417, -162.315201866925, + -162.64003295077077, -162.96511555690367, -163.29205326830424, -163.62078022025358, + -163.9496971026068, -164.27788257902984, -164.60632279261657, -164.93652376742696, + -165.26846111422094, -165.60065680035294, -165.93222401150882, -166.26405206032558, + -166.5975532366255, -166.9327393757321, -167.2682466574162, -167.60322357479959, + -167.93847000443222, -168.27530835178302, -168.61378183900877, -168.95263403217712, + -169.29104925446183, -169.62974494472823, -169.9699574813401, -170.31175703012775, + -170.6539879618333, -170.99587071205337, -171.33804689251744, -171.68167070253227, + -172.02683518428148, -172.37247918025702, -172.71785930237908, -173.06354756695924, + -173.4106198177865, -173.75918826314913, -174.10828013599635, -174.45718809095524, + -174.8064204116356, -175.1569783712803, -175.50898997238295, -175.86156501042154, + -176.21403187168687, -176.56684061134436, -176.9209216656706, -177.276415779213, + -177.63250973601328, -177.9885671847553, -178.34498510912093, -178.7026267789968, + -179.0616429301755, -179.42129201479344, -179.7809723347144, -180.14103262348885, + -180.50227258176474, -180.86485046896803, -181.2280913368986, -181.59142740879287, + -181.95516366594228, -182.3200397542152, -182.6862192544363, -183.0530889992954, + -183.42011429540196, -183.78756055865932, -184.15611080378343, -184.525931978699, + -184.8964681246396, -185.2672167028437, -185.638407452449, -186.01067008275334, + -186.3841731854121, -186.75841368027824, -187.132920178221, -187.50789034493104, + -187.88390380611247, -188.26112928818085, -188.6391124973969, -189.01741212654568, + -189.39619709895, -189.7760000696095, -190.15698858912273, -190.53875329031354, + -190.92088183004392, -191.30351746122224, -191.68714886802226, -192.07194129758645, + -192.4575266759198, -192.8435204676572, -193.23004308121793, -193.617542113636, + -194.0061795490326, -194.39562519327265, -194.78552113473754, -195.17596753027587, + -195.56737365493905, -195.95989742408074, -196.35324332333818, -196.74707886293638, + -197.14148632095296, -197.53683929553736, -197.933290967728, -198.33057750888972, + -198.7283906402854, -199.12679692660544, -199.52613681329163, -199.92655820874447, + -200.32782617456402, -200.72965543146975, -201.13209880120448, -201.53546597968077, + -201.93989917925003, -202.34518974707686, -202.751074198292, -203.1575933993827, + -203.56502857939287, -203.97351593447695, -204.38287067560304, -204.7928499203278, + -205.20348419671345, -205.6150284301484, -206.0276125727257, -206.44107345232243, + -206.85518761577225, -207.26997671022124, -207.68567140275596, -208.10239525551458, + -208.5200046331361, -208.93829436247796, -209.35727851912242, -209.77716544140384, + -210.19807222793216, -210.61987285855759, -211.04237931918513, -211.4655992857971, + -211.8897205841901, -212.3148538391931, -212.74088887477996, -213.16765374694393, + -213.5951507769902, -214.02354898389035, -214.45295256340523, -214.8832655549256, + -215.31433103073036, -215.74614688524275, -216.1788649289692, -216.61258302054992, + -217.04721792048107, -217.48262670125752, -217.91880365055158, -218.3558848648318, + -218.79396199767987, -219.23296316292127, -219.67275845698202, -220.11333928225932, + -220.5548274153216, -220.99730847034053, -221.44072066552712, -221.88494618630875, + -222.32997418117233, -222.77591340446227, -223.22284362421576, -223.67071202540126, + -224.119411989995, -224.56893096190808, -225.01936587844727, -225.4707908770044, + -225.9231610756851, -226.37638020375584, -226.83043447547186, -227.2854101278763, + -227.7413759005292, -228.1982939079816, -228.65607742107326, -229.11471183206265, + -229.57427371024147, -230.0348266430826, -230.4963388949898, -230.95873251621126, + -231.42199242410942, -231.88618647266296, -232.35137335201267, -232.81752671335113, + -233.2845766674393, -233.7525079495373, -234.22138057487498, -234.69124859655113, + -235.16209036671597, -235.6338433804674, -236.1064924352654, -236.58009051246535, + -237.05468729088872, -237.53026520903265, -238.00676851209414, -238.48418226093696, + -238.9625531403663, -239.44192671749772, -239.92228896806165, -240.40359029407247, + -240.88581618288202, -241.3690076965991, -241.85320655070794, -242.33840176912216, + -242.8245493571952, -243.31163535831513, -243.79969582627467, -244.2887688805353, + -244.77884615907135, -245.26988875560264, -245.7618833697687, -246.2548616058494, + -246.74885823676817, -247.2438671305236, -247.73985399131797, -248.23680624976333, + -248.7347515676374, -249.23372161331298, -249.73371214631064, -250.2346930390105, + -250.73665250571727, -251.23961472458112, -251.74360849280094, -252.2486311641883, + -252.75465637099336, -253.2616731450968, -253.7697025952815, -254.27877087145927, + -254.78887666179244, -255.29999698245618, -255.81212170080903, -256.3252692292884, + -256.8394632842479, -257.3547036618484, -257.87097041693903, -258.3882542568401, + -258.9065712326523, -259.4259428302655, -259.94636975763615, -260.4678347920479, + -260.99032947413946, -261.5138677937398, -262.0384691984253, -262.564135138717, + -263.09085082541867, -263.6186086167544, -264.14742070931345, -264.677304693403, + -265.208262616922, -265.7402818609308, -266.27335557821664, -266.807494410878, -267.3427142618617, + -267.8790176526088, -268.4163938951752, -268.9548369081826, -269.4943559912946, + -270.0349655189502, -270.5766683811865, -271.11945560417905, -271.6633218393314, + -272.20827523166446, -272.75432877508274, -273.3014856399149, -273.84973837039325, + -274.3990823145224, -274.94952462848454, -275.50107706299923, -276.0537429949804, + -276.6075163099427, -277.16239301421695, -277.7183794210768, -278.27548616510694, + -278.8337167688488, -279.39306630014715, -279.95353138416385, -280.5151176192923, + -281.07783464110923, -281.64168606790315, -282.2066680073117, -282.77277766335567, + -283.3400200314943, -283.90840385592054, -284.47793281036434, -285.0486039147941, + -285.6204149122557, -286.19337029282093, -286.7674780078702, -287.3427417545014, + -287.91915935135097, -288.4967290413007, -289.07545489372984, -289.655344157199, + -290.2364005271319, -290.81862251976554, -291.40200883968265, -291.986563208829, + -292.57229225484906, -293.15919965241676, -293.7472845257627, -294.3365460044106, + -294.9269875259936, -295.51861517154896, -296.1114325809504, -296.70543940721166, + -297.3006351696594, -297.89702307577414, -298.49460872719874, -299.0933957191521, + -299.69338416362336, -300.29457393640496, -300.8969680610969, -301.50057172055426, + -302.10538845895877, -302.71141878594295, -303.31866290235337, -303.92712368726046, + -304.53680595921657, -305.14771320782285, -305.7598462866421, -306.3732056921633, + -306.98779419222967, -307.6036162899247, -308.2206754190174, -308.83897273011434, + -309.4585089879684, -310.0792868772328, -310.70131062915857, -311.3245836222539, + -311.94910726337764, -312.5748825602012, -313.20191213766174, -313.830199994053, + -314.45974945461137, -315.0905621470852, -315.72263929872355, -316.3559834942804, + -316.9905985336233, -317.62648769178344, -318.2636527868513, -318.9020952442645, + -319.5418176247443, -320.18282356031045, -320.8251162796443, -321.4686977648914, + -322.11356962017334, -322.7597343954331, -323.4071955818418, -324.0559563661364, + -324.7060188719847, -325.35738486448656, -326.01005689360136, -326.6640383334178, + -327.3193323334846, -327.97594113975754, -328.63386666231656, -329.29311145984894, + -329.95367881022037, -330.6155718307366, -331.2787928732944, -331.94334397856255, + -332.6092277209164, -333.2764473002516, -333.9450058066361, -334.6149056840792, + -335.286149090949, -335.9587386228065, -336.6326774175029, -337.3079685428293, -337.9846145232684, + -338.6626176233944, -339.3419804642377, -340.022706135458, -340.7047976874095, -341.3882577153029, + -342.07308857971486, -342.75929293043066, -343.4468738209348, -344.1358342888007, + -344.826176991857, -345.5179043776647, -346.2110191272336, -346.90552426826605, + -347.601422829986, -348.29871752613354, -348.99741088331945, -349.6975056155889, + -350.3990047338262, -351.1019112630828, -351.80622796750447, -352.51195744580275, + -353.21910244634364, -353.9276659709034, -354.63765104426665, -355.3490604765017, + -356.0618969323631, -356.7761631954104, -357.4918622649251, -358.2089971690505, + -358.927570760162, -359.64758576380353, -360.36904499927863, -361.09195146903556, + -361.8163082079192, -362.5421181077289, -363.2693839502654, -363.99810859088376, + -364.72829504003204, -365.4599463423245, -366.1930654267157, -366.9276551273741, + -367.6637183358344, -368.4012580746632, -369.14027740104433, -369.88077927933233, + -370.6227665927477, -371.3662422690045, -372.11120934629133, -372.8576708969081, + -373.60562991928094, -374.3550893428728, -375.1060521314913, -375.85852134192436, + -376.6125000638935, -377.3679913289216, -378.12499811035156, -378.88352340794614, + -379.64357029961894, -380.4051418945974, -381.1682412568148, -381.9328714015242, + -382.69903536427796, -383.46673624626146, -384.2359771780845, -385.0067612556417, + -385.7790915344696, -386.5529710857383, -387.32840303572766, -388.1053905381184, + -388.8839367205077, -389.66404467738937, -390.4457175153868, -391.2289583874262, + -392.0137704717747, -392.8001569276271, -393.58812088735715, -394.37766549287124, + -395.1687939249686, -395.96150938755676, -396.7558150704628, -397.55171414010636, + -398.3492097647753, -399.1483051294121, -399.94900339695914, -400.7513076097339, + -401.55522051642305, -402.3607442126537, -403.1678793129748, -403.97662313703535, + -404.78696601570533, -405.5988841676912, -406.41232657422364, -407.22719193743313, + -408.04329038092243, -408.86028352164305, -409.67759674852243, -410.49430019593217, + -411.30896127385165, -412.11948227644524, -412.9229503097521, -413.71553969665734, + -414.49251289191534, -415.2483582419702, -415.9770785993685, -416.6726079072575, + -417.329295131207, -417.94237131855783, -418.5083166266741, -419.02506982614034, + -419.4920624207183, -419.91009801422723, -420.28112301456906, -420.60794297702034, + -420.89393277454684] +z: [8.660695913847082, 66.39862553863676, 127.55801351244567, 180.5416567670594, 210.78937741627567, + 205.98510123498664, 161.75492603580668, 84.86410600259634, -7.938204250916348, -95.46180594158663, + -159.6160710741801, -191.12823315769305, -191.1077383247354, -168.75160790760808, + -136.7976917935674, -106.70965336063549, -85.49283736476575, -74.97102632230975, + -72.98493565768426, -75.44155069558714, -78.2857794410305, -78.76734664689184, -75.79970900381551, + -69.60926545532763, -61.02352026219444, -50.731204460839145, -38.784872157834826, + -24.48063415135472, -6.604904189438815, 16.003062940241094, 43.61116166930451, 74.59937262542272, + 104.69471826704422, 127.10591770220823, 134.08234145821157, 119.62663119956167, + 82.25187144352125, 26.508018246556365, -37.60809657045731, -97.50294343727298, -142.05752447785434, + -165.01189926649766, -166.22428468257075, -150.6580066881632, -125.88440608451583, + -99.27427444087671, -76.01596349902346, -58.51176921704588, -46.89688221845846, + -40.0319509439571, -36.428785825300736, -34.84658879993211, -34.526729879516196, + -35.16461507569286, -36.74598621919215, -39.344955846661044, -42.94609674951552, + -47.33366981479339, -52.08037130315903, -56.64311306447958, -60.522268045019175, + -63.40062822000451, -65.19777275447987, -66.03688501834353, -66.16552673973, -65.87798906520248, + -65.46886057404075, -65.21885341822762, -65.38814742633711, -66.18701058321993, + -67.71310218370519, -69.87490985875417, -72.34304758340024, -74.57596554892733, + -75.9445158717209, -75.92859524663281, -74.30883578638903, -71.26712435663396, -67.34312951723155, + -63.24836193520065, -59.60140045704585, -56.690265423268045, -54.35501112675093, + -52.025856935869335, -48.89272594930154, -44.14172332474001, -37.177555708112656, + -27.766047180552132, -16.071976274482385, -2.617381566487062, 11.77309724198873, + 25.976102073264208, 38.53687298107297, 47.718551492321396, 51.76605423223608, 49.38878979505458, + 40.290429255399275, 25.491354807794263, 7.246430327597227, -11.475853168764028, + -27.772349573580907, -39.515015426681174, -45.713572144488516, -46.46431428981413, + -42.61798316112623, -35.37703866753643, -25.9934677516497, -15.628487382325641, + -5.320192334714672, 4.040136476501501, 11.774699698468378, 17.500102100664304, 21.15959975218581, + 22.97185786614963, 23.31019232970664, 22.558042956454567, 20.99209210295209, 18.729017300170803, + 15.74651164213902, 11.966823265504356, 7.376265857048469, 2.1431266936820315, -3.3126251807763514, + -8.343814311527554, -12.216153231932507, -14.333645421496714, -14.465721023384898, + -12.87582842967554, -10.283561071346963, -7.656458262095206, -5.898235147224569, + -5.548464248913495, -6.607585778672925, -8.55034572367405, -10.517780185318616, + -11.61186892570347, -11.181289206074855, -8.998149061838516, -5.278041737321532, + -0.5603284740797135, 4.485651709130345, 9.246488587633161, 13.28299530174339, 16.36756662020893, + 18.466571978647558, 19.704100343285322, 20.32815531831131, 20.677451138832144, 21.13258932476601, + 22.03831545911599, 23.60334269570648, 25.810407707445663, 28.386041772609907, 30.87468716575818, + 32.83047926730481, 34.087238764628125, 35.01230737770861, 36.621360874602765, 40.45070511179779, + 48.15601919282285, 60.917623297793895, 78.84123893360714, 100.59365670804146, 123.46645783546617, + 143.91789347099578, 158.44750364654655, 164.50260889528977, 161.097954064323, 148.96575759387107, + 130.25873364860564, 107.99215885210386, 85.46056101968273, 65.78797889606493, 51.62771188476158, + 44.92502099696882, 46.66619035735277, 56.63547648764704, 73.29795847361908, 93.94368722527568, + 115.14660187881398, 133.44963208761388, 146.06803902842015, 151.38700573272683, + 149.12923984177337, 140.2151559819424, 126.44367536795221, 110.13915686176617, 93.84830850562516, + 80.0820870481205, 71.04848324787652, 68.34789291609472, 72.6765134206983, 83.63977979446459, + 99.77023650435606, 118.77456269522624, 137.94387073801647, 154.60399035259434, 166.49431469790014, + 172.0327438962138, 170.4928109096748, 162.13218328502208, 148.26316136662118, 131.1890050423139, + 113.90461235170372, 99.51589491572973, 90.46166485727544, 87.7622777984477, 90.58353615476729, + 96.33891906881483, 101.3726007035209, 102.04139409749659, 95.84221536672281, 82.19477689643513, + 62.61034213227201, 40.20548872373059, 18.75852748002458, 1.657957295481827, -8.90300742366735, + -12.266306640477236, -9.163238878856461, -1.233167864960895, 9.564495981683649, + 21.43992022175332, 33.051379335207095, 43.56592729978036, 52.568382283902764, 59.9082904825656, + 65.55991544152265, 69.53869459501789, 71.88526118098649, 72.70284621092118, 72.21843649851635, + 70.83368992994872, 69.13827315439758, 67.87334691224184, 67.8505290321987, 69.8442171675916, + 74.47726967536957, 82.11311705302812, 92.75985141769938, 105.99298201503689, 120.91543676261807, + 136.18772657541675, 150.16300634223884, 161.13869607756567, 167.68973106972928, + 168.99857035076562, 165.07365193284676, 156.7707462316869, 145.59585156800276, 133.34673828809346, + 121.70779304048943, 111.92368383650255, 104.63857613545397, 99.9202392953937, 97.42415696314441, + 96.61636301843436, 96.97307879858319, 98.1025495789197, 99.77349073377508, 101.86901694512392, + 104.30470000982287, 106.95197406963027, 109.5967587731362, 111.94423545513219, 113.66135899930364, + 114.4352894158048, 114.02254875294729, 112.27073376540599, 109.10848671943762, 104.5142402049289, + 98.48427593329139, 91.02225206964965, 82.16482996592228, 72.04340823046537, 60.9644383318998, + 49.47596335726721, 38.38208716267078, 28.675265153703773, 21.380090006656577, 17.336295088765645, + 16.98025827773935, 20.1977232072601, 26.30579134436739, 34.1820237918946, 42.50787279716556, + 50.05364436655743, 55.91974464196498, 59.66868428102189, 61.32427965788826, 61.25993059906409, + 60.02892932141973, 58.19725874007958, 56.22498874652909, 54.41596991142811, 52.9291999133868, + 51.82801400185255, 51.138336589289274, 50.892598938155075, 51.146726044509975, 51.96875114225244, + 53.40564661532219, 55.43870808852528, 57.93823519360839, 60.62739916553108, 63.064899619087015, + 64.65655841637202, 64.7055801259163, 62.50676710339701, 57.47927602641731, 49.31661470793777, + 38.11706927966492, 24.4510077185352, 9.330391041091739, -5.928968195692799, -19.927807358089126, + -31.432334046946572, -39.58564206883777, -44.02742340146397, -44.89862410201391, + -42.74448812517229, -38.35747836190584, -32.61145550528262, -26.32954437918894, + -20.20719977067912, -14.789151242435427, -10.48250380540003, -7.582119063177211, + -6.287826913269372, -6.702621306839947, -8.81253570937736, -12.45851173987955, -17.315467703254903, + -22.89273066518226, -28.563759038476075, -33.62421205979971, -37.369384313159216, + -39.17768128465537, -38.587042278968454, -35.35458955341083, -29.49368865852931, + -21.285110785761752, -11.260045417067115, -0.15386876206445768, 11.167509802528333, + 21.80199162922029, 30.914335137198602, 37.83281681446311, 42.1250528920106, 43.6424129336149, + 42.527536031710504, 39.1838083377227, 34.20896823571095, 28.298619788384883, 22.130902226323823, + 16.250738211119412, 10.978234041445447, 6.366580287176825, 2.2267366046830612, -1.780708650958427, + -6.007319377392964, -10.670867462380551, -15.74701218807577, -20.935394624211764, + -25.71475763924228, -29.473174580935297, -31.674804797246836, -32.01284071766054, + -30.503625724620882, -27.49687795069467, -23.603695718045664, -19.567702223835404, + -16.117560259075034, -13.838347771722535, -13.087536620644972, -13.9646827725496, + -16.328965936199715, -19.84970900489466, -24.07290465216959, -28.48977493682292, + -32.59854442331317, -35.95539956520115, -38.2137508958304, -39.15228104770502, -38.692332219912934, + -36.90456601359803, -34.004068329378526, -30.332692841352, -26.327899158206776, + -22.478863588907455, -19.27298194233058, -17.13832959626637, -16.389291255919126, + -17.18279456348231, -19.4913537172405, -23.09696936219348, -27.60749078959273, -32.494672428897246, + -37.15076766150979, -40.95788530079359, -43.36162493187602, -43.93846520775633, + -42.446152023679346, -38.848786666305784, -33.313331043792914, -26.180492183033934, + -17.918310843908614, -9.069298172978261, -0.20074671809499756, 8.136492955152297, + 15.441031108092213, 21.29036758626846, 25.372508549246056, 27.522718958733332, 27.762289494620244, + 26.32911239456066, 23.684885172768034, 20.483910329661786, 17.495388855951955, 15.484035434486861, + 15.069215691030688, 16.59509470023471, 20.047924906971748, 25.048393934050562, 30.927904484071945, + 36.87324018466698, 42.10253497425529, 46.024752154331864, 48.33939153393884, 49.0517871424882, + 48.40580399084933, 46.76081807159332, 44.45507771693884, 41.69825116535199, 38.52289882103032, + 34.803261444171405, 30.327999320714078, 24.898546537842716, 18.420586405729864, + 10.962638364832946, 2.769145218537321, -5.769675941714455, -14.177570162462116, + -21.96287190097139, -28.682775714701705, -33.98993062535793, -37.65744218731808, + -39.584533342726225, -39.78848208739123, -38.38883996706238, -35.58823507210259, + -31.651659030216656, -26.884305680876444, -21.60752145164577, -16.133301088860883, + -10.739449278450719, -5.649094917111724, -1.018762093669203, 3.061876393936713, + 6.558023261640048, 9.468369818928435, 11.800068844557336, 13.547269650304042, 14.679655966520992, + 15.144569931188034, 14.882211925027542, 13.849398415516339, 12.04474239771272, 9.52770863530006, + 6.425839876890219, 2.9278589763920806, -0.735815798162439, -4.320509611419369, -7.596486438809738, + -10.37389619761122, -12.519116524619744, -13.960610192047202, -14.685038419335728, + -14.726561926961596, -14.153639370214792, -13.057964531219815, -11.549353043113381, + -9.758421987878664, -7.846069969532445, -6.015682302494043, -4.521627491850837, + -3.6670759284008367, -3.78622388922754, -5.210529940531722, -8.224269075041521, + -13.019390902549098, -19.661194514880624, -28.07379403356179, -38.04868886403423, + -49.2733259570762, -61.37184021856482, -73.94857243204568, -86.62633925391154, -99.07453382284537, + -111.02550604149512] +pitch: [-156.65718192265612, -156.82512813476856, -157.03476785128979, -157.2807477120766, + -157.55369295460645, -157.84461759860156, -158.14778763059294, -158.459737944698, + -158.77686003690096, -159.09532479965736, -159.41325989374954, -159.73163715474024, + -160.0521375845057, -160.3745530176387, -160.69700732848457, -161.0185104942475, + -161.3402653767862, -161.66397674111627, -161.98953153448417, -162.315201866925, + -162.64003295077077, -162.96511555690367, -163.29205326830424, -163.62078022025358, + -163.9496971026068, -164.27788257902984, -164.60632279261657, -164.93652376742696, + -165.26846111422094, -165.60065680035294, -165.93222401150882, -166.26405206032558, + -166.5975532366255, -166.9327393757321, -167.2682466574162, -167.60322357479959, + -167.93847000443222, -168.27530835178302, -168.61378183900877, -168.95263403217712, + -169.29104925446183, -169.62974494472823, -169.9699574813401, -170.31175703012775, + -170.6539879618333, -170.99587071205337, -171.33804689251744, -171.68167070253227, + -172.02683518428148, -172.37247918025702, -172.71785930237908, -173.06354756695924, + -173.4106198177865, -173.75918826314913, -174.10828013599635, -174.45718809095524, + -174.8064204116356, -175.1569783712803, -175.50898997238295, -175.86156501042154, + -176.21403187168687, -176.56684061134436, -176.9209216656706, -177.276415779213, + -177.63250973601328, -177.9885671847553, -178.34498510912093, -178.7026267789968, + -179.0616429301755, -179.42129201479344, -179.7809723347144, -180.14103262348885, + -180.50227258176474, -180.86485046896803, -181.2280913368986, -181.59142740879287, + -181.95516366594228, -182.3200397542152, -182.6862192544363, -183.0530889992954, + -183.42011429540196, -183.78756055865932, -184.15611080378343, -184.525931978699, + -184.8964681246396, -185.2672167028437, -185.638407452449, -186.01067008275334, + -186.3841731854121, -186.75841368027824, -187.132920178221, -187.50789034493104, + -187.88390380611247, -188.26112928818085, -188.6391124973969, -189.01741212654568, + -189.39619709895, -189.7760000696095, -190.15698858912273, -190.53875329031354, + -190.92088183004392, -191.30351746122224, -191.68714886802226, -192.07194129758645, + -192.4575266759198, -192.8435204676572, -193.23004308121793, -193.617542113636, + -194.0061795490326, -194.39562519327265, -194.78552113473754, -195.17596753027587, + -195.56737365493905, -195.95989742408074, -196.35324332333818, -196.74707886293638, + -197.14148632095296, -197.53683929553736, -197.933290967728, -198.33057750888972, + -198.7283906402854, -199.12679692660544, -199.52613681329163, -199.92655820874447, + -200.32782617456402, -200.72965543146975, -201.13209880120448, -201.53546597968077, + -201.93989917925003, -202.34518974707686, -202.751074198292, -203.1575933993827, + -203.56502857939287, -203.97351593447695, -204.38287067560304, -204.7928499203278, + -205.20348419671345, -205.6150284301484, -206.0276125727257, -206.44107345232243, + -206.85518761577225, -207.26997671022124, -207.68567140275596, -208.10239525551458, + -208.5200046331361, -208.93829436247796, -209.35727851912242, -209.77716544140384, + -210.19807222793216, -210.61987285855759, -211.04237931918513, -211.4655992857971, + -211.8897205841901, -212.3148538391931, -212.74088887477996, -213.16765374694393, + -213.5951507769902, -214.02354898389035, -214.45295256340523, -214.8832655549256, + -215.31433103073036, -215.74614688524275, -216.1788649289692, -216.61258302054992, + -217.04721792048107, -217.48262670125752, -217.91880365055158, -218.3558848648318, + -218.79396199767987, -219.23296316292127, -219.67275845698202, -220.11333928225932, + -220.5548274153216, -220.99730847034053, -221.44072066552712, -221.88494618630875, + -222.32997418117233, -222.77591340446227, -223.22284362421576, -223.67071202540126, + -224.119411989995, -224.56893096190808, -225.01936587844727, -225.4707908770044, + -225.9231610756851, -226.37638020375584, -226.83043447547186, -227.2854101278763, + -227.7413759005292, -228.1982939079816, -228.65607742107326, -229.11471183206265, + -229.57427371024147, -230.0348266430826, -230.4963388949898, -230.95873251621126, + -231.42199242410942, -231.88618647266296, -232.35137335201267, -232.81752671335113, + -233.2845766674393, -233.7525079495373, -234.22138057487498, -234.69124859655113, + -235.16209036671597, -235.6338433804674, -236.1064924352654, -236.58009051246535, + -237.05468729088872, -237.53026520903265, -238.00676851209414, -238.48418226093696, + -238.9625531403663, -239.44192671749772, -239.92228896806165, -240.40359029407247, + -240.88581618288202, -241.3690076965991, -241.85320655070794, -242.33840176912216, + -242.8245493571952, -243.31163535831513, -243.79969582627467, -244.2887688805353, + -244.77884615907135, -245.26988875560264, -245.7618833697687, -246.2548616058494, + -246.74885823676817, -247.2438671305236, -247.73985399131797, -248.23680624976333, + -248.7347515676374, -249.23372161331298, -249.73371214631064, -250.2346930390105, + -250.73665250571727, -251.23961472458112, -251.74360849280094, -252.2486311641883, + -252.75465637099336, -253.2616731450968, -253.7697025952815, -254.27877087145927, + -254.78887666179244, -255.29999698245618, -255.81212170080903, -256.3252692292884, + -256.8394632842479, -257.3547036618484, -257.87097041693903, -258.3882542568401, + -258.9065712326523, -259.4259428302655, -259.94636975763615, -260.4678347920479, + -260.99032947413946, -261.5138677937398, -262.0384691984253, -262.564135138717, + -263.09085082541867, -263.6186086167544, -264.14742070931345, -264.677304693403, + -265.208262616922, -265.7402818609308, -266.27335557821664, -266.807494410878, -267.3427142618617, + -267.8790176526088, -268.4163938951752, -268.9548369081826, -269.4943559912946, + -270.0349655189502, -270.5766683811865, -271.11945560417905, -271.6633218393314, + -272.20827523166446, -272.75432877508274, -273.3014856399149, -273.84973837039325, + -274.3990823145224, -274.94952462848454, -275.50107706299923, -276.0537429949804, + -276.6075163099427, -277.16239301421695, -277.7183794210768, -278.27548616510694, + -278.8337167688488, -279.39306630014715, -279.95353138416385, -280.5151176192923, + -281.07783464110923, -281.64168606790315, -282.2066680073117, -282.77277766335567, + -283.3400200314943, -283.90840385592054, -284.47793281036434, -285.0486039147941, + -285.6204149122557, -286.19337029282093, -286.7674780078702, -287.3427417545014, + -287.91915935135097, -288.4967290413007, -289.07545489372984, -289.655344157199, + -290.2364005271319, -290.81862251976554, -291.40200883968265, -291.986563208829, + -292.57229225484906, -293.15919965241676, -293.7472845257627, -294.3365460044106, + -294.9269875259936, -295.51861517154896, -296.1114325809504, -296.70543940721166, + -297.3006351696594, -297.89702307577414, -298.49460872719874, -299.0933957191521, + -299.69338416362336, -300.29457393640496, -300.8969680610969, -301.50057172055426, + -302.10538845895877, -302.71141878594295, -303.31866290235337, -303.92712368726046, + -304.53680595921657, -305.14771320782285, -305.7598462866421, -306.3732056921633, + -306.98779419222967, -307.6036162899247, -308.2206754190174, -308.83897273011434, + -309.4585089879684, -310.0792868772328, -310.70131062915857, -311.3245836222539, + -311.94910726337764, -312.5748825602012, -313.20191213766174, -313.830199994053, + -314.45974945461137, -315.0905621470852, -315.72263929872355, -316.3559834942804, + -316.9905985336233, -317.62648769178344, -318.2636527868513, -318.9020952442645, + -319.5418176247443, -320.18282356031045, -320.8251162796443, -321.4686977648914, + -322.11356962017334, -322.7597343954331, -323.4071955818418, -324.0559563661364, + -324.7060188719847, -325.35738486448656, -326.01005689360136, -326.6640383334178, + -327.3193323334846, -327.97594113975754, -328.63386666231656, -329.29311145984894, + -329.95367881022037, -330.6155718307366, -331.2787928732944, -331.94334397856255, + -332.6092277209164, -333.2764473002516, -333.9450058066361, -334.6149056840792, + -335.286149090949, -335.9587386228065, -336.6326774175029, -337.3079685428293, -337.9846145232684, + -338.6626176233944, -339.3419804642377, -340.022706135458, -340.7047976874095, -341.3882577153029, + -342.07308857971486, -342.75929293043066, -343.4468738209348, -344.1358342888007, + -344.826176991857, -345.5179043776647, -346.2110191272336, -346.90552426826605, + -347.601422829986, -348.29871752613354, -348.99741088331945, -349.6975056155889, + -350.3990047338262, -351.1019112630828, -351.80622796750447, -352.51195744580275, + -353.21910244634364, -353.9276659709034, -354.63765104426665, -355.3490604765017, + -356.0618969323631, -356.7761631954104, -357.4918622649251, -358.2089971690505, + -358.927570760162, -359.64758576380353, -360.36904499927863, -361.09195146903556, + -361.8163082079192, -362.5421181077289, -363.2693839502654, -363.99810859088376, + -364.72829504003204, -365.4599463423245, -366.1930654267157, -366.9276551273741, + -367.6637183358344, -368.4012580746632, -369.14027740104433, -369.88077927933233, + -370.6227665927477, -371.3662422690045, -372.11120934629133, -372.8576708969081, + -373.60562991928094, -374.3550893428728, -375.1060521314913, -375.85852134192436, + -376.6125000638935, -377.3679913289216, -378.12499811035156, -378.88352340794614, + -379.64357029961894, -380.4051418945974, -381.1682412568148, -381.9328714015242, + -382.69903536427796, -383.46673624626146, -384.2359771780845, -385.0067612556417, + -385.7790915344696, -386.5529710857383, -387.32840303572766, -388.1053905381184, + -388.8839367205077, -389.66404467738937, -390.4457175153868, -391.2289583874262, + -392.0137704717747, -392.8001569276271, -393.58812088735715, -394.37766549287124, + -395.1687939249686, -395.96150938755676, -396.7558150704628, -397.55171414010636, + -398.3492097647753, -399.1483051294121, -399.94900339695914, -400.7513076097339, + -401.55522051642305, -402.3607442126537, -403.1678793129748, -403.97662313703535, + -404.78696601570533, -405.5988841676912, -406.41232657422364, -407.22719193743313, + -408.04329038092243, -408.86028352164305, -409.67759674852243, -410.49430019593217, + -411.30896127385165, -412.11948227644524, -412.9229503097521, -413.71553969665734, + -414.49251289191534, -415.2483582419702, -415.9770785993685, -416.6726079072575, + -417.329295131207, -417.94237131855783, -418.5083166266741, -419.02506982614034, + -419.4920624207183, -419.91009801422723, -420.28112301456906, -420.60794297702034, + -420.89393277454684] +roll: [-156.65718192265612, -156.82512813476856, -157.03476785128979, -157.2807477120766, + -157.55369295460645, -157.84461759860156, -158.14778763059294, -158.459737944698, + -158.77686003690096, -159.09532479965736, -159.41325989374954, -159.73163715474024, + -160.0521375845057, -160.3745530176387, -160.69700732848457, -161.0185104942475, + -161.3402653767862, -161.66397674111627, -161.98953153448417, -162.315201866925, + -162.64003295077077, -162.96511555690367, -163.29205326830424, -163.62078022025358, + -163.9496971026068, -164.27788257902984, -164.60632279261657, -164.93652376742696, + -165.26846111422094, -165.60065680035294, -165.93222401150882, -166.26405206032558, + -166.5975532366255, -166.9327393757321, -167.2682466574162, -167.60322357479959, + -167.93847000443222, -168.27530835178302, -168.61378183900877, -168.95263403217712, + -169.29104925446183, -169.62974494472823, -169.9699574813401, -170.31175703012775, + -170.6539879618333, -170.99587071205337, -171.33804689251744, -171.68167070253227, + -172.02683518428148, -172.37247918025702, -172.71785930237908, -173.06354756695924, + -173.4106198177865, -173.75918826314913, -174.10828013599635, -174.45718809095524, + -174.8064204116356, -175.1569783712803, -175.50898997238295, -175.86156501042154, + -176.21403187168687, -176.56684061134436, -176.9209216656706, -177.276415779213, + -177.63250973601328, -177.9885671847553, -178.34498510912093, -178.7026267789968, + -179.0616429301755, -179.42129201479344, -179.7809723347144, -180.14103262348885, + -180.50227258176474, -180.86485046896803, -181.2280913368986, -181.59142740879287, + -181.95516366594228, -182.3200397542152, -182.6862192544363, -183.0530889992954, + -183.42011429540196, -183.78756055865932, -184.15611080378343, -184.525931978699, + -184.8964681246396, -185.2672167028437, -185.638407452449, -186.01067008275334, + -186.3841731854121, -186.75841368027824, -187.132920178221, -187.50789034493104, + -187.88390380611247, -188.26112928818085, -188.6391124973969, -189.01741212654568, + -189.39619709895, -189.7760000696095, -190.15698858912273, -190.53875329031354, + -190.92088183004392, -191.30351746122224, -191.68714886802226, -192.07194129758645, + -192.4575266759198, -192.8435204676572, -193.23004308121793, -193.617542113636, + -194.0061795490326, -194.39562519327265, -194.78552113473754, -195.17596753027587, + -195.56737365493905, -195.95989742408074, -196.35324332333818, -196.74707886293638, + -197.14148632095296, -197.53683929553736, -197.933290967728, -198.33057750888972, + -198.7283906402854, -199.12679692660544, -199.52613681329163, -199.92655820874447, + -200.32782617456402, -200.72965543146975, -201.13209880120448, -201.53546597968077, + -201.93989917925003, -202.34518974707686, -202.751074198292, -203.1575933993827, + -203.56502857939287, -203.97351593447695, -204.38287067560304, -204.7928499203278, + -205.20348419671345, -205.6150284301484, -206.0276125727257, -206.44107345232243, + -206.85518761577225, -207.26997671022124, -207.68567140275596, -208.10239525551458, + -208.5200046331361, -208.93829436247796, -209.35727851912242, -209.77716544140384, + -210.19807222793216, -210.61987285855759, -211.04237931918513, -211.4655992857971, + -211.8897205841901, -212.3148538391931, -212.74088887477996, -213.16765374694393, + -213.5951507769902, -214.02354898389035, -214.45295256340523, -214.8832655549256, + -215.31433103073036, -215.74614688524275, -216.1788649289692, -216.61258302054992, + -217.04721792048107, -217.48262670125752, -217.91880365055158, -218.3558848648318, + -218.79396199767987, -219.23296316292127, -219.67275845698202, -220.11333928225932, + -220.5548274153216, -220.99730847034053, -221.44072066552712, -221.88494618630875, + -222.32997418117233, -222.77591340446227, -223.22284362421576, -223.67071202540126, + -224.119411989995, -224.56893096190808, -225.01936587844727, -225.4707908770044, + -225.9231610756851, -226.37638020375584, -226.83043447547186, -227.2854101278763, + -227.7413759005292, -228.1982939079816, -228.65607742107326, -229.11471183206265, + -229.57427371024147, -230.0348266430826, -230.4963388949898, -230.95873251621126, + -231.42199242410942, -231.88618647266296, -232.35137335201267, -232.81752671335113, + -233.2845766674393, -233.7525079495373, -234.22138057487498, -234.69124859655113, + -235.16209036671597, -235.6338433804674, -236.1064924352654, -236.58009051246535, + -237.05468729088872, -237.53026520903265, -238.00676851209414, -238.48418226093696, + -238.9625531403663, -239.44192671749772, -239.92228896806165, -240.40359029407247, + -240.88581618288202, -241.3690076965991, -241.85320655070794, -242.33840176912216, + -242.8245493571952, -243.31163535831513, -243.79969582627467, -244.2887688805353, + -244.77884615907135, -245.26988875560264, -245.7618833697687, -246.2548616058494, + -246.74885823676817, -247.2438671305236, -247.73985399131797, -248.23680624976333, + -248.7347515676374, -249.23372161331298, -249.73371214631064, -250.2346930390105, + -250.73665250571727, -251.23961472458112, -251.74360849280094, -252.2486311641883, + -252.75465637099336, -253.2616731450968, -253.7697025952815, -254.27877087145927, + -254.78887666179244, -255.29999698245618, -255.81212170080903, -256.3252692292884, + -256.8394632842479, -257.3547036618484, -257.87097041693903, -258.3882542568401, + -258.9065712326523, -259.4259428302655, -259.94636975763615, -260.4678347920479, + -260.99032947413946, -261.5138677937398, -262.0384691984253, -262.564135138717, + -263.09085082541867, -263.6186086167544, -264.14742070931345, -264.677304693403, + -265.208262616922, -265.7402818609308, -266.27335557821664, -266.807494410878, -267.3427142618617, + -267.8790176526088, -268.4163938951752, -268.9548369081826, -269.4943559912946, + -270.0349655189502, -270.5766683811865, -271.11945560417905, -271.6633218393314, + -272.20827523166446, -272.75432877508274, -273.3014856399149, -273.84973837039325, + -274.3990823145224, -274.94952462848454, -275.50107706299923, -276.0537429949804, + -276.6075163099427, -277.16239301421695, -277.7183794210768, -278.27548616510694, + -278.8337167688488, -279.39306630014715, -279.95353138416385, -280.5151176192923, + -281.07783464110923, -281.64168606790315, -282.2066680073117, -282.77277766335567, + -283.3400200314943, -283.90840385592054, -284.47793281036434, -285.0486039147941, + -285.6204149122557, -286.19337029282093, -286.7674780078702, -287.3427417545014, + -287.91915935135097, -288.4967290413007, -289.07545489372984, -289.655344157199, + -290.2364005271319, -290.81862251976554, -291.40200883968265, -291.986563208829, + -292.57229225484906, -293.15919965241676, -293.7472845257627, -294.3365460044106, + -294.9269875259936, -295.51861517154896, -296.1114325809504, -296.70543940721166, + -297.3006351696594, -297.89702307577414, -298.49460872719874, -299.0933957191521, + -299.69338416362336, -300.29457393640496, -300.8969680610969, -301.50057172055426, + -302.10538845895877, -302.71141878594295, -303.31866290235337, -303.92712368726046, + -304.53680595921657, -305.14771320782285, -305.7598462866421, -306.3732056921633, + -306.98779419222967, -307.6036162899247, -308.2206754190174, -308.83897273011434, + -309.4585089879684, -310.0792868772328, -310.70131062915857, -311.3245836222539, + -311.94910726337764, -312.5748825602012, -313.20191213766174, -313.830199994053, + -314.45974945461137, -315.0905621470852, -315.72263929872355, -316.3559834942804, + -316.9905985336233, -317.62648769178344, -318.2636527868513, -318.9020952442645, + -319.5418176247443, -320.18282356031045, -320.8251162796443, -321.4686977648914, + -322.11356962017334, -322.7597343954331, -323.4071955818418, -324.0559563661364, + -324.7060188719847, -325.35738486448656, -326.01005689360136, -326.6640383334178, + -327.3193323334846, -327.97594113975754, -328.63386666231656, -329.29311145984894, + -329.95367881022037, -330.6155718307366, -331.2787928732944, -331.94334397856255, + -332.6092277209164, -333.2764473002516, -333.9450058066361, -334.6149056840792, + -335.286149090949, -335.9587386228065, -336.6326774175029, -337.3079685428293, -337.9846145232684, + -338.6626176233944, -339.3419804642377, -340.022706135458, -340.7047976874095, -341.3882577153029, + -342.07308857971486, -342.75929293043066, -343.4468738209348, -344.1358342888007, + -344.826176991857, -345.5179043776647, -346.2110191272336, -346.90552426826605, + -347.601422829986, -348.29871752613354, -348.99741088331945, -349.6975056155889, + -350.3990047338262, -351.1019112630828, -351.80622796750447, -352.51195744580275, + -353.21910244634364, -353.9276659709034, -354.63765104426665, -355.3490604765017, + -356.0618969323631, -356.7761631954104, -357.4918622649251, -358.2089971690505, + -358.927570760162, -359.64758576380353, -360.36904499927863, -361.09195146903556, + -361.8163082079192, -362.5421181077289, -363.2693839502654, -363.99810859088376, + -364.72829504003204, -365.4599463423245, -366.1930654267157, -366.9276551273741, + -367.6637183358344, -368.4012580746632, -369.14027740104433, -369.88077927933233, + -370.6227665927477, -371.3662422690045, -372.11120934629133, -372.8576708969081, + -373.60562991928094, -374.3550893428728, -375.1060521314913, -375.85852134192436, + -376.6125000638935, -377.3679913289216, -378.12499811035156, -378.88352340794614, + -379.64357029961894, -380.4051418945974, -381.1682412568148, -381.9328714015242, + -382.69903536427796, -383.46673624626146, -384.2359771780845, -385.0067612556417, + -385.7790915344696, -386.5529710857383, -387.32840303572766, -388.1053905381184, + -388.8839367205077, -389.66404467738937, -390.4457175153868, -391.2289583874262, + -392.0137704717747, -392.8001569276271, -393.58812088735715, -394.37766549287124, + -395.1687939249686, -395.96150938755676, -396.7558150704628, -397.55171414010636, + -398.3492097647753, -399.1483051294121, -399.94900339695914, -400.7513076097339, + -401.55522051642305, -402.3607442126537, -403.1678793129748, -403.97662313703535, + -404.78696601570533, -405.5988841676912, -406.41232657422364, -407.22719193743313, + -408.04329038092243, -408.86028352164305, -409.67759674852243, -410.49430019593217, + -411.30896127385165, -412.11948227644524, -412.9229503097521, -413.71553969665734, + -414.49251289191534, -415.2483582419702, -415.9770785993685, -416.6726079072575, + -417.329295131207, -417.94237131855783, -418.5083166266741, -419.02506982614034, + -419.4920624207183, -419.91009801422723, -420.28112301456906, -420.60794297702034, + -420.89393277454684] +x: [-131.00261478679988, -121.88886417422353, -111.9050389833139, -102.61368963497223, + -96.05947080078046, -94.12535169815399, -97.8607435820271, -107.14613364957096, + -120.82659085972966, -137.19720078410953, -154.5163549178483, -171.18694825080595, + -185.5568218223094, -195.704691059387, -199.6329868378576, -195.91936207338776, + -184.48915098542318, -167.07793507920195, -147.0574840422674, -128.53128958566316, + -115.04358889413827, -108.56400855374055, -109.19561099927448, -115.57413755530115, + -125.64926844955686, -137.46655539270932, -149.61816709281686, -161.26501144566066, + -171.91636446335687, -181.2216960780747, -188.87007846616464, -194.53289513944736, + -197.79145167358277, -198.0996571089822, -194.8802396222356, -187.78401698176944, + -177.02826515891735, -163.64756841648978, -149.46450567572836, -136.67890898964777, + -127.21395723237792, -122.15440349976664, -121.54367242044958, -124.56838026444774, + -129.9847428341204, -136.57776680869415, -143.4619977851926, -150.1463172881614, + -156.43047062938794, -162.25579819785062, -167.580799896415, -172.2857363783464, + -176.10313080152142, -178.60602094331426, -179.29466139771722, -177.77366590562514, + -173.94835807935246, -168.14763364573156, -161.10576879062413, -153.79437054295704, + -147.18005615509452, -142.03544698559352, -138.8812834694729, -138.0254327793118, + -139.60154077046317, -143.53795283987066, -149.46190564030064, -156.61307764750555, + -163.87292592523232, -169.97827533844975, -173.87402597845175, -175.0512378675456, + -173.7109894894428, -170.68112972530366, -167.11714893622806, -164.10386427943158, + -162.31622495964814, -161.86114761154576, -162.33151047253835, -163.02192881838113, + -163.2095501914243, -162.39248606696069, -160.40951035619491, -157.42206476020834, + -153.79159590063577, -149.91799716532543, -146.1123968434442, -142.55104990302257, + -139.31149673683075, -136.46131741930625, -134.16539879470983, -132.7805579230332, + -132.8976720404706, -135.27842864395015, -140.64645739243105, -149.3508159834204, + -161.00381971456645, -174.2605798886825, -186.9134811438676, -196.38261868432437, + -200.49211090980378, -198.23763830134754, -190.21480773205266, -178.51115567595826, + -166.0801280692791, -155.83164235826004, -149.80028431814193, -148.6986554765198, + -151.96202945833986, -158.18027510739103, -165.68937034330935, -173.07927043758525, + -179.46192687105753, -184.482441275634, -188.1614046703995, -190.6833573748501, + -192.21809683888495, -192.81960540582097, -192.41403431825069, -190.86577715074623, + -188.09301880254696, -184.18878513004086, -179.4955981906823, -174.5892113462465, + -170.1561255168205, -166.79776163996192, -164.8373602238494, -164.2126907946925, + -164.50294621069565, -165.0839384166419, -165.3557930296701, -164.95884643563477, + -163.898412256587, -162.534820649653, -161.44472376807636, -161.20582442689792, + -162.18450627688458, -164.40124437891336, -167.51392693816024, -170.91206169710102, + -173.8749652264376, -175.73083709030058, -175.97063351490164, -174.31004528380663, + -170.72511032331406, -165.48897950595216, -159.20946482285498, -152.82808750163824, + -147.5191667738631, -144.44886967752777, -144.4240273588793, -147.54449659368146, + -153.01617480666846, -159.25176211818498, -164.2877168274527, -166.41140144219656, + -164.78014816559207, -159.78674905727087, -153.00534915065177, -146.70549554069234, + -143.09178186665255, -143.5446658872191, -148.14637102277132, -155.6663204124734, + -164.00416538606794, -170.916565992789, -174.75161613413627, -174.9259359923934, + -171.99663265443107, -167.34336788125873, -162.6147423868518, -159.16048671989986, + -157.6477458947188, -157.96425018036857, -159.39300876091175, -160.95217491299363, + -161.7572333232152, -161.28442609094157, -159.4763125555903, -156.69932273396668, + -153.6090737832136, -150.9888364227075, -149.60413892450302, -150.0809012374839, + -152.79153608790728, -157.7397093479904, -164.46467355593433, -172.0172934256874, + -179.0661158741304, -184.15783677719142, -186.08855140295393, -184.27238379689192, + -178.96525805487465, -171.2372865037682, -162.67650112956372, -154.91277319136154, + -149.12741792008617, -145.72381647659327, -144.27134285886908, -143.72945580899287, + -142.85723879551645, -140.65447875831518, -136.68276519662683, -131.17312406130938, + -124.91209567953047, -118.9751543144055, -114.4168612960534, -112.01955243552163, + -112.15689719095518, -114.77332263747567, -119.44360198084003, -125.471236872446, + -132.00253791160844, -138.1561515203214, -143.17484043177828, -146.58907912429945, + -148.3511438380617, -148.87742279194816, -148.94591893398302, -149.43982973654823, + -150.99302147422435, -153.65215284576814, -156.69307614107936, -158.6972973355033, + -157.91096745292066, -152.8004175957856, -142.62726270976202, -127.83568739093043, + -110.09574251961111, -91.96442744166842, -76.26517339306562, -65.38756088681923, + -60.727455422828086, -62.41769838627875, -69.38103902919622, -79.63028303419432, + -90.6904492501862, -100.0330543381811, -105.46650890222921, -105.47370228165889, + -99.49440285791094, -88.11732926351014, -73.10854974665124, -57.20048636703596, + -43.619919989560685, -35.42935377805215, -34.84789242860901, -42.75227348465364, + -58.50946410168745, -80.17821602052324, -104.99065766337972, -129.94273353227604, + -152.31614487459336, -170.01709776381944, -181.70800379508364, -186.78019457154642, + -185.24347046638363, -177.59665180543396, -164.71393092910603, -147.75511927137686, + -128.09259386421706, -107.2415258381779, -86.77724029789665, -68.22375027449615, + -52.90610923413581, -41.779927174503385, -35.27752870209294, -33.22579478747385, + -34.88146090859914, -39.094144062824924, -44.55933540558859, -50.084953618202576, + -54.78496998250598, -58.13877584276208, -59.90622409641475, -59.945540069741, -58.02266713890794, + -53.710211558038985, -46.44646056518766, -35.76628477961155, -21.64346099537084, + -4.823099943406338, 13.00074336490228, 29.276996809341675, 40.992025758372115, 45.29133769691425, + 40.16014663983825, 24.958013623469462, 0.6430403690297408, -30.37648983818459, -64.70364605521273, + -98.5780766753714, -128.49010205980096, -151.64916889145232, -166.24865624155242, + -171.54102444522545, -167.77666186080413, -156.06265128020192, -138.17902607586348, + -116.36917642692488, -93.10932192371236, -70.85982959536072, -51.803566443571675, + -37.58102962803787, -29.04191085373232, -26.05140387409622, -27.411942444947794, + -30.971298702656785, -33.967139701487234, -33.59780771242022, -27.721946232536823, + -15.50944865087424, 2.165242029053751, 22.75109265450623, 42.51462881068035, 57.39178500711525, + 63.9603626762866, 60.24680348570627, 46.13482203280333, 23.280449691574223, -5.396531697017779, + -36.44546991778746, -66.59572190185905, -93.244393312115, -114.63002544381644, -129.7277803714285, + -137.99626292368302, -139.13919014515983, -133.01230035499455, -119.72497935278233, + -99.88864280213238, -74.88535670756845, -46.99987777401981, -19.290622507018494, + 4.836900082983619, 22.275753850864515, 30.9051736945987, 30.102091149122813, 20.89767984126534, + 5.709426552642763, -12.280539670085691, -29.912602539696113, -44.76409817954616, + -55.587777836517255, -62.37400184312159, -66.06509298635545, -68.05618356119531, + -69.6650415443935, -71.73438308006683, -74.46044693767506, -77.45330286152975, -79.96093287153582, + -81.15272296993173, -80.36351741010003, -77.23600916293604, -71.74756168638527, + -64.14841195780141, -54.85947253693819, -44.377179252428895, -33.21572713174938, + -21.893453397774184, -10.949679527408357, -0.9673128756345823, 7.423219742995925, + 13.573678927669173, 16.882316200266693, 16.880518451590333, 13.321463895553073, + 6.2494544216467425, -3.9662652022608986, -16.64105804227179, -30.827458079441, -45.40427423165941, + -59.19176164715269, -71.07926973763034, -80.1486617898492, -85.77441240031021, -87.6818651318595, + -85.95088814283162, -80.96333944498441, -73.30666637565525, -63.65782924774226, + -52.676828368491805, -40.93506347992221, -28.891766117399765, -16.916080004322772, + -5.3384835550584935, 5.4924058263232025, 15.171162899221802, 23.231051427618734, + 29.178380505050853, 32.559117153019926, 33.042995254450894, 30.506717141912656, + 25.098309221713915, 17.267877434957573, 7.754667766136128, -2.4738852870074073, + -12.328160893560188, -20.73549791063537, -26.79110541102302, -29.892155231726377, + -29.825103487837175, -26.78324113785426, -21.306403690118064, -14.15394397582623, + -6.139545262622449, 2.0335265366227753, 9.89870331065091, 17.286397543623664, 24.307308641748374, + 31.26525601401877, 38.52026696289829, 46.33161129748563, 54.71416332554038, 63.34061174435135, + 71.51679336934421, 78.24704133338132, 82.39018658725793, 82.88576123321361, 79.00811170033668, + 70.59040844420252, 58.15831519340492, 42.928737491281964, 26.660959489767816, 11.387156505595188, + -0.9161323493847441, -8.636955778598946, -10.746100985455664, -6.88302166146115, + 2.6901550272644528, 17.227609362490647, 35.67099777743684, 56.793973316018906, 79.28492204264502, + 101.76799145249652, 122.79490061004348, 140.8566568494178, 154.45830822835748, 162.2730575641024, + 163.35441171879694, 157.35115570417733, 144.6537822686842, 126.41052162787045, 104.38486553091724, + 80.67370587471055, 57.34955312756698, 36.11589971154659, 18.06234868471712, 3.576456180471944, + -7.5771785605298545, -16.047264197592554, -22.625071287068153, -27.99125205987563, + -32.544187050649704, -36.342770320132814, -39.160889188810124, -40.621845381417664, + -40.36566724147911, -38.20206823583871, -34.21329090178294, -28.788359919239177, + -22.587615544563256, -16.449965238654954, -11.263594052219574, -7.82420704968046, + -6.7043106382969215, -8.153633537426817, -12.045315570129013, -17.87558753135798, + -24.817072522162295, -31.818486356751524, -37.73735081273553, -41.48808157884077, + -42.185843095170476, -39.266966337888135, -32.56948414179687, -22.362390213218752, + -9.319317728622229, 5.559271102094557, 21.062973392548795, 35.90860732266084, 48.87612268299202, + 58.92359238196129, 65.26758898268541, 67.42353430763573, 65.20869447959286, 58.716140928984274, + 48.27012084952794, 34.372265068707556, 17.645386644363096, -1.221017147956034, -21.52101235133283, + -42.57613526226664, -63.77089174961588, -84.5781075128901, -104.57331194568057, + -123.43864361815416, -140.9579908724867, -157.005848574077] +y: [-99.19714419940159, -79.16394233266908, -57.87218080909418, -39.28438472983111, + -28.386800564847263, -29.413961436687575, -43.913473953576144, -69.67422837210728, + -101.14133069492664, -131.32148548272554, -154.36897298960562, -167.56000537729983, + -171.8480811942197, -171.05140505400772, -170.15303380273187, -173.34724789149277, + -182.51652550260764, -196.6077612179733, -212.0578803921079, -224.1551729270074, + -228.85174829123696, -224.25993586587964, -211.25206790585622, -193.0143316349947, + -173.74730833399678, -157.0391212367104, -144.63181352345478, -136.04353714588737, + -129.03535351100325, -120.64812612176986, -108.4578159331508, -91.66712058998779, + -71.6845532860305, -51.92454636690193, -36.75664559094005, -29.944229192958293, + -33.26253995101549, -45.8841436370077, -64.72238023555612, -85.55581883977085, -104.43639262659643, + -118.73440682883057, -127.4439469965765, -130.85855117578427, -129.9883150575577, + -126.05068565061471, -120.20491993068747, -113.51752511264922, -107.00751318510588, + -101.6120221067554, -98.037143648059, -96.5901783209796, -97.11117132586354, -99.05464430683851, + -101.69784273360709, -104.39663493912799, -106.78682465771945, -108.86073554241558, + -110.91272059008197, -113.391986702604, -116.71712944928908, -121.10899616745179, + -126.49101079170191, -132.48890828569083, -138.52907052668536, -143.98403266052748, + -148.28386085159676, -150.950353874693, -151.59029328770967, -149.92372750552684, + -145.8893613284024, -139.79752247093995, -132.44040508370935, -125.04855412827735, + -119.03041856644234, -115.55253894189944, -115.13281650745513, -117.42895405278668, + -121.3137432440983, -125.21048574002899, -127.55581585488525, -127.2035779940535, + -123.63075578035654, -116.92546848468061, -107.64003980195744, -96.63277766107122, + -84.99305975953892, -74.04821594432073, -65.34295582835624, -60.46526291514498, + -60.70407650618115, -66.66922658810824, -78.05651925687684, -93.67617143035707, + -111.7360977518272, -130.24058283704147, -147.30871275663227, -161.30727700805863, + -170.86975350241954, -174.96772570963563, -173.1273631811409, -165.72224178554154, + -154.15911230206518, -140.7708520660487, -128.34356994048002, -119.39667357114564, + -115.49538835769575, -116.87807764072566, -122.52767636389038, -130.62314678287214, + -139.1652566178275, -146.52616783065477, -151.74687442934803, -154.55565073083676, + -155.2031643789819, -154.2459690762812, -152.3741747997027, -150.31243759575744, + -148.7646263574894, -148.35253400075652, -149.52454396025652, -152.4546938221956, + -156.97962048361924, -162.61455065772643, -168.65639907954113, -174.33920996159839, + -178.98123335741235, -182.07734590854741, -183.33592461504546, -182.69582244314955, + -180.3576867141039, -176.82992166677565, -172.94908163552213, -169.81480317042735, + -168.6009806288589, -170.26752614676, -175.26636333032138, -183.36316952799345, + -193.6626336687613, -204.8455532302751, -215.5341659113945, -224.642821518713, -231.5841029386231, + -236.27823723741093, -239.00153643062308, -240.1602207969857, -240.0805222435039, + -238.8792739976468, -236.4402664233786, -232.4857796328019, -226.70748164073063, + -218.90832171524943, -209.10849307704876, -197.58443529231656, -184.83728952594203, + -171.51692947117536, -158.34183621150794, -146.03906486693822, -135.2931551378401, + -126.67274641338989, -120.522356840103, -116.8495422307382, -115.26711775119627, + -115.04262249541723, -115.26672709456207, -115.09929773879223, -114.01174175462747, + -111.938154926336, -109.27967994662575, -106.7610444266074, -105.19243755344766, + -105.22191192914899, -107.15903178753426, -110.91294427768851, -116.03909562366141, + -121.85319340040662, -127.56333385695282, -132.3915434702031, -135.68773579313347, + -137.0553530223522, -136.49152106606573, -134.50396087391331, -132.13096362061216, + -130.78914260327244, -131.92152103045797, -136.50685861312238, -144.58292188907654, + -154.9805743147388, -165.42722647501083, -173.05862816904528, -175.2162591822015, + -170.27334476780416, -158.19760009121686, -140.64912083959248, -120.58743118943534, + -101.54354695989151, -86.82037717684916, -78.86963873202836, -78.9730241819731, + -87.208732905478, -102.59534157990022, -123.30579810686955, -146.90854660058451, + -170.66096479222475, -191.89527725965974, -208.48091570828308, -219.25913002134993, + -224.29508133070183, -224.82455615157096, -222.87658961783313, -220.67901822911378, + -220.0419990907876, -221.92371042747453, -226.30932003382122, -232.4142180953472, + -239.1075434889348, -245.3854703853822, -250.72594095429886, -255.21957879524751, + -259.4638629985364, -264.29101495969917, -270.44645552037593, -278.33363978175873, + -287.8997813229622, -298.675957930859, -309.9292117127586, -320.853465714804, -330.7289512679556, + -339.01001631839443, -345.3404207556757, -349.52468900709295, -351.4950882757702, + -351.30843956420114, -349.19099656641225, -345.6266700588403, -341.4559943792539, + -337.9255863077082, -336.6122458269782, -339.1590394133497, -346.81553649131257, + -359.86602049319964, -377.1262582942168, -395.74022625809545, -411.4675647034107, + -419.50864481032374, -415.70559925146244, -397.7689969926942, -366.1070646451192, + -323.9312171058613, -276.5534397525774, -230.0823638333837, -189.94210230887091, + -159.6860391307984, -140.44018425867142, -131.05960164254728, -128.83405351298265, + -130.4331021797765, -132.77378964958245, -133.598664448043, -131.70104308630897, + -126.85857197498154, -119.59605542060795, -110.89414841971599, -101.91886722614183, + -93.80124103359078, -87.46894981676736, -83.52587581326873, -82.18044665496083, + -83.22666982990846, -86.0776105317449, -89.84325002250512, -93.43992023523293, -95.72026823608788, + -95.61789886265437, -92.30211434757715, -85.33026401582156, -74.77072267985237, + -61.2585993757004, -45.950045468071075, -30.363497680157124, -16.130369251753944, + -4.708318964022113, 2.8777155744834513, 6.212983448101315, 5.56140196148832, 1.82269284521927, + -3.6117451657027027, -9.093677781503999, -13.032649828418581, -14.265052388454068, + -12.419628464737789, -8.183075798478319, -3.351394348965721, -0.5798384940055397, + -2.8238203221278537, -12.574618457960362, -31.09374357688851, -57.88946050977006, + -90.62907055552884, -125.54979971296869, -158.26608815757967, -184.7370925829678, + -202.10993695070283, -209.20930494571166, -206.57443001611037, -196.09246662518265, + -180.3847681302668, -162.1383131743949, -143.5430153470272, -125.92848416505213, + -109.62850194687415, -94.06091918539822, -77.99447435013359, -59.96518435506764, + -38.78528395904798, -14.052481549971988, 13.469116037352999, 41.73558027808582, + 67.67668776494482, 87.82105256949613, 99.17127607438123, 100.09686637149439, 90.96891255317551, + 74.31360110106945, 54.401903491602475, 36.37112195454357, 25.122323308838894, 24.299071767105474, + 35.60733796084545, 58.60925251977363, 90.97083953417878, 129.02527666162507, 168.46442192730262, + 204.9931440177265, 234.84578165082002, 255.13458309044964, 264.04869333102295, 260.9405833712635, + 246.33164088648044, 221.85233074958896, 190.1149237954721, 154.50438006044362, 118.87017882929412, + 87.1135432104481, 62.69333693214143, 48.115270426306004, 44.50773230112809, 51.40175250111006, + 66.8050512534824, 87.59010715789734, 110.12403882647259, 130.98843565029367, 147.60355998106695, + 158.5997478392373, 163.85990646713972, 164.25977605801083, 161.2187754414792, 156.2146660131347, + 150.40095323538046, 144.40924932571875, 138.34575838676523, 131.9293358853309, 124.68710400298332, + 116.12724252945577, 105.83914725773546, 93.51279283344225, 78.90515326791086, 61.80006387437529, + 42.00492201327354, 19.40692541387674, -5.917558252890152, -33.573230874271864, -62.74274528444666, + -92.15503567861164, -120.15265932612857, -144.85998708249542, -164.4286388593254, + -177.31873975901365, -182.56747411912127, -179.99849715708353, -170.33317799853793, + -155.174775508723, -136.8502267694815, -118.11425365797368, -101.74838641351045, + -90.11915445215804, -84.78453470268744, -86.24244721296932, -93.89058890376597, + -106.21495538177916, -121.15951154482048, -136.5743658936049, -150.61621971325263, + -161.99385323020581, -170.00781819684602, -174.40729806224658, -175.15102714454724, + -172.18938784282471, -165.3700625780896, -154.51544144123787, -139.64662444269274, + -121.26379807895945, -100.56065686649099, -79.46407820455939, -60.445349586067294, + -46.126337444272174, -38.77528077288329, -39.82686472674805, -49.55634236027296, + -66.99108245478719, -90.07326313903191, -116.01965740549392, -141.78042183347972, + -164.48915325804674, -181.81824580195223, -192.1936050384186, -194.86464874708207, + -189.85637149914302, -177.8444701746055, -159.9936426666511, -137.78899220364173, + -112.8776101254829, -86.92681328173411, -61.4995443419655, -37.946187885812385, + -17.314095829999818, -0.2793256793802397, 12.892666001859144, 22.35087361058214, + 28.623738724825635, 32.547011711050814, 35.16166362154166, 37.59316158505565, 40.9252515429786, + 46.081001294230944, 53.72209705873961, 64.17527284982758, 77.39288792317001, 92.95301794423807, + 110.10246016313017, 127.84307430057432, 145.05732762285365, 160.66258701121782, + 173.77600795348275, 183.86412276724406, 190.84585708339466, 195.11799782738245, + 197.48122046939216, 198.9638867275426, 200.56755651863108, 202.98585640947218, 206.36735753017484, + 210.1940659607301, 213.32513104699035, 214.21331928977128, 211.25089340990704, 203.15829367329167, + 189.30944456733187, 169.9006771128133, 145.91404235801855, 118.88681324803126, 90.55706188617755, + 62.490765591022935, 35.79722544105233, 11.007111877948589, -11.866681217908095, + -33.12202103984155, -53.09189944897362, -71.89093420571022, -89.25892597900295, + -104.53982239765106, -116.79113539836814, -124.98292837106044, -128.22643422109374, + -125.9727544559038, -118.13810580734831, -105.13616058766276, -87.82215430794395, + -67.3716974347737, -45.12687495280552, -22.4432811717548, -0.5658122278379778, 19.44939304727038, + 36.762171246899385, 50.732584441996885, 60.87993008610584, 66.83886074810738, 68.3326809251376, + 65.17495028403165, 57.29773071625812, 44.792950118379224, 27.94724280533802, 7.252695883249127, + -16.615157231293985, -42.849357139418395, -70.58183967568638, -98.95676356556059, + -127.19340520577649, -154.63036780787843, -180.74870760607394] +yaw: [-156.65718192265612, -156.82512813476856, -157.03476785128979, -157.2807477120766, + -157.55369295460645, -157.84461759860156, -158.14778763059294, -158.459737944698, + -158.77686003690096, -159.09532479965736, -159.41325989374954, -159.73163715474024, + -160.0521375845057, -160.3745530176387, -160.69700732848457, -161.0185104942475, + -161.3402653767862, -161.66397674111627, -161.98953153448417, -162.315201866925, + -162.64003295077077, -162.96511555690367, -163.29205326830424, -163.62078022025358, + -163.9496971026068, -164.27788257902984, -164.60632279261657, -164.93652376742696, + -165.26846111422094, -165.60065680035294, -165.93222401150882, -166.26405206032558, + -166.5975532366255, -166.9327393757321, -167.2682466574162, -167.60322357479959, + -167.93847000443222, -168.27530835178302, -168.61378183900877, -168.95263403217712, + -169.29104925446183, -169.62974494472823, -169.9699574813401, -170.31175703012775, + -170.6539879618333, -170.99587071205337, -171.33804689251744, -171.68167070253227, + -172.02683518428148, -172.37247918025702, -172.71785930237908, -173.06354756695924, + -173.4106198177865, -173.75918826314913, -174.10828013599635, -174.45718809095524, + -174.8064204116356, -175.1569783712803, -175.50898997238295, -175.86156501042154, + -176.21403187168687, -176.56684061134436, -176.9209216656706, -177.276415779213, + -177.63250973601328, -177.9885671847553, -178.34498510912093, -178.7026267789968, + -179.0616429301755, -179.42129201479344, -179.7809723347144, -180.14103262348885, + -180.50227258176474, -180.86485046896803, -181.2280913368986, -181.59142740879287, + -181.95516366594228, -182.3200397542152, -182.6862192544363, -183.0530889992954, + -183.42011429540196, -183.78756055865932, -184.15611080378343, -184.525931978699, + -184.8964681246396, -185.2672167028437, -185.638407452449, -186.01067008275334, + -186.3841731854121, -186.75841368027824, -187.132920178221, -187.50789034493104, + -187.88390380611247, -188.26112928818085, -188.6391124973969, -189.01741212654568, + -189.39619709895, -189.7760000696095, -190.15698858912273, -190.53875329031354, + -190.92088183004392, -191.30351746122224, -191.68714886802226, -192.07194129758645, + -192.4575266759198, -192.8435204676572, -193.23004308121793, -193.617542113636, + -194.0061795490326, -194.39562519327265, -194.78552113473754, -195.17596753027587, + -195.56737365493905, -195.95989742408074, -196.35324332333818, -196.74707886293638, + -197.14148632095296, -197.53683929553736, -197.933290967728, -198.33057750888972, + -198.7283906402854, -199.12679692660544, -199.52613681329163, -199.92655820874447, + -200.32782617456402, -200.72965543146975, -201.13209880120448, -201.53546597968077, + -201.93989917925003, -202.34518974707686, -202.751074198292, -203.1575933993827, + -203.56502857939287, -203.97351593447695, -204.38287067560304, -204.7928499203278, + -205.20348419671345, -205.6150284301484, -206.0276125727257, -206.44107345232243, + -206.85518761577225, -207.26997671022124, -207.68567140275596, -208.10239525551458, + -208.5200046331361, -208.93829436247796, -209.35727851912242, -209.77716544140384, + -210.19807222793216, -210.61987285855759, -211.04237931918513, -211.4655992857971, + -211.8897205841901, -212.3148538391931, -212.74088887477996, -213.16765374694393, + -213.5951507769902, -214.02354898389035, -214.45295256340523, -214.8832655549256, + -215.31433103073036, -215.74614688524275, -216.1788649289692, -216.61258302054992, + -217.04721792048107, -217.48262670125752, -217.91880365055158, -218.3558848648318, + -218.79396199767987, -219.23296316292127, -219.67275845698202, -220.11333928225932, + -220.5548274153216, -220.99730847034053, -221.44072066552712, -221.88494618630875, + -222.32997418117233, -222.77591340446227, -223.22284362421576, -223.67071202540126, + -224.119411989995, -224.56893096190808, -225.01936587844727, -225.4707908770044, + -225.9231610756851, -226.37638020375584, -226.83043447547186, -227.2854101278763, + -227.7413759005292, -228.1982939079816, -228.65607742107326, -229.11471183206265, + -229.57427371024147, -230.0348266430826, -230.4963388949898, -230.95873251621126, + -231.42199242410942, -231.88618647266296, -232.35137335201267, -232.81752671335113, + -233.2845766674393, -233.7525079495373, -234.22138057487498, -234.69124859655113, + -235.16209036671597, -235.6338433804674, -236.1064924352654, -236.58009051246535, + -237.05468729088872, -237.53026520903265, -238.00676851209414, -238.48418226093696, + -238.9625531403663, -239.44192671749772, -239.92228896806165, -240.40359029407247, + -240.88581618288202, -241.3690076965991, -241.85320655070794, -242.33840176912216, + -242.8245493571952, -243.31163535831513, -243.79969582627467, -244.2887688805353, + -244.77884615907135, -245.26988875560264, -245.7618833697687, -246.2548616058494, + -246.74885823676817, -247.2438671305236, -247.73985399131797, -248.23680624976333, + -248.7347515676374, -249.23372161331298, -249.73371214631064, -250.2346930390105, + -250.73665250571727, -251.23961472458112, -251.74360849280094, -252.2486311641883, + -252.75465637099336, -253.2616731450968, -253.7697025952815, -254.27877087145927, + -254.78887666179244, -255.29999698245618, -255.81212170080903, -256.3252692292884, + -256.8394632842479, -257.3547036618484, -257.87097041693903, -258.3882542568401, + -258.9065712326523, -259.4259428302655, -259.94636975763615, -260.4678347920479, + -260.99032947413946, -261.5138677937398, -262.0384691984253, -262.564135138717, + -263.09085082541867, -263.6186086167544, -264.14742070931345, -264.677304693403, + -265.208262616922, -265.7402818609308, -266.27335557821664, -266.807494410878, -267.3427142618617, + -267.8790176526088, -268.4163938951752, -268.9548369081826, -269.4943559912946, + -270.0349655189502, -270.5766683811865, -271.11945560417905, -271.6633218393314, + -272.20827523166446, -272.75432877508274, -273.3014856399149, -273.84973837039325, + -274.3990823145224, -274.94952462848454, -275.50107706299923, -276.0537429949804, + -276.6075163099427, -277.16239301421695, -277.7183794210768, -278.27548616510694, + -278.8337167688488, -279.39306630014715, -279.95353138416385, -280.5151176192923, + -281.07783464110923, -281.64168606790315, -282.2066680073117, -282.77277766335567, + -283.3400200314943, -283.90840385592054, -284.47793281036434, -285.0486039147941, + -285.6204149122557, -286.19337029282093, -286.7674780078702, -287.3427417545014, + -287.91915935135097, -288.4967290413007, -289.07545489372984, -289.655344157199, + -290.2364005271319, -290.81862251976554, -291.40200883968265, -291.986563208829, + -292.57229225484906, -293.15919965241676, -293.7472845257627, -294.3365460044106, + -294.9269875259936, -295.51861517154896, -296.1114325809504, -296.70543940721166, + -297.3006351696594, -297.89702307577414, -298.49460872719874, -299.0933957191521, + -299.69338416362336, -300.29457393640496, -300.8969680610969, -301.50057172055426, + -302.10538845895877, -302.71141878594295, -303.31866290235337, -303.92712368726046, + -304.53680595921657, -305.14771320782285, -305.7598462866421, -306.3732056921633, + -306.98779419222967, -307.6036162899247, -308.2206754190174, -308.83897273011434, + -309.4585089879684, -310.0792868772328, -310.70131062915857, -311.3245836222539, + -311.94910726337764, -312.5748825602012, -313.20191213766174, -313.830199994053, + -314.45974945461137, -315.0905621470852, -315.72263929872355, -316.3559834942804, + -316.9905985336233, -317.62648769178344, -318.2636527868513, -318.9020952442645, + -319.5418176247443, -320.18282356031045, -320.8251162796443, -321.4686977648914, + -322.11356962017334, -322.7597343954331, -323.4071955818418, -324.0559563661364, + -324.7060188719847, -325.35738486448656, -326.01005689360136, -326.6640383334178, + -327.3193323334846, -327.97594113975754, -328.63386666231656, -329.29311145984894, + -329.95367881022037, -330.6155718307366, -331.2787928732944, -331.94334397856255, + -332.6092277209164, -333.2764473002516, -333.9450058066361, -334.6149056840792, + -335.286149090949, -335.9587386228065, -336.6326774175029, -337.3079685428293, -337.9846145232684, + -338.6626176233944, -339.3419804642377, -340.022706135458, -340.7047976874095, -341.3882577153029, + -342.07308857971486, -342.75929293043066, -343.4468738209348, -344.1358342888007, + -344.826176991857, -345.5179043776647, -346.2110191272336, -346.90552426826605, + -347.601422829986, -348.29871752613354, -348.99741088331945, -349.6975056155889, + -350.3990047338262, -351.1019112630828, -351.80622796750447, -352.51195744580275, + -353.21910244634364, -353.9276659709034, -354.63765104426665, -355.3490604765017, + -356.0618969323631, -356.7761631954104, -357.4918622649251, -358.2089971690505, + -358.927570760162, -359.64758576380353, -360.36904499927863, -361.09195146903556, + -361.8163082079192, -362.5421181077289, -363.2693839502654, -363.99810859088376, + -364.72829504003204, -365.4599463423245, -366.1930654267157, -366.9276551273741, + -367.6637183358344, -368.4012580746632, -369.14027740104433, -369.88077927933233, + -370.6227665927477, -371.3662422690045, -372.11120934629133, -372.8576708969081, + -373.60562991928094, -374.3550893428728, -375.1060521314913, -375.85852134192436, + -376.6125000638935, -377.3679913289216, -378.12499811035156, -378.88352340794614, + -379.64357029961894, -380.4051418945974, -381.1682412568148, -381.9328714015242, + -382.69903536427796, -383.46673624626146, -384.2359771780845, -385.0067612556417, + -385.7790915344696, -386.5529710857383, -387.32840303572766, -388.1053905381184, + -388.8839367205077, -389.66404467738937, -390.4457175153868, -391.2289583874262, + -392.0137704717747, -392.8001569276271, -393.58812088735715, -394.37766549287124, + -395.1687939249686, -395.96150938755676, -396.7558150704628, -397.55171414010636, + -398.3492097647753, -399.1483051294121, -399.94900339695914, -400.7513076097339, + -401.55522051642305, -402.3607442126537, -403.1678793129748, -403.97662313703535, + -404.78696601570533, -405.5988841676912, -406.41232657422364, -407.22719193743313, + -408.04329038092243, -408.86028352164305, -409.67759674852243, -410.49430019593217, + -411.30896127385165, -412.11948227644524, -412.9229503097521, -413.71553969665734, + -414.49251289191534, -415.2483582419702, -415.9770785993685, -416.6726079072575, + -417.329295131207, -417.94237131855783, -418.5083166266741, -419.02506982614034, + -419.4920624207183, -419.91009801422723, -420.28112301456906, -420.60794297702034, + -420.89393277454684] +z: [8.660695913847082, 66.39862553863676, 127.55801351244567, 180.5416567670594, 210.78937741627567, + 205.98510123498664, 161.75492603580668, 84.86410600259634, -7.938204250916348, -95.46180594158663, + -159.6160710741801, -191.12823315769305, -191.1077383247354, -168.75160790760808, + -136.7976917935674, -106.70965336063549, -85.49283736476575, -74.97102632230975, + -72.98493565768426, -75.44155069558714, -78.2857794410305, -78.76734664689184, -75.79970900381551, + -69.60926545532763, -61.02352026219444, -50.731204460839145, -38.784872157834826, + -24.48063415135472, -6.604904189438815, 16.003062940241094, 43.61116166930451, 74.59937262542272, + 104.69471826704422, 127.10591770220823, 134.08234145821157, 119.62663119956167, + 82.25187144352125, 26.508018246556365, -37.60809657045731, -97.50294343727298, -142.05752447785434, + -165.01189926649766, -166.22428468257075, -150.6580066881632, -125.88440608451583, + -99.27427444087671, -76.01596349902346, -58.51176921704588, -46.89688221845846, + -40.0319509439571, -36.428785825300736, -34.84658879993211, -34.526729879516196, + -35.16461507569286, -36.74598621919215, -39.344955846661044, -42.94609674951552, + -47.33366981479339, -52.08037130315903, -56.64311306447958, -60.522268045019175, + -63.40062822000451, -65.19777275447987, -66.03688501834353, -66.16552673973, -65.87798906520248, + -65.46886057404075, -65.21885341822762, -65.38814742633711, -66.18701058321993, + -67.71310218370519, -69.87490985875417, -72.34304758340024, -74.57596554892733, + -75.9445158717209, -75.92859524663281, -74.30883578638903, -71.26712435663396, -67.34312951723155, + -63.24836193520065, -59.60140045704585, -56.690265423268045, -54.35501112675093, + -52.025856935869335, -48.89272594930154, -44.14172332474001, -37.177555708112656, + -27.766047180552132, -16.071976274482385, -2.617381566487062, 11.77309724198873, + 25.976102073264208, 38.53687298107297, 47.718551492321396, 51.76605423223608, 49.38878979505458, + 40.290429255399275, 25.491354807794263, 7.246430327597227, -11.475853168764028, + -27.772349573580907, -39.515015426681174, -45.713572144488516, -46.46431428981413, + -42.61798316112623, -35.37703866753643, -25.9934677516497, -15.628487382325641, + -5.320192334714672, 4.040136476501501, 11.774699698468378, 17.500102100664304, 21.15959975218581, + 22.97185786614963, 23.31019232970664, 22.558042956454567, 20.99209210295209, 18.729017300170803, + 15.74651164213902, 11.966823265504356, 7.376265857048469, 2.1431266936820315, -3.3126251807763514, + -8.343814311527554, -12.216153231932507, -14.333645421496714, -14.465721023384898, + -12.87582842967554, -10.283561071346963, -7.656458262095206, -5.898235147224569, + -5.548464248913495, -6.607585778672925, -8.55034572367405, -10.517780185318616, + -11.61186892570347, -11.181289206074855, -8.998149061838516, -5.278041737321532, + -0.5603284740797135, 4.485651709130345, 9.246488587633161, 13.28299530174339, 16.36756662020893, + 18.466571978647558, 19.704100343285322, 20.32815531831131, 20.677451138832144, 21.13258932476601, + 22.03831545911599, 23.60334269570648, 25.810407707445663, 28.386041772609907, 30.87468716575818, + 32.83047926730481, 34.087238764628125, 35.01230737770861, 36.621360874602765, 40.45070511179779, + 48.15601919282285, 60.917623297793895, 78.84123893360714, 100.59365670804146, 123.46645783546617, + 143.91789347099578, 158.44750364654655, 164.50260889528977, 161.097954064323, 148.96575759387107, + 130.25873364860564, 107.99215885210386, 85.46056101968273, 65.78797889606493, 51.62771188476158, + 44.92502099696882, 46.66619035735277, 56.63547648764704, 73.29795847361908, 93.94368722527568, + 115.14660187881398, 133.44963208761388, 146.06803902842015, 151.38700573272683, + 149.12923984177337, 140.2151559819424, 126.44367536795221, 110.13915686176617, 93.84830850562516, + 80.0820870481205, 71.04848324787652, 68.34789291609472, 72.6765134206983, 83.63977979446459, + 99.77023650435606, 118.77456269522624, 137.94387073801647, 154.60399035259434, 166.49431469790014, + 172.0327438962138, 170.4928109096748, 162.13218328502208, 148.26316136662118, 131.1890050423139, + 113.90461235170372, 99.51589491572973, 90.46166485727544, 87.7622777984477, 90.58353615476729, + 96.33891906881483, 101.3726007035209, 102.04139409749659, 95.84221536672281, 82.19477689643513, + 62.61034213227201, 40.20548872373059, 18.75852748002458, 1.657957295481827, -8.90300742366735, + -12.266306640477236, -9.163238878856461, -1.233167864960895, 9.564495981683649, + 21.43992022175332, 33.051379335207095, 43.56592729978036, 52.568382283902764, 59.9082904825656, + 65.55991544152265, 69.53869459501789, 71.88526118098649, 72.70284621092118, 72.21843649851635, + 70.83368992994872, 69.13827315439758, 67.87334691224184, 67.8505290321987, 69.8442171675916, + 74.47726967536957, 82.11311705302812, 92.75985141769938, 105.99298201503689, 120.91543676261807, + 136.18772657541675, 150.16300634223884, 161.13869607756567, 167.68973106972928, + 168.99857035076562, 165.07365193284676, 156.7707462316869, 145.59585156800276, 133.34673828809346, + 121.70779304048943, 111.92368383650255, 104.63857613545397, 99.9202392953937, 97.42415696314441, + 96.61636301843436, 96.97307879858319, 98.1025495789197, 99.77349073377508, 101.86901694512392, + 104.30470000982287, 106.95197406963027, 109.5967587731362, 111.94423545513219, 113.66135899930364, + 114.4352894158048, 114.02254875294729, 112.27073376540599, 109.10848671943762, 104.5142402049289, + 98.48427593329139, 91.02225206964965, 82.16482996592228, 72.04340823046537, 60.9644383318998, + 49.47596335726721, 38.38208716267078, 28.675265153703773, 21.380090006656577, 17.336295088765645, + 16.98025827773935, 20.1977232072601, 26.30579134436739, 34.1820237918946, 42.50787279716556, + 50.05364436655743, 55.91974464196498, 59.66868428102189, 61.32427965788826, 61.25993059906409, + 60.02892932141973, 58.19725874007958, 56.22498874652909, 54.41596991142811, 52.9291999133868, + 51.82801400185255, 51.138336589289274, 50.892598938155075, 51.146726044509975, 51.96875114225244, + 53.40564661532219, 55.43870808852528, 57.93823519360839, 60.62739916553108, 63.064899619087015, + 64.65655841637202, 64.7055801259163, 62.50676710339701, 57.47927602641731, 49.31661470793777, + 38.11706927966492, 24.4510077185352, 9.330391041091739, -5.928968195692799, -19.927807358089126, + -31.432334046946572, -39.58564206883777, -44.02742340146397, -44.89862410201391, + -42.74448812517229, -38.35747836190584, -32.61145550528262, -26.32954437918894, + -20.20719977067912, -14.789151242435427, -10.48250380540003, -7.582119063177211, + -6.287826913269372, -6.702621306839947, -8.81253570937736, -12.45851173987955, -17.315467703254903, + -22.89273066518226, -28.563759038476075, -33.62421205979971, -37.369384313159216, + -39.17768128465537, -38.587042278968454, -35.35458955341083, -29.49368865852931, + -21.285110785761752, -11.260045417067115, -0.15386876206445768, 11.167509802528333, + 21.80199162922029, 30.914335137198602, 37.83281681446311, 42.1250528920106, 43.6424129336149, + 42.527536031710504, 39.1838083377227, 34.20896823571095, 28.298619788384883, 22.130902226323823, + 16.250738211119412, 10.978234041445447, 6.366580287176825, 2.2267366046830612, -1.780708650958427, + -6.007319377392964, -10.670867462380551, -15.74701218807577, -20.935394624211764, + -25.71475763924228, -29.473174580935297, -31.674804797246836, -32.01284071766054, + -30.503625724620882, -27.49687795069467, -23.603695718045664, -19.567702223835404, + -16.117560259075034, -13.838347771722535, -13.087536620644972, -13.9646827725496, + -16.328965936199715, -19.84970900489466, -24.07290465216959, -28.48977493682292, + -32.59854442331317, -35.95539956520115, -38.2137508958304, -39.15228104770502, -38.692332219912934, + -36.90456601359803, -34.004068329378526, -30.332692841352, -26.327899158206776, + -22.478863588907455, -19.27298194233058, -17.13832959626637, -16.389291255919126, + -17.18279456348231, -19.4913537172405, -23.09696936219348, -27.60749078959273, -32.494672428897246, + -37.15076766150979, -40.95788530079359, -43.36162493187602, -43.93846520775633, + -42.446152023679346, -38.848786666305784, -33.313331043792914, -26.180492183033934, + -17.918310843908614, -9.069298172978261, -0.20074671809499756, 8.136492955152297, + 15.441031108092213, 21.29036758626846, 25.372508549246056, 27.522718958733332, 27.762289494620244, + 26.32911239456066, 23.684885172768034, 20.483910329661786, 17.495388855951955, 15.484035434486861, + 15.069215691030688, 16.59509470023471, 20.047924906971748, 25.048393934050562, 30.927904484071945, + 36.87324018466698, 42.10253497425529, 46.024752154331864, 48.33939153393884, 49.0517871424882, + 48.40580399084933, 46.76081807159332, 44.45507771693884, 41.69825116535199, 38.52289882103032, + 34.803261444171405, 30.327999320714078, 24.898546537842716, 18.420586405729864, + 10.962638364832946, 2.769145218537321, -5.769675941714455, -14.177570162462116, + -21.96287190097139, -28.682775714701705, -33.98993062535793, -37.65744218731808, + -39.584533342726225, -39.78848208739123, -38.38883996706238, -35.58823507210259, + -31.651659030216656, -26.884305680876444, -21.60752145164577, -16.133301088860883, + -10.739449278450719, -5.649094917111724, -1.018762093669203, 3.061876393936713, + 6.558023261640048, 9.468369818928435, 11.800068844557336, 13.547269650304042, 14.679655966520992, + 15.144569931188034, 14.882211925027542, 13.849398415516339, 12.04474239771272, 9.52770863530006, + 6.425839876890219, 2.9278589763920806, -0.735815798162439, -4.320509611419369, -7.596486438809738, + -10.37389619761122, -12.519116524619744, -13.960610192047202, -14.685038419335728, + -14.726561926961596, -14.153639370214792, -13.057964531219815, -11.549353043113381, + -9.758421987878664, -7.846069969532445, -6.015682302494043, -4.521627491850837, + -3.6670759284008367, -3.78622388922754, -5.210529940531722, -8.224269075041521, + -13.019390902549098, -19.661194514880624, -28.07379403356179, -38.04868886403423, + -49.2733259570762, -61.37184021856482, -73.94857243204568, -86.62633925391154, -99.07453382284537, + -111.02550604149512] +pitch: [-157.8684778032057, -158.3698356698477, -159.00269434786534, -159.7713293302798, + -160.66230277415343, -161.6470704856313, -162.69256942436138, -163.77220996055956, + -164.87013794172887, -165.9791087337121, -167.09653844180943, -168.22171491746042, + -169.35450952912657, -170.49493872451055, -171.6430483690157, -172.79888945081638, + -173.96251395289818, -175.13397428130384, -176.31332320177768, -177.50061383591552, + -178.69589966306725, -179.89923452271375, -181.1106726168898, -182.33026851262574, + -183.55807714440536, -184.7941538166402, -186.0385542061607, -187.29133436472392, + -188.5525507215386, -189.8222600858066, -191.10051964928178, -192.38738698884652, + -193.6829200691045, -194.98717724499227, -196.3002172644072, -197.62209927085402, + -198.95288280610868, -200.29262781290038, -201.64139463761154, -202.99924403299593, + -204.36623716091532, -205.7424355950942, -207.12790132389352, -208.52269675310262, + -209.92688470875024, -211.3405284399345, -212.76369162167163, -214.19643835776446, + -215.6388331836894, -217.09094106950369, -218.55282742277168, -220.02455809151112, + -221.50619936715893, -222.99781798755748, -224.49948113996047, -226.0112564640592, + -227.53321205502968, -229.0654164665993, -230.60793871413497, -232.16084827775205, + -233.72421510544342, -235.2981096162307, -236.88260270333552, -238.47776573737366, + -240.08367056956885, -241.70038953498985, -243.3279954558082, -244.96656164457897, + -246.6161619075422, -248.27687054794822, -249.9487623694034, -251.63191267924097, + -253.32639729191183, -255.03229253240016, -256.7496752396611, -258.4786227700819, + -260.2192130009664, -261.9715243340426, -263.73563569899414, -265.5116265570161, + -267.2995769043935, -269.0995672761047, -270.9116787494495, -272.7359929477005, + -274.57259204377965, -276.42155876396043, -278.28297639159297, -280.15692877085615, + -282.04350031053394, -283.9427759878175, -285.85484135213284, -287.77978252899436, + -289.71768622388356, -291.66863972615556, -293.6327309129702, -295.61004825325045, + -297.6006808116674, -299.60471825265194, -301.62225084443344, -303.65336946310623, + -305.69816559672216, -307.7567313494122, -309.82915944553446, -311.91554323385157, + -314.0159766917348, -316.1305544293973, -318.2593716941557, -320.40252437472, -322.5601090055133, + -324.7322227710197, -326.91896351016163, -329.12042972070725, -331.33672056370636, + -333.56793586795794, -335.81417613450594, -338.0755425411669, -340.3521369470865, + -342.64406189732756, -344.9514206274901, -347.27431706835876, -349.61285585057675, + -351.9671423093153, -354.337282488798, -356.72338314601444, -359.1255517507706, + -361.5438964706455, -363.97852609840754, -366.4295497760087, -368.8970760521407, + -371.3812099206837, -373.88204421775976, -376.39963653729615, -378.9339521671109, + -381.48473467467466, -384.0512382159208, -386.631726774158, -389.2226378464643, + -391.81736161291474, -394.40474356794095, -396.9676796187925, -399.48243974549047, + -401.9194238634856, -404.2457230176494, -406.4291524303254, -408.4426925007373, + -410.267999574875, -411.89703489626, -413.33164749245606, -414.5816483484149, -415.66220963815863] +roll: [-157.8684778032057, -158.3698356698477, -159.00269434786534, -159.7713293302798, + -160.66230277415343, -161.6470704856313, -162.69256942436138, -163.77220996055956, + -164.87013794172887, -165.9791087337121, -167.09653844180943, -168.22171491746042, + -169.35450952912657, -170.49493872451055, -171.6430483690157, -172.79888945081638, + -173.96251395289818, -175.13397428130384, -176.31332320177768, -177.50061383591552, + -178.69589966306725, -179.89923452271375, -181.1106726168898, -182.33026851262574, + -183.55807714440536, -184.7941538166402, -186.0385542061607, -187.29133436472392, + -188.5525507215386, -189.8222600858066, -191.10051964928178, -192.38738698884652, + -193.6829200691045, -194.98717724499227, -196.3002172644072, -197.62209927085402, + -198.95288280610868, -200.29262781290038, -201.64139463761154, -202.99924403299593, + -204.36623716091532, -205.7424355950942, -207.12790132389352, -208.52269675310262, + -209.92688470875024, -211.3405284399345, -212.76369162167163, -214.19643835776446, + -215.6388331836894, -217.09094106950369, -218.55282742277168, -220.02455809151112, + -221.50619936715893, -222.99781798755748, -224.49948113996047, -226.0112564640592, + -227.53321205502968, -229.0654164665993, -230.60793871413497, -232.16084827775205, + -233.72421510544342, -235.2981096162307, -236.88260270333552, -238.47776573737366, + -240.08367056956885, -241.70038953498985, -243.3279954558082, -244.96656164457897, + -246.6161619075422, -248.27687054794822, -249.9487623694034, -251.63191267924097, + -253.32639729191183, -255.03229253240016, -256.7496752396611, -258.4786227700819, + -260.2192130009664, -261.9715243340426, -263.73563569899414, -265.5116265570161, + -267.2995769043935, -269.0995672761047, -270.9116787494495, -272.7359929477005, + -274.57259204377965, -276.42155876396043, -278.28297639159297, -280.15692877085615, + -282.04350031053394, -283.9427759878175, -285.85484135213284, -287.77978252899436, + -289.71768622388356, -291.66863972615556, -293.6327309129702, -295.61004825325045, + -297.6006808116674, -299.60471825265194, -301.62225084443344, -303.65336946310623, + -305.69816559672216, -307.7567313494122, -309.82915944553446, -311.91554323385157, + -314.0159766917348, -316.1305544293973, -318.2593716941557, -320.40252437472, -322.5601090055133, + -324.7322227710197, -326.91896351016163, -329.12042972070725, -331.33672056370636, + -333.56793586795794, -335.81417613450594, -338.0755425411669, -340.3521369470865, + -342.64406189732756, -344.9514206274901, -347.27431706835876, -349.61285585057675, + -351.9671423093153, -354.337282488798, -356.72338314601444, -359.1255517507706, + -361.5438964706455, -363.97852609840754, -366.4295497760087, -368.8970760521407, + -371.3812099206837, -373.88204421775976, -376.39963653729615, -378.9339521671109, + -381.48473467467466, -384.0512382159208, -386.631726774158, -389.2226378464643, + -391.81736161291474, -394.40474356794095, -396.9676796187925, -399.48243974549047, + -401.9194238634856, -404.2457230176494, -406.4291524303254, -408.4426925007373, + -410.267999574875, -411.89703489626, -413.33164749245606, -414.5816483484149, -415.66220963815863] +x: [-127.90269211939827, -131.2678097441197, -136.70543077518892, -142.80201308388703, + -147.68698584712374, -150.43090024970653, -151.93731066347436, -153.97975151042488, + -157.1611591307891, -160.11613483598717, -160.9024782819122, -159.04126159161888, + -156.07990100822514, -154.23554261200883, -154.56584426858808, -156.35317090044515, + -158.04548754985666, -158.68146511423873, -158.50567928984265, -158.41585507282485, + -158.95307629614172, -159.79740407636362, -160.1014847915644, -159.23338066405643, + -157.30653996810443, -155.20316918530557, -154.14843270998995, -155.05581305813232, + -157.9836096719787, -162.09393936591388, -166.22381774921027, -169.63236007909416, + -172.26672230472926, -174.36451975460542, -175.90099493885046, -176.49219286857985, + -175.798054128794, -173.93808909631377, -171.4925114649683, -169.12343773356466, + -167.17664041477232, -165.5708829941525, -164.00639105353508, -162.2887264315374, + -160.51670606493755, -159.0215878949382, -158.1431878867084, -158.0223901064516, + -158.5297306199403, -159.33939301424593, -160.09571223386598, -160.59095964674498, + -160.8477855836447, -161.02451272164438, -161.17795428146462, -161.05434333340307, + -160.10141346846075, -157.75281536914142, -153.82556545048772, -148.76090920354517, + -143.52247341799338, -139.17693928388738, -136.36789818536403, -134.95124204564573, + -133.9874074915166, -132.1254263634103, -128.22234525346727, -121.90425650473114, + -113.78832718004412, -105.27070727944219, -98.02475400195476, -93.46091461275282, + -92.30826635434332, -94.3390728032595, -98.26494379864691, -101.94885284908618, + -103.04849210213153, -99.9163743951314, -92.28166421723154, -81.29521224087058, + -68.95793224897206, -57.37532906713539, -48.286218251495455, -42.9311115531321, + -41.97664928415379, -45.25819014572354, -51.473054204178524, -58.23430997331244, + -62.75013392767508, -62.9223262047309, -58.280188016636316, -50.21077109811404, + -41.35681129895958, -34.4990081709638, -31.449867093567427, -32.42754723012978, + -36.15011519371447, -40.58089839438473, -43.94788650904792, -45.50317129106411, + -45.62757380883436, -45.28654859482272, -45.24609917358149, -45.57109091310874, + -45.68440108090419, -44.8673235962936, -42.817117026428, -39.89366389594422, -36.92267974205595, + -34.701751190785245, -33.51756732176649, -32.957648526197254, -32.118390119251, + -30.08038179415433, -26.373159258276946, -21.169894158050823, -15.12315035159766, + -8.966988036828536, -3.135614678740315, 2.3860480980408405, 7.925467603882083, 13.822714648008436, + 20.175371018999993, 26.7822639200393, 33.293134470121245, 39.42975802508738, 45.09936774714966, + 50.30205732126428, 54.88772677310383, 58.342229552134924, 59.78844212222738, 58.2636935332989, + 53.15779898393836, 44.5830880558185, 33.47443622424592, 21.363593342632985, 9.940566867866028, + 0.6026213709298839, -5.8399344345675575, -9.240708782689756, -10.022124423046609, + -8.998133886903553, -7.186216889606415, -5.63851099666342, -5.297145100741871, -6.879790086756265, + -10.809823525239507, -17.202765649327844, -25.90589109754088, -36.57282515330877] +y: [-95.1053320679519, -102.57942707703143, -115.79349366134936, -132.91150346877552, + -149.8018623891795, -161.15737205071278, -162.80068928833558, -153.8389080682052, + -137.30710232263985, -118.90086421096659, -104.39325930921773, -97.04253564148495, + -96.49753527612377, -99.89130313663716, -104.21834166352956, -108.11727712225031, + -111.8967411258529, -116.28298547066109, -121.23895629376239, -125.73427933608644, + -128.36883519321356, -128.19087124969568, -125.13859527382397, -120.00974008025909, + -114.2430140709469, -109.69248942902398, -108.19817332485465, -110.78624497979699, + -116.93512079376003, -124.6998747734192, -131.86718572831617, -137.27226089357666, + -141.20696974324156, -144.759479700215, -148.87001560457742, -153.88261513985736, + -159.68734476449134, -166.07420632507709, -172.96469725505096, -180.3920700150901, + -188.21937883314945, -195.72926730208002, -201.41736840430042, -203.31624017701841, + -199.82553623123383, -190.59338598173065, -176.88869378565937, -161.2238841338048, + -146.45822831996625, -134.87673586346156, -127.65308031796329, -124.80090469016707, + -125.45619356179108, -128.27549428216608, -131.84101844992867, -135.07530215082386, + -137.65337624491679, -140.25913711418366, -144.43087684329745, -151.8938368423789, + -163.67339640117606, -179.56328628213708, -198.32489175456183, -218.42489246839165, + -238.7164724172193, -258.60743976458997, -277.7429960783446, -295.56813001110913, + -311.0788097160963, -322.80675460113724, -328.93908316116415, -327.56037608593266, + -317.12330108413994, -297.14900905724227, -268.84769440086353, -235.16971112565426, + -200.04553112593794, -167.12032560737575, -138.66238790035453, -115.19890389580594, + -95.95279511481895, -79.72820892106228, -65.76755588438334, -54.22790801461051, + -46.13190044244114, -42.84004879066222, -45.24539383402826, -52.99497625151277, + -64.07018690090207, -74.97804538426473, -81.59594832574722, -80.41038856548796, + -69.63949990787273, -49.72242741663517, -22.966995663147852, 7.39427794510237, 38.19309913101399, + 66.89173432062842, 91.64289937473961, 111.13226208974095, 124.5518791391896, 131.77418236143038, + 133.46586276149228, 130.83435645169016, 124.99732180672979, 116.333135612117, 104.26680906118598, + 87.68178840678189, 65.74204569072383, 38.662165827940036, 8.014089907704136, -23.571276834584964, + -53.142563231420375, -78.2522773792973, -97.51345531907658, -110.70725605031265, + -118.5016237829998, -122.00842291412158, -122.404338258332, -120.6995916626668, + -117.59445385917022, -113.3407631971568, -107.62417911491204, -99.58313488352168, + -88.06247336126238, -72.06061544362858, -51.18535335279474, -25.910478578073977, + 2.4559772454057507, 32.01779445834423, 60.591762572708525, 85.94477375005923, 105.9576058260377, + 118.78964738570927, 123.11631379953998, 118.43389190043557, 105.33379726489348, + 85.60032102947753, 62.014359671431286, 37.84509121681435, 16.144470009647492, -0.9347463893425387, + -12.559784399157243, -19.23707231899498, -22.45859920077777, -24.150622679079273, + -26.143960951682608, -29.820707661306084, -35.98068482695456, -44.88212815398885] +yaw: [-157.8684778032057, -158.3698356698477, -159.00269434786534, -159.7713293302798, + -160.66230277415343, -161.6470704856313, -162.69256942436138, -163.77220996055956, + -164.87013794172887, -165.9791087337121, -167.09653844180943, -168.22171491746042, + -169.35450952912657, -170.49493872451055, -171.6430483690157, -172.79888945081638, + -173.96251395289818, -175.13397428130384, -176.31332320177768, -177.50061383591552, + -178.69589966306725, -179.89923452271375, -181.1106726168898, -182.33026851262574, + -183.55807714440536, -184.7941538166402, -186.0385542061607, -187.29133436472392, + -188.5525507215386, -189.8222600858066, -191.10051964928178, -192.38738698884652, + -193.6829200691045, -194.98717724499227, -196.3002172644072, -197.62209927085402, + -198.95288280610868, -200.29262781290038, -201.64139463761154, -202.99924403299593, + -204.36623716091532, -205.7424355950942, -207.12790132389352, -208.52269675310262, + -209.92688470875024, -211.3405284399345, -212.76369162167163, -214.19643835776446, + -215.6388331836894, -217.09094106950369, -218.55282742277168, -220.02455809151112, + -221.50619936715893, -222.99781798755748, -224.49948113996047, -226.0112564640592, + -227.53321205502968, -229.0654164665993, -230.60793871413497, -232.16084827775205, + -233.72421510544342, -235.2981096162307, -236.88260270333552, -238.47776573737366, + -240.08367056956885, -241.70038953498985, -243.3279954558082, -244.96656164457897, + -246.6161619075422, -248.27687054794822, -249.9487623694034, -251.63191267924097, + -253.32639729191183, -255.03229253240016, -256.7496752396611, -258.4786227700819, + -260.2192130009664, -261.9715243340426, -263.73563569899414, -265.5116265570161, + -267.2995769043935, -269.0995672761047, -270.9116787494495, -272.7359929477005, + -274.57259204377965, -276.42155876396043, -278.28297639159297, -280.15692877085615, + -282.04350031053394, -283.9427759878175, -285.85484135213284, -287.77978252899436, + -289.71768622388356, -291.66863972615556, -293.6327309129702, -295.61004825325045, + -297.6006808116674, -299.60471825265194, -301.62225084443344, -303.65336946310623, + -305.69816559672216, -307.7567313494122, -309.82915944553446, -311.91554323385157, + -314.0159766917348, -316.1305544293973, -318.2593716941557, -320.40252437472, -322.5601090055133, + -324.7322227710197, -326.91896351016163, -329.12042972070725, -331.33672056370636, + -333.56793586795794, -335.81417613450594, -338.0755425411669, -340.3521369470865, + -342.64406189732756, -344.9514206274901, -347.27431706835876, -349.61285585057675, + -351.9671423093153, -354.337282488798, -356.72338314601444, -359.1255517507706, + -361.5438964706455, -363.97852609840754, -366.4295497760087, -368.8970760521407, + -371.3812099206837, -373.88204421775976, -376.39963653729615, -378.9339521671109, + -381.48473467467466, -384.0512382159208, -386.631726774158, -389.2226378464643, + -391.81736161291474, -394.40474356794095, -396.9676796187925, -399.48243974549047, + -401.9194238634856, -404.2457230176494, -406.4291524303254, -408.4426925007373, + -410.267999574875, -411.89703489626, -413.33164749245606, -414.5816483484149, -415.66220963815863] +z: [27.487690876753174, 15.337246293823744, -6.519501713577381, -32.17378390020878, + -52.95176670190418, -61.55800766961198, -55.94481745228971, -40.02071700799717, + -21.554360535385083, -9.07142702001839, -8.346860847184745, -19.368396082981224, + -36.19083663458397, -50.83073806660286, -58.44885149238343, -59.53297302408236, + -57.86810718901493, -56.968343215030046, -58.092142359329614, -60.51600905693699, + -62.7444480158786, -63.35926464089802, -61.25799874825209, -55.70616111817719, -46.57263622352219, + -34.71402685096095, -22.12817933348742, -11.450779533332875, -4.771195360275572, + -2.4341668275669868, -2.815083023486069, -3.436788885507724, -2.639407740293576, + -0.49257916688823505, 1.6634057300540754, 2.4787157173774994, 1.5816568083905458, + -0.3081254457303777, -1.9465496236895794, -2.235946638392112, -0.587599350222585, + 3.0830448889107243, 8.585148043725034, 15.741871127579394, 24.588070885472288, 35.317967966866895, + 47.92047635319786, 61.70329222579224, 75.14570192551099, 86.40468533274036, 94.26512130476762, + 98.82097106678889, 101.29642767567256, 103.1652314081855, 105.33907861244916, 107.99131029859556, + 110.83900010952557, 113.33429248514499, 114.58664344276714, 113.36413234926317, + 108.50758731712286, 99.61208997647628, 87.50344465477376, 74.1768095655181, 62.22532988939497, + 54.03291376712356, 51.07089312083894, 53.5800909155043, 60.73095402033792, 71.08141473731635, + 83.00300927492155, 94.88248494409528, 105.19436856609353, 112.66234904016706, 116.5532902732588, + 116.89064945934301, 114.34290812223043, 109.7932211178687, 103.88036334169092, 96.83894755858785, + 88.72518958612778, 79.81089200833654, 70.81116373157523, 62.753896533262505, 56.569823287756314, + 52.671108932076606, 50.767905449313446, 49.98887184490788, 49.18533829409316, 47.24706022687302, + 43.33944398884515, 37.069957898660974, 28.60206393388892, 18.673743747050306, 8.448948216774712, + -0.7972607166725888, -8.036081276408424, -12.709833208187701, -14.735329114490181, + -14.3542414036342, -11.989651298267246, -8.214474065551915, -3.796221322329577, + 0.3127831108565613, 3.1349675066051397, 3.941404446156438, 2.4944463514526865, -0.8862482050049294, + -5.474872423279538, -10.422540767592642, -15.02656078493345, -18.872907062153832, + -21.8268990816907, -23.925876718830928, -25.25373825675896, -25.85191903924253, + -25.679848344814015, -24.61517234703336, -22.486711991951278, -19.140322650202826, + -14.528471985404261, -8.792284878821462, -2.2948582536620714, 4.415692468816276, + 10.697174253055142, 15.917112597767035, 19.536456023535326, 21.170365945226813, + 20.644086856810723, 18.052446724627877, 13.800892715061188, 8.584501211461651, 3.2736044258695483, + -1.2815934000814575, -4.466744501254597, -6.057544821897366, -6.251111895303721, + -5.565299343444047, -4.652409384326363, -4.10510631771834, -4.32579622671807, -5.493001584407323, + -7.61115583178264, -10.59834849765868, -14.363422130581966, -18.8448260745628, -24.011142744024184, + -29.84051753345265, -36.298155831550744, -43.32322171459229] From de016ea7075be38c0f98ca4544e0f027d7fd4e19 Mon Sep 17 00:00:00 2001 From: Alex Mitrevski Date: Fri, 26 Apr 2019 19:00:19 +0200 Subject: [PATCH 78/96] [move_arm_action] Added a dummy recovering state that sleeps for 5s --- .../ros/src/mdr_move_arm_action/action_states.py | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_move_arm_action/ros/src/mdr_move_arm_action/action_states.py b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_move_arm_action/ros/src/mdr_move_arm_action/action_states.py index ecb50344d..ad66ca749 100644 --- a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_move_arm_action/ros/src/mdr_move_arm_action/action_states.py +++ b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_move_arm_action/ros/src/mdr_move_arm_action/action_states.py @@ -21,6 +21,7 @@ def init(self): self.arm = moveit_commander.MoveGroupCommander(self.arm_name) except: rospy.logerr('[move_arm] %s could not be initialised', self.arm_name) + return FTSMTransitions.INIT_FAILED return FTSMTransitions.INITIALISED def running(self): @@ -65,6 +66,10 @@ def running(self): self.result = self.set_result(True) return FTSMTransitions.DONE + def recovering(self): + rospy.sleep(5.) + return FTSMTransitions.DONE_RECOVERING + def set_result(self, success): result = MoveArmResult() result.success = success From 65bc9122a6af55d807a7087fd970948393ecefaf Mon Sep 17 00:00:00 2001 From: Alex Mitrevski Date: Fri, 26 Apr 2019 19:11:50 +0200 Subject: [PATCH 79/96] [move_arm/dmp] Adding Abhishek's changes to the DMP API --- .../ros/src/mdr_move_arm_action/dmp.py | 46 +++++++++++++++---- 1 file changed, 36 insertions(+), 10 deletions(-) diff --git a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_move_arm_action/ros/src/mdr_move_arm_action/dmp.py b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_move_arm_action/ros/src/mdr_move_arm_action/dmp.py index 9e4989ad3..2a6f05cd5 100644 --- a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_move_arm_action/ros/src/mdr_move_arm_action/dmp.py +++ b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_move_arm_action/ros/src/mdr_move_arm_action/dmp.py @@ -6,6 +6,7 @@ import tf import actionlib from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal +from ros_dmp.srv import GenerateMotion, GenerateMotionRequest from mdr_move_arm_action.roll_dmp import RollDMP @@ -24,8 +25,11 @@ def __init__(self, dmp_name, tau): self.dmp_executor_path_topic = rospy.get_param('~path_topic', '/dmp_executor/path') self.move_base_server = rospy.get_param('~move_base_server', 'move_base/move') + # ros_dmp service + self.motion_client = rospy.ServiceProxy('/generate_motion_service', GenerateMotion) + self.number_of_sampling_points = 30 - self.goal_tolerance = 0.02 + self.goal_tolerance = 0.05 self.vel_publisher_arm = rospy.Publisher(self.cartesian_velocity_topic, TwistStamped, queue_size=1) self.vel_publisher_base = rospy.Publisher(self.base_vel_topic, Twist, queue_size=1) @@ -55,13 +59,32 @@ def sigma_values_cb(self, msg): def move_base(self): move_base_goal = MoveBaseGoal() move_base_goal.target_pose.header.frame_id = self.map_frame_name - print self.move_base_client.send_goal(move_base_goal) + print(self.move_base_client.send_goal(move_base_goal)) def generate_trajectory(self, goal, initial_pos): - goal = np.array([goal[0], goal[1], goal[2], 0.0, 0.0, 0.0]) - initial_pos = np.array([initial_pos[0], initial_pos[1], initial_pos[2], 0.0, 0.0, 0.0]) - self.roll = RollDMP(self.dmp_name, n_bfs=150) - self.pos, self.vel, self.acc = self.roll.roll(goal, initial_pos, self.tau) + req = GenerateMotionRequest() + req.goal_pose.pose.position.x = goal[0] + req.goal_pose.pose.position.y = goal[1] + req.goal_pose.pose.position.z = goal[2] + req.initial_pose.pose.position.x = initial_pos[0] + req.initial_pose.pose.position.y = initial_pos[1] + req.initial_pose.pose.position.z = initial_pos[2] + req.dmp_name = self.dmp_name + req.tau = self.tau + req.dt = 0.001 + + print('[dmp] Sending trajectory request: ', req) + response = self.motion_client(req) + pos_x = [] + pos_y = [] + pos_z = [] + for state in response.cart_traj.cartesian_state: + pos_x.append(state.pose.position.x) + pos_y.append(state.pose.position.y) + pos_z.append(state.pose.position.z) + self.pos = np.array(pos_x) + self.pos = np.vstack((self.pos, np.array(pos_y))) + self.pos = np.vstack((self.pos, np.array(pos_z))) def tranform_pose(self, pose): #transform goals to odom frame @@ -91,16 +114,17 @@ def publish_path(self): self.path_pub.publish(path) def trajectory_controller(self): - previous_pos = None count = 0 previous_index = 0 - path = self.pos[:, 0:3].T + path = self.pos path_x = path[0, :] path_y = path[1, :] path_z = path[2, :] while not rospy.is_shutdown(): try: - (trans, rot) = self.tf_listener.lookupTransform(self.odom_frame_name, self.palm_link_name, rospy.Time(0)) + (trans, rot) = self.tf_listener.lookupTransform(self.odom_frame_name, + self.palm_link_name, + rospy.Time(0)) break except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException): continue @@ -112,7 +136,9 @@ def trajectory_controller(self): old_pos_index = 0 while distance > self.goal_tolerance and not rospy.is_shutdown(): try: - (trans, rot) = self.tf_listener.lookupTransform(self.odom_frame_name, self.palm_link_name, rospy.Time(0)) + (trans, rot) = self.tf_listener.lookupTransform(self.odom_frame_name, + self.palm_link_name, + rospy.Time(0)) except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException): continue current_pos = np.array([trans[0], trans[1], trans[2]]) From 2c34e800ec29cb94bf5bcbbd2a0864f6bda0a59f Mon Sep 17 00:00:00 2001 From: Alex Mitrevski Date: Fri, 26 Apr 2019 19:43:51 +0200 Subject: [PATCH 80/96] [move_arm/dmp] Bug fixes --- .../ros/src/mdr_move_arm_action/dmp.py | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_move_arm_action/ros/src/mdr_move_arm_action/dmp.py b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_move_arm_action/ros/src/mdr_move_arm_action/dmp.py index 2a6f05cd5..ef3c20ba7 100644 --- a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_move_arm_action/ros/src/mdr_move_arm_action/dmp.py +++ b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_move_arm_action/ros/src/mdr_move_arm_action/dmp.py @@ -34,7 +34,7 @@ def __init__(self, dmp_name, tau): TwistStamped, queue_size=1) self.vel_publisher_base = rospy.Publisher(self.base_vel_topic, Twist, queue_size=1) self.feedforward_gain = 30 - self.feedback_gain = 1 + self.feedback_gain = 10 self.sigma_threshold_upper = 0.12 self.sigma_threshold_lower = 0.07 self.base_feedback_gain = 2.0 @@ -252,10 +252,10 @@ def execute(self, goal): self.generate_trajectory(goal, initial_pos) pos = [] - for i in range(self.pos.shape[0]): - pos.append(self.tranform_pose(self.pos[i, 0:3])) + for i in range(self.pos.shape[1]): + pos.append(self.tranform_pose(self.pos[:, i])) pos = np.array(pos) - self.pos = pos + self.pos = pos.T self.publish_path() # transform pose to base link From 7a6fd9261440aa42192288c9b9431ecb8ccc5aaf Mon Sep 17 00:00:00 2001 From: Alex Mitrevski Date: Fri, 26 Apr 2019 20:22:33 +0200 Subject: [PATCH 81/96] [mbot_interface] Unknown actions are now not dispatched --- .../src/mdr_mbot_interface/mbot_planner.py | 67 ++++++++++--------- 1 file changed, 34 insertions(+), 33 deletions(-) diff --git a/mdr_hri/mdr_mbot_interface/ros/src/mdr_mbot_interface/mbot_planner.py b/mdr_hri/mdr_mbot_interface/ros/src/mdr_mbot_interface/mbot_planner.py index b1dcad6b8..2d4075998 100644 --- a/mdr_hri/mdr_mbot_interface/ros/src/mdr_mbot_interface/mbot_planner.py +++ b/mdr_hri/mdr_mbot_interface/ros/src/mdr_mbot_interface/mbot_planner.py @@ -44,11 +44,14 @@ def interpretationCallback(self, msg): self.interpretation_received = True def process_interpretation(self): - ''' - Function that publish the right action depending on the interpretation received + '''Publishes an action depending on the interpretation received. ''' for action in self.received_interpretation.sentence_recognition: dispatch_msg = self.get_dispatch_msg(action) + if not dispatch_msg.name: + rospy.logwarn('[process_interpretation] Got an unknown action; ignoring request') + continue + rospy.loginfo('\033[1;36m[process_interpretation] Dispatching action {0}\033[0;37m'.format(dispatch_msg.name)) self.action_dispatch_pub.publish(dispatch_msg) while not self.action_completed and not self.action_failed: @@ -63,36 +66,35 @@ def process_interpretation(self): rospy.loginfo('\033[1;36m[process_interpretation] Done dispatching actions\033[0;37m') def get_dispatch_msg(self, action): - dispatch_msg = plan_dispatch_msgs.ActionDispatch() - - arg_msg = diag_msgs.KeyValue() - arg_msg.key = 'bot' - arg_msg.value = self.robot_name - dispatch_msg.parameters.append(arg_msg) - - if action.intention == 'go': - dispatch_msg.name = 'move_base' - for slot in action.slots: - arg_msg = diag_msgs.KeyValue() - if slot.type == 'destination': - arg_msg.key = 'to' - arg_msg.value = slot.data - dispatch_msg.parameters.append(arg_msg) - elif action.intention == 'take': - for slot in action.slots: - arg_msg = diag_msgs.KeyValue() - if slot.type == 'object': - arg_msg.key = 'obj' - elif slot.type == 'source': - dispatch_msg.name = 'pick' - arg_msg.key = 'surface' - elif slot.type == 'destination': - dispatch_msg.name = 'place' - arg_msg.key = 'surface' - arg_msg.value = slot.data - dispatch_msg.parameters.append(arg_msg) - - return dispatch_msg + dispatch_msg = plan_dispatch_msgs.ActionDispatch() + + arg_msg = diag_msgs.KeyValue() + arg_msg.key = 'bot' + arg_msg.value = self.robot_name + dispatch_msg.parameters.append(arg_msg) + + if action.intention == 'go': + dispatch_msg.name = 'move_base' + for slot in action.slots: + arg_msg = diag_msgs.KeyValue() + if slot.type == 'destination': + arg_msg.key = 'to' + arg_msg.value = slot.data + dispatch_msg.parameters.append(arg_msg) + elif action.intention == 'take': + for slot in action.slots: + arg_msg = diag_msgs.KeyValue() + if slot.type == 'object': + arg_msg.key = 'obj' + elif slot.type == 'source': + dispatch_msg.name = 'pick' + arg_msg.key = 'surface' + elif slot.type == 'destination': + dispatch_msg.name = 'place' + arg_msg.key = 'surface' + arg_msg.value = slot.data + dispatch_msg.parameters.append(arg_msg) + return dispatch_msg def action_feedback_cb(self, msg): if msg.status == 'action achieved': @@ -106,7 +108,6 @@ def wait_for_interpretation(self): # lower flag self.interpretation_received = False - # recognize intention self.process_interpretation() # action_slot_array_msg = ActionSlotArray() From e416032c1e5f8c61be119095d09ef4d2195bf9b8 Mon Sep 17 00:00:00 2001 From: Roberto Cai Date: Fri, 26 Apr 2019 20:37:11 +0200 Subject: [PATCH 82/96] [mdr_speech_recognition] Add kaldi support for offline recognition --- .../mdr_speech_recognition/speech_recognizer.py | 15 ++++++++++++++- 1 file changed, 14 insertions(+), 1 deletion(-) diff --git a/mdr_speech/mdr_speech_recognition/ros/src/mdr_speech_recognition/speech_recognizer.py b/mdr_speech/mdr_speech_recognition/ros/src/mdr_speech_recognition/speech_recognizer.py index 8501f230c..2d0abfcef 100755 --- a/mdr_speech/mdr_speech_recognition/ros/src/mdr_speech_recognition/speech_recognizer.py +++ b/mdr_speech/mdr_speech_recognition/ros/src/mdr_speech_recognition/speech_recognizer.py @@ -1,5 +1,6 @@ #!/usr/bin/env python from __future__ import print_function +import sys import rospy import httplib from std_msgs.msg import String @@ -10,7 +11,16 @@ class SpeechRecognizer(object): def __init__(self): rospy.init_node("speech_recognizer") self.pub = rospy.Publisher("speech_recognizer", String, latch=True, queue_size=1) + self.model_directory = rospy.get_param('~model_directory') + self.use_kaldi = rospy.get_param('~use_kaldi') self.recognizer = sr.Recognizer() + if self.use_kaldi: + try: + self.recognizer.load_kaldi_model(model_directory=self.model_directory) + except: + self.use_kaldi = False + rospy.logerr(sys.exc_info()[0]) + rospy.logerr('Unable to load Kaldi model. Using PocketSphinx as offline speech recognition') self.microphone = sr.Microphone() @staticmethod @@ -49,7 +59,10 @@ def recognize(self): rospy.logerr("Could not request results.") else: try: - recognized_speech = self.recognizer.recognize_sphinx(audio) + if self.use_kaldi: + recognized_speech = self.recognizer.recognize_kaldi(audio)[0] + else: + recognized_speech = self.recognizer.recognize_sphinx(audio) except sr.UnknownValueError: rospy.logerr("Could not understand audio.") except sr.RequestError: From 84b9e09c3e85c371c36c281a1bb9b6408dacad6e Mon Sep 17 00:00:00 2001 From: Roberto Cai Date: Fri, 26 Apr 2019 20:38:14 +0200 Subject: [PATCH 83/96] [mdr_speech_recognition] Parametrize launch file --- .../ros/launch/speech_recognition.launch | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/mdr_speech/mdr_speech_recognition/ros/launch/speech_recognition.launch b/mdr_speech/mdr_speech_recognition/ros/launch/speech_recognition.launch index 2b32cadb3..8be240f88 100644 --- a/mdr_speech/mdr_speech_recognition/ros/launch/speech_recognition.launch +++ b/mdr_speech/mdr_speech_recognition/ros/launch/speech_recognition.launch @@ -1,4 +1,9 @@ - + + + + + + From 130ae0c7840da19c99a2c15cc216a2ed12c5ddcb Mon Sep 17 00:00:00 2001 From: Roberto Cai Date: Fri, 26 Apr 2019 20:45:54 +0200 Subject: [PATCH 84/96] [mdr_speech_recognition] Update model directory --- .../ros/launch/speech_recognition.launch | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/mdr_speech/mdr_speech_recognition/ros/launch/speech_recognition.launch b/mdr_speech/mdr_speech_recognition/ros/launch/speech_recognition.launch index 8be240f88..7a536aba5 100644 --- a/mdr_speech/mdr_speech_recognition/ros/launch/speech_recognition.launch +++ b/mdr_speech/mdr_speech_recognition/ros/launch/speech_recognition.launch @@ -1,8 +1,7 @@ - - + From 48c31c5bf38c8875f90e60f1d21b91aa6f6f22df Mon Sep 17 00:00:00 2001 From: Minh Nguyen Date: Tue, 14 May 2019 10:07:42 +0200 Subject: [PATCH 85/96] remove legacy dependencies - remove deps on mdr_object_recognition_mean_circle - remove deps on mcr_dynamic_reconfigure_client - update docs --- .../mdr_object_recognition/CMakeLists.txt | 1 - .../mdr_perceive_plane_action/README.md | 17 +++----- .../mdr_perceive_plane_action/package.xml | 3 -- .../config/perceive_plane_configurations.yaml | 39 ------------------- 4 files changed, 6 insertions(+), 54 deletions(-) delete mode 100644 mdr_planning/mdr_actions/mdr_perception_actions/mdr_perceive_plane_action/ros/config/perceive_plane_configurations.yaml diff --git a/mdr_perception/mdr_object_recognition/CMakeLists.txt b/mdr_perception/mdr_object_recognition/CMakeLists.txt index d5219eff3..92fb1d234 100644 --- a/mdr_perception/mdr_object_recognition/CMakeLists.txt +++ b/mdr_perception/mdr_object_recognition/CMakeLists.txt @@ -6,7 +6,6 @@ find_package(catkin REQUIRED rospy std_msgs sensor_msgs - mcr_object_recognition_mean_circle mas_perception_libs ) diff --git a/mdr_planning/mdr_actions/mdr_perception_actions/mdr_perceive_plane_action/README.md b/mdr_planning/mdr_actions/mdr_perception_actions/mdr_perceive_plane_action/README.md index c248d6ed1..9d7368fed 100644 --- a/mdr_planning/mdr_actions/mdr_perception_actions/mdr_perceive_plane_action/README.md +++ b/mdr_planning/mdr_actions/mdr_perception_actions/mdr_perceive_plane_action/README.md @@ -7,10 +7,8 @@ interacts with ROSPlan. ### Goal: -* ``string plane_config``: Name of the configuration set for setting up the dynamic parameter server. Each plane should - have a set of configurations, and all configuration sets should be listed in - ``config/perceive_plane_configurations.yaml``. This is necessary for configuring the scene segmentation node in - ``mas_common_robotics`` +* ``string plane_config``: Name of the plane configuration to be perceived. + TODO(minhnh): confirm that this can be removed. * ``string plane_frame_prefix``: prefix to the plane names which will be written to the ``Plane`` objects in ``action_states.DetectObjects`` @@ -33,8 +31,6 @@ interacts with ROSPlan. ├── ros │   ├── action │   │   └── PerceivePlane.action -│   ├── config -│   │   └── perceive_plane_configurations.yaml │   ├── launch │   │   ├── perceive_plane_client.launch │   │   ├── perceive_plane.launch @@ -75,22 +71,21 @@ The following parameters need to be passed when launching the action client: * ``action_name``: Name of the action as used in the planning domain * ``server_name``: Name of the ``perceive_plane`` action server * ``action_timeout``: Maximum time (in seconds) that we are willing to wait for the action to be executed -* ``clear_plane_memory``: If set to true, all objects previously seen on a plane will be deleted from the knowledge base before new objects are added to it (default false) +* ``clear_plane_memory``: If set to true, all objects previously seen on a plane will be deleted from the + knowledge base before new objects are added to it (default false) * ``action_dispatch_topic``: Name of the topic at which the plan dispatcher sends action requests * ``action_feedback_topic``: Name of the topic at which the action sends feedback to the plan dispatcher ## Dependencies -* ``mdr_object_recognition_mean_circle`` * ``mas_perception_libs`` * ``mdr_object_recognition`` -* ``mcr_dynamic_reconfigure_client`` * ``mcr_perception_msgs`` ## Example usage 1. Change the ``service_module`` and ``service_class_name`` value to the desired extended service proxy. By default -a test service proxy of type ``std_srv/Empty`` is loaded. + a test service proxy of type ``std_srv/Empty`` is loaded. 2. Run the action server: ``roslaunch mdr_perceive_plane_action perceive_plane.launch``. -The ``perceive_plane_test.launch`` file will create a server of type ``std_srv/Empty`` for testing. + The ``perceive_plane_test.launch`` file will create a server of type ``std_srv/Empty`` for testing. 3. Run the client example: ``rosrun mdr_perceive_plane_action perceive_plane_client_test`` diff --git a/mdr_planning/mdr_actions/mdr_perception_actions/mdr_perceive_plane_action/package.xml b/mdr_planning/mdr_actions/mdr_perception_actions/mdr_perceive_plane_action/package.xml index 215b5ec63..88e73b767 100644 --- a/mdr_planning/mdr_actions/mdr_perception_actions/mdr_perceive_plane_action/package.xml +++ b/mdr_planning/mdr_actions/mdr_perception_actions/mdr_perceive_plane_action/package.xml @@ -31,9 +31,6 @@ actionlib_msgs message_runtime mcr_perception_msgs - mcr_states - mcr_dynamic_reconfigure_client - mdr_object_recognition diff --git a/mdr_planning/mdr_actions/mdr_perception_actions/mdr_perceive_plane_action/ros/config/perceive_plane_configurations.yaml b/mdr_planning/mdr_actions/mdr_perception_actions/mdr_perceive_plane_action/ros/config/perceive_plane_configurations.yaml deleted file mode 100644 index b9c0df58f..000000000 --- a/mdr_planning/mdr_actions/mdr_perception_actions/mdr_perceive_plane_action/ros/config/perceive_plane_configurations.yaml +++ /dev/null @@ -1,39 +0,0 @@ -# configuration_name: -# node_name: -# param_name: param_value -# param_name: param_value -# node_name: -# param_name: param_value -# param_name: param_value -# configuration_name: -# node_name: -# param_name: param_value -# param_name: param_value -# node_name: -# param_name: param_value -# param_name: param_value - -shelf_1: - /mcr_perception/voxel_filter: - filter_limit_min: 0.3 - filter_limit_max: 0.6 -shelf_2: - /mcr_perception/voxel_filter: - filter_limit_min: 0.3 - filter_limit_max: 0.6 -shelf_3: - /mcr_perception/voxel_filter: - filter_limit_min: 0.6 - filter_limit_max: 0.9 -shelf_4: - /mcr_perception/voxel_filter: - filter_limit_min: 0.9 - filter_limit_max: 1.3 -shelf_5: - /mcr_perception/voxel_filter: - filter_limit_min: 1.2 - filter_limit_max: 1.8 -table: - /mcr_perception/voxel_filter: - filter_limit_min: 0.6 - filter_limit_max: 1.5 From 643787a0fbf2fde86ffcae1d64ba796d7fc786de Mon Sep 17 00:00:00 2001 From: Minh Nguyen Date: Tue, 14 May 2019 10:13:02 +0200 Subject: [PATCH 86/96] remove exec dep on mean_circle --- mdr_perception/mdr_object_recognition/package.xml | 6 +----- .../mdr_perceive_plane_action/package.xml | 1 + 2 files changed, 2 insertions(+), 5 deletions(-) diff --git a/mdr_perception/mdr_object_recognition/package.xml b/mdr_perception/mdr_object_recognition/package.xml index 0a5132813..0b8119cc5 100644 --- a/mdr_perception/mdr_object_recognition/package.xml +++ b/mdr_perception/mdr_object_recognition/package.xml @@ -2,10 +2,7 @@ mdr_object_recognition 1.2.0 - This package provides launcher for - mcr_object_recognition_mean_circle. - - + This package contains recognition models for @Home scenarios Shehzad Ahmed MAS robotics @@ -19,5 +16,4 @@ sensor_msgs mas_perception_libs - mcr_object_recognition_mean_circle diff --git a/mdr_planning/mdr_actions/mdr_perception_actions/mdr_perceive_plane_action/package.xml b/mdr_planning/mdr_actions/mdr_perception_actions/mdr_perceive_plane_action/package.xml index 88e73b767..862e8d36c 100644 --- a/mdr_planning/mdr_actions/mdr_perception_actions/mdr_perceive_plane_action/package.xml +++ b/mdr_planning/mdr_actions/mdr_perception_actions/mdr_perceive_plane_action/package.xml @@ -31,6 +31,7 @@ actionlib_msgs message_runtime mcr_perception_msgs + mdr_object_recognition From f25a1bdf392b7c3bc19bfdea951b325659651352 Mon Sep 17 00:00:00 2001 From: Argentina Ortega Sainz Date: Mon, 20 May 2019 11:08:18 +0200 Subject: [PATCH 87/96] Remove dependency from mas_navigation The necessary dependencies can be found in mas_navigation_tools: https://github.com/b-it-bots/mas_navigation_tools This removes dependencies to other unused navigation packages, as a result: * faster build time * smaller docker image size --- mas-domestic.rosinstall | 4 ++-- mdr_environments/upload_param.launch | 4 ++-- mdr_navigation/mdr_2dnav/package.xml | 4 ++-- mdr_navigation/mdr_2dnav/ros/launch/nav_common.launch | 6 +++--- mdr_navigation/mdr_2dslam/package.xml | 2 +- .../mdr_move_base_action/ros/launch/move_base_action.launch | 2 +- .../mdr_robot_inspection/ros/launch/robot_inspection.launch | 2 +- .../mdr_speech_person_recognition/ros/launch/spr.launch | 2 +- 8 files changed, 13 insertions(+), 13 deletions(-) diff --git a/mas-domestic.rosinstall b/mas-domestic.rosinstall index 2d9771dbf..f02fb1125 100644 --- a/mas-domestic.rosinstall +++ b/mas-domestic.rosinstall @@ -9,8 +9,8 @@ version: kinetic - git: - local-name: mas_navigation - uri: https://github.com/b-it-bots/mas_navigation.git + local-name: mas_navigation_tools + uri: https://github.com/b-it-bots/mas_navigation_tools.git version: kinetic - git: diff --git a/mdr_environments/upload_param.launch b/mdr_environments/upload_param.launch index 5a9f8cd95..59a73bfc4 100644 --- a/mdr_environments/upload_param.launch +++ b/mdr_environments/upload_param.launch @@ -2,10 +2,10 @@ - + - + diff --git a/mdr_navigation/mdr_2dnav/package.xml b/mdr_navigation/mdr_2dnav/package.xml index 8a89859cc..dac21eb3c 100644 --- a/mdr_navigation/mdr_2dnav/package.xml +++ b/mdr_navigation/mdr_2dnav/package.xml @@ -2,7 +2,7 @@ mdr_2dnav 1.2.0 - 2d base navigation using ros move_base for Care-O-bot 3, Fraunhofer IPA + 2D base navigation using ros move_base MAS robotics @@ -15,7 +15,7 @@ actionlib amcl map_server - mcr_default_env_config + mdr_environments mcr_navigation_tools move_base move_base_msgs diff --git a/mdr_navigation/mdr_2dnav/ros/launch/nav_common.launch b/mdr_navigation/mdr_2dnav/ros/launch/nav_common.launch index b2cafc48a..f2bfee6d7 100644 --- a/mdr_navigation/mdr_2dnav/ros/launch/nav_common.launch +++ b/mdr_navigation/mdr_2dnav/ros/launch/nav_common.launch @@ -5,12 +5,12 @@ - + - - + + diff --git a/mdr_navigation/mdr_2dslam/package.xml b/mdr_navigation/mdr_2dslam/package.xml index cd2ccc4f8..b49f88aae 100644 --- a/mdr_navigation/mdr_2dslam/package.xml +++ b/mdr_navigation/mdr_2dslam/package.xml @@ -2,7 +2,7 @@ mdr_2dslam 1.2.0 - 2d base slam mapping based on ros gmapping for Care-O-bot 3, Fraunhofer IPA + 2D base slam mapping based on ros gmapping MAS robotics diff --git a/mdr_planning/mdr_actions/mdr_navigation_actions/mdr_move_base_action/ros/launch/move_base_action.launch b/mdr_planning/mdr_actions/mdr_navigation_actions/mdr_move_base_action/ros/launch/move_base_action.launch index b06d73a8f..d0630f1da 100644 --- a/mdr_planning/mdr_actions/mdr_navigation_actions/mdr_move_base_action/ros/launch/move_base_action.launch +++ b/mdr_planning/mdr_actions/mdr_navigation_actions/mdr_move_base_action/ros/launch/move_base_action.launch @@ -3,7 +3,7 @@ - + diff --git a/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_robot_inspection/ros/launch/robot_inspection.launch b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_robot_inspection/ros/launch/robot_inspection.launch index 0a3d824a4..344b48f62 100644 --- a/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_robot_inspection/ros/launch/robot_inspection.launch +++ b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_robot_inspection/ros/launch/robot_inspection.launch @@ -5,7 +5,7 @@ - + diff --git a/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_speech_person_recognition/ros/launch/spr.launch b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_speech_person_recognition/ros/launch/spr.launch index 3b321c59c..c9064fc6f 100644 --- a/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_speech_person_recognition/ros/launch/spr.launch +++ b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_speech_person_recognition/ros/launch/spr.launch @@ -10,7 +10,7 @@ - + From d1c75b67984a4e9ad80c622706f00cef58256e08 Mon Sep 17 00:00:00 2001 From: Minh Nguyen Date: Mon, 20 May 2019 16:04:37 +0200 Subject: [PATCH 88/96] remove old plane perceive package --- .../mdr_perceive_table/CHANGELOG.rst | 34 ------- .../mdr_perceive_table/CMakeLists.txt | 39 -------- .../mdr_perceive_table/package.xml | 34 ------- .../ros/action/PerceiveTable.action | 9 -- .../config/perceive_plane_configurations.yaml | 39 -------- .../ros/launch/perceive_table.launch | 7 -- .../ros/scripts/perceive_table | 90 ------------------- .../ros/scripts/perceive_table_client_test | 23 ----- .../ros/src/mdr_perceive_table/__init__.py | 0 .../src/mdr_perceive_table/action_states.py | 27 ------ .../mdr_perceive_table/setup.py | 11 --- 11 files changed, 313 deletions(-) delete mode 100644 mdr_planning/mdr_actions/mdr_perception_actions/mdr_perceive_table/CHANGELOG.rst delete mode 100644 mdr_planning/mdr_actions/mdr_perception_actions/mdr_perceive_table/CMakeLists.txt delete mode 100644 mdr_planning/mdr_actions/mdr_perception_actions/mdr_perceive_table/package.xml delete mode 100644 mdr_planning/mdr_actions/mdr_perception_actions/mdr_perceive_table/ros/action/PerceiveTable.action delete mode 100644 mdr_planning/mdr_actions/mdr_perception_actions/mdr_perceive_table/ros/config/perceive_plane_configurations.yaml delete mode 100644 mdr_planning/mdr_actions/mdr_perception_actions/mdr_perceive_table/ros/launch/perceive_table.launch delete mode 100755 mdr_planning/mdr_actions/mdr_perception_actions/mdr_perceive_table/ros/scripts/perceive_table delete mode 100755 mdr_planning/mdr_actions/mdr_perception_actions/mdr_perceive_table/ros/scripts/perceive_table_client_test delete mode 100644 mdr_planning/mdr_actions/mdr_perception_actions/mdr_perceive_table/ros/src/mdr_perceive_table/__init__.py delete mode 100644 mdr_planning/mdr_actions/mdr_perception_actions/mdr_perceive_table/ros/src/mdr_perceive_table/action_states.py delete mode 100644 mdr_planning/mdr_actions/mdr_perception_actions/mdr_perceive_table/setup.py diff --git a/mdr_planning/mdr_actions/mdr_perception_actions/mdr_perceive_table/CHANGELOG.rst b/mdr_planning/mdr_actions/mdr_perception_actions/mdr_perceive_table/CHANGELOG.rst deleted file mode 100644 index 486eb66f0..000000000 --- a/mdr_planning/mdr_actions/mdr_perception_actions/mdr_perceive_table/CHANGELOG.rst +++ /dev/null @@ -1,34 +0,0 @@ -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Changelog for package mdr_perceive_table -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ - -1.2.0 (2017-11-04) ------------------- -* Move actions into ros folder -* Contributors: Argentina Ortega Sainz - -1.1.2 (2017-10-29) ------------------- -* Add missing scripts to installs in CMakeLists.txt -* Contributors: Argentina Ortega Sainz - -1.1.1 (2017-09-20) ------------------- -* Change maintainer tags to MAS Robotics -* Update changelog for mdr_planning -* Contributors: Argentina Ortega Sainz - -1.1.0 (2017-09-18 18:15) ------------------------- - -1.0.1 (2017-09-18 18:04) ------------------------- -* Added changelogs for mdr_planning and mdr_msgs -* Merge branch 'refactor/mdr_planning' into 'indigo' - Move actions, behaviors, and scenarios to a new mdr_planning metapackage - See merge request !25 -* Refactored mdr_actions (created an mdr_perceive_table package for the perceive table action) -* Contributors: Alex Mitrevski - -1.0.0 (2017-04-11) ------------------- diff --git a/mdr_planning/mdr_actions/mdr_perception_actions/mdr_perceive_table/CMakeLists.txt b/mdr_planning/mdr_actions/mdr_perception_actions/mdr_perceive_table/CMakeLists.txt deleted file mode 100644 index 4c441e37c..000000000 --- a/mdr_planning/mdr_actions/mdr_perception_actions/mdr_perceive_table/CMakeLists.txt +++ /dev/null @@ -1,39 +0,0 @@ -cmake_minimum_required(VERSION 2.8.3) -project(mdr_perceive_table) - -find_package(catkin REQUIRED COMPONENTS - actionlib - actionlib_msgs - genmsg - message_generation - roslint - rospy -) - -catkin_python_setup() - -add_action_files(DIRECTORY ros/action - FILES - PerceiveTable.action -) - -generate_messages( - DEPENDENCIES - actionlib_msgs -) - -catkin_package( - CATKIN_DEPENDS - actionlib_msgs - message_runtime -) - -include_directories( - ${catkin_INCLUDE_DIRS} -) - -install(PROGRAMS - ros/scripts/perceive_table - ros/scripts/perceive_table_client_test - DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}/scripts -) diff --git a/mdr_planning/mdr_actions/mdr_perception_actions/mdr_perceive_table/package.xml b/mdr_planning/mdr_actions/mdr_perception_actions/mdr_perceive_table/package.xml deleted file mode 100644 index e1da0079f..000000000 --- a/mdr_planning/mdr_actions/mdr_perception_actions/mdr_perceive_table/package.xml +++ /dev/null @@ -1,34 +0,0 @@ - - - mdr_perceive_table - 1.2.0 - An action for perceiving a table - - Santosh Thoduka - Alex Moriarty - Daniel Vazquez - Iryna Ivanovska - Oscar Lima - - MAS robotics - - GPLv3 - - catkin - genmsg - actionlib - actionlib_msgs - message_generation - roslint - rospy - - rospy - actionlib - actionlib_msgs - message_runtime - mcr_perception_msgs - mcr_states - mcr_dynamic_reconfigure_client - - - diff --git a/mdr_planning/mdr_actions/mdr_perception_actions/mdr_perceive_table/ros/action/PerceiveTable.action b/mdr_planning/mdr_actions/mdr_perception_actions/mdr_perceive_table/ros/action/PerceiveTable.action deleted file mode 100644 index 48b6b0aff..000000000 --- a/mdr_planning/mdr_actions/mdr_perception_actions/mdr_perceive_table/ros/action/PerceiveTable.action +++ /dev/null @@ -1,9 +0,0 @@ -# goal definition -string location ---- -# result definition -bool success ---- -# feedback -string current_state -string text diff --git a/mdr_planning/mdr_actions/mdr_perception_actions/mdr_perceive_table/ros/config/perceive_plane_configurations.yaml b/mdr_planning/mdr_actions/mdr_perception_actions/mdr_perceive_table/ros/config/perceive_plane_configurations.yaml deleted file mode 100644 index b9c0df58f..000000000 --- a/mdr_planning/mdr_actions/mdr_perception_actions/mdr_perceive_table/ros/config/perceive_plane_configurations.yaml +++ /dev/null @@ -1,39 +0,0 @@ -# configuration_name: -# node_name: -# param_name: param_value -# param_name: param_value -# node_name: -# param_name: param_value -# param_name: param_value -# configuration_name: -# node_name: -# param_name: param_value -# param_name: param_value -# node_name: -# param_name: param_value -# param_name: param_value - -shelf_1: - /mcr_perception/voxel_filter: - filter_limit_min: 0.3 - filter_limit_max: 0.6 -shelf_2: - /mcr_perception/voxel_filter: - filter_limit_min: 0.3 - filter_limit_max: 0.6 -shelf_3: - /mcr_perception/voxel_filter: - filter_limit_min: 0.6 - filter_limit_max: 0.9 -shelf_4: - /mcr_perception/voxel_filter: - filter_limit_min: 0.9 - filter_limit_max: 1.3 -shelf_5: - /mcr_perception/voxel_filter: - filter_limit_min: 1.2 - filter_limit_max: 1.8 -table: - /mcr_perception/voxel_filter: - filter_limit_min: 0.6 - filter_limit_max: 1.5 diff --git a/mdr_planning/mdr_actions/mdr_perception_actions/mdr_perceive_table/ros/launch/perceive_table.launch b/mdr_planning/mdr_actions/mdr_perception_actions/mdr_perceive_table/ros/launch/perceive_table.launch deleted file mode 100644 index 062254f84..000000000 --- a/mdr_planning/mdr_actions/mdr_perception_actions/mdr_perceive_table/ros/launch/perceive_table.launch +++ /dev/null @@ -1,7 +0,0 @@ - - - - - - - diff --git a/mdr_planning/mdr_actions/mdr_perception_actions/mdr_perceive_table/ros/scripts/perceive_table b/mdr_planning/mdr_actions/mdr_perception_actions/mdr_perceive_table/ros/scripts/perceive_table deleted file mode 100755 index 671bd7666..000000000 --- a/mdr_planning/mdr_actions/mdr_perception_actions/mdr_perceive_table/ros/scripts/perceive_table +++ /dev/null @@ -1,90 +0,0 @@ -#!/usr/bin/env python -import rospy -import smach - -import mdr_common_states.common_states_manipulation as gms -import mcr_states.common.perception_states as gps -import mcr_states.common.basic_states as gbs - -# action lib -from smach_ros import ActionServerWrapper -from mdr_perceive_table.msg import PerceiveTableAction -from mdr_perceive_table.msg import PerceiveTableFeedback -from mdr_perceive_table.msg import PerceiveTableResult - - -class SetActionLibResult(smach.State): - def __init__(self, result): - smach.State.__init__(self, outcomes=['succeeded'], - input_keys=['perceive_table_goal'], - output_keys=['perceive_table_feedback', 'perceive_table_result']) - self.result = result - - def execute(self, userdata): - result = PerceiveTableResult() - result.success = self.result - userdata.perceive_table_result = result - return 'succeeded' - - -def main(): - rospy.init_node('perceive_table_server') - # Construct state machine - sm = smach.StateMachine( - outcomes=['OVERALL_SUCCESS', 'OVERALL_FAILED'], - input_keys=['perceive_table_goal'], - output_keys=['perceive_table_feedback', 'perceive_table_result']) - with sm: - smach.StateMachine.add('MOVE_ARM', gms.move_arm('look_at_table_front'), - transitions={'succeeded': 'MOVE_TORSO_BACK', - 'failed': 'MOVE_TORSO_FRONT'}) - - smach.StateMachine.add('MOVE_TORSO_FRONT', gms.move_part('torso', 'front'), - transitions={'succeeded': 'MOVE_ARM', - 'failed': 'MOVE_TORSO_FRONT'}) - - smach.StateMachine.add('MOVE_TORSO_BACK', gms.move_part('torso', 'back_extreme'), - transitions={'succeeded': 'MOVE_HEAD_BACK', - 'failed': 'MOVE_TORSO_BACK'}) - - smach.StateMachine.add('MOVE_HEAD_BACK', gms.move_part('head', 'back_table'), - transitions={'succeeded': 'CONFIGURE_TABLE', - 'failed': 'MOVE_HEAD_BACK'}) - - smach.StateMachine.add('CONFIGURE_TABLE', gbs.set_named_config('table'), - transitions={'success': 'START_WORKSPACE_FINDER', - 'failure': 'SET_ACTION_LIB_FAILURE', - 'timeout': 'CONFIGURE_TABLE'}) - - smach.StateMachine.add('START_WORKSPACE_FINDER', gbs.send_event( - event_list=[('/mcr_perception/mux_pointcloud/select', '/cam3d/depth_registered/points')]), - transitions={'success': 'RECOGNIZE_OBJECTS'}) - - smach.StateMachine.add('RECOGNIZE_OBJECTS', gps.find_objects(retries=1), - transitions={'objects_found': 'SET_ACTION_LIB_SUCCESS', - 'no_objects_found': 'SET_ACTION_LIB_FAILURE'}, - remapping={'found_objects': 'recognized_objects'}) - - smach.StateMachine.add('SET_ACTION_LIB_SUCCESS', SetActionLibResult(True), - transitions={'succeeded': 'OVERALL_SUCCESS'}) - - smach.StateMachine.add('SET_ACTION_LIB_FAILURE', SetActionLibResult(False), - transitions={'succeeded': 'OVERALL_FAILED'}) - - # Construct action server wrapper - asw = ActionServerWrapper( - server_name='perceive_table_server', - action_spec=PerceiveTableAction, - wrapped_container=sm, - succeeded_outcomes=['OVERALL_SUCCESS'], - aborted_outcomes=['OVERALL_FAILED'], - preempted_outcomes=['PREEMPTED'], - goal_key='perceive_table_goal', - feedback_key='perceive_table_feedback', - result_key='perceive_table_result') - # Run the server in a background thread - asw.run_server() - rospy.spin() - -if __name__ == '__main__': - main() diff --git a/mdr_planning/mdr_actions/mdr_perception_actions/mdr_perceive_table/ros/scripts/perceive_table_client_test b/mdr_planning/mdr_actions/mdr_perception_actions/mdr_perceive_table/ros/scripts/perceive_table_client_test deleted file mode 100755 index 100c8475c..000000000 --- a/mdr_planning/mdr_actions/mdr_perception_actions/mdr_perceive_table/ros/scripts/perceive_table_client_test +++ /dev/null @@ -1,23 +0,0 @@ -#! /usr/bin/env python -import rospy -import roslib -import actionlib - -import sys - -from mdr_perceive_table.msg import PerceiveTableAction, PerceiveTableGoal - -if __name__ == '__main__': - rospy.init_node('perceive_table_client_tester') - client = actionlib.SimpleActionClient('/mdr_actions/perceive_table_server', PerceiveTableAction) - client.wait_for_server() - goal = PerceiveTableGoal() - goal.location = 'anywhere' - try: - timeout = 45.0 - rospy.loginfo('Sending action lib goal to perceive_table_server') - client.send_goal(goal) - client.wait_for_result(rospy.Duration.from_sec(int(timeout))) - print client.get_result() - except: - pass diff --git a/mdr_planning/mdr_actions/mdr_perception_actions/mdr_perceive_table/ros/src/mdr_perceive_table/__init__.py b/mdr_planning/mdr_actions/mdr_perception_actions/mdr_perceive_table/ros/src/mdr_perceive_table/__init__.py deleted file mode 100644 index e69de29bb..000000000 diff --git a/mdr_planning/mdr_actions/mdr_perception_actions/mdr_perceive_table/ros/src/mdr_perceive_table/action_states.py b/mdr_planning/mdr_actions/mdr_perception_actions/mdr_perceive_table/ros/src/mdr_perceive_table/action_states.py deleted file mode 100644 index c43a228b1..000000000 --- a/mdr_planning/mdr_actions/mdr_perception_actions/mdr_perceive_table/ros/src/mdr_perceive_table/action_states.py +++ /dev/null @@ -1,27 +0,0 @@ -#!/usr/bin/python - -import rospy -import smach -import smach_ros -import actionlib -import mdr_perceive_table.msg - -class perceive_table(smach.State): - def __init__(self, timeout=15.0, action_server='perceive_table_server'): - smach.State.__init__(self, outcomes=['succeeded', 'failed']) - self.timeout = timeout - self.action_server = action_server - self.client = actionlib.SimpleActionClient(action_server, mdr_perceive_table.msg.PerceiveTableAction) - self.client.wait_for_server() - - def execute(self, userdata): - goal = mdr_perceive_table.msg.PerceiveTableGoal() - goal.location = 'anywhere' - rospy.loginfo('Sending actionlib goal to ' + self.action_server + ' with timeout: ' + str(self.timeout)) - self.client.send_goal(goal) - self.client.wait_for_result(rospy.Duration.from_sec(self.timeout)) - res = self.client.get_result() - if res and res.success: - return 'succeeded' - else: - return 'failed' diff --git a/mdr_planning/mdr_actions/mdr_perception_actions/mdr_perceive_table/setup.py b/mdr_planning/mdr_actions/mdr_perception_actions/mdr_perceive_table/setup.py deleted file mode 100644 index 7eeb60638..000000000 --- a/mdr_planning/mdr_actions/mdr_perception_actions/mdr_perceive_table/setup.py +++ /dev/null @@ -1,11 +0,0 @@ -#!/usr/bin/env python - -from distutils.core import setup -from catkin_pkg.python_setup import generate_distutils_setup - -d = generate_distutils_setup( - packages=['mdr_perceive_table'], - package_dir={'mdr_perceive_table': 'ros/src/mdr_perceive_table'} -) - -setup(**d) From 838f4a9320b33b189aee819205541f2c3220f16a Mon Sep 17 00:00:00 2001 From: Argentina Ortega Sainz Date: Mon, 20 May 2019 16:03:44 +0200 Subject: [PATCH 89/96] [mdr_navigation] Update dependency to mas_navigation_tools --- mdr_navigation/mdr_2dnav/package.xml | 2 +- mdr_navigation/mdr_2dnav/ros/launch/nav_common.launch | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/mdr_navigation/mdr_2dnav/package.xml b/mdr_navigation/mdr_2dnav/package.xml index dac21eb3c..868a5ca15 100644 --- a/mdr_navigation/mdr_2dnav/package.xml +++ b/mdr_navigation/mdr_2dnav/package.xml @@ -16,7 +16,7 @@ amcl map_server mdr_environments - mcr_navigation_tools + mas_navigation_tools move_base move_base_msgs rospy diff --git a/mdr_navigation/mdr_2dnav/ros/launch/nav_common.launch b/mdr_navigation/mdr_2dnav/ros/launch/nav_common.launch index f2bfee6d7..25ee282ec 100644 --- a/mdr_navigation/mdr_2dnav/ros/launch/nav_common.launch +++ b/mdr_navigation/mdr_2dnav/ros/launch/nav_common.launch @@ -9,7 +9,7 @@ - + From d2535b534dd53cff01a8e5be7d32cfdb297cdca1 Mon Sep 17 00:00:00 2001 From: Argentina Ortega Sainz Date: Mon, 20 May 2019 16:53:20 +0200 Subject: [PATCH 90/96] Add mdr_door_status from mas_perception --- mdr_perception/mdr_door_status/CMakeLists.txt | 40 ++++++++ mdr_perception/mdr_door_status/package.xml | 29 ++++++ .../ros/launch/door_status.launch | 11 +++ .../ros/launch/door_status_mockup.launch | 7 ++ .../ros/src/door_status_node.cpp | 94 +++++++++++++++++++ .../ros/src/door_status_node_mockup.cpp | 61 ++++++++++++ 6 files changed, 242 insertions(+) create mode 100644 mdr_perception/mdr_door_status/CMakeLists.txt create mode 100644 mdr_perception/mdr_door_status/package.xml create mode 100644 mdr_perception/mdr_door_status/ros/launch/door_status.launch create mode 100644 mdr_perception/mdr_door_status/ros/launch/door_status_mockup.launch create mode 100644 mdr_perception/mdr_door_status/ros/src/door_status_node.cpp create mode 100644 mdr_perception/mdr_door_status/ros/src/door_status_node_mockup.cpp diff --git a/mdr_perception/mdr_door_status/CMakeLists.txt b/mdr_perception/mdr_door_status/CMakeLists.txt new file mode 100644 index 000000000..cb5b88fc3 --- /dev/null +++ b/mdr_perception/mdr_door_status/CMakeLists.txt @@ -0,0 +1,40 @@ +cmake_minimum_required(VERSION 2.8.3) +project(mdr_door_status) + +find_package(catkin REQUIRED COMPONENTS roscpp sensor_msgs std_msgs rosconsole) + +catkin_package( + CATKIN_DEPENDS roscpp sensor_msgs std_msgs rosconsole +) + +include_directories( + ${catkin_INCLUDE_DIRS} +) + +add_executable(door_status_node ros/src/door_status_node.cpp) + +target_link_libraries(door_status_node + ${catkin_LIBRARIES} +) + +add_executable(door_status_node_mockup ros/src/door_status_node_mockup.cpp) + +target_link_libraries(door_status_node_mockup + ${catkin_LIBRARIES} +) + + +### TESTS +if(CATKIN_ENABLE_TESTING) + find_package(roslaunch REQUIRED) + roslaunch_add_file_check(ros/launch) +endif() + + +### INSTALLS +install( + TARGETS + door_status_node + door_status_node_mockup + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) diff --git a/mdr_perception/mdr_door_status/package.xml b/mdr_perception/mdr_door_status/package.xml new file mode 100644 index 000000000..f9f98b255 --- /dev/null +++ b/mdr_perception/mdr_door_status/package.xml @@ -0,0 +1,29 @@ + + mdr_door_status + 0.1.0 + + door status + + BSD + + Frederik Hegger + + Matthias Füller + Sven Schneider + Frederik Hegger + + catkin + + roscpp + sensor_msgs + std_msgs + rosconsole + + roscpp + sensor_msgs + std_msgs + rosconsole + + roslaunch + + diff --git a/mdr_perception/mdr_door_status/ros/launch/door_status.launch b/mdr_perception/mdr_door_status/ros/launch/door_status.launch new file mode 100644 index 000000000..966e00710 --- /dev/null +++ b/mdr_perception/mdr_door_status/ros/launch/door_status.launch @@ -0,0 +1,11 @@ + + + + + + + + + + + diff --git a/mdr_perception/mdr_door_status/ros/launch/door_status_mockup.launch b/mdr_perception/mdr_door_status/ros/launch/door_status_mockup.launch new file mode 100644 index 000000000..d3dcca057 --- /dev/null +++ b/mdr_perception/mdr_door_status/ros/launch/door_status_mockup.launch @@ -0,0 +1,7 @@ + + + + + + + diff --git a/mdr_perception/mdr_door_status/ros/src/door_status_node.cpp b/mdr_perception/mdr_door_status/ros/src/door_status_node.cpp new file mode 100644 index 000000000..10b3c1399 --- /dev/null +++ b/mdr_perception/mdr_door_status/ros/src/door_status_node.cpp @@ -0,0 +1,94 @@ +/* + * door_status_node.cpp + * + * Created on: Jan 26, 2011 + * Author: Frederik Hegger + */ + +#include +#include +#include + +sensor_msgs::LaserScanConstPtr g_pLaserScanFront; +bool bIsDoorOpen = false; +double dOpeningAngle = 10.0; +double dDistanceThreshold = 1.0; + +bool publish_door_state() +{ + double dAngle = 0.0; + double dSummedDistance = 0.0; + unsigned int dCountInAngleRange = 0; + + if (g_pLaserScanFront == 0) + return false; + + + //sum up distance in the desired opening angle + dAngle = g_pLaserScanFront->angle_min; + for (unsigned int i = 0; i < g_pLaserScanFront->ranges.size(); ++i, dAngle += g_pLaserScanFront->angle_increment) + { + //check angle against desired range and only take those distance measures + if ((dAngle >= -dOpeningAngle) && (dAngle <= dOpeningAngle)) + { + dSummedDistance += g_pLaserScanFront->ranges[i]; + ++dCountInAngleRange; + } + } + + //calc mean + dSummedDistance /= dCountInAngleRange; + + //check mean distance to decide weather door is open or not + if (dSummedDistance > dDistanceThreshold) + { + ROS_INFO("door status: door is OPEN"); + return true; + } + else + { + ROS_INFO("door status: door is CLOSED"); + return false; + } + + return true; +} + + +void laserScanCallback(const sensor_msgs::LaserScan::ConstPtr& msg_in) +{ + g_pLaserScanFront = msg_in; +} + +int main(int argc, char **argv) +{ + ros::init(argc, argv, "door_status"); + ros::NodeHandle nh("~"); + + ros::Subscriber subFrontScan = nh.subscribe("scan", 1, laserScanCallback); + ros::Publisher pubDoorStatus = nh.advertise("door_status", 1, true); + + // read params from parameter server file + if (nh.getParam("angular_range", dOpeningAngle) == false) + ROS_WARN("Parameter angular_range not specified in launch file, used default value: %lf degree", dOpeningAngle); + if (nh.getParam("distance_threshold", dDistanceThreshold) == false) + ROS_WARN("Parameter distance_threshold not specified in launch file, used default value: %lf meter", dDistanceThreshold); + + // divide degree by 2 (to have range -angle to angle+) and convert radians + dOpeningAngle = ((dOpeningAngle / 2.0) / 180.0) * M_PI; + + ros::Rate loop_rate(10); + + while (ros::ok()) + { + ros::spinOnce(); + bool open = publish_door_state(); + std_msgs::Bool open_msgs; + open_msgs.data = open; + + pubDoorStatus.publish(open_msgs); + loop_rate.sleep(); + } + + return 0; +} diff --git a/mdr_perception/mdr_door_status/ros/src/door_status_node_mockup.cpp b/mdr_perception/mdr_door_status/ros/src/door_status_node_mockup.cpp new file mode 100644 index 000000000..9eafef72e --- /dev/null +++ b/mdr_perception/mdr_door_status/ros/src/door_status_node_mockup.cpp @@ -0,0 +1,61 @@ +/* + * door_status_node.cpp + * + * Created on: Jan 26, 2011 + * Author: Frederik Hegger + */ + +#include +#include + +bool bIsDoorOpen = false; + +ros::Publisher pubDoorStatus; +ros::Subscriber subChangeDoorStatus; + +std_msgs::Bool bMsg; + +void publishDoorStatus() +{ + bMsg.data = bIsDoorOpen; + pubDoorStatus.publish(bMsg); +} + +void changeDoorStatus(const std_msgs::BoolConstPtr status) +{ + bIsDoorOpen = status->data; + + if (bIsDoorOpen) + { + ROS_INFO("Door status: door is OPEN"); + } + else + { + ROS_INFO("Door status: door is CLOSED"); + } + publishDoorStatus(); +} + + + +int main(int argc, char **argv) +{ + ros::init(argc, argv, "door_status_mockup"); + ros::NodeHandle nh("~"); + + + pubDoorStatus = nh.advertise("door_status", 1, true); + + subChangeDoorStatus = nh.subscribe("change_door_status", 1, changeDoorStatus); + + ros::Rate loop_rate(10); + + publishDoorStatus(); + while (ros::ok()) + { + ros::spinOnce(); + loop_rate.sleep(); + } + + return 0; +} From 4a61c95ea3d69f594cfd57af0b3eb52d4c91a67e Mon Sep 17 00:00:00 2001 From: Argentina Ortega Sainz Date: Mon, 20 May 2019 16:54:11 +0200 Subject: [PATCH 91/96] Update dependencies to mdr_door_status --- .../mdr_enter_door_action/CMakeLists.txt | 2 +- .../mdr_enter_door_action/README.md | 8 ++++---- .../mdr_enter_door_action/package.xml | 4 ++-- .../mdr_enter_door_action/ros/launch/enter_door.launch | 2 +- .../ros/src/mdr_enter_door_action/action_states.py | 2 +- .../mdr_robocup_tasks/mdr_robot_inspection/CMakeLists.txt | 4 ++-- .../mdr_robocup_tasks/mdr_robot_inspection/package.xml | 4 ++-- 7 files changed, 13 insertions(+), 13 deletions(-) diff --git a/mdr_planning/mdr_actions/mdr_navigation_actions/mdr_enter_door_action/CMakeLists.txt b/mdr_planning/mdr_actions/mdr_navigation_actions/mdr_enter_door_action/CMakeLists.txt index 44f7395f8..9a7badf74 100644 --- a/mdr_planning/mdr_actions/mdr_navigation_actions/mdr_enter_door_action/CMakeLists.txt +++ b/mdr_planning/mdr_actions/mdr_navigation_actions/mdr_enter_door_action/CMakeLists.txt @@ -9,7 +9,7 @@ find_package(catkin REQUIRED COMPONENTS roslint rospy std_msgs - mcr_door_status + mdr_door_status mdr_move_forward_action ) diff --git a/mdr_planning/mdr_actions/mdr_navigation_actions/mdr_enter_door_action/README.md b/mdr_planning/mdr_actions/mdr_navigation_actions/mdr_enter_door_action/README.md index a7e712ad3..eeb777007 100644 --- a/mdr_planning/mdr_actions/mdr_navigation_actions/mdr_enter_door_action/README.md +++ b/mdr_planning/mdr_actions/mdr_navigation_actions/mdr_enter_door_action/README.md @@ -20,17 +20,17 @@ The action takes no inputs. ## Dependencies * ``std_msgs`` -* ``mcr_door_status`` +* ``mdr_door_status`` * ``mdr_move_forward_action`` ## Example usage -1. Run the action server and its dependencies (namely ``mcr_door_status`` and ``mdr_move_forward_action``): ``roslaunch mdr_enter_door_action enter_door.launch`` +1. Run the action server and its dependencies (namely ``mdr_door_status`` and ``mdr_move_forward_action``): ``roslaunch mdr_enter_door_action enter_door.launch`` 2. Run the client example: ``rosrun mdr_enter_door_action enter_door_action_client_test`` ## Example usage (manual) -1. Launch the node that publishes the status of a door: ``roslaunch mcr_door_status door_status.launch`` +1. Launch the node that publishes the status of a door: ``roslaunch mdr_door_status door_status.launch`` 2. Run the move forward action server: ``roslaunch mdr_move_forward_action move_forward.launch`` 3. Run the action server: ``roslaunch mdr_enter_door_action enter_door_server.launch`` 4. Run the client example: ``rosrun mdr_enter_door_action enter_door_action_client_test`` @@ -40,4 +40,4 @@ The action takes no inputs. 1. Run the move forward action server: ``roslaunch mdr_move_forward_action move_forward.launch`` 2. Run the action server: ``roslaunch mdr_enter_door_action enter_door_server.launch`` 3. Run the client example: ``rosrun mdr_enter_door_action enter_door_action_client_test`` -4. Publish an ``open`` door status: ``rostopic pub -r 1 /mcr_perception/door_status/door_status std_msgs/Bool "data: true"`` +4. Publish an ``open`` door status: ``rostopic pub -r 1 /mdr_perception/door_status/door_status std_msgs/Bool "data: true"`` diff --git a/mdr_planning/mdr_actions/mdr_navigation_actions/mdr_enter_door_action/package.xml b/mdr_planning/mdr_actions/mdr_navigation_actions/mdr_enter_door_action/package.xml index c3451dfa4..d6d67693b 100644 --- a/mdr_planning/mdr_actions/mdr_navigation_actions/mdr_enter_door_action/package.xml +++ b/mdr_planning/mdr_actions/mdr_navigation_actions/mdr_enter_door_action/package.xml @@ -18,7 +18,7 @@ roslint rospy std_msgs - mcr_door_status + mdr_door_status mdr_move_forward_action std_msgs @@ -28,7 +28,7 @@ actionlib_msgs message_runtime std_msgs - mcr_door_status + mdr_door_status mdr_move_forward_action std_msgs diff --git a/mdr_planning/mdr_actions/mdr_navigation_actions/mdr_enter_door_action/ros/launch/enter_door.launch b/mdr_planning/mdr_actions/mdr_navigation_actions/mdr_enter_door_action/ros/launch/enter_door.launch index fdd17f435..ae3f9f7e3 100644 --- a/mdr_planning/mdr_actions/mdr_navigation_actions/mdr_enter_door_action/ros/launch/enter_door.launch +++ b/mdr_planning/mdr_actions/mdr_navigation_actions/mdr_enter_door_action/ros/launch/enter_door.launch @@ -1,5 +1,5 @@ - + diff --git a/mdr_planning/mdr_actions/mdr_navigation_actions/mdr_enter_door_action/ros/src/mdr_enter_door_action/action_states.py b/mdr_planning/mdr_actions/mdr_navigation_actions/mdr_enter_door_action/ros/src/mdr_enter_door_action/action_states.py index 8c0b5882d..7325ec5a0 100644 --- a/mdr_planning/mdr_actions/mdr_navigation_actions/mdr_enter_door_action/ros/src/mdr_enter_door_action/action_states.py +++ b/mdr_planning/mdr_actions/mdr_navigation_actions/mdr_enter_door_action/ros/src/mdr_enter_door_action/action_states.py @@ -10,7 +10,7 @@ class EnterDoorSM(ActionSMBase): def __init__(self, waiting_sleep_duration=1., - door_status_topic='/mcr_perception/door_status/door_status', + door_status_topic='/mdr_perception/door_status/door_status', timeout=120.0, move_forward_server='move_forward_server', movement_duration=10., speed=0.1, diff --git a/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_robot_inspection/CMakeLists.txt b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_robot_inspection/CMakeLists.txt index a5e580cdb..88da30df2 100644 --- a/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_robot_inspection/CMakeLists.txt +++ b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_robot_inspection/CMakeLists.txt @@ -8,7 +8,7 @@ find_package(catkin REQUIRED COMPONENTS smach actionlib std_msgs - mcr_door_status + mdr_door_status mdr_enter_door_action mdr_move_base_action mas_execution_manager @@ -22,7 +22,7 @@ catkin_package( actionlib std_msgs zbar_ros - mcr_door_status + mdr_door_status mdr_move_base_action mdr_enter_door_action mas_execution_manager diff --git a/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_robot_inspection/package.xml b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_robot_inspection/package.xml index f4c64f841..e62cba063 100644 --- a/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_robot_inspection/package.xml +++ b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_robot_inspection/package.xml @@ -18,7 +18,7 @@ smach_ros actionlib std_msgs - mcr_door_status + mdr_door_status mdr_enter_door_action mdr_move_base_action mas_execution_manager @@ -29,7 +29,7 @@ actionlib std_msgs zbar_ros - mcr_door_status + mdr_door_status mdr_enter_door_action mdr_move_base_action mas_execution_manager From 7700b5d6333296686e5dbb8130abcf5c4c173ba5 Mon Sep 17 00:00:00 2001 From: Argentina Ortega Sainz Date: Mon, 20 May 2019 17:07:43 +0200 Subject: [PATCH 92/96] Update dependencies to mas_perception_msgs --- .../mdr_pickup_action/CMakeLists.txt | 4 ++-- .../mdr_pickup_action/README.md | 2 +- .../mdr_pickup_action/package.xml | 4 ++-- .../mdr_pickup_action/ros/scripts/pickup_client | 2 +- .../mdr_place_action/CMakeLists.txt | 4 ++-- .../mdr_place_action/README.md | 2 +- .../mdr_place_action/package.xml | 4 ++-- .../mdr_place_action/ros/scripts/place_client | 2 +- .../mdr_place_action/ros/scripts/throw_client | 2 +- .../mdr_follow_person/CMakeLists.txt | 4 ++-- .../mdr_follow_person/package.xml | 4 ++-- .../mdr_follow_person/ros/src/follow_person_node.cpp | 12 ++++++------ .../mdr_find_object_action/CMakeLists.txt | 6 +++--- .../mdr_find_object_action/README.md | 4 ++-- .../mdr_find_object_action/package.xml | 4 ++-- .../ros/action/FindObject.action | 2 +- .../mdr_perceive_plane_action/CMakeLists.txt | 4 ++-- .../mdr_perceive_plane_action/README.md | 8 ++++---- .../mdr_perceive_plane_action/package.xml | 4 ++-- .../ros/action/PerceivePlane.action | 2 +- .../ros/scripts/perceive_plane_client | 2 +- .../src/mdr_perceive_plane_action/action_states.py | 2 +- .../pick_closest_from_surface.py | 2 +- .../ros/scripts/tests/c069_scenario_initialiser | 2 +- 24 files changed, 44 insertions(+), 44 deletions(-) diff --git a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_pickup_action/CMakeLists.txt b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_pickup_action/CMakeLists.txt index b8c9cab95..22ed6aa75 100644 --- a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_pickup_action/CMakeLists.txt +++ b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_pickup_action/CMakeLists.txt @@ -16,7 +16,7 @@ find_package(catkin REQUIRED COMPONENTS rosplan_dispatch_msgs rosplan_knowledge_msgs diagnostic_msgs - mcr_perception_msgs + mas_perception_msgs mdr_rosplan_interface mdr_move_arm_action mdr_move_base_action @@ -48,7 +48,7 @@ catkin_package( geometry_msgs trajectory_msgs diagnostic_msgs - mcr_perception_msgs + mas_perception_msgs mdr_rosplan_interface mdr_move_arm_action mdr_move_base_action diff --git a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_pickup_action/README.md b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_pickup_action/README.md index a61fdf432..075f83add 100644 --- a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_pickup_action/README.md +++ b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_pickup_action/README.md @@ -100,7 +100,7 @@ The action performs grasping with respect to the `base_link` frame (even if the * ``rosplan_dispatch_msgs`` * ``rosplan_knowledge_msgs`` * ``diagnostic_msgs`` -* ``mcr_perception_msgs`` +* ``mas_perception_msgs`` * ``mdr_rosplan_interface`` * ``mdr_move_arm_action`` * ``mdr_move_base_action`` diff --git a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_pickup_action/package.xml b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_pickup_action/package.xml index 6e75ac03b..837831a68 100644 --- a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_pickup_action/package.xml +++ b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_pickup_action/package.xml @@ -25,7 +25,7 @@ rosplan_dispatch_msgs rosplan_knowledge_msgs diagnostic_msgs - mcr_perception_msgs + mas_perception_msgs mdr_rosplan_interface mdr_move_arm_action mdr_move_base_action @@ -43,7 +43,7 @@ rosplan_dispatch_msgs rosplan_knowledge_msgs diagnostic_msgs - mcr_perception_msgs + mas_perception_msgs mdr_rosplan_interface mdr_move_arm_action mdr_move_base_action diff --git a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_pickup_action/ros/scripts/pickup_client b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_pickup_action/ros/scripts/pickup_client index 574bf5241..aa9d73460 100755 --- a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_pickup_action/ros/scripts/pickup_client +++ b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_pickup_action/ros/scripts/pickup_client @@ -3,7 +3,7 @@ import rospy import actionlib import tf -from mcr_perception_msgs.msg import Object +from mas_perception_msgs.msg import Object from mdr_rosplan_interface.action_client_base import ActionClientBase from mas_knowledge_base.knowledge_base_interface import KBException from mdr_pickup_action.msg import PickupAction, PickupGoal diff --git a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_place_action/CMakeLists.txt b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_place_action/CMakeLists.txt index 4e43300ae..d685fbe90 100644 --- a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_place_action/CMakeLists.txt +++ b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_place_action/CMakeLists.txt @@ -16,7 +16,7 @@ find_package(catkin REQUIRED COMPONENTS rosplan_dispatch_msgs rosplan_knowledge_msgs diagnostic_msgs - mcr_perception_msgs + mas_perception_msgs mdr_rosplan_interface mdr_move_arm_action mdr_move_base_action @@ -48,7 +48,7 @@ catkin_package( geometry_msgs trajectory_msgs diagnostic_msgs - mcr_perception_msgs + mas_perception_msgs mdr_rosplan_interface mdr_move_arm_action mdr_move_base_action diff --git a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_place_action/README.md b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_place_action/README.md index b432607b6..444df01db 100644 --- a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_place_action/README.md +++ b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_place_action/README.md @@ -92,7 +92,7 @@ The action performs placing with respect to the `base_link` frame (even if the g * ``rosplan_dispatch_msgs`` * ``rosplan_knowledge_msgs`` * ``diagnostic_msgs`` -* ``mcr_perception_msgs`` +* ``mas_perception_msgs`` * ``mdr_rosplan_interface`` * ``mdr_move_arm_action`` * ``mdr_move_base_action`` diff --git a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_place_action/package.xml b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_place_action/package.xml index b647a5d85..a3572177f 100644 --- a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_place_action/package.xml +++ b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_place_action/package.xml @@ -25,7 +25,7 @@ rosplan_dispatch_msgs rosplan_knowledge_msgs diagnostic_msgs - mcr_perception_msgs + mas_perception_msgs mdr_rosplan_interface mdr_move_arm_action mdr_move_base_action @@ -43,7 +43,7 @@ rosplan_dispatch_msgs rosplan_knowledge_msgs diagnostic_msgs - mcr_perception_msgs + mas_perception_msgs mdr_rosplan_interface mdr_move_arm_action mdr_move_base_action diff --git a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_place_action/ros/scripts/place_client b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_place_action/ros/scripts/place_client index a4e4ece18..6c495d3bd 100755 --- a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_place_action/ros/scripts/place_client +++ b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_place_action/ros/scripts/place_client @@ -10,7 +10,7 @@ from geometry_msgs.msg import Point, Quaternion, Pose, PoseStamped, PointStamped import rosplan_dispatch_msgs.msg as plan_dispatch_msgs import rosplan_knowledge_msgs.srv as rosplan_srvs import diagnostic_msgs.msg as diag_msgs -from mcr_perception_msgs.msg import Object, Plane +from mas_perception_msgs.msg import Object, Plane from action_execution.action import Action from action_execution.utils.configuration import Configuration diff --git a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_place_action/ros/scripts/throw_client b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_place_action/ros/scripts/throw_client index ac4f7c3e0..30065cbc6 100755 --- a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_place_action/ros/scripts/throw_client +++ b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_place_action/ros/scripts/throw_client @@ -8,7 +8,7 @@ from std_msgs.msg import Header from geometry_msgs.msg import Point, Quaternion, Pose, PoseStamped import rosplan_dispatch_msgs.msg as plan_dispatch_msgs -from mcr_perception_msgs.msg import Object +from mas_perception_msgs.msg import Object from action_execution.action import Action from ae_ros_message_converter.converter_factory import ConverterFactory diff --git a/mdr_planning/mdr_actions/mdr_navigation_actions/mdr_follow_person/CMakeLists.txt b/mdr_planning/mdr_actions/mdr_navigation_actions/mdr_follow_person/CMakeLists.txt index 7a247c43d..701cfc162 100644 --- a/mdr_planning/mdr_actions/mdr_navigation_actions/mdr_follow_person/CMakeLists.txt +++ b/mdr_planning/mdr_actions/mdr_navigation_actions/mdr_follow_person/CMakeLists.txt @@ -7,7 +7,7 @@ find_package(catkin REQUIRED geometry_msgs nav_msgs mcr_algorithms - mcr_perception_msgs + mas_perception_msgs tf std_srvs ) @@ -17,7 +17,7 @@ catkin_package( geometry_msgs nav_msgs mcr_algorithms - mcr_perception_msgs + mas_perception_msgs std_srvs ) diff --git a/mdr_planning/mdr_actions/mdr_navigation_actions/mdr_follow_person/package.xml b/mdr_planning/mdr_actions/mdr_navigation_actions/mdr_follow_person/package.xml index 1b07371be..1f5a2b44d 100644 --- a/mdr_planning/mdr_actions/mdr_navigation_actions/mdr_follow_person/package.xml +++ b/mdr_planning/mdr_actions/mdr_navigation_actions/mdr_follow_person/package.xml @@ -16,14 +16,14 @@ geometry_msgs nav_msgs mcr_algorithms - mcr_perception_msgs + mas_perception_msgs tf std_srvs geometry_msgs nav_msgs mcr_algorithms - mcr_perception_msgs + mas_perception_msgs std_srvs roslaunch diff --git a/mdr_planning/mdr_actions/mdr_navigation_actions/mdr_follow_person/ros/src/follow_person_node.cpp b/mdr_planning/mdr_actions/mdr_navigation_actions/mdr_follow_person/ros/src/follow_person_node.cpp index f2e6a268a..fa9706384 100644 --- a/mdr_planning/mdr_actions/mdr_navigation_actions/mdr_follow_person/ros/src/follow_person_node.cpp +++ b/mdr_planning/mdr_actions/mdr_navigation_actions/mdr_follow_person/ros/src/follow_person_node.cpp @@ -15,8 +15,8 @@ #include #include -#include -#include +#include +#include enum EnumBehaviorState { @@ -31,7 +31,7 @@ class PersonFollowBehavior PersonFollowBehavior(ros::NodeHandle &nh); ~PersonFollowBehavior(); - void personTrackerCallback(const mcr_perception_msgs::PersonList::ConstPtr &inputPersonList); + void personTrackerCallback(const mas_perception_msgs::PersonList::ConstPtr &inputPersonList); void odometryCallback(const nav_msgs::Odometry::ConstPtr &odom_msgs); void nextStep(); @@ -51,7 +51,7 @@ class PersonFollowBehavior PIController* pi_controller_x_; PIController* pi_controller_theta_; tf::TransformListener* _pTransformListener; - mcr_perception_msgs::PersonList _personList; + mas_perception_msgs::PersonList _personList; nav_msgs::Odometry current_odom_; double _dMaxTranslationalVelocity; double _dMaxRotationalVelocity; @@ -88,7 +88,7 @@ PersonFollowBehavior::PersonFollowBehavior(ros::NodeHandle &nh) this->_eState = eBehaviorStateInit; // subscribe and advertise - this->_subPersonTracker = this->_nh->subscribe < mcr_perception_msgs::PersonList + this->_subPersonTracker = this->_nh->subscribe < mas_perception_msgs::PersonList > ("people_positions", 1, &PersonFollowBehavior::personTrackerCallback, this); this->_subOdom = this->_nh->subscribe < nav_msgs::Odometry > ("odometry", 1, &PersonFollowBehavior::odometryCallback, this); this->_pubBaseCommands = this->_nh->advertise < geometry_msgs::Twist > ("cmd_vel", 1); @@ -370,7 +370,7 @@ void PersonFollowBehavior::nextStep() _dLastSpeed = fabs(this->_dTranslationalVelocityToSet) + fabs(this->_dRotationalVelocityToSet); } -void PersonFollowBehavior::personTrackerCallback(const mcr_perception_msgs::PersonList::ConstPtr& inputPersonList) +void PersonFollowBehavior::personTrackerCallback(const mas_perception_msgs::PersonList::ConstPtr& inputPersonList) { this->_personList = *inputPersonList; } diff --git a/mdr_planning/mdr_actions/mdr_perception_actions/mdr_find_object_action/CMakeLists.txt b/mdr_planning/mdr_actions/mdr_perception_actions/mdr_find_object_action/CMakeLists.txt index c8d3c5a66..f22fecf15 100644 --- a/mdr_planning/mdr_actions/mdr_perception_actions/mdr_find_object_action/CMakeLists.txt +++ b/mdr_planning/mdr_actions/mdr_perception_actions/mdr_find_object_action/CMakeLists.txt @@ -9,7 +9,7 @@ find_package(catkin REQUIRED COMPONENTS actionlib actionlib_msgs mas_perception_libs - mcr_perception_msgs + mas_perception_msgs mas_knowledge_base mdr_move_base_action mdr_perceive_plane_action @@ -26,7 +26,7 @@ add_action_files(DIRECTORY ros/action generate_messages( DEPENDENCIES actionlib_msgs - mcr_perception_msgs + mas_perception_msgs ) catkin_package( @@ -36,7 +36,7 @@ catkin_package( actionlib_msgs message_runtime mas_perception_libs - mcr_perception_msgs + mas_perception_msgs mas_knowledge_base mdr_move_base_action mdr_perceive_plane_action diff --git a/mdr_planning/mdr_actions/mdr_perception_actions/mdr_find_object_action/README.md b/mdr_planning/mdr_actions/mdr_perception_actions/mdr_find_object_action/README.md index f94c4e35e..bf31990e1 100644 --- a/mdr_planning/mdr_actions/mdr_perception_actions/mdr_find_object_action/README.md +++ b/mdr_planning/mdr_actions/mdr_perception_actions/mdr_find_object_action/README.md @@ -19,7 +19,7 @@ An action for finding objects in a familiar environment. The action includes an ### Result: * ``bool success``: Indicates whether the action was successfully completed -* ``mcr_perception_msgs/Object obj``: The object (if found) +* ``mas_perception_msgs/Object obj``: The object (if found) * ``string object_location``: The location of the object if the object was found * ``string relation``: The relation of the found object and its location (such as "on" or "in") @@ -76,7 +76,7 @@ The following parameters need to be passed when launching the action client: ## Dependencies * ``mas_perception_libs`` -* ``mcr_perception_msgs`` +* ``mas_perception_msgs`` * ``mas_knowledge_base`` * ``mdr_move_base_action`` * ``mdr_perceive_plane_action`` diff --git a/mdr_planning/mdr_actions/mdr_perception_actions/mdr_find_object_action/package.xml b/mdr_planning/mdr_actions/mdr_perception_actions/mdr_find_object_action/package.xml index 55e647ecf..24057f5ae 100644 --- a/mdr_planning/mdr_actions/mdr_perception_actions/mdr_find_object_action/package.xml +++ b/mdr_planning/mdr_actions/mdr_perception_actions/mdr_find_object_action/package.xml @@ -18,7 +18,7 @@ actionlib actionlib_msgs mas_perception_libs - mcr_perception_msgs + mas_perception_msgs mas_knowledge_base mdr_move_base_action mdr_perceive_plane_action @@ -28,7 +28,7 @@ actionlib_msgs message_runtime mas_perception_libs - mcr_perception_msgs + mas_perception_msgs mas_knowledge_base mdr_move_base_action mdr_perceive_plane_action diff --git a/mdr_planning/mdr_actions/mdr_perception_actions/mdr_find_object_action/ros/action/FindObject.action b/mdr_planning/mdr_actions/mdr_perception_actions/mdr_find_object_action/ros/action/FindObject.action index 9bf4223a5..288034db1 100644 --- a/mdr_planning/mdr_actions/mdr_perception_actions/mdr_find_object_action/ros/action/FindObject.action +++ b/mdr_planning/mdr_actions/mdr_perception_actions/mdr_find_object_action/ros/action/FindObject.action @@ -7,7 +7,7 @@ string object_name --- # result definition bool success -mcr_perception_msgs/Object obj +mas_perception_msgs/Object obj string object_location string relation --- diff --git a/mdr_planning/mdr_actions/mdr_perception_actions/mdr_perceive_plane_action/CMakeLists.txt b/mdr_planning/mdr_actions/mdr_perception_actions/mdr_perceive_plane_action/CMakeLists.txt index 7157b9353..8e2423e92 100644 --- a/mdr_planning/mdr_actions/mdr_perception_actions/mdr_perceive_plane_action/CMakeLists.txt +++ b/mdr_planning/mdr_actions/mdr_perception_actions/mdr_perceive_plane_action/CMakeLists.txt @@ -9,7 +9,7 @@ find_package(catkin REQUIRED COMPONENTS roslint rospy roslaunch - mcr_perception_msgs + mas_perception_msgs mas_perception_libs ) @@ -23,7 +23,7 @@ add_action_files(DIRECTORY ros/action generate_messages( DEPENDENCIES actionlib_msgs - mcr_perception_msgs + mas_perception_msgs ) catkin_package( diff --git a/mdr_planning/mdr_actions/mdr_perception_actions/mdr_perceive_plane_action/README.md b/mdr_planning/mdr_actions/mdr_perception_actions/mdr_perceive_plane_action/README.md index 9d7368fed..eb90fe9d7 100644 --- a/mdr_planning/mdr_actions/mdr_perception_actions/mdr_perceive_plane_action/README.md +++ b/mdr_planning/mdr_actions/mdr_perception_actions/mdr_perceive_plane_action/README.md @@ -15,7 +15,7 @@ interacts with ROSPlan. ### Result: * ``bool success`` -* ``mcr_perception_msgs/PlaneList recognized_planes`` +* ``mas_perception_msgs/PlaneList recognized_planes`` ### Feedback: @@ -54,9 +54,9 @@ The following arguments may be passed when launching the action server: * ``target_frame``: name of the reference frame the object and plane poses will be transformed to (default: '/base_link') * ``detection_action_name``: name of the action server used for object detection. - Action file: `mcr_perception_msgs/DetectScene.action`. + Action file: `mas_perception_msgs/DetectScene.action`. * ``recognition_service_name``: the name of the image recognition service for classifying object. - Service file: `mcr_perception_msgs/RecognizeImage.srv`. + Service file: `mas_perception_msgs/RecognizeImage.srv`. * ``recognition_model_name``: the name of the image classification model located in the `mdr_object_recognition` package. * ``preprocess_input_module``: the name of the module containing the image preprocessing function to be executed on @@ -80,7 +80,7 @@ The following parameters need to be passed when launching the action client: * ``mas_perception_libs`` * ``mdr_object_recognition`` -* ``mcr_perception_msgs`` +* ``mas_perception_msgs`` ## Example usage diff --git a/mdr_planning/mdr_actions/mdr_perception_actions/mdr_perceive_plane_action/package.xml b/mdr_planning/mdr_actions/mdr_perception_actions/mdr_perceive_plane_action/package.xml index 862e8d36c..da933b2f4 100644 --- a/mdr_planning/mdr_actions/mdr_perception_actions/mdr_perceive_plane_action/package.xml +++ b/mdr_planning/mdr_actions/mdr_perception_actions/mdr_perceive_plane_action/package.xml @@ -18,7 +18,7 @@ genmsg actionlib - mcr_perception_msgs + mas_perception_msgs mas_perception_libs actionlib_msgs message_generation @@ -30,7 +30,7 @@ actionlib actionlib_msgs message_runtime - mcr_perception_msgs + mas_perception_msgs mdr_object_recognition diff --git a/mdr_planning/mdr_actions/mdr_perception_actions/mdr_perceive_plane_action/ros/action/PerceivePlane.action b/mdr_planning/mdr_actions/mdr_perception_actions/mdr_perceive_plane_action/ros/action/PerceivePlane.action index af988b4df..d13047aca 100644 --- a/mdr_planning/mdr_actions/mdr_perception_actions/mdr_perceive_plane_action/ros/action/PerceivePlane.action +++ b/mdr_planning/mdr_actions/mdr_perception_actions/mdr_perceive_plane_action/ros/action/PerceivePlane.action @@ -4,7 +4,7 @@ string plane_frame_prefix --- # result definition bool success -mcr_perception_msgs/PlaneList recognized_planes +mas_perception_msgs/PlaneList recognized_planes --- # feedback string current_state diff --git a/mdr_planning/mdr_actions/mdr_perception_actions/mdr_perceive_plane_action/ros/scripts/perceive_plane_client b/mdr_planning/mdr_actions/mdr_perception_actions/mdr_perceive_plane_action/ros/scripts/perceive_plane_client index c0af27784..1ef3a8264 100755 --- a/mdr_planning/mdr_actions/mdr_perception_actions/mdr_perceive_plane_action/ros/scripts/perceive_plane_client +++ b/mdr_planning/mdr_actions/mdr_perception_actions/mdr_perceive_plane_action/ros/scripts/perceive_plane_client @@ -3,7 +3,7 @@ import rospy import actionlib import rosplan_dispatch_msgs.msg as plan_dispatch_msgs -from mcr_perception_msgs.msg import Plane, Object +from mas_perception_msgs.msg import Plane, Object from mdr_rosplan_interface.action_client_base import ActionClientBase from mdr_perceive_plane_action.msg import PerceivePlaneAction, PerceivePlaneGoal diff --git a/mdr_planning/mdr_actions/mdr_perception_actions/mdr_perceive_plane_action/ros/src/mdr_perceive_plane_action/action_states.py b/mdr_planning/mdr_actions/mdr_perception_actions/mdr_perceive_plane_action/ros/src/mdr_perceive_plane_action/action_states.py index 267a9039f..5d5ac487e 100644 --- a/mdr_planning/mdr_actions/mdr_perception_actions/mdr_perceive_plane_action/ros/src/mdr_perceive_plane_action/action_states.py +++ b/mdr_planning/mdr_actions/mdr_perception_actions/mdr_perceive_plane_action/ros/src/mdr_perceive_plane_action/action_states.py @@ -3,7 +3,7 @@ import sensor_msgs.msg from pyftsm.ftsm import FTSMTransitions from mas_execution.action_sm_base import ActionSMBase -from mcr_perception_msgs.msg import PlaneList +from mas_perception_msgs.msg import PlaneList from mas_perception_libs import ObjectDetector, RecognizeImageServiceProxy from mdr_perceive_plane_action.msg import PerceivePlaneResult, PerceivePlaneFeedback diff --git a/mdr_planning/mdr_behaviours/mdr_manipulation_behaviours/ros/src/mdr_manipulation_behaviours/pick_closest_from_surface.py b/mdr_planning/mdr_behaviours/mdr_manipulation_behaviours/ros/src/mdr_manipulation_behaviours/pick_closest_from_surface.py index b64c38ee3..fd1117479 100644 --- a/mdr_planning/mdr_behaviours/mdr_manipulation_behaviours/ros/src/mdr_manipulation_behaviours/pick_closest_from_surface.py +++ b/mdr_planning/mdr_behaviours/mdr_manipulation_behaviours/ros/src/mdr_manipulation_behaviours/pick_closest_from_surface.py @@ -6,7 +6,7 @@ import rosplan_dispatch_msgs.msg as plan_dispatch_msgs import diagnostic_msgs.msg as diag_msgs -from mcr_perception_msgs.msg import Object +from mas_perception_msgs.msg import Object from mas_execution_manager.scenario_state_base import ScenarioStateBase class PickClosestFromSurface(ScenarioStateBase): diff --git a/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_throw_table_objects/ros/scripts/tests/c069_scenario_initialiser b/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_throw_table_objects/ros/scripts/tests/c069_scenario_initialiser index 446a4ed44..a409d372d 100755 --- a/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_throw_table_objects/ros/scripts/tests/c069_scenario_initialiser +++ b/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_throw_table_objects/ros/scripts/tests/c069_scenario_initialiser @@ -6,7 +6,7 @@ import rosplan_knowledge_msgs.srv as rosplan_srvs import diagnostic_msgs.msg as diag_msgs from mongodb_store.message_store import MessageStoreProxy -from mcr_perception_msgs.msg import Object, Plane +from mas_perception_msgs.msg import Object, Plane class C069ScenarioInitialiser(object): def __init__(self, objects=list(), obj_names=list()): From 704ba7d48992ffa67a981cf11062ae4ad8523862 Mon Sep 17 00:00:00 2001 From: Argentina Ortega Sainz Date: Mon, 20 May 2019 17:22:12 +0200 Subject: [PATCH 93/96] [rosinstall] Update dependencies The dependencies were substituted by * mas_perception_libs * mas_perception_msgs Related PR: https://github.com/b-it-bots/mas_domestic_robotics/pull/149 --- mas-domestic.rosinstall | 9 +++++++-- 1 file changed, 7 insertions(+), 2 deletions(-) diff --git a/mas-domestic.rosinstall b/mas-domestic.rosinstall index 2d9771dbf..d45e1245b 100644 --- a/mas-domestic.rosinstall +++ b/mas-domestic.rosinstall @@ -14,8 +14,13 @@ version: kinetic - git: - local-name: mas_perception - uri: https://github.com/b-it-bots/mas_perception.git + local-name: mas_perception_libs + uri: https://github.com/b-it-bots/mas_perception_libs.git + version: kinetic + +- git: + local-name: mas_perception_msgs + uri: https://github.com/b-it-bots/mas_perception_msgs.git version: kinetic - git: From e1deb9f8f021ed26fb469e3d35d77f0944dff22e Mon Sep 17 00:00:00 2001 From: Minh Nguyen Date: Mon, 20 May 2019 20:45:24 +0200 Subject: [PATCH 94/96] remove unused launch file --- .../ros/launch/mean_circle.launch | 20 ------------------- 1 file changed, 20 deletions(-) delete mode 100644 mdr_perception/mdr_object_recognition/ros/launch/mean_circle.launch diff --git a/mdr_perception/mdr_object_recognition/ros/launch/mean_circle.launch b/mdr_perception/mdr_object_recognition/ros/launch/mean_circle.launch deleted file mode 100644 index 76109ba35..000000000 --- a/mdr_perception/mdr_object_recognition/ros/launch/mean_circle.launch +++ /dev/null @@ -1,20 +0,0 @@ - - - - - - - - - - - - - - - - - - - - From 3b9b1806d9aef0c28e2a6bbfca9e9d17f026f085 Mon Sep 17 00:00:00 2001 From: Argentina Ortega Sainz Date: Mon, 20 May 2019 22:50:56 +0200 Subject: [PATCH 95/96] [travis] Add sonarcloud addon --- .travis.yml | 10 ++++++++++ sonar-project.properties | 16 ++++++++++++++++ 2 files changed, 26 insertions(+) create mode 100644 sonar-project.properties diff --git a/.travis.yml b/.travis.yml index bc85e2e9a..9142016d4 100644 --- a/.travis.yml +++ b/.travis.yml @@ -29,6 +29,10 @@ jobs: - stage: build script: - docker-compose build travis + - stage: build + name: "Sonarcloud.io" + script: + - sonar-scanner - stage: deploy name: "Docker Hub" skip_cleanup: true @@ -41,3 +45,9 @@ jobs: notifications: slack: secure: RUiplbRX8scnvplFZhBIvSJxPDs/qCcBiMmSZnocILQsHrZMCIhv0eNoAT6FWIkXpJbS4l56BFbSdZH25/qNl/cdNwbn/YCc4N4fketeYduLcWNpwOfbf41vjFIXEbzo6wboFjlIPJDZgQFbtMThoJ2Gx6J2nBLB719j4Skj5C58yx6/ScioXZZ1LIXEE1aeOwRnju+OqX/o0dvUNrnP+6XnMtuYSicml10JV5tj3r9IJQuSAD6Bv0l/2eMparJ5UboRMwtCy5GVLfetky6WARmESJP+bKhvYE1+FR3AdRmzA47rmE+c7fZzyybSRKeUlhWPUVatTLbWxpB4ivXxKfRWqe1RiYUIAQz8U2EhWgiuR47/1qYvQ4Lefq2pgZzbCTy4Z8rL4+CjuQqRrweAIS2AQuyWfcnv01BuJX7mmdR+jA0FYWSBxdTgrJLJ70y6ZiV6QGWYFW4x9mot3E6O+vh1LfI56ctpJfwHgIFwc0xN8rMhhkTNzEG911QAZPLsNp1+v18vXF5StHXc4sTM62IagVEifTjoVkJCTytKtqBIn+ZrAcxIBBZk8Mre9doT3ulCjRtzmGLEmGGHN429jY0ufjw1pdAml/a6FDKlKwp7/QZ49qeg23/OkkZCJ1MQ3kducjtO0qCV2DcCS7dS2b8q7ZJGH+gcWGc82XAsFgs= + +addons: + sonarcloud: + organization: "b-it-bots" + token: + secure: "dRKXZht+QEdakRytXcHQQ2BeCbc6aJnKgsJoRfew90Os6jwYHsblZxgd6hT339wks8/sIq7QEQHj7Mk7h/WL9TCbkQuSmrK9sNLV0O8RYFtvxPJ7XLHM9eV4IkJSijTxfoY3KiKi/C70tWnMs8fNLVRAm68WqzHwyF0DTtr6bZU2P3YeGs8Vv559j2Rh7kXp4k7l5P9ZTo6HE5vOpt307wsNBNZLiJTSjA6RQUOUlr+LxwT7bTgiIakIbmZ7kLP+dAlF8Dwc00dnBGzOeDJ0Gocy8aoxwGKxyllOVgs3F4+7fUKSf0J7qP3JM92xv+CeKgOu/L/ktAnDwpwat0Xoweo4I1sIhaR1mCDBbHNNS8WnfVX3ItqL0F7n/6+BAn76BuFMnjVzqYHXNwLQL9H5sJhj7OMX5apOEsBcxDErgDA/umeWjvccgjqd79yCfVkq5zHueBeTEDAzfrQNyLypusZZSjfAkrbtmxK6KhWXuz4kmyR20sLNbTZiC6RgVAsSpVaJt8qpqac2bG7dqj/8vx8sso8VLRFmmPGXuF4UQI/ZYb7yWpb4/fU3kNTi+sFyNDpZe4zQITSnWGtplpS7gyejY/mMj6yQ2lElTZAvQXqeuWr5N08BB8oDRqxFQJjXae18yOoWvsDKSw0nSou7E3qHA6aqKGnCCU8FvPbv0hA=" diff --git a/sonar-project.properties b/sonar-project.properties new file mode 100644 index 000000000..1c841903b --- /dev/null +++ b/sonar-project.properties @@ -0,0 +1,16 @@ +sonar.projectKey=mas_domestic_robotics +sonar.projectName=mas_domestic_robotics +sonar.projectVersion=1.0 +sonar.sources=. + +## Don't check for C++ files +sonar.c.file.suffixes=- +sonar.cpp.file.suffixes=- +sonar.objc.file.suffixes=- + +# Meta-data +sonar.links.homepage=https://github.com/b-it-bots/mas_domestic_robotics +sonar.links.ci=https://travis-ci.org/b-it-bots/mas_domestic_robotics +sonar.links.scm=https://github.com/b-it-bots/mas_domestic_robotics +sonar.links.issue=https://github.com/b-it-bots/mas_domestic_robotics/issues + From 6960b2bfdbadcf630b81500ef66a816aac6315b3 Mon Sep 17 00:00:00 2001 From: Argentina Ortega Sainz Date: Wed, 22 May 2019 13:25:27 +0200 Subject: [PATCH 96/96] Add @PatrickNa as code owner --- .github/CODEOWNERS | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/CODEOWNERS b/.github/CODEOWNERS index 3bba08038..66571e811 100644 --- a/.github/CODEOWNERS +++ b/.github/CODEOWNERS @@ -1,7 +1,7 @@ # Default owners for everything in the repo. # Unless a later match takes precedence, they will be requested for review # when someone opens a pull request. -* @argenos @alex-mitrevski +* @argenos @alex-mitrevski @PatrickNa mdr_perception @minhnh mdr_navigation @argenos