Upgrade ign-sensors and support custom sensors (#617)

Signed-off-by: Louise Poubel <[email protected]>

Co-authored-by: Alejandro Hernández Cordero <[email protected]>

CMakeLists.txt

@@ -105,16 +105,24 @@ set(IGN_PHYSICS_VER ${ignition-physics5_VERSION_MAJOR})
 #--------------------------------------
 # Find ignition-sensors
 ign_find_package(ignition-sensors6 REQUIRED
+  # component order is important
   COMPONENTS
-    rendering
+    # non-rendering
     air_pressure
     altimeter
-    camera
-    gpu_lidar
     imu
     logical_camera
     magnetometer
+
+    # rendering
+    rendering
+    lidar
+    gpu_lidar
+
+    # cameras
+    camera
     depth_camera
+    rgbd_camera
     thermal_camera
 )
 set(IGN_SENSORS_VER ${ignition-sensors6_VERSION_MAJOR})

examples/plugin/custom_sensor_system/CMakeLists.txt

@@ -0,0 +1,36 @@
+cmake_minimum_required(VERSION 3.11.0 FATAL_ERROR)
+
+find_package(ignition-cmake2 REQUIRED)
+
+project(OdometerSystem)
+
+ign_find_package(ignition-plugin1 REQUIRED COMPONENTS register)
+set(IGN_PLUGIN_VER ${ignition-plugin1_VERSION_MAJOR})
+
+ign_find_package(ignition-gazebo6 REQUIRED)
+set(IGN_GAZEBO_VER ${ignition-gazebo6_VERSION_MAJOR})
+
+find_package(ignition-sensors6 REQUIRED)
+set(IGN_SENSORS_VER ${ignition-sensors6_VERSION_MAJOR})
+
+# Fetch the custom sensor example from ign-sensors
+# Users won't commonly use this to fetch their sensors. The sensor may be part
+# of the system's CMake project, or installed from another project, etc...
+include(FetchContent)
+FetchContent_Declare(
+  sensors_clone
+  GIT_REPOSITORY https://github.com/ignitionrobotics/ign-sensors
+  GIT_TAG main
+)
+FetchContent_Populate(sensors_clone)
+add_subdirectory(${sensors_clone_SOURCE_DIR}/examples/custom_sensor ${sensors_clone_BINARY_DIR})
+
+add_library(${PROJECT_NAME} SHARED ${PROJECT_NAME}.cc)
+target_link_libraries(${PROJECT_NAME}
+  PRIVATE ignition-plugin${IGN_PLUGIN_VER}::ignition-plugin${IGN_PLUGIN_VER}
+  PRIVATE ignition-gazebo${IGN_GAZEBO_VER}::ignition-gazebo${IGN_GAZEBO_VER}
+  PRIVATE ignition-sensors${IGN_SENSORS_VER}::ignition-sensors${IGN_SENSORS_VER}
+  PRIVATE odometer
+)
+target_include_directories(${PROJECT_NAME}
+    PUBLIC ${sensors_clone_SOURCE_DIR}/examples/custom_sensor)

examples/plugin/custom_sensor_system/OdometerSystem.cc

@@ -0,0 +1,132 @@
+/*
+ * Copyright (C) 2021 Open Source Robotics Foundation
+ *
+ * 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.
+ *
+ */
+
+#include <ignition/msgs/double.pb.h>
+
+#include <string>
+#include <unordered_map>
+#include <utility>
+
+#include <ignition/common/Profiler.hh>
+#include <ignition/plugin/Register.hh>
+#include <ignition/sensors/Noise.hh>
+#include <ignition/sensors/SensorFactory.hh>
+
+#include <sdf/Sensor.hh>
+
+#include <ignition/gazebo/components/CustomSensor.hh>
+#include <ignition/gazebo/components/Name.hh>
+#include <ignition/gazebo/components/ParentEntity.hh>
+#include <ignition/gazebo/components/Sensor.hh>
+#include <ignition/gazebo/components/World.hh>
+#include <ignition/gazebo/EntityComponentManager.hh>
+#include <ignition/gazebo/Util.hh>
+
+#include "Odometer.hh"
+#include "OdometerSystem.hh"
+
+using namespace custom;
+
+//////////////////////////////////////////////////
+void OdometerSystem::PreUpdate(const ignition::gazebo::UpdateInfo &,
+    ignition::gazebo::EntityComponentManager &_ecm)
+{
+  _ecm.EachNew<ignition::gazebo::components::CustomSensor,
+               ignition::gazebo::components::ParentEntity>(
+    [&](const ignition::gazebo::Entity &_entity,
+        const ignition::gazebo::components::CustomSensor *_custom,
+        const ignition::gazebo::components::ParentEntity *_parent)->bool
+      {
+        // Get sensor's scoped name without the world
+        auto sensorScopedName = ignition::gazebo::removeParentScope(
+            ignition::gazebo::scopedName(_entity, _ecm, "::", false), "::");
+        sdf::Sensor data = _custom->Data();
+        data.SetName(sensorScopedName);
+
+        // Default to scoped name as topic
+        if (data.Topic().empty())
+        {
+          std::string topic = scopedName(_entity, _ecm) + "/odometer";
+          data.SetTopic(topic);
+        }
+
+        ignition::sensors::SensorFactory sensorFactory;
+        auto sensor = sensorFactory.CreateSensor<custom::Odometer>(data);
+        if (nullptr == sensor)
+        {
+          ignerr << "Failed to create odometer [" << sensorScopedName << "]"
+                 << std::endl;
+          return false;
+        }
+
+        // Set sensor parent
+        auto parentName = _ecm.Component<ignition::gazebo::components::Name>(
+            _parent->Data())->Data();
+        sensor->SetParent(parentName);
+
+        // Set topic on Gazebo
+        _ecm.CreateComponent(_entity,
+            ignition::gazebo::components::SensorTopic(sensor->Topic()));
+
+        // Keep track of this sensor
+        this->entitySensorMap.insert(std::make_pair(_entity,
+            std::move(sensor)));
+
+        return true;
+      });
+}
+
+//////////////////////////////////////////////////
+void OdometerSystem::PostUpdate(const ignition::gazebo::UpdateInfo &_info,
+    const ignition::gazebo::EntityComponentManager &_ecm)
+{
+  // Only update and publish if not paused.
+  if (!_info.paused)
+  {
+    for (auto &[entity, sensor] : this->entitySensorMap)
+    {
+      sensor->NewPosition(ignition::gazebo::worldPose(entity, _ecm).Pos());
+      sensor->Update(_info.simTime);
+    }
+  }
+
+  this->RemoveSensorEntities(_ecm);
+}
+
+//////////////////////////////////////////////////
+void OdometerSystem::RemoveSensorEntities(
+    const ignition::gazebo::EntityComponentManager &_ecm)
+{
+  _ecm.EachRemoved<ignition::gazebo::components::CustomSensor>(
+    [&](const ignition::gazebo::Entity &_entity,
+        const ignition::gazebo::components::CustomSensor *)->bool
+      {
+        if (this->entitySensorMap.erase(_entity) == 0)
+        {
+          ignerr << "Internal error, missing odometer for entity ["
+                         << _entity << "]" << std::endl;
+        }
+        return true;
+      });
+}
+
+IGNITION_ADD_PLUGIN(OdometerSystem, ignition::gazebo::System,
+  OdometerSystem::ISystemPreUpdate,
+  OdometerSystem::ISystemPostUpdate
+)
+
+IGNITION_ADD_PLUGIN_ALIAS(OdometerSystem, "custom::OdometerSystem")

examples/plugin/custom_sensor_system/OdometerSystem.hh

@@ -0,0 +1,56 @@
+/*
+ * Copyright (C) 2021 Open Source Robotics Foundation
+ *
+ * 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 ODOMETERSYSTEM_HH_
+#define ODOMETERSYSTEM_HH_
+
+#include <ignition/gazebo/System.hh>
+#include <ignition/sensors/Sensor.hh>
+#include <ignition/transport/Node.hh>
+
+namespace custom
+{
+  /// \brief Example showing how to tie a custom sensor, in this case an
+  /// odometer, into simulation
+  class OdometerSystem:
+    public ignition::gazebo::System,
+    public ignition::gazebo::ISystemPreUpdate,
+    public ignition::gazebo::ISystemPostUpdate
+  {
+    // Documentation inherited.
+    // During PreUpdate, check for new sensors that were inserted
+    // into simulation and create more components as needed.
+    public: void PreUpdate(const ignition::gazebo::UpdateInfo &_info,
+        ignition::gazebo::EntityComponentManager &_ecm) final;
+
+    // Documentation inherited.
+    // During PostUpdate, update the known sensors and publish their data.
+    // Also remove sensors that have been deleted.
+    public: void PostUpdate(const ignition::gazebo::UpdateInfo &_info,
+        const ignition::gazebo::EntityComponentManager &_ecm) final;
+
+    /// \brief Remove custom sensors if their entities have been removed from
+    /// simulation.
+    /// \param[in] _ecm Immutable reference to ECM.
+    private: void RemoveSensorEntities(
+        const ignition::gazebo::EntityComponentManager &_ecm);
+
+    /// \brief A map of custom entities to their sensors
+    private: std::unordered_map<ignition::gazebo::Entity,
+        std::shared_ptr<Odometer>> entitySensorMap;
+  };
+}
+#endif

examples/plugin/custom_sensor_system/README.md

@@ -0,0 +1,45 @@
+# Custom sensor system
+
+This example shows how to use a custom sensor with Ignition Gazebo.
+
+It uses the odometer created on this example:
+[ign-sensors/examples/custom_sensor](https://github.com/ignitionrobotics/ign-sensors/tree/main/examples/custom_sensor).
+
+## Build
+
+From the root of the `ign-gazebo` repository, do the following to build the example:
+
+~~~
+cd examples/plugins/custom_sensor_system
+mkdir build
+cd build
+cmake ..
+make
+~~~
+
+This will generate the `OdometerSystem` library under `build`.
+
+## Run
+
+The plugin must be attached to an entity to be loaded. This is demonstrated in
+the `odometer.sdf` file that's going to be loaded.
+
+Before starting Gazebo, we must make sure it can find the plugin by doing:
+
+~~~
+cd examples/plugins/custom_sensor_system
+export IGN_GAZEBO_SYSTEM_PLUGIN_PATH=`pwd`/build
+~~~
+
+Then load the example world:
+
+    ign gazebo -r odometer.sdf
+
+You should see a box slowly moving in a straight line.
+
+Listen to the odometer data with:
+
+```
+ign topic -e -t /world/odometer_world/model/model_with_sensor/link/link/sensor/an_odometer/odometer
+```
+