diff --git a/include/ignition/sensors/ForceTorqueSensor.hh b/include/ignition/sensors/ForceTorqueSensor.hh new file mode 100644 index 00000000..303b51dc --- /dev/null +++ b/include/ignition/sensors/ForceTorqueSensor.hh @@ -0,0 +1,127 @@ +/* + * 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. + * +*/ + +#ifndef IGNITION_SENSORS_FORCETORQUESENSOR_HH_ +#define IGNITION_SENSORS_FORCETORQUESENSOR_HH_ + +#include + +#include + +#include +#include + +#include + +#include +#include + +#include "ignition/sensors/Sensor.hh" + +namespace ignition +{ + namespace sensors + { + // Inline bracket to help doxygen filtering. + inline namespace IGNITION_SENSORS_VERSION_NAMESPACE { + // + /// \brief forward declarations + class ForceTorqueSensorPrivate; + + /// \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 + { + /// \brief constructor + public: ForceTorqueSensor(); + + /// \brief destructor + public: virtual ~ForceTorqueSensor(); + + /// \brief Load the sensor based on data from an sdf::Sensor object. + /// \param[in] _sdf SDF Sensor parameters. + /// \return true if loading was successful + public: virtual bool Load(const sdf::Sensor &_sdf) override; + + /// \brief Load the sensor with SDF parameters. + /// \param[in] _sdf SDF Sensor parameters. + /// \return true if loading was successful + public: virtual bool Load(sdf::ElementPtr _sdf) override; + + /// \brief Initialize values in the sensor + /// \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 Update( + const std::chrono::steady_clock::duration &_now) override; + + /// \brief Get the current joint force. + /// \return The latest measured force. + public: math::Vector3d Force() const; + + /// \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 latest measured torque. + public: math::Vector3d Torque() const; + + /// \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 + /// \internal + private: std::unique_ptr dataPtr; + IGN_COMMON_WARN_RESUME__DLL_INTERFACE_MISSING + }; + } + } +} + +#endif diff --git a/include/ignition/sensors/SensorTypes.hh b/include/ignition/sensors/SensorTypes.hh index 479e671b..875ccb07 100644 --- a/include/ignition/sensors/SensorTypes.hh +++ b/include/ignition/sensors/SensorTypes.hh @@ -149,6 +149,30 @@ namespace ignition /// \sa Lidar LIDAR_NOISE = 14, + /// \brief Force body-frame X axis noise in N + /// \sa ForceTorqueSensor + FORCE_X_NOISE_N = 15, + + /// \brief Force body-frame Y axis noise in N + /// \sa ForceTorqueSensor + FORCE_Y_NOISE_N = 16, + + /// \brief Force body-frame Z axis noise in N + /// \sa ForceTorqueSensor + FORCE_Z_NOISE_N = 17, + + /// \brief Torque body-frame X axis noise in Nm + /// \sa ForceTorqueSensor + TORQUE_X_NOISE_N_M = 18, + + /// \brief Torque body-frame Y axis noise in Nm + /// \sa ForceTorqueSensor + TORQUE_Y_NOISE_N_M = 19, + + /// \brief Torque body-frame Z axis noise in Nm + /// \sa ForceTorqueSensor + TORQUE_Z_NOISE_N_M = 20, + /// \internal /// \brief Indicator used to create an iterator over the enum. Do not /// use this. diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index 263a1589..e6408671 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -111,6 +111,9 @@ ign_add_component(magnetometer SOURCES ${magnetometer_sources} GET_TARGET_NAME m set(imu_sources ImuSensor.cc) ign_add_component(imu SOURCES ${imu_sources} GET_TARGET_NAME imu_target) +set(force_torque_sources ForceTorqueSensor.cc) +ign_add_component(force_torque SOURCES ${force_torque_sources} GET_TARGET_NAME force_torque_target) + set(altimeter_sources AltimeterSensor.cc) ign_add_component(altimeter SOURCES ${altimeter_sources} GET_TARGET_NAME altimeter_target) diff --git a/src/ForceTorqueSensor.cc b/src/ForceTorqueSensor.cc new file mode 100644 index 00000000..8cb4d636 --- /dev/null +++ b/src/ForceTorqueSensor.cc @@ -0,0 +1,314 @@ +/* + * 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. + * +*/ + +#ifdef _WIN32 +#pragma warning(push) +#pragma warning(disable: 4005) +#pragma warning(disable: 4251) +#endif +#include +#ifdef _WIN32 +#pragma warning(pop) +#endif + +#include +#include + +#include "ignition/sensors/ForceTorqueSensor.hh" +#include "ignition/sensors/Noise.hh" +#include "ignition/sensors/SensorFactory.hh" +#include "ignition/sensors/SensorTypes.hh" + +using namespace ignition; +using namespace sensors; + +/// \brief Private data for ForceTorqueSensor +class ignition::sensors::ForceTorqueSensorPrivate +{ + /// \brief node to create publisher + public: transport::Node node; + + /// \brief publisher to publish Wrench messages. + public: transport::Node::Publisher pub; + + /// \brief true if Load() has been called and was successful + public: bool initialized = false; + + /// \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 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 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; + + /// \brief Previous update time step. + public: std::chrono::steady_clock::duration prevStep + {std::chrono::steady_clock::duration::zero()}; + + /// \brief Noise added to sensor data + public: std::map noises; +}; + +////////////////////////////////////////////////// +ForceTorqueSensor::ForceTorqueSensor() + : dataPtr(std::make_unique()) +{ +} + +////////////////////////////////////////////////// +ForceTorqueSensor::~ForceTorqueSensor() = default; + +////////////////////////////////////////////////// +bool ForceTorqueSensor::Init() +{ + return this->Sensor::Init(); +} + +////////////////////////////////////////////////// +bool ForceTorqueSensor::Load(const sdf::Sensor &_sdf) +{ + if (!Sensor::Load(_sdf)) + return false; + + // Check if this is the right type + if (_sdf.Type() != sdf::SensorType::FORCE_TORQUE) + { + ignerr << "Attempting to a load a Force Torque sensor, but received " + << "a " << _sdf.TypeStr() << std::endl; + } + + if (_sdf.ForceTorqueSensor() == nullptr) + { + ignerr << "Attempting to a load a Force Torque sensor, but received " + << "a null sensor." << std::endl; + return false; + } + + this->dataPtr->measureFrame = _sdf.ForceTorqueSensor()->Frame(); + this->dataPtr->measureDirection = + _sdf.ForceTorqueSensor()->MeasureDirection(); + + if (this->Topic().empty()) + this->SetTopic("/forcetorque"); + + this->dataPtr->pub = + this->dataPtr->node.Advertise(this->Topic()); + + if (!this->dataPtr->pub) + { + ignerr << "Unable to create publisher on topic[" << this->Topic() << "].\n"; + return false; + } + + const std::map noises = { + {FORCE_X_NOISE_N, _sdf.ForceTorqueSensor()->ForceXNoise()}, + {FORCE_Y_NOISE_N, _sdf.ForceTorqueSensor()->ForceYNoise()}, + {FORCE_Z_NOISE_N, _sdf.ForceTorqueSensor()->ForceZNoise()}, + {TORQUE_X_NOISE_N_M, _sdf.ForceTorqueSensor()->TorqueXNoise()}, + {TORQUE_Y_NOISE_N_M, _sdf.ForceTorqueSensor()->TorqueYNoise()}, + {TORQUE_Z_NOISE_N_M, _sdf.ForceTorqueSensor()->TorqueZNoise()}, + }; + + for (const auto & [noiseType, noiseSdf] : noises) + { + if (noiseSdf.Type() != sdf::NoiseType::NONE) + { + this->dataPtr->noises[noiseType] = NoiseFactory::NewNoiseModel(noiseSdf); + } + } + + this->dataPtr->initialized = true; + return true; +} + +////////////////////////////////////////////////// +bool ForceTorqueSensor::Load(sdf::ElementPtr _sdf) +{ + sdf::Sensor sdfSensor; + sdfSensor.Load(_sdf); + return this->Load(sdfSensor); +} + +////////////////////////////////////////////////// +bool ForceTorqueSensor::Update(const std::chrono::steady_clock::duration &_now) +{ + IGN_PROFILE("ForceTorqueSensor::Update"); + if (!this->dataPtr->initialized) + { + ignerr << "Not initialized, update ignored.\n"; + return false; + } + + // If time has gone backwards, reinitialize. + if (_now < this->dataPtr->prevStep) + { + this->dataPtr->timeInitialized = false; + } + + // Only compute dt if time is initialized and increasing. + double dt; + if (this->dataPtr->timeInitialized) + { + auto delay = std::chrono::duration_cast>( + _now - this->dataPtr->prevStep); + dt = delay.count(); + } + else + { + dt = 0.0; + } + // Get the force and torque in the appropriate frame. + ignition::math::Vector3d measuredForce; + ignition::math::Vector3d measuredTorque; + + if (this->dataPtr->measureFrame == sdf::ForceTorqueFrame::PARENT) + { + measuredForce = + this->dataPtr->rotationParentInSensor.Inverse() * this->dataPtr->force; + measuredTorque = + this->dataPtr->rotationParentInSensor.Inverse() * this->dataPtr->torque; + } + else if (this->dataPtr->measureFrame == sdf::ForceTorqueFrame::CHILD) + { + measuredForce = + this->dataPtr->rotationChildInSensor.Inverse() * this->dataPtr->force; + measuredTorque = + this->dataPtr->rotationChildInSensor.Inverse() * this->dataPtr->torque; + } + else if (this->dataPtr->measureFrame == sdf::ForceTorqueFrame::SENSOR) + { + measuredForce = this->dataPtr->force; + measuredTorque = this->dataPtr->torque; + } + else + { + ignerr << "measureFrame must be PARENT_LINK, CHILD_LINK or SENSOR\n"; + } + + 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()) + { + 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); + + // publish + this->AddSequence(msg.mutable_header()); + this->dataPtr->pub.Publish(msg); + this->dataPtr->prevStep = _now; + this->dataPtr->timeInitialized = true; + return true; +} + +////////////////////////////////////////////////// +math::Vector3d ForceTorqueSensor::Force() const +{ + return this->dataPtr->force; +} + +////////////////////////////////////////////////// +void ForceTorqueSensor::SetForce(const math::Vector3d &_force) +{ + this->dataPtr->force = _force; +} + +////////////////////////////////////////////////// +math::Vector3d ForceTorqueSensor::Torque() const +{ + return this->dataPtr->torque; +} + +////////////////////////////////////////////////// +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; +} + +////////////////////////////////////////////////// +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 acca1b8c..31e8acee 100644 --- a/test/integration/CMakeLists.txt +++ b/test/integration/CMakeLists.txt @@ -12,6 +12,7 @@ set(dri_tests set(tests air_pressure.cc altimeter.cc + force_torque.cc logical_camera.cc magnetometer.cc imu.cc @@ -53,5 +54,6 @@ ign_build_tests(TYPE INTEGRATION ${PROJECT_LIBRARY_TARGET_NAME}-logical_camera ${PROJECT_LIBRARY_TARGET_NAME}-magnetometer ${PROJECT_LIBRARY_TARGET_NAME}-imu + ${PROJECT_LIBRARY_TARGET_NAME}-force_torque ) diff --git a/test/integration/air_pressure.cc b/test/integration/air_pressure.cc index 347b2649..f0670319 100644 --- a/test/integration/air_pressure.cc +++ b/test/integration/air_pressure.cc @@ -170,7 +170,6 @@ TEST_F(AirPressureSensorTest, SensorReadings) 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; auto sensor = sf.CreateSensor( airPressureSdf); diff --git a/test/integration/altimeter.cc b/test/integration/altimeter.cc index 653f07df..f568d185 100644 --- a/test/integration/altimeter.cc +++ b/test/integration/altimeter.cc @@ -176,7 +176,6 @@ TEST_F(AltimeterSensorTest, SensorReadings) 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; auto sensor = sf.CreateSensor(altimeterSdf); diff --git a/test/integration/force_torque.cc b/test/integration/force_torque.cc new file mode 100644 index 00000000..b28b3ebf --- /dev/null +++ b/test/integration/force_torque.cc @@ -0,0 +1,309 @@ +/* + * 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 std::string &_frame, + const std::string &_measureDir, + 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 << "" + << " " + << " " << _frame << "" + << " " << _measureDir << "" + << " " + << " " + << " " + << " " << _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::TestWithParam> +{ + // 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 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, "child", "child_to_parent", {}, {}, {}, {}, + 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_P(ForceTorqueSensorTest, SensorReadings) +{ + namespace math = ignition::math; + + std::string frame, measureDirection; + std::tie(frame, measureDirection) = GetParam(); + + // Create SDF describing a a force torque sensor + const std::string name = "TestForceTorqueSensor"; + const std::string topic = "/ignition/sensors/test/force_torque"; + 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, frame, measureDirection, forceNoiseMean, + forceNoiseStddev, torqueNoiseMean, torqueNoiseStddev, + forcetorqueSdf); + + ASSERT_NE(nullptr, forcetorqueSdf); + + // create the sensor using sensor factory + 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)); + + // Set rotation of child + const math::Quaterniond rotChildInSensor{ + math::Vector3d{IGN_PI_4, IGN_PI_2, 0}}; + const math::Quaterniond rotParentInSensor{ + math::Vector3d{0, IGN_PI_4, IGN_PI_4}}; + + sensor->SetRotationChildInSensor(rotChildInSensor); + EXPECT_EQ(rotChildInSensor, sensor->RotationChildInSensor()); + sensor->SetRotationParentInSensor(rotParentInSensor); + EXPECT_EQ(rotParentInSensor, sensor->RotationParentInSensor()); + + 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()); + + if (measureDirection == "parent_to_child") + { + if (frame == "child") + { + EXPECT_EQ((rotChildInSensor.Inverse() * force) + forceNoiseMean, + ignition::msgs::Convert(msg.force())); + EXPECT_EQ((rotChildInSensor.Inverse() * torque) + torqueNoiseMean, + ignition::msgs::Convert(msg.torque())); + } + else if (frame == "parent") + { + EXPECT_EQ((rotParentInSensor.Inverse() * force) + forceNoiseMean, + ignition::msgs::Convert(msg.force())); + EXPECT_EQ((rotParentInSensor.Inverse() * torque) + torqueNoiseMean, + ignition::msgs::Convert(msg.torque())); + } + else + { + EXPECT_EQ(force + forceNoiseMean, ignition::msgs::Convert(msg.force())); + EXPECT_EQ(torque + torqueNoiseMean, + ignition::msgs::Convert(msg.torque())); + } + } + else + { + if (frame == "child") + { + EXPECT_EQ(-(rotChildInSensor.Inverse() * force) + forceNoiseMean, + ignition::msgs::Convert(msg.force())); + EXPECT_EQ(-(rotChildInSensor.Inverse() * torque) + torqueNoiseMean, + ignition::msgs::Convert(msg.torque())); + } + else if (frame == "parent") + { + EXPECT_EQ(-(rotParentInSensor.Inverse() * force) + forceNoiseMean, + ignition::msgs::Convert(msg.force())); + EXPECT_EQ(-(rotParentInSensor.Inverse() * torque) + torqueNoiseMean, + ignition::msgs::Convert(msg.torque())); + } + else + { + 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()); +} + +INSTANTIATE_TEST_CASE_P( + FrameAndDirection, ForceTorqueSensorTest, + ::testing::Combine(::testing::Values("child", "parent", "sensor"), + ::testing::Values("parent_to_child", + "child_to_parent")), ); + +int main(int argc, char **argv) +{ + ::testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/test/integration/imu.cc b/test/integration/imu.cc index 43c6a991..2b630443 100644 --- a/test/integration/imu.cc +++ b/test/integration/imu.cc @@ -111,7 +111,6 @@ TEST_F(ImuSensorTest, SensorReadings) updateRate, topic, alwaysOn, visualize); // 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(imuSdf); ASSERT_NE(nullptr, sensor); diff --git a/test/integration/magnetometer.cc b/test/integration/magnetometer.cc index a96fe766..4ba223f8 100644 --- a/test/integration/magnetometer.cc +++ b/test/integration/magnetometer.cc @@ -181,7 +181,6 @@ TEST_F(MagnetometerSensorTest, SensorReadings) sensorPose, updateRate, noiseTopic, 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; auto sensor = sf.CreateSensor( magnetometerSdf);