diff --git a/msg/tools/uorb_rtps_message_ids.yaml b/msg/tools/uorb_rtps_message_ids.yaml index 80c3f962bc..0d5c5797d4 100644 --- a/msg/tools/uorb_rtps_message_ids.yaml +++ b/msg/tools/uorb_rtps_message_ids.yaml @@ -349,7 +349,7 @@ rtps: - msg: vehicle_angular_velocity_groundtruth id: 168 alias: vehicle_angular_velocity - - msg: vehicle_visual_odometry_aligned + - msg: estimator_visual_odometry_aligned id: 169 alias: vehicle_odometry - msg: estimator_innovation_variances diff --git a/msg/vehicle_odometry.msg b/msg/vehicle_odometry.msg index 4a9dce823f..4e2a0d13f9 100644 --- a/msg/vehicle_odometry.msg +++ b/msg/vehicle_odometry.msg @@ -62,5 +62,5 @@ float32 yawspeed # Angular velocity about Z body axis # If angular velocity covariance invalid/unknown, 16th cell is NaN float32[21] velocity_covariance -# TOPICS vehicle_odometry vehicle_mocap_odometry vehicle_visual_odometry vehicle_visual_odometry_aligned +# TOPICS vehicle_odometry vehicle_mocap_odometry vehicle_visual_odometry # TOPICS estimator_odometry estimator_visual_odometry_aligned diff --git a/src/modules/ekf2/EKF2.cpp b/src/modules/ekf2/EKF2.cpp index f354f09ef4..6428dae8dc 100644 --- a/src/modules/ekf2/EKF2.cpp +++ b/src/modules/ekf2/EKF2.cpp @@ -54,8 +54,6 @@ EKF2::EKF2(int instance, const px4::wq_config_t &config, int imu, int mag, bool _local_position_pub(_multi_mode ? ORB_ID(estimator_local_position) : ORB_ID(vehicle_local_position)), _global_position_pub(_multi_mode ? ORB_ID(estimator_global_position) : ORB_ID(vehicle_global_position)), _odometry_pub(_multi_mode ? ORB_ID(estimator_odometry) : ORB_ID(vehicle_odometry)), - _visual_odometry_aligned_pub(_multi_mode ? ORB_ID(estimator_visual_odometry_aligned) : - ORB_ID(vehicle_visual_odometry_aligned)), _params(_ekf.getParamHandle()), _param_ekf2_min_obs_dt(_params->sensor_interval_min_ms), _param_ekf2_mag_delay(_params->mag_delay_ms), @@ -171,7 +169,6 @@ EKF2::EKF2(int instance, const px4::wq_config_t &config, int imu, int mag, bool _local_position_pub.advertise(); _global_position_pub.advertise(); _odometry_pub.advertise(); - _visual_odometry_aligned_pub.advertise(); _ekf2_timestamps_pub.advertise(); _ekf_gps_drift_pub.advertise(); @@ -182,6 +179,7 @@ EKF2::EKF2(int instance, const px4::wq_config_t &config, int imu, int mag, bool _estimator_sensor_bias_pub.advertise(); _estimator_states_pub.advertise(); _estimator_status_pub.advertise(); + _estimator_visual_odometry_aligned_pub.advertised(); _wind_pub.advertise(); _yaw_est_pub.advertise(); @@ -851,7 +849,7 @@ void EKF2::Run() ev_quat_aligned.copyTo(aligned_ev_odom.q); quat_ev2ekf.copyTo(aligned_ev_odom.q_offset); - _visual_odometry_aligned_pub.publish(aligned_ev_odom); + _estimator_visual_odometry_aligned_pub.publish(aligned_ev_odom); } if (_ekf.global_position_is_valid() && !_preflt_checker.hasFailed()) { diff --git a/src/modules/ekf2/EKF2.hpp b/src/modules/ekf2/EKF2.hpp index 963b74f15c..0b1f8778a6 100644 --- a/src/modules/ekf2/EKF2.hpp +++ b/src/modules/ekf2/EKF2.hpp @@ -225,6 +225,7 @@ private: uORB::PublicationMulti _estimator_sensor_bias_pub{ORB_ID(estimator_sensor_bias)}; uORB::PublicationMulti _estimator_states_pub{ORB_ID(estimator_states)}; uORB::PublicationMultiData _estimator_status_pub{ORB_ID(estimator_status)}; + uORB::PublicationMultiData _estimator_visual_odometry_aligned_pub{ORB_ID(estimator_visual_odometry_aligned)}; uORB::PublicationMulti _yaw_est_pub{ORB_ID(yaw_estimator_status)}; uORB::PublicationMulti _wind_pub{ORB_ID(wind_estimate)}; @@ -233,7 +234,6 @@ private: uORB::PublicationMultiData _local_position_pub; uORB::PublicationMultiData _global_position_pub; uORB::PublicationMulti _odometry_pub; - uORB::PublicationMultiData _visual_odometry_aligned_pub; Ekf _ekf; diff --git a/src/modules/ekf2/EKF2Selector.cpp b/src/modules/ekf2/EKF2Selector.cpp index cb802dd519..9425fec97f 100644 --- a/src/modules/ekf2/EKF2Selector.cpp +++ b/src/modules/ekf2/EKF2Selector.cpp @@ -586,18 +586,6 @@ void EKF2Selector::Run() } } } - - // selected estimator_visual_odometry_aligned -> vehicle_visual_odometry_aligned - if (_instance[_selected_instance].estimator_visual_odometry_aligned_sub.updated()) { - vehicle_odometry_s vehicle_visual_odometry_aligned; - - if (_instance[_selected_instance].estimator_visual_odometry_aligned_sub.update(&vehicle_visual_odometry_aligned)) { - if (vehicle_visual_odometry_aligned.timestamp >= _instance[_selected_instance].estimator_status.timestamp_sample) { - vehicle_visual_odometry_aligned.timestamp = hrt_absolute_time(); - _vehicle_visual_odometry_aligned_pub.publish(vehicle_visual_odometry_aligned); - } - } - } } if (_lockstep_component == -1) { diff --git a/src/modules/ekf2/EKF2Selector.hpp b/src/modules/ekf2/EKF2Selector.hpp index 07fca36889..61c4f40e1c 100644 --- a/src/modules/ekf2/EKF2Selector.hpp +++ b/src/modules/ekf2/EKF2Selector.hpp @@ -86,7 +86,6 @@ private: estimator_local_position_sub{ORB_ID(estimator_local_position), i}, estimator_global_position_sub{ORB_ID(estimator_global_position), i}, estimator_odometry_sub{ORB_ID(estimator_odometry), i}, - estimator_visual_odometry_aligned_sub{ORB_ID(estimator_visual_odometry_aligned), i}, instance(i) {} @@ -96,7 +95,6 @@ private: uORB::Subscription estimator_local_position_sub; uORB::Subscription estimator_global_position_sub; uORB::Subscription estimator_odometry_sub; - uORB::Subscription estimator_visual_odometry_aligned_sub; estimator_status_s estimator_status{}; @@ -187,7 +185,6 @@ private: uORB::Publication _vehicle_global_position_pub{ORB_ID(vehicle_global_position)}; uORB::Publication _vehicle_local_position_pub{ORB_ID(vehicle_local_position)}; uORB::Publication _vehicle_odometry_pub{ORB_ID(vehicle_odometry)}; - uORB::Publication _vehicle_visual_odometry_aligned_pub{ORB_ID(vehicle_visual_odometry_aligned)}; DEFINE_PARAMETERS( (ParamFloat) _param_ekf2_sel_err_red, diff --git a/src/modules/logger/logged_topics.cpp b/src/modules/logger/logged_topics.cpp index 41b8897ae0..11cac52d25 100644 --- a/src/modules/logger/logged_topics.cpp +++ b/src/modules/logger/logged_topics.cpp @@ -123,6 +123,7 @@ void LoggedTopics::add_default_topics() add_topic_multi("estimator_sensor_bias", 1000, MAX_ESTIMATOR_INSTANCES); add_topic_multi("estimator_states", 1000, MAX_ESTIMATOR_INSTANCES); add_topic_multi("estimator_status", 500, MAX_ESTIMATOR_INSTANCES); + add_topic_multi("estimator_visual_odometry_aligned", 200, MAX_ESTIMATOR_INSTANCES); add_topic_multi("yaw_estimator_status", 1000, MAX_ESTIMATOR_INSTANCES); add_topic_multi("wind_estimate", 1000); // published by both ekf2 and airspeed_selector @@ -192,7 +193,6 @@ void LoggedTopics::add_estimator_replay_topics() add_topic("vehicle_magnetometer"); add_topic("vehicle_status"); add_topic("vehicle_visual_odometry"); - add_topic("vehicle_visual_odometry_aligned"); add_topic_multi("distance_sensor"); }