redid actions to use a counter and not have race conditions
Before, there were a lot of ways for the client-side action code to lock
up.
diff --git a/frc971/actions/action.h b/frc971/actions/action.h
index 6fe29bb..528d36d 100644
--- a/frc971/actions/action.h
+++ b/frc971/actions/action.h
@@ -2,6 +2,7 @@
#define FRC971_ACTIONS_ACTION_H_
#include <stdio.h>
+#include <inttypes.h>
#include <functional>
@@ -28,7 +29,7 @@
action_q_->goal.FetchNextBlocking();
}
- if (!action_q_->status.MakeWithBuilder().running(false).Send()) {
+ if (!action_q_->status.MakeWithBuilder().running(0).Send()) {
LOG(ERROR, "Failed to send the status.\n");
}
while (true) {
@@ -36,22 +37,30 @@
LOG(INFO, "Waiting for an action request.\n");
action_q_->goal.FetchNextBlocking();
if (!action_q_->goal->run) {
- if (!action_q_->status.MakeWithBuilder().running(false).Send()) {
+ if (!action_q_->status.MakeWithBuilder().running(0).Send()) {
LOG(ERROR, "Failed to send the status.\n");
}
}
}
- LOG(INFO, "Starting action\n");
- if (!action_q_->status.MakeWithBuilder().running(true).Send()) {
+
+ const uint32_t running_id = action_q_->goal->run;
+ LOG(INFO, "Starting action %" PRIx32 "\n", running_id);
+ if (!action_q_->status.MakeWithBuilder().running(running_id).Send()) {
LOG(ERROR, "Failed to send the status.\n");
}
RunAction();
- LOG(INFO, "Done with action\n");
- if (!action_q_->status.MakeWithBuilder().running(false).Send()) {
- LOG(ERROR, "Failed to send the status.\n");
+ LOG(INFO, "Done with action %" PRIx32 "\n", running_id);
+
+ // If we have a new one to run, we shouldn't say we're stopped in between.
+ if (action_q_->goal->run == 0) {
+ if (!action_q_->status.MakeWithBuilder().running(0).Send()) {
+ LOG(ERROR, "Failed to send the status.\n");
+ }
}
- while (action_q_->goal->run) {
- LOG(INFO, "Waiting for the action to be stopped.\n");
+
+ while (action_q_->goal->run == running_id) {
+ LOG(INFO, "Waiting for the action (%" PRIx32 ") to be stopped.\n",
+ running_id);
action_q_->goal.FetchNextBlocking();
}
}
diff --git a/frc971/actions/action.q b/frc971/actions/action.q
new file mode 100644
index 0000000..a104b72
--- /dev/null
+++ b/frc971/actions/action.q
@@ -0,0 +1,24 @@
+package frc971.actions;
+
+interface StatusInterface {
+ // 0 if the action isn't running or the value from goal.run.
+ uint32_t running;
+};
+
+interface GoalInterface {
+ // 0 to stop or an arbitrary value to put in status.running.
+ uint32_t run;
+};
+
+message Status {
+ uint32_t running;
+};
+
+message Goal {
+ uint32_t run;
+};
+
+interface ActionQueueGroup {
+ queue Status status;
+ queue Goal goal;
+};
diff --git a/frc971/actions/action_client.h b/frc971/actions/action_client.h
index 4b3e048..20b8ec7 100644
--- a/frc971/actions/action_client.h
+++ b/frc971/actions/action_client.h
@@ -1,7 +1,12 @@
#ifndef FRC971_ACTIONS_ACTION_CLIENT_H_
#define FRC971_ACTIONS_ACTION_CLIENT_H_
+#include <inttypes.h>
+#include <sys/types.h>
+#include <unistd.h>
+
#include <type_traits>
+#include <atomic>
#include "aos/common/logging/logging.h"
#include "aos/common/queue.h"
@@ -40,81 +45,149 @@
TypedAction(T *queue_group)
: queue_group_(queue_group),
goal_(queue_group_->goal.MakeMessage()),
- has_started_(false) {}
+ run_value_(run_counter.fetch_add(1, ::std::memory_order_relaxed) |
+ ((getpid() & 0xFFFF) << 16)) {
+ LOG(INFO, "Action %" PRIx32 " created on queue %s\n", run_value_,
+ queue_group_->goal.name());
+ }
// Returns the current goal that will be sent when the action is sent.
GoalType *GetGoal() { return goal_.get(); }
virtual ~TypedAction() {
- LOG(INFO, "Calling destructor\n");
+ LOG(DEBUG, "Calling destructor of %" PRIx32"\n", run_value_);
DoCancel();
}
private:
// Cancels the action.
virtual void DoCancel() {
- LOG(INFO, "Canceling action on queue %s\n", queue_group_->goal.name());
- queue_group_->goal.MakeWithBuilder().run(false).Send();
+ if (!sent_started_) {
+ LOG(INFO, "action %" PRIx32 " was never started\n", run_value_);
+ } else {
+ if (interrupted_) {
+ LOG(INFO, "action %" PRIx32 " was interrupted -> not cancelling\n",
+ run_value_);
+ } else {
+ LOG(INFO, "Canceling action %" PRIx32 " on queue %s\n", run_value_,
+ queue_group_->goal.name());
+ queue_group_->goal.MakeWithBuilder().run(0).Send();
+ }
+ }
}
// Returns true if the action is running or we don't have an initial response
// back from it to signal whether or not it is running.
virtual bool DoRunning() {
+ if (!sent_started_) return false;
if (has_started_) {
queue_group_->status.FetchLatest();
+ CheckInterrupted();
} else if (queue_group_->status.FetchLatest()) {
- if (queue_group_->status->running) {
- // Wait until it reports that it is running to start.
- has_started_ = true;
- }
+ CheckStarted();
}
- return !has_started_ ||
- (queue_group_->status.get() && queue_group_->status->running);
+ if (interrupted_) return false;
+ if (!has_started_) return true;
+ return queue_group_->status.get() &&
+ queue_group_->status->running == run_value_;
}
- // Returns true if the action is running or we don't have an initial response
- // back from it to signal whether or not it is running.
virtual void DoWaitUntilDone() {
+ assert(sent_started_);
queue_group_->status.FetchLatest();
+ CheckInterrupted();
while (true) {
- if (has_started_) {
- queue_group_->status.FetchNextBlocking();
- } else {
- if (queue_group_->status->running) {
- has_started_ = true;
- }
- queue_group_->status.FetchNextBlocking();
- if (queue_group_->status->running) {
- // Wait until it reports that it is running to start.
- has_started_ = true;
- }
- }
- if (has_started_ &&
- (queue_group_->status.get() && !queue_group_->status->running)) {
+ if (interrupted_) return;
+ CheckStarted();
+ queue_group_->status.FetchNextBlocking();
+ CheckStarted();
+ CheckInterrupted();
+ if (has_started_ && (queue_group_->status.get() &&
+ queue_group_->status->running != run_value_)) {
return;
}
}
}
+ void CheckStarted() {
+ if (has_started_) return;
+ if (queue_group_->status.get()) {
+ if (queue_group_->status->running == run_value_) {
+ // It's currently running our instance.
+ has_started_ = true;
+ LOG(DEBUG, "action %" PRIx32 " has been started\n", run_value_);
+ } else if (queue_group_->status->running == old_run_value_) {
+ // It's still running an old instance (or still doing nothing).
+ } else {
+ LOG(WARNING,
+ "action %" PRIx32 " interrupted by %" PRIx32 " before starting\n",
+ run_value_, queue_group_->status->running);
+ has_started_ = true;
+ interrupted_ = true;
+ }
+ }
+ }
+
+ void CheckInterrupted() {
+ if (!interrupted_ && has_started_ && queue_group_->status.get()) {
+ if (queue_group_->status->running != 0 &&
+ queue_group_->status->running != run_value_) {
+ LOG(WARNING,
+ "action %" PRIx32 " interrupted by %" PRIx32 " after starting\n",
+ run_value_, queue_group_->status->running);
+ }
+ }
+ }
+
// Starts the action if a goal has been created.
virtual void DoStart() {
if (goal_) {
- goal_->run = true;
- goal_.Send();
- has_started_ = false;
- LOG(INFO, "Starting action\n");
+ LOG(INFO, "Starting action %" PRIx32 "\n", run_value_);
+ goal_->run = run_value_;
+ sent_started_ = true;
+ if (!goal_.Send()) {
+ LOG(ERROR, "sending goal for action %" PRIx32 " failed\n", run_value_);
+ // Don't wait to see a message with it.
+ has_started_ = true;
+ }
+ queue_group_->status.FetchLatest();
+ if (queue_group_->status.get()) {
+ old_run_value_ = queue_group_->status->running;
+ LOG(INFO, "action %" PRIx32 " already running\n", old_run_value_);
+ } else {
+ old_run_value_ = 0;
+ }
} else {
- has_started_ = true;
+ LOG(WARNING, "action %" PRIx32 " already started\n", run_value_);
}
}
- T *queue_group_;
+ T *const queue_group_;
::aos::ScopedMessagePtr<GoalType> goal_;
+
// Track if we have seen a response to the start message.
- // If we haven't, we are considered running regardless.
- bool has_started_;
+ bool has_started_ = false;
+ // Track if we have sent an initial start message.
+ bool sent_started_ = false;
+
+ // Gets set to true if we ever see somebody else's value in running.
+ bool interrupted_ = false;
+
+ // The value we're going to use for goal.run etc.
+ const uint32_t run_value_;
+
+ // The old value for running that we may have seen. If we see any value other
+ // than this or run_value_, somebody else got in the way and we're done. 0 if
+ // there was nothing there to start with. Only valid after sent_started_
+ // changes to true.
+ uint32_t old_run_value_;
+
+ static ::std::atomic<uint16_t> run_counter;
};
+template <typename T>
+::std::atomic<uint16_t> TypedAction<T>::run_counter{0};
+
} // namespace frc971
#endif // FRC971_ACTIONS_ACTION_CLIENT_H_
diff --git a/frc971/actions/actions.gyp b/frc971/actions/actions.gyp
index d2350c1..d2eb82f 100644
--- a/frc971/actions/actions.gyp
+++ b/frc971/actions/actions.gyp
@@ -16,6 +16,15 @@
],
},
{
+ 'target_name': 'action_queue',
+ 'type': 'static_library',
+ 'sources': ['action.q'],
+ 'variables': {
+ 'header_path': 'frc971/actions',
+ },
+ 'includes': ['../../aos/build/queues.gypi'],
+ },
+ {
'target_name': 'shoot_action_queue',
'type': 'static_library',
'sources': ['shoot_action.q'],
@@ -23,10 +32,10 @@
'header_path': 'frc971/actions',
},
'dependencies': [
- '<(AOS)/common/common.gyp:queues',
+ 'action_queue',
],
'export_dependent_settings': [
- '<(AOS)/common/common.gyp:queues',
+ 'action_queue',
],
'includes': ['../../aos/build/queues.gypi'],
},
@@ -38,14 +47,14 @@
'header_path': 'frc971/actions',
},
'dependencies': [
- '<(AOS)/common/common.gyp:queues',
+ 'action_queue',
],
'export_dependent_settings': [
- '<(AOS)/common/common.gyp:queues',
+ 'action_queue',
],
'includes': ['../../aos/build/queues.gypi'],
},
- {
+ {
'target_name': 'selfcatch_action_queue',
'type': 'static_library',
'sources': ['selfcatch_action.q'],
@@ -53,10 +62,10 @@
'header_path': 'frc971/actions',
},
'dependencies': [
- '<(AOS)/common/common.gyp:queues',
+ 'action_queue',
],
'export_dependent_settings': [
- '<(AOS)/common/common.gyp:queues',
+ 'action_queue',
],
'includes': ['../../aos/build/queues.gypi'],
},
@@ -68,10 +77,10 @@
'header_path': 'frc971/actions',
},
'dependencies': [
- '<(AOS)/common/common.gyp:queues',
+ 'action_queue',
],
'export_dependent_settings': [
- '<(AOS)/common/common.gyp:queues',
+ 'action_queue',
],
'includes': ['../../aos/build/queues.gypi'],
},
@@ -184,7 +193,7 @@
'<(AOS)/linux_code/linux_code.gyp:init',
'shoot_action_queue',
'shoot_action_lib',
- 'action',
+ 'action',
],
},
{
@@ -210,7 +219,7 @@
'<(AOS)/linux_code/linux_code.gyp:init',
'selfcatch_action_queue',
'selfcatch_action_lib',
- 'action',
+ 'action',
],
},
{
@@ -223,7 +232,7 @@
'<(AOS)/linux_code/linux_code.gyp:init',
'catch_action_queue',
'catch_action_lib',
- 'action',
+ 'action',
],
},
],
diff --git a/frc971/actions/catch_action.q b/frc971/actions/catch_action.q
index 33b21f0..73d21ed 100644
--- a/frc971/actions/catch_action.q
+++ b/frc971/actions/catch_action.q
@@ -1,19 +1,17 @@
package frc971.actions;
+import "frc971/actions/action.q";
+
queue_group CatchActionGroup {
- message Status {
- bool running;
- };
+ implements frc971.actions.ActionQueueGroup;
message Goal {
- // If true, run this action. If false, cancel the action if it is
- // currently running.
- bool run;
+ uint32_t run;
double catch_angle;
};
queue Goal goal;
- queue Status status;
+ queue frc971.actions.Status status;
};
queue_group CatchActionGroup catch_action;
diff --git a/frc971/actions/drivetrain_action.q b/frc971/actions/drivetrain_action.q
index cde518c..5797378 100644
--- a/frc971/actions/drivetrain_action.q
+++ b/frc971/actions/drivetrain_action.q
@@ -1,9 +1,9 @@
package frc971.actions;
+import "frc971/actions/action.q";
+
queue_group DrivetrainActionQueueGroup {
- message Status {
- bool running;
- };
+ implements frc971.actions.ActionQueueGroup;
message Goal {
// If true, run this action. If false, cancel the action if it is
@@ -16,7 +16,7 @@
};
queue Goal goal;
- queue Status status;
+ queue frc971.actions.Status status;
};
queue_group DrivetrainActionQueueGroup drivetrain_action;
diff --git a/frc971/actions/selfcatch_action.q b/frc971/actions/selfcatch_action.q
index 9ba21ad..bd8ab55 100644
--- a/frc971/actions/selfcatch_action.q
+++ b/frc971/actions/selfcatch_action.q
@@ -1,19 +1,17 @@
package frc971.actions;
+import "frc971/actions/action.q";
+
queue_group SelfCatchActionGroup {
- message Status {
- bool running;
- };
+ implements frc971.actions.ActionQueueGroup;
message Goal {
- // If true, run this action. If false, cancel the action if it is
- // currently running.
- bool run;
+ uint32_t run;
double shot_angle;
};
queue Goal goal;
- queue Status status;
+ queue frc971.actions.Status status;
};
queue_group SelfCatchActionGroup selfcatch_action;
diff --git a/frc971/actions/shoot_action.q b/frc971/actions/shoot_action.q
index 081d627..4552c64 100644
--- a/frc971/actions/shoot_action.q
+++ b/frc971/actions/shoot_action.q
@@ -1,18 +1,12 @@
package frc971.actions;
+import "frc971/actions/action.q";
+
queue_group ShootActionQueueGroup {
- message Status {
- bool running;
- };
+ implements frc971.actions.ActionQueueGroup;
- message Goal {
- // If true, run this action. If false, cancel the action if it is
- // currently running.
- bool run;
- };
-
- queue Goal goal;
- queue Status status;
+ queue frc971.actions.Goal goal;
+ queue frc971.actions.Status status;
};
queue_group ShootActionQueueGroup shoot_action;
diff --git a/frc971/autonomous/auto.cc b/frc971/autonomous/auto.cc
index 46f4fc3..80ffa17 100644
--- a/frc971/autonomous/auto.cc
+++ b/frc971/autonomous/auto.cc
@@ -177,7 +177,8 @@
void InitializeEncoders() {
control_loops::drivetrain.position.FetchLatest();
while (!control_loops::drivetrain.position.get()) {
- LOG(WARNING, "No previous drivetrain position packet, trying to fetch again\n");
+ LOG(WARNING,
+ "No previous drivetrain position packet, trying to fetch again\n");
control_loops::drivetrain.position.FetchNextBlocking();
}
left_initial_position =
diff --git a/frc971/input/joystick_reader.cc b/frc971/input/joystick_reader.cc
index ecb93f2..ddb03fe 100644
--- a/frc971/input/joystick_reader.cc
+++ b/frc971/input/joystick_reader.cc
@@ -183,7 +183,7 @@
// Returns true if any action is running or could be running.
// For a one cycle faster response, call Tick before running this.
- bool Running() { return (bool)current_action_; }
+ bool Running() { return static_cast<bool>(current_action_); }
private:
::std::unique_ptr<Action> current_action_;