diff --git a/source/RobotAPI/libraries/armem_robot_state/server/description/Segment.cpp b/source/RobotAPI/libraries/armem_robot_state/server/description/Segment.cpp index 35a7ca578c0bac7ecbaa70fe5cf7f9ee8a8aff02..6f247e9bef4328d2d40dc289d0953cb2a3ddcd55 100644 --- a/source/RobotAPI/libraries/armem_robot_state/server/description/Segment.cpp +++ b/source/RobotAPI/libraries/armem_robot_state/server/description/Segment.cpp @@ -26,6 +26,7 @@ #include <RobotAPI/libraries/armem_robot/aron_conversions.h> #include <RobotAPI/libraries/armem_robot/robot_conversions.h> #include <RobotAPI/libraries/armem_robot/aron/RobotDescription.aron.generated.h> +#include <RobotAPI/libraries/armem_robot_state/aron/Proprioception.aron.generated.h> namespace armarx::armem::server::robot_state::description diff --git a/source/RobotAPI/libraries/armem_robot_state/server/proprioception/Segment.cpp b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/Segment.cpp index 821f1019123e7eb814e8313e2a7d94a9091c73dd..5e01e84ebc5724f672c7f32d01497624d0759b3e 100644 --- a/source/RobotAPI/libraries/armem_robot_state/server/proprioception/Segment.cpp +++ b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/Segment.cpp @@ -21,7 +21,7 @@ #include <RobotAPI/libraries/armem_robot/robot_conversions.h> #include <RobotAPI/libraries/armem_robot/aron/Robot.aron.generated.h> -#include <RobotAPI/libraries/armem_robot_state/aron/JointState.aron.generated.h> +#include <RobotAPI/libraries/armem_robot_state/aron/Proprioception.aron.generated.h> #include <RobotAPI/libraries/armem_objects/aron_conversions.h> @@ -31,7 +31,7 @@ namespace armarx::armem::server::robot_state::proprioception { Segment::Segment(armem::server::MemoryToIceAdapter& memoryToIceAdapter) : - SpecializedSegment(memoryToIceAdapter, nullptr, + SpecializedSegment(memoryToIceAdapter, prop::arondto::Proprioception::toAronType(), "Proprioception", 1024) { } @@ -106,23 +106,20 @@ namespace armarx::armem::server::robot_state::proprioception TIMING_END_COMMENT_STREAM(tGetLatestInstance, "tGetLatestInstance " + std::to_string(i), ARMARX_DEBUG); TIMING_START(tCastJointState); - // tryCast is too slow. - // const auto jointState = tryCast<armarx::armem::arondto::JointState>(entityInstance); aron::datanavigator::DictNavigatorPtr data = entityInstance.data(); // Avoid throwing an exception. if (!(data->hasElement("name") && data->hasElement("position"))) { continue; } - armarx::armem::arondto::JointState jointState; - jointState.name = adn::StringNavigator::DynamicCastAndCheck(data->getElement("name"))->getValue(); - jointState.position = adn::FloatNavigator::DynamicCastAndCheck(data->getElement("position"))->getValue(); + const std::string jointName = adn::StringNavigator::DynamicCastAndCheck(data->getElement("name"))->getValue(); + const float jointPosition = adn::FloatNavigator::DynamicCastAndCheck(data->getElement("position"))->getValue(); TIMING_END_COMMENT_STREAM(tCastJointState, "tCastJointState " + std::to_string(i), ARMARX_DEBUG); TIMING_START(tEmplace); - if (jointState.name.size() > 0) + if (not jointName.empty()) { - jointMap[robotName].emplace(jointState.name, jointState.position); + jointMap[robotName].emplace(jointName, jointPosition); } else {