Support wide angle camera

include/sdf/Sensor.hh

@@ -125,7 +125,10 @@ namespace sdf
     BOUNDINGBOX_CAMERA = 23,
 
     /// \brief A custom sensor
-    CUSTOM = 24
+    CUSTOM = 24,
+
+    /// \brief A wide angle camera sensor
+    WIDE_ANGLE_CAMERA = 25
   };
 
   /// \brief Information about an SDF sensor.

src/Sensor.cc

@@ -64,7 +64,8 @@ const std::vector<std::string> sensorTypeStrs =
   "navsat",
   "segmentation_camera",
   "boundingbox_camera",
-  "custom"
+  "custom",
+  "wide_angle_camera"
 };
 
 class sdf::Sensor::Implementation
@@ -169,6 +170,7 @@ bool Sensor::operator==(const Sensor &_sensor) const
     case SensorType::DEPTH_CAMERA:
     case SensorType::RGBD_CAMERA:
     case SensorType::THERMAL_CAMERA:
+    case SensorType::WIDE_ANGLE_CAMERA:
     case SensorType::SEGMENTATION_CAMERA:
     case SensorType::BOUNDINGBOX_CAMERA:
       return *(this->dataPtr->camera) == *(_sensor.dataPtr->camera);
@@ -302,6 +304,13 @@ Errors Sensor::Load(ElementPtr _sdf)
     Errors err = this->dataPtr->camera->Load(_sdf->GetElement("camera"));
     errors.insert(errors.end(), err.begin(), err.end());
   }
+  else if (type == "wideanglecamera" || type == "wide_angle_camera")
+  {
+    this->dataPtr->type = SensorType::WIDE_ANGLE_CAMERA;
+    this->dataPtr->camera.emplace();
+    Errors err = this->dataPtr->camera->Load(_sdf->GetElement("camera"));
+    errors.insert(errors.end(), err.begin(), err.end());
+  }
   else if (type == "force_torque")
   {
     this->dataPtr->type = SensorType::FORCE_TORQUE;

src/Sensor_TEST.cc

@@ -263,7 +263,8 @@ TEST(DOMSensor, Type)
     sdf::SensorType::WIRELESS_RECEIVER,
     sdf::SensorType::WIRELESS_TRANSMITTER,
     sdf::SensorType::THERMAL_CAMERA,
-    sdf::SensorType::CUSTOM
+    sdf::SensorType::CUSTOM,
+    sdf::SensorType::WIDE_ANGLE_CAMERA
   };
   std::vector<std::string> typeStrs =
   {
@@ -288,7 +289,8 @@ TEST(DOMSensor, Type)
     "wireless_receiver",
     "wireless_transmitter",
     "thermal_camera",
-    "custom"
+    "custom",
+    "wide_angle_camera"
   };
 
   for (size_t i = 0; i < types.size(); ++i)

test/integration/link_dom.cc

@@ -243,7 +243,7 @@ TEST(DOMLink, Sensors)
   const sdf::Link *link = model->LinkByIndex(0);
   ASSERT_NE(nullptr, link);
   EXPECT_EQ("link", link->Name());
-  EXPECT_EQ(25u, link->SensorCount());
+  EXPECT_EQ(26u, link->SensorCount());
 
   // Get the altimeter sensor
   const sdf::Sensor *altimeterSensor = link->SensorByIndex(0);
@@ -647,6 +647,37 @@ TEST(DOMLink, Sensors)
   EXPECT_DOUBLE_EQ(3.4, airSensor->PressureNoise().Mean());
   EXPECT_DOUBLE_EQ(5.6, airSensor->PressureNoise().StdDev());
   EXPECT_DOUBLE_EQ(123.4, airSensor->ReferenceAltitude());
+
+  // Get the wide angle camera sensor
+  EXPECT_TRUE(link->SensorNameExists("wide_angle_camera_sensor"));
+  const sdf::Sensor *wideAngleCameraSensor =
+      link->SensorByName("wide_angle_camera_sensor");
+  ASSERT_NE(nullptr, wideAngleCameraSensor);
+  EXPECT_EQ("wide_angle_camera_sensor", wideAngleCameraSensor->Name());
+  EXPECT_EQ(sdf::SensorType::WIDE_ANGLE_CAMERA, wideAngleCameraSensor->Type());
+  EXPECT_EQ(ignition::math::Pose3d(20, 30, 40, 0, 0, 0),
+      wideAngleCameraSensor->RawPose());
+  EXPECT_FALSE(wideAngleCameraSensor->EnableMetrics());
+  const sdf::Camera *wideAngleCam = wideAngleCameraSensor->CameraSensor();
+  ASSERT_NE(nullptr, wideAngleCam);
+  EXPECT_EQ("wide_angle_cam", wideAngleCam->Name());
+  EXPECT_EQ(ignition::math::Pose3d(0.2, 0.3, 0.4, 0, 0, 0),
+            wideAngleCam->RawPose());
+  EXPECT_DOUBLE_EQ(3.14, wideAngleCam->HorizontalFov().Radian());
+  EXPECT_EQ(320u, wideAngleCam->ImageWidth());
+  EXPECT_EQ(240u, wideAngleCam->ImageHeight());
+  EXPECT_EQ(sdf::PixelFormatType::RGB_INT8, wideAngleCam->PixelFormat());
+  EXPECT_DOUBLE_EQ(0.1, wideAngleCam->NearClip());
+  EXPECT_DOUBLE_EQ(100, wideAngleCam->FarClip());
+  EXPECT_EQ("custom", wideAngleCam->LensType());
+  EXPECT_TRUE(wideAngleCam->LensScaleToHfov());
+  EXPECT_DOUBLE_EQ(1.05, wideAngleCam->LensC1());
+  EXPECT_DOUBLE_EQ(4.0, wideAngleCam->LensC2());
+  EXPECT_DOUBLE_EQ(0.0, wideAngleCam->LensC3());
+  EXPECT_DOUBLE_EQ(1.0, wideAngleCam->LensFocalLength());
+  EXPECT_EQ("tan", wideAngleCam->LensFunction());
+  EXPECT_DOUBLE_EQ(3.1415, wideAngleCam->LensCutoffAngle().Radian());
+  EXPECT_EQ(512, wideAngleCam->LensEnvironmentTextureSize());
 }
 
 /////////////////////////////////////////////////

test/sdf/sensors.sdf

@@ -633,6 +633,37 @@
           </pressure>
         </air_pressure>
       </sensor>
+
+      <sensor name="wide_angle_camera_sensor" type="wideanglecamera">
+        <pose>20 30 40 0 0 0</pose>
+        <camera name="wide_angle_cam">
+          <pose>0.2 0.3 0.4 0 0 0</pose>
+          <horizontal_fov>3.14</horizontal_fov>
+          <image>
+            <width>320</width>
+            <height>240</height>
+          </image>
+          <clip>
+            <near>0.1</near>
+            <far>100</far>
+          </clip>
+          <lens>
+            <type>custom</type>
+            <custom_function>
+              <c1>1.05</c1>
+              <c2>4</c2>
+              <f>1.0</f>
+              <fun>tan</fun>
+            </custom_function>
+            <scale_to_hfov>true</scale_to_hfov>
+            <cutoff_angle>3.1415</cutoff_angle>
+            <env_texture_size>512</env_texture_size>
+          </lens>
+        </camera>
+        <always_on>1</always_on>
+        <update_rate>30</update_rate>
+        <topic>wideanglecamera</topic>
+      </sensor>
     </link>
   </model>
 </sdf>