Triggered Camera

CMakeLists.txt

@@ -86,7 +86,7 @@ set(IGN_MSGS_VER ${ignition-msgs8_VERSION_MAJOR})
 
 #--------------------------------------
 # Find SDFormat
-ign_find_package(sdformat12 REQUIRED)
+ign_find_package(sdformat12 REQUIRED VERSION 12.4.0)
 set(SDF_VER ${sdformat12_VERSION_MAJOR})
 
 set(IGN_SENSORS_PLUGIN_PATH ${CMAKE_INSTALL_PREFIX}/${CMAKE_INSTALL_LIBDIR})

src/CameraSensor.cc

@@ -51,6 +51,10 @@ using namespace sensors;
 /// \brief Private data for CameraSensor
 class ignition::sensors::CameraSensorPrivate
 {
+  /// \brief Callback for triggered subscription
+  /// \param[in] _msg Boolean message
+  public: void OnTrigger(const ignition::msgs::Boolean &_msg);
+
   /// \brief Save an image
   /// \param[in] _data the image data to be saved
   /// \param[in] _width width of image in pixels
@@ -98,6 +102,15 @@ class ignition::sensors::CameraSensorPrivate
   /// \brief Just a mutex for thread safety
   public: std::mutex mutex;
 
+  /// \brief True if camera is triggered by a topic
+  public: bool isTriggeredCamera = false;
+
+  /// \brief True if camera has been triggered by a topic
+  public: bool isTriggered = false;
+
+  /// \brief Topic for camera trigger
+  public: std::string triggerTopic = "";
+
   /// \brief True to save images
   public: bool saveImage = false;
 
@@ -281,6 +294,30 @@ bool CameraSensor::Load(const sdf::Sensor &_sdf)
   igndbg << "Camera images for [" << this->Name() << "] advertised on ["
          << this->Topic() << "]" << std::endl;
 
+  if (_sdf.CameraSensor()->Triggered())
+  {
+    if (!_sdf.CameraSensor()->TriggerTopic().empty())
+    {
+      this->dataPtr->triggerTopic = _sdf.CameraSensor()->TriggerTopic();
+    }
+    else
+    {
+      this->dataPtr->triggerTopic =
+          transport::TopicUtils::AsValidTopic(this->dataPtr->triggerTopic);
+
+      if (this->dataPtr->triggerTopic.empty())
+      {
+        ignerr << "Invalid trigger topic name" << std::endl;
+        return false;
+      }
+    }
+
+    this->dataPtr->node.Subscribe(this->dataPtr->triggerTopic,
+        &CameraSensorPrivate::OnTrigger, this->dataPtr.get());
+
+    this->dataPtr->isTriggeredCamera = true;
+  }
+
   if (!this->AdvertiseInfo())
     return false;
 
@@ -347,6 +384,12 @@ bool CameraSensor::Update(const std::chrono::steady_clock::duration &_now)
   this->dataPtr->camera->SetLocalPose(this->Pose());
 
   // render only if necessary
+  if (this->dataPtr->isTriggeredCamera &&
+      !this->dataPtr->isTriggered)
+  {
+    return true;
+  }
+
   if (!this->dataPtr->pub.HasConnections() &&
       this->dataPtr->imageEvent.ConnectionCount() <= 0 &&
       !this->dataPtr->saveImage)
@@ -443,9 +486,21 @@ bool CameraSensor::Update(const std::chrono::steady_clock::duration &_now)
     this->dataPtr->SaveImage(data, width, height, format);
   }
 
+  if (this->dataPtr->isTriggeredCamera)
+  {
+    return this->dataPtr->isTriggered = false;
+  }
+
   return true;
 }
 
+//////////////////////////////////////////////////
+void CameraSensorPrivate::OnTrigger(const ignition::msgs::Boolean &/*_msg*/)
+{
+  std::lock_guard<std::mutex> lock(this->mutex);
+  this->isTriggered = true;
+}
+
 //////////////////////////////////////////////////
 bool CameraSensorPrivate::SaveImage(const unsigned char *_data,
     unsigned int _width, unsigned int _height,

test/integration/CMakeLists.txt

@@ -6,8 +6,9 @@ set(dri_tests
   distortion_camera.cc
   gpu_lidar_sensor.cc
   rgbd_camera.cc
-  thermal_camera.cc
   segmentation_camera.cc
+  thermal_camera.cc
+  triggered_camera.cc
 )
 
 set(tests

test/integration/triggered_camera.cc

@@ -0,0 +1,176 @@
+/*
+ * Copyright (C) 2022 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 <gtest/gtest.h>
+
+#include <ignition/common/Console.hh>
+#include <ignition/common/Filesystem.hh>
+#include <ignition/sensors/CameraSensor.hh>
+#include <ignition/sensors/Manager.hh>
+
+// TODO(louise) Remove these pragmas once ign-rendering is disabling the
+// warnings
+#ifdef _WIN32
+#pragma warning(push)
+#pragma warning(disable: 4251)
+#endif
+#include <ignition/rendering/RenderEngine.hh>
+#include <ignition/rendering/RenderingIface.hh>
+#include <ignition/rendering/Scene.hh>
+#ifdef _WIN32
+#pragma warning(pop)
+#endif
+
+#ifdef _WIN32
+#pragma warning(push)
+#pragma warning(disable: 4005)
+#pragma warning(disable: 4251)
+#endif
+#include <ignition/msgs.hh>
+#ifdef _WIN32
+#pragma warning(pop)
+#endif
+
+#include "test_config.h"  // NOLINT(build/include)
+#include "TransportTestTools.hh"
+
+using namespace std::chrono_literals;
+
+class CameraSensorTest: public testing::Test,
+  public testing::WithParamInterface<const char *>
+{
+  // Documentation inherited
+  protected: void SetUp() override
+  {
+    ignition::common::Console::SetVerbosity(4);
+  }
+
+  // Create a Camera sensor from a SDF and gets a image message
+  public: void ImagesWithBuiltinSDF(const std::string &_renderEngine);
+};
+
+void CameraSensorTest::ImagesWithBuiltinSDF(const std::string &_renderEngine)
+{
+  std::string path = ignition::common::joinPaths(PROJECT_SOURCE_PATH, "test",
+      "sdf", "triggered_camera_sensor_builtin.sdf");
+  sdf::SDFPtr doc(new sdf::SDF());
+  sdf::init(doc);
+  ASSERT_TRUE(sdf::readFile(path, doc));
+  ASSERT_NE(nullptr, doc->Root());
+  ASSERT_TRUE(doc->Root()->HasElement("model"));
+  auto modelPtr = doc->Root()->GetElement("model");
+  ASSERT_TRUE(modelPtr->HasElement("link"));
+  auto linkPtr = modelPtr->GetElement("link");
+  ASSERT_TRUE(linkPtr->HasElement("sensor"));
+  auto sensorPtr = linkPtr->GetElement("sensor");
+
+  // If ogre is not the engine, don't run the test
+  if (_renderEngine.compare("ogre") != 0)
+  {
+    igndbg << "Engine '" << _renderEngine
+      << "' doesn't support segmentation cameras" << std::endl;
+    return;
+  }
+
+  // Setup ign-rendering with an empty scene
+  auto *engine = ignition::rendering::engine(_renderEngine);
+  if (!engine)
+  {
+    igndbg << "Engine '" << _renderEngine
+           << "' is not supported" << std::endl;
+    return;
+  }
+
+  ignition::rendering::ScenePtr scene = engine->CreateScene("scene");
+
+  ignition::sensors::Manager mgr;
+
+  ignition::sensors::CameraSensor *sensor =
+      mgr.CreateSensor<ignition::sensors::CameraSensor>(sensorPtr);
+  ASSERT_NE(sensor, nullptr);
+  sensor->SetScene(scene);
+
+  sdf::Sensor sdfSensor;
+  sdfSensor.Load(sensorPtr);
+  EXPECT_EQ(true, sdfSensor.CameraSensor()->Triggered());
+
+  ASSERT_NE(sensor->RenderingCamera(), nullptr);
+  EXPECT_NE(sensor->Id(), sensor->RenderingCamera()->Id());
+  EXPECT_EQ(256u, sensor->ImageWidth());
+  EXPECT_EQ(257u, sensor->ImageHeight());
+
+  // check camera image before trigger
+  {
+    std::string imageTopic =
+        "/test/integration/TriggeredCameraPlugin_imagesWithBuiltinSDF";
+    WaitForMessageTestHelper<ignition::msgs::Image> helper(imageTopic);
+    mgr.RunOnce(std::chrono::steady_clock::duration::zero(), true);
+    EXPECT_FALSE(helper.WaitForMessage(3s)) << helper;
+  }
+
+  // trigger camera through topic
+  ignition::transport::Node triggerNode;
+  std::string triggerTopic =
+      "/test/integration/TriggeredCameraPlugin_imagesWithBuiltinSDF/trigger";
+
+  auto pub = triggerNode.Advertise<ignition::msgs::Boolean>(triggerTopic);
+  ignition::msgs::Boolean msg;
+  msg.set_data(true);
+  pub.Publish(msg);
+
+  // check camera image after trigger
+  {
+    std::string imageTopic =
+        "/test/integration/TriggeredCameraPlugin_imagesWithBuiltinSDF";
+    WaitForMessageTestHelper<ignition::msgs::Image> helper(imageTopic);
+    mgr.RunOnce(std::chrono::steady_clock::duration::zero(), true);
+    EXPECT_TRUE(helper.WaitForMessage(3s)) << helper;
+  }
+
+  // test removing sensor
+  // first make sure the sensor objects do exist
+  auto sensorId = sensor->Id();
+  auto cameraId = sensor->RenderingCamera()->Id();
+  EXPECT_EQ(sensor, mgr.Sensor(sensorId));
+  EXPECT_EQ(sensor->RenderingCamera(), scene->SensorById(cameraId));
+  // remove and check sensor objects no longer exist in both sensors and
+  // rendering
+  EXPECT_TRUE(mgr.Remove(sensorId));
+  EXPECT_EQ(nullptr, mgr.Sensor(sensorId));
+  EXPECT_EQ(nullptr, scene->SensorById(cameraId));
+
+  // Clean up
+  engine->DestroyScene(scene);
+  ignition::rendering::unloadEngine(engine->Name());
+}
+
+//////////////////////////////////////////////////
+TEST_P(CameraSensorTest, ImagesWithBuiltinSDF)
+{
+  ImagesWithBuiltinSDF(GetParam());
+}
+
+INSTANTIATE_TEST_CASE_P(CameraSensor, CameraSensorTest,
+    RENDER_ENGINE_VALUES, ignition::rendering::PrintToStringParam());
+
+//////////////////////////////////////////////////
+int main(int argc, char **argv)
+{
+  ignition::common::Console::SetVerbosity(4);
+  ::testing::InitGoogleTest(&argc, argv);
+  return RUN_ALL_TESTS();
+}

test/sdf/triggered_camera_sensor_builtin.sdf

@@ -0,0 +1,23 @@
+<?xml version="1.0"?>
+<sdf version="1.6">
+  <model name="m1">
+    <link name="link1">
+      <sensor name="camera1" type="camera">
+        <update_rate>10</update_rate>
+        <topic>/test/integration/TriggeredCameraPlugin_imagesWithBuiltinSDF</topic>
+        <camera>
+          <triggered>true</triggered>
+          <horizontal_fov>1.05</horizontal_fov>
+          <image>
+            <width>256</width>
+            <height>257</height>
+          </image>
+          <clip>
+            <near>0.1</near>
+            <far>10.0</far>
+          </clip>
+        </camera>
+      </sensor>
+    </link>
+  </model>
+</sdf>