From 8059726a7a2c0da54e719de5512de1d08621f20a Mon Sep 17 00:00:00 2001
From: Diego Ferigo <diego.ferigo@iit.it>
Date: Sun, 8 Nov 2020 13:11:44 +0100
Subject: [PATCH] add context helpers for sensors

---
 .../gym_ignition/context/gazebo/__init__.py   |  2 +
 python/gym_ignition/context/gazebo/sensors.py | 59 +++++++++++++++++++
 python/gym_ignition/context/gazebo/shapes.py  | 38 ++++++++++++
 3 files changed, 99 insertions(+)
 create mode 100644 python/gym_ignition/context/gazebo/sensors.py
 create mode 100644 python/gym_ignition/context/gazebo/shapes.py

diff --git a/python/gym_ignition/context/gazebo/__init__.py b/python/gym_ignition/context/gazebo/__init__.py
index 9b3a446d4..fb4022c4d 100644
--- a/python/gym_ignition/context/gazebo/__init__.py
+++ b/python/gym_ignition/context/gazebo/__init__.py
@@ -3,4 +3,6 @@
 # GNU Lesser General Public License v2.1 or any later version.
 
 from . import plugin
+from . import shapes
+from . import sensors
 from . import controllers
diff --git a/python/gym_ignition/context/gazebo/sensors.py b/python/gym_ignition/context/gazebo/sensors.py
new file mode 100644
index 000000000..e760503c4
--- /dev/null
+++ b/python/gym_ignition/context/gazebo/sensors.py
@@ -0,0 +1,59 @@
+# Copyright (C) 2020 Istituto Italiano di Tecnologia (IIT). All rights reserved.
+# This software may be modified and distributed under the terms of the
+# GNU Lesser General Public License v2.1 or any later version.
+
+from dataclasses import dataclass
+from gym_ignition.context import plugin
+
+
+@dataclass
+class DepthCamera(plugin.Plugin):  # TODO: this is not a plugin, make a sensor template?
+
+    name: str
+    width: int = 256
+    height: int = 256
+    fov: float = 1.05
+
+    topic: str = ""
+    # TODO: static
+
+    def to_xml(self) -> str:  # .sdf()?
+        xml = f"""
+        <model name="{self.model_name}">
+          <static>true</static>
+    
+          <link name="{self.name}_link">
+            <sensor name="{self.name}" type="depth_camera">
+              <always_on>1</always_on>
+              <visualize>true</visualize>
+              <topic>{self.topic}</topic>
+              <camera>
+                <horizontal_fov>{self.fov}</horizontal_fov>
+                <image>
+                  <width>{self.width}</width>
+                  <height>{self.height}</height>
+                  <format>R_FLOAT32</format>
+                </image>
+                <clip>
+                  <near>0.1</near>
+                  <far>10.0</far>
+                </clip>
+              </camera>
+            </sensor>
+            <visual name="visual">
+              <geometry>
+                <box>
+                  <size>0.1 0.1 0.1</size>
+                </box>
+              </geometry>
+            </visual>
+          </link>
+
+        </model>
+        """
+
+        return DepthCamera.wrap_in_sdf(xml)
+
+    @property
+    def model_name(self) -> str:
+        return f"{self.name}_model"
diff --git a/python/gym_ignition/context/gazebo/shapes.py b/python/gym_ignition/context/gazebo/shapes.py
new file mode 100644
index 000000000..6b5f58863
--- /dev/null
+++ b/python/gym_ignition/context/gazebo/shapes.py
@@ -0,0 +1,38 @@
+from dataclasses import dataclass
+# TODO: urdf or sdf? Urdf for iDynTree, otherwise SDF.
+
+
+@dataclass
+class BoxURDF:
+
+    mass: float = 5.0
+    edge: float = 0.2
+    name: str = "cube_robot"
+
+    def urdf(self) -> str:
+
+        i = 1.0 / 12 * self.mass * (self.edge ** 2 + self.edge ** 2)
+        urdf = f"""
+        <robot name="{self.name}" xmlns:xacro="http://www.ros.org/wiki/xacro">
+            <link name="cube">
+                <inertial>
+                  <origin rpy="0 0 0" xyz="0 0 0"/>
+                  <mass value="{self.mass}"/>
+                  <inertia ixx="{i}" ixy="0" ixz="0" iyy="{i}" iyz="0" izz="{i}"/>
+                </inertial>
+                <visual>
+                  <geometry>
+                    <box size="{self.edge} {self.edge} {self.edge}"/>
+                  </geometry>
+                  <origin rpy="0 0 0" xyz="0 0 0"/>
+                </visual>
+                <collision>
+                  <geometry>
+                    <box size="{self.edge} {self.edge} {self.edge}"/>
+                  </geometry>
+                  <origin rpy="0 0 0" xyz="0 0 0"/>
+                </collision>
+            </link>
+        </robot>"""
+
+        return urdf