diff --git a/.github/ci/after_make.sh b/.github/ci/after_make.sh new file mode 100644 index 00000000..ed11bdbc --- /dev/null +++ b/.github/ci/after_make.sh @@ -0,0 +1,12 @@ +#!/bin/sh -l + +set -x +set -e + +# Install (needed for some tests) +make install + +Xvfb :1 -screen 0 1280x1024x24 & +export DISPLAY=:1.0 +export RENDER_ENGINE_VALUES=ogre2 +export MESA_GL_VERSION_OVERRIDE=3.3 diff --git a/.github/ci/packages.apt b/.github/ci/packages.apt new file mode 100644 index 00000000..b72f3427 --- /dev/null +++ b/.github/ci/packages.apt @@ -0,0 +1,10 @@ +libignition-cmake2-dev +libignition-common3-dev +libignition-math6-dev +libignition-msgs5-dev +libignition-plugin-dev +libignition-rendering3-dev +libignition-tools-dev +libignition-transport8-dev +libsdformat9-dev +xvfb diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml new file mode 100644 index 00000000..80010192 --- /dev/null +++ b/.github/workflows/ci.yml @@ -0,0 +1,25 @@ +name: Ubuntu CI + +on: [push] + +jobs: + bionic-ci: + runs-on: ubuntu-latest + name: Ubuntu Bionic CI + steps: + - name: Checkout + uses: actions/checkout@v2 + - name: Compile and test + id: ci + uses: ignition-tooling/action-ignition-ci@master + with: + codecov-token: ${{ secrets.CODECOV_TOKEN }} + focal-ci: + runs-on: ubuntu-latest + name: Ubuntu Focal CI + steps: + - name: Checkout + uses: actions/checkout@v2 + - name: Compile and test + id: ci + uses: ignition-tooling/action-ignition-ci@focal diff --git a/Changelog.md b/Changelog.md index 93dd9d77..9c08e546 100644 --- a/Changelog.md +++ b/Changelog.md @@ -29,6 +29,17 @@ ## Ignition Sensors 2 +### Ignition Sensors 2.9.0 (2020-08-07) + +1. Add noise to RGBD camera. + * [Pull Request 35](https://github.com/ignitionrobotics/ign-sensors/pull/35) + +1. Make sure all sensors have a default topic.When invalid topics are passed + in, convert them to valid topics if possible. If not possible to convert + into valid topic, fail gracefully. + * [Pull Request 33](https://github.com/ignitionrobotics/ign-sensors/pull/33) + + ### Ignition Sensors 2.8.0 (2020-03-04) 1. Added sequence numbers to sensor data messages. diff --git a/include/ignition/sensors/Sensor.hh b/include/ignition/sensors/Sensor.hh index 9cfb46f2..99e8271c 100644 --- a/include/ignition/sensors/Sensor.hh +++ b/include/ignition/sensors/Sensor.hh @@ -132,6 +132,11 @@ namespace ignition /// \return Topic sensor publishes data to public: std::string Topic() const; + /// \brief Set topic where sensor data is published. + /// \param[in] _topic Topic sensor publishes data to. + /// \return True if a valid topic was set. + public: bool SetTopic(const std::string &_topic); + /// \brief Get parent link of the sensor. /// \return Parent link of sensor. public: std::string Parent() const; diff --git a/include/ignition/sensors/config.hh.in b/include/ignition/sensors/config.hh.in index 14b69ffb..fee36ae3 100644 --- a/include/ignition/sensors/config.hh.in +++ b/include/ignition/sensors/config.hh.in @@ -1,9 +1,9 @@ /* Config.hh. Generated by CMake for @PROJECT_NAME_NO_VERSION@. */ /* Version number */ -#define IGNITION_SENSORS_MAJOR_VERSION ${PROJECT_MAJOR_VERSION} -#define IGNITION_SENSORS_MINOR_VERSION ${PROJECT_MINOR_VERSION} -#define IGNITION_SENSORS_PATCH_VERSION ${PROJECT_PATCH_VERSION} +#define IGNITION_SENSORS_MAJOR_VERSION ${PROJECT_VERSION_MAJOR} +#define IGNITION_SENSORS_MINOR_VERSION ${PROJECT_VERSION_MINOR} +#define IGNITION_SENSORS_PATCH_VERSION ${PROJECT_VERSION_PATCH} #define IGNITION_SENSORS_VERSION "${PROJECT_VERSION}" #define IGNITION_SENSORS_VERSION_FULL "${PROJECT_VERSION_FULL}" diff --git a/src/AirPressureSensor.cc b/src/AirPressureSensor.cc index d9cff8f8..53c64824 100644 --- a/src/AirPressureSensor.cc +++ b/src/AirPressureSensor.cc @@ -100,16 +100,16 @@ bool AirPressureSensor::Load(const sdf::Sensor &_sdf) return false; } - std::string topic = this->Topic(); - if (topic.empty()) - topic = "/air_pressure"; + if (this->Topic().empty()) + this->SetTopic("/air_pressure"); this->dataPtr->pub = - this->dataPtr->node.Advertise(topic); + this->dataPtr->node.Advertise( + this->Topic()); if (!this->dataPtr->pub) { - ignerr << "Unable to create publisher on topic[" << topic << "].\n"; + ignerr << "Unable to create publisher on topic[" << this->Topic() << "].\n"; return false; } diff --git a/src/AltimeterSensor.cc b/src/AltimeterSensor.cc index 6b0f15bf..f860b289 100644 --- a/src/AltimeterSensor.cc +++ b/src/AltimeterSensor.cc @@ -88,16 +88,15 @@ bool AltimeterSensor::Load(const sdf::Sensor &_sdf) return false; } - std::string topic = this->Topic(); - if (topic.empty()) - topic = "/altimeter"; + if (this->Topic().empty()) + this->SetTopic("/altimeter"); this->dataPtr->pub = - this->dataPtr->node.Advertise(topic); + this->dataPtr->node.Advertise(this->Topic()); if (!this->dataPtr->pub) { - ignerr << "Unable to create publisher on topic[" << topic << "].\n"; + ignerr << "Unable to create publisher on topic[" << this->Topic() << "].\n"; return false; } diff --git a/src/CameraSensor.cc b/src/CameraSensor.cc index c31d63af..f10e8c19 100644 --- a/src/CameraSensor.cc +++ b/src/CameraSensor.cc @@ -247,6 +247,9 @@ bool CameraSensor::Load(const sdf::Sensor &_sdf) this->dataPtr->sdfSensor = _sdf; + if (this->Topic().empty()) + this->SetTopic("/camera"); + this->dataPtr->pub = this->dataPtr->node.Advertise( this->Topic()); diff --git a/src/Camera_TEST.cc b/src/Camera_TEST.cc index 7c056415..d59fbb4b 100644 --- a/src/Camera_TEST.cc +++ b/src/Camera_TEST.cc @@ -29,7 +29,7 @@ sdf::ElementPtr cameraToBadSdf() << "" << " " << " " - << " " + << " " << " " << " " << " " @@ -46,7 +46,7 @@ sdf::ElementPtr cameraToBadSdf() ->GetElement("sensor"); } -sdf::ElementPtr cameraToSdf(const std::string &_type, +sdf::ElementPtr CameraToSdf(const std::string &_type, const std::string &_name, double _updateRate, const std::string &_topic, bool _alwaysOn, bool _visualize) { @@ -128,7 +128,7 @@ TEST(Camera_TEST, CreateCamera) { ignition::sensors::Manager mgr; - sdf::ElementPtr camSdf = cameraToSdf("camera", "my_camera", 60.0, "/cam", + sdf::ElementPtr camSdf = CameraToSdf("camera", "my_camera", 60.0, "/cam", true, true); // Create a CameraSensor @@ -157,6 +157,65 @@ TEST(Camera_TEST, CreateCamera) EXPECT_TRUE(badCam == nullptr); } +///////////////////////////////////////////////// +TEST(Camera_TEST, Topic) +{ + const std::string type = "camera"; + const std::string name = "TestCamera"; + const double updateRate = 30; + const bool alwaysOn = 1; + const bool visualize = 1; + + // Factory + ignition::sensors::Manager mgr; + + // Default topic + { + const std::string topic; + auto cameraSdf = CameraToSdf(type, name, updateRate, topic, alwaysOn, + visualize); + + auto sensorId = mgr.CreateSensor(cameraSdf); + EXPECT_NE(ignition::sensors::NO_SENSOR, sensorId); + + auto sensor = mgr.Sensor(sensorId); + EXPECT_NE(nullptr, sensor); + + auto camera = dynamic_cast(sensor); + ASSERT_NE(nullptr, camera); + + EXPECT_EQ("/camera", camera->Topic()); + } + + // Convert to valid topic + { + const std::string topic = "/topic with spaces/@~characters//"; + auto cameraSdf = CameraToSdf(type, name, updateRate, topic, alwaysOn, + visualize); + + auto sensorId = mgr.CreateSensor(cameraSdf); + EXPECT_NE(ignition::sensors::NO_SENSOR, sensorId); + + auto sensor = mgr.Sensor(sensorId); + EXPECT_NE(nullptr, sensor); + + auto camera = dynamic_cast(sensor); + ASSERT_NE(nullptr, camera); + + EXPECT_EQ("/topic_with_spaces/characters", camera->Topic()); + } + + // Invalid topic + { + const std::string topic = "@@@"; + auto cameraSdf = CameraToSdf(type, name, updateRate, topic, alwaysOn, + visualize); + + auto sensorId = mgr.CreateSensor(cameraSdf); + EXPECT_EQ(ignition::sensors::NO_SENSOR, sensorId); + } +} + ////////////////////////////////////////////////// int main(int argc, char **argv) { diff --git a/src/DepthCameraSensor.cc b/src/DepthCameraSensor.cc index da762ac3..ffee9943 100644 --- a/src/DepthCameraSensor.cc +++ b/src/DepthCameraSensor.cc @@ -264,6 +264,9 @@ bool DepthCameraSensor::Load(const sdf::Sensor &_sdf) this->dataPtr->sdfSensor = _sdf; + if (this->Topic().empty()) + this->SetTopic("/camera/depth"); + this->dataPtr->pub = this->dataPtr->node.Advertise( this->Topic()); diff --git a/src/GpuLidarSensor.cc b/src/GpuLidarSensor.cc index 829642ab..2cbc059a 100644 --- a/src/GpuLidarSensor.cc +++ b/src/GpuLidarSensor.cc @@ -122,14 +122,16 @@ bool GpuLidarSensor::Load(const sdf::Sensor &_sdf) std::bind(&GpuLidarSensor::SetScene, this, std::placeholders::_1)); // Create the point cloud publisher + this->SetTopic(this->Topic() + "/points"); + this->dataPtr->pointPub = this->dataPtr->node.Advertise( - this->Topic() + "/points"); + this->Topic()); if (!this->dataPtr->pointPub) { ignerr << "Unable to create publisher on topic[" - << this->Topic() + "/points" << "].\n"; + << this->Topic() << "].\n"; return false; } diff --git a/src/ImageNoise.cc b/src/ImageNoise.cc index f3c5e8a8..4ae51480 100644 --- a/src/ImageNoise.cc +++ b/src/ImageNoise.cc @@ -44,7 +44,7 @@ NoisePtr ImageNoiseFactory::NewNoiseModel(const sdf::Noise &_sdf, { if (_sensorType == "camera" || _sensorType == "depth" || _sensorType == "multicamera" || _sensorType == "wideanglecamera" || - _sensorType == "thermal_camera") + _sensorType == "thermal_camera" || _sensorType == "rgbd_camera") { noise.reset(new ImageGaussianNoiseModel()); } diff --git a/src/ImuSensor.cc b/src/ImuSensor.cc index e80513af..ad617027 100644 --- a/src/ImuSensor.cc +++ b/src/ImuSensor.cc @@ -104,16 +104,15 @@ bool ImuSensor::Load(const sdf::Sensor &_sdf) return false; } - std::string topic = this->Topic(); - if (topic.empty()) - topic = "/imu"; + if (this->Topic().empty()) + this->SetTopic("/imu"); this->dataPtr->pub = - this->dataPtr->node.Advertise(topic); + this->dataPtr->node.Advertise(this->Topic()); if (!this->dataPtr->pub) { - ignerr << "Unable to create publisher on topic[" << topic << "].\n"; + ignerr << "Unable to create publisher on topic[" << this->Topic() << "].\n"; return false; } diff --git a/src/Lidar.cc b/src/Lidar.cc index 2a69d755..d74040c0 100644 --- a/src/Lidar.cc +++ b/src/Lidar.cc @@ -106,6 +106,9 @@ bool Lidar::Load(const sdf::Sensor &_sdf) } // Register publisher + if (this->Topic().empty()) + this->SetTopic("/lidar"); + this->dataPtr->pub = this->dataPtr->node.Advertise( this->Topic()); diff --git a/src/LogicalCameraSensor.cc b/src/LogicalCameraSensor.cc index a37774be..82579baf 100644 --- a/src/LogicalCameraSensor.cc +++ b/src/LogicalCameraSensor.cc @@ -104,16 +104,16 @@ bool LogicalCameraSensor::Load(sdf::ElementPtr _sdf) if (!Sensor::Load(_sdf)) return false; - std::string topic = this->Topic(); - if (topic.empty()) - topic = "/logical_camera"; + if (this->Topic().empty()) + this->SetTopic("/logical_camera"); this->dataPtr->pub = - this->dataPtr->node.Advertise(topic); + this->dataPtr->node.Advertise( + this->Topic()); if (!this->dataPtr->pub) { - ignerr << "Unable to create publisher on topic[" << topic << "].\n"; + ignerr << "Unable to create publisher on topic[" << this->Topic() << "].\n"; return false; } diff --git a/src/MagnetometerSensor.cc b/src/MagnetometerSensor.cc index 793eadb1..87698227 100644 --- a/src/MagnetometerSensor.cc +++ b/src/MagnetometerSensor.cc @@ -92,16 +92,16 @@ bool MagnetometerSensor::Load(const sdf::Sensor &_sdf) return false; } - std::string topic = this->Topic(); - if (topic.empty()) - topic = "/magnetometer"; + if (this->Topic().empty()) + this->SetTopic("/magnetometer"); this->dataPtr->pub = - this->dataPtr->node.Advertise(topic); + this->dataPtr->node.Advertise( + this->Topic()); if (!this->dataPtr->pub) { - ignerr << "Unable to create publisher on topic[" << topic << "].\n"; + ignerr << "Unable to create publisher on topic[" << this->Topic() << "].\n"; return false; } diff --git a/src/Noise.cc b/src/Noise.cc index ef59d2d1..10f589ba 100644 --- a/src/Noise.cc +++ b/src/Noise.cc @@ -57,7 +57,7 @@ NoisePtr NoiseFactory::NewNoiseModel(const sdf::Noise &_sdf, { if (_sensorType == "camera" || _sensorType == "depth" || _sensorType == "multicamera" || _sensorType == "wideanglecamera" || - _sensorType == "thermal_camera") + _sensorType == "thermal_camera" || _sensorType == "rgbd_camera") { ignerr << "Image noise requested. " << "Please use ImageNoiseFactory::NoiseModel instead" diff --git a/src/RgbdCameraSensor.cc b/src/RgbdCameraSensor.cc index b60e91dc..eafeaed6 100644 --- a/src/RgbdCameraSensor.cc +++ b/src/RgbdCameraSensor.cc @@ -28,6 +28,8 @@ #include +#include "ignition/sensors/ImageGaussianNoiseModel.hh" +#include "ignition/sensors/ImageNoise.hh" #include "ignition/sensors/RgbdCameraSensor.hh" #include "ignition/sensors/RenderingEvents.hh" #include "ignition/sensors/SensorFactory.hh" @@ -102,6 +104,9 @@ class ignition::sensors::RgbdCameraSensorPrivate /// \brief Pointer to an image to be published public: ignition::rendering::Image image; + /// \brief Noise added to sensor data + public: std::map noises; + /// \brief Connection from depth camera with new depth data public: ignition::common::ConnectionPtr depthConnection; @@ -277,6 +282,30 @@ bool RgbdCameraSensor::CreateCameras() this->AddSensor(this->dataPtr->depthCamera); + const std::map noises = { + {CAMERA_NOISE, cameraSdf->ImageNoise()}, + }; + + for (const auto & [noiseType, noiseSdf] : noises) + { + // Add gaussian noise to camera sensor + if (noiseSdf.Type() == sdf::NoiseType::GAUSSIAN) + { + this->dataPtr->noises[noiseType] = + ImageNoiseFactory::NewNoiseModel(noiseSdf, "rgbd_camera"); + + std::dynamic_pointer_cast( + this->dataPtr->noises[noiseType])->SetCamera( + this->dataPtr->depthCamera); + } + else if (noiseSdf.Type() != sdf::NoiseType::NONE) + { + ignwarn << "The depth camera sensor only supports Gaussian noise. " + << "The supplied noise type[" << static_cast(noiseSdf.Type()) + << "] is not supported." << std::endl; + } + } + // \todo(nkoeng) these parameters via sdf this->dataPtr->depthCamera->SetAntiAliasing(2); diff --git a/src/Sensor.cc b/src/Sensor.cc index f52ed940..6a09277b 100644 --- a/src/Sensor.cc +++ b/src/Sensor.cc @@ -21,6 +21,7 @@ #include #include #include +#include using namespace ignition::sensors; @@ -30,6 +31,11 @@ class ignition::sensors::SensorPrivate /// \brief Populates fields from a DOM public: bool PopulateFromSDF(const sdf::Sensor &_sdf); + /// \brief Set topic where sensor data is published. + /// \param[in] _topic Topic sensor publishes data to. + /// \return True if a valid topic was set. + public: bool SetTopic(const std::string &_topic); + /// \brief id given to sensor when constructed public: SensorId id; @@ -88,7 +94,11 @@ bool SensorPrivate::PopulateFromSDF(const sdf::Sensor &_sdf) // \todo(nkoenig) how to use frame? this->name = _sdf.Name(); - this->topic = _sdf.Topic(); + if (!_sdf.Topic().empty()) + { + if (!this->SetTopic(_sdf.Topic())) + return false; + } // Try resolving the pose first, and only use the raw pose if that fails auto semPose = _sdf.SemanticPose(); @@ -165,6 +175,26 @@ std::string Sensor::Topic() const return this->dataPtr->topic; } +////////////////////////////////////////////////// +bool Sensor::SetTopic(const std::string &_topic) +{ + return this->dataPtr->SetTopic(_topic); +} + +////////////////////////////////////////////////// +bool SensorPrivate::SetTopic(const std::string &_topic) +{ + auto validTopic = transport::TopicUtils::AsValidTopic(_topic); + if (validTopic.empty()) + { + ignerr << "Failed to set sensor topic [" << _topic << "]" << std::endl; + return false; + } + + this->topic = validTopic; + return true; +} + ////////////////////////////////////////////////// ignition::math::Pose3d Sensor::Pose() const { diff --git a/src/Sensor_TEST.cc b/src/Sensor_TEST.cc index a608962d..8acd0db0 100644 --- a/src/Sensor_TEST.cc +++ b/src/Sensor_TEST.cc @@ -63,6 +63,7 @@ TEST(Sensor_TEST, Sensor) EXPECT_EQ(nullptr, sensor.SDF()); } +////////////////////////////////////////////////// TEST(Sensor_TEST, AddSequence) { TestSensor sensor; @@ -97,6 +98,21 @@ TEST(Sensor_TEST, AddSequence) EXPECT_EQ("0", header2.data(0).value(0)); } +////////////////////////////////////////////////// +TEST(Sensor_TEST, Topic) +{ + TestSensor sensor; + EXPECT_EQ("", sensor.Topic()); + + EXPECT_TRUE(sensor.SetTopic("/topic")); + EXPECT_EQ("/topic", sensor.Topic()); + + EXPECT_TRUE(sensor.SetTopic("/topic with spaces/@~characters//")); + EXPECT_EQ("/topic_with_spaces/characters", sensor.Topic()); + + EXPECT_FALSE(sensor.SetTopic("")); +} + ////////////////////////////////////////////////// int main(int argc, char **argv) { diff --git a/test/integration/air_pressure_plugin.cc b/test/integration/air_pressure_plugin.cc index dbf2b0a9..d42c1a43 100644 --- a/test/integration/air_pressure_plugin.cc +++ b/test/integration/air_pressure_plugin.cc @@ -222,6 +222,63 @@ TEST_F(AirPressureSensorTest, SensorReadings) EXPECT_DOUBLE_EQ(sqrt(0.2), msgNoise.variance()); } +///////////////////////////////////////////////// +TEST_F(AirPressureSensorTest, Topic) +{ + const std::string name = "TestAirPressure"; + const double updateRate = 30; + const bool alwaysOn = 1; + const bool visualize = 1; + auto sensorPose = ignition::math::Pose3d(); + + // Factory + ignition::sensors::SensorFactory factory; + factory.AddPluginPaths(ignition::common::joinPaths(PROJECT_BUILD_PATH, + "lib")); + + // Default topic + { + const std::string topic; + auto airPressureSdf = AirPressureToSdf(name, sensorPose, + updateRate, topic, alwaysOn, visualize); + + auto sensor = factory.CreateSensor(airPressureSdf); + EXPECT_NE(nullptr, sensor); + + auto airPressure = + dynamic_cast(sensor.release()); + ASSERT_NE(nullptr, airPressure); + + EXPECT_EQ("/air_pressure", airPressure->Topic()); + } + + // Convert to valid topic + { + const std::string topic = "/topic with spaces/@~characters//"; + auto airPressureSdf = AirPressureToSdf(name, sensorPose, + updateRate, topic, alwaysOn, visualize); + + auto sensor = factory.CreateSensor(airPressureSdf); + EXPECT_NE(nullptr, sensor); + + auto airPressure = + dynamic_cast(sensor.release()); + ASSERT_NE(nullptr, airPressure); + + EXPECT_EQ("/topic_with_spaces/characters", airPressure->Topic()); + } + + // Invalid topic + { + const std::string topic = "@@@"; + auto airPressureSdf = AirPressureToSdf(name, sensorPose, + updateRate, topic, alwaysOn, visualize); + + auto sensor = factory.CreateSensor(airPressureSdf); + ASSERT_EQ(nullptr, sensor); + } +} + int main(int argc, char **argv) { ::testing::InitGoogleTest(&argc, argv); diff --git a/test/integration/altimeter_plugin.cc b/test/integration/altimeter_plugin.cc index e52dc9e0..e0788604 100644 --- a/test/integration/altimeter_plugin.cc +++ b/test/integration/altimeter_plugin.cc @@ -27,7 +27,7 @@ #include "TransportTestTools.hh" /// \brief Helper function to create an altimeter sdf element -sdf::ElementPtr AltimeterToSDF(const std::string &_name, +sdf::ElementPtr AltimeterToSdf(const std::string &_name, const ignition::math::Pose3d &_pose, const double _updateRate, const std::string &_topic, const bool _alwaysOn, const bool _visualize) @@ -124,7 +124,7 @@ TEST_F(AltimeterSensorTest, CreateAltimeter) // Create sensor SDF ignition::math::Pose3d sensorPose(ignition::math::Vector3d(0.25, 0.0, 0.5), ignition::math::Quaterniond::Identity); - sdf::ElementPtr altimeterSDF = AltimeterToSDF(name, sensorPose, + sdf::ElementPtr altimeterSdf = AltimeterToSdf(name, sensorPose, updateRate, topic, alwaysOn, visualize); sdf::ElementPtr altimeterSdfNoise = AltimeterToSdfWithNoise(name, sensorPose, @@ -134,7 +134,7 @@ TEST_F(AltimeterSensorTest, CreateAltimeter) ignition::sensors::SensorFactory sf; sf.AddPluginPaths(ignition::common::joinPaths(PROJECT_BUILD_PATH, "lib")); std::unique_ptr sensor = - sf.CreateSensor(altimeterSDF); + sf.CreateSensor(altimeterSdf); EXPECT_TRUE(sensor != nullptr); EXPECT_EQ(name, sensor->Name()); @@ -164,7 +164,7 @@ TEST_F(AltimeterSensorTest, SensorReadings) // Create sensor SDF ignition::math::Pose3d sensorPose(ignition::math::Vector3d(0.25, 0.0, 0.5), ignition::math::Quaterniond::Identity); - sdf::ElementPtr altimeterSDF = AltimeterToSDF(name, sensorPose, + sdf::ElementPtr altimeterSdf = AltimeterToSdf(name, sensorPose, updateRate, topic, alwaysOn, visualize); sdf::ElementPtr altimeterSdfNoise = AltimeterToSdfWithNoise(name, sensorPose, @@ -175,7 +175,7 @@ TEST_F(AltimeterSensorTest, SensorReadings) ignition::sensors::SensorFactory sf; sf.AddPluginPaths(ignition::common::joinPaths(PROJECT_BUILD_PATH, "lib")); std::unique_ptr s = - sf.CreateSensor(altimeterSDF); + sf.CreateSensor(altimeterSdf); std::unique_ptr sensor( dynamic_cast(s.release())); @@ -250,6 +250,63 @@ TEST_F(AltimeterSensorTest, SensorReadings) EXPECT_FALSE(ignition::math::equal(vertVel, msgNoise.vertical_velocity())); } +///////////////////////////////////////////////// +TEST_F(AltimeterSensorTest, Topic) +{ + const std::string name = "TestAltimeter"; + const double updateRate = 30; + const bool alwaysOn = 1; + const bool visualize = 1; + auto sensorPose = ignition::math::Pose3d(); + + // Factory + ignition::sensors::SensorFactory factory; + factory.AddPluginPaths(ignition::common::joinPaths(PROJECT_BUILD_PATH, + "lib")); + + // Default topic + { + const std::string topic; + auto altimeterSdf = AltimeterToSdf(name, sensorPose, + updateRate, topic, alwaysOn, visualize); + + auto sensor = factory.CreateSensor(altimeterSdf); + EXPECT_NE(nullptr, sensor); + + auto altimeter = + dynamic_cast(sensor.release()); + ASSERT_NE(nullptr, altimeter); + + EXPECT_EQ("/altimeter", altimeter->Topic()); + } + + // Convert to valid topic + { + const std::string topic = "/topic with spaces/@~characters//"; + auto altimeterSdf = AltimeterToSdf(name, sensorPose, + updateRate, topic, alwaysOn, visualize); + + auto sensor = factory.CreateSensor(altimeterSdf); + EXPECT_NE(nullptr, sensor); + + auto altimeter = + dynamic_cast(sensor.release()); + ASSERT_NE(nullptr, altimeter); + + EXPECT_EQ("/topic_with_spaces/characters", altimeter->Topic()); + } + + // Invalid topic + { + const std::string topic = "@@@"; + auto altimeterSdf = AltimeterToSdf(name, sensorPose, + updateRate, topic, alwaysOn, visualize); + + auto sensor = factory.CreateSensor(altimeterSdf); + ASSERT_EQ(nullptr, sensor); + } +} + int main(int argc, char **argv) { ::testing::InitGoogleTest(&argc, argv); diff --git a/test/integration/depth_camera_plugin.cc b/test/integration/depth_camera_plugin.cc index 2c046277..fded2bcb 100644 --- a/test/integration/depth_camera_plugin.cc +++ b/test/integration/depth_camera_plugin.cc @@ -432,7 +432,7 @@ void DepthCameraSensorTest::ImagesWithBuiltinSDF( for (unsigned int j = 0; j < depthSensor->ImageWidth(); ++j) { float d = g_depthBuffer[step + j]; - EXPECT_FLOAT_EQ(expectedDepth, d); + EXPECT_NEAR(expectedDepth, d, DOUBLE_TOL); } } } @@ -445,7 +445,7 @@ void DepthCameraSensorTest::ImagesWithBuiltinSDF( for (unsigned int j = 0; j < depthSensor->ImageWidth(); ++j) { float x = g_pointsXYZBuffer[step + j*3]; - EXPECT_FLOAT_EQ(expectedDepth, x); + EXPECT_NEAR(expectedDepth, x, DOUBLE_TOL); } } diff --git a/test/integration/gpu_lidar_sensor_plugin.cc b/test/integration/gpu_lidar_sensor_plugin.cc index f8438f6d..c4c0d8e6 100644 --- a/test/integration/gpu_lidar_sensor_plugin.cc +++ b/test/integration/gpu_lidar_sensor_plugin.cc @@ -41,7 +41,7 @@ #define VERTICAL_LASER_TOL 1e-3 #define WAIT_TIME 0.02 -sdf::ElementPtr GpuLidarToSDF(const std::string &name, +sdf::ElementPtr GpuLidarToSdf(const std::string &name, const ignition::math::Pose3d &pose, const double updateRate, const std::string &topic, const double horzSamples, const double horzResolution, const double horzMinAngle, @@ -138,6 +138,9 @@ class GpuLidarSensorTest: public testing::Test, // Test manually updating sensors public: void ManualUpdate(const std::string &_renderEngine); + + // Test topics + public: void Topic(const std::string &_renderEngine); }; ///////////////////////////////////////////////// @@ -166,7 +169,7 @@ void GpuLidarSensorTest::CreateGpuLidar(const std::string &_renderEngine) // Create sensor description in SDF ignition::math::Pose3d testPose(ignition::math::Vector3d(0, 0, 0.1), ignition::math::Quaterniond::Identity); - sdf::ElementPtr lidarSDF = GpuLidarToSDF(name, testPose, updateRate, topic, + sdf::ElementPtr lidarSdf = GpuLidarToSdf(name, testPose, updateRate, topic, horzSamples, horzResolution, horzMinAngle, horzMaxAngle, vertSamples, vertResolution, vertMinAngle, vertMaxAngle, rangeResolution, rangeMin, rangeMax, alwaysOn, visualize); @@ -192,7 +195,7 @@ void GpuLidarSensorTest::CreateGpuLidar(const std::string &_renderEngine) // Create a GpuLidarSensor ignition::sensors::GpuLidarSensor *sensor = - mgr.CreateSensor(lidarSDF); + mgr.CreateSensor(lidarSdf); sensor->SetParent(parent); // Make sure the above dynamic cast worked. EXPECT_TRUE(sensor != nullptr); @@ -283,7 +286,7 @@ void GpuLidarSensorTest::DetectBox(const std::string &_renderEngine) // Create sensor SDF ignition::math::Pose3d testPose(ignition::math::Vector3d(0.0, 0.0, 0.1), ignition::math::Quaterniond::Identity); - sdf::ElementPtr lidarSDF = GpuLidarToSDF(name, testPose, updateRate, topic, + sdf::ElementPtr lidarSdf = GpuLidarToSdf(name, testPose, updateRate, topic, horzSamples, horzResolution, horzMinAngle, horzMaxAngle, vertSamples, vertResolution, vertMinAngle, vertMaxAngle, rangeResolution, rangeMin, rangeMax, alwaysOn, visualize); @@ -319,7 +322,7 @@ void GpuLidarSensorTest::DetectBox(const std::string &_renderEngine) // Create a GpuLidarSensor ignition::sensors::GpuLidarSensor *sensor = - mgr.CreateSensor(lidarSDF); + mgr.CreateSensor(lidarSdf); // Make sure the above dynamic cast worked. EXPECT_TRUE(sensor != nullptr); sensor->SetParent(parent); @@ -429,7 +432,7 @@ void GpuLidarSensorTest::TestThreeBoxes(const std::string &_renderEngine) // Create sensor SDF ignition::math::Pose3d testPose1(ignition::math::Vector3d(0, 0, 0.1), ignition::math::Quaterniond::Identity); - sdf::ElementPtr lidarSDF1 = GpuLidarToSDF(name1, testPose1, updateRate, + sdf::ElementPtr lidarSdf1 = GpuLidarToSdf(name1, testPose1, updateRate, topic1, horzSamples, horzResolution, horzMinAngle, horzMaxAngle, vertSamples, vertResolution, vertMinAngle, vertMaxAngle, rangeResolution, rangeMin, rangeMax, alwaysOn, visualize); @@ -437,7 +440,7 @@ void GpuLidarSensorTest::TestThreeBoxes(const std::string &_renderEngine) // Create a second sensor SDF rotated ignition::math::Pose3d testPose2(ignition::math::Vector3d(0, 0, 0.1), ignition::math::Quaterniond(IGN_PI/2.0, 0, 0)); - sdf::ElementPtr lidarSDF2 = GpuLidarToSDF(name2, testPose2, updateRate, + sdf::ElementPtr lidarSdf2 = GpuLidarToSdf(name2, testPose2, updateRate, topic2, horzSamples, horzResolution, horzMinAngle, horzMaxAngle, vertSamples, vertResolution, vertMinAngle, vertMaxAngle, rangeResolution, rangeMin, rangeMax, alwaysOn, visualize); @@ -463,11 +466,11 @@ void GpuLidarSensorTest::TestThreeBoxes(const std::string &_renderEngine) // Create a GpuLidarSensors ignition::sensors::GpuLidarSensor *sensor1 = - mgr.CreateSensor(lidarSDF1); + mgr.CreateSensor(lidarSdf1); // Create second GpuLidarSensor ignition::sensors::GpuLidarSensor *sensor2 = - mgr.CreateSensor(lidarSDF2); + mgr.CreateSensor(lidarSdf2); // Make sure the above dynamic cast worked. EXPECT_TRUE(sensor1 != nullptr); @@ -572,7 +575,7 @@ void GpuLidarSensorTest::VerticalLidar(const std::string &_renderEngine) // Create sensor SDF ignition::math::Pose3d testPose(ignition::math::Vector3d(0.25, 0.0, 0.5), ignition::math::Quaterniond::Identity); - sdf::ElementPtr lidarSDF = GpuLidarToSDF(name, testPose, updateRate, topic, + sdf::ElementPtr lidarSdf = GpuLidarToSdf(name, testPose, updateRate, topic, horzSamples, horzResolution, horzMinAngle, horzMaxAngle, vertSamples, vertResolution, vertMinAngle, vertMaxAngle, rangeResolution, rangeMin, rangeMax, alwaysOn, visualize); @@ -609,7 +612,7 @@ void GpuLidarSensorTest::VerticalLidar(const std::string &_renderEngine) // Create a GpuLidarSensor ignition::sensors::GpuLidarSensor *sensor = - mgr.CreateSensor(lidarSDF); + mgr.CreateSensor(lidarSdf); // Make sure the above dynamic cast worked. EXPECT_TRUE(sensor != nullptr); @@ -693,7 +696,7 @@ void GpuLidarSensorTest::ManualUpdate(const std::string &_renderEngine) // Create sensor SDF ignition::math::Pose3d testPose1(ignition::math::Vector3d(0, 0, 0.1), ignition::math::Quaterniond::Identity); - sdf::ElementPtr lidarSDF1 = GpuLidarToSDF(name1, testPose1, updateRate, + sdf::ElementPtr lidarSdf1 = GpuLidarToSdf(name1, testPose1, updateRate, topic1, horzSamples, horzResolution, horzMinAngle, horzMaxAngle, vertSamples, vertResolution, vertMinAngle, vertMaxAngle, rangeResolution, rangeMin, rangeMax, alwaysOn, visualize); @@ -701,7 +704,7 @@ void GpuLidarSensorTest::ManualUpdate(const std::string &_renderEngine) // Create a second sensor SDF at an xy offset of 1 ignition::math::Pose3d testPose2(ignition::math::Vector3d(1, 1, 0.1), ignition::math::Quaterniond::Identity); - sdf::ElementPtr lidarSDF2 = GpuLidarToSDF(name2, testPose2, updateRate, + sdf::ElementPtr lidarSdf2 = GpuLidarToSdf(name2, testPose2, updateRate, topic2, horzSamples, horzResolution, horzMinAngle, horzMaxAngle, vertSamples, vertResolution, vertMinAngle, vertMaxAngle, rangeResolution, rangeMin, rangeMax, alwaysOn, visualize); @@ -727,11 +730,11 @@ void GpuLidarSensorTest::ManualUpdate(const std::string &_renderEngine) // Create a GpuLidarSensors ignition::sensors::GpuLidarSensor *sensor1 = - mgr.CreateSensor(lidarSDF1); + mgr.CreateSensor(lidarSdf1); // Create second GpuLidarSensor ignition::sensors::GpuLidarSensor *sensor2 = - mgr.CreateSensor(lidarSDF2); + mgr.CreateSensor(lidarSdf2); // Make sure the above dynamic cast worked. EXPECT_TRUE(sensor1 != nullptr); @@ -783,6 +786,95 @@ void GpuLidarSensorTest::ManualUpdate(const std::string &_renderEngine) ignition::rendering::unloadEngine(engine->Name()); } +///////////////////////////////////////////////// +void GpuLidarSensorTest::Topic(const std::string &_renderEngine) +{ + const std::string name = "TestGpuLidar"; + const double updateRate = 30; + const unsigned int horzSamples = 640; + const double horzResolution = 1; + const double horzMinAngle = -1.396263; + const double horzMaxAngle = 1.396263; + const double vertResolution = 1; + const unsigned int vertSamples = 1; + const double vertMinAngle = 0; + const double vertMaxAngle = 0; + const double rangeResolution = 0.01; + const double rangeMin = 0.08; + const double rangeMax = 10.0; + const bool alwaysOn = 1; + const bool visualize = 1; + auto testPose = ignition::math::Pose3d(); + + // Scene + auto engine = ignition::rendering::engine(_renderEngine); + if (!engine) + { + igndbg << "Engine '" << _renderEngine + << "' is not supported" << std::endl; + return; + } + auto scene = engine->CreateScene("scene"); + EXPECT_NE(nullptr, scene); + + // Create a GpuLidarSensor + ignition::sensors::Manager mgr; + mgr.AddPluginPaths(ignition::common::joinPaths(PROJECT_BUILD_PATH, "lib")); + + + // Default topic + { + const std::string topic; + auto lidarSdf = GpuLidarToSdf(name, testPose, updateRate, topic, + horzSamples, horzResolution, horzMinAngle, horzMaxAngle, + vertSamples, vertResolution, vertMinAngle, vertMaxAngle, + rangeResolution, rangeMin, rangeMax, alwaysOn, visualize); + + auto sensorId = mgr.CreateSensor(lidarSdf); + EXPECT_NE(ignition::sensors::NO_SENSOR, sensorId); + + auto sensor = mgr.Sensor(sensorId); + EXPECT_NE(nullptr, sensor); + + auto lidar = dynamic_cast(sensor); + ASSERT_NE(nullptr, lidar); + + EXPECT_EQ("/lidar/points", lidar->Topic()); + } + + // Convert to valid topic + { + const std::string topic = "/topic with spaces/@~characters//"; + auto lidarSdf = GpuLidarToSdf(name, testPose, updateRate, topic, + horzSamples, horzResolution, horzMinAngle, horzMaxAngle, + vertSamples, vertResolution, vertMinAngle, vertMaxAngle, + rangeResolution, rangeMin, rangeMax, alwaysOn, visualize); + + auto sensorId = mgr.CreateSensor(lidarSdf); + EXPECT_NE(ignition::sensors::NO_SENSOR, sensorId); + + auto sensor = mgr.Sensor(sensorId); + EXPECT_NE(nullptr, sensor); + + auto lidar = dynamic_cast(sensor); + ASSERT_NE(nullptr, lidar); + + EXPECT_EQ("/topic_with_spaces/characters/points", lidar->Topic()); + } + + // Invalid topic + { + const std::string topic = "@@@"; + auto lidarSdf = GpuLidarToSdf(name, testPose, updateRate, topic, + horzSamples, horzResolution, horzMinAngle, horzMaxAngle, + vertSamples, vertResolution, vertMinAngle, vertMaxAngle, + rangeResolution, rangeMin, rangeMax, alwaysOn, visualize); + + auto sensorId = mgr.CreateSensor(lidarSdf); + EXPECT_EQ(ignition::sensors::NO_SENSOR, sensorId); + } +} + TEST_P(GpuLidarSensorTest, CreateGpuLidar) { CreateGpuLidar(GetParam()); @@ -808,6 +900,11 @@ TEST_P(GpuLidarSensorTest, ManualUpdate) ManualUpdate(GetParam()); } +TEST_P(GpuLidarSensorTest, Topic) +{ + Topic(GetParam()); +} + INSTANTIATE_TEST_CASE_P(GpuLidarSensor, GpuLidarSensorTest, RENDER_ENGINE_VALUES, ignition::rendering::PrintToStringParam()); diff --git a/test/integration/imu_plugin.cc b/test/integration/imu_plugin.cc index d2a0461f..d7c52794 100644 --- a/test/integration/imu_plugin.cc +++ b/test/integration/imu_plugin.cc @@ -26,7 +26,7 @@ #include "TransportTestTools.hh" /// \brief Helper function to create an imu sdf element -sdf::ElementPtr ImuToSDF(const std::string &_name, +sdf::ElementPtr ImuToSdf(const std::string &_name, const ignition::math::Pose3d &_pose, const double _updateRate, const std::string &_topic, const bool _alwaysOn, const bool _visualize) @@ -74,14 +74,14 @@ TEST_F(ImuSensorTest, CreateImu) // Create sensor SDF ignition::math::Pose3d sensorPose(ignition::math::Vector3d(0.25, 0.0, 0.5), ignition::math::Quaterniond::Identity); - sdf::ElementPtr imuSDF = ImuToSDF(name, sensorPose, + sdf::ElementPtr imuSdf = ImuToSdf(name, sensorPose, updateRate, topic, alwaysOn, visualize); // create the sensor using sensor factory ignition::sensors::SensorFactory sf; sf.AddPluginPaths(ignition::common::joinPaths(PROJECT_BUILD_PATH, "lib")); std::unique_ptr sensor = - sf.CreateSensor(imuSDF); + sf.CreateSensor(imuSdf); EXPECT_TRUE(sensor != nullptr); EXPECT_EQ(name, sensor->Name()); @@ -102,7 +102,7 @@ TEST_F(ImuSensorTest, SensorReadings) // Create sensor SDF ignition::math::Pose3d sensorPose(ignition::math::Vector3d(0.25, 0.0, 0.5), ignition::math::Quaterniond::Identity); - sdf::ElementPtr imuSDF = ImuToSDF(name, sensorPose, + sdf::ElementPtr imuSdf = ImuToSdf(name, sensorPose, updateRate, topic, alwaysOn, visualize); // create the sensor using sensor factory @@ -110,7 +110,7 @@ TEST_F(ImuSensorTest, SensorReadings) ignition::sensors::SensorFactory sf; sf.AddPluginPaths(ignition::common::joinPaths(PROJECT_BUILD_PATH, "lib")); std::unique_ptr s = - sf.CreateSensor(imuSDF); + sf.CreateSensor(imuSdf); std::unique_ptr sensor( dynamic_cast(s.release())); @@ -226,6 +226,62 @@ TEST_F(ImuSensorTest, SensorReadings) ignition::msgs::Convert(msg.orientation())); } +///////////////////////////////////////////////// +TEST_F(ImuSensorTest, Topic) +{ + const std::string name = "TestImu"; + const double updateRate = 30; + const bool alwaysOn = 1; + const bool visualize = 1; + auto sensorPose = ignition::math::Pose3d(); + + // Factory + ignition::sensors::SensorFactory factory; + factory.AddPluginPaths(ignition::common::joinPaths(PROJECT_BUILD_PATH, + "lib")); + + // Default topic + { + const std::string topic; + auto imuSdf = ImuToSdf(name, sensorPose, + updateRate, topic, alwaysOn, visualize); + + auto sensor = factory.CreateSensor(imuSdf); + EXPECT_NE(nullptr, sensor); + + auto imu = dynamic_cast(sensor.release()); + ASSERT_NE(nullptr, imu); + + EXPECT_EQ("/imu", imu->Topic()); + } + + // Convert to valid topic + { + const std::string topic = "/topic with spaces/@~characters//"; + auto imuSdf = ImuToSdf(name, sensorPose, + updateRate, topic, alwaysOn, visualize); + + auto sensor = factory.CreateSensor(imuSdf); + EXPECT_NE(nullptr, sensor); + + auto imu = + dynamic_cast(sensor.release()); + ASSERT_NE(nullptr, imu); + + EXPECT_EQ("/topic_with_spaces/characters", imu->Topic()); + } + + // Invalid topic + { + const std::string topic = "@@@"; + auto imuSdf = ImuToSdf(name, sensorPose, + updateRate, topic, alwaysOn, visualize); + + auto sensor = factory.CreateSensor(imuSdf); + ASSERT_EQ(nullptr, sensor); + } +} + int main(int argc, char **argv) { ::testing::InitGoogleTest(&argc, argv); diff --git a/test/integration/logical_camera_plugin.cc b/test/integration/logical_camera_plugin.cc index ce7fc5de..403367fb 100644 --- a/test/integration/logical_camera_plugin.cc +++ b/test/integration/logical_camera_plugin.cc @@ -39,7 +39,7 @@ #endif /// \brief Helper function to create a logical camera sdf element -sdf::ElementPtr LogicalCameraToSDF(const std::string &_name, +sdf::ElementPtr LogicalCameraToSdf(const std::string &_name, const ignition::math::Pose3d &_pose, const double _updateRate, const std::string &_topic, const double _near, const double _far, const double _horzFov, @@ -99,7 +99,7 @@ TEST_F(LogicalCameraSensorTest, CreateLogicalCamera) // Create sensor SDF ignition::math::Pose3d sensorPose(ignition::math::Vector3d(0.25, 0.0, 0.5), ignition::math::Quaterniond::Identity); - sdf::ElementPtr logicalCameraSDF = LogicalCameraToSDF(name, sensorPose, + sdf::ElementPtr logicalCameraSdf = LogicalCameraToSdf(name, sensorPose, updateRate, topic, near, far, horzFov, aspectRatio, alwaysOn, visualize); @@ -107,7 +107,7 @@ TEST_F(LogicalCameraSensorTest, CreateLogicalCamera) ignition::sensors::SensorFactory sf; sf.AddPluginPaths(ignition::common::joinPaths(PROJECT_BUILD_PATH, "lib")); std::unique_ptr sensor = - sf.CreateSensor(logicalCameraSDF); + sf.CreateSensor(logicalCameraSdf); EXPECT_TRUE(sensor != nullptr); EXPECT_EQ(name, sensor->Name()); @@ -137,7 +137,7 @@ TEST_F(LogicalCameraSensorTest, DetectBox) // Create sensor SDF ignition::math::Pose3d sensorPose(ignition::math::Vector3d(0.25, 0.0, 0.5), ignition::math::Quaterniond::Identity); - sdf::ElementPtr logicalCameraSDF = LogicalCameraToSDF(name, sensorPose, + sdf::ElementPtr logicalCameraSdf = LogicalCameraToSdf(name, sensorPose, updateRate, topic, near, far, horzFov, aspectRatio, alwaysOn, visualize); @@ -146,7 +146,7 @@ TEST_F(LogicalCameraSensorTest, DetectBox) ignition::sensors::SensorFactory sf; sf.AddPluginPaths(ignition::common::joinPaths(PROJECT_BUILD_PATH, "lib")); std::unique_ptr s = - sf.CreateSensor(logicalCameraSDF); + sf.CreateSensor(logicalCameraSdf); std::unique_ptr sensor( dynamic_cast(s.release())); @@ -230,6 +230,72 @@ TEST_F(LogicalCameraSensorTest, DetectBox) EXPECT_EQ(0, img.model().size()); } +///////////////////////////////////////////////// +TEST_F(LogicalCameraSensorTest, Topic) +{ + const std::string name = "TestLogicalCamera"; + const double updateRate = 30; + const double near = 0.55; + const double far = 5; + const double horzFov = 1.04719755; + const double aspectRatio = 1.778; + const bool alwaysOn = 1; + const bool visualize = 1; + auto sensorPose = ignition::math::Pose3d(); + + // Factory + ignition::sensors::SensorFactory factory; + factory.AddPluginPaths(ignition::common::joinPaths(PROJECT_BUILD_PATH, + "lib")); + + // Default topic + { + const std::string topic; + auto logicalCameraSdf = LogicalCameraToSdf(name, sensorPose, + updateRate, topic, near, far, horzFov, aspectRatio, alwaysOn, + visualize); + + auto sensor = factory.CreateSensor(logicalCameraSdf); + EXPECT_NE(nullptr, sensor); + + auto logicalCamera = + dynamic_cast( + sensor.release()); + ASSERT_NE(nullptr, logicalCamera); + + EXPECT_EQ("/logical_camera", logicalCamera->Topic()); + } + + // Convert to valid topic + { + const std::string topic = "/topic with spaces/@~characters//"; + auto logicalCameraSdf = LogicalCameraToSdf(name, sensorPose, + updateRate, topic, near, far, horzFov, aspectRatio, alwaysOn, + visualize); + + auto sensor = factory.CreateSensor(logicalCameraSdf); + EXPECT_NE(nullptr, sensor); + + auto logicalCamera = + dynamic_cast( + sensor.release()); + ASSERT_NE(nullptr, logicalCamera); + + EXPECT_EQ("/topic_with_spaces/characters", logicalCamera->Topic()); + } + + // Invalid topic + { + const std::string topic = "@@@"; + auto logicalCameraSdf = LogicalCameraToSdf(name, sensorPose, + updateRate, topic, near, far, horzFov, aspectRatio, alwaysOn, + visualize); + + auto sensor = factory.CreateSensor(logicalCameraSdf); + ASSERT_EQ(nullptr, sensor); + } +} + int main(int argc, char **argv) { ::testing::InitGoogleTest(&argc, argv); diff --git a/test/integration/magnetometer_plugin.cc b/test/integration/magnetometer_plugin.cc index d1fe19bb..694448b3 100644 --- a/test/integration/magnetometer_plugin.cc +++ b/test/integration/magnetometer_plugin.cc @@ -26,7 +26,7 @@ #include "TransportTestTools.hh" /// \brief Helper function to create an magnetometer sdf element -sdf::ElementPtr MagnetometerToSDF(const std::string &_name, +sdf::ElementPtr MagnetometerToSdf(const std::string &_name, const ignition::math::Pose3d &_pose, const double _updateRate, const std::string &_topic, const bool _alwaysOn, const bool _visualize) @@ -131,7 +131,7 @@ TEST_F(MagnetometerSensorTest, CreateMagnetometer) // Create sensor SDF ignition::math::Pose3d sensorPose(ignition::math::Vector3d(0.25, 0.0, 0.5), ignition::math::Quaterniond::Identity); - sdf::ElementPtr magnetometerSDF = MagnetometerToSDF(name, sensorPose, + sdf::ElementPtr magnetometerSdf = MagnetometerToSdf(name, sensorPose, updateRate, topic, alwaysOn, visualize); sdf::ElementPtr magnetometerNoiseSdf = MagnetometerToSdfWithNoise(name, @@ -141,7 +141,7 @@ TEST_F(MagnetometerSensorTest, CreateMagnetometer) ignition::sensors::SensorFactory sf; sf.AddPluginPaths(ignition::common::joinPaths(PROJECT_BUILD_PATH, "lib")); std::unique_ptr sensor = - sf.CreateSensor(magnetometerSDF); + sf.CreateSensor(magnetometerSdf); EXPECT_TRUE(sensor != nullptr); std::unique_ptr sensorNoise = @@ -171,7 +171,7 @@ TEST_F(MagnetometerSensorTest, SensorReadings) // Create sensor SDF ignition::math::Pose3d sensorPose(ignition::math::Vector3d(0.25, 0.0, 0.5), ignition::math::Quaterniond::Identity); - sdf::ElementPtr magnetometerSDF = MagnetometerToSDF(name, sensorPose, + sdf::ElementPtr magnetometerSdf = MagnetometerToSdf(name, sensorPose, updateRate, topic, alwaysOn, visualize); sdf::ElementPtr magnetometerSdfNoise = MagnetometerToSdfWithNoise(name, sensorPose, updateRate, noiseTopic, alwaysOn, visualize, 1.0, 0.2, 10.0); @@ -181,7 +181,7 @@ TEST_F(MagnetometerSensorTest, SensorReadings) ignition::sensors::SensorFactory sf; sf.AddPluginPaths(ignition::common::joinPaths(PROJECT_BUILD_PATH, "lib")); std::unique_ptr s = - sf.CreateSensor(magnetometerSDF); + sf.CreateSensor(magnetometerSdf); std::unique_ptr sensor( dynamic_cast(s.release())); @@ -285,6 +285,63 @@ TEST_F(MagnetometerSensorTest, SensorReadings) EXPECT_EQ(localField, ignition::msgs::Convert(msg.field_tesla())); } +///////////////////////////////////////////////// +TEST_F(MagnetometerSensorTest, Topic) +{ + const std::string name = "TestMagnetometer"; + const double updateRate = 30; + const bool alwaysOn = 1; + const bool visualize = 1; + auto sensorPose = ignition::math::Pose3d(); + + // Factory + ignition::sensors::SensorFactory factory; + factory.AddPluginPaths(ignition::common::joinPaths(PROJECT_BUILD_PATH, + "lib")); + + // Default topic + { + const std::string topic; + auto magnetometerSdf = MagnetometerToSdf(name, sensorPose, + updateRate, topic, alwaysOn, visualize); + + auto sensor = factory.CreateSensor(magnetometerSdf); + EXPECT_NE(nullptr, sensor); + + auto magnetometer = + dynamic_cast(sensor.release()); + ASSERT_NE(nullptr, magnetometer); + + EXPECT_EQ("/magnetometer", magnetometer->Topic()); + } + + // Convert to valid topic + { + const std::string topic = "/topic with spaces/@~characters//"; + auto magnetometerSdf = MagnetometerToSdf(name, sensorPose, + updateRate, topic, alwaysOn, visualize); + + auto sensor = factory.CreateSensor(magnetometerSdf); + EXPECT_NE(nullptr, sensor); + + auto magnetometer = + dynamic_cast(sensor.release()); + ASSERT_NE(nullptr, magnetometer); + + EXPECT_EQ("/topic_with_spaces/characters", magnetometer->Topic()); + } + + // Invalid topic + { + const std::string topic = "@@@"; + auto magnetometerSdf = MagnetometerToSdf(name, sensorPose, + updateRate, topic, alwaysOn, visualize); + + auto sensor = factory.CreateSensor(magnetometerSdf); + ASSERT_EQ(nullptr, sensor); + } +} + int main(int argc, char **argv) { ::testing::InitGoogleTest(&argc, argv);