From 7c3999e00ea0cc681001f80aa28d9d88c94018ec Mon Sep 17 00:00:00 2001
From: Daniel Agar <daniel@agar.ca>
Date: Sun, 10 Feb 2019 16:32:59 -0500
Subject: [PATCH] update mavlink and sitl_gazebo to latest with odometry
velocity covariance
---
Jenkinsfile | 10 ++++++----
Tools/sitl_gazebo | 2 +-
mavlink/include/mavlink/v2.0 | 2 +-
src/modules/mavlink/mavlink_messages.cpp | 10 +++++++---
src/modules/mavlink/mavlink_receiver.cpp | 16 ++++++++++------
src/modules/simulator/simulator_mavlink.cpp | 7 ++++---
6 files changed, 29 insertions(+), 18 deletions(-)
diff --git a/Jenkinsfile b/Jenkinsfile
index 64bd03d33a..058aea4ed9 100644
--- a/Jenkinsfile
+++ b/Jenkinsfile
@@ -9,7 +9,7 @@ pipeline {
stage('Catkin build on ROS workspace') {
agent {
docker {
- image 'px4io/px4-dev-ros:2019-01-27'
+ image 'px4io/px4-dev-ros:2019-02-09'
args '-e CCACHE_BASEDIR=$WORKSPACE -v ${CCACHE_DIR}:${CCACHE_DIR}:rw -e HOME=$WORKSPACE'
}
}
@@ -19,7 +19,8 @@ pipeline {
echo $0;
mkdir -p catkin_ws/src;
cd catkin_ws;
- git clone --recursive https://github.com/PX4/sitl_gazebo.git src/mavlink_sitl_gazebo;
+ git -C ${WORKSPACE}/catkin_ws/src/Firmware submodule update --init --recursive --force Tools/sitl_gazebo
+ git clone --recursive ${WORKSPACE}/catkin_ws/src/Firmware/Tools/sitl_gazebo src/mavlink_sitl_gazebo;
source /opt/ros/melodic/setup.bash;
catkin init;
catkin build -j$(nproc) -l$(nproc);
@@ -44,7 +45,7 @@ pipeline {
stage('Colcon build on ROS2 workspace') {
agent {
docker {
- image 'px4io/px4-dev-ros2-bouncy:2019-01-27'
+ image 'px4io/px4-dev-ros2-bouncy:2019-02-09'
args '-e CCACHE_BASEDIR=$WORKSPACE -v ${CCACHE_DIR}:${CCACHE_DIR}:rw -e HOME=$WORKSPACE'
}
}
@@ -55,7 +56,8 @@ pipeline {
unset ROS_DISTRO;
mkdir -p colcon_ws/src;
cd colcon_ws;
- git clone --recursive https://github.com/PX4/sitl_gazebo.git src/mavlink_sitl_gazebo;
+ git -C ${WORKSPACE}/colcon_ws/src/Firmware submodule update --init --recursive --force Tools/sitl_gazebo
+ git clone --recursive ${WORKSPACE}/colcon_ws/src/Firmware/Tools/sitl_gazebo src/mavlink_sitl_gazebo;
source /opt/ros/bouncy/setup.sh;
source /opt/ros/melodic/setup.sh;
colcon build --event-handlers console_direct+ --symlink-install;
diff --git a/Tools/sitl_gazebo b/Tools/sitl_gazebo
index 34d06e042c..f8b4f545f1 160000
--- a/Tools/sitl_gazebo
+++ b/Tools/sitl_gazebo
@@ -1 +1 @@
-Subproject commit 34d06e042c9dbd336899dbba373e32a6f19828b7
+Subproject commit f8b4f545f1b2d90000e37cc53f744849d939fb52
diff --git a/mavlink/include/mavlink/v2.0 b/mavlink/include/mavlink/v2.0
index cf858b4513..bf68eed6f6 160000
--- a/mavlink/include/mavlink/v2.0
+++ b/mavlink/include/mavlink/v2.0
@@ -1 +1 @@
-Subproject commit cf858b4513d4eb1689b7c45a30531ea2e65b589c
+Subproject commit bf68eed6f6819ca1eca44217955794af554d0369
diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp
index 366e9b84f2..0e9fcba42a 100644
--- a/src/modules/mavlink/mavlink_messages.cpp
+++ b/src/modules/mavlink/mavlink_messages.cpp
@@ -2361,11 +2361,15 @@ protected:
msg.yawspeed = odom.yawspeed;
// get the covariance matrix size
+
+ // pose_covariance
static constexpr size_t POS_URT_SIZE = sizeof(odom.pose_covariance) / sizeof(odom.pose_covariance[0]);
- static constexpr size_t VEL_URT_SIZE = sizeof(odom.velocity_covariance) / sizeof(odom.velocity_covariance[0]);
static_assert(POS_URT_SIZE == (sizeof(msg.pose_covariance) / sizeof(msg.pose_covariance[0])),
"Odometry Pose Covariance matrix URT array size mismatch");
- static_assert(VEL_URT_SIZE == (sizeof(msg.twist_covariance) / sizeof(msg.twist_covariance[0])),
+
+ // velocity_covariance
+ static constexpr size_t VEL_URT_SIZE = sizeof(odom.velocity_covariance) / sizeof(odom.velocity_covariance[0]);
+ static_assert(VEL_URT_SIZE == (sizeof(msg.velocity_covariance) / sizeof(msg.velocity_covariance[0])),
"Odometry Velocity Covariance matrix URT array size mismatch");
// copy pose covariances
@@ -2376,7 +2380,7 @@ protected:
// copy velocity covariances
//TODO: Apply rotation matrix to transform from body-fixed NED to earth-fixed NED frame
for (size_t i = 0; i < VEL_URT_SIZE; i++) {
- msg.twist_covariance[i] = odom.velocity_covariance[i];
+ msg.velocity_covariance[i] = odom.velocity_covariance[i];
}
mavlink_msg_odometry_send_struct(_mavlink->get_channel(), &msg);
diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp
index 9c365da8f4..1d1acf49cb 100644
--- a/src/modules/mavlink/mavlink_receiver.cpp
+++ b/src/modules/mavlink/mavlink_receiver.cpp
@@ -1254,11 +1254,14 @@ MavlinkReceiver::handle_message_odometry(mavlink_message_t *msg)
// - add usage on the estimator side
odometry.local_frame = odometry.LOCAL_FRAME_NED;
- const size_t POS_URT_SIZE = sizeof(odometry.pose_covariance) / sizeof(odometry.pose_covariance[0]);
- const size_t VEL_URT_SIZE = sizeof(odometry.velocity_covariance) / sizeof(odometry.velocity_covariance[0]);
+ // pose_covariance
+ static constexpr size_t POS_URT_SIZE = sizeof(odometry.pose_covariance) / sizeof(odometry.pose_covariance[0]);
static_assert(POS_URT_SIZE == (sizeof(odom.pose_covariance) / sizeof(odom.pose_covariance[0])),
"Odometry Pose Covariance matrix URT array size mismatch");
- static_assert(VEL_URT_SIZE == (sizeof(odom.twist_covariance) / sizeof(odom.twist_covariance[0])),
+
+ // velocity_covariance
+ static constexpr size_t VEL_URT_SIZE = sizeof(odometry.velocity_covariance) / sizeof(odometry.velocity_covariance[0]);
+ static_assert(VEL_URT_SIZE == (sizeof(odom.velocity_covariance) / sizeof(odom.velocity_covariance[0])),
"Odometry Velocity Covariance matrix URT array size mismatch");
// create a method to simplify covariance copy
@@ -1285,7 +1288,7 @@ MavlinkReceiver::handle_message_odometry(mavlink_message_t *msg)
//TODO: Apply rotation matrix to transform from body-fixed NED to earth-fixed NED frame
for (size_t i = 0; i < VEL_URT_SIZE; i++) {
- odometry.velocity_covariance[i] = odom.twist_covariance[i];
+ odometry.velocity_covariance[i] = odom.velocity_covariance[i];
}
} else if (odom.child_frame_id == MAV_FRAME_BODY_NED) { /* WRT to vehicle body-NED frame */
@@ -1307,13 +1310,14 @@ MavlinkReceiver::handle_message_odometry(mavlink_message_t *msg)
//TODO: Apply rotation matrix to transform from body-fixed to earth-fixed NED frame
for (size_t i = 0; i < VEL_URT_SIZE; i++) {
- odometry.velocity_covariance[i] = odom.twist_covariance[i];
+ odometry.velocity_covariance[i] = odom.velocity_covariance[i];
}
}
} else if (odom.child_frame_id == MAV_FRAME_VISION_NED || /* WRT to vehicle local NED frame */
odom.child_frame_id == MAV_FRAME_MOCAP_NED) {
+
if (updated) {
orb_copy(ORB_ID(vehicle_attitude), _vehicle_attitude_sub, &_att);
@@ -1332,7 +1336,7 @@ MavlinkReceiver::handle_message_odometry(mavlink_message_t *msg)
//TODO: Apply rotation matrix to transform from earth-fixed to body-fixed NED frame
for (size_t i = 0; i < VEL_URT_SIZE; i++) {
- odometry.velocity_covariance[i] = odom.twist_covariance[i];
+ odometry.velocity_covariance[i] = odom.velocity_covariance[i];
}
}
diff --git a/src/modules/simulator/simulator_mavlink.cpp b/src/modules/simulator/simulator_mavlink.cpp
index b8a85defb7..1c1b4bb92c 100644
--- a/src/modules/simulator/simulator_mavlink.cpp
+++ b/src/modules/simulator/simulator_mavlink.cpp
@@ -1126,13 +1126,14 @@ int Simulator::publish_odometry_topic(mavlink_message_t *odom_mavlink)
odom.pitchspeed = odom_msg.pitchspeed;
odom.yawspeed = odom_msg.yawspeed;
- const size_t VEL_URT_SIZE = sizeof(odom.velocity_covariance) / sizeof(odom.velocity_covariance[0]);
- static_assert(VEL_URT_SIZE == (sizeof(odom_msg.twist_covariance) / sizeof(odom_msg.twist_covariance[0])),
+ // velocity_covariance
+ static constexpr size_t VEL_URT_SIZE = sizeof(odom.velocity_covariance) / sizeof(odom.velocity_covariance[0]);
+ static_assert(VEL_URT_SIZE == (sizeof(odom_msg.velocity_covariance) / sizeof(odom_msg.velocity_covariance[0])),
"Odometry Velocity Covariance matrix URT array size mismatch");
/* The velocity covariance URT */
for (size_t i = 0; i < VEL_URT_SIZE; i++) {
- odom.velocity_covariance[i] = odom_msg.twist_covariance[i];
+ odom.velocity_covariance[i] = odom_msg.velocity_covariance[i];
}
} else if (odom_mavlink->msgid == MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE) {
--
GitLab