From 675ff8c668f3bc5505d8574c51134af1d61365ea Mon Sep 17 00:00:00 2001 From: "Addisu Z. Taddese" Date: Wed, 24 Mar 2021 17:21:07 -0500 Subject: [PATCH] Add Joint DOM API to access joint sensors (#517) * Add Joint DOM API to access joint sensors Signed-off-by: Addisu Z. Taddese * Add missing file Signed-off-by: Addisu Z. Taddese * modified Joint::SensorNameExists Signed-off-by: Jenn Nguyen Co-authored-by: Jenn Nguyen --- include/sdf/Joint.hh | 24 +++++++++++++++++++ src/Joint.cc | 41 +++++++++++++++++++++++++++++++ src/Joint_TEST.cc | 7 ++++++ test/integration/joint_dom.cc | 45 +++++++++++++++++++++++++++++++++++ test/sdf/joint_sensors.sdf | 18 ++++++++++++++ 5 files changed, 135 insertions(+) create mode 100644 test/sdf/joint_sensors.sdf diff --git a/include/sdf/Joint.hh b/include/sdf/Joint.hh index a9fd3bb3f..4a97d12e8 100644 --- 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 index 78731090d..f33e8e90d 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 "Utils.hh" @@ -74,6 +75,9 @@ class sdf::JointPrivate /// \brief Weak pointer to model's Pose Relative-To Graph. public: std::weak_ptr poseRelativeToGraph; + + /// \brief The sensors specified in this joint. + public: std::vector 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(_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 index 1f1fc6100..fae9c7ad4 100644 --- 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 index 379b644b1..bc8ac2868 100644 --- 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 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 + + + + +