diff --git a/source/RobotAPI/components/units/InertialMeasurementUnitObserver.cpp b/source/RobotAPI/components/units/InertialMeasurementUnitObserver.cpp index 3f387568df7d54c2b6c93ea27852269ff39ba562..e5ae05a54c63e5fd262a7c0025df9d54a7ffdf24 100644 --- a/source/RobotAPI/components/units/InertialMeasurementUnitObserver.cpp +++ b/source/RobotAPI/components/units/InertialMeasurementUnitObserver.cpp @@ -23,6 +23,7 @@ */ #include "InertialMeasurementUnitObserver.h" +#include <ArmarXCore/core/exceptions/local/ExpressionException.h> #include <ArmarXCore/observers/checks/ConditionCheckEquals.h> #include <ArmarXCore/observers/checks/ConditionCheckLarger.h> #include <ArmarXCore/observers/checks/ConditionCheckSmaller.h> @@ -66,7 +67,9 @@ void InertialMeasurementUnitObserver::onExitObserver() } } -void InertialMeasurementUnitObserver::reportSensorValues(const std::string& device, const std::string& name, const IMUData& values, const TimestampBasePtr& timestamp, const Ice::Current& c) +void InertialMeasurementUnitObserver::reportSensorValues( + const std::string& device, const std::string& name, const IMUData& values, + const TimestampBasePtr& timestamp, const Ice::Current&) { ScopedLock lock(dataMutex); diff --git a/source/RobotAPI/libraries/core/Pose.cpp b/source/RobotAPI/libraries/core/Pose.cpp index cf2ef51c61f126f2e7dd25a79d68937fb91b505d..f1370001de11c7ddabc21403e637215e5a050220 100644 --- a/source/RobotAPI/libraries/core/Pose.cpp +++ b/source/RobotAPI/libraries/core/Pose.cpp @@ -29,6 +29,9 @@ #include <VirtualRobot/LinkedCoordinate.h> #include <VirtualRobot/VirtualRobot.h> +#include <ArmarXCore/core/exceptions/local/ExpressionException.h> + + using namespace Eigen; using namespace std; @@ -79,7 +82,7 @@ namespace armarx y = vec[1]; } - string Vector2::output(const Ice::Current& c) const + string Vector2::output(const Ice::Current&) const { std::stringstream s; s << toEigen(); @@ -94,7 +97,7 @@ namespace armarx obj->setFloat("y", y); } - void Vector2::deserialize(const ObjectSerializerBasePtr& serializer, const ::Ice::Current& c) + void Vector2::deserialize(const ObjectSerializerBasePtr& serializer, const ::Ice::Current&) { AbstractObjectSerializerPtr obj = AbstractObjectSerializerPtr::dynamicCast(serializer); @@ -144,7 +147,7 @@ namespace armarx z = vec[2]; } - string Vector3::output(const Ice::Current& c) const + string Vector3::output(const Ice::Current&) const { std::stringstream s; s << toEigen().format(ArmarXEigenFormat); @@ -160,7 +163,7 @@ namespace armarx obj->setFloat("z", z); } - void Vector3::deserialize(const ObjectSerializerBasePtr& serializer, const ::Ice::Current& c) + void Vector3::deserialize(const ObjectSerializerBasePtr& serializer, const ::Ice::Current&) { AbstractObjectSerializerPtr obj = AbstractObjectSerializerPtr::dynamicCast(serializer); @@ -170,7 +173,7 @@ namespace armarx } - Quaternion::Quaternion() + Quaternion::Quaternion() : Quaternion(Eigen::Quaternionf::Identity()) { } @@ -200,13 +203,7 @@ namespace armarx Matrix3f Quaternion::toEigen() const { - Matrix3f rot; - rot = Quaternionf( - this->qw, - this->qx, - this->qy, - this->qz); - return rot; + return toEigenQuaternion().toRotationMatrix(); } Eigen::Quaternionf Quaternion::toEigenQuaternion() const @@ -244,7 +241,7 @@ namespace armarx return result; } - string Quaternion::output(const Ice::Current& c) const + string Quaternion::output(const Ice::Current&) const { std::stringstream s; s << toEigen()/*.format(ArmarXEigenFormat)*/; @@ -261,7 +258,7 @@ namespace armarx obj->setFloat("qw", qw); } - void Quaternion::deserialize(const ObjectSerializerBasePtr& serializer, const ::Ice::Current& c) + void Quaternion::deserialize(const ObjectSerializerBasePtr& serializer, const ::Ice::Current&) { AbstractObjectSerializerPtr obj = AbstractObjectSerializerPtr::dynamicCast(serializer); @@ -271,6 +268,14 @@ namespace armarx qw = obj->getFloat("qw"); } + + Pose::Pose() + { + this->orientation = new Quaternion(); + this->position = new Vector3(); + init(); + } + Pose::Pose(const Eigen::Matrix3f& m, const Eigen::Vector3f& v) { this->orientation = new Quaternion(m); @@ -285,13 +290,6 @@ namespace armarx init(); } - Pose::Pose() - { - this->orientation = new Quaternion(); - this->position = new Vector3(); - init(); - } - Pose::Pose(const Pose& source) : IceUtil::Shared(source), PoseBase(source) @@ -325,7 +323,7 @@ namespace armarx return m; } - string Pose::output(const Ice::Current& c) const + string Pose::output(const Ice::Current&) const { std::stringstream s; s << toEigen()/*.format(ArmarXEigenFormat)*/; @@ -339,7 +337,7 @@ namespace armarx orientation->serialize(serializer, c); } - void Pose::deserialize(const ObjectSerializerBasePtr& serializer, const ::Ice::Current& c) + void Pose::deserialize(const ObjectSerializerBasePtr& serializer, const ::Ice::Current&) { AbstractObjectSerializerPtr obj = AbstractObjectSerializerPtr::dynamicCast(serializer); diff --git a/source/RobotAPI/libraries/core/Pose.h b/source/RobotAPI/libraries/core/Pose.h index 6911f787073c25528b707b632444c638a55e766e..74be8b0fa09652a0b9db2dba733d5fdb93305a76 100644 --- a/source/RobotAPI/libraries/core/Pose.h +++ b/source/RobotAPI/libraries/core/Pose.h @@ -28,7 +28,6 @@ #include <ArmarXCore/observers/variant/Variant.h> #include <ArmarXCore/observers/AbstractObjectSerializer.h> -#include <ArmarXCore/core/exceptions/local/ExpressionException.h> #include <Eigen/Core> #include <Eigen/Geometry> @@ -70,16 +69,16 @@ namespace armarx { return this->clone(); } - VariantDataClassPtr clone(const Ice::Current& c = ::Ice::Current()) const override + VariantDataClassPtr clone(const Ice::Current& = ::Ice::Current()) const override { return new Vector2(*this); } std::string output(const Ice::Current& c = ::Ice::Current()) const override; - VariantTypeId getType(const Ice::Current& c = ::Ice::Current()) const override + VariantTypeId getType(const Ice::Current& = ::Ice::Current()) const override { return VariantType::Vector2; } - bool validate(const Ice::Current& c = ::Ice::Current()) override + bool validate(const Ice::Current& = ::Ice::Current()) override { return true; } @@ -127,11 +126,11 @@ namespace armarx return new Vector3(*this); } std::string output(const Ice::Current& c = ::Ice::Current()) const override; - VariantTypeId getType(const Ice::Current& c = ::Ice::Current()) const override + VariantTypeId getType(const Ice::Current& = ::Ice::Current()) const override { return VariantType::Vector3; } - bool validate(const Ice::Current& c = ::Ice::Current()) override + bool validate(const Ice::Current& = ::Ice::Current()) override { return true; } @@ -161,6 +160,8 @@ namespace armarx virtual public QuaternionBase { public: + + /// Construct an identity quaternion. Quaternion(); Quaternion(const Eigen::Matrix4f&); Quaternion(const Eigen::Matrix3f&); @@ -178,16 +179,16 @@ namespace armarx { return this->clone(); } - VariantDataClassPtr clone(const Ice::Current& c = ::Ice::Current()) const override + VariantDataClassPtr clone(const Ice::Current& = ::Ice::Current()) const override { return new Quaternion(*this); } std::string output(const Ice::Current& c = ::Ice::Current()) const override; - VariantTypeId getType(const Ice::Current& c = ::Ice::Current()) const override + VariantTypeId getType(const Ice::Current& = ::Ice::Current()) const override { return VariantType::Quaternion; } - bool validate(const Ice::Current& c = ::Ice::Current()) override + bool validate(const Ice::Current& = ::Ice::Current()) override { return true; } @@ -235,16 +236,16 @@ namespace armarx { return this->clone(); } - VariantDataClassPtr clone(const Ice::Current& c = ::Ice::Current()) const override + VariantDataClassPtr clone(const Ice::Current& = ::Ice::Current()) const override { return new Pose(*this); } - std::string output(const Ice::Current& c = ::Ice::Current()) const override; - VariantTypeId getType(const Ice::Current& c = ::Ice::Current()) const override + std::string output(const Ice::Current& = ::Ice::Current()) const override; + VariantTypeId getType(const Ice::Current& = ::Ice::Current()) const override { return VariantType::Pose; } - bool validate(const Ice::Current& c = ::Ice::Current()) override + bool validate(const Ice::Current& = ::Ice::Current()) override { return true; }