rename vehicle_visual_odometry_aligned -> estimator_visual_odometry_aligned

- saves a small amount of work for the ekf2 selector in multi-EKF mode (visual_odometry_aligned now ignored)
 - helps to distinguish the origin/purpose from vehicle_odometry and vehicle_visual_odometry

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

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

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

src/modules/ekf2/EKF2.hpp

@@ -225,6 +225,7 @@ private:
 	uORB::PublicationMulti<estimator_sensor_bias_s>		_estimator_sensor_bias_pub{ORB_ID(estimator_sensor_bias)};
 	uORB::PublicationMulti<estimator_states_s>		_estimator_states_pub{ORB_ID(estimator_states)};
 	uORB::PublicationMultiData<estimator_status_s>		_estimator_status_pub{ORB_ID(estimator_status)};
+	uORB::PublicationMultiData<vehicle_odometry_s>          _estimator_visual_odometry_aligned_pub{ORB_ID(estimator_visual_odometry_aligned)};
 	uORB::PublicationMulti<yaw_estimator_status_s>		_yaw_est_pub{ORB_ID(yaw_estimator_status)};
 	uORB::PublicationMulti<wind_estimate_s>			_wind_pub{ORB_ID(wind_estimate)};
 
@@ -233,7 +234,6 @@ private:
 	uORB::PublicationMultiData<vehicle_local_position_s>  _local_position_pub;
 	uORB::PublicationMultiData<vehicle_global_position_s> _global_position_pub;
 	uORB::PublicationMulti<vehicle_odometry_s>            _odometry_pub;
-	uORB::PublicationMultiData<vehicle_odometry_s>        _visual_odometry_aligned_pub;
 
 	Ekf _ekf;
 

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

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_s>   _vehicle_global_position_pub{ORB_ID(vehicle_global_position)};
 	uORB::Publication<vehicle_local_position_s>    _vehicle_local_position_pub{ORB_ID(vehicle_local_position)};
 	uORB::Publication<vehicle_odometry_s>          _vehicle_odometry_pub{ORB_ID(vehicle_odometry)};
-	uORB::Publication<vehicle_odometry_s>          _vehicle_visual_odometry_aligned_pub{ORB_ID(vehicle_visual_odometry_aligned)};
 
 	DEFINE_PARAMETERS(
 		(ParamFloat<px4::params::EKF2_SEL_ERR_RED>) _param_ekf2_sel_err_red,

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");
 }