diff --git a/doc/release/master.md b/doc/release/master.md new file mode 100644 index 00000000000..d9b658504af --- /dev/null +++ b/doc/release/master.md @@ -0,0 +1,5 @@ +master {#master} +----------- + +* Branch changes +The angular acceleration and linear velocity values measured by a particular sensor can now be extracted and used via the sensor remapper. diff --git a/src/devices/messages/multipleAnalogSensorsMsgs/idl_generated_code/SensorRPCData.cpp b/src/devices/messages/multipleAnalogSensorsMsgs/idl_generated_code/SensorRPCData.cpp index c1e8ec0d177..80e8e0dcb9f 100644 --- a/src/devices/messages/multipleAnalogSensorsMsgs/idl_generated_code/SensorRPCData.cpp +++ b/src/devices/messages/multipleAnalogSensorsMsgs/idl_generated_code/SensorRPCData.cpp @@ -20,7 +20,9 @@ SensorRPCData::SensorRPCData(const std::vector& ThreeAxisGyrosco const std::vector& ContactLoadCellArrays, const std::vector& EncoderArrays, const std::vector& SkinPatches, - const std::vector& PositionSensors) : + const std::vector& PositionSensors, + const std::vector& LinearVelocitySensors, + const std::vector& ThreeAxisAngularAccelerometers) : WirePortable(), ThreeAxisGyroscopes(ThreeAxisGyroscopes), ThreeAxisLinearAccelerometers(ThreeAxisLinearAccelerometers), @@ -31,7 +33,9 @@ SensorRPCData::SensorRPCData(const std::vector& ThreeAxisGyrosco ContactLoadCellArrays(ContactLoadCellArrays), EncoderArrays(EncoderArrays), SkinPatches(SkinPatches), - PositionSensors(PositionSensors) + PositionSensors(PositionSensors), + LinearVelocitySensors(LinearVelocitySensors), + ThreeAxisAngularAccelerometers(ThreeAxisAngularAccelerometers) { } @@ -68,6 +72,12 @@ bool SensorRPCData::read(yarp::os::idl::WireReader& reader) if (!read_PositionSensors(reader)) { return false; } + if (!read_LinearVelocitySensors(reader)) { + return false; + } + if (!read_ThreeAxisAngularAccelerometers(reader)) { + return false; + } if (reader.isError()) { return false; } @@ -78,7 +88,7 @@ bool SensorRPCData::read(yarp::os::idl::WireReader& reader) bool SensorRPCData::read(yarp::os::ConnectionReader& connection) { yarp::os::idl::WireReader reader(connection); - if (!reader.readListHeader(10)) { + if (!reader.readListHeader(12)) { return false; } if (!read(reader)) { @@ -120,6 +130,12 @@ bool SensorRPCData::write(const yarp::os::idl::WireWriter& writer) const if (!write_PositionSensors(writer)) { return false; } + if (!write_LinearVelocitySensors(writer)) { + return false; + } + if (!write_ThreeAxisAngularAccelerometers(writer)) { + return false; + } if (writer.isError()) { return false; } @@ -130,7 +146,7 @@ bool SensorRPCData::write(const yarp::os::idl::WireWriter& writer) const bool SensorRPCData::write(yarp::os::ConnectionWriter& connection) const { yarp::os::idl::WireWriter writer(connection); - if (!writer.writeListHeader(10)) { + if (!writer.writeListHeader(12)) { return false; } if (!write(writer)) { @@ -1128,3 +1144,199 @@ bool SensorRPCData::nested_write_PositionSensors(const yarp::os::idl::WireWriter } return true; } + +// read LinearVelocitySensors field +bool SensorRPCData::read_LinearVelocitySensors(yarp::os::idl::WireReader& reader) +{ + if (reader.noMore()) { + reader.fail(); + return false; + } + size_t _csize; + yarp::os::idl::WireState _etype; + reader.readListBegin(_etype, _csize); + // WireReader removes BOTTLE_TAG_LIST from the tag + constexpr int expected_tag = ((BOTTLE_TAG_LIST) & (~BOTTLE_TAG_LIST)); + if constexpr (expected_tag != 0) { + if (_csize != 0 && _etype.code != expected_tag) { + return false; + } + } + LinearVelocitySensors.resize(_csize); + for (size_t _i = 0; _i < _csize; ++_i) { + if (reader.noMore()) { + reader.fail(); + return false; + } + if (!reader.readNested(LinearVelocitySensors[_i])) { + reader.fail(); + return false; + } + } + reader.readListEnd(); + return true; +} + +// write LinearVelocitySensors field +bool SensorRPCData::write_LinearVelocitySensors(const yarp::os::idl::WireWriter& writer) const +{ + if (!writer.writeListBegin(BOTTLE_TAG_LIST, LinearVelocitySensors.size())) { + return false; + } + for (const auto& _item : LinearVelocitySensors) { + if (!writer.writeNested(_item)) { + return false; + } + } + if (!writer.writeListEnd()) { + return false; + } + return true; +} + +// read (nested) LinearVelocitySensors field +bool SensorRPCData::nested_read_LinearVelocitySensors(yarp::os::idl::WireReader& reader) +{ + if (reader.noMore()) { + reader.fail(); + return false; + } + size_t _csize; + yarp::os::idl::WireState _etype; + reader.readListBegin(_etype, _csize); + // WireReader removes BOTTLE_TAG_LIST from the tag + constexpr int expected_tag = ((BOTTLE_TAG_LIST) & (~BOTTLE_TAG_LIST)); + if constexpr (expected_tag != 0) { + if (_csize != 0 && _etype.code != expected_tag) { + return false; + } + } + LinearVelocitySensors.resize(_csize); + for (size_t _i = 0; _i < _csize; ++_i) { + if (reader.noMore()) { + reader.fail(); + return false; + } + if (!reader.readNested(LinearVelocitySensors[_i])) { + reader.fail(); + return false; + } + } + reader.readListEnd(); + return true; +} + +// write (nested) LinearVelocitySensors field +bool SensorRPCData::nested_write_LinearVelocitySensors(const yarp::os::idl::WireWriter& writer) const +{ + if (!writer.writeListBegin(BOTTLE_TAG_LIST, LinearVelocitySensors.size())) { + return false; + } + for (const auto& _item : LinearVelocitySensors) { + if (!writer.writeNested(_item)) { + return false; + } + } + if (!writer.writeListEnd()) { + return false; + } + return true; +} + +// read ThreeAxisAngularAccelerometers field +bool SensorRPCData::read_ThreeAxisAngularAccelerometers(yarp::os::idl::WireReader& reader) +{ + if (reader.noMore()) { + reader.fail(); + return false; + } + size_t _csize; + yarp::os::idl::WireState _etype; + reader.readListBegin(_etype, _csize); + // WireReader removes BOTTLE_TAG_LIST from the tag + constexpr int expected_tag = ((BOTTLE_TAG_LIST) & (~BOTTLE_TAG_LIST)); + if constexpr (expected_tag != 0) { + if (_csize != 0 && _etype.code != expected_tag) { + return false; + } + } + ThreeAxisAngularAccelerometers.resize(_csize); + for (size_t _i = 0; _i < _csize; ++_i) { + if (reader.noMore()) { + reader.fail(); + return false; + } + if (!reader.readNested(ThreeAxisAngularAccelerometers[_i])) { + reader.fail(); + return false; + } + } + reader.readListEnd(); + return true; +} + +// write ThreeAxisAngularAccelerometers field +bool SensorRPCData::write_ThreeAxisAngularAccelerometers(const yarp::os::idl::WireWriter& writer) const +{ + if (!writer.writeListBegin(BOTTLE_TAG_LIST, ThreeAxisAngularAccelerometers.size())) { + return false; + } + for (const auto& _item : ThreeAxisAngularAccelerometers) { + if (!writer.writeNested(_item)) { + return false; + } + } + if (!writer.writeListEnd()) { + return false; + } + return true; +} + +// read (nested) ThreeAxisAngularAccelerometers field +bool SensorRPCData::nested_read_ThreeAxisAngularAccelerometers(yarp::os::idl::WireReader& reader) +{ + if (reader.noMore()) { + reader.fail(); + return false; + } + size_t _csize; + yarp::os::idl::WireState _etype; + reader.readListBegin(_etype, _csize); + // WireReader removes BOTTLE_TAG_LIST from the tag + constexpr int expected_tag = ((BOTTLE_TAG_LIST) & (~BOTTLE_TAG_LIST)); + if constexpr (expected_tag != 0) { + if (_csize != 0 && _etype.code != expected_tag) { + return false; + } + } + ThreeAxisAngularAccelerometers.resize(_csize); + for (size_t _i = 0; _i < _csize; ++_i) { + if (reader.noMore()) { + reader.fail(); + return false; + } + if (!reader.readNested(ThreeAxisAngularAccelerometers[_i])) { + reader.fail(); + return false; + } + } + reader.readListEnd(); + return true; +} + +// write (nested) ThreeAxisAngularAccelerometers field +bool SensorRPCData::nested_write_ThreeAxisAngularAccelerometers(const yarp::os::idl::WireWriter& writer) const +{ + if (!writer.writeListBegin(BOTTLE_TAG_LIST, ThreeAxisAngularAccelerometers.size())) { + return false; + } + for (const auto& _item : ThreeAxisAngularAccelerometers) { + if (!writer.writeNested(_item)) { + return false; + } + } + if (!writer.writeListEnd()) { + return false; + } + return true; +} diff --git a/src/devices/messages/multipleAnalogSensorsMsgs/idl_generated_code/SensorRPCData.h b/src/devices/messages/multipleAnalogSensorsMsgs/idl_generated_code/SensorRPCData.h index fe7054892e9..dd8b0558761 100644 --- a/src/devices/messages/multipleAnalogSensorsMsgs/idl_generated_code/SensorRPCData.h +++ b/src/devices/messages/multipleAnalogSensorsMsgs/idl_generated_code/SensorRPCData.h @@ -30,6 +30,8 @@ class SensorRPCData : std::vector EncoderArrays{}; std::vector SkinPatches{}; std::vector PositionSensors{}; + std::vector LinearVelocitySensors{}; + std::vector ThreeAxisAngularAccelerometers{}; // Default constructor SensorRPCData() = default; @@ -44,7 +46,9 @@ class SensorRPCData : const std::vector& ContactLoadCellArrays, const std::vector& EncoderArrays, const std::vector& SkinPatches, - const std::vector& PositionSensors); + const std::vector& PositionSensors, + const std::vector& LinearVelocitySensors, + const std::vector& ThreeAxisAngularAccelerometers); // Read structure on a Wire bool read(yarp::os::idl::WireReader& reader) override; @@ -124,6 +128,18 @@ class SensorRPCData : bool write_PositionSensors(const yarp::os::idl::WireWriter& writer) const; bool nested_read_PositionSensors(yarp::os::idl::WireReader& reader); bool nested_write_PositionSensors(const yarp::os::idl::WireWriter& writer) const; + + // read/write LinearVelocitySensors field + bool read_LinearVelocitySensors(yarp::os::idl::WireReader& reader); + bool write_LinearVelocitySensors(const yarp::os::idl::WireWriter& writer) const; + bool nested_read_LinearVelocitySensors(yarp::os::idl::WireReader& reader); + bool nested_write_LinearVelocitySensors(const yarp::os::idl::WireWriter& writer) const; + + // read/write ThreeAxisAngularAccelerometers field + bool read_ThreeAxisAngularAccelerometers(yarp::os::idl::WireReader& reader); + bool write_ThreeAxisAngularAccelerometers(const yarp::os::idl::WireWriter& writer) const; + bool nested_read_ThreeAxisAngularAccelerometers(yarp::os::idl::WireReader& reader); + bool nested_write_ThreeAxisAngularAccelerometers(const yarp::os::idl::WireWriter& writer) const; }; #endif // YARP_THRIFT_GENERATOR_STRUCT_SENSORRPCDATA_H diff --git a/src/devices/messages/multipleAnalogSensorsMsgs/idl_generated_code/SensorStreamingData.cpp b/src/devices/messages/multipleAnalogSensorsMsgs/idl_generated_code/SensorStreamingData.cpp index 4b81915717a..d9cb451debe 100644 --- a/src/devices/messages/multipleAnalogSensorsMsgs/idl_generated_code/SensorStreamingData.cpp +++ b/src/devices/messages/multipleAnalogSensorsMsgs/idl_generated_code/SensorStreamingData.cpp @@ -20,7 +20,9 @@ SensorStreamingData::SensorStreamingData(const SensorMeasurements& ThreeAxisGyro const SensorMeasurements& ContactLoadCellArrays, const SensorMeasurements& EncoderArrays, const SensorMeasurements& SkinPatches, - const SensorMeasurements& PositionSensors) : + const SensorMeasurements& PositionSensors, + const SensorMeasurements& LinearVelocitySensors, + const SensorMeasurements& ThreeAxisAngularAccelerometers) : WirePortable(), ThreeAxisGyroscopes(ThreeAxisGyroscopes), ThreeAxisLinearAccelerometers(ThreeAxisLinearAccelerometers), @@ -31,7 +33,9 @@ SensorStreamingData::SensorStreamingData(const SensorMeasurements& ThreeAxisGyro ContactLoadCellArrays(ContactLoadCellArrays), EncoderArrays(EncoderArrays), SkinPatches(SkinPatches), - PositionSensors(PositionSensors) + PositionSensors(PositionSensors), + LinearVelocitySensors(LinearVelocitySensors), + ThreeAxisAngularAccelerometers(ThreeAxisAngularAccelerometers) { } @@ -68,6 +72,12 @@ bool SensorStreamingData::read(yarp::os::idl::WireReader& reader) if (!read_PositionSensors(reader)) { return false; } + if (!read_LinearVelocitySensors(reader)) { + return false; + } + if (!read_ThreeAxisAngularAccelerometers(reader)) { + return false; + } if (reader.isError()) { return false; } @@ -78,7 +88,7 @@ bool SensorStreamingData::read(yarp::os::idl::WireReader& reader) bool SensorStreamingData::read(yarp::os::ConnectionReader& connection) { yarp::os::idl::WireReader reader(connection); - if (!reader.readListHeader(10)) { + if (!reader.readListHeader(12)) { return false; } if (!read(reader)) { @@ -120,6 +130,12 @@ bool SensorStreamingData::write(const yarp::os::idl::WireWriter& writer) const if (!write_PositionSensors(writer)) { return false; } + if (!write_LinearVelocitySensors(writer)) { + return false; + } + if (!write_ThreeAxisAngularAccelerometers(writer)) { + return false; + } if (writer.isError()) { return false; } @@ -130,7 +146,7 @@ bool SensorStreamingData::write(const yarp::os::idl::WireWriter& writer) const bool SensorStreamingData::write(yarp::os::ConnectionWriter& connection) const { yarp::os::idl::WireWriter writer(connection); - if (!writer.writeListHeader(10)) { + if (!writer.writeListHeader(12)) { return false; } if (!write(writer)) { @@ -608,3 +624,95 @@ bool SensorStreamingData::nested_write_PositionSensors(const yarp::os::idl::Wire } return true; } + +// read LinearVelocitySensors field +bool SensorStreamingData::read_LinearVelocitySensors(yarp::os::idl::WireReader& reader) +{ + if (reader.noMore()) { + reader.fail(); + return false; + } + if (!reader.read(LinearVelocitySensors)) { + reader.fail(); + return false; + } + return true; +} + +// write LinearVelocitySensors field +bool SensorStreamingData::write_LinearVelocitySensors(const yarp::os::idl::WireWriter& writer) const +{ + if (!writer.write(LinearVelocitySensors)) { + return false; + } + return true; +} + +// read (nested) LinearVelocitySensors field +bool SensorStreamingData::nested_read_LinearVelocitySensors(yarp::os::idl::WireReader& reader) +{ + if (reader.noMore()) { + reader.fail(); + return false; + } + if (!reader.readNested(LinearVelocitySensors)) { + reader.fail(); + return false; + } + return true; +} + +// write (nested) LinearVelocitySensors field +bool SensorStreamingData::nested_write_LinearVelocitySensors(const yarp::os::idl::WireWriter& writer) const +{ + if (!writer.writeNested(LinearVelocitySensors)) { + return false; + } + return true; +} + +// read ThreeAxisAngularAccelerometers field +bool SensorStreamingData::read_ThreeAxisAngularAccelerometers(yarp::os::idl::WireReader& reader) +{ + if (reader.noMore()) { + reader.fail(); + return false; + } + if (!reader.read(ThreeAxisAngularAccelerometers)) { + reader.fail(); + return false; + } + return true; +} + +// write ThreeAxisAngularAccelerometers field +bool SensorStreamingData::write_ThreeAxisAngularAccelerometers(const yarp::os::idl::WireWriter& writer) const +{ + if (!writer.write(ThreeAxisAngularAccelerometers)) { + return false; + } + return true; +} + +// read (nested) ThreeAxisAngularAccelerometers field +bool SensorStreamingData::nested_read_ThreeAxisAngularAccelerometers(yarp::os::idl::WireReader& reader) +{ + if (reader.noMore()) { + reader.fail(); + return false; + } + if (!reader.readNested(ThreeAxisAngularAccelerometers)) { + reader.fail(); + return false; + } + return true; +} + +// write (nested) ThreeAxisAngularAccelerometers field +bool SensorStreamingData::nested_write_ThreeAxisAngularAccelerometers(const yarp::os::idl::WireWriter& writer) const +{ + if (!writer.writeNested(ThreeAxisAngularAccelerometers)) { + return false; + } + return true; +} diff --git a/src/devices/messages/multipleAnalogSensorsMsgs/idl_generated_code/SensorStreamingData.h b/src/devices/messages/multipleAnalogSensorsMsgs/idl_generated_code/SensorStreamingData.h index 08f6398c3e5..01455b03de3 100644 --- a/src/devices/messages/multipleAnalogSensorsMsgs/idl_generated_code/SensorStreamingData.h +++ b/src/devices/messages/multipleAnalogSensorsMsgs/idl_generated_code/SensorStreamingData.h @@ -30,6 +30,8 @@ class SensorStreamingData : SensorMeasurements EncoderArrays{}; SensorMeasurements SkinPatches{}; SensorMeasurements PositionSensors{}; + SensorMeasurements LinearVelocitySensors{}; + SensorMeasurements ThreeAxisAngularAccelerometers{}; // Default constructor SensorStreamingData() = default; @@ -44,7 +46,9 @@ class SensorStreamingData : const SensorMeasurements& ContactLoadCellArrays, const SensorMeasurements& EncoderArrays, const SensorMeasurements& SkinPatches, - const SensorMeasurements& PositionSensors); + const SensorMeasurements& PositionSensors, + const SensorMeasurements& LinearVelocitySensors, + const SensorMeasurements& ThreeAxisAngularAccelerometers); // Read structure on a Wire bool read(yarp::os::idl::WireReader& reader) override; @@ -124,6 +128,18 @@ class SensorStreamingData : bool write_PositionSensors(const yarp::os::idl::WireWriter& writer) const; bool nested_read_PositionSensors(yarp::os::idl::WireReader& reader); bool nested_write_PositionSensors(const yarp::os::idl::WireWriter& writer) const; + + // read/write LinearVelocitySensors field + bool read_LinearVelocitySensors(yarp::os::idl::WireReader& reader); + bool write_LinearVelocitySensors(const yarp::os::idl::WireWriter& writer) const; + bool nested_read_LinearVelocitySensors(yarp::os::idl::WireReader& reader); + bool nested_write_LinearVelocitySensors(const yarp::os::idl::WireWriter& writer) const; + + // read/write ThreeAxisAngularAccelerometers field + bool read_ThreeAxisAngularAccelerometers(yarp::os::idl::WireReader& reader); + bool write_ThreeAxisAngularAccelerometers(const yarp::os::idl::WireWriter& writer) const; + bool nested_read_ThreeAxisAngularAccelerometers(yarp::os::idl::WireReader& reader); + bool nested_write_ThreeAxisAngularAccelerometers(const yarp::os::idl::WireWriter& writer) const; }; #endif // YARP_THRIFT_GENERATOR_STRUCT_SENSORSTREAMINGDATA_H diff --git a/src/devices/messages/multipleAnalogSensorsMsgs/multipleAnalogSensorsSerializations.thrift b/src/devices/messages/multipleAnalogSensorsMsgs/multipleAnalogSensorsSerializations.thrift index ac8c4aed47a..16622722833 100644 --- a/src/devices/messages/multipleAnalogSensorsMsgs/multipleAnalogSensorsSerializations.thrift +++ b/src/devices/messages/multipleAnalogSensorsMsgs/multipleAnalogSensorsSerializations.thrift @@ -31,6 +31,8 @@ struct SensorStreamingData 8: SensorMeasurements EncoderArrays; 9: SensorMeasurements SkinPatches; 10: SensorMeasurements PositionSensors; + 11: SensorMeasurements LinearVelocitySensors; + 12: SensorMeasurements ThreeAxisAngularAccelerometers; } struct SensorMetadata { @@ -51,6 +53,8 @@ struct SensorRPCData 8: list EncoderArrays; 9: list SkinPatches; 10: list PositionSensors; + 11: list LinearVelocitySensors; + 12: list ThreeAxisAngularAccelerometers; } service MultipleAnalogSensorsMetadata diff --git a/src/devices/multipleanalogsensorsremapper/MultipleAnalogSensorsRemapper.cpp b/src/devices/multipleanalogsensorsremapper/MultipleAnalogSensorsRemapper.cpp index 100cee3a11d..5e0c5f2d9ee 100644 --- a/src/devices/multipleanalogsensorsremapper/MultipleAnalogSensorsRemapper.cpp +++ b/src/devices/multipleanalogsensorsremapper/MultipleAnalogSensorsRemapper.cpp @@ -18,8 +18,8 @@ namespace { YARP_LOG_COMPONENT(MULTIPLEANALOGSENSORSREMAPPER, "yarp.device.multipleanalogsensorsremapper") } -const size_t MAS_NrOfSensorTypes{10}; -static_assert(MAS_SensorType::PositionSensors+1 == MAS_NrOfSensorTypes, "Consistency error between MAS_NrOfSensorTypes and MAS_SensorType"); +const size_t MAS_NrOfSensorTypes{12}; +static_assert(MAS_SensorType::ThreeAxisAngularAccelerometers+1 == MAS_NrOfSensorTypes, "Consistency error between MAS_NrOfSensorTypes and MAS_SensorType"); /** * Internal identifier of the type of sensors. @@ -34,6 +34,9 @@ inline std::string MAS_getTagFromEnum(const MAS_SensorType type) case ThreeAxisLinearAccelerometers: return "ThreeAxisLinearAccelerometers"; break; + case ThreeAxisAngularAccelerometers: + return "ThreeAxisAngularAccelerometers"; + break; case ThreeAxisMagnetometers: return "ThreeAxisMagnetometers"; break; @@ -58,6 +61,9 @@ inline std::string MAS_getTagFromEnum(const MAS_SensorType type) case PositionSensors: return "PositionSensors"; break; + case LinearVelocitySensors: + return "LinearVelocitySensors"; + break; default: assert(false); return "MAS_getTagFromEnum_notExpectedEnum"; @@ -224,10 +230,14 @@ bool MultipleAnalogSensorsRemapper::attachAll(const PolyDriverList &polylist) &IThreeAxisGyroscopes::getThreeAxisGyroscopeName, &IThreeAxisGyroscopes::getNrOfThreeAxisGyroscopes); ok = ok && genericAttachAll(ThreeAxisLinearAccelerometers, m_iThreeAxisLinearAccelerometers, polylist, &IThreeAxisLinearAccelerometers::getThreeAxisLinearAccelerometerName, &IThreeAxisLinearAccelerometers::getNrOfThreeAxisLinearAccelerometers); + ok = ok && genericAttachAll(ThreeAxisAngularAccelerometers, m_iThreeAxisAngularAccelerometers, polylist, + &IThreeAxisAngularAccelerometers::getThreeAxisAngularAccelerometerName, &IThreeAxisAngularAccelerometers::getNrOfThreeAxisAngularAccelerometers); ok = ok && genericAttachAll(ThreeAxisMagnetometers, m_iThreeAxisMagnetometers, polylist, &IThreeAxisMagnetometers::getThreeAxisMagnetometerName, &IThreeAxisMagnetometers::getNrOfThreeAxisMagnetometers); ok = ok && genericAttachAll(PositionSensors, m_iPositionSensors, polylist, &IPositionSensors::getPositionSensorName, &IPositionSensors::getNrOfPositionSensors); + ok = ok && genericAttachAll(LinearVelocitySensors, m_iLinearVelocitySensors, polylist, + &ILinearVelocitySensors::getLinearVelocitySensorName, &ILinearVelocitySensors::getNrOfLinearVelocitySensors); ok = ok && genericAttachAll(OrientationSensors, m_iOrientationSensors, polylist, &IOrientationSensors::getOrientationSensorName, &IOrientationSensors::getNrOfOrientationSensors); ok = ok && genericAttachAll(TemperatureSensors, m_iTemperatureSensors, polylist, @@ -248,8 +258,10 @@ bool MultipleAnalogSensorsRemapper::detachAll() { m_iThreeAxisGyroscopes.resize(0); m_iThreeAxisLinearAccelerometers.resize(0); + m_iThreeAxisAngularAccelerometers.resize(0); m_iThreeAxisMagnetometers.resize(0); m_iPositionSensors.resize(0); + m_iLinearVelocitySensors.resize(0); m_iOrientationSensors.resize(0); m_iTemperatureSensors.resize(0); m_iSixAxisForceTorqueSensors.resize(0); @@ -443,6 +455,31 @@ bool MultipleAnalogSensorsRemapper::getThreeAxisLinearAccelerometerMeasure(size_ return genericGetMeasure(ThreeAxisLinearAccelerometers, sens_index, out, timestamp, m_iThreeAxisLinearAccelerometers, &IThreeAxisLinearAccelerometers::getThreeAxisLinearAccelerometerMeasure); } +size_t MultipleAnalogSensorsRemapper::getNrOfThreeAxisAngularAccelerometers() const +{ + return m_indicesMap[ThreeAxisAngularAccelerometers].size(); +} + +MAS_status MultipleAnalogSensorsRemapper::getThreeAxisAngularAccelerometerStatus(size_t sens_index) const +{ + return genericGetStatus(ThreeAxisAngularAccelerometers, sens_index, m_iThreeAxisAngularAccelerometers, &IThreeAxisAngularAccelerometers::getThreeAxisAngularAccelerometerStatus); +} + +bool MultipleAnalogSensorsRemapper::getThreeAxisAngularAccelerometerName(size_t sens_index, std::string& name) const +{ + return genericGetName(ThreeAxisAngularAccelerometers, sens_index, name, m_iThreeAxisAngularAccelerometers, &IThreeAxisAngularAccelerometers::getThreeAxisAngularAccelerometerName); +} + +bool MultipleAnalogSensorsRemapper::getThreeAxisAngularAccelerometerFrameName(size_t sens_index, std::string& frameName) const +{ + return genericGetFrameName(ThreeAxisAngularAccelerometers, sens_index, frameName, m_iThreeAxisAngularAccelerometers, &IThreeAxisAngularAccelerometers::getThreeAxisAngularAccelerometerFrameName); +} + +bool MultipleAnalogSensorsRemapper::getThreeAxisAngularAccelerometerMeasure(size_t sens_index, yarp::sig::Vector& out, double& timestamp) const +{ + return genericGetMeasure(ThreeAxisAngularAccelerometers, sens_index, out, timestamp, m_iThreeAxisAngularAccelerometers, &IThreeAxisAngularAccelerometers::getThreeAxisAngularAccelerometerMeasure); +} + size_t MultipleAnalogSensorsRemapper::getNrOfThreeAxisMagnetometers() const { return m_indicesMap[ThreeAxisMagnetometers].size(); @@ -493,6 +530,31 @@ bool MultipleAnalogSensorsRemapper::getPositionSensorMeasure(size_t sens_index, return genericGetMeasure(PositionSensors, sens_index, out, timestamp, m_iPositionSensors, &IPositionSensors::getPositionSensorMeasure); } +size_t MultipleAnalogSensorsRemapper::getNrOfLinearVelocitySensors() const +{ + return m_indicesMap[LinearVelocitySensors].size(); +} + +MAS_status MultipleAnalogSensorsRemapper::getLinearVelocitySensorStatus(size_t sens_index) const +{ + return genericGetStatus(LinearVelocitySensors, sens_index, m_iLinearVelocitySensors, &ILinearVelocitySensors::getLinearVelocitySensorStatus); +} + +bool MultipleAnalogSensorsRemapper::getLinearVelocitySensorName(size_t sens_index, std::string& name) const +{ + return genericGetName(LinearVelocitySensors, sens_index, name, m_iLinearVelocitySensors, &ILinearVelocitySensors::getLinearVelocitySensorName); +} + +bool MultipleAnalogSensorsRemapper::getLinearVelocitySensorFrameName(size_t sens_index, std::string& frameName) const +{ + return genericGetFrameName(LinearVelocitySensors, sens_index, frameName, m_iLinearVelocitySensors, &ILinearVelocitySensors::getLinearVelocitySensorFrameName); +} + +bool MultipleAnalogSensorsRemapper::getLinearVelocitySensorMeasure(size_t sens_index, yarp::sig::Vector& out, double& timestamp) const +{ + return genericGetMeasure(LinearVelocitySensors, sens_index, out, timestamp, m_iLinearVelocitySensors, &ILinearVelocitySensors::getLinearVelocitySensorMeasure); +} + size_t MultipleAnalogSensorsRemapper::getNrOfOrientationSensors() const { return m_indicesMap[OrientationSensors].size(); diff --git a/src/devices/multipleanalogsensorsremapper/MultipleAnalogSensorsRemapper.h b/src/devices/multipleanalogsensorsremapper/MultipleAnalogSensorsRemapper.h index 625741593ca..a9c7af19245 100644 --- a/src/devices/multipleanalogsensorsremapper/MultipleAnalogSensorsRemapper.h +++ b/src/devices/multipleanalogsensorsremapper/MultipleAnalogSensorsRemapper.h @@ -28,7 +28,9 @@ enum MAS_SensorType ContactLoadCellArrays=6, EncoderArrays=7, SkinPatches=8, - PositionSensors=9 + PositionSensors=9, + LinearVelocitySensors=10, + ThreeAxisAngularAccelerometers=11 }; /** @@ -86,6 +88,7 @@ class MultipleAnalogSensorsRemapper : public yarp::dev::IMultipleWrapper, public yarp::dev::IThreeAxisGyroscopes, public yarp::dev::IThreeAxisLinearAccelerometers, + public yarp::dev::IThreeAxisAngularAccelerometers, public yarp::dev::IThreeAxisMagnetometers, public yarp::dev::IOrientationSensors, public yarp::dev::ITemperatureSensors, @@ -93,7 +96,8 @@ class MultipleAnalogSensorsRemapper : public yarp::dev::IContactLoadCellArrays, public yarp::dev::IEncoderArrays, public yarp::dev::ISkinPatches, - public yarp::dev::IPositionSensors + public yarp::dev::IPositionSensors, + public yarp::dev::ILinearVelocitySensors { private: bool m_verbose{false}; @@ -118,6 +122,7 @@ class MultipleAnalogSensorsRemapper : std::vector m_iThreeAxisGyroscopes; std::vector m_iThreeAxisLinearAccelerometers; + std::vector m_iThreeAxisAngularAccelerometers; std::vector m_iThreeAxisMagnetometers; std::vector m_iOrientationSensors; std::vector m_iTemperatureSensors; @@ -126,6 +131,7 @@ class MultipleAnalogSensorsRemapper : std::vector m_iEncoderArrays; std::vector m_iSkinPatches; std::vector m_iPositionSensors; + std::vector m_iLinearVelocitySensors; // Templated methods to streamline of the function implementation for all the different sensors @@ -189,6 +195,13 @@ class MultipleAnalogSensorsRemapper : bool getThreeAxisLinearAccelerometerFrameName(size_t sens_index, std::string &frameName) const override; bool getThreeAxisLinearAccelerometerMeasure(size_t sens_index, yarp::sig::Vector& out, double& timestamp) const override; + /* IThreeAxisAngularAccelerometers methods */ + size_t getNrOfThreeAxisAngularAccelerometers() const override; + yarp::dev::MAS_status getThreeAxisAngularAccelerometerStatus(size_t sens_index) const override; + bool getThreeAxisAngularAccelerometerName(size_t sens_index, std::string &name) const override; + bool getThreeAxisAngularAccelerometerFrameName(size_t sens_index, std::string &frameName) const override; + bool getThreeAxisAngularAccelerometerMeasure(size_t sens_index, yarp::sig::Vector& out, double& timestamp) const override; + /* IThreeAxisMagnetometers methods */ size_t getNrOfThreeAxisMagnetometers() const override; yarp::dev::MAS_status getThreeAxisMagnetometerStatus(size_t sens_index) const override; @@ -245,6 +258,13 @@ class MultipleAnalogSensorsRemapper : bool getPositionSensorName(size_t sens_index, std::string& name) const override; bool getPositionSensorFrameName(size_t sens_index, std::string& frameName) const override; bool getPositionSensorMeasure(size_t sens_index, yarp::sig::Vector& xyz, double& timestamp) const override; + + /* ILinearVelocitySensors methods */ + size_t getNrOfLinearVelocitySensors() const override; + yarp::dev::MAS_status getLinearVelocitySensorStatus(size_t sens_index) const override; + bool getLinearVelocitySensorName(size_t sens_index, std::string& name) const override; + bool getLinearVelocitySensorFrameName(size_t sens_index, std::string& frameName) const override; + bool getLinearVelocitySensorMeasure(size_t sens_index, yarp::sig::Vector& xyz, double& timestamp) const override; }; diff --git a/src/devices/networkWrappers/multipleanalogsensorsclient/MultipleAnalogSensorsClient.cpp b/src/devices/networkWrappers/multipleanalogsensorsclient/MultipleAnalogSensorsClient.cpp index a8d9fe36227..ee2a322e34d 100644 --- a/src/devices/networkWrappers/multipleanalogsensorsclient/MultipleAnalogSensorsClient.cpp +++ b/src/devices/networkWrappers/multipleanalogsensorsclient/MultipleAnalogSensorsClient.cpp @@ -345,6 +345,33 @@ bool MultipleAnalogSensorsClient::getThreeAxisLinearAccelerometerMeasure(size_t m_streamingPort.receivedData.ThreeAxisLinearAccelerometers, sens_index, out, timestamp); } +size_t MultipleAnalogSensorsClient::getNrOfThreeAxisAngularAccelerometers() const +{ + return genericGetNrOfSensors(m_sensorsMetadata.ThreeAxisAngularAccelerometers, + m_streamingPort.receivedData.ThreeAxisAngularAccelerometers); +} + +yarp::dev::MAS_status MultipleAnalogSensorsClient::getThreeAxisAngularAccelerometerStatus(size_t sens_index) const +{ + return genericGetStatus(); +} + +bool MultipleAnalogSensorsClient::getThreeAxisAngularAccelerometerName(size_t sens_index, std::string &name) const +{ + return genericGetName(m_sensorsMetadata.ThreeAxisAngularAccelerometers, "ThreeAxisAngularAccelerometers", sens_index, name); +} + +bool MultipleAnalogSensorsClient::getThreeAxisAngularAccelerometerFrameName(size_t sens_index, std::string &frameName) const +{ + return genericGetFrameName(m_sensorsMetadata.ThreeAxisAngularAccelerometers, "ThreeAxisAngularAccelerometers", sens_index, frameName); +} + +bool MultipleAnalogSensorsClient::getThreeAxisAngularAccelerometerMeasure(size_t sens_index, yarp::sig::Vector& out, double& timestamp) const +{ + return genericGetMeasure(m_sensorsMetadata.ThreeAxisAngularAccelerometers, "ThreeAxisAngularAccelerometers", + m_streamingPort.receivedData.ThreeAxisAngularAccelerometers, sens_index, out, timestamp); +} + size_t MultipleAnalogSensorsClient::getNrOfThreeAxisMagnetometers() const { return genericGetNrOfSensors(m_sensorsMetadata.ThreeAxisMagnetometers, @@ -425,6 +452,32 @@ bool MultipleAnalogSensorsClient::getPositionSensorMeasure(size_t sens_index, ya return genericGetMeasure(m_sensorsMetadata.PositionSensors, "PositionSensors", m_streamingPort.receivedData.PositionSensors, sens_index, out, timestamp); } +size_t MultipleAnalogSensorsClient::getNrOfLinearVelocitySensors() const +{ + return genericGetNrOfSensors(m_sensorsMetadata.LinearVelocitySensors, + m_streamingPort.receivedData.LinearVelocitySensors); +} + +yarp::dev::MAS_status MultipleAnalogSensorsClient::getLinearVelocitySensorStatus(size_t sens_index) const +{ + return genericGetStatus(); +} + +bool MultipleAnalogSensorsClient::getLinearVelocitySensorName(size_t sens_index, std::string& name) const +{ + return genericGetName(m_sensorsMetadata.LinearVelocitySensors, "LinearVelocitySensors", sens_index, name); +} + +bool MultipleAnalogSensorsClient::getLinearVelocitySensorFrameName(size_t sens_index, std::string& frameName) const +{ + return genericGetFrameName(m_sensorsMetadata.LinearVelocitySensors, "LinearVelocitySensors", sens_index, frameName); +} + +bool MultipleAnalogSensorsClient::getLinearVelocitySensorMeasure(size_t sens_index, yarp::sig::Vector& out, double& timestamp) const +{ + return genericGetMeasure(m_sensorsMetadata.LinearVelocitySensors, "LinearVelocitySensors", m_streamingPort.receivedData.LinearVelocitySensors, sens_index, out, timestamp); +} + size_t MultipleAnalogSensorsClient::getNrOfTemperatureSensors() const { return genericGetNrOfSensors(m_sensorsMetadata.TemperatureSensors, diff --git a/src/devices/networkWrappers/multipleanalogsensorsclient/MultipleAnalogSensorsClient.h b/src/devices/networkWrappers/multipleanalogsensorsclient/MultipleAnalogSensorsClient.h index a9fe9714340..dfa4fce3647 100644 --- a/src/devices/networkWrappers/multipleanalogsensorsclient/MultipleAnalogSensorsClient.h +++ b/src/devices/networkWrappers/multipleanalogsensorsclient/MultipleAnalogSensorsClient.h @@ -47,8 +47,10 @@ class MultipleAnalogSensorsClient : public yarp::dev::DeviceDriver, public yarp::dev::IThreeAxisGyroscopes, public yarp::dev::IThreeAxisLinearAccelerometers, + public yarp::dev::IThreeAxisAngularAccelerometers, public yarp::dev::IThreeAxisMagnetometers, public yarp::dev::IPositionSensors, + public yarp::dev::ILinearVelocitySensors, public yarp::dev::IOrientationSensors, public yarp::dev::ITemperatureSensors, public yarp::dev::ISixAxisForceTorqueSensors, @@ -96,6 +98,13 @@ class MultipleAnalogSensorsClient : bool getThreeAxisLinearAccelerometerFrameName(size_t sens_index, std::string &frameName) const override; bool getThreeAxisLinearAccelerometerMeasure(size_t sens_index, yarp::sig::Vector& out, double& timestamp) const override; + /* IThreeAxisAngularAccelerometers methods */ + size_t getNrOfThreeAxisAngularAccelerometers() const override; + yarp::dev::MAS_status getThreeAxisAngularAccelerometerStatus(size_t sens_index) const override; + bool getThreeAxisAngularAccelerometerName(size_t sens_index, std::string &name) const override; + bool getThreeAxisAngularAccelerometerFrameName(size_t sens_index, std::string &frameName) const override; + bool getThreeAxisAngularAccelerometerMeasure(size_t sens_index, yarp::sig::Vector& out, double& timestamp) const override; + /* IThreeAxisMagnetometers methods */ size_t getNrOfThreeAxisMagnetometers() const override; yarp::dev::MAS_status getThreeAxisMagnetometerStatus(size_t sens_index) const override; @@ -110,6 +119,13 @@ class MultipleAnalogSensorsClient : bool getPositionSensorFrameName(size_t sens_index, std::string& frameName) const override; bool getPositionSensorMeasure(size_t sens_index, yarp::sig::Vector& xyz, double& timestamp) const override; + /* ILinearVelocitySensors methods */ + size_t getNrOfLinearVelocitySensors() const override; + yarp::dev::MAS_status getLinearVelocitySensorStatus(size_t sens_index) const override; + bool getLinearVelocitySensorName(size_t sens_index, std::string& name) const override; + bool getLinearVelocitySensorFrameName(size_t sens_index, std::string& frameName) const override; + bool getLinearVelocitySensorMeasure(size_t sens_index, yarp::sig::Vector& xyz, double& timestamp) const override; + /* IOrientationSensors methods */ size_t getNrOfOrientationSensors() const override; yarp::dev::MAS_status getOrientationSensorStatus(size_t sens_index) const override; diff --git a/src/devices/networkWrappers/multipleanalogsensorsserver/MultipleAnalogSensorsServer.cpp b/src/devices/networkWrappers/multipleanalogsensorsserver/MultipleAnalogSensorsServer.cpp index 5dda2b54a68..23635866338 100644 --- a/src/devices/networkWrappers/multipleanalogsensorsserver/MultipleAnalogSensorsServer.cpp +++ b/src/devices/networkWrappers/multipleanalogsensorsserver/MultipleAnalogSensorsServer.cpp @@ -32,7 +32,9 @@ enum MAS_SensorTypeServer ContactLoadCellArrays=6, EncoderArrays=7, SkinPatches=8, - PositionSensors=9 + PositionSensors=9, + LinearVelocitySensors=10, + ThreeAxisAngularAccelerometers=11 }; /** @@ -48,6 +50,9 @@ inline size_t MAS_getMeasureSizeFromEnum(const MAS_SensorTypeServer type) case ThreeAxisLinearAccelerometers: return 3; break; + case ThreeAxisAngularAccelerometers: + return 3; + break; case ThreeAxisMagnetometers: return 3; break; @@ -57,6 +62,9 @@ inline size_t MAS_getMeasureSizeFromEnum(const MAS_SensorTypeServer type) case PositionSensors: return 3; break; + case LinearVelocitySensors: + return 3; + break; case TemperatureSensors: return 1; break; @@ -243,6 +251,10 @@ bool MultipleAnalogSensorsServer::populateAllSensorsMetadata() &yarp::dev::IThreeAxisLinearAccelerometers::getNrOfThreeAxisLinearAccelerometers, &yarp::dev::IThreeAxisLinearAccelerometers::getThreeAxisLinearAccelerometerName, &yarp::dev::IThreeAxisLinearAccelerometers::getThreeAxisLinearAccelerometerFrameName); + ok = ok && populateSensorsMetadata(m_iThreeAxisAngularAccelerometers, m_sensorMetadata.ThreeAxisAngularAccelerometers, "ThreeAxisAngularAccelerometers", + &yarp::dev::IThreeAxisAngularAccelerometers::getNrOfThreeAxisAngularAccelerometers, + &yarp::dev::IThreeAxisAngularAccelerometers::getThreeAxisAngularAccelerometerName, + &yarp::dev::IThreeAxisAngularAccelerometers::getThreeAxisAngularAccelerometerFrameName); ok = ok && populateSensorsMetadata(m_iThreeAxisMagnetometers, m_sensorMetadata.ThreeAxisMagnetometers, "ThreeAxisMagnetometers", &yarp::dev::IThreeAxisMagnetometers::getNrOfThreeAxisMagnetometers, &yarp::dev::IThreeAxisMagnetometers::getThreeAxisMagnetometerName, @@ -251,6 +263,10 @@ bool MultipleAnalogSensorsServer::populateAllSensorsMetadata() &yarp::dev::IPositionSensors::getNrOfPositionSensors, &yarp::dev::IPositionSensors::getPositionSensorName, &yarp::dev::IPositionSensors::getPositionSensorFrameName); + ok = ok && populateSensorsMetadata(m_iLinearVelocitySensors, m_sensorMetadata.LinearVelocitySensors, "LinearVelocitySensors", + &yarp::dev::ILinearVelocitySensors::getNrOfLinearVelocitySensors, + &yarp::dev::ILinearVelocitySensors::getLinearVelocitySensorName, + &yarp::dev::ILinearVelocitySensors::getLinearVelocitySensorFrameName); ok = ok && populateSensorsMetadata(m_iOrientationSensors, m_sensorMetadata.OrientationSensors, "OrientationSensors", &yarp::dev::IOrientationSensors::getNrOfOrientationSensors, &yarp::dev::IOrientationSensors::getOrientationSensorName, @@ -325,10 +341,14 @@ bool MultipleAnalogSensorsServer::resizeAllMeasureVectors(SensorStreamingData& s streamingData.ThreeAxisGyroscopes.measurements, MAS_getMeasureSizeFromEnum(ThreeAxisGyroscopes)); ok = ok && resizeMeasureVectors(m_iThreeAxisLinearAccelerometers, m_sensorMetadata.ThreeAxisLinearAccelerometers, streamingData.ThreeAxisLinearAccelerometers.measurements, MAS_getMeasureSizeFromEnum(ThreeAxisLinearAccelerometers)); + ok = ok && resizeMeasureVectors(m_iThreeAxisAngularAccelerometers, m_sensorMetadata.ThreeAxisAngularAccelerometers, + streamingData.ThreeAxisAngularAccelerometers.measurements, MAS_getMeasureSizeFromEnum(ThreeAxisAngularAccelerometers)); ok = ok && resizeMeasureVectors(m_iThreeAxisMagnetometers, m_sensorMetadata.ThreeAxisMagnetometers, streamingData.ThreeAxisMagnetometers.measurements, MAS_getMeasureSizeFromEnum(ThreeAxisMagnetometers)); ok = ok && resizeMeasureVectors(m_iPositionSensors, m_sensorMetadata.PositionSensors, streamingData.PositionSensors.measurements, MAS_getMeasureSizeFromEnum(PositionSensors)); + ok = ok && resizeMeasureVectors(m_iLinearVelocitySensors, m_sensorMetadata.LinearVelocitySensors, + streamingData.LinearVelocitySensors.measurements, MAS_getMeasureSizeFromEnum(LinearVelocitySensors)); ok = ok && resizeMeasureVectors(m_iOrientationSensors, m_sensorMetadata.OrientationSensors, streamingData.OrientationSensors.measurements, MAS_getMeasureSizeFromEnum(OrientationSensors)); ok = ok && resizeMeasureVectors(m_iTemperatureSensors, m_sensorMetadata.TemperatureSensors, @@ -379,8 +399,10 @@ bool MultipleAnalogSensorsServer::attachAll(const yarp::dev::PolyDriverList& p) // View all the interfaces poly->view(m_iThreeAxisGyroscopes); poly->view(m_iThreeAxisLinearAccelerometers); + poly->view(m_iThreeAxisAngularAccelerometers); poly->view(m_iThreeAxisMagnetometers); poly->view(m_iPositionSensors); + poly->view(m_iLinearVelocitySensors); poly->view(m_iOrientationSensors); poly->view(m_iTemperatureSensors); poly->view(m_iSixAxisForceTorqueSensors); @@ -512,6 +534,12 @@ void MultipleAnalogSensorsServer::run() &yarp::dev::IThreeAxisLinearAccelerometers::getThreeAxisLinearAccelerometerMeasure, "ThreeAxisLinearAccelerometer"); + ok = ok && genericStreamData(m_iThreeAxisAngularAccelerometers, m_sensorMetadata.ThreeAxisAngularAccelerometers, + streamingData.ThreeAxisAngularAccelerometers.measurements, + &yarp::dev::IThreeAxisAngularAccelerometers::getThreeAxisAngularAccelerometerStatus, + &yarp::dev::IThreeAxisAngularAccelerometers::getThreeAxisAngularAccelerometerMeasure, + "ThreeAxisAngularAccelerometer"); + ok = ok && genericStreamData(m_iThreeAxisMagnetometers, m_sensorMetadata.ThreeAxisMagnetometers, streamingData.ThreeAxisMagnetometers.measurements, &yarp::dev::IThreeAxisMagnetometers::getThreeAxisMagnetometerStatus, @@ -524,6 +552,12 @@ void MultipleAnalogSensorsServer::run() &yarp::dev::IPositionSensors::getPositionSensorMeasure, "PositionSensor"); + ok = ok && genericStreamData(m_iLinearVelocitySensors, m_sensorMetadata.LinearVelocitySensors, + streamingData.LinearVelocitySensors.measurements, + &yarp::dev::ILinearVelocitySensors::getLinearVelocitySensorStatus, + &yarp::dev::ILinearVelocitySensors::getLinearVelocitySensorMeasure, + "LinearVelocitySensor"); + ok = ok && genericStreamData(m_iOrientationSensors, m_sensorMetadata.OrientationSensors, streamingData.OrientationSensors.measurements, &yarp::dev::IOrientationSensors::getOrientationSensorStatus, diff --git a/src/devices/networkWrappers/multipleanalogsensorsserver/MultipleAnalogSensorsServer.h b/src/devices/networkWrappers/multipleanalogsensorsserver/MultipleAnalogSensorsServer.h index 15e315987b4..6cbb4555164 100644 --- a/src/devices/networkWrappers/multipleanalogsensorsserver/MultipleAnalogSensorsServer.h +++ b/src/devices/networkWrappers/multipleanalogsensorsserver/MultipleAnalogSensorsServer.h @@ -49,8 +49,10 @@ class MultipleAnalogSensorsServer : // Interface of the wrapped device yarp::dev::IThreeAxisGyroscopes* m_iThreeAxisGyroscopes{nullptr}; yarp::dev::IThreeAxisLinearAccelerometers* m_iThreeAxisLinearAccelerometers{nullptr}; + yarp::dev::IThreeAxisAngularAccelerometers* m_iThreeAxisAngularAccelerometers{nullptr}; yarp::dev::IThreeAxisMagnetometers* m_iThreeAxisMagnetometers{nullptr}; yarp::dev::IPositionSensors* m_iPositionSensors{nullptr}; + yarp::dev::ILinearVelocitySensors* m_iLinearVelocitySensors{nullptr}; yarp::dev::IOrientationSensors* m_iOrientationSensors{nullptr}; yarp::dev::ITemperatureSensors* m_iTemperatureSensors{nullptr}; yarp::dev::ISixAxisForceTorqueSensors* m_iSixAxisForceTorqueSensors{nullptr}; diff --git a/src/libYARP_dev/src/yarp/dev/MultipleAnalogSensorsInterfaces.h b/src/libYARP_dev/src/yarp/dev/MultipleAnalogSensorsInterfaces.h index 22909d2cf07..8a1c5a50f2c 100644 --- a/src/libYARP_dev/src/yarp/dev/MultipleAnalogSensorsInterfaces.h +++ b/src/libYARP_dev/src/yarp/dev/MultipleAnalogSensorsInterfaces.h @@ -15,8 +15,10 @@ namespace yarp::dev { class IThreeAxisGyroscopes; class IThreeAxisLinearAccelerometers; +class IThreeAxisAngularAccelerometers; class IThreeAxisMagnetometers; class IPositionSensors; +class ILinearVelocitySensors; class IOrientationSensors; class ITemperatureSensors; class ISixAxisForceTorqueSensors; @@ -145,6 +147,44 @@ class YARP_dev_API yarp::dev::IThreeAxisLinearAccelerometers virtual ~IThreeAxisLinearAccelerometers(){} }; +class YARP_dev_API yarp::dev::IThreeAxisAngularAccelerometers +{ +public: + /** + * \brief Get the number of three axis angular accelerometers exposed by this device. + */ + virtual size_t getNrOfThreeAxisAngularAccelerometers() const = 0; + + /** + * Get the status of the specified sensor. + */ + virtual yarp::dev::MAS_status getThreeAxisAngularAccelerometerStatus(size_t sens_index) const = 0; + + /** + * Get the name of the specified sensor. + * @return false if an error occurred, true otherwise. + */ + virtual bool getThreeAxisAngularAccelerometerName(size_t sens_index, std::string &name) const = 0; + + /** + * Get the name of the frame of the specified sensor. + * @return false if an error occurred, true otherwise. + */ + virtual bool getThreeAxisAngularAccelerometerFrameName(size_t sens_index, std::string &frameName) const = 0; + + /** + * Get the last reading of the specified sensor. + * + * @param[in] sens_index The index of the specified sensor (should be between 0 and getNrOfThreeAxisAngularAccelerometers()-1). + * @param[out] out The requested measure. The vector should be 3-dimensional. The measure is expressed in degrees^2/seconds. + * @param[out] timestamp The timestamp of the requested measure, expressed in seconds. + * @return false if an error occurred, true otherwise. + */ + virtual bool getThreeAxisAngularAccelerometerMeasure(size_t sens_index, yarp::sig::Vector& out, double& timestamp) const = 0; + + virtual ~IThreeAxisAngularAccelerometers(){} +}; + /** * @ingroup dev_iface_multiple_analog * @@ -249,6 +289,50 @@ class YARP_dev_API yarp::dev::IPositionSensors } }; +class YARP_dev_API yarp::dev::ILinearVelocitySensors +{ +public: + /** + * Get the number of linear velocity sensors exposed by this device. + */ + virtual size_t getNrOfLinearVelocitySensors() const = 0; + + /** + * Get the status of the specified sensor. + */ + virtual yarp::dev::MAS_status getLinearVelocitySensorStatus(size_t sens_index) const = 0; + + /** + * Get the name of the specified sensor. + * @return false if an error occurred, true otherwise. + */ + virtual bool getLinearVelocitySensorName(size_t sens_index, std::string& name) const = 0; + + /** + * Get the name of the frame of the specified sensor. + * + * @note This is an implementation specific method, that may return the name of the sensor + * frame in a scenegraph + * + * @return false if an error occurred, true otherwise. + */ + virtual bool getLinearVelocitySensorFrameName(size_t sens_index, std::string& frameName) const = 0; + + /** + * Get the last reading of the linear velocity sensor as x y z. + * + * @param[in] sens_index The index of the specified sensor (should be between 0 and getNrOfLinearVelocitySensors()-1). + * @param[out] out The requested measure. The vector should be 3-dimensional. The measure is expressed in meters/second. + * @param[out] timestamp The timestamp of the requested measure, expressed in seconds. + * @return false if an error occurred, true otherwise. + */ + virtual bool getLinearVelocitySensorMeasure(size_t sens_index, yarp::sig::Vector& xyz, double& timestamp) const = 0; + + virtual ~ILinearVelocitySensors() + { + } +}; + /** * @ingroup dev_iface_multiple_analog * diff --git a/src/portmonitors/image_rotation/imageRotation.cpp b/src/portmonitors/image_rotation/imageRotation.cpp index 0327ac3881b..0d709c16385 100644 --- a/src/portmonitors/image_rotation/imageRotation.cpp +++ b/src/portmonitors/image_rotation/imageRotation.cpp @@ -5,12 +5,13 @@ #include "imageRotation.h" -#include -#include - #include #include + #include + +#include +#include #include #include #include @@ -29,62 +30,42 @@ YARP_LOG_COMPONENT(IMAGEROTATION, bool ImageRotation::create(const yarp::os::Property& options) { - //parse the user parameters + // parse the user parameters yarp::os::Property m_user_params; yCDebug(IMAGEROTATION) << "user params:" << options.toString(); std::string str = options.find("carrier").asString(); getParamsFromCommandLine(str, m_user_params); yCDebug(IMAGEROTATION) << "parsed params:" << m_user_params.toString(); - //get the value of the parameters - if (m_user_params.check("options_rotate")) - { + // get the value of the parameters + if (m_user_params.check("options_rotate")) { m_options_rotate_str = m_user_params.find("options_rotate").asString(); } - if (m_user_params.check("options_flip")) - { + if (m_user_params.check("options_flip")) { m_options_flip_str = m_user_params.find("options_flip").asString(); } - //translate the parameters in opencv - if (m_options_rotate_str == std::string("rotate_cw")) - { + // translate the parameters in opencv + if (m_options_rotate_str == std::string("rotate_cw")) { m_rot_flags = cv::ROTATE_90_CLOCKWISE; - } - else if (m_options_rotate_str == std::string("rotate_ccw")) - { + } else if (m_options_rotate_str == std::string("rotate_ccw")) { m_rot_flags = cv::ROTATE_90_COUNTERCLOCKWISE; - } - else if (m_options_rotate_str == std::string("rotate_180")) - { + } else if (m_options_rotate_str == std::string("rotate_180")) { m_rot_flags = cv::ROTATE_180; - } - else if (m_options_rotate_str == std::string("rotate_none")) - { - } - else - { + } else if (m_options_rotate_str == std::string("rotate_none")) { + } else { yCDebug(IMAGEROTATION) << "Invalid value of `options_rotate` parameter"; return false; } - if (m_options_flip_str == std::string("flip_x")) - { + if (m_options_flip_str == std::string("flip_x")) { m_flip_code = 0; - } - else if (m_options_flip_str == std::string("flip_y")) - { + } else if (m_options_flip_str == std::string("flip_y")) { m_flip_code = 1; - } - else if (m_options_flip_str == std::string("flip_xy")) - { + } else if (m_options_flip_str == std::string("flip_xy")) { m_flip_code = -1; - } - else if (m_options_flip_str == std::string("flip_none")) - { - } - else - { + } else if (m_options_flip_str == std::string("flip_none")) { + } else { yCDebug(IMAGEROTATION) << "Invalid value of `options_flip` parameter"; return false; } @@ -112,12 +93,10 @@ void ImageRotation::getParamsFromCommandLine(std::string carrierString, yarp::os split(carrierString, '+', parameters); // Iterate over result strings - for (std::string param : parameters) - { + for (std::string param : parameters) { // If there is no '.', then the param is bad formatted, skip it. auto pointPosition = param.find('.'); - if (pointPosition == std::string::npos) - { + if (pointPosition == std::string::npos) { continue; } @@ -127,7 +106,7 @@ void ImageRotation::getParamsFromCommandLine(std::string carrierString, yarp::os std::string s = param.substr(pointPosition + 1, param.length()); paramValue.fromString(s.c_str()); - //and append to the returned property + // and append to the returned property prop.put(paramKey, paramValue); } return; @@ -146,14 +125,11 @@ bool ImageRotation::getparam(yarp::os::Property& params) bool ImageRotation::accept(yarp::os::Things& thing) { auto* img = thing.cast_as(); - if(img == nullptr) - { + if (img == nullptr) { yCError(IMAGEROTATION, "Expected type Image, but got wrong data type!"); return false; } - if (img->getPixelCode() != VOCAB_PIXEL_RGB && - img->getPixelCode() != VOCAB_PIXEL_MONO_FLOAT) - { + if (img->getPixelCode() != VOCAB_PIXEL_RGB && img->getPixelCode() != VOCAB_PIXEL_MONO_FLOAT) { yCError(IMAGEROTATION, "Received image with invalid/unsupported pixelCode!"); return false; } @@ -162,70 +138,53 @@ bool ImageRotation::accept(yarp::os::Things& thing) yarp::os::Things& ImageRotation::update(yarp::os::Things& thing) { - yarp::sig::Image* yarpimg = thing.cast_as(); - if (yarpimg->getPixelCode() == VOCAB_PIXEL_RGB) - { + yarp::sig::Image* yarpimg = thing.cast_as(); + if (yarpimg->getPixelCode() == VOCAB_PIXEL_RGB) { m_cvInImage = yarp::cv::toCvMat(*yarpimg); m_outImgRgb.resize(yarpimg->width(), yarpimg->height()); m_outImgRgb.zero(); - if (m_options_flip_str == "flip_none" && m_options_rotate_str != "rotation_none") - { - //just rotation + if (m_options_flip_str == "flip_none" && m_options_rotate_str != "rotation_none") { + // just rotation cv::rotate(m_cvInImage, m_cvOutImage1, m_rot_flags); m_outImgRgb = yarp::cv::fromCvMat(m_cvOutImage1); - } - else if (m_options_flip_str != "flip_none" && m_options_rotate_str == "rotation_none") - { - //just flip + } else if (m_options_flip_str != "flip_none" && m_options_rotate_str == "rotation_none") { + // just flip cv::flip(m_cvInImage, m_cvOutImage1, m_flip_code); m_outImgRgb = yarp::cv::fromCvMat(m_cvOutImage1); - } - else if (m_options_flip_str == "flip_none" && m_options_rotate_str == "rotation_none") - { - //just copy + } else if (m_options_flip_str == "flip_none" && m_options_rotate_str == "rotation_none") { + // just copy m_outImgRgb = yarp::cv::fromCvMat(m_cvInImage); - } - else - { - //first a rotation, then a flip + } else { + // first a rotation, then a flip cv::rotate(m_cvInImage, m_cvOutImage1, m_rot_flags); cv::flip(m_cvOutImage1, m_cvOutImage2, m_flip_code); m_outImgRgb = yarp::cv::fromCvMat(m_cvOutImage2); } m_th.setPortWriter(&m_outImgRgb); - } - else if (yarpimg->getPixelCode() == VOCAB_PIXEL_MONO_FLOAT) - { + } else if (yarpimg->getPixelCode() == VOCAB_PIXEL_MONO_FLOAT) { m_cvInImage = yarp::cv::toCvMat(*yarpimg); m_outImgFloat.resize(yarpimg->width(), yarpimg->height()); m_outImgFloat.zero(); - if (m_options_flip_str == "flip_none") - { + if (m_options_flip_str == "flip_none") { // just rotation cv::rotate(m_cvInImage, m_cvOutImage1, m_rot_flags); m_outImgFloat = yarp::cv::fromCvMat(m_cvOutImage1); - } - else if (m_options_flip_str == "rotation_none") - { + } else if (m_options_flip_str == "rotation_none") { // just flip cv::flip(m_cvInImage, m_cvOutImage1, m_flip_code); m_outImgFloat = yarp::cv::fromCvMat(m_cvOutImage1); - } - else - { + } else { // first a rotation, then a flip cv::rotate(m_cvInImage, m_cvOutImage1, m_rot_flags); cv::flip(m_cvOutImage1, m_cvOutImage2, m_flip_code); m_outImgFloat = yarp::cv::fromCvMat(m_cvOutImage2); } m_th.setPortWriter(&m_outImgFloat); - } - else - { + } else { yCError(IMAGEROTATION, "Invalid Image type!"); } return m_th;