From 725290098fa4f8f4ffc17f805000806e02ab667a Mon Sep 17 00:00:00 2001 From: "Addisu Z. Taddese" Date: Tue, 21 Sep 2021 22:11:36 -0500 Subject: [PATCH] Fix tests, add rotations to parent and child --- include/ignition/sensors/ForceTorqueSensor.hh | 46 ++- src/ForceTorqueSensor.cc | 171 ++++++---- test/integration/CMakeLists.txt | 2 +- test/integration/force_torque.cc | 285 ++++++++++++++++ test/integration/force_torque_plugin.cc | 311 ------------------ 5 files changed, 414 insertions(+), 401 deletions(-) create mode 100644 test/integration/force_torque.cc delete mode 100644 test/integration/force_torque_plugin.cc diff --git a/include/ignition/sensors/ForceTorqueSensor.hh b/include/ignition/sensors/ForceTorqueSensor.hh index 7e81876a..80868d76 100644 --- a/include/ignition/sensors/ForceTorqueSensor.hh +++ b/include/ignition/sensors/ForceTorqueSensor.hh @@ -45,8 +45,8 @@ namespace ignition /// \brief Force Torque Sensor Class /// /// A force-torque Sensor that reports force and torque applied on a joint. - - class IGNITION_SENSORS_FORCE_TORQUE_VISIBLE ForceTorqueSensor : public Sensor + class IGNITION_SENSORS_FORCE_TORQUE_VISIBLE ForceTorqueSensor + : public Sensor { /// \brief constructor public: ForceTorqueSensor(); @@ -68,37 +68,51 @@ namespace ignition /// \return True on success public: virtual bool Init() override; - // /// \brief Update the sensor and generate data - // /// \param[in] _now The current time - // /// \return true if the update was successfull - // public: virtual bool IGN_DEPRECATED(4) Update( - // const ignition::common::Time &_now) override; - /// \brief Update the sensor and generate data /// \param[in] _now The current time /// \return true if the update was successfull public: virtual bool Update( const std::chrono::steady_clock::duration &_now) override; - public: bool Update( - const ignition::common::Time &_now); - /// \brief Get the current joint force. - /// \return The latested measured force. + /// \return The latest measured force. public: math::Vector3d Force() const; - /// \brief Set the force vector. + /// \brief Set the force vector in sensor frame and where the force is + /// applied on the child (parent-to-child) /// \param[in] _force force vector in newton. public: void SetForce(const math::Vector3d &_force); /// \brief Get the current joint torque. - /// \return The latested measured torque. + /// \return The latest measured torque. public: math::Vector3d Torque() const; - /// \brief Set the torque vector. + /// \brief Set the torque vector in sensor frame and where the torque is + /// applied on the child (parent-to-child) /// \param[in] _torque torque vector in newton. public: void SetTorque(const math::Vector3d &_torque); - + + /// \brief Set the rotation of the joint parent relative to the sensor + /// frame. + /// \return The current rotation the parent in the sensor frame. + public: math::Quaterniond RotationParentInSensor() const; + + /// \brief Set the rotation of the joint parent relative to the sensor + /// frame. + /// \param[in] _rotParentInSensorRotation of parent in sensor frame. + public: void SetRotationParentInSensor( + const math::Quaterniond &_rotParentInSensor); + + /// \brief Set the rotation of the joint parent relative to the sensor + /// frame. + /// \return The current rotation the child in the sensor frame. + public: math::Quaterniond RotationChildInSensor() const; + + /// \brief Set the rotation of the joint child relative to the sensor + /// frame. + /// \param[in] _rotChildInSensor Rotation of child in sensor frame. + public: void SetRotationChildInSensor( + const math::Quaterniond &_rotChildInSensor); IGN_COMMON_WARN_IGNORE__DLL_INTERFACE_MISSING /// \brief Data pointer for private data diff --git a/src/ForceTorqueSensor.cc b/src/ForceTorqueSensor.cc index d5a13aa3..4de1a252 100644 --- a/src/ForceTorqueSensor.cc +++ b/src/ForceTorqueSensor.cc @@ -48,22 +48,33 @@ class ignition::sensors::ForceTorqueSensorPrivate /// \brief true if Load() has been called and was successful public: bool initialized = false; - /// \brief Noise free force - public: ignition::math::Vector3d force; + /// \brief Noise free force as set by SetForce + public: ignition::math::Vector3d force{0, 0, 0}; + + /// \brief Noise free torque as set by SetTorque + public: ignition::math::Vector3d torque{0, 0, 0}; - /// \brief Noise free torque - public: ignition::math::Vector3d torque; - /// \brief Frame in which we return the measured force torque info. public: sdf::ForceTorqueFrame measureFrame; /// \brief Direction in which we return the measured force torque info. public: sdf::ForceTorqueMeasureDirection measureDirection; - /// \brief Rotation matrix than transforms a vector expressed in child - /// orientation in a vector expressed in joint orientation. - /// Necessary is the measure is specified in joint frame. - public: ignition::math::Matrix3d rotationSensorChild; + /// \brief Rotation matrix that transforms a vector expressed in the parent + /// frame to a vector expressed in the sensor frame. + /// \note We store the rotation as a 3x3 matrix because matrix-vector + /// product is than quaternion-vector when there are a lot of vectors and the + /// rotation is not changing frequently. + public: ignition::math::Matrix3d rotationParentInSensor{ + ignition::math::Matrix3d::Identity}; + + /// \brief Rotation matrix that transforms a vector expressed in the child + /// frame to a vector expressed in the sensor frame. + /// \note We store the rotation as a 3x3 matrix because matrix-vector + /// product is than quaternion-vector when there are a lot of vectors and the + /// rotation is not changing frequently. + public: ignition::math::Matrix3d rotationChildInSensor{ + ignition::math::Matrix3d::Identity}; /// \brief Flag for if time has been initialized public: bool timeInitialized = false; @@ -83,9 +94,7 @@ ForceTorqueSensor::ForceTorqueSensor() } ////////////////////////////////////////////////// -ForceTorqueSensor::~ForceTorqueSensor() -{ -} +ForceTorqueSensor::~ForceTorqueSensor() = default; ////////////////////////////////////////////////// bool ForceTorqueSensor::Init() @@ -114,7 +123,8 @@ bool ForceTorqueSensor::Load(const sdf::Sensor &_sdf) } this->dataPtr->measureFrame = _sdf.ForceTorqueSensor()->Frame(); - this->dataPtr->measureDirection = _sdf.ForceTorqueSensor()->MeasureDirection(); + this->dataPtr->measureDirection = + _sdf.ForceTorqueSensor()->MeasureDirection(); if (this->Topic().empty()) this->SetTopic("/forcetorque"); @@ -157,13 +167,6 @@ bool ForceTorqueSensor::Load(sdf::ElementPtr _sdf) return this->Load(sdfSensor); } -////////////////////////////////////////////////// -bool ForceTorqueSensor::Update( - const ignition::common::Time &_now) -{ - return this->Update(math::secNsecToDuration(_now.sec, _now.nsec)); -} - ////////////////////////////////////////////////// bool ForceTorqueSensor::Update(const std::chrono::steady_clock::duration &_now) { @@ -192,78 +195,77 @@ bool ForceTorqueSensor::Update(const std::chrono::steady_clock::duration &_now) { dt = 0.0; } - - // Convenience method to apply noise to a channel, if present. - auto applyNoise = [&](SensorNoiseType noiseType, double & value) - { - if (this->dataPtr->noises.find(noiseType) != this->dataPtr->noises.end()) { - value = this->dataPtr->noises[noiseType]->Apply(value, dt); - } - }; - - applyNoise(FORCE_X_NOISE_N, this->dataPtr->force.X()); - applyNoise(FORCE_Y_NOISE_N, this->dataPtr->force.Y()); - applyNoise(FORCE_Z_NOISE_N, this->dataPtr->force.Z()); - applyNoise(TORQUE_X_NOISE_N_M, this->dataPtr->torque.X()); - applyNoise(TORQUE_Y_NOISE_N_M, this->dataPtr->torque.Y()); - applyNoise(TORQUE_Z_NOISE_N_M, this->dataPtr->torque.Z()); - - msgs::Wrench msg; - *msg.mutable_header()->mutable_stamp() = msgs::Convert(_now); - auto frame = msg.mutable_header()->add_data(); - frame->set_key("frame_id"); - frame->add_value(this->Name()); - // Get the force and torque in the appropriate frame. ignition::math::Vector3d measuredForce; ignition::math::Vector3d measuredTorque; if (this->dataPtr->measureFrame == sdf::ForceTorqueFrame::PARENT) { - if (this->dataPtr->measureDirection == sdf::ForceTorqueMeasureDirection::PARENT_TO_CHILD) - { - measuredForce = this->dataPtr->force; - measuredTorque = this->dataPtr->torque; - } - else - { - measuredForce = -1*this->dataPtr->force; - measuredTorque = -1*this->dataPtr->torque; - } + measuredForce = + this->dataPtr->rotationParentInSensor.Inverse() * this->dataPtr->force; + measuredTorque = + this->dataPtr->rotationParentInSensor.Inverse() * this->dataPtr->torque; + } else if (this->dataPtr->measureFrame == sdf::ForceTorqueFrame::CHILD) { - if (this->dataPtr->measureDirection == sdf::ForceTorqueMeasureDirection::CHILD_TO_PARENT) + measuredForce = + this->dataPtr->rotationChildInSensor.Inverse() * this->dataPtr->force; + measuredTorque = + this->dataPtr->rotationChildInSensor.Inverse() * this->dataPtr->torque; + + if (this->dataPtr->measureDirection == + sdf::ForceTorqueMeasureDirection::CHILD_TO_PARENT) { - measuredForce = this->dataPtr->force; - measuredTorque = this->dataPtr->torque; - ignerr << "Warning: ForceTorqueSensor::Update() " << std::endl; + measuredForce *= -1; + measuredTorque *= -1; } - else + } + else if (this->dataPtr->measureFrame == sdf::ForceTorqueFrame::SENSOR) + { + measuredForce = this->dataPtr->force; + measuredTorque = this->dataPtr->torque; + + if (this->dataPtr->measureDirection == + sdf::ForceTorqueMeasureDirection::CHILD_TO_PARENT) { - measuredForce = -1*this->dataPtr->force; - measuredTorque = -1*this->dataPtr->torque; + measuredForce *= -1; + measuredTorque *= -1; } } else { ignerr << "measureFrame must be PARENT_LINK, CHILD_LINK or SENSOR\n"; + } - if (this->dataPtr->measureDirection == sdf::ForceTorqueMeasureDirection::CHILD_TO_PARENT) - { - measuredForce = this->dataPtr->rotationSensorChild * - this->dataPtr->force; - measuredTorque = this->dataPtr->rotationSensorChild * - this->dataPtr->torque; - } - else + if (this->dataPtr->measureDirection == + sdf::ForceTorqueMeasureDirection::CHILD_TO_PARENT) + { + measuredForce *= -1; + measuredTorque *= -1; + } + + // Convenience method to apply noise to a channel, if present. + auto applyNoise = [&](SensorNoiseType noiseType, double &value) + { + if (this->dataPtr->noises.find(noiseType) != this->dataPtr->noises.end()) { - measuredForce = this->dataPtr->rotationSensorChild * - (-1*this->dataPtr->force); - measuredTorque = this->dataPtr->rotationSensorChild * - (-1*this->dataPtr->torque); + value = this->dataPtr->noises[noiseType]->Apply(value, dt); } - } + }; + + applyNoise(FORCE_X_NOISE_N, measuredForce.X()); + applyNoise(FORCE_Y_NOISE_N, measuredForce.Y()); + applyNoise(FORCE_Z_NOISE_N, measuredForce.Z()); + applyNoise(TORQUE_X_NOISE_N_M, measuredTorque.X()); + applyNoise(TORQUE_Y_NOISE_N_M, measuredTorque.Y()); + applyNoise(TORQUE_Z_NOISE_N_M, measuredTorque.Z()); + + msgs::Wrench msg; + *msg.mutable_header()->mutable_stamp() = msgs::Convert(_now); + auto frame = msg.mutable_header()->add_data(); + frame->set_key("frame_id"); + frame->add_value(this->Name()); msgs::Set(msg.mutable_force(), measuredForce); msgs::Set(msg.mutable_torque(), measuredTorque); @@ -300,5 +302,28 @@ void ForceTorqueSensor::SetTorque(const math::Vector3d &_torque) this->dataPtr->torque = _torque; } +////////////////////////////////////////////////// +math::Quaterniond ForceTorqueSensor::RotationParentInSensor() const +{ + return math::Quaterniond(this->dataPtr->rotationParentInSensor); +} + +////////////////////////////////////////////////// +void ForceTorqueSensor::SetRotationParentInSensor( + const math::Quaterniond &_rotParentInSensor) +{ + this->dataPtr->rotationParentInSensor = _rotParentInSensor; +} -IGN_SENSORS_REGISTER_SENSOR(ForceTorqueSensor) +////////////////////////////////////////////////// +void ForceTorqueSensor::SetRotationChildInSensor( + const math::Quaterniond &_rotChildInSensor) +{ + this->dataPtr->rotationChildInSensor = _rotChildInSensor; +} + +////////////////////////////////////////////////// +math::Quaterniond ForceTorqueSensor::RotationChildInSensor() const +{ + return math::Quaterniond(this->dataPtr->rotationChildInSensor); +} diff --git a/test/integration/CMakeLists.txt b/test/integration/CMakeLists.txt index 587f381d..c289f1c8 100644 --- a/test/integration/CMakeLists.txt +++ b/test/integration/CMakeLists.txt @@ -11,10 +11,10 @@ set(dri_tests set(tests air_pressure.cc altimeter.cc + force_torque.cc logical_camera.cc magnetometer.cc imu.cc - force_torque_plugin.cc ) link_directories(${PROJECT_BINARY_DIR}/test) diff --git a/test/integration/force_torque.cc b/test/integration/force_torque.cc new file mode 100644 index 00000000..4b36f674 --- /dev/null +++ b/test/integration/force_torque.cc @@ -0,0 +1,285 @@ +/* + * Copyright (C) 2021 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ + +#include + +#include + +#include + +#include +#include + +#include "test_config.h" // NOLINT(build/include) +#include "TransportTestTools.hh" + +/// \brief Helper function to create a force torque sensor sdf element. +void CreateForceTorqueToSdf(const std::string &_name, + const ignition::math::Pose3d &_pose, + const double _updateRate, const std::string &_topic, + const bool _alwaysOn, const bool _visualize, + const ignition::math::Vector3d &_forceNoiseMean, + const ignition::math::Vector3d &_forceNoiseStddev, + const ignition::math::Vector3d &_torqueNoiseMean, + const ignition::math::Vector3d &_torqueNoiseStddev, + sdf::ElementPtr &_sensorSdf) +{ + std::ostringstream stream; + stream + << "" + << "" + << " " + << " " + << " " + << " " + << " link1" + << " link2" + << " " + << " " << _pose << "" + << " " << _topic << "" + << " "<< _updateRate <<"" + << " " << _alwaysOn <<"" + << " " << _visualize << "" + << " " + << " child" + << " child_to_parent" + << " " + << " " + << " " + << " " << _forceNoiseMean.X() << "" + << " " << _forceNoiseStddev.X() << "" + << " " + << " " + << " " + << " " + << " " << _forceNoiseMean.Y() << "" + << " " << _forceNoiseStddev.Y() << "" + << " " + << " " + << " " + << " " + << " " << _forceNoiseMean.Z() << "" + << " " << _forceNoiseStddev.Z() << "" + << " " + << " " + << " " + << " " + << " " + << " " + << " " << _torqueNoiseMean.X() << "" + << " " << _torqueNoiseStddev.X() << "" + << " " + << " " + << " " + << " " + << " " << _torqueNoiseMean.Y() << "" + << " " << _torqueNoiseStddev.Y() << "" + << " " + << " " + << " " + << " " + << " " << _torqueNoiseMean.Z() << "" + << " " << _torqueNoiseStddev.Z() << "" + << " " + << " " + << " " + << " " + << " " + << " " + << " " + << ""; + + sdf::Root root; + sdf::Errors errors = root.LoadSdfString(stream.str()); + if (errors.empty()) + { + auto model = root.Model(); + ASSERT_NE(nullptr, model); + auto joint = model->JointByIndex(0); + ASSERT_NE(nullptr, joint); + auto sensor = joint->SensorByIndex(0); + ASSERT_NE(nullptr, sensor); + _sensorSdf = sensor->Element(); + } + else + { + _sensorSdf = nullptr; + } +} + +/// \brief Test force torque sensor +class ForceTorqueSensorTest: public testing::Test +{ + // Documentation inherited + protected: void SetUp() override + { + ignition::common::Console::SetVerbosity(4); + } +}; + +///////////////////////////////////////////////// +TEST_F(ForceTorqueSensorTest, CreateForceTorqueSensor) +{ + // Create SDF describing a force torque sensor + const std::string name = "TestForceTorqueSensor"; + const std::string topic = "/ignition/sensors/test/force_torque"; + + // const std::string topicNoise = "/ignition/sensors/test/altimeter_noise"; + const double updateRate = 30; + const bool alwaysOn = 1; + const bool visualize = 1; + + // Create sensor SDF + ignition::math::Pose3d sensorPose(ignition::math::Vector3d(0.25, 0.0, 0.5), + ignition::math::Quaterniond::Identity); + sdf::ElementPtr forcetorqueSdf; + CreateForceTorqueToSdf(name, sensorPose, updateRate, topic, alwaysOn, + visualize, {}, {}, {}, {}, forcetorqueSdf); + + ASSERT_NE(nullptr, forcetorqueSdf); + + // create the sensor using sensor factory + ignition::sensors::SensorFactory sf; + auto sensor = + sf.CreateSensor(forcetorqueSdf); + ASSERT_NE(nullptr, sensor); + + EXPECT_EQ(name, sensor->Name()); + EXPECT_EQ(topic, sensor->Topic()); + EXPECT_DOUBLE_EQ(updateRate, sensor->UpdateRate()); +} + +///////////////////////////////////////////////// +TEST_F(ForceTorqueSensorTest, SensorReadings) +{ + namespace math = ignition::math; + + // Create SDF describing a a force torque sensor + const std::string name = "TestForceTorqueSensor"; + const std::string topic = "/ignition/sensors/test/force_torque"; + // const std::string topicNoise = "/ignition/sensors/test/altimeter_noise"; + const double updateRate = 30; + const bool alwaysOn = 1; + const bool visualize = 1; + + const math::Vector3d forceNoiseMean{0.1, 0.2, 0.3}; + // Set the stddev to 0s so we can test the mean values + const math::Vector3d forceNoiseStddev{0.0, 0.0, 0.0}; + + const math::Vector3d torqueNoiseMean{0.5, 0.6, 0.7}; + // Set the stddev to 0s so we can test the mean values + const math::Vector3d torqueNoiseStddev{0.0, 0.0, 0.0}; + + // Create sensor SDF + math::Pose3d sensorPose{math::Vector3d(0.25, 0.0, 0.5), + math::Quaterniond::Identity}; + sdf::ElementPtr forcetorqueSdf; + CreateForceTorqueToSdf(name, sensorPose, updateRate, topic, alwaysOn, + visualize, forceNoiseMean, forceNoiseStddev, + torqueNoiseMean, torqueNoiseStddev, forcetorqueSdf); + + ASSERT_NE(nullptr, forcetorqueSdf); + + // create the sensor using sensor factory + // try creating without specifying the sensor type and then cast it + ignition::sensors::SensorFactory sf; + auto sensor = + sf.CreateSensor(forcetorqueSdf); + + ASSERT_NE(nullptr, sensor); + + // verify initial readings + EXPECT_EQ(math::Vector3d::Zero, sensor->Force()); + EXPECT_EQ(math::Vector3d::Zero, sensor->Torque()); + EXPECT_EQ(math::Quaterniond::Identity, sensor->RotationParentInSensor()); + EXPECT_EQ(math::Quaterniond::Identity, sensor->RotationChildInSensor()); + + // set state and verify readings + math::Vector3d force{0, 0, 1}; + sensor->SetForce(force); + EXPECT_EQ(force, sensor->Force()); + + math::Vector3d torque{0, 1, 0}; + sensor->SetTorque(torque); + EXPECT_EQ(torque, sensor->Torque()); + + // verify msg received on the topic + WaitForMessageTestHelper msgHelper(topic); + auto dt = std::chrono::steady_clock::duration(std::chrono::seconds(1)); + { + sensor->Update(dt); + EXPECT_TRUE(msgHelper.WaitForMessage()) << msgHelper; + auto msg = msgHelper.Message(); + EXPECT_EQ(1, msg.header().stamp().sec()); + EXPECT_EQ(0, msg.header().stamp().nsec()); + + EXPECT_EQ(force + forceNoiseMean, ignition::msgs::Convert(msg.force())); + EXPECT_EQ(torque + torqueNoiseMean, ignition::msgs::Convert(msg.torque())); + + // The Force() and Torque() functions return the noise-free forces and + // torques in the sensor frame + EXPECT_EQ(force, sensor->Force()); + EXPECT_EQ(torque, sensor->Torque()); + } + + // Set rotation of child + const math::Quaterniond rotChildInSensor{ + math::Vector3d{IGN_PI_4, IGN_PI_2, 0}}; + sensor->SetRotationChildInSensor(rotChildInSensor); + EXPECT_EQ(rotChildInSensor, sensor->RotationChildInSensor()); + { + sensor->Update(dt); + EXPECT_TRUE(msgHelper.WaitForMessage()) << msgHelper; + auto msg = msgHelper.Message(); + + EXPECT_EQ((rotChildInSensor.Inverse() * force) + forceNoiseMean, + ignition::msgs::Convert(msg.force())); + EXPECT_EQ((rotChildInSensor.Inverse() * torque) + torqueNoiseMean, + ignition::msgs::Convert(msg.torque())); + // The Force() and Torque() functions return the noise-free forces and + // torques in the sensor frame + EXPECT_EQ(force, sensor->Force()); + EXPECT_EQ(torque, sensor->Torque()); + } + + // Set rotation of parent. This should not affect the ouptut because the + // measurement frame is "child". + const math::Quaterniond rotParentInSensor{math::Vector3d{0.1, 0.2, 0.3}}; + sensor->SetRotationParentInSensor(rotParentInSensor); + EXPECT_EQ(rotParentInSensor, sensor->RotationParentInSensor()); + { + sensor->Update(dt); + EXPECT_TRUE(msgHelper.WaitForMessage()) << msgHelper; + auto msg = msgHelper.Message(); + + // Using rotChildInSensor since the measurement frame is still "child" + EXPECT_EQ((rotChildInSensor.Inverse() * force) + forceNoiseMean, + ignition::msgs::Convert(msg.force())); + EXPECT_EQ((rotChildInSensor.Inverse() * torque) + torqueNoiseMean, + ignition::msgs::Convert(msg.torque())); + // The Force() and Torque() functions return the noise-free forces and + // torques in the sensor frame + EXPECT_EQ(force, sensor->Force()); + EXPECT_EQ(torque, sensor->Torque()); + } +} + +int main(int argc, char **argv) +{ + ::testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/test/integration/force_torque_plugin.cc b/test/integration/force_torque_plugin.cc deleted file mode 100644 index a83fbc94..00000000 --- a/test/integration/force_torque_plugin.cc +++ /dev/null @@ -1,311 +0,0 @@ -/* - * Copyright (C) 2021 Open Source Robotics Foundation - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - * -*/ - -#include - -#include - -#include - -#include -#include - -#include "test_config.h" // NOLINT(build/include) -#include "TransportTestTools.hh" - -/// \brief Helper function to create a force torque sdf element. -sdf::ElementPtr ForceTorqueToSdf(const std::string &_name, - const ignition::math::Pose3d &_pose, const double _updateRate, - const std::string &_topic, const bool _alwaysOn, - const bool _visualize) -{ - std::ostringstream stream; - stream - << "" - << "" - << " " - << " " - << " " - << " " << _pose << "" - << " " << _topic << "" - << " "<< _updateRate <<"" - << " " << _alwaysOn <<"" - << " " << _visualize << "" - << " " - << " " - << " " - << ""; - - sdf::SDFPtr sdfParsed(new sdf::SDF()); - sdf::init(sdfParsed); - if (!sdf::readString(stream.str(), sdfParsed)) - return sdf::ElementPtr(); - - return sdfParsed->Root()->GetElement("model")->GetElement("link") - ->GetElement("sensor"); -} - -// /// \brief Helper function to create an altimeter sdf element with noise -// sdf::ElementPtr AltimeterToSdfWithNoise(const std::string &_name, -// const ignition::math::Pose3d &_pose, const double _updateRate, -// const std::string &_topic, const bool _alwaysOn, -// const bool _visualize, double _mean, double _stddev, double _bias) -// { -// std::ostringstream stream; -// stream -// << "" -// << "" -// << " " -// << " " -// << " " -// << " " << _pose << "" -// << " " << _topic << "" -// << " "<< _updateRate <<"" -// << " " << _alwaysOn <<"" -// << " " << _visualize << "" -// << " " -// << " " -// << " " -// << " " << _mean << "" -// << " " << _stddev << "" -// << " " << _bias << "" -// << " " -// << " " -// << " " -// << " " -// << " " << _mean << "" -// << " " << _stddev << "" -// << " " << _bias << "" -// << " " -// << " " -// << " " -// << " " -// << " " -// << " " -// << ""; - -// sdf::SDFPtr sdfParsed(new sdf::SDF()); -// sdf::init(sdfParsed); -// if (!sdf::readString(stream.str(), sdfParsed)) -// return sdf::ElementPtr(); - -// return sdfParsed->Root()->GetElement("model")->GetElement("link") -// ->GetElement("sensor"); -// } - -/// \brief Test force torque sensor -class ForceTorqueSensorTest: public testing::Test -{ - // Documentation inherited - protected: void SetUp() override - { - ignition::common::Console::SetVerbosity(4); - } -}; - -///////////////////////////////////////////////// -TEST_F(ForceTorqueSensorTest, CreateForceTorqueSensor) -{ - // Create SDF describing a force torque sensor - const std::string name = "TestForceTorqueSensor"; - const std::string topic = "/ignition/sensors/test/force_torque"; - - // const std::string topicNoise = "/ignition/sensors/test/altimeter_noise"; - const double updateRate = 30; - const bool alwaysOn = 1; - const bool visualize = 1; - - // Create sensor SDF - ignition::math::Pose3d sensorPose(ignition::math::Vector3d(0.25, 0.0, 0.5), - ignition::math::Quaterniond::Identity); - sdf::ElementPtr forcetorqueSdf = ForceTorqueToSdf(name, sensorPose, - updateRate, topic, alwaysOn, visualize); - - // sdf::ElementPtr altimeterSdfNoise = AltimeterToSdfWithNoise(name, sensorPose, - // updateRate, topicNoise, alwaysOn, visualize, 1.0, 0.2, 10.0); - - // create the sensor using sensor factory - ignition::sensors::SensorFactory sf; - std::unique_ptr sensor = - sf.CreateSensor(forcetorqueSdf); - ASSERT_NE(nullptr, sensor); - - EXPECT_EQ(name, sensor->Name()); - EXPECT_EQ(topic, sensor->Topic()); - EXPECT_DOUBLE_EQ(updateRate, sensor->UpdateRate()); - - // std::unique_ptr sensorNoise = - // sf.CreateSensor(altimeterSdfNoise); - // ASSERT_NE(nullptr, sensorNoise); - - // EXPECT_EQ(name, sensorNoise->Name()); - // EXPECT_EQ(topicNoise, sensorNoise->Topic()); - // EXPECT_DOUBLE_EQ(updateRate, sensorNoise->UpdateRate()); -} - -// ///////////////////////////////////////////////// -TEST_F(ForceTorqueSensorTest, SensorReadings) -{ - // Create SDF describing a a force torque sensor - const std::string name = "TestForceTorqueSensor"; - const std::string topic = "/ignition/sensors/test/force_torque"; - // const std::string topicNoise = "/ignition/sensors/test/altimeter_noise"; - const double updateRate = 30; - const bool alwaysOn = 1; - const bool visualize = 1; - - // Create sensor SDF - ignition::math::Pose3d sensorPose(ignition::math::Vector3d(0.25, 0.0, 0.5), - ignition::math::Quaterniond::Identity); - sdf::ElementPtr forcetorqueSdf = ForceTorqueToSdf(name, sensorPose, - updateRate, topic, alwaysOn, visualize); - - // sdf::ElementPtr altimeterSdfNoise = AltimeterToSdfWithNoise(name, sensorPose, - // updateRate, topicNoise, alwaysOn, visualize, 1.0, 0.2, 10.0); - - // create the sensor using sensor factory - // try creating without specifying the sensor type and then cast it - ignition::sensors::SensorFactory sf; - std::unique_ptr s = - sf.CreateSensor(forcetorqueSdf); - std::unique_ptr sensor( - dynamic_cast(s.release())); - - // Make sure the above dynamic cast worked. - ASSERT_NE(nullptr, sensor); - - // std::unique_ptr sNoise = - // sf.CreateSensor(altimeterSdfNoise); - // std::unique_ptr sensorNoise( - // dynamic_cast(sNoise.release())); - - // Make sure the above dynamic cast worked. - // ASSERT_NE(nullptr, sensorNoise); - - // verify initial readings - EXPECT_EQ(ignition::math::Vector3d::Zero, sensor->Force()); - EXPECT_EQ(ignition::math::Vector3d::Zero, sensor->Torque()); - - // verify initial readings - // EXPECT_DOUBLE_EQ(0.0, sensorNoise->force()); - // EXPECT_DOUBLE_EQ(0.0, sensorNoise->torque()); - - // set state and verify readings - - // TODO : Add Noise tests - ignition::math::Vector3d force(0,0,1); - sensor->SetForce(force); - EXPECT_EQ(force, sensor->Force()); - - ignition::math::Vector3d torque(0,0,1); - sensor->SetTorque(torque); - EXPECT_EQ(torque, sensor->Torque()); - - - // double pos = 2.0; - // sensor->SetPosition(pos); - // sensorNoise->SetPosition(pos); - // EXPECT_DOUBLE_EQ(pos - vertRef, sensor->VerticalPosition()); - // EXPECT_DOUBLE_EQ(pos - vertRef, sensorNoise->VerticalPosition()); - - // verify msg received on the topic - WaitForMessageTestHelper msgHelper(topic); - sensor->Update(std::chrono::steady_clock::duration(std::chrono::seconds(1))); - EXPECT_TRUE(msgHelper.WaitForMessage()) << msgHelper; - auto msg = msgHelper.Message(); - EXPECT_EQ(1, msg.header().stamp().sec()); - EXPECT_EQ(0, msg.header().stamp().nsec()); - - EXPECT_EQ(ignition::math::Vector3d(0, 0, 1), ignition::msgs::Convert(msg.force())); - EXPECT_EQ(ignition::math::Vector3d(0, 0, 1), ignition::msgs::Convert(msg.torque())); - - - // // verify msg with noise received on the topic - // WaitForMessageTestHelper - // msgHelperNoise(topicNoise); - // sensorNoise->Update(std::chrono::steady_clock::duration( - // std::chrono::seconds(1))); - // EXPECT_TRUE(msgHelperNoise.WaitForMessage()) << msgHelperNoise; - // auto msgNoise = msgHelperNoise.Message(); - // EXPECT_EQ(1, msg.header().stamp().sec()); - // EXPECT_EQ(0, msg.header().stamp().nsec()); - // EXPECT_DOUBLE_EQ(vertRef, msgNoise.vertical_reference()); - // EXPECT_FALSE(ignition::math::equal(pos - vertRef, - // msgNoise.vertical_position())); - // EXPECT_FALSE(ignition::math::equal(vertVel, msgNoise.vertical_velocity())); -} - -///////////////////////////////////////////////// -TEST_F(ForceTorqueSensorTest, Topic) -{ - const std::string name = "TestForceTorqueSensor"; - const double updateRate = 30; - const bool alwaysOn = 1; - const bool visualize = 1; - auto sensorPose = ignition::math::Pose3d(); - - // Factory - ignition::sensors::SensorFactory factory; - - // Default topic - { - const std::string topic; - auto forcetorqueSdf = ForceTorqueToSdf(name, sensorPose, - updateRate, topic, alwaysOn, visualize); - - auto sensor = factory.CreateSensor(forcetorqueSdf); - EXPECT_NE(nullptr, sensor); - - auto force_torque = - dynamic_cast(sensor.release()); - ASSERT_NE(nullptr, force_torque); - - EXPECT_EQ("/forcetorque", force_torque->Topic()); - } - - // Convert to valid topic - { - const std::string topic = "/topic with spaces/@~characters//"; - auto forcetorqueSdf = ForceTorqueToSdf(name, sensorPose, - updateRate, topic, alwaysOn, visualize); - - auto sensor = factory.CreateSensor(forcetorqueSdf); - EXPECT_NE(nullptr, sensor); - - auto force_torque = - dynamic_cast(sensor.release()); - ASSERT_NE(nullptr, force_torque); - - EXPECT_EQ("/topic_with_spaces/characters", force_torque->Topic()); - } - - // Invalid topic - { - const std::string topic = "@@@"; - auto forcetorqueSdf = ForceTorqueToSdf(name, sensorPose, - updateRate, topic, alwaysOn, visualize); - - auto sensor = factory.CreateSensor(forcetorqueSdf); - ASSERT_EQ(nullptr, sensor); - } -} - -int main(int argc, char **argv) -{ - ::testing::InitGoogleTest(&argc, argv); - return RUN_ALL_TESTS(); -}