diff --git a/include/sdf/Sensor.hh b/include/sdf/Sensor.hh index bbde81403..5f6860a8d 100644 --- a/include/sdf/Sensor.hh +++ b/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. diff --git a/src/Sensor.cc b/src/Sensor.cc index 7943c0655..7e6adbb8a 100644 --- a/src/Sensor.cc +++ b/src/Sensor.cc @@ -64,7 +64,8 @@ const std::vector 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; diff --git a/src/Sensor_TEST.cc b/src/Sensor_TEST.cc index 28ef3cb77..6881022c9 100644 --- a/src/Sensor_TEST.cc +++ b/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 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) diff --git a/test/integration/link_dom.cc b/test/integration/link_dom.cc index 1c98a1ba2..396c1cd11 100644 --- a/test/integration/link_dom.cc +++ b/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()); } ///////////////////////////////////////////////// diff --git a/test/sdf/sensors.sdf b/test/sdf/sensors.sdf index c4e6c6744..1c67f2b72 100644 --- a/test/sdf/sensors.sdf +++ b/test/sdf/sensors.sdf @@ -633,6 +633,37 @@ + + + 20 30 40 0 0 0 + + 0.2 0.3 0.4 0 0 0 + 3.14 + + 320 + 240 + + + 0.1 + 100 + + + custom + + 1.05 + 4 + 1.0 + tan + + true + 3.1415 + 512 + + + 1 + 30 + wideanglecamera +