Yes, I was sending offboard signals at a constant rate higher than 2Hz.
I noticed that the /pose topic is not being published initially by the slam_toolbox package. To work around this, I used the /tf topic to extract the relevant position and calculated both velocity and angular velocity using the previous and current position along with quaternion parameters (q). Now able to arm the drone.
However, the computed values for position, velocity, and angular velocity exhibit sudden spikes, resulting in erratic drone behavior—moving inconsistently or appearing to jump around.
Below is the code I used to generate data for the topic /fmu/in/vehicle_visual_odometry. Could you please confirm whether this is the correct approach for calculating position, velocity and angular_velocity to be published to this topic, or suggest any changes that may be required?
or needs to add any other topic data for calculation?
void publish_visual_odometry_from_tf() {
try {
auto tf_map_odom = tf_buffer_.lookupTransform("map", "odom", tf2::TimePointZero);
auto tf_odom_base = tf_buffer_.lookupTransform("odom", "base_footprint", tf2::TimePointZero);
geometry_msgs::msg::TransformStamped tf_map_base;
tf2::doTransform(tf_odom_base, tf_map_base, tf_map_odom);
rclcpp::Time now = this->get_clock()->now();
// ENU pose
Eigen::Vector3d pos_enu(tf_map_base.transform.translation.x,
tf_map_base.transform.translation.y,
tf_map_base.transform.translation.z);
Eigen::Quaterniond q_enu(tf_map_base.transform.rotation.w,
tf_map_base.transform.rotation.x,
tf_map_base.transform.rotation.y,
tf_map_base.transform.rotation.z);
// Convert to NED
Eigen::Vector3d pos_ned = px4_ros_com::frame_transforms::enu_to_ned_local_frame(pos_enu);
Eigen::Quaterniond q_ned = px4_ros_com::frame_transforms::enu_to_ned_orientation(q_enu);
Eigen::Quaterniond q_ned_aircraft =
px4_ros_com::frame_transforms::aircraft_to_baselink_orientation(q_ned);
// Velocity
Eigen::Vector3d vel_ned(0, 0, 0);
Eigen::Vector3d ang_vel_ned(0, 0, 0);
if (has_prev_ && (now - prev_time_).seconds() > 0.01) {
double dt = (now - prev_time_).seconds();
Eigen::Vector3d vel_enu = (pos_enu - prev_pos_) / dt;
vel_ned = px4_ros_com::frame_transforms::enu_to_ned_local_frame(vel_enu);
Eigen::Quaterniond delta_q = prev_q_enu_.inverse() * q_enu;
if (delta_q.norm() > 1e-6) {
Eigen::AngleAxisd angle_axis(delta_q);
Eigen::Vector3d ang_vel_enu = angle_axis.axis() * angle_axis.angle() / dt;
ang_vel_ned = px4_ros_com::frame_transforms::enu_to_ned_local_frame(ang_vel_enu);
}
}
// Update stored state
prev_pos_ = pos_enu;
prev_q_enu_ = q_enu;
prev_time_ = now;
has_prev_ = true;
// Fill and publish VehicleOdometry message
px4_msgs::msg::VehicleOdometry vo;
vo.timestamp = now.nanoseconds() / 1000;
vo.pose_frame = px4_msgs::msg::VehicleOdometry::POSE_FRAME_NED;
vo.velocity_frame = px4_msgs::msg::VehicleOdometry::VELOCITY_FRAME_NED;
vo.position = {
static_cast<float>(pos_ned.x()),
static_cast<float>(pos_ned.y()),
static_cast<float>(pos_ned.z())
};
vo.q = {
static_cast<float>(q_ned_aircraft.x()),
static_cast<float>(q_ned_aircraft.y()),
static_cast<float>(q_ned_aircraft.z()),
static_cast<float>(q_ned_aircraft.w())
};
vo.velocity = {
static_cast<float>(vel_ned.x()),
static_cast<float>(vel_ned.y()),
static_cast<float>(vel_ned.z())
};
vo.angular_velocity = {
static_cast<float>(ang_vel_ned.x()),
static_cast<float>(ang_vel_ned.y()),
static_cast<float>(ang_vel_ned.z())
};
vo.position_variance = {0.01f, 0.01f, 0.02f};
vo.orientation_variance = {0.001f, 0.001f, 0.002f};
external_odom_publisher_->publish(vo);
} catch (const tf2::TransformException &ex) {
RCLCPP_WARN_THROTTLE(this->get_logger(), *this->get_clock(), 3000,
"TF error: %s", ex.what());
}
}
