Type conversion to finroc data types
Current behavior
I'm currently trying to convert a datatype (sensor_msgs::JointState
) to a finroc datatype (tVec3d
). It is received from the ros plugins which is doing the conversion and directly writes the type into a finroc port.
I wrote a simple converter which takes the values from the JointState type and puts the into the vector in plugins/ros/converter/converters.cpp where also other datatypes are converted. (its only done for the rostofinroc direction)
const tTypeConverterBase& cJOINTSTATE_CONVERTER = tTypeConverterLambdaFunction<rrlib::math::tVec3d, sensor_msgs::JointState>
{
[](const rrlib::math::tVec3d & finroc, sensor_msgs::JointState & ros)
{
// ros.x = finroc.X();
// ros.y = finroc.Y();
// ros.z = finroc.Z();
},
[](const sensor_msgs::JointState & ros, rrlib::math::tVec3d & finroc)
{
finroc = rrlib::math::tVec3d(ros.position, ros.velocity, ros.effort);
}
};
This converter should be used when connecting to the ros of type JointState. out_state is a finroc port of type tVec3d into which the data should be written.
this->RegisterROSSubscriber<rrlib::math::tVec3d, sensor_msgs::JointState>(spar_source_ros_node.Get(), 1000, this->out_state);
If this code is executed I get an error message that there is no converter between the data types:
Main Thread/Hardware Interface/Joint State Controller::OnRosReady [sources/cpp/projects/humantech/hardware_abstraction/hardware/mBaubotJointStateController.cpp:110] >> OnRosReady
Main Thread/Hardware Interface/Joint State Controller::RegisterROSSubscriber [sources/cpp/plugins/ros/tROSModule.hpp:97] >> ERROR: Lookup of registered conversion operation ROSToFinroc with specified types failed
Main Thread/Hardware Interface/Joint State Controller::RegisterROSSubscriber [sources/cpp/plugins/ros/tROSModule.hpp:98] >> ERROR: No type-converter registered for converting between ROS-type 'sensor_msgs/JointState' and Finroc-type 'Vector3d'!
Main Thread/Hardware Interface/Joint State Controller::RegisterROSSubscriber [sources/cpp/plugins/ros/tROSModule.hpp:102] >> ROS RTTI Type: N11sensor_msgs11JointState_ISaIvEEE, Finroc RTTI Type: N5rrlib4math7tVectorILm3EdNS0_6vector9CartesianEJEEE
Aborted (core dumped)
I don't understand why the conversion is not found/used?