Skip to content
Snippets Groups Projects
Commit 45ee816c authored by Patrick Hegemann's avatar Patrick Hegemann
Browse files

Use correct timestamp in getGlobalRobotPose

parent 455b662a
No related branches found
No related tags found
1 merge request!218Add method to get global robot pose to RobotStateMemory
......@@ -21,6 +21,7 @@
*/
#include "RobotStateMemory.h"
#include "RobotAPI/libraries/armem/core/forward_declarations.h"
#include <RobotAPI/interface/core/PoseBase.h>
#include <RobotAPI/libraries/core/Pose.h>
......@@ -33,7 +34,10 @@
#include <ArmarXCore/libraries/ArmarXCoreComponentPlugins/DebugObserverComponentPlugin.h>
#include <ArmarXCore/core/logging/Logging.h>
#include <IceUtil/Time.h>
#include <SimoxUtility/algorithm/get_map_keys_values.h>
#include <SimoxUtility/algorithm/contains.h>
#include <SimoxUtility/algorithm/string.h>
......@@ -187,9 +191,11 @@ namespace armarx::armem::server::robot_state
armarx::PoseBasePtr RobotStateMemory::getGlobalRobotPose(Ice::Long timestamp, const std::string& robotName, const ::Ice::Current& /*unused*/)
{
auto poseMap = localizationSegment.getRobotGlobalPoses(armem::Time::now());
auto poseMap = localizationSegment.getRobotGlobalPoses(armem::Time::microSeconds(timestamp));
bool robotNameFound = !(poseMap.find(robotName) == poseMap.end());
ARMARX_DEBUG << "Getting robot pose of robot " << robotName << " at timestamp " << armem::Time::microSeconds(timestamp);
bool robotNameFound = simox::alg::contains_key(poseMap, robotName);
ARMARX_CHECK(robotNameFound)
<< "Robot with name " << robotName << " does not exist. "
<< "Available robots are: " << simox::alg::get_keys(poseMap);
......
......@@ -77,7 +77,7 @@ namespace armarx::armem::server::robot_state
// GlobalRobotPoseProvider interface
virtual armarx::PoseBasePtr getGlobalRobotPose(Ice::Long timestamp, const std::string& robotName, const ::Ice::Current&) override;
armarx::PoseBasePtr getGlobalRobotPose(Ice::Long timestamp, const std::string& robotName, const ::Ice::Current&) override;
protected:
......
......@@ -30,6 +30,7 @@
module armarx{
interface GlobalRobotPoseProvider extends armem::server::MemoryInterface {
// timestamp in microseconds
PoseBase getGlobalRobotPose(long timestamp, string robotName);
}
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment