From a2b4904adf7ab262861ff135ea6f31e2ebf58859 Mon Sep 17 00:00:00 2001 From: Louise Poubel Date: Tue, 4 Aug 2020 14:23:59 -0700 Subject: [PATCH] Make topics valid (#33) Signed-off-by: Louise Poubel --- .github/ci/after_make.sh | 12 ++ .github/ci/packages.apt | 10 ++ .github/workflows/{ci-bionic.yml => ci.yml} | 8 +- CMakeLists.txt | 2 +- include/ignition/sensors/Sensor.hh | 5 + src/AirPressureSensor.cc | 10 +- src/AltimeterSensor.cc | 9 +- src/CameraSensor.cc | 3 + src/Camera_TEST.cc | 65 +++++++++- src/DepthCameraSensor.cc | 3 + src/GpuLidarSensor.cc | 6 +- src/ImuSensor.cc | 9 +- src/Lidar.cc | 3 + src/LogicalCameraSensor.cc | 10 +- src/MagnetometerSensor.cc | 10 +- src/Sensor.cc | 32 ++++- src/Sensor_TEST.cc | 16 +++ test/integration/air_pressure_plugin.cc | 57 +++++++++ test/integration/altimeter_plugin.cc | 67 ++++++++++- test/integration/depth_camera_plugin.cc | 4 +- test/integration/gpu_lidar_sensor_plugin.cc | 127 +++++++++++++++++--- test/integration/imu_plugin.cc | 66 +++++++++- test/integration/logical_camera_plugin.cc | 76 +++++++++++- test/integration/magnetometer_plugin.cc | 67 ++++++++++- 24 files changed, 604 insertions(+), 73 deletions(-) create mode 100644 .github/ci/after_make.sh create mode 100644 .github/ci/packages.apt rename .github/workflows/{ci-bionic.yml => ci.yml} (77%) 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..744456b5 --- /dev/null +++ b/.github/ci/packages.apt @@ -0,0 +1,10 @@ +libignition-cmake2-dev +libignition-common3-dev +libignition-math6-dev +libignition-msgs4-dev +libignition-plugin-dev +libignition-rendering2-dev +libignition-tools-dev +libignition-transport7-dev +libsdformat8-dev +xvfb diff --git a/.github/workflows/ci-bionic.yml b/.github/workflows/ci.yml similarity index 77% rename from .github/workflows/ci-bionic.yml rename to .github/workflows/ci.yml index d0907f2c..5f7d3202 100644 --- a/.github/workflows/ci-bionic.yml +++ b/.github/workflows/ci.yml @@ -1,6 +1,6 @@ -name: Ubuntu Bionic CI +name: Ubuntu CI -on: [push, pull_request] +on: [push] jobs: bionic-ci: @@ -9,9 +9,9 @@ jobs: steps: - name: Checkout uses: actions/checkout@v2 - - name: Bionic CI + - name: Compile and test id: ci - uses: ignition-tooling/ubuntu-bionic-ci-action@master + uses: ignition-tooling/action-ignition-ci@master with: apt-dependencies: 'libignition-cmake2-dev libignition-common3-dev libignition-math6-dev libignition-msgs4-dev libignition-rendering2-dev libignition-transport7-dev libignition-tools-dev libignition-plugin-dev libsdformat8-dev' codecov-token: ${{ secrets.CODECOV_TOKEN }} diff --git a/CMakeLists.txt b/CMakeLists.txt index 2a431b54..de9c963b 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -55,7 +55,7 @@ set(IGN_COMMON_VER ${ignition-common3_VERSION_MAJOR}) #-------------------------------------- # Find ignition-transport -ign_find_package(ignition-transport7 REQUIRED) +ign_find_package(ignition-transport7 REQUIRED VERSION 7.5) set(IGN_TRANSPORT_VER ${ignition-transport7_VERSION_MAJOR}) #-------------------------------------- diff --git a/include/ignition/sensors/Sensor.hh b/include/ignition/sensors/Sensor.hh index 4ce0ddf6..d4db4de5 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/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 02bcc16f..252b8c91 100644 --- a/src/CameraSensor.cc +++ b/src/CameraSensor.cc @@ -234,6 +234,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 99a89371..3233e640 100644 --- a/src/Camera_TEST.cc +++ b/src/Camera_TEST.cc @@ -28,7 +28,7 @@ sdf::ElementPtr cameraToBadSdf() << "" << " " << " " - << " " + << " " << " " << " " << " " @@ -45,7 +45,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) { @@ -127,7 +127,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 @@ -156,6 +156,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 25cd34c7..dfcfdf75 100644 --- a/src/DepthCameraSensor.cc +++ b/src/DepthCameraSensor.cc @@ -252,6 +252,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 9ada79eb..78775a0e 100644 --- a/src/GpuLidarSensor.cc +++ b/src/GpuLidarSensor.cc @@ -121,14 +121,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/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 465e553e..ef31f44f 100644 --- a/src/Lidar.cc +++ b/src/Lidar.cc @@ -104,6 +104,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 bb12ec4d..2b518b13 100644 --- a/src/LogicalCameraSensor.cc +++ b/src/LogicalCameraSensor.cc @@ -103,16 +103,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/Sensor.cc b/src/Sensor.cc index 1a8efdf3..a9223fff 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; + } this->pose = _sdf.Pose(); this->updateRate = _sdf.UpdateRate(); return true; @@ -157,6 +167,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);