diff --git a/source/RobotAPI/libraries/armem/core/Time.h b/source/RobotAPI/libraries/armem/core/Time.h index cc00c9febfc9927d7a4985a63b1dabf2d8621300..328d1f59d793cbcc9a7d0febc0951775d200013a 100644 --- a/source/RobotAPI/libraries/armem/core/Time.h +++ b/source/RobotAPI/libraries/armem/core/Time.h @@ -34,4 +34,4 @@ namespace armarx::armem */ Time timeFromStringMicroSeconds(const std::string& microSeconds); -} +} // namespace armarx::armem diff --git a/source/RobotAPI/libraries/armem_robot_mapping/MappingDataReader.cpp b/source/RobotAPI/libraries/armem_robot_mapping/MappingDataReader.cpp index cc9d4c8f00a7a53b7cffcc26524ef54fb7af4f6c..aae0106dab35ac2252e90f78ee5dd79a37c9188c 100644 --- a/source/RobotAPI/libraries/armem_robot_mapping/MappingDataReader.cpp +++ b/source/RobotAPI/libraries/armem_robot_mapping/MappingDataReader.cpp @@ -97,9 +97,7 @@ namespace armarx::armem .coreSegments().withName(properties.mappingMemoryName) .providerSegments().withName(query.agent) .entities().all() - .snapshots().timeRange( - IceUtil::Time::microSeconds(query.timeRange.min), - IceUtil::Time::microSeconds(query.timeRange.max)); + .snapshots().timeRange(query.timeRange.min, query.timeRange.max); // clang-format on } else @@ -109,9 +107,7 @@ namespace armarx::armem .coreSegments().withName(properties.mappingMemoryName) .providerSegments().withName(query.agent) .entities().withNames(query.sensorList) - .snapshots().timeRange( - IceUtil::Time::microSeconds(query.timeRange.min), - IceUtil::Time::microSeconds(query.timeRange.max)); + .snapshots().timeRange(query.timeRange.min,query.timeRange.max); // clang-format on } diff --git a/source/RobotAPI/libraries/armem_robot_mapping/MappingDataReader.h b/source/RobotAPI/libraries/armem_robot_mapping/MappingDataReader.h index da3bcc6eefbe427f6350aae26ffa9d0c461ae149..359b88fe8550d9b64f066fe942a5c6fda7dce172 100644 --- a/source/RobotAPI/libraries/armem_robot_mapping/MappingDataReader.h +++ b/source/RobotAPI/libraries/armem_robot_mapping/MappingDataReader.h @@ -45,8 +45,8 @@ namespace armarx::armem struct TimeRange { - std::int64_t min; - std::int64_t max; + Time min; + Time max; }; /** diff --git a/source/RobotAPI/libraries/armem_robot_mapping/MappingDataWriter.cpp b/source/RobotAPI/libraries/armem_robot_mapping/MappingDataWriter.cpp index a91fa36c263e8f93fc0603b99805c70ee174f595..dd0299ece229eaac2fb4f44758fae23f763f612d 100644 --- a/source/RobotAPI/libraries/armem_robot_mapping/MappingDataWriter.cpp +++ b/source/RobotAPI/libraries/armem_robot_mapping/MappingDataWriter.cpp @@ -63,7 +63,7 @@ namespace armarx::armem return false; } - const auto iceTimestamp = IceUtil::Time::microSeconds(timestamp); + const auto iceTimestamp = Time::microSeconds(timestamp); const auto providerId = armem::MemoryID(result.segmentID); const auto entityID = @@ -75,7 +75,7 @@ namespace armarx::armem arondto::LaserScanStamped aronSensorData; // currently only sets the header - toAron(laserScan, timestamp, frame, agentName, aronSensorData); + toAron(laserScan, iceTimestamp, frame, agentName, aronSensorData); auto dict = aronSensorData.toAron(); dict->addElement("scan", toAron(laserScan)); diff --git a/source/RobotAPI/libraries/armem_robot_mapping/aron_conversions.cpp b/source/RobotAPI/libraries/armem_robot_mapping/aron_conversions.cpp index 83925942422d78c25e3d733011b870a7a72005bf..40471a632c66708ecb8085ea63a65acf6013d5a2 100644 --- a/source/RobotAPI/libraries/armem_robot_mapping/aron_conversions.cpp +++ b/source/RobotAPI/libraries/armem_robot_mapping/aron_conversions.cpp @@ -1,6 +1,7 @@ #include "aron_conversions.h" #include <algorithm> +#include <cstdint> #include <iterator> #include <RobotAPI/interface/units/LaserScannerUnit.h> @@ -16,11 +17,17 @@ namespace armarx /************ fromAron ************/ + armem::Time timeFromAron(const int64_t timestamp) + { + return armem::Time::microSeconds(timestamp); + } + SensorHeader fromAron(const arondto::SensorHeader& aronSensorHeader) { + return {.agent = aronSensorHeader.agent, .frame = aronSensorHeader.frame, - .timestamp = aronSensorHeader.timestamp}; + .timestamp = timeFromAron(aronSensorHeader.timestamp)}; } void fromAron(const arondto::LaserScanStamped& aronLaserScan, @@ -38,7 +45,7 @@ namespace armarx // laserScan = fromAron(aronLaserScan.data); - timestamp = header.timestamp; + timestamp = header.timestamp.toMicroSeconds(); frame = header.frame; agentName = header.agent; } @@ -52,13 +59,18 @@ namespace armarx // aronLaserScan.scan = toAron(laserScan); // } + int64_t toAron(const armem::Time& timestamp) + { + return timestamp.toMicroSeconds(); + } + arondto::SensorHeader toAron(const SensorHeader& sensorHeader) { arondto::SensorHeader aronSensorHeader; aronSensorHeader.agent = sensorHeader.agent; aronSensorHeader.frame = sensorHeader.frame; - aronSensorHeader.timestamp = sensorHeader.timestamp; + aronSensorHeader.timestamp = toAron(sensorHeader.timestamp); return aronSensorHeader; } @@ -71,7 +83,7 @@ namespace armarx } void toAron(const LaserScan& laserScan, - const std::int64_t& timestamp, + const armem::Time& timestamp, const std::string& frame, const std::string& agentName, arondto::LaserScanStamped& aronLaserScanStamped) diff --git a/source/RobotAPI/libraries/armem_robot_mapping/aron_conversions.h b/source/RobotAPI/libraries/armem_robot_mapping/aron_conversions.h index c7840f48f30a4ee6da6984f6b216dd41a686e9be..7e0d690a48da10f3d07290f6bf32281251a877e5 100644 --- a/source/RobotAPI/libraries/armem_robot_mapping/aron_conversions.h +++ b/source/RobotAPI/libraries/armem_robot_mapping/aron_conversions.h @@ -25,6 +25,7 @@ #include <RobotAPI/interface/units/LaserScannerUnit.h> #include <RobotAPI/libraries/aron/converter/common/VectorConverter.h> #include <RobotAPI/libraries/aron/core/navigator/data/complex/NDArray.h> +#include <RobotAPI/libraries/armem/core/Time.h> namespace armarx { @@ -55,7 +56,7 @@ namespace armarx void toAron( const LaserScan& laserScan, - const std::int64_t& timestamp, + const armem::Time& timestamp, const std::string& frame, const std::string& agentName, arondto::LaserScanStamped& aronLaserScan); diff --git a/source/RobotAPI/libraries/armem_robot_mapping/types.h b/source/RobotAPI/libraries/armem_robot_mapping/types.h index 6e8a08414215d6155da0bc691cce228870c53102..d822597e103177b0833014d25f0530e25f2b2075 100644 --- a/source/RobotAPI/libraries/armem_robot_mapping/types.h +++ b/source/RobotAPI/libraries/armem_robot_mapping/types.h @@ -21,6 +21,7 @@ #pragma once +#include <RobotAPI/libraries/armem/core/Time.h> #include <RobotAPI/interface/units/LaserScannerUnit.h> namespace armarx @@ -30,7 +31,7 @@ namespace armarx { std::string agent; std::string frame; - std::int64_t timestamp; + armem::Time timestamp; }; struct LaserScanStamped