Add Joint DOM API to access joint sensors (gazebosim#517)

* Add Joint DOM API to access joint sensors Signed-off-by: Addisu Z. Taddese <[email protected]> * Add missing file Signed-off-by: Addisu Z. Taddese <[email protected]> * modified Joint::SensorNameExists Signed-off-by: Jenn Nguyen <[email protected]> Co-authored-by: Jenn Nguyen <[email protected]>
azeey · Oct 27, 2021 · 675ff8c · 675ff8c
1 parent 6fc2f6e
commit 675ff8c
Show file tree

Hide file tree

Showing 5 changed files with 135 additions and 0 deletions.
diff --git a/include/sdf/Joint.hh b/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.

diff --git a/src/Joint.cc b/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"
 
@@ -74,6 +75,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;
 };
 
 /////////////////////////////////////////////////
@@ -265,6 +269,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;
 }
 
@@ -420,3 +428,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;
+}
diff --git a/src/Joint_TEST.cc b/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"));
 }
 
 /////////////////////////////////////////////////

diff --git a/test/integration/joint_dom.cc b/test/integration/joint_dom.cc
@@ -20,12 +20,14 @@
 
 #include "sdf/Element.hh"
 #include "sdf/Filesystem.hh"
+#include "sdf/ForceTorque.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"
@@ -564,3 +566,46 @@ 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::testing::TestFile("sdf", "joint_sensors.sdf");
+
+  // Load the SDF file
+  sdf::Root root;
+  auto errors = root.Load(testFile);
+  EXPECT_TRUE(errors.empty());
+  for (const auto &err : errors)
+  {
+    std::cout << err << std::endl;
+  }
+
+  // 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());
+}
diff --git a/test/sdf/joint_sensors.sdf b/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>