aau-cns / mars_ros Goto Github PK

Here is a geometry_msgs/PoseStamped message I get running an instance of a gps_node:

header: 
  seq: 12188
  stamp: 
    secs: 1661984306
    nsecs: 715818644
  frame_id: ''
pose: 
  position: 
    x: -0.6634237632091935
    y: 3.4947216960559073
    z: 0.06363313476750493
  orientation: 
    x: -0.005933376892896241
    y: 0.18921987844821742
    z: 0.02400976216308664
    w: 0.9816232291259008

(Notice the empty frame_id)

Without frame_id we can't visualize poses, for example.

Thanks for sharing your work,

Summary

Hey @Chris-Bee, as discussed when initializing, the ros wrappers do not wait for the initial states to be set through the first sensor measurement, although they already include a code to set the inital states p_wi_init_ and q_wi_init_:

Detailed Information

The following core init call does not wait for the do_state_init_ to be set to true

-  if (!core_logic_.core_is_initialized_ )
+  if (!core_logic_.core_is_initialized_ && do_state_init_)

This can yield the issue of the first couple of published states being wrong due to the filter converging after initializing its core state to identity (0 in position, and unit quaternion in orientation).

This issue can also apply to other wrappers and is especially present if the pose sensor provides updates significantly different from the identity/origin.

Hi, thanks for the great and helpful work! I converted INSANE dataset into rosbag, and tried to fuse 6 DoF VIO with IMU data using mars_ros. However, the orientation of the fused pose was diverged (it kept rotating very fast). The result is the same for other datasets I've tested. But if I set "vision1_att_meas_noise" to very large values like [10.0, 10.0, 10.0], the fused pose's orientation seems getting stable. I don't think this is right. I was wondering whether you could provide some minimal examples of mars_ros for VIO/IMU/GNSS fusion using public datasets like Euroc or INSANE. Then it would be really helpful for us to try mars_ros on more scenarios.

Looking forward to your reply. Thanks.

Hi! As the title says shoudn't this:

be like this: