Add set_rate service to all sensors

src/CMakeLists.txt

@@ -43,6 +43,7 @@ target_link_libraries(${PROJECT_LIBRARY_TARGET_NAME}
     sdformat${SDF_VER}::sdformat${SDF_VER}
   PRIVATE
     ignition-common${IGN_COMMON_VER}::profiler
+    ignition-msgs${IGN_MSGS_VER}::ignition-msgs${IGN_MSGS_VER}
 )
 target_compile_definitions(${PROJECT_LIBRARY_TARGET_NAME} PUBLIC DepthPoints_EXPORTS)
 

src/Sensor.cc

@@ -20,6 +20,8 @@
 #include <vector>
 #include <ignition/common/Console.hh>
 #include <ignition/common/Profiler.hh>
+#include <ignition/msgs/double.pb.h>
+#include <ignition/transport/Node.hh>
 #include <ignition/transport/TopicUtils.hh>
 
 #include <ignition/sensors/Manager.hh>
@@ -37,6 +39,14 @@ class ignition::sensors::SensorPrivate
   /// \return True if a valid topic was set.
   public: bool SetTopic(const std::string &_topic);
 
+  /// \brief Set the rate on which the sensor should publish its data. This
+  /// method doesn't allow to set a higher rate than what is in the SDF.
+  /// \param[in] _rate Maximum rate of the sensor. It is capped by the
+  /// <update_rate> value from SDF, and zero is allowed only when zero is also
+  /// in SDF.
+  /// \return True if a valid topic was set.
+  public: void SetRate(const ignition::msgs::Double& _rate);
+
   /// \brief id given to sensor when constructed
   public: SensorId id;
 
@@ -55,9 +65,17 @@ class ignition::sensors::SensorPrivate
   /// \brief Pose of the sensor
   public: ignition::math::Pose3d pose;
 
-  /// \brief How many times the sensor will generate data per second
+  /// \brief How many times the sensor will generate data per second (value from
+  /// SDF.
+  public: double sdfUpdateRate = 0.0;
+
+  /// \brief How many times the sensor will generate data per second (currently
+  /// used value).
   public: double updateRate = 0.0;
 
+  /// \brief node to create rate update service server
+  public: transport::Node node;
+
   /// \brief What sim time should this sensor update at
   public: std::chrono::steady_clock::duration nextUpdateTime
     {std::chrono::steady_clock::duration::zero()};
@@ -110,7 +128,7 @@ bool SensorPrivate::PopulateFromSDF(const sdf::Sensor &_sdf)
     this->pose = _sdf.RawPose();
   }
 
-  this->updateRate = _sdf.UpdateRate();
+  this->sdfUpdateRate = this->updateRate = _sdf.UpdateRate();
   return true;
 }
 
@@ -135,7 +153,25 @@ Sensor::~Sensor()
 //////////////////////////////////////////////////
 bool Sensor::Load(const sdf::Sensor &_sdf)
 {
-  return this->dataPtr->PopulateFromSDF(_sdf);
+  const bool success = this->dataPtr->PopulateFromSDF(_sdf);
+  if (!success)
+    return false;
+
+  auto sensorTopic = this->Topic();
+  if (sensorTopic.empty())
+    sensorTopic = "/" + this->Name();
+
+  const auto rateTopic = sensorTopic + "/set_rate";
+
+  if (!this->dataPtr->node.Advertise(rateTopic,
+    &SensorPrivate::SetRate, this->dataPtr.get()))
+  {
+    ignerr << "Unable to create service server on topic["
+           << rateTopic << "].\n";
+    return false;
+  }
+
+  return true;
 }
 
 //////////////////////////////////////////////////
@@ -150,7 +186,7 @@ bool Sensor::Load(sdf::ElementPtr _sdf)
 
   sdf::Sensor sdfSensor;
   sdfSensor.Load(_sdf);
-  return this->dataPtr->PopulateFromSDF(sdfSensor);
+  return this->Load(sdfSensor);
 }
 
 //////////////////////////////////////////////////
@@ -197,6 +233,42 @@ bool SensorPrivate::SetTopic(const std::string &_topic)
   return true;
 }
 
+//////////////////////////////////////////////////
+void SensorPrivate::SetRate(const ignition::msgs::Double& _rate)
+{
+  auto rate = _rate.data();
+  if (rate < 0.0)
+    rate = 0.0;
+
+  // if SDF has zero, any value can be set; for non-zero SDF values, we need to
+  // check whether they are in bounds, i.e. greater than zero and lower or equal
+  // to the SDF value
+  if (!ignition::math::lessOrNearEqual(this->sdfUpdateRate, 0.0))
+  {
+    if (ignition::math::lessOrNearEqual(rate, 0.0))
+    {
+      ignerr << "Cannot set update rate of sensor " << this->name << " to zero "
+             << "because the <update_rate> SDF element is non-zero."
+             << std::endl;
+      return;
+    }
+    // apply the upper rate limit from SDF
+    else if (!ignition::math::lessOrNearEqual(rate, this->sdfUpdateRate))
+    {
+      ignerr << "Trying to set update rate of sensor " << this->name << " to "
+             << rate << ", but the maximum rate in <update_rate> SDF element "
+             << "is " << this->sdfUpdateRate << ". Ignoring the request."
+             << std::endl;
+      return;
+    }
+  }
+
+  igndbg << "Setting update rate of sensor " << this->name << " to " << rate
+         << " Hz" << std::endl;
+
+  this->updateRate = rate;
+}
+
 //////////////////////////////////////////////////
 ignition::math::Pose3d Sensor::Pose() const
 {

src/Sensor_TEST.cc

@@ -19,6 +19,7 @@
 #include <ignition/common/Console.hh>
 #include <ignition/sensors/Export.hh>
 #include <ignition/sensors/Sensor.hh>
+#include <ignition/transport/Node.hh>
 
 using namespace ignition;
 using namespace sensors;
@@ -129,6 +130,140 @@ TEST(Sensor_TEST, Topic)
   EXPECT_FALSE(sensor.SetTopic(""));
 }
 
+//////////////////////////////////////////////////
+TEST(Sensor_TEST, SetRateService)
+{
+  std::ostringstream stream;
+  stream
+    << "<?xml version='1.0'?>"
+    << "<sdf version='1.6'>"
+    << " <model name='m1'>"
+    << "  <link name='link1'>"
+    << "    <sensor name='test' type='altimeter'>"
+    << "      <topic>test_topic</topic>"
+    << "      <update_rate>10.0</update_rate>"
+    << "    </sensor>"
+    << "  </link>"
+    << " </model>"
+    << "</sdf>";
+
+  sdf::SDFPtr sdfParsed(new sdf::SDF());
+  sdf::init(sdfParsed);
+  ASSERT_TRUE(sdf::readString(stream.str(), sdfParsed));
+
+  TestSensor sensor;
+  ASSERT_TRUE(sensor.Load(
+    sdfParsed->Root()->GetElement("model")->GetElement("link")
+      ->GetElement("sensor")));
+
+  EXPECT_EQ("test_topic", sensor.Topic());
+  EXPECT_FLOAT_EQ(10.0, sensor.UpdateRate());
+
+  ignition::transport::Node node;
+
+  std::vector<std::string> services;
+  node.ServiceList(services);
+  ASSERT_LT(0u, services.size());
+
+  const auto serviceIt =
+    std::find(services.begin(), services.end(), "/test_topic/set_rate");
+  ASSERT_NE(services.end(), serviceIt);
+
+  std::vector<ignition::transport::ServicePublisher> publishers;
+  ASSERT_TRUE(node.ServiceInfo("/test_topic/set_rate", publishers));
+
+  ASSERT_LT(0u, publishers.size());
+
+  ignition::msgs::Double msg;
+  ignition::msgs::Empty rep;
+  bool res;
+
+  // can set value lower than in SDF
+  msg.set_data(5.0);
+  res = false;
+  EXPECT_TRUE(node.Request("/test_topic/set_rate", msg, 1000, rep, res));
+  EXPECT_TRUE(res);
+
+  EXPECT_FLOAT_EQ(5.0, sensor.UpdateRate());
+
+  // cannot set 0 if SDF has non-zero
+  msg.set_data(0.0);
+  res = false;
+  EXPECT_TRUE(node.Request("/test_topic/set_rate", msg, 1000, rep, res));
+  EXPECT_TRUE(res);
+
+  EXPECT_FLOAT_EQ(5.0, sensor.UpdateRate());
+
+  // cannot set higher than SDF value
+  msg.set_data(20.0);
+  res = false;
+  EXPECT_TRUE(node.Request("/test_topic/set_rate", msg, 1000, rep, res));
+  EXPECT_TRUE(res);
+
+  EXPECT_FLOAT_EQ(5.0, sensor.UpdateRate());
+}
+
+//////////////////////////////////////////////////
+TEST(Sensor_TEST, SetRateZeroService)
+{
+  std::ostringstream stream;
+  stream
+    << "<?xml version='1.0'?>"
+    << "<sdf version='1.6'>"
+    << " <model name='m1'>"
+    << "  <link name='link1'>"
+    << "    <sensor name='test' type='altimeter'>"
+    << "      <topic>test_topic2</topic>"
+    << "      <update_rate>0.0</update_rate>"
+    << "    </sensor>"
+    << "  </link>"
+    << " </model>"
+    << "</sdf>";
+
+  sdf::SDFPtr sdfParsed(new sdf::SDF());
+  sdf::init(sdfParsed);
+  ASSERT_TRUE(sdf::readString(stream.str(), sdfParsed));
+
+  TestSensor sensor;
+  ASSERT_TRUE(sensor.Load(
+    sdfParsed->Root()->GetElement("model")->GetElement("link")
+      ->GetElement("sensor")));
+
+  EXPECT_EQ("test_topic2", sensor.Topic());
+  EXPECT_FLOAT_EQ(0.0, sensor.UpdateRate());
+
+  ignition::transport::Node node;
+
+  ignition::msgs::Double msg;
+  ignition::msgs::Empty rep;
+  bool res;
+
+  // can set any value if SDF has 0
+  msg.set_data(5.0);
+  res = false;
+  EXPECT_TRUE(node.Request("/test_topic2/set_rate", msg, 1000, rep, res));
+  EXPECT_TRUE(res);
+
+  EXPECT_FLOAT_EQ(5.0, sensor.UpdateRate());
+
+  // can set 0 if SDF has zero
+  msg.set_data(0.0);
+  res = false;
+  EXPECT_TRUE(node.Request("/test_topic2/set_rate", msg, 1000, rep, res));
+  EXPECT_TRUE(res);
+
+  EXPECT_FLOAT_EQ(0.0, sensor.UpdateRate());
+
+  // can set any value if SDF has 0
+  msg.set_data(20.0);
+  res = false;
+  EXPECT_TRUE(node.Request("/test_topic2/set_rate", msg, 1000, rep, res));
+  EXPECT_TRUE(res);
+
+  EXPECT_FLOAT_EQ(20.0, sensor.UpdateRate());
+}
+
+
 //////////////////////////////////////////////////
 int main(int argc, char **argv)
 {