Make topics valid

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;

src/AirPressureSensor.cc

@@ -100,16 +100,15 @@ 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<ignition::msgs::FluidPressure>(topic);
+      this->dataPtr->node.Advertise<ignition::msgs::FluidPressure>(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;
   }
 

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<ignition::msgs::Altimeter>(topic);
+      this->dataPtr->node.Advertise<ignition::msgs::Altimeter>(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;
   }
 

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<ignition::msgs::Image>(
           this->Topic());

src/Camera_TEST.cc

@@ -28,7 +28,7 @@ sdf::ElementPtr cameraToBadSdf()
     << "<sdf version='1.6'>"
     << " <model name='m1'>"
     << "  <link name='link1'>"
-    << "    <sensor name='cam_name' type='camera'>"
+    << "    <sensor name='cam_name' type='not_camera'>"
     << "      <not_camera>"
     << "      </not_camera>"
     << "    </sensor>"
@@ -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<ignition::sensors::CameraSensor *>(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<ignition::sensors::CameraSensor *>(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)
 {

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<ignition::msgs::Image>(
           this->Topic());

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<ignition::msgs::PointCloudPacked>(
-          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;
   }
 

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<ignition::msgs::IMU>(topic);
+      this->dataPtr->node.Advertise<ignition::msgs::IMU>(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;
   }
 

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<ignition::msgs::LaserScan>(
         this->Topic());

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<ignition::msgs::LogicalCameraImage>(topic);
+      this->dataPtr->node.Advertise<ignition::msgs::LogicalCameraImage>(
+      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;
   }
 

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<ignition::msgs::Magnetometer>(topic);
+      this->dataPtr->node.Advertise<ignition::msgs::Magnetometer>(
+      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;
   }
 

src/Sensor.cc

@@ -21,6 +21,7 @@
 #include <ignition/sensors/Manager.hh>
 #include <ignition/common/Console.hh>
 #include <ignition/common/Profiler.hh>
+#include <ignition/transport/TopicUtils.hh>
 
 using namespace ignition::sensors;
 
@@ -30,6 +31,11 @@ class ignition::sensors::SensorPrivate
   /// \brief Populates fields from a <sensor> 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
 {

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)
 {