[estimation][control] SO3 attitude control

[sugikazu75]

What is this

#548 is recreated from current master branch.

[tongtybj]

@sugikazu75

I have came up with the solution of replacing the gavity direciton with quaternion for the imu topic from spinal to ROS. I will implement it in few days.

[sugikazu75]

@sugikazu75

I have came up with the solution of replacing the gavity direciton with quaternion for the imu topic from spinal to ROS. I will implement it in few days.

Ok, I think it's better way.
Will quaternion be estimated by imu and magnetometer, right?

[sugikazu75]

memo: I had to calibrate magnetometer before merging 7b8a8cc

[sugikazu75]

memo: I had to calibrate magnetometer before merging 7b8a8cc

I solved this by 32db881

[tongtybj]

@sugikazu75

I forgot to tell you that I am refactoring this branch.
So please pause the further development on this branch, and create another temporal branch for any other patch.
Could you revert to 47e6c27 for this branch?

[sugikazu75]

@tongtybj
I need to add a patch sugikazu75@4fa365a to enable to deploy lidar with x-axis pointing down. But how is the progress of your development? If there is no effect to this pathc, I want you to send current state to this brnach.

After that, Im planning to rebase to curent master branhc .

[tongtybj]

@sugikazu75

Sorry for my late progress.
#598 (comment) have been done by create a new PR #671.
So I want to close this PR. Please add any necessary commits to #671, but not here.

Besides,

enable to deploy lidar with x-axis pointing down. But how is the progress of your development? If there is no effect to this pathc, I want you to send current state to this brnach.

I also found some bug in fast_lio when the sensor is not level. But I think it should be solved inside fast_lio, but not in our system. I think the crucial reason of this bug is that fast_lio does not estimate the orientation of sensor w.r.t. the inertial frame. And this bug should be easily solved inside fast_lio. I will tackle this problem in the near future.