combine.cpp

Go to the documentation of this file.

#include "combine.h"
 
#include <sstream>
 
#include <ArmarXCore/core/logging/Logging.h>
 
#include <RobotAPI/libraries/armem/core/Time.h>
#include <RobotAPI/libraries/armem_robot_state/server/proprioception/SensorValues.h>
#include <RobotAPI/libraries/armem_robot_state/types.h>
 
namespace armarx::armem::server::robot_state
{
 
    armem::robot_state::Robots
    combine(const description::RobotDescriptionMap& robotDescriptions,
            const localization::RobotPoseMap& globalPoses,
            const proprioception::SensorValuesMap& sensorValues,
            const armem::Time& timestamp)
    {
        std::stringstream logs;
 
        armem::robot_state::Robots robots;
        for (const auto& [robotName, robotDescription] : robotDescriptions)
        {
            // Handle missing values gracefully instead of skipping the robot altogether.
 
            armem::robot_state::Robot& robot = robots.emplace_back();
 
            robot.description = robotDescription;
            robot.instance = ""; // TODO(fabian.reister): set this properly
            robot.config.timestamp = timestamp;
            robot.config.globalPose = Eigen::Isometry3f::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;
    }
 
} // namespace armarx::armem::server::robot_state