Add Joint DOM API to access joint sensors

Signed-off-by: Addisu Z. Taddese <[email protected]>

include/sdf/Joint.hh

@@ -38,6 +38,7 @@ namespace sdf
   struct FrameAttachedToGraph;
   struct PoseRelativeToGraph;
   template <typename T> class ScopedGraph;
+  class Sensor;
 
   /// \enum JointType
   /// \brief The set of joint types. INVALID indicates that joint type has
@@ -201,6 +202,29 @@ namespace sdf
     /// \return SemanticPose object for this link.
     public: sdf::SemanticPose SemanticPose() const;
 
+    /// \brief Get the number of sensors.
+    /// \return Number of sensors contained in this Joint object.
+    public: uint64_t SensorCount() const;
+
+    /// \brief Get a sensor based on an index.
+    /// \param[in] _index Index of the sensor. The index should be in the
+    /// range [0..SensorCount()).
+    /// \return Pointer to the sensor. Nullptr if the index does not exist.
+    /// \sa uint64_t SensorCount() const
+    public: const Sensor *SensorByIndex(const uint64_t _index) const;
+
+    /// \brief Get whether a sensor name exists.
+    /// \param[in] _name Name of the sensor to check.
+    /// \return True if there exists a sensor with the given name.
+    public: bool SensorNameExists(const std::string &_name) const;
+
+    /// \brief Get a sensor based on a name.
+    /// \param[in] _name Name of the sensor.
+    /// \return Pointer to the sensor. Nullptr if a sensor with the given name
+    ///  does not exist.
+    /// \sa bool SensorNameExists(const std::string &_name) const
+    public: const Sensor *SensorByName(const std::string &_name) const;
+
     /// \brief Give the scoped FrameAttachedToGraph to be used for resolving
     /// parent and child link names. This is private and is intended to be
     /// called by Model::Load.

src/Joint.cc

@@ -23,6 +23,7 @@
 #include "sdf/Error.hh"
 #include "sdf/Joint.hh"
 #include "sdf/JointAxis.hh"
+#include "sdf/Sensor.hh"
 #include "sdf/Types.hh"
 #include "FrameSemantics.hh"
 #include "ScopedGraph.hh"
@@ -65,6 +66,9 @@ class sdf::Joint::Implementation
 
   /// \brief Scoped Pose Relative-To graph at the parent model scope.
   public: sdf::ScopedGraph<sdf::PoseRelativeToGraph> poseRelativeToGraph;
+
+  /// \brief The sensors specified in this joint.
+  public: std::vector<Sensor> sensors;
 };
 
 /////////////////////////////////////////////////
@@ -215,6 +219,10 @@ Errors Joint::Load(ElementPtr _sdf)
         "A joint type is required, but is not set."});
   }
 
+  // Load all the sensors.
+  Errors sensorLoadErrors = loadUniqueRepeated<Sensor>(_sdf, "sensor",
+      this->dataPtr->sensors);
+  errors.insert(errors.end(), sensorLoadErrors.begin(), sensorLoadErrors.end());
   return errors;
 }
 
@@ -405,3 +413,43 @@ sdf::ElementPtr Joint::Element() const
 {
   return this->dataPtr->sdf;
 }
+
+/////////////////////////////////////////////////
+uint64_t Joint::SensorCount() const
+{
+  return this->dataPtr->sensors.size();
+}
+
+/////////////////////////////////////////////////
+const Sensor *Joint::SensorByIndex(const uint64_t _index) const
+{
+  if (_index < this->dataPtr->sensors.size())
+    return &this->dataPtr->sensors[_index];
+  return nullptr;
+}
+
+/////////////////////////////////////////////////
+bool Joint::SensorNameExists(const std::string &_name) const
+{
+  for (auto const &s : this->dataPtr->sensors)
+  {
+    if (s.Name() == _name)
+    {
+      return true;
+    }
+  }
+  return false;
+}
+
+/////////////////////////////////////////////////
+const Sensor *Joint::SensorByName(const std::string &_name) const
+{
+  for (auto const &s : this->dataPtr->sensors)
+  {
+    if (s.Name() == _name)
+    {
+      return &s;
+    }
+  }
+  return nullptr;
+}

src/Joint_TEST.cc

@@ -104,6 +104,13 @@ TEST(DOMJoint, Construction)
   const double threadPitch = 0.1;
   joint.SetThreadPitch(threadPitch);
   EXPECT_DOUBLE_EQ(threadPitch, joint.ThreadPitch());
+
+  EXPECT_EQ(0u, joint.SensorCount());
+  EXPECT_EQ(nullptr, joint.SensorByIndex(0));
+  EXPECT_EQ(nullptr, joint.SensorByIndex(1));
+  EXPECT_EQ(nullptr, joint.SensorByName("empty"));
+  EXPECT_FALSE(joint.SensorNameExists(""));
+  EXPECT_FALSE(joint.SensorNameExists("default"));
 }
 
 /////////////////////////////////////////////////

test/integration/joint_dom.cc

@@ -20,13 +20,15 @@
 
 #include "sdf/Element.hh"
 #include "sdf/Filesystem.hh"
+#include "sdf/ForceTorque.hh"
 #include "sdf/Frame.hh"
 #include "sdf/Joint.hh"
 #include "sdf/JointAxis.hh"
 #include "sdf/Link.hh"
 #include "sdf/Model.hh"
 #include "sdf/Root.hh"
 #include "sdf/SDFImpl.hh"
+#include "sdf/Sensor.hh"
 #include "sdf/Types.hh"
 #include "sdf/parser.hh"
 #include "test_config.h"
@@ -857,3 +859,43 @@ TEST(DOMJoint, LoadJointNestedParentChild)
     EXPECT_EQ(Pose(0, -1, 1, IGN_PI_2, 0, 0), pose);
   }
 }
+
+
+//////////////////////////////////////////////////
+TEST(DOMJoint, Sensors)
+{
+  const std::string testFile =
+      sdf::testing::TestFile("sdf", "joint_sensors.sdf");
+
+  // Load the SDF file
+  sdf::Root root;
+  auto errors = root.Load(testFile);
+  EXPECT_TRUE(errors.empty()) << errors;
+
+  // Get the first model
+  const sdf::Model *model = root.Model();
+  ASSERT_NE(nullptr, model);
+  EXPECT_EQ("model", model->Name());
+
+  // Get the first joint
+  const sdf::Joint *joint = model->JointByIndex(0);
+  ASSERT_NE(nullptr, joint);
+  EXPECT_EQ("joint", joint->Name());
+  EXPECT_EQ(1u, joint->SensorCount());
+
+  ignition::math::Pose3d pose;
+
+  // Get the force_torque sensor
+  const sdf::Sensor *forceTorqueSensor =
+    joint->SensorByName("force_torque_sensor");
+  ASSERT_NE(nullptr, forceTorqueSensor);
+  EXPECT_EQ("force_torque_sensor", forceTorqueSensor->Name());
+  EXPECT_EQ(sdf::SensorType::FORCE_TORQUE, forceTorqueSensor->Type());
+  EXPECT_EQ(ignition::math::Pose3d(10, 11, 12, 0, 0, 0),
+      forceTorqueSensor->RawPose());
+  auto forceTorqueSensorConfig = forceTorqueSensor->ForceTorqueSensor();
+  ASSERT_NE(nullptr, forceTorqueSensorConfig);
+  EXPECT_EQ(sdf::ForceTorqueFrame::PARENT, forceTorqueSensorConfig->Frame());
+  EXPECT_EQ(sdf::ForceTorqueMeasureDirection::PARENT_TO_CHILD,
+      forceTorqueSensorConfig->MeasureDirection());
+}