Merge "Added Tests for moving out of collisions"
diff --git a/.bazelrc b/.bazelrc
index c80339f..57f01ea 100644
--- a/.bazelrc
+++ b/.bazelrc
@@ -50,6 +50,10 @@
build:msan --test_env MSAN_SYMBOLIZER_PATH=/usr/bin/llvm-symbolizer-3.6
build:msan --define have_msan=true
+# Sometime, we want to be able to have eigen assertions run so that we can
+# catch potential issues (e.g., accessing invalid indices).
+build:eigen --copt -UNDEBUG
+
# Show paths to a few more than just 1 target.
build --show_result 5
# Dump the output of the failing test to stdout.
diff --git a/aos/events/event-loop.h b/aos/events/event-loop.h
index a187981..0590d7b 100644
--- a/aos/events/event-loop.h
+++ b/aos/events/event-loop.h
@@ -59,11 +59,13 @@
const T &operator*() const { return *get(); }
const T *operator->() const { return get(); }
- // Sends the message to the queue. Should only be called once.
- void Send() {
+ // Sends the message to the queue. Should only be called once. Returns true
+ // if the message was successfully sent, and false otherwise.
+ bool Send() {
RawSender *sender = &msg_.get_deleter();
msg_->SetTimeToNow();
- sender->Send(reinterpret_cast<RawSender::SendContext *>(msg_.release()));
+ return sender->Send(
+ reinterpret_cast<RawSender::SendContext *>(msg_.release()));
}
private:
@@ -91,6 +93,10 @@
// Current time.
virtual monotonic_clock::time_point monotonic_now() = 0;
+ // Note, it is supported to create:
+ // multiple fetchers, and (one sender or one watcher) per <path, type>
+ // tuple.
+
// Makes a class that will always fetch the most recent value
// sent to path.
template <typename T>
diff --git a/aos/events/event-loop_param_test.cc b/aos/events/event-loop_param_test.cc
index 78e744a..efac8f9 100644
--- a/aos/events/event-loop_param_test.cc
+++ b/aos/events/event-loop_param_test.cc
@@ -55,44 +55,54 @@
EXPECT_TRUE(happened);
}
-// Verify that making a fetcher and handler for "/test" dies.
-TEST_P(AbstractEventLoopTest, FetcherAndHandler) {
+// Verify that making a fetcher and watcher for "/test" succeeds.
+TEST_P(AbstractEventLoopTest, FetcherAndWatcher) {
auto loop = Make();
auto fetcher = loop->MakeFetcher<TestMessage>("/test");
- EXPECT_DEATH(loop->MakeWatcher("/test", [&](const TestMessage &) {}), "/test");
+ loop->MakeWatcher("/test", [&](const TestMessage &) {});
}
-// Verify that making 2 fetchers for "/test" fails.
+// Verify that making 2 fetchers for "/test" succeeds.
TEST_P(AbstractEventLoopTest, TwoFetcher) {
auto loop = Make();
auto fetcher = loop->MakeFetcher<TestMessage>("/test");
- EXPECT_DEATH(loop->MakeFetcher<TestMessage>("/test"), "/test");
+ auto fetcher2 = loop->MakeFetcher<TestMessage>("/test");
}
-// Verify that registering a handler twice for "/test" fails.
-TEST_P(AbstractEventLoopTest, TwoHandler) {
+// Verify that registering a watcher twice for "/test" fails.
+TEST_P(AbstractEventLoopTest, TwoWatcher) {
auto loop = Make();
loop->MakeWatcher("/test", [&](const TestMessage &) {});
- EXPECT_DEATH(loop->MakeWatcher("/test", [&](const TestMessage &) {}), "/test");
+ EXPECT_DEATH(loop->MakeWatcher("/test", [&](const TestMessage &) {}),
+ "/test");
+}
+
+// Verify that registering a watcher and a sender for "/test" fails.
+TEST_P(AbstractEventLoopTest, WatcherAndSender) {
+ auto loop = Make();
+ auto sender = loop->MakeSender<TestMessage>("/test");
+ EXPECT_DEATH(loop->MakeWatcher("/test", [&](const TestMessage &) {}),
+ "/test");
}
// Verify that Quit() works when there are multiple watchers.
-TEST_P(AbstractEventLoopTest, MultipleFetcherQuit) {
- auto loop = Make();
+TEST_P(AbstractEventLoopTest, MultipleWatcherQuit) {
+ auto loop1 = Make();
+ auto loop2 = Make();
- auto sender = loop->MakeSender<TestMessage>("/test2");
+ auto sender = loop1->MakeSender<TestMessage>("/test2");
{
auto msg = sender.MakeMessage();
msg->msg_value = 200;
msg.Send();
}
- loop->MakeWatcher("/test1", [&](const TestMessage &) {});
- loop->MakeWatcher("/test2", [&](const TestMessage &message) {
+ loop2->MakeWatcher("/test1", [&](const TestMessage &) {});
+ loop2->MakeWatcher("/test2", [&](const TestMessage &message) {
EXPECT_EQ(message.msg_value, 200);
- loop->Exit();
+ loop2->Exit();
});
- loop->Run();
+ loop2->Run();
}
// Verify that timer intervals and duration function properly.
diff --git a/aos/events/shm-event-loop.cc b/aos/events/shm-event-loop.cc
index 945ebc0..781c963 100644
--- a/aos/events/shm-event-loop.cc
+++ b/aos/events/shm-event-loop.cc
@@ -1,12 +1,14 @@
#include "aos/events/shm-event-loop.h"
-#include "aos/logging/logging.h"
-#include "aos/queue.h"
#include <sys/timerfd.h>
+#include <algorithm>
#include <atomic>
#include <chrono>
#include <stdexcept>
+#include "aos/logging/logging.h"
+#include "aos/queue.h"
+
namespace aos {
ShmEventLoop::ShmEventLoop() : thread_state_(std::make_shared<ThreadState>()) {}
@@ -173,13 +175,13 @@
std::unique_ptr<RawFetcher> ShmEventLoop::MakeRawFetcher(
const std::string &path, const QueueTypeInfo &type) {
- Take(path);
return std::unique_ptr<RawFetcher>(new ShmFetcher(
RawQueue::Fetch(path.c_str(), type.size, type.hash, type.queue_length)));
}
std::unique_ptr<RawSender> ShmEventLoop::MakeRawSender(
const std::string &path, const QueueTypeInfo &type) {
+ Take(path);
return std::unique_ptr<RawSender>(new ShmSender(
RawQueue::Fetch(path.c_str(), type.size, type.hash, type.queue_length)));
}
@@ -269,8 +271,12 @@
if (is_running()) {
::aos::Die("Cannot add new objects while running.\n");
}
- if (!taken_.emplace(path).second) {
- ::aos::Die("%s already has a listener / watcher.", path.c_str());
+
+ const auto prior = ::std::find(taken_.begin(), taken_.end(), path);
+ if (prior != taken_.end()) {
+ ::aos::Die("%s is already being used.", path.c_str());
+ } else {
+ taken_.emplace_back(path);
}
}
diff --git a/aos/events/shm-event-loop.h b/aos/events/shm-event-loop.h
index 49d65f7..e8ff267 100644
--- a/aos/events/shm-event-loop.h
+++ b/aos/events/shm-event-loop.h
@@ -17,6 +17,10 @@
// Specialization of EventLoop that is built from queues running out of shared
// memory. See more details at aos/queue.h
+//
+// This object must be interacted with from one thread, but the Senders and
+// Fetchers may be used from multiple threads afterwords (as long as their
+// destructors are called back in one thread again)
class ShmEventLoop : public EventLoop {
public:
ShmEventLoop();
@@ -73,12 +77,13 @@
};
// Exclude multiple of the same type for path.
- void Take(const std::string &path);
std::vector<std::function<void()>> on_run_;
std::shared_ptr<ThreadState> thread_state_;
- std::unordered_set<std::string> taken_;
+ void Take(const std::string &path);
+
+ std::vector<::std::string> taken_;
};
} // namespace aos
diff --git a/aos/events/simulated-event-loop.cc b/aos/events/simulated-event-loop.cc
index e82c381..a23b18c 100644
--- a/aos/events/simulated-event-loop.cc
+++ b/aos/events/simulated-event-loop.cc
@@ -1,4 +1,7 @@
#include "aos/events/simulated-event-loop.h"
+
+#include <algorithm>
+
#include "aos/queue.h"
namespace aos {
@@ -80,13 +83,13 @@
std::unique_ptr<RawSender> SimulatedEventLoop::MakeRawSender(
const std::string &path, const QueueTypeInfo &type) {
+ Take(path);
::std::pair<::std::string, QueueTypeInfo> key(path, type);
return GetSimulatedQueue(key)->MakeRawSender();
}
std::unique_ptr<RawFetcher> SimulatedEventLoop::MakeRawFetcher(
const std::string &path, const QueueTypeInfo &type) {
- Take(path);
::std::pair<::std::string, QueueTypeInfo> key(path, type);
return GetSimulatedQueue(key)->MakeRawFetcher();
}
@@ -116,12 +119,15 @@
return std::unique_ptr<RawFetcher>(new SimulatedFetcher(this));
}
-void SimulatedEventLoop::Take(const std::string &path) {
+void SimulatedEventLoop::Take(const ::std::string &path) {
if (is_running()) {
::aos::Die("Cannot add new objects while running.\n");
}
- if (!taken_.emplace(path).second) {
- ::aos::Die("%s already has a listener / watcher.", path.c_str());
+ const auto prior = ::std::find(taken_.begin(), taken_.end(), path);
+ if (prior != taken_.end()) {
+ ::aos::Die("%s is already being used.", path.c_str());
+ } else {
+ taken_.emplace_back(path);
}
}
} // namespace aos
diff --git a/aos/events/simulated-event-loop.h b/aos/events/simulated-event-loop.h
index 79b48b8..b5c94bf 100644
--- a/aos/events/simulated-event-loop.h
+++ b/aos/events/simulated-event-loop.h
@@ -212,8 +212,7 @@
std::function<void(const aos::Message *message)> watcher) override;
TimerHandler *AddTimer(::std::function<void()> callback) override {
- timers_.emplace_back(
- new SimulatedTimerHandler(scheduler_, callback));
+ timers_.emplace_back(new SimulatedTimerHandler(scheduler_, callback));
return timers_.back().get();
}
@@ -232,19 +231,20 @@
SimulatedQueue *GetSimulatedQueue(
const ::std::pair<::std::string, QueueTypeInfo> &);
- void Take(const std::string &path);
+ void Take(const ::std::string &path);
private:
EventScheduler *scheduler_;
- std::map<::std::pair<::std::string, QueueTypeInfo>, SimulatedQueue> *queues_;
- std::unordered_set<std::string> taken_;
- std::vector<std::unique_ptr<TimerHandler>> timers_;
+ ::std::map<::std::pair<::std::string, QueueTypeInfo>, SimulatedQueue>
+ *queues_;
+ ::std::vector<std::string> taken_;
+ ::std::vector<std::unique_ptr<TimerHandler>> timers_;
};
class SimulatedEventLoopFactory {
public:
- std::unique_ptr<EventLoop> CreateEventLoop() {
- return std::unique_ptr<EventLoop>(
+ ::std::unique_ptr<EventLoop> CreateEventLoop() {
+ return ::std::unique_ptr<EventLoop>(
new SimulatedEventLoop(&scheduler_, &queues_));
}
diff --git a/frc971/control_loops/drivetrain/drivetrain.cc b/frc971/control_loops/drivetrain/drivetrain.cc
index a06bc3b..a50b429 100644
--- a/frc971/control_loops/drivetrain/drivetrain.cc
+++ b/frc971/control_loops/drivetrain/drivetrain.cc
@@ -207,7 +207,7 @@
}
LOG(DEBUG,
- "New IMU value from ADIS16448, rate is %f, angle %f, fused %f, bias "
+ "New IMU value, rate is %f, angle %f, fused %f, bias "
"%f\n",
rate, angle, down_estimator_.X_hat(0), down_estimator_.X_hat(1));
down_U_(0, 0) = rate;
diff --git a/frc971/control_loops/profiled_subsystem.q b/frc971/control_loops/profiled_subsystem.q
index 784b92f..f3015c6 100644
--- a/frc971/control_loops/profiled_subsystem.q
+++ b/frc971/control_loops/profiled_subsystem.q
@@ -78,7 +78,7 @@
.frc971.HallEffectAndPositionEstimatorState estimator_state;
};
-struct AbsoluteProfiledJointStatus {
+struct PotAndAbsoluteEncoderProfiledJointStatus {
// Is the subsystem zeroed?
bool zeroed;
diff --git a/frc971/control_loops/static_zeroing_single_dof_profiled_subsystem.h b/frc971/control_loops/static_zeroing_single_dof_profiled_subsystem.h
index 9ad7439..2709315 100644
--- a/frc971/control_loops/static_zeroing_single_dof_profiled_subsystem.h
+++ b/frc971/control_loops/static_zeroing_single_dof_profiled_subsystem.h
@@ -65,7 +65,8 @@
void Iterate(const StaticZeroingSingleDOFProfiledSubsystemGoal *goal,
const typename ZeroingEstimator::Position *position,
double *output,
- ::frc971::control_loops::AbsoluteProfiledJointStatus *status);
+ ::frc971::control_loops::PotAndAbsoluteEncoderProfiledJointStatus
+ *status);
// Resets the profiled subsystem and returns to uninitialized
void Reset();
@@ -119,7 +120,7 @@
void StaticZeroingSingleDOFProfiledSubsystem<ZeroingEstimator>::Iterate(
const StaticZeroingSingleDOFProfiledSubsystemGoal *goal,
const typename ZeroingEstimator::Position *position, double *output,
- ::frc971::control_loops::AbsoluteProfiledJointStatus *status) {
+ ::frc971::control_loops::PotAndAbsoluteEncoderProfiledJointStatus *status) {
bool disabled = output == nullptr;
profiled_subsystem_.Correct(*position);
diff --git a/frc971/control_loops/static_zeroing_single_dof_profiled_subsystem_test.cc b/frc971/control_loops/static_zeroing_single_dof_profiled_subsystem_test.cc
index 8805842..df26c56 100644
--- a/frc971/control_loops/static_zeroing_single_dof_profiled_subsystem_test.cc
+++ b/frc971/control_loops/static_zeroing_single_dof_profiled_subsystem_test.cc
@@ -62,7 +62,7 @@
struct TestIntakeSystemData {
::frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystemGoal goal;
- ::frc971::control_loops::AbsoluteProfiledJointStatus status;
+ ::frc971::control_loops::PotAndAbsoluteEncoderProfiledJointStatus status;
::frc971::PotAndAbsolutePosition position;
diff --git a/tools/ci/run-tests.sh b/tools/ci/run-tests.sh
index e0e7bb6..91fbb2d 100755
--- a/tools/ci/run-tests.sh
+++ b/tools/ci/run-tests.sh
@@ -4,7 +4,9 @@
TARGETS='//... @com_github_google_glog//... @com_google_ceres_solver//...'
-bazel test -c opt --curses=no --color=no ${TARGETS}
+# Include --config=eigen to enable Eigen assertions so that we catch potential
+# bugs with Eigen.
+bazel test -c opt --config=eigen --curses=no --color=no ${TARGETS}
bazel build -c opt --curses=no --color=no ${TARGETS} --cpu=roborio
bazel build --curses=no --color=no ${TARGETS} --cpu=armhf-debian
bazel build -c opt --curses=no --color=no //motors/... --cpu=cortex-m4f
diff --git a/y2017/control_loops/superstructure/intake/intake.cc b/y2017/control_loops/superstructure/intake/intake.cc
index 89145a4..76c44a4 100644
--- a/y2017/control_loops/superstructure/intake/intake.cc
+++ b/y2017/control_loops/superstructure/intake/intake.cc
@@ -32,7 +32,7 @@
void Intake::Iterate(
const control_loops::IntakeGoal *unsafe_goal,
const ::frc971::PotAndAbsolutePosition *position, double *output,
- ::frc971::control_loops::AbsoluteProfiledJointStatus *status) {
+ ::frc971::control_loops::PotAndAbsoluteEncoderProfiledJointStatus *status) {
bool disable = output == nullptr;
profiled_subsystem_.Correct(*position);
diff --git a/y2017/control_loops/superstructure/intake/intake.h b/y2017/control_loops/superstructure/intake/intake.h
index 40f49fc..db1442a 100644
--- a/y2017/control_loops/superstructure/intake/intake.h
+++ b/y2017/control_loops/superstructure/intake/intake.h
@@ -34,7 +34,8 @@
void Iterate(const control_loops::IntakeGoal *unsafe_goal,
const ::frc971::PotAndAbsolutePosition *position, double *output,
- ::frc971::control_loops::AbsoluteProfiledJointStatus *status);
+ ::frc971::control_loops::PotAndAbsoluteEncoderProfiledJointStatus
+ *status);
void Reset();
diff --git a/y2017/control_loops/superstructure/superstructure.q b/y2017/control_loops/superstructure/superstructure.q
index 60c5e16..5c0771c 100644
--- a/y2017/control_loops/superstructure/superstructure.q
+++ b/y2017/control_loops/superstructure/superstructure.q
@@ -200,7 +200,7 @@
bool estopped;
// Each subsystems status.
- .frc971.control_loops.AbsoluteProfiledJointStatus intake;
+ .frc971.control_loops.PotAndAbsoluteEncoderProfiledJointStatus intake;
.frc971.control_loops.IndexProfiledJointStatus hood;
ShooterStatus shooter;
diff --git a/y2018/control_loops/superstructure/BUILD b/y2018/control_loops/superstructure/BUILD
index efe0c1d..a646ab6 100644
--- a/y2018/control_loops/superstructure/BUILD
+++ b/y2018/control_loops/superstructure/BUILD
@@ -36,6 +36,7 @@
cc_test(
name = "superstructure_lib_test",
+ timeout = "long",
srcs = [
"superstructure_lib_test.cc",
],
@@ -44,9 +45,9 @@
":superstructure_queue",
"//aos:math",
"//aos:queues",
- "//aos/time:time",
"//aos/controls:control_loop_test",
"//aos/testing:googletest",
+ "//aos/time",
"//frc971/control_loops:position_sensor_sim",
"//frc971/control_loops:team_number_test_environment",
"//y2018/control_loops/superstructure/intake:intake_plants",
@@ -67,12 +68,12 @@
cc_library(
name = "debouncer",
- hdrs = [
- "debouncer.h",
- ],
srcs = [
"debouncer.cc",
],
+ hdrs = [
+ "debouncer.h",
+ ],
)
cc_test(
diff --git a/y2019/control_loops/superstructure/superstructure.q b/y2019/control_loops/superstructure/superstructure.q
index 2cd7238..fef9ced 100644
--- a/y2019/control_loops/superstructure/superstructure.q
+++ b/y2019/control_loops/superstructure/superstructure.q
@@ -67,10 +67,10 @@
bool has_piece;
// Status of each subsystem.
- .frc971.control_loops.AbsoluteProfiledJointStatus elevator;
- .frc971.control_loops.AbsoluteProfiledJointStatus wrist;
- .frc971.control_loops.AbsoluteProfiledJointStatus intake;
- .frc971.control_loops.AbsoluteProfiledJointStatus stilts;
+ .frc971.control_loops.PotAndAbsoluteEncoderProfiledJointStatus elevator;
+ .frc971.control_loops.PotAndAbsoluteEncoderProfiledJointStatus wrist;
+ .frc971.control_loops.PotAndAbsoluteEncoderProfiledJointStatus intake;
+ .frc971.control_loops.PotAndAbsoluteEncoderProfiledJointStatus stilts;
};
message Position {