dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

gz_bridge: fix missing orientation and frames in odometryCallback (#21348) 

Signed-off-by: Beniamino Pozzan <beniamino.pozzan@phd.unipd.it>
This commit is contained in:
Beniamino Pozzan 2023-04-12 13:46:16 -07:00 committed by GitHub
parent 67d03461d1
commit 98026a3e10
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
2 changed files with 60 additions and 25 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

75
src/modules/simulation/gz_bridge/GZBridge.cpp
View File

@ -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()) {

10
src/modules/simulation/gz_bridge/GZBridge.hpp
View File

@ -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};