From 4eaf726fec96b76bc3212d2117f264bab92f8934 Mon Sep 17 00:00:00 2001 From: Nick Tremaroli Date: Tue, 10 Sep 2024 11:30:33 +0200 Subject: [PATCH 01/13] Initial commit for support for velocity sensors --- .../idl_generated_code/SensorRPCData.cpp | 1 + .../idl_generated_code/SensorRPCData.h | 7 +++ ...multipleAnalogSensorsSerializations.thrift | 2 + .../MultipleAnalogSensorsClient.cpp | 26 +++++++++++ .../MultipleAnalogSensorsClient.h | 8 ++++ .../MultipleAnalogSensorsRemapper.cpp | 31 +++++++++++++ .../MultipleAnalogSensorsRemapper.h | 14 +++++- .../MultipleAnalogSensorsServer.cpp | 4 ++ .../MultipleAnalogSensorsServer.h | 1 + .../dev/MultipleAnalogSensorsInterfaces.h | 45 +++++++++++++++++++ 10 files changed, 137 insertions(+), 2 deletions(-) diff --git a/src/devices/multipleAnalogSensorsMsgs/idl_generated_code/SensorRPCData.cpp b/src/devices/multipleAnalogSensorsMsgs/idl_generated_code/SensorRPCData.cpp index c1e8ec0d177..42c59d803d6 100644 --- a/src/devices/multipleAnalogSensorsMsgs/idl_generated_code/SensorRPCData.cpp +++ b/src/devices/multipleAnalogSensorsMsgs/idl_generated_code/SensorRPCData.cpp @@ -35,6 +35,7 @@ SensorRPCData::SensorRPCData(const std::vector& ThreeAxisGyrosco { } + // Read structure on a Wire bool SensorRPCData::read(yarp::os::idl::WireReader& reader) { diff --git a/src/devices/multipleAnalogSensorsMsgs/idl_generated_code/SensorRPCData.h b/src/devices/multipleAnalogSensorsMsgs/idl_generated_code/SensorRPCData.h index fe7054892e9..52e0c98206f 100644 --- a/src/devices/multipleAnalogSensorsMsgs/idl_generated_code/SensorRPCData.h +++ b/src/devices/multipleAnalogSensorsMsgs/idl_generated_code/SensorRPCData.h @@ -30,6 +30,7 @@ class SensorRPCData : std::vector EncoderArrays{}; std::vector SkinPatches{}; std::vector PositionSensors{}; + std::vector VelocitySensors{}; // Default constructor SensorRPCData() = default; @@ -124,6 +125,12 @@ 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 VelocitySensors field + bool read_VelocitySensors(yarp::os::idl::WireReader& reader); + bool write_VelocitySensors(const yarp::os::idl::WireWriter& writer) const; + bool nested_read_VelocitySensors(yarp::os::idl::WireReader& reader); + bool nested_write_VelocitySensors(const yarp::os::idl::WireWriter& writer) const; }; #endif // YARP_THRIFT_GENERATOR_STRUCT_SENSORRPCDATA_H diff --git a/src/devices/multipleAnalogSensorsMsgs/multipleAnalogSensorsSerializations.thrift b/src/devices/multipleAnalogSensorsMsgs/multipleAnalogSensorsSerializations.thrift index ac8c4aed47a..808e98ed24a 100644 --- a/src/devices/multipleAnalogSensorsMsgs/multipleAnalogSensorsSerializations.thrift +++ b/src/devices/multipleAnalogSensorsMsgs/multipleAnalogSensorsSerializations.thrift @@ -31,6 +31,7 @@ struct SensorStreamingData 8: SensorMeasurements EncoderArrays; 9: SensorMeasurements SkinPatches; 10: SensorMeasurements PositionSensors; + 11: SensorMeasurements VelocitySensors; } struct SensorMetadata { @@ -51,6 +52,7 @@ struct SensorRPCData 8: list EncoderArrays; 9: list SkinPatches; 10: list PositionSensors; + 11: list VelocitySensors; } service MultipleAnalogSensorsMetadata diff --git a/src/devices/multipleanalogsensorsclient/MultipleAnalogSensorsClient.cpp b/src/devices/multipleanalogsensorsclient/MultipleAnalogSensorsClient.cpp index 9e2c348c805..5aae8b20ce8 100644 --- a/src/devices/multipleanalogsensorsclient/MultipleAnalogSensorsClient.cpp +++ b/src/devices/multipleanalogsensorsclient/MultipleAnalogSensorsClient.cpp @@ -458,6 +458,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::getNrOfVelocitySensors() const +{ + return genericGetNrOfSensors(m_sensorsMetadata.VelocitySensors, + m_streamingPort.receivedData.VelocitySensors); +} + +yarp::dev::MAS_status MultipleAnalogSensorsClient::getVelocitySensorStatus(size_t sens_index) const +{ + return genericGetStatus(); +} + +bool MultipleAnalogSensorsClient::getVelocitySensorName(size_t sens_index, std::string& name) const +{ + return genericGetName(m_sensorsMetadata.VelocitySensors, "VelocitySensors", sens_index, name); +} + +bool MultipleAnalogSensorsClient::getVelocitySensorFrameName(size_t sens_index, std::string& frameName) const +{ + return genericGetFrameName(m_sensorsMetadata.VelocitySensors, "VelocitySensors", sens_index, frameName); +} + +bool MultipleAnalogSensorsClient::getVelocitySensorMeasure(size_t sens_index, yarp::sig::Vector& out, double& timestamp) const +{ + return genericGetMeasure(m_sensorsMetadata.VelocitySensors, "VelocitySensors", m_streamingPort.receivedData.VelocitySensors, sens_index, out, timestamp); +} + size_t MultipleAnalogSensorsClient::getNrOfTemperatureSensors() const { return genericGetNrOfSensors(m_sensorsMetadata.TemperatureSensors, diff --git a/src/devices/multipleanalogsensorsclient/MultipleAnalogSensorsClient.h b/src/devices/multipleanalogsensorsclient/MultipleAnalogSensorsClient.h index aab3f74e166..2aef37483e3 100644 --- a/src/devices/multipleanalogsensorsclient/MultipleAnalogSensorsClient.h +++ b/src/devices/multipleanalogsensorsclient/MultipleAnalogSensorsClient.h @@ -58,6 +58,7 @@ class MultipleAnalogSensorsClient : public yarp::dev::IThreeAxisLinearAccelerometers, public yarp::dev::IThreeAxisMagnetometers, public yarp::dev::IPositionSensors, + public yarp::dev::IVelocitySensors, public yarp::dev::IOrientationSensors, public yarp::dev::ITemperatureSensors, public yarp::dev::ISixAxisForceTorqueSensors, @@ -126,6 +127,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; + /* IVelocitySensors methods */ + size_t getNrOfVelocitySensors() const override; + yarp::dev::MAS_status getVelocitySensorStatus(size_t sens_index) const override; + bool getVelocitySensorName(size_t sens_index, std::string& name) const override; + bool getVelocitySensorFrameName(size_t sens_index, std::string& frameName) const override; + bool getVelocitySensorMeasure(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/multipleanalogsensorsremapper/MultipleAnalogSensorsRemapper.cpp b/src/devices/multipleanalogsensorsremapper/MultipleAnalogSensorsRemapper.cpp index 100cee3a11d..52d98124354 100644 --- a/src/devices/multipleanalogsensorsremapper/MultipleAnalogSensorsRemapper.cpp +++ b/src/devices/multipleanalogsensorsremapper/MultipleAnalogSensorsRemapper.cpp @@ -58,6 +58,9 @@ inline std::string MAS_getTagFromEnum(const MAS_SensorType type) case PositionSensors: return "PositionSensors"; break; + case VelocitySensors: + return "VelocitySensors"; + break; default: assert(false); return "MAS_getTagFromEnum_notExpectedEnum"; @@ -228,6 +231,8 @@ bool MultipleAnalogSensorsRemapper::attachAll(const PolyDriverList &polylist) &IThreeAxisMagnetometers::getThreeAxisMagnetometerName, &IThreeAxisMagnetometers::getNrOfThreeAxisMagnetometers); ok = ok && genericAttachAll(PositionSensors, m_iPositionSensors, polylist, &IPositionSensors::getPositionSensorName, &IPositionSensors::getNrOfPositionSensors); + ok = ok && genericAttachAll(VelocitySensors, m_iVelocitySensors, polylist, + &IVelocitySensors::getVelocitySensorName, &IVelocitySensors::getNrOfVelocitySensors); ok = ok && genericAttachAll(OrientationSensors, m_iOrientationSensors, polylist, &IOrientationSensors::getOrientationSensorName, &IOrientationSensors::getNrOfOrientationSensors); ok = ok && genericAttachAll(TemperatureSensors, m_iTemperatureSensors, polylist, @@ -250,6 +255,7 @@ bool MultipleAnalogSensorsRemapper::detachAll() m_iThreeAxisLinearAccelerometers.resize(0); m_iThreeAxisMagnetometers.resize(0); m_iPositionSensors.resize(0); + m_iVelocitySensors.resize(0); m_iOrientationSensors.resize(0); m_iTemperatureSensors.resize(0); m_iSixAxisForceTorqueSensors.resize(0); @@ -493,6 +499,31 @@ bool MultipleAnalogSensorsRemapper::getPositionSensorMeasure(size_t sens_index, return genericGetMeasure(PositionSensors, sens_index, out, timestamp, m_iPositionSensors, &IPositionSensors::getPositionSensorMeasure); } +size_t MultipleAnalogSensorsRemapper::getNrOfVelocitySensors() const +{ + return m_indicesMap[VelocitySensors].size(); +} + +MAS_status MultipleAnalogSensorsRemapper::getVelocitySensorStatus(size_t sens_index) const +{ + return genericGetStatus(VelocitySensors, sens_index, m_iVelocitySensors, &IVelocitySensors::getVelocitySensorStatus); +} + +bool MultipleAnalogSensorsRemapper::getVelocitySensorName(size_t sens_index, std::string& name) const +{ + return genericGetName(VelocitySensors, sens_index, name, m_iVelocitySensors, &IVelocitySensors::getVelocitySensorName); +} + +bool MultipleAnalogSensorsRemapper::getVelocitySensorFrameName(size_t sens_index, std::string& frameName) const +{ + return genericGetFrameName(VelocitySensors, sens_index, frameName, m_iVelocitySensors, &IVelocitySensors::getVelocitySensorFrameName); +} + +bool MultipleAnalogSensorsRemapper::getVelocitySensorMeasure(size_t sens_index, yarp::sig::Vector& out, double& timestamp) const +{ + return genericGetMeasure(VelocitySensors, sens_index, out, timestamp, m_iVelocitySensors, &IVelocitySensors::getVelocitySensorMeasure); +} + 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 841ad9feedd..dafce3a6278 100644 --- a/src/devices/multipleanalogsensorsremapper/MultipleAnalogSensorsRemapper.h +++ b/src/devices/multipleanalogsensorsremapper/MultipleAnalogSensorsRemapper.h @@ -28,7 +28,8 @@ enum MAS_SensorType ContactLoadCellArrays=6, EncoderArrays=7, SkinPatches=8, - PositionSensors=9 + PositionSensors=9, + VelocitySensors=10 }; /** @@ -93,7 +94,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::IVelocitySensors { private: bool m_verbose{false}; @@ -126,6 +128,7 @@ class MultipleAnalogSensorsRemapper : std::vector m_iEncoderArrays; std::vector m_iSkinPatches; std::vector m_iPositionSensors; + std::vector m_iVelocitySensors; // Templated methods to streamline of the function implementation for all the different sensors @@ -245,6 +248,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; + + /* IVelocitySensors methods */ + size_t getNrOfVelocitySensors() const override; + yarp::dev::MAS_status getVelocitySensorStatus(size_t sens_index) const override; + bool getVelocitySensorName(size_t sens_index, std::string& name) const override; + bool getVelocitySensorFrameName(size_t sens_index, std::string& frameName) const override; + bool getVelocitySensorMeasure(size_t sens_index, yarp::sig::Vector& xyz, double& timestamp) const override; }; diff --git a/src/devices/multipleanalogsensorsserver/MultipleAnalogSensorsServer.cpp b/src/devices/multipleanalogsensorsserver/MultipleAnalogSensorsServer.cpp index f430257a25c..a2580f692f8 100644 --- a/src/devices/multipleanalogsensorsserver/MultipleAnalogSensorsServer.cpp +++ b/src/devices/multipleanalogsensorsserver/MultipleAnalogSensorsServer.cpp @@ -307,6 +307,10 @@ bool MultipleAnalogSensorsServer::populateAllSensorsMetadata() &yarp::dev::IPositionSensors::getNrOfPositionSensors, &yarp::dev::IPositionSensors::getPositionSensorName, &yarp::dev::IPositionSensors::getPositionSensorFrameName); + ok = ok && populateSensorsMetadata(m_iVelocitySensors, m_sensorMetadata.VelocitySensors, "VelocitySensors", + &yarp::dev::IVelocitySensors::getNrOfVelocitySensors, + &yarp::dev::IVelocitySensors::getVelocitySensorName, + &yarp::dev::IVelocitySensors::getVelocitySensorFrameName); ok = ok && populateSensorsMetadata(m_iOrientationSensors, m_sensorMetadata.OrientationSensors, "OrientationSensors", &yarp::dev::IOrientationSensors::getNrOfOrientationSensors, &yarp::dev::IOrientationSensors::getOrientationSensorName, diff --git a/src/devices/multipleanalogsensorsserver/MultipleAnalogSensorsServer.h b/src/devices/multipleanalogsensorsserver/MultipleAnalogSensorsServer.h index cf885c167db..bf70161e666 100644 --- a/src/devices/multipleanalogsensorsserver/MultipleAnalogSensorsServer.h +++ b/src/devices/multipleanalogsensorsserver/MultipleAnalogSensorsServer.h @@ -61,6 +61,7 @@ class MultipleAnalogSensorsServer : yarp::dev::IThreeAxisLinearAccelerometers* m_iThreeAxisLinearAccelerometers{nullptr}; yarp::dev::IThreeAxisMagnetometers* m_iThreeAxisMagnetometers{nullptr}; yarp::dev::IPositionSensors* m_iPositionSensors{nullptr}; + yarp::dev::IVelocitySensors* m_iVelocitySensors{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..cd9a00e6772 100644 --- a/src/libYARP_dev/src/yarp/dev/MultipleAnalogSensorsInterfaces.h +++ b/src/libYARP_dev/src/yarp/dev/MultipleAnalogSensorsInterfaces.h @@ -17,6 +17,7 @@ class IThreeAxisGyroscopes; class IThreeAxisLinearAccelerometers; class IThreeAxisMagnetometers; class IPositionSensors; +class IVelocitySensors; class IOrientationSensors; class ITemperatureSensors; class ISixAxisForceTorqueSensors; @@ -249,6 +250,50 @@ class YARP_dev_API yarp::dev::IPositionSensors } }; +class YARP_dev_API yarp::dev::IVelocitySensors +{ +public: + /** + * Get the number of velocity sensors exposed by this device. + */ + virtual size_t getNrOfVelocitySensors() const = 0; + + /** + * Get the status of the specified sensor. + */ + virtual yarp::dev::MAS_status getVelocitySensorStatus(size_t sens_index) const = 0; + + /** + * Get the name of the specified sensor. + * @return false if an error occurred, true otherwise. + */ + virtual bool getVelocitySensorName(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 getVelocitySensorFrameName(size_t sens_index, std::string& frameName) const = 0; + + /** + * Get the last reading of the velocity sensor as x y z. + * + * @param[in] sens_index The index of the specified sensor (should be between 0 and getNrOfVelocitySensors()-1). + * @param[out] out The requested measure. The vector should be 3-dimensional. The measure is expressed in meters. + * @param[out] timestamp The timestamp of the requested measure, expressed in seconds. + * @return false if an error occurred, true otherwise. + */ + virtual bool getVelocitySensorMeasure(size_t sens_index, yarp::sig::Vector& xyz, double& timestamp) const = 0; + + virtual ~IVelocitySensors() + { + } +}; + /** * @ingroup dev_iface_multiple_analog * From 1b3e98fcb0ae247e65222089aab2d5629c4ac25a Mon Sep 17 00:00:00 2001 From: Nick Tremaroli Date: Wed, 11 Sep 2024 11:20:47 +0200 Subject: [PATCH 02/13] Finished added Velocity Sensor support --- cmake/YarpIDL.cmake | 2 +- .../MultipleAnalogSensorsRemapper.cpp | 4 ++-- .../MultipleAnalogSensorsServer.cpp | 15 ++++++++++++++- 3 files changed, 17 insertions(+), 4 deletions(-) diff --git a/cmake/YarpIDL.cmake b/cmake/YarpIDL.cmake index 76450f090c3..bc4f90c7638 100644 --- a/cmake/YarpIDL.cmake +++ b/cmake/YarpIDL.cmake @@ -193,7 +193,7 @@ endfunction() function(YARP_IDL_TO_DIR) # Flag to control whether IDL generation is allowed. - option(ALLOW_IDL_GENERATION "Allow YARP to (re)build IDL files as needed" OFF) + option(ALLOW_IDL_GENERATION "Allow YARP to (re)build IDL files as needed" ON) set(_options VERBOSE diff --git a/src/devices/multipleanalogsensorsremapper/MultipleAnalogSensorsRemapper.cpp b/src/devices/multipleanalogsensorsremapper/MultipleAnalogSensorsRemapper.cpp index 52d98124354..d76ea80c66b 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{11}; +static_assert(MAS_SensorType::VelocitySensors+1 == MAS_NrOfSensorTypes, "Consistency error between MAS_NrOfSensorTypes and MAS_SensorType"); /** * Internal identifier of the type of sensors. diff --git a/src/devices/multipleanalogsensorsserver/MultipleAnalogSensorsServer.cpp b/src/devices/multipleanalogsensorsserver/MultipleAnalogSensorsServer.cpp index a2580f692f8..15fc4a49419 100644 --- a/src/devices/multipleanalogsensorsserver/MultipleAnalogSensorsServer.cpp +++ b/src/devices/multipleanalogsensorsserver/MultipleAnalogSensorsServer.cpp @@ -32,7 +32,8 @@ enum MAS_SensorTypeServer ContactLoadCellArrays=6, EncoderArrays=7, SkinPatches=8, - PositionSensors=9 + PositionSensors=9, + VelocitySensors=10 }; /** @@ -57,6 +58,9 @@ inline size_t MAS_getMeasureSizeFromEnum(const MAS_SensorTypeServer type) case PositionSensors: return 3; break; + case VelocitySensors: + return 3; + break; case TemperatureSensors: return 1; break; @@ -389,6 +393,8 @@ bool MultipleAnalogSensorsServer::resizeAllMeasureVectors(SensorStreamingData& s streamingData.ThreeAxisMagnetometers.measurements, MAS_getMeasureSizeFromEnum(ThreeAxisMagnetometers)); ok = ok && resizeMeasureVectors(m_iPositionSensors, m_sensorMetadata.PositionSensors, streamingData.PositionSensors.measurements, MAS_getMeasureSizeFromEnum(PositionSensors)); + ok = ok && resizeMeasureVectors(m_iVelocitySensors, m_sensorMetadata.VelocitySensors, + streamingData.VelocitySensors.measurements, MAS_getMeasureSizeFromEnum(VelocitySensors)); ok = ok && resizeMeasureVectors(m_iOrientationSensors, m_sensorMetadata.OrientationSensors, streamingData.OrientationSensors.measurements, MAS_getMeasureSizeFromEnum(OrientationSensors)); ok = ok && resizeMeasureVectors(m_iTemperatureSensors, m_sensorMetadata.TemperatureSensors, @@ -441,6 +447,7 @@ bool MultipleAnalogSensorsServer::attachAll(const yarp::dev::PolyDriverList& p) poly->view(m_iThreeAxisLinearAccelerometers); poly->view(m_iThreeAxisMagnetometers); poly->view(m_iPositionSensors); + poly->view(m_iVelocitySensors); poly->view(m_iOrientationSensors); poly->view(m_iTemperatureSensors); poly->view(m_iSixAxisForceTorqueSensors); @@ -584,6 +591,12 @@ void MultipleAnalogSensorsServer::run() &yarp::dev::IPositionSensors::getPositionSensorMeasure, "PositionSensor"); + ok = ok && genericStreamData(m_iVelocitySensors, m_sensorMetadata.VelocitySensors, + streamingData.VelocitySensors.measurements, + &yarp::dev::IVelocitySensors::getVelocitySensorStatus, + &yarp::dev::IVelocitySensors::getVelocitySensorMeasure, + "VelocitySensor"); + ok = ok && genericStreamData(m_iOrientationSensors, m_sensorMetadata.OrientationSensors, streamingData.OrientationSensors.measurements, &yarp::dev::IOrientationSensors::getOrientationSensorStatus, From fa95a8c6757b4f5e75d2e983d62ae8ef60083f42 Mon Sep 17 00:00:00 2001 From: Nick Tremaroli Date: Thu, 19 Sep 2024 11:13:06 +0200 Subject: [PATCH 03/13] Finished adding angular acceleration interface --- ...multipleAnalogSensorsSerializations.thrift | 38 +++++++++--------- .../MultipleAnalogSensorsClient.cpp | 27 +++++++++++++ .../MultipleAnalogSensorsClient.h | 8 ++++ .../MultipleAnalogSensorsRemapper.cpp | 33 +++++++++++++++- .../MultipleAnalogSensorsRemapper.h | 28 ++++++++----- .../MultipleAnalogSensorsServer.cpp | 35 ++++++++++++----- .../MultipleAnalogSensorsServer.h | 1 + .../dev/MultipleAnalogSensorsInterfaces.h | 39 +++++++++++++++++++ 8 files changed, 172 insertions(+), 37 deletions(-) diff --git a/src/devices/multipleAnalogSensorsMsgs/multipleAnalogSensorsSerializations.thrift b/src/devices/multipleAnalogSensorsMsgs/multipleAnalogSensorsSerializations.thrift index 808e98ed24a..c7197fdcca5 100644 --- a/src/devices/multipleAnalogSensorsMsgs/multipleAnalogSensorsSerializations.thrift +++ b/src/devices/multipleAnalogSensorsMsgs/multipleAnalogSensorsSerializations.thrift @@ -23,15 +23,16 @@ struct SensorStreamingData { 1: SensorMeasurements ThreeAxisGyroscopes; 2: SensorMeasurements ThreeAxisLinearAccelerometers; - 3: SensorMeasurements ThreeAxisMagnetometers; - 4: SensorMeasurements OrientationSensors; - 5: SensorMeasurements TemperatureSensors; - 6: SensorMeasurements SixAxisForceTorqueSensors; - 7: SensorMeasurements ContactLoadCellArrays; - 8: SensorMeasurements EncoderArrays; - 9: SensorMeasurements SkinPatches; - 10: SensorMeasurements PositionSensors; - 11: SensorMeasurements VelocitySensors; + 3: SensorMeasurements ThreeAxisAngularAccelerometers; + 4: SensorMeasurements ThreeAxisMagnetometers; + 5: SensorMeasurements OrientationSensors; + 6: SensorMeasurements TemperatureSensors; + 7: SensorMeasurements SixAxisForceTorqueSensors; + 8: SensorMeasurements ContactLoadCellArrays; + 9: SensorMeasurements EncoderArrays; + 10: SensorMeasurements SkinPatches; + 11: SensorMeasurements PositionSensors; + 12: SensorMeasurements VelocitySensors; } struct SensorMetadata { @@ -44,15 +45,16 @@ struct SensorRPCData { 1: list ThreeAxisGyroscopes; 2: list ThreeAxisLinearAccelerometers; - 3: list ThreeAxisMagnetometers; - 4: list OrientationSensors; - 5: list TemperatureSensors; - 6: list SixAxisForceTorqueSensors; - 7: list ContactLoadCellArrays; - 8: list EncoderArrays; - 9: list SkinPatches; - 10: list PositionSensors; - 11: list VelocitySensors; + 3: list ThreeAxisAngularAccelerometers; + 4: list ThreeAxisMagnetometers; + 5: list OrientationSensors; + 6: list TemperatureSensors; + 7: list SixAxisForceTorqueSensors; + 8: list ContactLoadCellArrays; + 9: list EncoderArrays; + 10: list SkinPatches; + 11: list PositionSensors; + 12: list VelocitySensors; } service MultipleAnalogSensorsMetadata diff --git a/src/devices/multipleanalogsensorsclient/MultipleAnalogSensorsClient.cpp b/src/devices/multipleanalogsensorsclient/MultipleAnalogSensorsClient.cpp index 5aae8b20ce8..08fc9cdb488 100644 --- a/src/devices/multipleanalogsensorsclient/MultipleAnalogSensorsClient.cpp +++ b/src/devices/multipleanalogsensorsclient/MultipleAnalogSensorsClient.cpp @@ -378,6 +378,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, diff --git a/src/devices/multipleanalogsensorsclient/MultipleAnalogSensorsClient.h b/src/devices/multipleanalogsensorsclient/MultipleAnalogSensorsClient.h index 2aef37483e3..4409bd12f10 100644 --- a/src/devices/multipleanalogsensorsclient/MultipleAnalogSensorsClient.h +++ b/src/devices/multipleanalogsensorsclient/MultipleAnalogSensorsClient.h @@ -56,6 +56,7 @@ 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::IVelocitySensors, @@ -113,6 +114,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; diff --git a/src/devices/multipleanalogsensorsremapper/MultipleAnalogSensorsRemapper.cpp b/src/devices/multipleanalogsensorsremapper/MultipleAnalogSensorsRemapper.cpp index d76ea80c66b..3b02691d3a7 100644 --- a/src/devices/multipleanalogsensorsremapper/MultipleAnalogSensorsRemapper.cpp +++ b/src/devices/multipleanalogsensorsremapper/MultipleAnalogSensorsRemapper.cpp @@ -18,7 +18,7 @@ namespace { YARP_LOG_COMPONENT(MULTIPLEANALOGSENSORSREMAPPER, "yarp.device.multipleanalogsensorsremapper") } -const size_t MAS_NrOfSensorTypes{11}; +const size_t MAS_NrOfSensorTypes{12}; static_assert(MAS_SensorType::VelocitySensors+1 == MAS_NrOfSensorTypes, "Consistency error between MAS_NrOfSensorTypes and MAS_SensorType"); /** @@ -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; @@ -227,6 +230,8 @@ 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, @@ -253,6 +258,7 @@ bool MultipleAnalogSensorsRemapper::detachAll() { m_iThreeAxisGyroscopes.resize(0); m_iThreeAxisLinearAccelerometers.resize(0); + m_iThreeAxisAngularAccelerometers.resize(0); m_iThreeAxisMagnetometers.resize(0); m_iPositionSensors.resize(0); m_iVelocitySensors.resize(0); @@ -449,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(); diff --git a/src/devices/multipleanalogsensorsremapper/MultipleAnalogSensorsRemapper.h b/src/devices/multipleanalogsensorsremapper/MultipleAnalogSensorsRemapper.h index dafce3a6278..78ffc09805b 100644 --- a/src/devices/multipleanalogsensorsremapper/MultipleAnalogSensorsRemapper.h +++ b/src/devices/multipleanalogsensorsremapper/MultipleAnalogSensorsRemapper.h @@ -21,15 +21,16 @@ enum MAS_SensorType { ThreeAxisGyroscopes=0, ThreeAxisLinearAccelerometers=1, - ThreeAxisMagnetometers=2, - OrientationSensors=3, - TemperatureSensors=4, - SixAxisForceTorqueSensors=5, - ContactLoadCellArrays=6, - EncoderArrays=7, - SkinPatches=8, - PositionSensors=9, - VelocitySensors=10 + ThreeAxisAngularAccelerometers=2, + ThreeAxisMagnetometers=3, + OrientationSensors=4, + TemperatureSensors=5, + SixAxisForceTorqueSensors=6, + ContactLoadCellArrays=7, + EncoderArrays=8, + SkinPatches=9, + PositionSensors=10, + VelocitySensors=11 }; /** @@ -87,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, @@ -120,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; @@ -192,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; diff --git a/src/devices/multipleanalogsensorsserver/MultipleAnalogSensorsServer.cpp b/src/devices/multipleanalogsensorsserver/MultipleAnalogSensorsServer.cpp index 15fc4a49419..b6355f36eb3 100644 --- a/src/devices/multipleanalogsensorsserver/MultipleAnalogSensorsServer.cpp +++ b/src/devices/multipleanalogsensorsserver/MultipleAnalogSensorsServer.cpp @@ -25,15 +25,16 @@ enum MAS_SensorTypeServer { ThreeAxisGyroscopes=0, ThreeAxisLinearAccelerometers=1, - ThreeAxisMagnetometers=2, - OrientationSensors=3, - TemperatureSensors=4, - SixAxisForceTorqueSensors=5, - ContactLoadCellArrays=6, - EncoderArrays=7, - SkinPatches=8, - PositionSensors=9, - VelocitySensors=10 + ThreeAxisAngularAccelerometers=2, + ThreeAxisMagnetometers=3, + OrientationSensors=4, + TemperatureSensors=5, + SixAxisForceTorqueSensors=6, + ContactLoadCellArrays=7, + EncoderArrays=8, + SkinPatches=9, + PositionSensors=10, + VelocitySensors=11 }; /** @@ -49,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; @@ -303,6 +307,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, @@ -389,6 +397,8 @@ 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, @@ -445,6 +455,7 @@ 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_iVelocitySensors); @@ -579,6 +590,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, diff --git a/src/devices/multipleanalogsensorsserver/MultipleAnalogSensorsServer.h b/src/devices/multipleanalogsensorsserver/MultipleAnalogSensorsServer.h index bf70161e666..8da015a9830 100644 --- a/src/devices/multipleanalogsensorsserver/MultipleAnalogSensorsServer.h +++ b/src/devices/multipleanalogsensorsserver/MultipleAnalogSensorsServer.h @@ -59,6 +59,7 @@ 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::IVelocitySensors* m_iVelocitySensors{nullptr}; diff --git a/src/libYARP_dev/src/yarp/dev/MultipleAnalogSensorsInterfaces.h b/src/libYARP_dev/src/yarp/dev/MultipleAnalogSensorsInterfaces.h index cd9a00e6772..7312a20a809 100644 --- a/src/libYARP_dev/src/yarp/dev/MultipleAnalogSensorsInterfaces.h +++ b/src/libYARP_dev/src/yarp/dev/MultipleAnalogSensorsInterfaces.h @@ -15,6 +15,7 @@ namespace yarp::dev { class IThreeAxisGyroscopes; class IThreeAxisLinearAccelerometers; +class IThreeAxisAngularAccelerometers; class IThreeAxisMagnetometers; class IPositionSensors; class IVelocitySensors; @@ -146,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 linear 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 meters^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 * From 3c5625e2c8a430da81a5a96e593e305145f7ae02 Mon Sep 17 00:00:00 2001 From: Nick Tremaroli Date: Sat, 16 Nov 2024 14:58:04 +0100 Subject: [PATCH 04/13] Reset IDL generation flag --- cmake/YarpIDL.cmake | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/cmake/YarpIDL.cmake b/cmake/YarpIDL.cmake index bc4f90c7638..76450f090c3 100644 --- a/cmake/YarpIDL.cmake +++ b/cmake/YarpIDL.cmake @@ -193,7 +193,7 @@ endfunction() function(YARP_IDL_TO_DIR) # Flag to control whether IDL generation is allowed. - option(ALLOW_IDL_GENERATION "Allow YARP to (re)build IDL files as needed" ON) + option(ALLOW_IDL_GENERATION "Allow YARP to (re)build IDL files as needed" OFF) set(_options VERBOSE From f2318c7cf39fc5ba7af5fe0932460cf605c25ffd Mon Sep 17 00:00:00 2001 From: Nick Tremaroli Date: Sat, 16 Nov 2024 16:03:33 +0100 Subject: [PATCH 05/13] Changed name: VelocitySensor to LinearVelocitySensor --- .../idl_generated_code/SensorRPCData.h | 2 +- ...multipleAnalogSensorsSerializations.thrift | 4 +-- .../MultipleAnalogSensorsRemapper.cpp | 32 +++++++++---------- .../MultipleAnalogSensorsRemapper.h | 18 +++++------ .../MultipleAnalogSensorsClient.cpp | 20 ++++++------ .../MultipleAnalogSensorsClient.h | 14 ++++---- .../MultipleAnalogSensorsServer.cpp | 28 ++++++++-------- .../MultipleAnalogSensorsServer.h | 2 +- .../dev/MultipleAnalogSensorsInterfaces.h | 22 ++++++------- 9 files changed, 71 insertions(+), 71 deletions(-) diff --git a/src/devices/messages/multipleAnalogSensorsMsgs/idl_generated_code/SensorRPCData.h b/src/devices/messages/multipleAnalogSensorsMsgs/idl_generated_code/SensorRPCData.h index 52e0c98206f..a99425b8475 100644 --- a/src/devices/messages/multipleAnalogSensorsMsgs/idl_generated_code/SensorRPCData.h +++ b/src/devices/messages/multipleAnalogSensorsMsgs/idl_generated_code/SensorRPCData.h @@ -30,7 +30,7 @@ class SensorRPCData : std::vector EncoderArrays{}; std::vector SkinPatches{}; std::vector PositionSensors{}; - std::vector VelocitySensors{}; + std::vector LinearVelocitySensors{}; // Default constructor SensorRPCData() = default; diff --git a/src/devices/messages/multipleAnalogSensorsMsgs/multipleAnalogSensorsSerializations.thrift b/src/devices/messages/multipleAnalogSensorsMsgs/multipleAnalogSensorsSerializations.thrift index c7197fdcca5..6a4379d3818 100644 --- a/src/devices/messages/multipleAnalogSensorsMsgs/multipleAnalogSensorsSerializations.thrift +++ b/src/devices/messages/multipleAnalogSensorsMsgs/multipleAnalogSensorsSerializations.thrift @@ -32,7 +32,7 @@ struct SensorStreamingData 9: SensorMeasurements EncoderArrays; 10: SensorMeasurements SkinPatches; 11: SensorMeasurements PositionSensors; - 12: SensorMeasurements VelocitySensors; + 12: SensorMeasurements LinearVelocitySensors; } struct SensorMetadata { @@ -54,7 +54,7 @@ struct SensorRPCData 9: list EncoderArrays; 10: list SkinPatches; 11: list PositionSensors; - 12: list VelocitySensors; + 12: list LinearVelocitySensors; } service MultipleAnalogSensorsMetadata diff --git a/src/devices/multipleanalogsensorsremapper/MultipleAnalogSensorsRemapper.cpp b/src/devices/multipleanalogsensorsremapper/MultipleAnalogSensorsRemapper.cpp index 3b02691d3a7..e97b5e67e2d 100644 --- a/src/devices/multipleanalogsensorsremapper/MultipleAnalogSensorsRemapper.cpp +++ b/src/devices/multipleanalogsensorsremapper/MultipleAnalogSensorsRemapper.cpp @@ -19,7 +19,7 @@ YARP_LOG_COMPONENT(MULTIPLEANALOGSENSORSREMAPPER, "yarp.device.multipleanalogsen } const size_t MAS_NrOfSensorTypes{12}; -static_assert(MAS_SensorType::VelocitySensors+1 == MAS_NrOfSensorTypes, "Consistency error between MAS_NrOfSensorTypes and MAS_SensorType"); +static_assert(MAS_SensorType::LinearVelocitySensors+1 == MAS_NrOfSensorTypes, "Consistency error between MAS_NrOfSensorTypes and MAS_SensorType"); /** * Internal identifier of the type of sensors. @@ -61,8 +61,8 @@ inline std::string MAS_getTagFromEnum(const MAS_SensorType type) case PositionSensors: return "PositionSensors"; break; - case VelocitySensors: - return "VelocitySensors"; + case LinearVelocitySensors: + return "LinearVelocitySensors"; break; default: assert(false); @@ -236,8 +236,8 @@ bool MultipleAnalogSensorsRemapper::attachAll(const PolyDriverList &polylist) &IThreeAxisMagnetometers::getThreeAxisMagnetometerName, &IThreeAxisMagnetometers::getNrOfThreeAxisMagnetometers); ok = ok && genericAttachAll(PositionSensors, m_iPositionSensors, polylist, &IPositionSensors::getPositionSensorName, &IPositionSensors::getNrOfPositionSensors); - ok = ok && genericAttachAll(VelocitySensors, m_iVelocitySensors, polylist, - &IVelocitySensors::getVelocitySensorName, &IVelocitySensors::getNrOfVelocitySensors); + 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, @@ -261,7 +261,7 @@ bool MultipleAnalogSensorsRemapper::detachAll() m_iThreeAxisAngularAccelerometers.resize(0); m_iThreeAxisMagnetometers.resize(0); m_iPositionSensors.resize(0); - m_iVelocitySensors.resize(0); + m_iLinearVelocitySensors.resize(0); m_iOrientationSensors.resize(0); m_iTemperatureSensors.resize(0); m_iSixAxisForceTorqueSensors.resize(0); @@ -530,29 +530,29 @@ bool MultipleAnalogSensorsRemapper::getPositionSensorMeasure(size_t sens_index, return genericGetMeasure(PositionSensors, sens_index, out, timestamp, m_iPositionSensors, &IPositionSensors::getPositionSensorMeasure); } -size_t MultipleAnalogSensorsRemapper::getNrOfVelocitySensors() const +size_t MultipleAnalogSensorsRemapper::getNrOfLinearVelocitySensors() const { - return m_indicesMap[VelocitySensors].size(); + return m_indicesMap[LinearVelocitySensors].size(); } -MAS_status MultipleAnalogSensorsRemapper::getVelocitySensorStatus(size_t sens_index) const +MAS_status MultipleAnalogSensorsRemapper::getLinearVelocitySensorStatus(size_t sens_index) const { - return genericGetStatus(VelocitySensors, sens_index, m_iVelocitySensors, &IVelocitySensors::getVelocitySensorStatus); + return genericGetStatus(LinearVelocitySensors, sens_index, m_iLinearVelocitySensors, &ILinearVelocitySensors::getLinearVelocitySensorStatus); } -bool MultipleAnalogSensorsRemapper::getVelocitySensorName(size_t sens_index, std::string& name) const +bool MultipleAnalogSensorsRemapper::getLinearVelocitySensorName(size_t sens_index, std::string& name) const { - return genericGetName(VelocitySensors, sens_index, name, m_iVelocitySensors, &IVelocitySensors::getVelocitySensorName); + return genericGetName(LinearVelocitySensors, sens_index, name, m_iLinearVelocitySensors, &ILinearVelocitySensors::getLinearVelocitySensorName); } -bool MultipleAnalogSensorsRemapper::getVelocitySensorFrameName(size_t sens_index, std::string& frameName) const +bool MultipleAnalogSensorsRemapper::getLinearVelocitySensorFrameName(size_t sens_index, std::string& frameName) const { - return genericGetFrameName(VelocitySensors, sens_index, frameName, m_iVelocitySensors, &IVelocitySensors::getVelocitySensorFrameName); + return genericGetFrameName(LinearVelocitySensors, sens_index, frameName, m_iLinearVelocitySensors, &ILinearVelocitySensors::getLinearVelocitySensorFrameName); } -bool MultipleAnalogSensorsRemapper::getVelocitySensorMeasure(size_t sens_index, yarp::sig::Vector& out, double& timestamp) const +bool MultipleAnalogSensorsRemapper::getLinearVelocitySensorMeasure(size_t sens_index, yarp::sig::Vector& out, double& timestamp) const { - return genericGetMeasure(VelocitySensors, sens_index, out, timestamp, m_iVelocitySensors, &IVelocitySensors::getVelocitySensorMeasure); + return genericGetMeasure(LinearVelocitySensors, sens_index, out, timestamp, m_iLinearVelocitySensors, &ILinearVelocitySensors::getLinearVelocitySensorMeasure); } size_t MultipleAnalogSensorsRemapper::getNrOfOrientationSensors() const diff --git a/src/devices/multipleanalogsensorsremapper/MultipleAnalogSensorsRemapper.h b/src/devices/multipleanalogsensorsremapper/MultipleAnalogSensorsRemapper.h index d8bef5c9943..adc99cfb926 100644 --- a/src/devices/multipleanalogsensorsremapper/MultipleAnalogSensorsRemapper.h +++ b/src/devices/multipleanalogsensorsremapper/MultipleAnalogSensorsRemapper.h @@ -30,7 +30,7 @@ enum MAS_SensorType EncoderArrays=8, SkinPatches=9, PositionSensors=10, - VelocitySensors=11 + LinearVelocitySensors=11 }; /** @@ -97,7 +97,7 @@ class MultipleAnalogSensorsRemapper : public yarp::dev::IEncoderArrays, public yarp::dev::ISkinPatches, public yarp::dev::IPositionSensors, - public yarp::dev::IVelocitySensors + public yarp::dev::ILinearVelocitySensors { private: bool m_verbose{false}; @@ -131,7 +131,7 @@ class MultipleAnalogSensorsRemapper : std::vector m_iEncoderArrays; std::vector m_iSkinPatches; std::vector m_iPositionSensors; - std::vector m_iVelocitySensors; + std::vector m_iLinearVelocitySensors; // Templated methods to streamline of the function implementation for all the different sensors @@ -259,12 +259,12 @@ class MultipleAnalogSensorsRemapper : 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; - /* IVelocitySensors methods */ - size_t getNrOfVelocitySensors() const override; - yarp::dev::MAS_status getVelocitySensorStatus(size_t sens_index) const override; - bool getVelocitySensorName(size_t sens_index, std::string& name) const override; - bool getVelocitySensorFrameName(size_t sens_index, std::string& frameName) const override; - bool getVelocitySensorMeasure(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 d2ae498a49e..ee2a322e34d 100644 --- a/src/devices/networkWrappers/multipleanalogsensorsclient/MultipleAnalogSensorsClient.cpp +++ b/src/devices/networkWrappers/multipleanalogsensorsclient/MultipleAnalogSensorsClient.cpp @@ -452,30 +452,30 @@ 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::getNrOfVelocitySensors() const +size_t MultipleAnalogSensorsClient::getNrOfLinearVelocitySensors() const { - return genericGetNrOfSensors(m_sensorsMetadata.VelocitySensors, - m_streamingPort.receivedData.VelocitySensors); + return genericGetNrOfSensors(m_sensorsMetadata.LinearVelocitySensors, + m_streamingPort.receivedData.LinearVelocitySensors); } -yarp::dev::MAS_status MultipleAnalogSensorsClient::getVelocitySensorStatus(size_t sens_index) const +yarp::dev::MAS_status MultipleAnalogSensorsClient::getLinearVelocitySensorStatus(size_t sens_index) const { return genericGetStatus(); } -bool MultipleAnalogSensorsClient::getVelocitySensorName(size_t sens_index, std::string& name) const +bool MultipleAnalogSensorsClient::getLinearVelocitySensorName(size_t sens_index, std::string& name) const { - return genericGetName(m_sensorsMetadata.VelocitySensors, "VelocitySensors", sens_index, name); + return genericGetName(m_sensorsMetadata.LinearVelocitySensors, "LinearVelocitySensors", sens_index, name); } -bool MultipleAnalogSensorsClient::getVelocitySensorFrameName(size_t sens_index, std::string& frameName) const +bool MultipleAnalogSensorsClient::getLinearVelocitySensorFrameName(size_t sens_index, std::string& frameName) const { - return genericGetFrameName(m_sensorsMetadata.VelocitySensors, "VelocitySensors", sens_index, frameName); + return genericGetFrameName(m_sensorsMetadata.LinearVelocitySensors, "LinearVelocitySensors", sens_index, frameName); } -bool MultipleAnalogSensorsClient::getVelocitySensorMeasure(size_t sens_index, yarp::sig::Vector& out, double& timestamp) const +bool MultipleAnalogSensorsClient::getLinearVelocitySensorMeasure(size_t sens_index, yarp::sig::Vector& out, double& timestamp) const { - return genericGetMeasure(m_sensorsMetadata.VelocitySensors, "VelocitySensors", m_streamingPort.receivedData.VelocitySensors, sens_index, out, timestamp); + return genericGetMeasure(m_sensorsMetadata.LinearVelocitySensors, "LinearVelocitySensors", m_streamingPort.receivedData.LinearVelocitySensors, sens_index, out, timestamp); } size_t MultipleAnalogSensorsClient::getNrOfTemperatureSensors() const diff --git a/src/devices/networkWrappers/multipleanalogsensorsclient/MultipleAnalogSensorsClient.h b/src/devices/networkWrappers/multipleanalogsensorsclient/MultipleAnalogSensorsClient.h index f541e9dc971..dfa4fce3647 100644 --- a/src/devices/networkWrappers/multipleanalogsensorsclient/MultipleAnalogSensorsClient.h +++ b/src/devices/networkWrappers/multipleanalogsensorsclient/MultipleAnalogSensorsClient.h @@ -50,7 +50,7 @@ class MultipleAnalogSensorsClient : public yarp::dev::IThreeAxisAngularAccelerometers, public yarp::dev::IThreeAxisMagnetometers, public yarp::dev::IPositionSensors, - public yarp::dev::IVelocitySensors, + public yarp::dev::ILinearVelocitySensors, public yarp::dev::IOrientationSensors, public yarp::dev::ITemperatureSensors, public yarp::dev::ISixAxisForceTorqueSensors, @@ -119,12 +119,12 @@ 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; - /* IVelocitySensors methods */ - size_t getNrOfVelocitySensors() const override; - yarp::dev::MAS_status getVelocitySensorStatus(size_t sens_index) const override; - bool getVelocitySensorName(size_t sens_index, std::string& name) const override; - bool getVelocitySensorFrameName(size_t sens_index, std::string& frameName) const override; - bool getVelocitySensorMeasure(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; diff --git a/src/devices/networkWrappers/multipleanalogsensorsserver/MultipleAnalogSensorsServer.cpp b/src/devices/networkWrappers/multipleanalogsensorsserver/MultipleAnalogSensorsServer.cpp index 73d07423a29..8998fed7cf2 100644 --- a/src/devices/networkWrappers/multipleanalogsensorsserver/MultipleAnalogSensorsServer.cpp +++ b/src/devices/networkWrappers/multipleanalogsensorsserver/MultipleAnalogSensorsServer.cpp @@ -34,7 +34,7 @@ enum MAS_SensorTypeServer EncoderArrays=8, SkinPatches=9, PositionSensors=10, - VelocitySensors=11 + LinearVelocitySensors=11 }; /** @@ -62,7 +62,7 @@ inline size_t MAS_getMeasureSizeFromEnum(const MAS_SensorTypeServer type) case PositionSensors: return 3; break; - case VelocitySensors: + case LinearVelocitySensors: return 3; break; case TemperatureSensors: @@ -263,10 +263,10 @@ bool MultipleAnalogSensorsServer::populateAllSensorsMetadata() &yarp::dev::IPositionSensors::getNrOfPositionSensors, &yarp::dev::IPositionSensors::getPositionSensorName, &yarp::dev::IPositionSensors::getPositionSensorFrameName); - ok = ok && populateSensorsMetadata(m_iVelocitySensors, m_sensorMetadata.VelocitySensors, "VelocitySensors", - &yarp::dev::IVelocitySensors::getNrOfVelocitySensors, - &yarp::dev::IVelocitySensors::getVelocitySensorName, - &yarp::dev::IVelocitySensors::getVelocitySensorFrameName); + 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, @@ -347,8 +347,8 @@ bool MultipleAnalogSensorsServer::resizeAllMeasureVectors(SensorStreamingData& s streamingData.ThreeAxisMagnetometers.measurements, MAS_getMeasureSizeFromEnum(ThreeAxisMagnetometers)); ok = ok && resizeMeasureVectors(m_iPositionSensors, m_sensorMetadata.PositionSensors, streamingData.PositionSensors.measurements, MAS_getMeasureSizeFromEnum(PositionSensors)); - ok = ok && resizeMeasureVectors(m_iVelocitySensors, m_sensorMetadata.VelocitySensors, - streamingData.VelocitySensors.measurements, MAS_getMeasureSizeFromEnum(VelocitySensors)); + 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, @@ -402,7 +402,7 @@ bool MultipleAnalogSensorsServer::attachAll(const yarp::dev::PolyDriverList& p) poly->view(m_iThreeAxisAngularAccelerometers); poly->view(m_iThreeAxisMagnetometers); poly->view(m_iPositionSensors); - poly->view(m_iVelocitySensors); + poly->view(m_iLinearVelocitySensors); poly->view(m_iOrientationSensors); poly->view(m_iTemperatureSensors); poly->view(m_iSixAxisForceTorqueSensors); @@ -552,11 +552,11 @@ void MultipleAnalogSensorsServer::run() &yarp::dev::IPositionSensors::getPositionSensorMeasure, "PositionSensor"); - ok = ok && genericStreamData(m_iVelocitySensors, m_sensorMetadata.VelocitySensors, - streamingData.VelocitySensors.measurements, - &yarp::dev::IVelocitySensors::getVelocitySensorStatus, - &yarp::dev::IVelocitySensors::getVelocitySensorMeasure, - "VelocitySensor"); + 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, diff --git a/src/devices/networkWrappers/multipleanalogsensorsserver/MultipleAnalogSensorsServer.h b/src/devices/networkWrappers/multipleanalogsensorsserver/MultipleAnalogSensorsServer.h index 2244de5a7d9..6cbb4555164 100644 --- a/src/devices/networkWrappers/multipleanalogsensorsserver/MultipleAnalogSensorsServer.h +++ b/src/devices/networkWrappers/multipleanalogsensorsserver/MultipleAnalogSensorsServer.h @@ -52,7 +52,7 @@ class MultipleAnalogSensorsServer : yarp::dev::IThreeAxisAngularAccelerometers* m_iThreeAxisAngularAccelerometers{nullptr}; yarp::dev::IThreeAxisMagnetometers* m_iThreeAxisMagnetometers{nullptr}; yarp::dev::IPositionSensors* m_iPositionSensors{nullptr}; - yarp::dev::IVelocitySensors* m_iVelocitySensors{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 7312a20a809..21fa3bf44dc 100644 --- a/src/libYARP_dev/src/yarp/dev/MultipleAnalogSensorsInterfaces.h +++ b/src/libYARP_dev/src/yarp/dev/MultipleAnalogSensorsInterfaces.h @@ -18,7 +18,7 @@ class IThreeAxisLinearAccelerometers; class IThreeAxisAngularAccelerometers; class IThreeAxisMagnetometers; class IPositionSensors; -class IVelocitySensors; +class ILinearVelocitySensors; class IOrientationSensors; class ITemperatureSensors; class ISixAxisForceTorqueSensors; @@ -289,24 +289,24 @@ class YARP_dev_API yarp::dev::IPositionSensors } }; -class YARP_dev_API yarp::dev::IVelocitySensors +class YARP_dev_API yarp::dev::ILinearVelocitySensors { public: /** - * Get the number of velocity sensors exposed by this device. + * Get the number of linear velocity sensors exposed by this device. */ - virtual size_t getNrOfVelocitySensors() const = 0; + virtual size_t getNrOfLinearVelocitySensors() const = 0; /** * Get the status of the specified sensor. */ - virtual yarp::dev::MAS_status getVelocitySensorStatus(size_t sens_index) const = 0; + 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 getVelocitySensorName(size_t sens_index, std::string& name) const = 0; + virtual bool getLinearVelocitySensorName(size_t sens_index, std::string& name) const = 0; /** * Get the name of the frame of the specified sensor. @@ -316,19 +316,19 @@ class YARP_dev_API yarp::dev::IVelocitySensors * * @return false if an error occurred, true otherwise. */ - virtual bool getVelocitySensorFrameName(size_t sens_index, std::string& frameName) const = 0; + virtual bool getLinearVelocitySensorFrameName(size_t sens_index, std::string& frameName) const = 0; /** - * Get the last reading of the velocity sensor as x y z. + * 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 getNrOfVelocitySensors()-1). + * @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. * @param[out] timestamp The timestamp of the requested measure, expressed in seconds. * @return false if an error occurred, true otherwise. */ - virtual bool getVelocitySensorMeasure(size_t sens_index, yarp::sig::Vector& xyz, double& timestamp) const = 0; + virtual bool getLinearVelocitySensorMeasure(size_t sens_index, yarp::sig::Vector& xyz, double& timestamp) const = 0; - virtual ~IVelocitySensors() + virtual ~ILinearVelocitySensors() { } }; From 8b9ddb2ee16e54552f1d2af8c77c3d9dd4ec1fa6 Mon Sep 17 00:00:00 2001 From: Nick Tremaroli Date: Tue, 19 Nov 2024 11:56:59 +0100 Subject: [PATCH 06/13] Changed order in thrift files --- ...multipleAnalogSensorsSerializations.thrift | 40 +++++++++---------- 1 file changed, 20 insertions(+), 20 deletions(-) diff --git a/src/devices/messages/multipleAnalogSensorsMsgs/multipleAnalogSensorsSerializations.thrift b/src/devices/messages/multipleAnalogSensorsMsgs/multipleAnalogSensorsSerializations.thrift index 6a4379d3818..16622722833 100644 --- a/src/devices/messages/multipleAnalogSensorsMsgs/multipleAnalogSensorsSerializations.thrift +++ b/src/devices/messages/multipleAnalogSensorsMsgs/multipleAnalogSensorsSerializations.thrift @@ -23,16 +23,16 @@ struct SensorStreamingData { 1: SensorMeasurements ThreeAxisGyroscopes; 2: SensorMeasurements ThreeAxisLinearAccelerometers; - 3: SensorMeasurements ThreeAxisAngularAccelerometers; - 4: SensorMeasurements ThreeAxisMagnetometers; - 5: SensorMeasurements OrientationSensors; - 6: SensorMeasurements TemperatureSensors; - 7: SensorMeasurements SixAxisForceTorqueSensors; - 8: SensorMeasurements ContactLoadCellArrays; - 9: SensorMeasurements EncoderArrays; - 10: SensorMeasurements SkinPatches; - 11: SensorMeasurements PositionSensors; - 12: SensorMeasurements LinearVelocitySensors; + 3: SensorMeasurements ThreeAxisMagnetometers; + 4: SensorMeasurements OrientationSensors; + 5: SensorMeasurements TemperatureSensors; + 6: SensorMeasurements SixAxisForceTorqueSensors; + 7: SensorMeasurements ContactLoadCellArrays; + 8: SensorMeasurements EncoderArrays; + 9: SensorMeasurements SkinPatches; + 10: SensorMeasurements PositionSensors; + 11: SensorMeasurements LinearVelocitySensors; + 12: SensorMeasurements ThreeAxisAngularAccelerometers; } struct SensorMetadata { @@ -45,16 +45,16 @@ struct SensorRPCData { 1: list ThreeAxisGyroscopes; 2: list ThreeAxisLinearAccelerometers; - 3: list ThreeAxisAngularAccelerometers; - 4: list ThreeAxisMagnetometers; - 5: list OrientationSensors; - 6: list TemperatureSensors; - 7: list SixAxisForceTorqueSensors; - 8: list ContactLoadCellArrays; - 9: list EncoderArrays; - 10: list SkinPatches; - 11: list PositionSensors; - 12: list LinearVelocitySensors; + 3: list ThreeAxisMagnetometers; + 4: list OrientationSensors; + 5: list TemperatureSensors; + 6: list SixAxisForceTorqueSensors; + 7: list ContactLoadCellArrays; + 8: list EncoderArrays; + 9: list SkinPatches; + 10: list PositionSensors; + 11: list LinearVelocitySensors; + 12: list ThreeAxisAngularAccelerometers; } service MultipleAnalogSensorsMetadata From b4811f61f07d702261a18aba62fbedf9235dc98b Mon Sep 17 00:00:00 2001 From: Nick Tremaroli Date: Tue, 19 Nov 2024 11:57:24 +0100 Subject: [PATCH 07/13] Added autogenerated IDL code --- .../idl_generated_code/SensorRPCData.cpp | 221 +++++++++++++++++- .../idl_generated_code/SensorRPCData.h | 21 +- .../SensorStreamingData.cpp | 116 ++++++++- .../idl_generated_code/SensorStreamingData.h | 18 +- 4 files changed, 360 insertions(+), 16 deletions(-) diff --git a/src/devices/messages/multipleAnalogSensorsMsgs/idl_generated_code/SensorRPCData.cpp b/src/devices/messages/multipleAnalogSensorsMsgs/idl_generated_code/SensorRPCData.cpp index 42c59d803d6..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,11 +33,12 @@ SensorRPCData::SensorRPCData(const std::vector& ThreeAxisGyrosco ContactLoadCellArrays(ContactLoadCellArrays), EncoderArrays(EncoderArrays), SkinPatches(SkinPatches), - PositionSensors(PositionSensors) + PositionSensors(PositionSensors), + LinearVelocitySensors(LinearVelocitySensors), + ThreeAxisAngularAccelerometers(ThreeAxisAngularAccelerometers) { } - // Read structure on a Wire bool SensorRPCData::read(yarp::os::idl::WireReader& reader) { @@ -69,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; } @@ -79,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)) { @@ -121,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; } @@ -131,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)) { @@ -1129,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 a99425b8475..dd8b0558761 100644 --- a/src/devices/messages/multipleAnalogSensorsMsgs/idl_generated_code/SensorRPCData.h +++ b/src/devices/messages/multipleAnalogSensorsMsgs/idl_generated_code/SensorRPCData.h @@ -31,6 +31,7 @@ class SensorRPCData : std::vector SkinPatches{}; std::vector PositionSensors{}; std::vector LinearVelocitySensors{}; + std::vector ThreeAxisAngularAccelerometers{}; // Default constructor SensorRPCData() = default; @@ -45,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; @@ -126,11 +129,17 @@ class SensorRPCData : bool nested_read_PositionSensors(yarp::os::idl::WireReader& reader); bool nested_write_PositionSensors(const yarp::os::idl::WireWriter& writer) const; - // read/write VelocitySensors field - bool read_VelocitySensors(yarp::os::idl::WireReader& reader); - bool write_VelocitySensors(const yarp::os::idl::WireWriter& writer) const; - bool nested_read_VelocitySensors(yarp::os::idl::WireReader& reader); - bool nested_write_VelocitySensors(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 From 8ec2a36b969862e466e87ce3044598b332820df3 Mon Sep 17 00:00:00 2001 From: Nick Tremaroli Date: Tue, 26 Nov 2024 14:36:27 +0100 Subject: [PATCH 08/13] Adjusted enumeration for compatibility --- .../MultipleAnalogSensorsRemapper.cpp | 2 +- .../MultipleAnalogSensorsRemapper.h | 20 +++++++++---------- .../MultipleAnalogSensorsServer.cpp | 20 +++++++++---------- 3 files changed, 21 insertions(+), 21 deletions(-) diff --git a/src/devices/multipleanalogsensorsremapper/MultipleAnalogSensorsRemapper.cpp b/src/devices/multipleanalogsensorsremapper/MultipleAnalogSensorsRemapper.cpp index e97b5e67e2d..5e0c5f2d9ee 100644 --- a/src/devices/multipleanalogsensorsremapper/MultipleAnalogSensorsRemapper.cpp +++ b/src/devices/multipleanalogsensorsremapper/MultipleAnalogSensorsRemapper.cpp @@ -19,7 +19,7 @@ YARP_LOG_COMPONENT(MULTIPLEANALOGSENSORSREMAPPER, "yarp.device.multipleanalogsen } const size_t MAS_NrOfSensorTypes{12}; -static_assert(MAS_SensorType::LinearVelocitySensors+1 == MAS_NrOfSensorTypes, "Consistency error between MAS_NrOfSensorTypes and MAS_SensorType"); +static_assert(MAS_SensorType::ThreeAxisAngularAccelerometers+1 == MAS_NrOfSensorTypes, "Consistency error between MAS_NrOfSensorTypes and MAS_SensorType"); /** * Internal identifier of the type of sensors. diff --git a/src/devices/multipleanalogsensorsremapper/MultipleAnalogSensorsRemapper.h b/src/devices/multipleanalogsensorsremapper/MultipleAnalogSensorsRemapper.h index adc99cfb926..a9c7af19245 100644 --- a/src/devices/multipleanalogsensorsremapper/MultipleAnalogSensorsRemapper.h +++ b/src/devices/multipleanalogsensorsremapper/MultipleAnalogSensorsRemapper.h @@ -21,16 +21,16 @@ enum MAS_SensorType { ThreeAxisGyroscopes=0, ThreeAxisLinearAccelerometers=1, - ThreeAxisAngularAccelerometers=2, - ThreeAxisMagnetometers=3, - OrientationSensors=4, - TemperatureSensors=5, - SixAxisForceTorqueSensors=6, - ContactLoadCellArrays=7, - EncoderArrays=8, - SkinPatches=9, - PositionSensors=10, - LinearVelocitySensors=11 + ThreeAxisMagnetometers=2, + OrientationSensors=3, + TemperatureSensors=4, + SixAxisForceTorqueSensors=5, + ContactLoadCellArrays=6, + EncoderArrays=7, + SkinPatches=8, + PositionSensors=9, + LinearVelocitySensors=10, + ThreeAxisAngularAccelerometers=11 }; /** diff --git a/src/devices/networkWrappers/multipleanalogsensorsserver/MultipleAnalogSensorsServer.cpp b/src/devices/networkWrappers/multipleanalogsensorsserver/MultipleAnalogSensorsServer.cpp index 8998fed7cf2..23635866338 100644 --- a/src/devices/networkWrappers/multipleanalogsensorsserver/MultipleAnalogSensorsServer.cpp +++ b/src/devices/networkWrappers/multipleanalogsensorsserver/MultipleAnalogSensorsServer.cpp @@ -25,16 +25,16 @@ enum MAS_SensorTypeServer { ThreeAxisGyroscopes=0, ThreeAxisLinearAccelerometers=1, - ThreeAxisAngularAccelerometers=2, - ThreeAxisMagnetometers=3, - OrientationSensors=4, - TemperatureSensors=5, - SixAxisForceTorqueSensors=6, - ContactLoadCellArrays=7, - EncoderArrays=8, - SkinPatches=9, - PositionSensors=10, - LinearVelocitySensors=11 + ThreeAxisMagnetometers=2, + OrientationSensors=3, + TemperatureSensors=4, + SixAxisForceTorqueSensors=5, + ContactLoadCellArrays=6, + EncoderArrays=7, + SkinPatches=8, + PositionSensors=9, + LinearVelocitySensors=10, + ThreeAxisAngularAccelerometers=11 }; /** From 97057d4b1f22b79b37f6004bd12dc91d0f97dc0a Mon Sep 17 00:00:00 2001 From: Nick Tremaroli Date: Tue, 26 Nov 2024 14:41:28 +0100 Subject: [PATCH 09/13] Updated release notes --- doc/release/master.md | 5 +++++ 1 file changed, 5 insertions(+) create mode 100644 doc/release/master.md 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. From f2a0b04ad2fade48f9bfa76492695af1a5778dca Mon Sep 17 00:00:00 2001 From: Nick Tremaroli Date: Thu, 5 Dec 2024 09:28:14 +0100 Subject: [PATCH 10/13] Corrected units in comment Co-authored-by: Silvio Traversaro --- src/libYARP_dev/src/yarp/dev/MultipleAnalogSensorsInterfaces.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/libYARP_dev/src/yarp/dev/MultipleAnalogSensorsInterfaces.h b/src/libYARP_dev/src/yarp/dev/MultipleAnalogSensorsInterfaces.h index 21fa3bf44dc..17f4ba965ba 100644 --- a/src/libYARP_dev/src/yarp/dev/MultipleAnalogSensorsInterfaces.h +++ b/src/libYARP_dev/src/yarp/dev/MultipleAnalogSensorsInterfaces.h @@ -322,7 +322,7 @@ class YARP_dev_API yarp::dev::ILinearVelocitySensors * 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. + * @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. */ From b1ae3df86f8f9f00656554a450d76731c0a70842 Mon Sep 17 00:00:00 2001 From: Nick Tremaroli Date: Thu, 5 Dec 2024 09:29:09 +0100 Subject: [PATCH 11/13] Corrected units in comment for angular acceleration Co-authored-by: Silvio Traversaro --- src/libYARP_dev/src/yarp/dev/MultipleAnalogSensorsInterfaces.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/libYARP_dev/src/yarp/dev/MultipleAnalogSensorsInterfaces.h b/src/libYARP_dev/src/yarp/dev/MultipleAnalogSensorsInterfaces.h index 17f4ba965ba..7eb94825c7f 100644 --- a/src/libYARP_dev/src/yarp/dev/MultipleAnalogSensorsInterfaces.h +++ b/src/libYARP_dev/src/yarp/dev/MultipleAnalogSensorsInterfaces.h @@ -176,7 +176,7 @@ class YARP_dev_API yarp::dev::IThreeAxisAngularAccelerometers * 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 meters^2/seconds. + * @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. */ From cb5002222d556da69fb930b7ce0cf53fc1e4f9ad Mon Sep 17 00:00:00 2001 From: Nick Tremaroli Date: Thu, 5 Dec 2024 09:29:37 +0100 Subject: [PATCH 12/13] Corrected units in comment for angular acceleration Co-authored-by: Silvio Traversaro --- src/libYARP_dev/src/yarp/dev/MultipleAnalogSensorsInterfaces.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/libYARP_dev/src/yarp/dev/MultipleAnalogSensorsInterfaces.h b/src/libYARP_dev/src/yarp/dev/MultipleAnalogSensorsInterfaces.h index 7eb94825c7f..8a1c5a50f2c 100644 --- a/src/libYARP_dev/src/yarp/dev/MultipleAnalogSensorsInterfaces.h +++ b/src/libYARP_dev/src/yarp/dev/MultipleAnalogSensorsInterfaces.h @@ -151,7 +151,7 @@ class YARP_dev_API yarp::dev::IThreeAxisAngularAccelerometers { public: /** - * \brief Get the number of three axis linear accelerometers exposed by this device. + * \brief Get the number of three axis angular accelerometers exposed by this device. */ virtual size_t getNrOfThreeAxisAngularAccelerometers() const = 0; From d496ecd40f8a318fd20862938df93729c4fcc9c6 Mon Sep 17 00:00:00 2001 From: Nick Tremaroli Date: Thu, 5 Dec 2024 10:26:44 +0100 Subject: [PATCH 13/13] Applied clang format on new merged file for CI --- .../image_rotation/imageRotation.cpp | 119 ++++++------------ 1 file changed, 39 insertions(+), 80 deletions(-) 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;