Added package for robonomics interoperability

rasms.repos

@@ -10,4 +10,12 @@ repositories:
   plansys2:
     type: git
     url: https://github.com/IntelligentRoboticsLabs/ros2_planning_system
-    version: master
+    version: master
+  behavior_tree:
+    type: git
+    url: https://github.com/berkeleyauv/behavior_tree
+    version: master
+  gazebo_ros_link_attacher:
+    type: git
+    url: https://github.com/davidorchansky/gazebo_ros_link_attacher.git
+    version: foxy-devel

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`

robonomics/package.xml

@@ -0,0 +1,21 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>robonomics</name>
+  <version>0.1.0</version>
+  <description>Package for Robossember-Robonomics interoperability</description>
+  <maintainer email="movefasta@dezcom.org">movefasta</maintainer>
+  <license>Apache license 2.0</license>
+
+  <depend>rclpy</depend>
+  <depend>plansys2_msgs</depend>
+
+  <test_depend>ament_copyright</test_depend>
+  <test_depend>ament_flake8</test_depend>
+  <test_depend>ament_pep257</test_depend>
+  <test_depend>python3-pytest</test_depend>
+
+  <export>
+    <build_type>ament_python</build_type>
+  </export>
+</package>

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

robonomics/setup.cfg

@@ -0,0 +1,4 @@
+[develop]
+script_dir=$base/lib/robonomics
+[install]
+install_scripts=$base/lib/robonomics

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'
+        ],
+    },
+)

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'

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)

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'