Skip to content

Commit eeca83e

Browse files
jonas-eschmannjonas-eschmann
committed
ripping out visual odometry checks
1 parent 01bc215 commit eeca83e

2 files changed

Lines changed: 19 additions & 190 deletions

File tree

‎src/modules/mc_raptor/mc_raptor.cpp‎

Lines changed: 17 additions & 170 deletions
Original file line numberDiff line numberDiff line change
@@ -25,8 +25,6 @@ Raptor::Raptor(): ModuleParams(nullptr), ScheduledWorkItem(MODULE_NAME, px4::wq_
2525
_tune_control_pub.advertise();
2626
}
2727
void Raptor::reset(){
28-
this->visual_odometry_stale_counter = 0;
29-
this->timestamp_last_visual_odometry_stale_set = false;
3028
for(TI action_i=0; action_i < EXECUTOR_SPEC::OUTPUT_DIM; action_i++){
3129
this->previous_action[action_i] = RESET_PREVIOUS_ACTION_VALUE;
3230
}
@@ -115,10 +113,6 @@ bool Raptor::init()
115113
PX4_ERR("vehicle_local_position_sub callback registration failed");
116114
return false;
117115
}
118-
if (!_vehicle_visual_odometry_sub.registerCallback()) {
119-
PX4_ERR("vehicle_visual_odometry_sub callback registration failed");
120-
return false;
121-
}
122116
if (!_vehicle_angular_velocity_sub.registerCallback()) {
123117
PX4_ERR("vehicle_angular_velocity_sub callback registration failed");
124118
return false;
@@ -298,13 +292,6 @@ void Raptor::observe(rl_tools::inference::applications::l2f::Observation<EXECUTO
298292

299293
{ // Orientation
300294
// FRD to FLU
301-
// Validating Julia code:
302-
// using Rotations
303-
// FRD2FLU = [1 0 0; 0 -1 0; 0 0 -1]
304-
// q = rand(UnitQuaternion)
305-
// q2 = UnitQuaternion(q.q.s, q.q.v1, -q.q.v2, -q.q.v3)
306-
// diff = q2 - FRD2FLU * q * transpose(FRD2FLU)
307-
// @assert sum(abs.(diff)) < 1e-10
308295
T q_target[4];
309296
q_target[0] = cosf(0.5f * _trajectory_setpoint.yaw); // minus because the setpoint yaw is in NED
310297
q_target[1] = 0;
@@ -395,7 +382,6 @@ void Raptor::updateArmingCheckReply(bool active){
395382
void Raptor::Run(){
396383
if (should_exit()) {
397384
_vehicle_local_position_sub.unregisterCallback();
398-
_vehicle_visual_odometry_sub.unregisterCallback();
399385
_vehicle_angular_velocity_sub.unregisterCallback();
400386
_vehicle_attitude_sub.unregisterCallback();
401387
if(flightmode_state >= FlightModeState::REGISTERED){
@@ -451,25 +437,6 @@ void Raptor::Run(){
451437
status.substep = 0;
452438
status.active = false;
453439
status.control_interval = NAN;
454-
status.visual_odometry_stale_counter = this->visual_odometry_stale_counter;
455-
status.visual_odometry_age = 0;
456-
status.visual_odometry_dt_mean = 0;
457-
status.visual_odometry_dt_std = 0;
458-
for(TI odometry_dt_i = 0; odometry_dt_i < raptor_status_s::NUM_VISUAL_ODOMETRY_DT_MAX; odometry_dt_i++){
459-
status.visual_odometry_dt_max[odometry_dt_i] = 0;
460-
}
461-
462-
if(Raptor::ODOMETRY_SOURCE == Raptor::OdometrySource::LOCAL_POSITION){
463-
status.odometry_source = raptor_status_s::ODOMETRY_SOURCE_LOCAL_POSITION;
464-
}
465-
else{
466-
if(Raptor::ODOMETRY_SOURCE == Raptor::OdometrySource::VISUAL_ODOMETRY){
467-
status.odometry_source = raptor_status_s::ODOMETRY_SOURCE_VISUAL_ODOMETRY;
468-
}
469-
else{
470-
status.odometry_source = raptor_status_s::ODOMETRY_SOURCE_UNKNOWN;
471-
}
472-
}
473440

474441
bool angular_velocity_update = false;
475442
status.subscription_update_angular_velocity = _vehicle_angular_velocity_sub.update(&_vehicle_angular_velocity);
@@ -486,37 +453,6 @@ void Raptor::Run(){
486453
timestamp_last_local_position_set = true;
487454
}
488455

489-
status.subscription_update_visual_odometry = _vehicle_visual_odometry_sub.update(&_vehicle_visual_odometry);
490-
if(status.subscription_update_visual_odometry){
491-
if(timestamp_last_visual_odometry_set){
492-
odometry_dts[odometry_dt_index] = current_time - timestamp_last_visual_odometry;
493-
odometry_dt_index = (odometry_dt_index + 1) % NUM_ODOMETRY_DTS;
494-
if(odometry_dt_index == 0){
495-
odometry_dts_full = true;
496-
}
497-
}
498-
timestamp_last_visual_odometry = current_time;
499-
timestamp_last_visual_odometry_set = true;
500-
}
501-
for(TI odometry_dt_i = 0; odometry_dt_i < (odometry_dts_full ? NUM_ODOMETRY_DTS : odometry_dt_index); odometry_dt_i++){
502-
auto value = odometry_dts[odometry_dt_i];
503-
status.visual_odometry_dt_mean += value;
504-
status.visual_odometry_dt_std += value * value;
505-
TI max_index = 0;
506-
bool max_index_set = false;
507-
for(TI odometry_dt_max_i = 0; odometry_dt_max_i < raptor_status_s::NUM_VISUAL_ODOMETRY_DT_MAX; odometry_dt_max_i++){
508-
if(value > status.visual_odometry_dt_max[odometry_dt_max_i]){
509-
max_index = odometry_dt_max_i;
510-
max_index_set = true;
511-
}
512-
}
513-
if(max_index_set){
514-
status.visual_odometry_dt_max[max_index] = value;
515-
}
516-
}
517-
status.visual_odometry_dt_mean /= NUM_ODOMETRY_DTS;
518-
status.visual_odometry_dt_std = sqrt(status.visual_odometry_dt_std / NUM_ODOMETRY_DTS - status.visual_odometry_dt_mean * status.visual_odometry_dt_mean);
519-
520456
status.subscription_update_attitude = _vehicle_attitude_sub.update(&_vehicle_attitude);
521457
if(status.subscription_update_attitude){
522458
timestamp_last_attitude = current_time;
@@ -555,8 +491,7 @@ void Raptor::Run(){
555491
return;
556492
}
557493

558-
bool timestamp_last_odometry_set = (Raptor::ODOMETRY_SOURCE == Raptor::OdometrySource::LOCAL_POSITION && timestamp_last_local_position_set) || (Raptor::ODOMETRY_SOURCE == Raptor::OdometrySource::VISUAL_ODOMETRY && timestamp_last_visual_odometry_set);
559-
if(!timestamp_last_angular_velocity_set || !timestamp_last_odometry_set || !timestamp_last_attitude_set){
494+
if(!timestamp_last_angular_velocity_set || !timestamp_last_local_position_set || !timestamp_last_attitude_set){
560495
status.exit_reason = raptor_status_s::EXIT_REASON_NOT_ALL_OBSERVATIONS_SET;
561496
if constexpr(PUBLISH_NON_COMPLETE_STATUS){
562497
_raptor_status_pub.publish(status);
@@ -576,113 +511,25 @@ void Raptor::Run(){
576511
updateArmingCheckReply(false);
577512
return;
578513
}
579-
if(Raptor::ODOMETRY_SOURCE == Raptor::OdometrySource::LOCAL_POSITION){
580-
if((current_time - timestamp_last_local_position) > OBSERVATION_TIMEOUT_LOCAL_POSITION){
581-
status.exit_reason = raptor_status_s::EXIT_REASON_LOCAL_POSITION_STALE;
582-
if constexpr(PUBLISH_NON_COMPLETE_STATUS){
583-
_raptor_status_pub.publish(status);
584-
}
585-
if(!timeout_message_sent){
586-
PX4_ERR("local position timeout");
587-
timeout_message_sent = true;
588-
}
589-
updateArmingCheckReply(false);
590-
return;
514+
if((current_time - timestamp_last_local_position) > OBSERVATION_TIMEOUT_LOCAL_POSITION){
515+
status.exit_reason = raptor_status_s::EXIT_REASON_LOCAL_POSITION_STALE;
516+
if constexpr(PUBLISH_NON_COMPLETE_STATUS){
517+
_raptor_status_pub.publish(status);
591518
}
592-
else{
593-
position[0] = _vehicle_local_position.x;
594-
position[1] = _vehicle_local_position.y;
595-
position[2] = _vehicle_local_position.z;
596-
linear_velocity[0] = _vehicle_local_position.vx;
597-
linear_velocity[1] = _vehicle_local_position.vy;
598-
linear_velocity[2] = _vehicle_local_position.vz;
519+
if(!timeout_message_sent){
520+
PX4_ERR("local position timeout");
521+
timeout_message_sent = true;
599522
}
523+
updateArmingCheckReply(false);
524+
return;
600525
}
601-
{
602-
status.visual_odometry_age = current_time - timestamp_last_visual_odometry;
603-
if((current_time - timestamp_last_visual_odometry) > OBSERVATION_TIMEOUT_VISUAL_ODOMETRY){
604-
if(!timestamp_last_visual_odometry_stale_set || timestamp_last_visual_odometry_stale != timestamp_last_visual_odometry){
605-
// rising edge
606-
visual_odometry_stale_counter++;
607-
tune_control_s tune_control;
608-
tune_control.timestamp = current_time;
609-
tune_control.tune_id = 0;
610-
tune_control.volume = 100; //tune_control_s::VOLUME_LEVEL_DEFAULT;
611-
tune_control.tune_override = true;
612-
tune_control.frequency = 1000;
613-
tune_control.duration = 10000;
614-
_tune_control_pub.publish(tune_control);
615-
PX4_WARN("Visual odometry stale: begin");
616-
}
617-
timestamp_last_visual_odometry_stale = timestamp_last_visual_odometry;
618-
timestamp_last_visual_odometry_stale_set = true;
619-
if(Raptor::ODOMETRY_SOURCE == Raptor::OdometrySource::VISUAL_ODOMETRY){
620-
status.exit_reason = raptor_status_s::EXIT_REASON_VISUAL_ODOMETRY_STALE;
621-
if constexpr(PUBLISH_NON_COMPLETE_STATUS){
622-
_raptor_status_pub.publish(status);
623-
}
624-
if(!timeout_message_sent){
625-
PX4_ERR("Visual odometry timeout");
626-
timeout_message_sent = true;
627-
}
628-
updateArmingCheckReply(false);
629-
return;
630-
}
631-
}
632-
else{
633-
if(timestamp_last_visual_odometry_stale_set){
634-
// falling edge
635-
auto diff = current_time - timestamp_last_visual_odometry_stale;
636-
tune_control_s tune_control;
637-
tune_control.timestamp = current_time;
638-
tune_control.tune_id = 0;
639-
tune_control.volume = 100; //tune_control_s::VOLUME_LEVEL_DEFAULT;
640-
tune_control.tune_override = true;
641-
tune_control.frequency = 2000;
642-
tune_control.duration = min(10000000, diff);
643-
_tune_control_pub.publish(tune_control);
644-
PX4_WARN("Visual odometry stale: end %llu uS", (unsigned long long)diff);
645-
}
646-
timestamp_last_visual_odometry_stale_set = false;
647-
if(Raptor::ODOMETRY_SOURCE == Raptor::OdometrySource::VISUAL_ODOMETRY){
648-
if(_vehicle_visual_odometry.pose_frame != vehicle_odometry_s::POSE_FRAME_NED){
649-
status.exit_reason = raptor_status_s::EXIT_REASON_VISUAL_ODOMETRY_WRONG_FRAME;
650-
if constexpr(PUBLISH_NON_COMPLETE_STATUS){
651-
_raptor_status_pub.publish(status);
652-
}
653-
if(!timeout_message_sent){
654-
PX4_ERR("Visual odometry wrong frame (should be NED)");
655-
timeout_message_sent = true;
656-
}
657-
updateArmingCheckReply(false);
658-
return;
659-
}
660-
else{
661-
bool position_nan = std::isnan(_vehicle_visual_odometry.position[0]) || std::isnan(_vehicle_visual_odometry.position[1]) || std::isnan(_vehicle_visual_odometry.position[2]);
662-
bool velocity_nan = std::isnan(_vehicle_visual_odometry.velocity[0]) || std::isnan(_vehicle_visual_odometry.velocity[1]) || std::isnan(_vehicle_visual_odometry.velocity[2]);
663-
if(position_nan || velocity_nan){
664-
status.exit_reason = raptor_status_s::EXIT_REASON_VISUAL_ODOMETRY_NAN;
665-
if constexpr(PUBLISH_NON_COMPLETE_STATUS){
666-
_raptor_status_pub.publish(status);
667-
}
668-
if(!timeout_message_sent){
669-
PX4_ERR("Visual odometry contains NANs in position or velocity");
670-
timeout_message_sent = true;
671-
}
672-
updateArmingCheckReply(false);
673-
return;
674-
}
675-
else{
676-
position[0] = _vehicle_visual_odometry.position[0];
677-
position[1] = _vehicle_visual_odometry.position[1];
678-
position[2] = _vehicle_visual_odometry.position[2];
679-
linear_velocity[0] = _vehicle_visual_odometry.velocity[0];
680-
linear_velocity[1] = _vehicle_visual_odometry.velocity[1];
681-
linear_velocity[2] = _vehicle_visual_odometry.velocity[2];
682-
}
683-
}
684-
}
685-
}
526+
else{
527+
position[0] = _vehicle_local_position.x;
528+
position[1] = _vehicle_local_position.y;
529+
position[2] = _vehicle_local_position.z;
530+
linear_velocity[0] = _vehicle_local_position.vx;
531+
linear_velocity[1] = _vehicle_local_position.vy;
532+
linear_velocity[2] = _vehicle_local_position.vz;
686533
}
687534
// position and linear_velocity are guaranteed to be set after this point
688535

‎src/modules/mc_raptor/mc_raptor.hpp‎

Lines changed: 2 additions & 20 deletions
Original file line numberDiff line numberDiff line change
@@ -101,18 +101,10 @@ class Raptor : public ModuleBase<Raptor>, public ModuleParams, public px4::Sched
101101
// node constants
102102
static constexpr TI OBSERVATION_TIMEOUT_ANGULAR_VELOCITY = 10 * 1000;
103103
static constexpr TI OBSERVATION_TIMEOUT_LOCAL_POSITION = 100 * 1000;
104-
static constexpr TI OBSERVATION_TIMEOUT_VISUAL_ODOMETRY = 100 * 1000;
105104
static constexpr TI OBSERVATION_TIMEOUT_ATTITUDE = 50 * 1000;
106105
static constexpr TI TRAJECTORY_SETPOINT_TIMEOUT = 100 * 1000;
107106
static constexpr T RESET_PREVIOUS_ACTION_VALUE = 0; // -1 to 1
108107

109-
enum class OdometrySource: TI{
110-
LOCAL_POSITION = 0,
111-
VISUAL_ODOMETRY = 1,
112-
};
113-
// static constexpr OdometrySource ODOMETRY_SOURCE = OdometrySource::VISUAL_ODOMETRY;
114-
static constexpr OdometrySource ODOMETRY_SOURCE = OdometrySource::LOCAL_POSITION;
115-
116108
T min(T a, T b) {
117109
return a < b ? a : b;
118110
}
@@ -134,30 +126,20 @@ class Raptor : public ModuleBase<Raptor>, public ModuleParams, public px4::Sched
134126

135127
// node state
136128
vehicle_local_position_s _vehicle_local_position{};
137-
vehicle_odometry_s _vehicle_visual_odometry{};
138129
vehicle_angular_velocity_s _vehicle_angular_velocity{};
139130
vehicle_attitude_s _vehicle_attitude{};
140131
vehicle_status_s _vehicle_status{};
141132
trajectory_setpoint_s _trajectory_setpoint{};
142-
hrt_abstime timestamp_last_local_position, timestamp_last_visual_odometry, timestamp_last_visual_odometry_stale, timestamp_last_angular_velocity, timestamp_last_attitude, timestamp_last_trajectory_setpoint, timestamp_last_manual_control_input, timestamp_last_vehicle_status;
143-
bool timestamp_last_local_position_set = false, timestamp_last_visual_odometry_set = false, timestamp_last_visual_odometry_stale_set = false, timestamp_last_angular_velocity_set = false, timestamp_last_attitude_set = false, timestamp_last_trajectory_setpoint_set = false, timestamp_last_manual_control_input_set = false, timestamp_last_vehicle_status_set = false;
133+
hrt_abstime timestamp_last_local_position, timestamp_last_angular_velocity, timestamp_last_attitude, timestamp_last_trajectory_setpoint, timestamp_last_manual_control_input, timestamp_last_vehicle_status;
134+
bool timestamp_last_local_position_set = false, timestamp_last_angular_velocity_set = false, timestamp_last_attitude_set = false, timestamp_last_trajectory_setpoint_set = false, timestamp_last_manual_control_input_set = false, timestamp_last_vehicle_status_set = false;
144135
bool timeout_message_sent = false;
145136
bool previous_trajectory_setpoint_stale = false;
146137
bool previous_active = false;
147138

148-
static constexpr TI NUM_ODOMETRY_DTS = 1000;
149-
TI odometry_dts[NUM_ODOMETRY_DTS];
150-
TI odometry_dt_index = 0;
151-
bool odometry_dts_full = false;
152-
TI visual_odometry_stale_counter = 0;
153-
154-
155139
T position[3];
156140
T linear_velocity[3];
157141

158-
// uORB::Subscription _rl_tools_command_sub{ORB_ID(rl_tools_command)};
159142
uORB::SubscriptionCallbackWorkItem _vehicle_local_position_sub{this, ORB_ID(vehicle_local_position)};
160-
uORB::SubscriptionCallbackWorkItem _vehicle_visual_odometry_sub{this, ORB_ID(vehicle_visual_odometry)};
161143
uORB::SubscriptionCallbackWorkItem _vehicle_angular_velocity_sub{this, ORB_ID(vehicle_angular_velocity)};
162144
uORB::SubscriptionCallbackWorkItem _vehicle_attitude_sub{this, ORB_ID(vehicle_attitude)};
163145
uORB::Subscription _register_ext_component_reply_sub{ORB_ID(register_ext_component_reply)};

0 commit comments

Comments
 (0)