Galactic Search Path Fetcher Loading and Executing Splines
Change-Id: Icdc3d76d269d8a1545dbd80b965f2a67fa9cb358
diff --git a/y2020/actors/BUILD b/y2020/actors/BUILD
index 434b8db..aacf77a 100644
--- a/y2020/actors/BUILD
+++ b/y2020/actors/BUILD
@@ -59,6 +59,8 @@
"//y2020/control_loops/drivetrain:drivetrain_base",
"//y2020/control_loops/superstructure:superstructure_goal_fbs",
"//y2020/control_loops/superstructure:superstructure_status_fbs",
+ "//y2020/vision:galactic_search_path_fbs",
+ "//frc971/control_loops/drivetrain:spline",
],
)
diff --git a/y2020/actors/auto_splines.h b/y2020/actors/auto_splines.h
index 5ea1748..4690fb8 100644
--- a/y2020/actors/auto_splines.h
+++ b/y2020/actors/auto_splines.h
@@ -20,7 +20,15 @@
public:
AutonomousSplines()
: test_spline_(aos::JsonFileToFlatbuffer<frc971::MultiSpline>(
- "splines/test_spline.json")) {}
+ "splines/test_spline.json")),
+ spline_red_a_(aos::JsonFileToFlatbuffer<frc971::MultiSpline>(
+ "splines/spline_red_a.json")),
+ spline_blue_a_(aos::JsonFileToFlatbuffer<frc971::MultiSpline>(
+ "splines/spline_blue_a.json")),
+ spline_red_b_(aos::JsonFileToFlatbuffer<frc971::MultiSpline>(
+ "splines/spline_red_b.json")),
+ spline_blue_b_(aos::JsonFileToFlatbuffer<frc971::MultiSpline>(
+ "splines/spline_blue_b.json")) {}
static flatbuffers::Offset<frc971::MultiSpline> BasicSSpline(
aos::Sender<frc971::control_loops::drivetrain::Goal>::Builder *builder,
@@ -33,9 +41,33 @@
return aos::CopyFlatBuffer<frc971::MultiSpline>(test_spline_,
builder->fbb());
}
+ flatbuffers::Offset<frc971::MultiSpline> SplineRedA(
+ aos::Sender<frc971::control_loops::drivetrain::Goal>::Builder *builder) {
+ return aos::CopyFlatBuffer<frc971::MultiSpline>(spline_red_a_,
+ builder->fbb());
+ }
+ flatbuffers::Offset<frc971::MultiSpline> SplineBlueA(
+ aos::Sender<frc971::control_loops::drivetrain::Goal>::Builder *builder) {
+ return aos::CopyFlatBuffer<frc971::MultiSpline>(spline_blue_a_,
+ builder->fbb());
+ }
+ flatbuffers::Offset<frc971::MultiSpline> SplineRedB(
+ aos::Sender<frc971::control_loops::drivetrain::Goal>::Builder *builder) {
+ return aos::CopyFlatBuffer<frc971::MultiSpline>(spline_red_b_,
+ builder->fbb());
+ }
+ flatbuffers::Offset<frc971::MultiSpline> SplineBlueB(
+ aos::Sender<frc971::control_loops::drivetrain::Goal>::Builder *builder) {
+ return aos::CopyFlatBuffer<frc971::MultiSpline>(spline_blue_b_,
+ builder->fbb());
+ }
private:
aos::FlatbufferDetachedBuffer<frc971::MultiSpline> test_spline_;
+ aos::FlatbufferDetachedBuffer<frc971::MultiSpline> spline_red_a_;
+ aos::FlatbufferDetachedBuffer<frc971::MultiSpline> spline_blue_a_;
+ aos::FlatbufferDetachedBuffer<frc971::MultiSpline> spline_red_b_;
+ aos::FlatbufferDetachedBuffer<frc971::MultiSpline> spline_blue_b_;
};
} // namespace actors
diff --git a/y2020/actors/autonomous_actor.cc b/y2020/actors/autonomous_actor.cc
index 752ba95..4223332 100644
--- a/y2020/actors/autonomous_actor.cc
+++ b/y2020/actors/autonomous_actor.cc
@@ -8,10 +8,13 @@
#include "aos/logging/logging.h"
#include "aos/util/math.h"
#include "frc971/control_loops/drivetrain/localizer_generated.h"
+#include "frc971/control_loops/drivetrain/spline.h"
#include "y2020/actors/auto_splines.h"
#include "y2020/control_loops/drivetrain/drivetrain_base.h"
DEFINE_bool(spline_auto, true, "If true, define a spline autonomous mode");
+DEFINE_bool(galactic_search, false,
+ "If true, do the galactic search autonomous");
namespace y2020 {
namespace actors {
@@ -30,6 +33,8 @@
"/drivetrain")),
joystick_state_fetcher_(
event_loop->MakeFetcher<aos::JoystickState>("/aos")),
+ path_fetcher_(event_loop->MakeFetcher<y2020::vision::GalacticSearchPath>(
+ "/pi1/camera")),
auto_splines_() {
set_max_drivetrain_voltage(2.0);
}
@@ -42,31 +47,6 @@
CHECK(joystick_state_fetcher_.get() != nullptr)
<< "Expect at least one JoystickState message before running auto...";
alliance_ = joystick_state_fetcher_->alliance();
- // Set up the starting position for the blue alliance.
- // Currently just arbitrarily chosen for testing purposes, such that the
- // robot starts on the side of the field nearest where it would score
- // (although I do not know if this is actually a legal starting position).
- Eigen::Vector2d starting_position(0.0, 0.0);
- double starting_heading = 0.0;
- if (alliance_ == aos::Alliance::kRed) {
- starting_position *= -1.0;
- starting_heading = aos::math::NormalizeAngle(starting_heading + M_PI);
- }
- if (FLAGS_spline_auto) {
- // TODO(james): Resetting the localizer breaks the left/right statespace
- // controller. That is a bug, but we can fix that later by not resetting.
- auto builder = localizer_control_sender_.MakeBuilder();
-
- LocalizerControl::Builder localizer_control_builder =
- builder.MakeBuilder<LocalizerControl>();
- localizer_control_builder.add_x(starting_position.x());
- localizer_control_builder.add_y(starting_position.y());
- localizer_control_builder.add_theta(starting_heading);
- localizer_control_builder.add_theta_uncertainty(0.00001);
- if (!builder.Send(localizer_control_builder.Finish())) {
- AOS_LOG(ERROR, "Failed to reset localizer.\n");
- }
- }
}
bool AutonomousActor::RunAction(
@@ -77,7 +57,9 @@
AOS_LOG(INFO, "Aborting autonomous due to invalid alliance selection.");
return false;
}
- if (FLAGS_spline_auto) {
+ if (FLAGS_galactic_search) {
+ GalacticSearch();
+ } else if (FLAGS_spline_auto) {
SplineAuto();
} else {
return DriveFwd();
@@ -85,10 +67,79 @@
return true;
}
+void AutonomousActor::SendStartingPosition(double x, double y, double theta) {
+ // Set up the starting position for the blue alliance.
+ double starting_heading = theta;
+
+ // TODO(james): Resetting the localizer breaks the left/right statespace
+ // controller. That is a bug, but we can fix that later by not resetting.
+ auto builder = localizer_control_sender_.MakeBuilder();
+
+ LocalizerControl::Builder localizer_control_builder =
+ builder.MakeBuilder<LocalizerControl>();
+ localizer_control_builder.add_x(x);
+ localizer_control_builder.add_y(y);
+ localizer_control_builder.add_theta(starting_heading);
+ localizer_control_builder.add_theta_uncertainty(0.00001);
+ if (!builder.Send(localizer_control_builder.Finish())) {
+ AOS_LOG(ERROR, "Failed to reset localizer.\n");
+ }
+}
+
+void AutonomousActor::GalacticSearch() {
+ path_fetcher_.Fetch();
+ CHECK(path_fetcher_.get() != nullptr)
+ << "Expect at least one GalacticSearchPath message before running "
+ "auto...";
+ if (path_fetcher_->alliance() == y2020::vision::Alliance::kUnknown) {
+ AOS_LOG(ERROR, "The galactic search path is unknown, doing nothing.");
+ } else {
+ SplineHandle spline1 = PlanSpline(
+ [this](aos::Sender<frc971::control_loops::drivetrain::Goal>::Builder
+ *builder) {
+ flatbuffers::Offset<frc971::MultiSpline> target_spline;
+ if (path_fetcher_->alliance() == y2020::vision::Alliance::kRed) {
+ if (path_fetcher_->letter() == y2020::vision::Letter::kA) {
+ target_spline = auto_splines_.SplineRedA(builder);
+ } else {
+ CHECK(path_fetcher_->letter() == y2020::vision::Letter::kB);
+ target_spline = auto_splines_.SplineRedB(builder);
+ }
+ } else {
+ if (path_fetcher_->letter() == y2020::vision::Letter::kA) {
+ target_spline = auto_splines_.SplineBlueA(builder);
+ } else {
+ CHECK(path_fetcher_->letter() == y2020::vision::Letter::kB);
+ target_spline = auto_splines_.SplineBlueB(builder);
+ }
+ }
+ const frc971::MultiSpline *const spline =
+ flatbuffers::GetTemporaryPointer(*builder->fbb(), target_spline);
+
+ SendStartingPosition(CHECK_NOTNULL(spline));
+
+ return target_spline;
+ },
+ SplineDirection::kForward);
+
+ if (!spline1.WaitForPlan()) return;
+ spline1.Start();
+
+ if (!spline1.WaitForSplineDistanceRemaining(0.02)) return;
+ }
+}
+
void AutonomousActor::SplineAuto() {
SplineHandle spline1 = PlanSpline(
[this](aos::Sender<frc971::control_loops::drivetrain::Goal>::Builder
- *builder) { return auto_splines_.TestSpline(builder); },
+ *builder) {
+ flatbuffers::Offset<frc971::MultiSpline> target_spline;
+ target_spline = auto_splines_.TestSpline(builder);
+ const frc971::MultiSpline *const spline =
+ flatbuffers::GetTemporaryPointer(*builder->fbb(), target_spline);
+ SendStartingPosition(CHECK_NOTNULL(spline));
+ return target_spline;
+ },
SplineDirection::kForward);
if (!spline1.WaitForPlan()) return;
@@ -97,6 +148,22 @@
if (!spline1.WaitForSplineDistanceRemaining(0.02)) return;
}
+void AutonomousActor::SendStartingPosition(
+ const frc971::MultiSpline *const spline) {
+ float x = spline->spline_x()->Get(0);
+ float y = spline->spline_y()->Get(0);
+
+ Eigen::Matrix<double, 2, 6> control_points;
+ for (size_t ii = 0; ii < 6; ++ii) {
+ control_points(0, ii) = spline->spline_x()->Get(ii);
+ control_points(1, ii) = spline->spline_y()->Get(ii);
+ }
+
+ frc971::control_loops::drivetrain::Spline spline_object(control_points);
+
+ SendStartingPosition(x, y, spline_object.Theta(0));
+}
+
ProfileParametersT MakeProfileParametersT(const float max_velocity,
const float max_acceleration) {
ProfileParametersT params;
@@ -106,6 +173,7 @@
}
bool AutonomousActor::DriveFwd() {
+ SendStartingPosition(0, 0, 0);
const ProfileParametersT kDrive = MakeProfileParametersT(0.3f, 1.0f);
const ProfileParametersT kTurn = MakeProfileParametersT(5.0f, 15.0f);
StartDrive(1.0, 0.0, kDrive, kTurn);
diff --git a/y2020/actors/autonomous_actor.h b/y2020/actors/autonomous_actor.h
index a58e3f9..12b8e77 100644
--- a/y2020/actors/autonomous_actor.h
+++ b/y2020/actors/autonomous_actor.h
@@ -8,6 +8,7 @@
#include "frc971/control_loops/drivetrain/drivetrain_config.h"
#include "frc971/control_loops/drivetrain/localizer_generated.h"
#include "y2020/actors/auto_splines.h"
+#include "y2020/vision/galactic_search_path_generated.h"
namespace y2020 {
namespace actors {
@@ -22,11 +23,15 @@
private:
void Reset();
void SplineAuto();
+ void SendStartingPosition(double x, double y, double theta);
+ void SendStartingPosition(const frc971::MultiSpline *const spline);
+ void GalacticSearch();
bool DriveFwd();
::aos::Sender<::frc971::control_loops::drivetrain::LocalizerControl>
localizer_control_sender_;
aos::Fetcher<aos::JoystickState> joystick_state_fetcher_;
+ aos::Fetcher<y2020::vision::GalacticSearchPath> path_fetcher_;
aos::Alliance alliance_ = aos::Alliance::kInvalid;
AutonomousSplines auto_splines_;
};
diff --git a/y2020/actors/splines/spline_blue_a.json b/y2020/actors/splines/spline_blue_a.json
new file mode 100644
index 0000000..733d516
--- /dev/null
+++ b/y2020/actors/splines/spline_blue_a.json
@@ -0,0 +1 @@
+{"spline_count": 1, "spline_x": [0, 0.4, 0.4, 0.6, 0.6, 1.0], "spline_y": [0, 0, 0.05, 0.1, 0.15, 0.15], "constraints": [{"constraint_type": "LONGITUDINAL_ACCELERATION", "value": 1}, {"constraint_type": "LATERAL_ACCELERATION", "value": 1}, {"constraint_type": "VOLTAGE", "value": 2}]}
diff --git a/y2020/actors/splines/spline_blue_b.json b/y2020/actors/splines/spline_blue_b.json
new file mode 100644
index 0000000..733d516
--- /dev/null
+++ b/y2020/actors/splines/spline_blue_b.json
@@ -0,0 +1 @@
+{"spline_count": 1, "spline_x": [0, 0.4, 0.4, 0.6, 0.6, 1.0], "spline_y": [0, 0, 0.05, 0.1, 0.15, 0.15], "constraints": [{"constraint_type": "LONGITUDINAL_ACCELERATION", "value": 1}, {"constraint_type": "LATERAL_ACCELERATION", "value": 1}, {"constraint_type": "VOLTAGE", "value": 2}]}
diff --git a/y2020/actors/splines/spline_red_a.json b/y2020/actors/splines/spline_red_a.json
new file mode 100644
index 0000000..733d516
--- /dev/null
+++ b/y2020/actors/splines/spline_red_a.json
@@ -0,0 +1 @@
+{"spline_count": 1, "spline_x": [0, 0.4, 0.4, 0.6, 0.6, 1.0], "spline_y": [0, 0, 0.05, 0.1, 0.15, 0.15], "constraints": [{"constraint_type": "LONGITUDINAL_ACCELERATION", "value": 1}, {"constraint_type": "LATERAL_ACCELERATION", "value": 1}, {"constraint_type": "VOLTAGE", "value": 2}]}
diff --git a/y2020/actors/splines/spline_red_b.json b/y2020/actors/splines/spline_red_b.json
new file mode 100644
index 0000000..733d516
--- /dev/null
+++ b/y2020/actors/splines/spline_red_b.json
@@ -0,0 +1 @@
+{"spline_count": 1, "spline_x": [0, 0.4, 0.4, 0.6, 0.6, 1.0], "spline_y": [0, 0, 0.05, 0.1, 0.15, 0.15], "constraints": [{"constraint_type": "LONGITUDINAL_ACCELERATION", "value": 1}, {"constraint_type": "LATERAL_ACCELERATION", "value": 1}, {"constraint_type": "VOLTAGE", "value": 2}]}