Unverified Commit df2f26eb authored by Daniel Agar's avatar Daniel Agar Committed by GitHub
Browse files

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
parent d5e68bc0
......@@ -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
......
......@@ -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
......@@ -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()) {
......
......@@ -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;
......
......@@ -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) {
......
......@@ -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,
......
......@@ -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");
}
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment