diff --git a/src/modules/simulation/gz_bridge/GZBridge.cpp b/src/modules/simulation/gz_bridge/GZBridge.cpp index 4d7632e612..db7517fc47 100644 --- a/src/modules/simulation/gz_bridge/GZBridge.cpp +++ b/src/modules/simulation/gz_bridge/GZBridge.cpp @@ -486,17 +486,6 @@ void GZBridge::poseInfoCallback(const gz::msgs::Pose_V &pose) gz::msgs::Vector3d pose_position = pose.pose(p).position(); gz::msgs::Quaternion pose_orientation = pose.pose(p).orientation(); - static const auto q_FLU_to_FRD = gz::math::Quaterniond(0, 1, 0, 0); - - /** - * @brief Quaternion for rotation between ENU and NED frames - * - * NED to ENU: +PI/2 rotation about Z (Down) followed by a +PI rotation around X (old North/new East) - * ENU to NED: +PI/2 rotation about Z (Up) followed by a +PI rotation about X (old East/new North) - * This rotation is symmetric, so q_ENU_to_NED == q_NED_to_ENU. - */ - static const auto q_ENU_to_NED = gz::math::Quaterniond(0, 0.70711, 0.70711, 0); - // ground truth gz::math::Quaterniond q_gr = gz::math::Quaterniond( pose_orientation.w(), @@ -504,8 +493,8 @@ void GZBridge::poseInfoCallback(const gz::msgs::Pose_V &pose) pose_orientation.y(), pose_orientation.z()); - gz::math::Quaterniond q_gb = q_gr * q_FLU_to_FRD.Inverse(); - gz::math::Quaterniond q_nb = q_ENU_to_NED * q_gb; + gz::math::Quaterniond q_nb; + GZBridge::rotateQuaternion(q_nb, q_gr); // publish attitude groundtruth vehicle_attitude_s vehicle_attitude_groundtruth{}; @@ -621,35 +610,53 @@ void GZBridge::odometryCallback(const gz::msgs::OdometryWithCovariance &odometry odom.timestamp_sample = hrt_absolute_time(); odom.timestamp = hrt_absolute_time(); #endif - odom.pose_frame = vehicle_odometry_s::POSE_FRAME_NED; + // gz odometry position is in ENU frame and needs to be converted to NED + odom.pose_frame = vehicle_odometry_s::POSE_FRAME_NED; odom.position[0] = odometry.pose_with_covariance().pose().position().y(); odom.position[1] = odometry.pose_with_covariance().pose().position().x(); odom.position[2] = -odometry.pose_with_covariance().pose().position().z(); - odom.velocity[0] = odometry.twist_with_covariance().twist().linear().y(); - odom.velocity[1] = odometry.twist_with_covariance().twist().linear().x(); + // gz odometry orientation is "body FLU->ENU" and needs to be converted in "body FRD->NED" + gz::msgs::Quaternion pose_orientation = odometry.pose_with_covariance().pose().orientation(); + gz::math::Quaterniond q_gr = gz::math::Quaterniond( + pose_orientation.w(), + pose_orientation.x(), + pose_orientation.y(), + pose_orientation.z()); + gz::math::Quaterniond q_nb; + GZBridge::rotateQuaternion(q_nb, q_gr); + odom.q[0] = q_nb.W(); + odom.q[1] = q_nb.X(); + odom.q[2] = q_nb.Y(); + odom.q[3] = q_nb.Z(); + + // gz odometry linear velocity is in body FLU and needs to be converted in body FRD + odom.velocity_frame = vehicle_odometry_s::VELOCITY_FRAME_BODY_FRD; + odom.velocity[0] = odometry.twist_with_covariance().twist().linear().x(); + odom.velocity[1] = -odometry.twist_with_covariance().twist().linear().y(); odom.velocity[2] = -odometry.twist_with_covariance().twist().linear().z(); - odom.angular_velocity[0] = odometry.twist_with_covariance().twist().angular().y(); - odom.angular_velocity[1] = odometry.twist_with_covariance().twist().angular().x(); + // gz odometry angular velocity is in body FLU and need to be converted in body FRD + odom.angular_velocity[0] = odometry.twist_with_covariance().twist().angular().x(); + odom.angular_velocity[1] = -odometry.twist_with_covariance().twist().angular().y(); odom.angular_velocity[2] = -odometry.twist_with_covariance().twist().angular().z(); // VISION_POSITION_ESTIMATE covariance - // Row-major representation of pose 6x6 cross-covariance matrix upper right triangle - // (states: x, y, z, roll, pitch, yaw; first six entries are the first ROW, next five entries are the second ROW, etc.). + // pose 6x6 cross-covariance matrix + // (states: x, y, z, roll, pitch, yaw). // If unknown, assign NaN value to first element in the array. - odom.position_variance[0] = odometry.pose_with_covariance().covariance().data(0); // X row 0, col 0 - odom.position_variance[1] = odometry.pose_with_covariance().covariance().data(7); // Y row 1, col 1 + odom.position_variance[0] = odometry.pose_with_covariance().covariance().data(7); // Y row 1, col 1 + odom.position_variance[1] = odometry.pose_with_covariance().covariance().data(0); // X row 0, col 0 odom.position_variance[2] = odometry.pose_with_covariance().covariance().data(14); // Z row 2, col 2 odom.orientation_variance[0] = odometry.pose_with_covariance().covariance().data(21); // R row 3, col 3 odom.orientation_variance[1] = odometry.pose_with_covariance().covariance().data(28); // P row 4, col 4 odom.orientation_variance[2] = odometry.pose_with_covariance().covariance().data(35); // Y row 5, col 5 - odom.velocity_variance[0] = odometry.twist_with_covariance().covariance().data(0); // R row 3, col 3 - odom.velocity_variance[1] = odometry.twist_with_covariance().covariance().data(7); // P row 4, col 4 - odom.velocity_variance[2] = odometry.twist_with_covariance().covariance().data(14); // Y row 5, col 5 + odom.velocity_variance[0] = odometry.twist_with_covariance().covariance().data(7); // Y row 1, col 1 + odom.velocity_variance[1] = odometry.twist_with_covariance().covariance().data(0); // X row 0, col 0 + odom.velocity_variance[2] = odometry.twist_with_covariance().covariance().data(14); // Z row 2, col 2 // odom.reset_counter = vpe.reset_counter; _visual_odometry_pub.publish(odom); @@ -657,6 +664,24 @@ void GZBridge::odometryCallback(const gz::msgs::OdometryWithCovariance &odometry pthread_mutex_unlock(&_node_mutex); } +void GZBridge::rotateQuaternion(gz::math::Quaterniond &q_FRD_to_NED, const gz::math::Quaterniond q_FLU_to_ENU) +{ + // FLU (ROS) to FRD (PX4) static rotation + static const auto q_FLU_to_FRD = gz::math::Quaterniond(0, 1, 0, 0); + + /** + * @brief Quaternion for rotation between ENU and NED frames + * + * NED to ENU: +PI/2 rotation about Z (Down) followed by a +PI rotation around X (old North/new East) + * ENU to NED: +PI/2 rotation about Z (Up) followed by a +PI rotation about X (old East/new North) + * This rotation is symmetric, so q_ENU_to_NED == q_NED_to_ENU. + */ + static const auto q_ENU_to_NED = gz::math::Quaterniond(0, 0.70711, 0.70711, 0); + + // final rotation composition + q_FRD_to_NED = q_ENU_to_NED * q_FLU_to_ENU * q_FLU_to_FRD.Inverse(); +} + void GZBridge::Run() { if (should_exit()) { diff --git a/src/modules/simulation/gz_bridge/GZBridge.hpp b/src/modules/simulation/gz_bridge/GZBridge.hpp index 9ea89d36a7..b7d40eef36 100644 --- a/src/modules/simulation/gz_bridge/GZBridge.hpp +++ b/src/modules/simulation/gz_bridge/GZBridge.hpp @@ -103,6 +103,16 @@ private: void poseInfoCallback(const gz::msgs::Pose_V &pose); void odometryCallback(const gz::msgs::OdometryWithCovariance &odometry); + /** + * + * Convert a quaterion from FLU_to_ENU frames (ROS convention) + * to FRD_to_NED frames (PX4 convention) + * + * @param q_FRD_to_NED output quaterion in PX4 conventions + * @param q_FLU_to_ENU input quaterion in ROS conventions + */ + static void rotateQuaternion(gz::math::Quaterniond &q_FRD_to_NED, const gz::math::Quaterniond q_FLU_to_ENU); + // Subscriptions uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};