forked from Archive/PX4-Autopilot
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:
parent
67d03461d1
commit
98026a3e10
|
@ -486,17 +486,6 @@ void GZBridge::poseInfoCallback(const gz::msgs::Pose_V &pose)
|
||||||
gz::msgs::Vector3d pose_position = pose.pose(p).position();
|
gz::msgs::Vector3d pose_position = pose.pose(p).position();
|
||||||
gz::msgs::Quaternion pose_orientation = pose.pose(p).orientation();
|
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
|
// ground truth
|
||||||
gz::math::Quaterniond q_gr = gz::math::Quaterniond(
|
gz::math::Quaterniond q_gr = gz::math::Quaterniond(
|
||||||
pose_orientation.w(),
|
pose_orientation.w(),
|
||||||
|
@ -504,8 +493,8 @@ void GZBridge::poseInfoCallback(const gz::msgs::Pose_V &pose)
|
||||||
pose_orientation.y(),
|
pose_orientation.y(),
|
||||||
pose_orientation.z());
|
pose_orientation.z());
|
||||||
|
|
||||||
gz::math::Quaterniond q_gb = q_gr * q_FLU_to_FRD.Inverse();
|
gz::math::Quaterniond q_nb;
|
||||||
gz::math::Quaterniond q_nb = q_ENU_to_NED * q_gb;
|
GZBridge::rotateQuaternion(q_nb, q_gr);
|
||||||
|
|
||||||
// publish attitude groundtruth
|
// publish attitude groundtruth
|
||||||
vehicle_attitude_s vehicle_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_sample = hrt_absolute_time();
|
||||||
odom.timestamp = hrt_absolute_time();
|
odom.timestamp = hrt_absolute_time();
|
||||||
#endif
|
#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[0] = odometry.pose_with_covariance().pose().position().y();
|
||||||
odom.position[1] = odometry.pose_with_covariance().pose().position().x();
|
odom.position[1] = odometry.pose_with_covariance().pose().position().x();
|
||||||
odom.position[2] = -odometry.pose_with_covariance().pose().position().z();
|
odom.position[2] = -odometry.pose_with_covariance().pose().position().z();
|
||||||
|
|
||||||
odom.velocity[0] = odometry.twist_with_covariance().twist().linear().y();
|
// gz odometry orientation is "body FLU->ENU" and needs to be converted in "body FRD->NED"
|
||||||
odom.velocity[1] = odometry.twist_with_covariance().twist().linear().x();
|
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.velocity[2] = -odometry.twist_with_covariance().twist().linear().z();
|
||||||
|
|
||||||
odom.angular_velocity[0] = odometry.twist_with_covariance().twist().angular().y();
|
// gz odometry angular velocity is in body FLU and need to be converted in body FRD
|
||||||
odom.angular_velocity[1] = odometry.twist_with_covariance().twist().angular().x();
|
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();
|
odom.angular_velocity[2] = -odometry.twist_with_covariance().twist().angular().z();
|
||||||
|
|
||||||
// VISION_POSITION_ESTIMATE covariance
|
// VISION_POSITION_ESTIMATE covariance
|
||||||
// Row-major representation of pose 6x6 cross-covariance matrix upper right triangle
|
// pose 6x6 cross-covariance matrix
|
||||||
// (states: x, y, z, roll, pitch, yaw; first six entries are the first ROW, next five entries are the second ROW, etc.).
|
// (states: x, y, z, roll, pitch, yaw).
|
||||||
// If unknown, assign NaN value to first element in the array.
|
// 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[0] = odometry.pose_with_covariance().covariance().data(7); // Y row 1, col 1
|
||||||
odom.position_variance[1] = 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.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[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[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.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[0] = odometry.twist_with_covariance().covariance().data(7); // Y row 1, col 1
|
||||||
odom.velocity_variance[1] = odometry.twist_with_covariance().covariance().data(7); // P row 4, col 4
|
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); // Y row 5, col 5
|
odom.velocity_variance[2] = odometry.twist_with_covariance().covariance().data(14); // Z row 2, col 2
|
||||||
|
|
||||||
// odom.reset_counter = vpe.reset_counter;
|
// odom.reset_counter = vpe.reset_counter;
|
||||||
_visual_odometry_pub.publish(odom);
|
_visual_odometry_pub.publish(odom);
|
||||||
|
@ -657,6 +664,24 @@ void GZBridge::odometryCallback(const gz::msgs::OdometryWithCovariance &odometry
|
||||||
pthread_mutex_unlock(&_node_mutex);
|
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()
|
void GZBridge::Run()
|
||||||
{
|
{
|
||||||
if (should_exit()) {
|
if (should_exit()) {
|
||||||
|
|
|
@ -103,6 +103,16 @@ private:
|
||||||
void poseInfoCallback(const gz::msgs::Pose_V &pose);
|
void poseInfoCallback(const gz::msgs::Pose_V &pose);
|
||||||
void odometryCallback(const gz::msgs::OdometryWithCovariance &odometry);
|
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
|
// Subscriptions
|
||||||
uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
|
uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
|
||||||
|
|
||||||
|
|
Loading…
Reference in New Issue