Hello,
As I am trying to apply vo from rtabmap, I do have problems converting the ros enu odometry message from rtabmap to feed into px4 vio. The general setup is to use gz (harmonic) as the simulator, and the x500_depth model with 5002 configuration in a warehouse (previously tugbot in a warehouse in which I removed tugbot), initially towards east. The drone is at some position else than (0,0,0) (the original tugbot location), and I use this position as the ros map and drone’s odom frame. rtabmap works well with an extra camera I added in the gz x500_depth model in rgbd mode. If I run the rtabmap as well as other simulation setups, the drone is flying normally and rtabmap showing somehow correct trajectory with drifts. But, once I bring up my node subscribing the odometry from rtabmap and feed it to fmu/in/visual_odometry with the navigation interface from ros2_cpp_lib, the vo would only work without rotation. If I takeoff and move around without rotation (always towards east), the vo works well, and the drone is much more stable than that without vo. But as long as I horizontally rotate the drone to point some direction else than east, the drone would start circling
And the larger the angle I yawed, the larger the radius of the circle is:
So I doubt if I had my vo node wrong. My conversion code:
auto onRtabmapOdom = [this](const nav_msgs::msg::Odometry &odomMsg) {
// PoseWithCovariance
Eigen::Affine3d poseEnu;
tf2::fromMsg(odomMsg.pose.pose, poseEnu);
Eigen::Quaterniond qNed{frame_transforms::ros_to_px4_orientation(
Eigen::Quaterniond(poseEnu.linear()))};
qNed.normalize();
Eigen::Vector3d poseNed{frame_transforms::enu_to_ned_local_frame(
Eigen::Vector3d(poseEnu.translation()))};
const frame_transforms::Covariance6d &poseCovEnu =
odomMsg.pose.covariance;
frame_transforms::Covariance6d poseCovNed =
frame_transforms::transform_static_frame(
poseCovEnu, frame_transforms::StaticTF::ENU_TO_NED);
// TwistWithCovariance
Eigen::Matrix<double, 6, 1> twistEnu;
tf2::fromMsg(odomMsg.twist.twist, twistEnu);
Eigen::Vector3d linearVelocityEnu{twistEnu.head(3)};
Eigen::Vector3d linearVelocityNed{
frame_transforms::enu_to_ned_local_frame(linearVelocityEnu)};
// struct LocalPositionMeasurement does not contain angular velocity so
// ignore the angualr velocity
Eigen::Quaterniond qAngularVelocityEnu{
frame_transforms::utils::quaternion::quaternion_from_euler(
twistEnu.tail(3))};
Eigen::Quaterniond qAngularVelocityNed{
frame_transforms::ros_to_px4_orientation(qAngularVelocityEnu)};
const frame_transforms::Covariance6d &twistCovEnu =
odomMsg.twist.covariance;
frame_transforms::Covariance6d twistCovNed =
frame_transforms::transform_static_frame(
twistCovEnu, frame_transforms::StaticTF::ENU_TO_NED);
// Updating localPose
px4_ros2::LocalPositionMeasurement localPose;
localPose.timestamp_sample = odomMsg.header.stamp;
// Pose part
localPose.position_xy = poseNed.head(2).cast<float>();
localPose.position_xy_variance =
Eigen::Vector2f{poseCovNed[0], poseCovNed[1 + 1 * 6]};
localPose.position_z = poseNed(2);
localPose.position_z_variance = poseCovNed[2 + 2 * 6];
localPose.velocity_xy = linearVelocityNed.head(2).cast<float>();
localPose.velocity_xy_variance =
Eigen::Vector2f{twistCovNed[0], twistCovNed[1 + 1 * 6]};
localPose.velocity_z = linearVelocityNed(2);
localPose.velocity_z_variance = twistCovNed[2 + 2 * 6];
localPose.attitude_quaternion = qNed.cast<float>();
localPose.attitude_variance =
Eigen::Vector3f{static_cast<float>(poseCovNed[3 + 3 * 6]),
static_cast<float>(poseCovNed[4 + 4 * 6]),
static_cast<float>(poseCovNed[5 + 5 * 6])};
this->_interface->updateLocalPosition(localPose);
};
The pose frame and velocity frame of the vehicleodometry are both set to localned.
So I would doubt that there is something wrong in the conversion here. And if not I would appreciate any suggestion on how to further debug this problem. Thanks