Experimental support to sensors and depth camera example

bindings/gazebo/gazebo.i

@@ -9,6 +9,7 @@
 #include "scenario/gazebo/Model.h"
 #include "scenario/gazebo/utils.h"
 #include "scenario/gazebo/World.h"
+#include "scenario/gazebo/sensors/DepthCamera.h" // TODO
 #include "scenario/plugins/gazebo/ECMSingleton.h"
 #include <cstdint>
 %}
@@ -40,6 +41,9 @@ namespace scenario::gazebo::utils {
 %include <std_vector.i>
 %include <std_shared_ptr.i>
 
+// Sensors TODO
+%shared_ptr(scenario::gazebo::sensors::DepthCamera)
+
 // Import the module with core classes
 // From http://www.swig.org/Doc4.0/Modules.html
 %import "../core/core.i"
@@ -58,6 +62,7 @@ namespace scenario::gazebo::utils {
 %rename("") JointType;
 %rename("") Verbosity;
 %rename("") JointLimit;
+%rename("") DepthCamera;
 %rename("") ContactPoint;
 %rename("") ECMSingleton;
 %rename("") GazeboEntity;
@@ -74,6 +79,7 @@ namespace scenario::gazebo::utils {
 %shared_ptr(scenario::gazebo::Model)
 %shared_ptr(scenario::gazebo::World)
 %shared_ptr(scenario::gazebo::GazeboEntity)
+%shared_ptr(scenario::gazebo::sensors::DepthCamera)
 
 // Ignored methods
 %ignore scenario::gazebo::GazeboEntity::ecm;
@@ -98,6 +104,7 @@ namespace scenario::gazebo::utils {
 %include "scenario/gazebo/GazeboEntity.h"
 
 // ScenarI/O headers
+%include "scenario/gazebo/sensors/DepthCamera.h"
 %include "scenario/gazebo/Joint.h"
 %include "scenario/gazebo/Link.h"
 %include "scenario/gazebo/Model.h"

cpp/scenario/gazebo/CMakeLists.txt

@@ -43,6 +43,8 @@ set(EXTRA_COMPONENTS_PUBLIC_HEADERS
     include/scenario/gazebo/components/Timestamp.h
     include/scenario/gazebo/components/JointControllerPeriod.h
     include/scenario/gazebo/components/JointAcceleration.h
+    include/scenario/gazebo/components/DepthCameraPtr.h
+    include/scenario/gazebo/components/SensorsPlugin.h
     )
 
 add_library(ExtraComponents INTERFACE)
@@ -76,7 +78,8 @@ set(SCENARIO_GAZEBO_PUBLIC_HDRS
     include/scenario/gazebo/Log.h
     include/scenario/gazebo/utils.h
     include/scenario/gazebo/helpers.h
-    include/scenario/gazebo/exceptions.h)
+    include/scenario/gazebo/exceptions.h
+    include/scenario/gazebo/sensors/DepthCamera.h)
 
 add_library(ScenarioGazebo
     ${SCENARIO_GAZEBO_PUBLIC_HDRS}
@@ -85,7 +88,8 @@ add_library(ScenarioGazebo
     src/Joint.cpp
     src/Link.cpp
     src/utils.cpp
-    src/helpers.cpp)
+    src/helpers.cpp
+    src/DepthCamera.cpp)
 add_library(ScenarioGazebo::ScenarioGazebo ALIAS ScenarioGazebo)
 
 target_include_directories(ScenarioGazebo PUBLIC
@@ -99,7 +103,9 @@ target_link_libraries(ScenarioGazebo
     ${ignition-common.ignition-common}
     PRIVATE
     ScenarioCore::CoreUtils
-    ScenarioGazebo::ExtraComponents)
+    ScenarioGazebo::ExtraComponents
+    ${ignition-sensors.ignition-sensors-all}
+    ${ignition-rendering.ignition-rendering})
 
 set_target_properties(ScenarioGazebo PROPERTIES
     PUBLIC_HEADER "${SCENARIO_GAZEBO_PUBLIC_HDRS}")

cpp/scenario/gazebo/include/scenario/gazebo/Model.h

@@ -43,6 +43,10 @@ namespace scenario::gazebo {
     class Model;
 } // namespace scenario::gazebo
 
+namespace scenario::gazebo::sensors {
+    class DepthCamera;
+} // namespace scenario::gazebo::sensors
+
 class scenario::gazebo::Model final
     : public scenario::core::Model
     , public scenario::gazebo::GazeboEntity
@@ -343,6 +347,21 @@ class scenario::gazebo::Model final
 
     std::array<double, 3> baseWorldAngularAccelerationTarget() const override;
 
+    // =======
+    // Sensors
+    // =======
+
+    std::vector<std::string> sensorNames() const;
+
+    /**
+     * Return a depth camera belonging to this model.
+     *
+     * @param name The name of the depth camera sensor.
+     * @return The desired depth camera sensor.
+     */
+    std::shared_ptr<sensors::DepthCamera>
+    depthCamera(const std::string& name) const;
+
 private:
     class Impl;
     std::unique_ptr<Impl> pImpl;

cpp/scenario/gazebo/include/scenario/gazebo/World.h

@@ -138,6 +138,17 @@ class scenario::gazebo::World final
      */
     bool removeModel(const std::string& modelName);
 
+    /**
+     * Enable the sensors system.
+     *
+     * @note Disabling the sensors once they are already running is not yet
+     * supported.
+     *
+     * @param enable True to enable, false to disable.
+     * @return True for success, false otherwise.
+     */
+    bool enableSensors(const bool enable = true);
+
     // ==========
     // World Core
     // ==========

cpp/scenario/gazebo/include/scenario/gazebo/components/DepthCameraPtr.h

@@ -0,0 +1,55 @@
+/*
+ * Copyright (C) 2020 Istituto Italiano di Tecnologia (IIT)
+ * All rights reserved.
+ *
+ * This project is dual licensed under LGPL v2.1+ or Apache License.
+ *
+ * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
+ *
+ * This software may be modified and distributed under the terms of the
+ * GNU Lesser General Public License v2.1 or any later version.
+ *
+ * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
+ *
+ * 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.
+ */
+
+#ifndef IGNITION_GAZEBO_COMPONENTS_DEPTHCAMERAPTR_H
+#define IGNITION_GAZEBO_COMPONENTS_DEPTHCAMERAPTR_H
+
+#include <ignition/gazebo/components/Component.hh>
+#include <ignition/gazebo/components/Factory.hh>
+#include <ignition/gazebo/config.hh>
+#include <ignition/sensors/config.hh>
+
+namespace ignition::sensors {
+    inline namespace IGNITION_SENSORS_VERSION_NAMESPACE {
+        class DepthCameraSensor;
+    } // namespace IGNITION_SENSORS_VERSION_NAMESPACE
+} // namespace ignition::sensors
+
+namespace ignition::gazebo {
+    // Inline bracket to help doxygen filtering.
+    inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE {
+        namespace components {
+            /// \brief TODO
+            using DepthCameraPtr =
+                Component<sensors::DepthCameraSensor*, class DepthCameraPtrTag>;
+            IGN_GAZEBO_REGISTER_COMPONENT(
+                "ign_gazebo_components.DepthCameraPtr",
+                DepthCameraPtr)
+        } // namespace components
+    } // namespace IGNITION_GAZEBO_VERSION_NAMESPACE
+} // namespace ignition::gazebo
+
+#endif // IGNITION_GAZEBO_COMPONENTS_DEPTHCAMERAPTR_H

cpp/scenario/gazebo/include/scenario/gazebo/components/SensorsPlugin.h

@@ -0,0 +1,47 @@
+/*
+ * Copyright (C) 2020 Istituto Italiano di Tecnologia (IIT)
+ * All rights reserved.
+ *
+ * This project is dual licensed under LGPL v2.1+ or Apache License.
+ *
+ * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
+ *
+ * This software may be modified and distributed under the terms of the
+ * GNU Lesser General Public License v2.1 or any later version.
+ *
+ * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
+ *
+ * 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.
+ */
+
+#ifndef IGNITION_GAZEBO_COMPONENTS_SENSORSPLUGIN_H
+#define IGNITION_GAZEBO_COMPONENTS_SENSORSPLUGIN_H
+
+#include <ignition/gazebo/components/Component.hh>
+#include <ignition/gazebo/components/Factory.hh>
+#include <ignition/gazebo/config.hh>
+
+namespace ignition::gazebo {
+    // Inline bracket to help doxygen filtering.
+    inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE {
+        namespace components {
+            /// \brief Marks whether a world has a Sensors plugin.
+            using SensorsPlugin = Component<bool, class SensorsPluginTag>;
+            IGN_GAZEBO_REGISTER_COMPONENT(
+                "ign_gazebo_components.SensorsPlugin",
+                SensorsPlugin)
+        } // namespace components
+    } // namespace IGNITION_GAZEBO_VERSION_NAMESPACE
+} // namespace ignition::gazebo
+
+#endif // IGNITION_GAZEBO_COMPONENTS_SENSORSPLUGIN_H

cpp/scenario/gazebo/include/scenario/gazebo/helpers.h

@@ -79,6 +79,9 @@ namespace scenario::gazebo::utils {
                                const long rows,
                                const long cols);
 
+    std::vector<std::string> tokenize(const std::string& input,
+                                      const std::string& delimiter);
+
     class FixedSizeQueue
     {
     public:

cpp/scenario/gazebo/include/scenario/gazebo/sensors/DepthCamera.h

@@ -0,0 +1,64 @@
+/*
+ * 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.
+ */
+
+#ifndef SCENARIO_GAZEBO_SENSORS_DEPTHCAMERA_H
+#define SCENARIO_GAZEBO_SENSORS_DEPTHCAMERA_H
+
+#include "scenario/gazebo/GazeboEntity.h"
+
+#include <ignition/gazebo/Entity.hh>
+#include <ignition/gazebo/EntityComponentManager.hh>
+#include <ignition/gazebo/EventManager.hh>
+
+#include <memory>
+#include <string>
+#include <vector>
+
+namespace scenario::gazebo::sensors {
+    class DepthCamera;
+} // namespace scenario::gazebo::sensors
+
+class scenario::gazebo::sensors::DepthCamera final
+    : public scenario::gazebo::GazeboEntity
+{
+public:
+    DepthCamera();
+    virtual ~DepthCamera();
+
+    // =============
+    // Gazebo Entity
+    // =============
+
+    uint64_t id() const override;
+
+    bool initialize(const ignition::gazebo::Entity parentEntity,
+                    ignition::gazebo::EntityComponentManager* ecm,
+                    ignition::gazebo::EventManager* eventManager) override;
+
+    bool createECMResources() override;
+
+    // ===========
+    // Camera Core
+    // ===========
+
+    std::string name(const bool scoped = false) const;
+
+    double width() const;
+    double height() const;
+
+    double farClip() const;
+    double nearClip() const;
+
+    const std::vector<float>& image() const;
+
+private:
+    class Impl;
+    std::unique_ptr<Impl> pImpl;
+};
+
+#endif // SCENARIO_GAZEBO_SENSORS_DEPTHCAMERA_H