combine.cpp

Go to the documentation of this file.

#include "combine.h"
 
#include <RobotAPI/libraries/armem_robot/types.h>
#include <RobotAPI/libraries/armem/core/Time.h>
#include <RobotAPI/libraries/armem_robot_state/server/proprioception/SensorValues.h>
 
#include <ArmarXCore/core/logging/Logging.h>
 
#include <sstream>
 
 
namespace armarx::armem::server
{
 
    robot::Robots
    robot_state::combine(
        const description::RobotDescriptionMap& robotDescriptions,
        const localization::RobotPoseMap& globalPoses,
        const proprioception::SensorValuesMap& sensorValues,
        const armem::Time& timestamp)
    {
        std::stringstream logs;
 
        robot::Robots robots;
        for (const auto& [robotName, robotDescription] : robotDescriptions)
        {
            // Handle missing values gracefully instead of skipping the robot altogether.
 
            robot::Robot& robot = robots.emplace_back();
 
            robot.description = robotDescription;
            robot.instance    = ""; // TODO(fabian.reister): set this properly
            robot.config.timestamp  = timestamp;
            robot.config.globalPose = Eigen::Affine3f::Identity();
            robot.config.jointMap   = {};
            robot.timestamp   = timestamp;
 
            if (auto it = globalPoses.find(robotName); it != globalPoses.end())
            {
                robot.config.globalPose = it->second;
            }
            else
            {
                logs << "\nNo global pose for robot '" << robotName << "'.";
            }
            if (auto it = sensorValues.find(robotName); it != sensorValues.end())
            {
                for (const auto& [name, values] : it->second.jointValueMap)
                {
                    robot.config.jointMap.emplace(name, values.position);
                }
            }
            else
            {
                logs << "\nNo joint positions for robot '" << robotName << "'.";
            }
        }
 
        if (not logs.str().empty())
        {
            // These are handled, so they are no warnings.
            ARMARX_VERBOSE << deactivateSpam(60) << logs.str();
        }
 
        return robots;
    }
 
}