From 7c3999e00ea0cc681001f80aa28d9d88c94018ec Mon Sep 17 00:00:00 2001 From: Daniel Agar 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) {