yemai/bizhang_-obav
1
0
Fork 0
mirror of https://gitee.com/xiaohuolufeihua/bizhang_-obav.git synced 2026-05-22 09:28:58 +00:00
Code Issues Packages Projects Releases Wiki Activity

assert over the covariance matrices URT size matching

Browse Source
This commit is contained in:
TSC21
2018-07-26 16:12:40 +01:00
committed by Lorenz Meier
parent eeca8d4efe
commit 7db57bedb7
3 changed files with 18 additions and 9 deletions
Download Patch File Download Diff File Expand all files Collapse all files

6
src/modules/simulator/simulator_mavlink.cpp
View File

@@ -1156,7 +1156,7 @@ int Simulator::publish_odometry_topic(mavlink_message_t *odom_mavlink)
q.copyTo(odom.q);
/* The pose covariance URT */
for (size_t i = 0; i < 21; i++) {
for (size_t i = 0; i < (sizeof(odom.pose_covariance) / sizeof(odom.pose_covariance[0])); i++) {
odom.pose_covariance[i] = odom_msg.pose_covariance[i];
}
@@ -1170,7 +1170,7 @@ int Simulator::publish_odometry_topic(mavlink_message_t *odom_mavlink)
odom.yawspeed = odom_msg.yawspeed;
/* The velocity covariance URT */
for (size_t i = 0; i < 21; i++) {
for (size_t i = 0; i < (sizeof(odom.velocity_covariance) / sizeof(odom.velocity_covariance[0])); i++) {
odom.velocity_covariance[i] = odom_msg.twist_covariance[i];
}
@@ -1187,7 +1187,7 @@ int Simulator::publish_odometry_topic(mavlink_message_t *odom_mavlink)
q.copyTo(odom.q);
/* The pose covariance URT */
for (size_t i = 0; i < 21; i++) {
for (size_t i = 0; i < (sizeof(odom.pose_covariance) / sizeof(odom.pose_covariance[0])); i++) {
odom.pose_covariance[i] = ev.covariance[i];
}