search

mavlink / mavros

MAVLink to ROS gateway with proxy for Ground Control Station
Other
879 stars 989 forks source link

local_position twist and pose as a ***WithCovarianceStamped msg #516

Closed quentin1002 closed 8 years ago

quentin1002 commented 8 years ago

Hi!

I want to interface mavros with some SLAM algorithms. To perfom an efficient sensor fusion, I need msgs of the type ***WithCovarianceStamped. Unfortunatly, the local_position messages don't include the covariance. Is there a simple way to get the messages with the covariance? I am still new to ROS programming...

For you info, I am using both px4 pixhawk and px4flow. I guess that we could use the covariance derived from the IMU to add it the local_position msgs... But I really don't know how to do that.

Thanks for you help

scott-eddy commented 8 years ago

While mavlink supports a LOCAL_POSITION_NED_COV message that passes the estimate and the covariance, and the PX4 can stream that message, there is no mavros plugin to parse it yet. You would have to alter local_position plugin to get that data.

If you do you should submit a PR with the changes

quentin1002 commented 8 years ago

In the mean time, I developed a solution that looks better to me. Inspired by the hector_quadrotor, I broke down the process like that:

_The TF tree: _ base_link -> base_stabilized: I use the IMU information returned by mavros in the hector_imu_attitude_to_tf node

base_stabilized -> base_footprint: I simply subscribe to the altitude topic of mavros and publish the z translation. I still have some reliability problems as the barometer is not really accurate. I might try to use the sonar height combined with the IMU information. To be continued

base_footprint -> odom: I use robot_localization. It is working in 2d mode. It combines the IMU and the optical flow. To use the mavros_msgs/OpticalFlowRad in robot_localization, I use make use of optflow_odometry. At the end of the day, robot_localization is also publishing my 2D odom.

odom -> map: I use another EKF, again robot_localization, to fuse my 2D odom with my SLAM position.

For the moment, everything looks good, except the base_stabilized -> base_footprint. This problem does not affect the system. Do you think my solution is a good one, compared to the use of a single mavros usage? Am I creating to much computing and processing requirements or maybe inducing errors?