Sensor base backend - first structure

.gitignore

@@ -3,7 +3,12 @@
 docker/
 earth_moving/clutter/__pycache__/
 pyenv/
+.vscode/
+__pycache__/
+outputs/
+earth_moving/clutter/urd5/
 
 # files
 core.*
+*.pyc
 

earth_moving/clutter/scratchpad_ral.ipynb

@@ -0,0 +1,83 @@
+{
+ "cells": [
+  {
+   "cell_type": "code",
+   "execution_count": 1,
+   "metadata": {},
+   "outputs": [
+    {
+     "ename": "ImportError",
+     "evalue": "cannot import name 'BaseSensorBackend' from partially initialized module 'sensor.sensor_backend' (most likely due to a circular import) (/home/fedeoli/Documents/Work/earth_moving/earth_moving/ral/sensor/sensor_backend.py)",
+     "output_type": "error",
+     "traceback": [
+      "\u001b[0;31m---------------------------------------------------------------------------\u001b[0m",
+      "\u001b[0;31mImportError\u001b[0m                               Traceback (most recent call last)",
+      "Cell \u001b[0;32mIn[1], line 6\u001b[0m\n\u001b[1;32m      3\u001b[0m PATH \u001b[38;5;241m=\u001b[39m \u001b[38;5;124m'\u001b[39m\u001b[38;5;124m/home/fedeoli/Documents/Work/earth_moving/earth_moving/ral\u001b[39m\u001b[38;5;124m'\u001b[39m\n\u001b[1;32m      4\u001b[0m sys\u001b[38;5;241m.\u001b[39mpath\u001b[38;5;241m.\u001b[39mappend(PATH)\n\u001b[0;32m----> 6\u001b[0m \u001b[38;5;28;01mfrom\u001b[39;00m \u001b[38;5;21;01msensor\u001b[39;00m\u001b[38;5;21;01m.\u001b[39;00m\u001b[38;5;21;01msensor_backend\u001b[39;00m \u001b[38;5;28;01mimport\u001b[39;00m BaseSensorBackend\n\u001b[1;32m      7\u001b[0m \u001b[38;5;28;01mfrom\u001b[39;00m \u001b[38;5;21;01msensor\u001b[39;00m\u001b[38;5;21;01m.\u001b[39;00m\u001b[38;5;21;01msensor_rgb\u001b[39;00m \u001b[38;5;28;01mimport\u001b[39;00m SensorRGBBackend\n\u001b[1;32m      8\u001b[0m \u001b[38;5;28;01mfrom\u001b[39;00m \u001b[38;5;21;01mbackend\u001b[39;00m\u001b[38;5;21;01m.\u001b[39;00m\u001b[38;5;21;01mpybullet_backend\u001b[39;00m \u001b[38;5;28;01mimport\u001b[39;00m PybulletBackend\n",
+      "File \u001b[0;32m~/Documents/Work/earth_moving/earth_moving/ral/sensor/sensor_backend.py:2\u001b[0m\n\u001b[1;32m      1\u001b[0m \u001b[38;5;28;01mimport\u001b[39;00m \u001b[38;5;21;01mnumpy\u001b[39;00m \u001b[38;5;28;01mas\u001b[39;00m \u001b[38;5;21;01mnp\u001b[39;00m\n\u001b[0;32m----> 2\u001b[0m \u001b[38;5;28;01mfrom\u001b[39;00m \u001b[38;5;21;01mbackend\u001b[39;00m\u001b[38;5;21;01m.\u001b[39;00m\u001b[38;5;21;01mbase_backend\u001b[39;00m \u001b[38;5;28;01mimport\u001b[39;00m BaseBackend\n\u001b[1;32m      5\u001b[0m \u001b[38;5;28;01mclass\u001b[39;00m \u001b[38;5;21;01mBaseSensorBackend\u001b[39;00m(BaseBackend): \n\u001b[1;32m      7\u001b[0m     \u001b[38;5;28;01mdef\u001b[39;00m \u001b[38;5;21m__init__\u001b[39m(\u001b[38;5;28mself\u001b[39m, \u001b[38;5;241m*\u001b[39m\u001b[38;5;241m*\u001b[39mkwargs) \u001b[38;5;241m-\u001b[39m\u001b[38;5;241m>\u001b[39m \u001b[38;5;28;01mNone\u001b[39;00m:\n",
+      "File \u001b[0;32m~/Documents/Work/earth_moving/earth_moving/ral/backend/base_backend.py:3\u001b[0m\n\u001b[1;32m      1\u001b[0m \u001b[38;5;28;01mimport\u001b[39;00m \u001b[38;5;21;01mnumpy\u001b[39;00m \u001b[38;5;28;01mas\u001b[39;00m \u001b[38;5;21;01mnp\u001b[39;00m\n\u001b[1;32m      2\u001b[0m \u001b[38;5;28;01mfrom\u001b[39;00m \u001b[38;5;21;01mabc\u001b[39;00m \u001b[38;5;28;01mimport\u001b[39;00m ABC, abstractmethod\n\u001b[0;32m----> 3\u001b[0m \u001b[38;5;28;01mfrom\u001b[39;00m \u001b[38;5;21;01msensor\u001b[39;00m\u001b[38;5;21;01m.\u001b[39;00m\u001b[38;5;21;01msensor_backend\u001b[39;00m \u001b[38;5;28;01mimport\u001b[39;00m BaseSensorBackend\n\u001b[1;32m      4\u001b[0m \u001b[38;5;28;01mclass\u001b[39;00m \u001b[38;5;21;01mBaseBackend\u001b[39;00m(ABC):\n\u001b[1;32m      5\u001b[0m     \u001b[38;5;28;01mdef\u001b[39;00m \u001b[38;5;21m__init__\u001b[39m(\u001b[38;5;28mself\u001b[39m) \u001b[38;5;241m-\u001b[39m\u001b[38;5;241m>\u001b[39m \u001b[38;5;28;01mNone\u001b[39;00m:\n",
+      "\u001b[0;31mImportError\u001b[0m: cannot import name 'BaseSensorBackend' from partially initialized module 'sensor.sensor_backend' (most likely due to a circular import) (/home/fedeoli/Documents/Work/earth_moving/earth_moving/ral/sensor/sensor_backend.py)"
+     ]
+    }
+   ],
+   "source": [
+    "import sys\n",
+    "\n",
+    "PATH = '/home/fedeoli/Documents/Work/earth_moving/earth_moving/ral'\n",
+    "sys.path.append(PATH)\n",
+    "\n",
+    "from sensor.sensor_backend import BaseSensorBackend\n",
+    "from sensor.sensor_rgb import SensorRGBBackend\n",
+    "from backend.pybullet_backend import PybulletBackend"
+   ]
+  },
+  {
+   "cell_type": "code",
+   "execution_count": 16,
+   "metadata": {},
+   "outputs": [
+    {
+     "ename": "AttributeError",
+     "evalue": "'PybulletBackend' object has no attribute '_timedelta'",
+     "output_type": "error",
+     "traceback": [
+      "\u001b[0;31m---------------------------------------------------------------------------\u001b[0m",
+      "\u001b[0;31mAttributeError\u001b[0m                            Traceback (most recent call last)",
+      "Cell \u001b[0;32mIn[16], line 1\u001b[0m\n\u001b[0;32m----> 1\u001b[0m backend \u001b[38;5;241m=\u001b[39m \u001b[43mPybulletBackend\u001b[49m\u001b[43m(\u001b[49m\u001b[43mtimedelta\u001b[49m\u001b[38;5;241;43m=\u001b[39;49m\u001b[38;5;241;43m0.1\u001b[39;49m\u001b[43m)\u001b[49m\n\u001b[1;32m      2\u001b[0m cam \u001b[38;5;241m=\u001b[39m backend\u001b[38;5;241m.\u001b[39mPybulletSensorRGBBackend(\n\u001b[1;32m      3\u001b[0m     name\u001b[38;5;241m=\u001b[39m\u001b[38;5;124m'\u001b[39m\u001b[38;5;124mcam\u001b[39m\u001b[38;5;124m'\u001b[39m,\n\u001b[1;32m      4\u001b[0m     pose \u001b[38;5;241m=\u001b[39m [\u001b[38;5;241m0\u001b[39m,\u001b[38;5;241m0\u001b[39m,\u001b[38;5;241m0\u001b[39m,\u001b[38;5;241m0\u001b[39m,\u001b[38;5;241m0\u001b[39m,\u001b[38;5;241m0\u001b[39m],\n\u001b[1;32m      5\u001b[0m     imgW\u001b[38;5;241m=\u001b[39m\u001b[38;5;124m'\u001b[39m\u001b[38;5;124m120\u001b[39m\u001b[38;5;124m'\u001b[39m,\n\u001b[1;32m      6\u001b[0m     imgH\u001b[38;5;241m=\u001b[39m\u001b[38;5;124m'\u001b[39m\u001b[38;5;124m100\u001b[39m\u001b[38;5;124m'\u001b[39m\n\u001b[1;32m      7\u001b[0m     )\n",
+      "File \u001b[0;32m~/Documents/Work/earth_moving/earth_moving/ral/backend/pybullet_backend.py:10\u001b[0m, in \u001b[0;36mPybulletBackend.__init__\u001b[0;34m(self, timedelta)\u001b[0m\n\u001b[1;32m      8\u001b[0m \u001b[38;5;28;01mdef\u001b[39;00m \u001b[38;5;21m__init__\u001b[39m(\u001b[38;5;28mself\u001b[39m, timedelta) \u001b[38;5;241m-\u001b[39m\u001b[38;5;241m>\u001b[39m \u001b[38;5;28;01mNone\u001b[39;00m:\n\u001b[1;32m      9\u001b[0m     \u001b[38;5;28msuper\u001b[39m()\u001b[38;5;241m.\u001b[39m\u001b[38;5;21m__init__\u001b[39m()\n\u001b[0;32m---> 10\u001b[0m     \u001b[38;5;28;43mself\u001b[39;49m\u001b[38;5;241;43m.\u001b[39;49m\u001b[43m_timedelta\u001b[49m\n",
+      "\u001b[0;31mAttributeError\u001b[0m: 'PybulletBackend' object has no attribute '_timedelta'"
+     ]
+    }
+   ],
+   "source": [
+    "backend = PybulletBackend(timedelta=0.1)\n",
+    "cam = backend.PybulletSensorRGBBackend(\n",
+    "    name='cam',\n",
+    "    pose = [0,0,0,0,0,0],\n",
+    "    imgW='120',\n",
+    "    imgH='100'\n",
+    "    )"
+   ]
+  }
+ ],
+ "metadata": {
+  "kernelspec": {
+   "display_name": "earthmov",
+   "language": "python",
+   "name": "python3"
+  },
+  "language_info": {
+   "codemirror_mode": {
+    "name": "ipython",
+    "version": 3
+   },
+   "file_extension": ".py",
+   "mimetype": "text/x-python",
+   "name": "python",
+   "nbconvert_exporter": "python",
+   "pygments_lexer": "ipython3",
+   "version": "3.12.3"
+  }
+ },
+ "nbformat": 4,
+ "nbformat_minor": 2
+}

earth_moving/ral/backend/base_backend.py

@@ -1,24 +1,24 @@
-import numpy as np
-
-
-class BaseSensorBackend: # TODO: move to a separate file?
-    def __init__(self, **kwargs) -> None:
-        self._kwargs = kwargs
-        self._pose = ...
-
-    def get(self) -> np.array: # TODO: what if numpy isn't installed?
-        raise NotImplementedError()
-
-    def update_pose(self):
-        raise NotImplementedError()
-
-
-class BaseBackend:
-    def __init__(self) -> None:
+from abc import ABC, abstractmethod
+from ral.sensor.sensor_backend import BaseSensorBackend
+class BaseBackend(ABC):
+    def __init__(self,**kwargs) -> None:
         pass
-
+
+    def initiate_backend(self,**kwargs) -> None:
+        _simulation = kwargs.get('simulation')
+        self._backend_type = kwargs.get('backend_type')
+        if self._backend_type == 'pybullet':
+            from ral.backend.pybullet_backend import PybulletBackend
+            return PybulletBackend(simulation=_simulation)
+
     def step(self):
         raise NotImplementedError()
 
     def initiate_rgb_sensor(self) -> BaseSensorBackend:
         raise NotImplementedError()
+
+    def initiate_imu_sensor(self) -> BaseSensorBackend:
+        raise NotImplementedError()
+
+    def initiate_gyro_sensor(self) -> BaseSensorBackend:
+        raise NotImplementedError()

earth_moving/ral/backend/pybullet_backend.py

@@ -1,14 +1,85 @@
 import pybullet as p
-from earth_moving.ral.backend.base_backend import BaseBackend, BaseSensorBackend
+import pybullet_data
+import time as t
+import os
+import matplotlib.pyplot as plt
+import numpy as np
+from ral.sensor.sensor_backend import BaseSensorBackend
+from ral.backend.base_backend import BaseBackend
 
 class PybulletBackend(BaseBackend):
 
-    def __init__(self, timedelta) -> None:
-        super().__init__()
-        self._timedelta
+    def __init__(self, **kwargs) -> None:
+        self._kwargs = kwargs
+        simulation = self._kwargs.get('simulation')
+        self._timedelta = simulation.get('timedelta')
+        self._gui = simulation.get('gui')  
+        if self._gui:
+            self._physicsClient = p.connect(p.GUI)
+        else:
+            self._physicsClient = p.connect(p.DIRECT)
+        self._gravity = simulation.get('gravity')
+        p.setGravity(self._gravity[0], self._gravity[1], self._gravity[2])
+        p.setAdditionalSearchPath(pybullet_data.getDataPath())  #  PyBullet_data package (see doc)       
+        self._ID = []
+        self._ID.append(p.loadURDF("plane.urdf"))
+        self._ID.append(p.loadURDF('r2d2.urdf', basePosition=[0.0, 0.0, 1]))        
+
+    def step(self):
+        p.stepSimulation()
+        t.sleep(self._timedelta)
+
 
-    def initiate_rgb_sensor(self) -> BaseSensorBackend: # TODO: unlike ROS, this needs to happen for all sensors that we want at the beginning of the run
-        class PybulletSensorBackend(BaseSensorBackend):
+    def initiate_rgb_sensor(self,**kwargs) -> BaseSensorBackend: # TODO: unlike ROS, this needs to happen for all sensors that we want at the beginning of the run
+
+        class PybulletSensorRGBBackend(BaseSensorBackend):
+            def __init__(self,**kwargs) -> None:
+                super().__init__(**kwargs)
+                self._imgW = self._sensor.get('imgW')
+                self._imgH = self._sensor.get('imgH')
+
+                self._camera_pose = self._sensor.get('camera_pose')
+                self._target_pose = self._sensor.get('target_pose')
+                self._camera_up_vector = self._sensor.get('camera_up_vector')
+                self._viewMatrix = p.computeViewMatrix(self._camera_pose, self._target_pose, self._camera_up_vector)
+
+                self._fov = self._sensor.get('fov')
+                self._aspect = self._imgW / self._imgH
+                self._near = self._sensor.get('near')
+                self._far = self._sensor.get('far')
+                self._projectionMatrix = p.computeProjectionMatrixFOV(self._fov, self._aspect, self._near, self._far)
+
+            def get_data(self) -> np.array:                
+                data = p.getCameraImage(self._imgW,self._imgH,self._viewMatrix,self._projectionMatrix,renderer=p.ER_BULLET_HARDWARE_OPENGL)
+                return data
+
+            def plot_data(self,**kwargs):
+                data = kwargs.get('data')
+                rgb = np.reshape(data[2], (self._imgH, self._imgW, 4)) * 1. / 255.
+                plt.imshow(rgb)
+                plt.title('RGB image')
+                plt.show()
+
+            def save_data(self,**kwargs):
+                data = kwargs.get('data')
+                save_path = kwargs.get('path')
+                rgb = np.reshape(data[2], (self._imgH, self._imgW, 4)) * 1. / 255.
+                name =  kwargs.get('name') + '.png'
+                if name in os.listdir(save_path):
+                    os.remove(save_path + name)
+                plt.title('RGB image')
+                plt.imsave(save_path + name,rgb,format='png')
+
+        sensor_backend = PybulletSensorRGBBackend(**kwargs)
+        return sensor_backend
+
+    def initiate_imu_sensor(self) -> BaseSensorBackend:   
+        raise NotImplementedError() 
+
+    def initiate_gyro_sensor(self) -> BaseSensorBackend:   
+        raise NotImplementedError()  
+
+        '''class PybulletSensorRGBBackend(SensorRGBBackend):
             def __init__(self, **kwargs) -> None:
                 super().__init__(kwargs)
                 self._imgW = self._kwargs.get('imgW')
@@ -27,6 +98,6 @@ def get(self):
                     viewMatrix, projectionMatrix = f(linkWorldPosition, linkWorldOrientation)
                 img = p.getCameraImage(self._imgW, self._imgH, 
                                        viewMatrix, projectionMatrix, renderer=p.ER_BULLET_HARDWARE_OPENGL) # TODO: what if camera is mounted on the robot?
-                return img
-        sensor_backend = PybulletSensorBackend()
-        return sensor_backend
+                return img'''
+                
+

earth_moving/ral/sensor/sensor_backend.py

@@ -0,0 +1,14 @@
+import numpy as np
+from abc import ABC, abstractmethod
+
+
+class BaseSensorBackend: 
+
+    def __init__(self, **kwargs) -> None:
+        self._kwargs = kwargs
+        self._name = self._kwargs.get('name')
+        self._sensor = self._kwargs.get('sensor')
+
+    @abstractmethod # ! Is this correct? I want Pybullet/ROS to handle the actual data fetching
+    def get_data(self) -> np.array: # TODO: what if numpy isn't installed?
+        pass

earth_moving/runners/my_basic_test_scenario_pybullet_runner.py

@@ -1,19 +1,30 @@
-from earth_moving.scenarios.basic_test_scenario import basic_test_scenario
+from scenarios.basic_test_scenario import basic_test_scenario
+
 pybullet_running_camera_config = {
     'backend_type': 'pybullet',    
     'sensor':
-        {'imgW': 180,
-         'imgH': 360,
-         'camera_pose_method': lambda t: 5*t + 2}, # TODO: still need to think about `t`
+        {
+         'imgW': 400,
+         'imgH': 400,
+         'camera_pose': [10.0, 10.0, 5.0],
+         'camera_up_vector': [0.0, 0.0, 1.0],
+         'target_pose': [0.0, 0.0, 0.0],
+         'fov': 30,
+         'near': 0.0,
+         'far': 10,
+         'save_path': './outputs/fig/',
+         'camera_pose_method': lambda t: 5*t + 2 # TODO: still need to think about `t`
+        },
     'robot':
-        {'field1': 'blah',
-         'field2': 'blahblah',
+        {
+        },
     'simulation':
         {
-            'timedelta': 1.0/30,
+            'timedelta': 1.0/240,
+            'gravity': (0, 0, -9.81),
+            'gui': False
         }
     }
-}
 
 
 if __name__ == '__main__':

earth_moving/scenarios/basic_test_scenario.py

@@ -1,4 +1,17 @@
-
+from ral.backend.base_backend import BaseBackend
 
 def basic_test_scenario(config):
-    pass
+    Basebackend = BaseBackend()
+    _simulation = config.get('simulation')
+    _backend = config.get('backend_type')
+    Backend = Basebackend.initiate_backend(simulation=_simulation, backend_type=_backend)
+
+    _sensor = config.get('sensor')
+    _path = _sensor.get('save_path')
+    Camera = Backend.initiate_rgb_sensor(name='cam',sensor=_sensor)
+
+    # while True:
+    Backend.step()
+    _data = Camera.get_data()
+    # Camera.plot_data(data=_data)
+    Camera.save_data(data=_data,path=_path, name='cam_image')