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

Improve warning messages in getGlobalRobotPose

parent 45ee816c
No related branches found
No related tags found
Loading
......@@ -189,15 +189,16 @@ namespace armarx::armem::server::robot_state
}
armarx::PoseBasePtr RobotStateMemory::getGlobalRobotPose(Ice::Long timestamp, const std::string& robotName, const ::Ice::Current& /*unused*/)
armarx::PoseBasePtr RobotStateMemory::getGlobalRobotPose(Ice::Long timestamp_us, const std::string& robotName, const ::Ice::Current& /*unused*/)
{
auto poseMap = localizationSegment.getRobotGlobalPoses(armem::Time::microSeconds(timestamp));
ARMARX_DEBUG << "Getting robot pose of robot " << robotName << " at timestamp " << armem::Time::microSeconds(timestamp);
auto timestamp = armem::Time::microSeconds(timestamp_us);
ARMARX_DEBUG << "Getting robot pose of robot " << robotName << " at timestamp " << timestamp << ".";
auto poseMap = localizationSegment.getRobotGlobalPoses(timestamp);
bool robotNameFound = simox::alg::contains_key(poseMap, robotName);
ARMARX_CHECK(robotNameFound)
<< "Robot with name " << robotName << " does not exist. "
<< "Robot with name " << robotName << " does not exist at or before timestamp " << timestamp << ".\n"
<< "Available robots are: " << simox::alg::get_keys(poseMap);
return new Pose(poseMap[robotName].matrix());
......
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