Most classes in ETSI have their reference position at the front of the vehicle. The RVIZ display did account for that, but assumed, that the transformation to the center is just (-length/2, 0, height/2). The mistake was that we are not applying this transform inside the vehicle's frame (child_scene_node), but we are setting this node. So we need to apply its rotation to the transformation.
The ETSI messages come in a frame like UTM_32N and have thus huge numbers inside the position fields. As RVIZ internally renders stuff using OGRE, which (by default) only uses single precision floats, these tend to flicker a lot due to rounding errors. If the UTM frame is our fixed frame, there's nothing we can do about this (except from building OGRE from source with double precision by setting OGRE_DOUBLE_PRECISION in OgreConfig.h to 1, but this would then require to build RVIZ from source, which we might want to do in future, but not in this MR). But if the fixed frame is a map frame somewhere near the ETSI messages, we can first compute all the transformations in tf, which uses double precision, and then give the resulting (small) numbers to OGRE w/o loss of precision.
Minor refactoring in DENM plugin, mainly to supress compile-warnings.
(Minor change: If sim time is used, but the time source (e.g. rosbag) sends first a CAM and then a clock message, the first CAM is now discarded by checking for current_time==0)
Screenshot showing the old plugin: (CAM in blue vs ground truth position in red)
Three changes:
(-length/2, 0, height/2). The mistake was that we are not applying this transform inside the vehicle's frame (child_scene_node), but we are setting this node. So we need to apply its rotation to the transformation.UTM_32Nand have thus huge numbers inside the position fields. As RVIZ internally renders stuff using OGRE, which (by default) only uses single precision floats, these tend to flicker a lot due to rounding errors. If the UTM frame is our fixed frame, there's nothing we can do about this (except from building OGRE from source with double precision by settingOGRE_DOUBLE_PRECISIONinOgreConfig.hto1, but this would then require to build RVIZ from source, which we might want to do in future, but not in this MR). But if the fixed frame is amapframe somewhere near the ETSI messages, we can first compute all the transformations in tf, which uses double precision, and then give the resulting (small) numbers to OGRE w/o loss of precision.(Minor change: If sim time is used, but the time source (e.g. rosbag) sends first a CAM and then a clock message, the first CAM is now discarded by checking for
current_time==0)Screenshot showing the old plugin: (CAM in blue vs ground truth position in red)
and with this fix: