From 6a75fa6d0028750571179054e878fb2b17557325 Mon Sep 17 00:00:00 2001 From: "Addisu Z. Taddese" Date: Wed, 17 Mar 2021 18:46:55 -0500 Subject: [PATCH 1/3] Add Joint DOM API to access joint sensors Signed-off-by: Addisu Z. Taddese --- include/sdf/Joint.hh | 24 ++++++++++++++++++ src/Joint.cc | 48 +++++++++++++++++++++++++++++++++++ src/Joint_TEST.cc | 7 +++++ test/integration/joint_dom.cc | 42 ++++++++++++++++++++++++++++++ 4 files changed, 121 insertions(+) diff --git a/include/sdf/Joint.hh b/include/sdf/Joint.hh index c53d54e19..fc1eabaa6 100644 --- a/include/sdf/Joint.hh +++ b/include/sdf/Joint.hh @@ -38,6 +38,7 @@ namespace sdf struct FrameAttachedToGraph; struct PoseRelativeToGraph; template 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. diff --git a/src/Joint.cc b/src/Joint.cc index 227cbf1ba..387ba3309 100644 --- 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 "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 poseRelativeToGraph; + + /// \brief The sensors specified in this joint. + public: std::vector 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(_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; +} diff --git a/src/Joint_TEST.cc b/src/Joint_TEST.cc index 26a5f7359..65d3cebca 100644 --- a/src/Joint_TEST.cc +++ b/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")); } ///////////////////////////////////////////////// diff --git a/test/integration/joint_dom.cc b/test/integration/joint_dom.cc index 08c417257..d983dc9e2 100644 --- a/test/integration/joint_dom.cc +++ b/test/integration/joint_dom.cc @@ -20,6 +20,7 @@ #include "sdf/Element.hh" #include "sdf/Filesystem.hh" +#include "sdf/ForceTorque.hh" #include "sdf/Frame.hh" #include "sdf/Joint.hh" #include "sdf/JointAxis.hh" @@ -27,6 +28,7 @@ #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()); +} From 8d50882e74804f3dc899dc2892ff90b63475446b Mon Sep 17 00:00:00 2001 From: "Addisu Z. Taddese" Date: Wed, 17 Mar 2021 20:29:40 -0500 Subject: [PATCH 2/3] Add missing file Signed-off-by: Addisu Z. Taddese --- test/sdf/joint_sensors.sdf | 18 ++++++++++++++++++ 1 file changed, 18 insertions(+) create mode 100644 test/sdf/joint_sensors.sdf diff --git a/test/sdf/joint_sensors.sdf b/test/sdf/joint_sensors.sdf new file mode 100644 index 000000000..16b63c89b --- /dev/null +++ b/test/sdf/joint_sensors.sdf @@ -0,0 +1,18 @@ + + + + + + + link1 + link2 + + 10 11 12 0 0 0 + + parent + parent_to_child + + + + + From cf387a4f63b5773b5958ae44456050160347368b Mon Sep 17 00:00:00 2001 From: Jenn Nguyen Date: Wed, 24 Mar 2021 14:35:19 -0700 Subject: [PATCH 3/3] modified Joint::SensorNameExists Signed-off-by: Jenn Nguyen --- src/Joint.cc | 9 +-------- 1 file changed, 1 insertion(+), 8 deletions(-) diff --git a/src/Joint.cc b/src/Joint.cc index 387ba3309..114aecab1 100644 --- a/src/Joint.cc +++ b/src/Joint.cc @@ -431,14 +431,7 @@ const Sensor *Joint::SensorByIndex(const uint64_t _index) const ///////////////////////////////////////////////// bool Joint::SensorNameExists(const std::string &_name) const { - for (auto const &s : this->dataPtr->sensors) - { - if (s.Name() == _name) - { - return true; - } - } - return false; + return nullptr != this->SensorByName(_name); } /////////////////////////////////////////////////