diff --git a/robonomics/README.md b/robonomics/README.md new file mode 100644 index 0000000..a7eda78 --- /dev/null +++ b/robonomics/README.md @@ -0,0 +1,31 @@ +# Robonomics ROS2 package + +> **On alpha developement stage! Don't use in production!** + +Package for interoperability between Robonomics Blockchain and ROS2 Planning System. + +## Workflow +- Subscribe for events in local robonomics node +- When event recieved convert `H256` hash to `IPFS` hash and download it's content from IPFS network +- Validate `domain` and `problem` PDDL specifications through `GetPlan` Plansys2 service. If Succeed add problem to Plansys2 and get generated plan +- Execute plan with Plansys2 `Executor` + +## Supported robonomics events +- `NewLaunch` + +## Requrements +- `go-ipfs` 0.7.0 +- `robonomics` blockchain node +- Python packages + - `robonomicsinterface` 0.10.1 + - `ipfshttpclient` 0.7.0 +- ROS2 packages + - `plansys2` + +## Future plans +- `Write Datalog` on successful plan execution +- `Write Datalog` on every completed action in plan +- Support other Robonomics Extrinsics: + - `Digital Twin` + - `Liability` + - `RWS` \ No newline at end of file diff --git a/robonomics/package.xml b/robonomics/package.xml new file mode 100644 index 0000000..cc40c3c --- /dev/null +++ b/robonomics/package.xml @@ -0,0 +1,21 @@ + + + + robonomics + 0.1.0 + Package for Robossember-Robonomics interoperability + movefasta + Apache license 2.0 + + rclpy + plansys2_msgs + + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + + + ament_python + + diff --git a/robonomics/resource/robonomics b/robonomics/resource/robonomics new file mode 100644 index 0000000..e69de29 diff --git a/robonomics/robonomics/__init__.py b/robonomics/robonomics/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/robonomics/robonomics/client.py b/robonomics/robonomics/client.py new file mode 100644 index 0000000..3c263d1 --- /dev/null +++ b/robonomics/robonomics/client.py @@ -0,0 +1,151 @@ +import sys +import ipfshttpclient as IPFS +import robonomicsinterface as RI +import rclpy + +from base58 import b58decode, b58encode +from rclpy.node import Node +from rclpy.action import ActionClient +from plansys2_msgs.srv import GetDomain, GetPlan, AddProblem +from plansys2_msgs.action import ExecutePlan +from action_msgs.msg import GoalStatus + +class Planner(Node): + + def __init__(self): + super().__init__('planner_client_async') + self.cli = self.create_client(GetPlan, '/planner/get_plan') + while not self.cli.wait_for_service(timeout_sec=1.0): + self.get_logger().info('service not available, waiting again...') + self.req = GetPlan.Request() + + def get_plan(self, domain, problem): + self.req.domain = domain + self.req.problem = problem + self.future = self.cli.call_async(self.req) + +class ProblemAdder(Node): + + def __init__(self): + super().__init__('problem_expert_client_async') + self.cli = self.create_client(AddProblem, '/problem_expert/add_problem') + while not self.cli.wait_for_service(timeout_sec=1.0): + self.get_logger().info('service not available, waiting again...') + self.req = AddProblem.Request() + + def add_problem(self, problem): + self.req.problem = problem + self.future = self.cli.call_async(self.req) + + +class Executor(Node): + + def __init__(self): + super().__init__('execute_plan_action_client') + self._action_client = ActionClient(self, ExecutePlan, 'execute_plan') + + def goal_response_callback(self, future): + goal_handle = future.result() + if not goal_handle.accepted: + self.get_logger().info('Goal rejected :(') + return + + self._goal_handle = goal_handle + + self.get_logger().info('Goal accepted :)') + + self._get_result_future = goal_handle.get_result_async() + self._get_result_future.add_done_callback(self.get_result_callback) + + def feedback_callback(self, feedback): + self.get_logger().info('Received feedback: {0}'.format(feedback.feedback.action_execution_status)) + + def get_result_callback(self, future): + result = future.result().result + status = future.result().status + if status == GoalStatus.STATUS_SUCCEEDED: + self.get_logger().info('Goal succeeded! Result: {0}'.format(result.action_execution_status)) + else: + self.get_logger().info('Goal failed with status: {0}'.format(status)) + + # Shutdown after receiving a result + rclpy.shutdown() + + def send_goal(self, plan): + self.get_logger().info('Waiting for action server...') + self._action_client.wait_for_server() + + goal_msg = ExecutePlan.Goal() + goal_msg.plan = plan + + self.get_logger().info('Sending goal request...') + + self._send_goal_future = self._action_client.send_goal_async(goal_msg) + # feedback_callback=self.feedback_callback) + + self._send_goal_future.add_done_callback(self.goal_response_callback) + + +def main(args=None): + rclpy.init(args=args) + seed = sys.argv[1] + robonomics = RI.RobonomicsInterface(seed=seed, remote_ws='ws://127.0.0.1:9944') + addr = robonomics.define_address() + + def launch_event_execution(data): + # Fetch IPFS content for task specification + ipfs = IPFS.connect() + cid = robonomics.ipfs_32_bytes_to_qm_hash(data[2]) + dir = ipfs.object.get(cid) + print("Task specification CID: ", cid) + for link in dir['Links']: + if link['Name'] == 'domain.pddl': + domain = ipfs.cat(link['Hash']).decode("utf-8") + print("Got PDDL domain", domain) + if link['Name'] == 'problem.pddl': + problem = ipfs.cat(link['Hash']).decode("utf-8") + print("Got PDDL problem", problem) + + # Init & run ROS2 service client + planner_client = Planner() + planner_client.get_plan(domain, problem) + rclpy.spin_until_future_complete(planner_client, planner_client.future) + if planner_client.future.done(): + try: + response = planner_client.future.result() + except Exception as e: + planner_client.get_logger().info( + 'GetPlan call failed %r' % (e,)) + else: + if response.success: + plan = response.plan + planner_client.get_logger().info('Got Plan: %s' % (response.plan)) + planner_client.destroy_node() + + problem_client = ProblemAdder() + problem_client.add_problem(problem) + problem_client.get_logger().info('Problem Adder Client Started') + rclpy.spin_until_future_complete(problem_client, problem_client.future) + if problem_client.future.done(): + problem_client.get_logger().info('Problem client future done') + try: + response = problem_client.future.result() + except Exception as e: + problem_client.get_logger().info( + 'AddProblem failed %r' % (e,)) + else: + if response.success: + problem_client.get_logger().info('Problem Added! Starting Plan Execution...') + executor_client = Executor() + executor_client.send_goal(plan) + rclpy.spin(executor_client) + else: + problem_client.get_logger().info('Problem Add Failed: %s', response.error_info) + problem_client.destroy_node() + + while rclpy.ok(): + RI.Subscriber(robonomics, RI.SubEvent.NewLaunch, launch_event_execution, addr) + + +if __name__ == '__main__': + main() \ No newline at end of file diff --git a/robonomics/setup.cfg b/robonomics/setup.cfg new file mode 100644 index 0000000..2bc9663 --- /dev/null +++ b/robonomics/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/robonomics +[install] +install_scripts=$base/lib/robonomics diff --git a/robonomics/setup.py b/robonomics/setup.py new file mode 100644 index 0000000..fa105bf --- /dev/null +++ b/robonomics/setup.py @@ -0,0 +1,26 @@ +from setuptools import setup + +package_name = 'robonomics' + +setup( + name=package_name, + version='0.0.0', + packages=[package_name], + data_files=[ + ('share/ament_index/resource_index/packages', + ['resource/' + package_name]), + ('share/' + package_name, ['package.xml']), + ], + install_requires=['setuptools'], + zip_safe=True, + maintainer='movefasta', + maintainer_email='movefasta@dezcom.org', + description='Service for Robossember-Robonomics interoperability', + license='Apache 2.0', + tests_require=['pytest'], + entry_points={ + 'console_scripts': [ + 'client = robonomics.client:main' + ], + }, +) diff --git a/robonomics/test/test_copyright.py b/robonomics/test/test_copyright.py new file mode 100644 index 0000000..cc8ff03 --- /dev/null +++ b/robonomics/test/test_copyright.py @@ -0,0 +1,23 @@ +# Copyright 2015 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from ament_copyright.main import main +import pytest + + +@pytest.mark.copyright +@pytest.mark.linter +def test_copyright(): + rc = main(argv=['.', 'test']) + assert rc == 0, 'Found errors' diff --git a/robonomics/test/test_flake8.py b/robonomics/test/test_flake8.py new file mode 100644 index 0000000..27ee107 --- /dev/null +++ b/robonomics/test/test_flake8.py @@ -0,0 +1,25 @@ +# Copyright 2017 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from ament_flake8.main import main_with_errors +import pytest + + +@pytest.mark.flake8 +@pytest.mark.linter +def test_flake8(): + rc, errors = main_with_errors(argv=[]) + assert rc == 0, \ + 'Found %d code style errors / warnings:\n' % len(errors) + \ + '\n'.join(errors) diff --git a/robonomics/test/test_pep257.py b/robonomics/test/test_pep257.py new file mode 100644 index 0000000..b234a38 --- /dev/null +++ b/robonomics/test/test_pep257.py @@ -0,0 +1,23 @@ +# Copyright 2015 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from ament_pep257.main import main +import pytest + + +@pytest.mark.linter +@pytest.mark.pep257 +def test_pep257(): + rc = main(argv=['.', 'test']) + assert rc == 0, 'Found code style errors / warnings'