Merge pull request #12 from gramaziokohler/TrajectoryElementAttachmen…

…tfromConfig

Trajectory element attachmentfrom config

docs/examples/scripts/docker/docker-compose.yml

@@ -0,0 +1,68 @@
+version: '2'
+services:
+  moveit-demo:
+    image: gramaziokohler/ros-noetic-moveit
+    container_name: moveit-demo
+    environment:
+      - ROS_HOSTNAME=moveit-demo
+      - ROS_MASTER_URI=http://ros-core:11311
+      # GUI Option 1: To forward the GUI to an external X11 server (eg. XMing), uncomment the following line
+      # - DISPLAY=host.docker.internal:0.0
+      # GUI Option 2: To use the web-based GUI, uncomment the following line
+      # - DISPLAY=gui:0.0
+    depends_on:
+      - ros-core
+      # To use the web-based GUI, uncomment the following line
+      # - gui
+    command:
+      - roslaunch
+      - --wait
+      # To change the robot, select the corresponding package name here, eg. `ur10e_moveit_config`
+      - ur10e_moveit_config
+      - demo.launch
+      # To launch the RVIZ GUI, change the following to true and activate one of the two GUI options above
+      - use_rviz:=false
+
+  ros-core:
+    image: gramaziokohler/ros-noetic-moveit
+    container_name: ros-core
+    ports:
+      - "11311:11311"
+    command:
+      - roscore
+
+  ros-bridge:
+    image: gramaziokohler/ros-noetic-moveit
+    container_name: ros-bridge
+    environment:
+      - "ROS_HOSTNAME=ros-bridge"
+      - "ROS_MASTER_URI=http://ros-core:11311"
+    ports:
+      - "9090:9090"
+    depends_on:
+      - ros-core
+    command:
+      - roslaunch
+      - --wait
+      - rosbridge_server
+      - rosbridge_websocket.launch
+
+  ros-fileserver:
+    image: gramaziokohler/ros-noetic-moveit
+    container_name: ros-fileserver
+    environment:
+      - ROS_HOSTNAME=ros-fileserver
+      - ROS_MASTER_URI=http://ros-core:11311
+    depends_on:
+      - ros-core
+    command:
+      - roslaunch
+      - --wait
+      - file_server
+      - file_server.launch
+
+  # To use the web-based GUI, uncomment the following lines
+  # gui:
+  #   image: gramaziokohler/novnc:latest
+  #   ports:
+  #     - "8080:8080"

docs/examples/scripts/firebase_config/compas_xr_example.json

@@ -0,0 +1 @@
+{"apiKey": "AIzaSyDm-KDwiar0NyfHfjTHFtF-tr2tgzKS3bM", "authDomain": "test-project-94f41.firebaseapp.com", "databaseURL": "https://test-project-94f41-default-rtdb.europe-west1.firebasedatabase.app", "storageBucket": "test-project-94f41.appspot.com"}

src/compas_xr/ghpython/components/Cx_GetTrajectoryRequest/code.py

@@ -1,27 +1,59 @@
 """
-Background Task component.
+Get Trajectory Request Component.
 
-Executes a long-running task in the background, while keeping Grasshopper reactive.
+A custom Compas Eve MQTT Subscriber component for receiving planning requests from the application.
 
-COMPAS EVE v0.3.4
+COMPAS XR v0.1.0
 """
 
+from ghpythonlib.componentbase import executingcomponent as component
+import functools
+import Grasshopper, GhPython
+import System
+import Rhino
+import rhinoscriptsyntax as rs
+import scriptcontext as sc
+import time
 import threading
 
-import scriptcontext as sc
+from compas.data import json_dump
+from compas_eve import Message
+from compas_eve import Topic
+from compas_eve import Subscriber
+from compas_eve.mqtt import MqttTransport
 from compas_eve.ghpython import BackgroundWorker
-from ghpythonlib.componentbase import executingcomponent as component
+from compas_xr.mqtt import GetTrajectoryRequest
+
 
-DEBUG = False
+def start_server(worker, options):
+    topic_name_request = 'compas_xr/get_trajectory_request/' + options.project_name
+
+    worker.count = 0
+
+    def get_trajectory_requested(request_message):
+        worker.count += 1
+        worker.display_message("Request #{} started".format(worker.count))
+        worker.update_result(request_message, 10)
+
+    tx = MqttTransport(options.host)
+    topic = Topic(topic_name_request, GetTrajectoryRequest)
+    worker.subscriber = Subscriber(topic, callback=get_trajectory_requested, transport=tx)
+    worker.subscriber.subscribe()
+    worker.display_message("Subscribed")
+
+def stop_server(worker):
+    if hasattr(worker, "subscriber"):
+        worker.subscriber.unsubscribe()
+    worker.display_message("Stopped")
 
 
 class BackgroundTaskComponent(component):
-    def RunScript(self, reset, task, on):
+    def RunScript(self, options, reset, on):
         if not on:
             BackgroundWorker.stop_instance_by_component(ghenv)  # noqa: F821
             return None
 
-        self.worker = BackgroundWorker.instance_by_component(ghenv, task, force_new=reset)  # noqa: F821
+        self.worker = BackgroundWorker.instance_by_component(ghenv, start_server, dispose_function=stop_server, force_new=reset, auto_set_done=False, args=(options,))  # noqa: F821
 
         if not self.worker.is_working() and not self.worker.is_done() and reset:
             self.worker.start_work()
@@ -32,6 +64,7 @@ def RunScript(self, reset, task, on):
         if hasattr(self.worker, "thread"):
             print("Worker thread: {}".format(self.worker.thread))
 
+        DEBUG = False
         if DEBUG:
             other_workers = []
             for key in sc.sticky.keys():
@@ -57,6 +90,9 @@ def RunScript(self, reset, task, on):
                     print(" - " + thread.name)
 
         if hasattr(self.worker, "result"):
-            return self.worker.result
+            element_id = self.worker.result.element_id
+            robot_name = self.worker.result.robot_name
+            return element_id, robot_name
         else:
-            return None
+            return None, None
+

src/compas_xr/ghpython/components/Cx_GetTrajectoryResult/code.py

@@ -0,0 +1,45 @@
+"""
+Get Trajectory Request Component.
+
+A custom Compas Eve MQTT Publisher component for publishing the planning result to all devices.
+
+COMPAS XR v0.1.0
+"""
+
+from ghpythonlib.componentbase import executingcomponent as component
+import time
+import Grasshopper, GhPython
+import System
+import Rhino
+import rhinoscriptsyntax as rs
+
+from compas_eve import Message
+from compas_eve import Topic
+from compas_eve import Publisher
+from compas_eve.mqtt import MqttTransport
+
+from compas_xr.mqtt import GetTrajectoryResult
+
+class PlanningServiceResponseComponent(component):
+
+    def RunScript(self, options, result, publish):
+        if not result:
+            self.Message = "Null Result, unable to publish"
+            return
+
+        if publish:
+            topic_name_result = 'compas_xr/get_trajectory_result/' + options.project_name
+            topic = Topic(topic_name_result, GetTrajectoryResult)
+            tx = MqttTransport(options.host)
+            publisher = Publisher(topic, transport=tx)
+            message = GetTrajectoryResult(
+                element_id=result.requested_element_id,
+                robot_name=options.robot_name,
+                robot_base_frame=result.robot_base_frame,            
+                trajectory=result.trajectory,
+                pick_and_place=result.pick_and_place,
+                pick_index=result.pick_index,
+                end_effector_link_name = result.end_effector_link_name
+                )
+            publisher.publish(message)
+            self.Message = "Send trajectory for #{}".format(result.requested_element_id)

src/compas_xr/ghpython/components/Cx_GetTrajectoryResult/metadata.json

@@ -0,0 +1,35 @@
+{
+    "name": "Background Task",
+    "nickname": "BG Task",
+    "category": "COMPAS XR",
+    "subcategory": "Tasks",
+    "description": "Launch long-running background tasks.",
+    "exposure": 4,
+
+    "ghpython": {
+        "isAdvancedMode": true,
+        "iconDisplay": 2,
+        "inputParameters": [
+            {
+                "name": "reset",
+                "description": "Resets the background worker."
+            },
+            {
+                "name": "task",
+                "description": "A Python function that will be executed by the background worker. The function does not need to return quickly, it can even have an infinite loop and keep running, it will not block the UI."
+            },
+            {
+                "name": "on",
+                "description": "Turn ON or OFF the background worker.",
+                "typeHintID": "bool"
+            }
+
+        ],
+        "outputParameters": [
+            {
+                "name": "result",
+                "description": "The result of the long-running task."
+            }
+        ]
+    }
+}

src/compas_xr/ghpython/components/Cx_SendTrajectory/code.py

@@ -0,0 +1,97 @@
+"""
+Get Trajectory Request Component.
+
+A custom Compas Eve MQTT Subscriber component for receiving exacution requests from the application.
+
+COMPAS XR v0.1.0
+"""
+
+from ghpythonlib.componentbase import executingcomponent as component
+import functools
+import Grasshopper, GhPython
+import System
+import Rhino
+import rhinoscriptsyntax as rs
+import time
+
+from compas_eve import Message
+from compas_eve import Topic
+from compas_eve import Subscriber
+from compas_eve.mqtt import MqttTransport
+from compas_eve.ghpython import BackgroundWorker
+
+from compas_xr.mqtt import SendTrajectory
+from compas.data import json_dump
+
+def start_server(worker, options):
+    topic_name_request = 'compas_xr/send_trajectory/' + options.project_name
+
+    worker.count = 0
+
+    def execute_trajectory_requested(request_message):
+        worker.count += 1
+        worker.display_message("Request #{} started".format(worker.count))
+        worker.update_result(request_message, 10)
+
+    tx = MqttTransport(options.host)
+    topic = Topic(topic_name_request, SendTrajectory)
+    worker.subscriber = Subscriber(topic, callback=execute_trajectory_requested, transport=tx)
+    worker.subscriber.subscribe()
+    worker.display_message("Subscribed")
+
+def stop_server(worker):
+    if hasattr(worker, "subscriber"):
+        worker.subscriber.unsubscribe()
+    worker.display_message("Stopped")
+
+
+class BackgroundTaskComponent(component):
+    def RunScript(self, options, reset, on):
+        if not on:
+            BackgroundWorker.stop_instance_by_component(ghenv)  # noqa: F821
+            return None
+
+        self.worker = BackgroundWorker.instance_by_component(ghenv, start_server, dispose_function=stop_server, force_new=reset, auto_set_done=False, args=(options,))  # noqa: F821
+
+        if not self.worker.is_working() and not self.worker.is_done() and reset:
+            self.worker.start_work()
+
+        print("Worker ID: {}".format(id(self.worker)))
+        print("Is worker running? {}".format(self.worker.is_working()))
+        print("Worker completed? {}".format(self.worker.is_done()))
+        if hasattr(self.worker, "thread"):
+            print("Worker thread: {}".format(self.worker.thread))
+
+        DEBUG = False
+        if DEBUG:
+            other_workers = []
+            for key in sc.sticky.keys():
+                if key.startswith("background_worker_"):
+                    worker = sc.sticky[key]
+                    if worker != self.worker:
+                        other_workers.append(worker)
+
+            if len(other_workers):
+                print
+                print("Found {} more workers:".format(len(other_workers)))
+                for worker in other_workers:
+                    print("* Worker ID: {}".format(id(worker)))
+                    print(" - Is worker running? {}".format(worker.is_working()))
+                    print(" - Worker completed? {}".format(worker.is_done()))
+                    print(" - Worker thread: {}".format(worker.thread))
+
+            non_main_threads = [thread for thread in threading.enumerate() if thread.name != "MainThread"]
+            if len(non_main_threads):
+                print
+                print("Found {} background threads running:".format(len(non_main_threads)))
+                for thread in non_main_threads:
+                    print(" - " + thread.name)
+
+        if hasattr(self.worker, "result"):
+            element_id = self.worker.result.element_id
+            robot_name = self.worker.result.robot_name
+            return element_id, robot_name
+        else:
+            return None, None
+
+

src/compas_xr/ghpython/components/Cx_SendTrajectory/metadata.json

@@ -0,0 +1,35 @@
+{
+    "name": "Background Task",
+    "nickname": "BG Task",
+    "category": "COMPAS XR",
+    "subcategory": "Tasks",
+    "description": "Launch long-running background tasks.",
+    "exposure": 4,
+
+    "ghpython": {
+        "isAdvancedMode": true,
+        "iconDisplay": 2,
+        "inputParameters": [
+            {
+                "name": "reset",
+                "description": "Resets the background worker."
+            },
+            {
+                "name": "task",
+                "description": "A Python function that will be executed by the background worker. The function does not need to return quickly, it can even have an infinite loop and keep running, it will not block the UI."
+            },
+            {
+                "name": "on",
+                "description": "Turn ON or OFF the background worker.",
+                "typeHintID": "bool"
+            }
+
+        ],
+        "outputParameters": [
+            {
+                "name": "result",
+                "description": "The result of the long-running task."
+            }
+        ]
+    }
+}