(Backport) Joint DOM API to access joint sensors (#517)

* Add Joint DOM API to access joint sensors

Signed-off-by: Jenn Nguyen <[email protected]>
Co-authored-by: Jenn Nguyen <[email protected]>

include/sdf/Joint.hh

@@ -36,6 +36,7 @@ namespace sdf
   class JointAxis;
   class JointPrivate;
   struct PoseRelativeToGraph;
+  class Sensor;
 
   /// \enum JointType
   /// \brief The set of joint types. INVALID indicates that joint type has
@@ -240,6 +241,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 a weak pointer to the PoseRelativeToGraph to be used
     /// for resolving poses. 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 "Utils.hh"
 
@@ -75,6 +76,9 @@ class sdf::JointPrivate
 
   /// \brief Weak pointer to model's Pose Relative-To Graph.
   public: std::weak_ptr<const sdf::PoseRelativeToGraph> poseRelativeToGraph;
+
+  /// \brief The sensors specified in this joint.
+  public: std::vector<Sensor> sensors;
 };
 
 /////////////////////////////////////////////////
@@ -266,6 +270,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;
 }
 
@@ -421,3 +429,36 @@ 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
+{
+  return nullptr != this->SensorByName(_name);
+}
+
+/////////////////////////////////////////////////
+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

@@ -98,6 +98,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,12 +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"
@@ -572,3 +575,47 @@ TEST(DOMJoint, LoadURDFJointPoseRelativeTo)
     model->JointByName("joint12")->Axis()->ResolveXyz(vec3, "joint12").empty());
   EXPECT_EQ(Vector3(0, 1.0, 0), vec3);
 }
+
+//////////////////////////////////////////////////
+TEST(DOMJoint, Sensors)
+{
+  const std::string testFile =
+    sdf::filesystem::append(PROJECT_SOURCE_PATH, "test", "sdf",
+        "joint_sensors.sdf");
+
+  // Load the SDF file
+  sdf::Root root;
+  auto errors = root.Load(testFile);
+  for (auto e : errors)
+    std::cout << e << std::endl;
+  EXPECT_TRUE(errors.empty());
+
+  using Pose = ignition::math::Pose3d;
+
+  // Get the first model
+  const sdf::Model *model = root.ModelByIndex(0);
+  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());
+}

test/sdf/joint_sensors.sdf

@@ -0,0 +1,18 @@
+<?xml version="1.0" ?>
+<sdf version="1.7">
+  <model name="model">
+    <link name="link1"/>
+    <link name="link2"/>
+    <joint name="joint" type="fixed">
+      <parent>link1</parent>
+      <child>link2</child>
+      <sensor name="force_torque_sensor" type="force_torque">
+        <pose>10 11 12 0 0 0</pose>
+        <force_torque>
+          <frame>parent</frame>
+          <measure_direction>parent_to_child</measure_direction>
+        </force_torque>
+      </sensor>
+    </joint>
+  </model>
+</sdf>