Merge "Add an undo button to ArmUI"
diff --git a/frc971/control_loops/drivetrain/line_follow_drivetrain.cc b/frc971/control_loops/drivetrain/line_follow_drivetrain.cc
index ea20bd0..9823f0c 100644
--- a/frc971/control_loops/drivetrain/line_follow_drivetrain.cc
+++ b/frc971/control_loops/drivetrain/line_follow_drivetrain.cc
@@ -284,6 +284,9 @@
} else if (minU < -12.0) {
U_ = U_ - U_.Ones() * (minU + 12.0);
}
+ if (!U_.allFinite()) {
+ U_.setZero();
+ }
}
flatbuffers::Offset<LineFollowLogging> LineFollowDrivetrain::PopulateStatus(
diff --git a/frc971/vision/BUILD b/frc971/vision/BUILD
index 133281b..84f3be7 100644
--- a/frc971/vision/BUILD
+++ b/frc971/vision/BUILD
@@ -166,6 +166,7 @@
":target_map_fbs",
"//aos/events:simulated_event_loop",
"//frc971/control_loops:control_loop",
+ "//frc971/vision:visualize_robot",
"//frc971/vision/ceres:pose_graph_3d_lib",
"//third_party:opencv",
"@com_google_ceres_solver//:ceres",
diff --git a/frc971/vision/target_mapper.cc b/frc971/vision/target_mapper.cc
index 3eba919..0d6b9e9 100644
--- a/frc971/vision/target_mapper.cc
+++ b/frc971/vision/target_mapper.cc
@@ -10,9 +10,12 @@
DEFINE_double(distortion_noise_scalar, 1.0,
"Scale the target pose distortion factor by this when computing "
"the noise.");
-DEFINE_uint64(
+DEFINE_int32(
frozen_target_id, 1,
"Freeze the pose of this target so the map can have one fixed point.");
+DEFINE_int32(min_target_id, 1, "Minimum target id to solve for");
+DEFINE_int32(max_target_id, 8, "Maximum target id to solve for");
+DEFINE_bool(visualize_solver, false, "If true, visualize the solving process.");
namespace frc971::vision {
Eigen::Affine3d PoseUtils::Pose3dToAffine3d(
@@ -211,19 +214,28 @@
TargetMapper::TargetMapper(
std::string_view target_poses_path,
const ceres::examples::VectorOfConstraints &target_constraints)
- : target_constraints_(target_constraints) {
+ : target_constraints_(target_constraints),
+ T_frozen_actual_(Eigen::Vector3d::Zero()),
+ R_frozen_actual_(Eigen::Quaterniond::Identity()),
+ vis_robot_(cv::Size(1280, 1000)) {
aos::FlatbufferDetachedBuffer<TargetMap> target_map =
aos::JsonFileToFlatbuffer<TargetMap>(target_poses_path);
for (const auto *target_pose_fbs : *target_map.message().target_poses()) {
- target_poses_[target_pose_fbs->id()] =
+ ideal_target_poses_[target_pose_fbs->id()] =
PoseUtils::TargetPoseFromFbs(*target_pose_fbs).pose;
}
+ target_poses_ = ideal_target_poses_;
}
TargetMapper::TargetMapper(
const ceres::examples::MapOfPoses &target_poses,
const ceres::examples::VectorOfConstraints &target_constraints)
- : target_poses_(target_poses), target_constraints_(target_constraints) {}
+ : ideal_target_poses_(target_poses),
+ target_poses_(ideal_target_poses_),
+ target_constraints_(target_constraints),
+ T_frozen_actual_(Eigen::Vector3d::Zero()),
+ R_frozen_actual_(Eigen::Quaterniond::Identity()),
+ vis_robot_(cv::Size(1280, 1000)) {}
std::optional<TargetMapper::TargetPose> TargetMapper::GetTargetPoseById(
std::vector<TargetMapper::TargetPose> target_poses, TargetId target_id) {
@@ -248,7 +260,7 @@
// Taken from ceres/examples/slam/pose_graph_3d/pose_graph_3d.cc
// Constructs the nonlinear least squares optimization problem from the pose
// graph constraints.
-void TargetMapper::BuildOptimizationProblem(
+void TargetMapper::BuildTargetPoseOptimizationProblem(
const ceres::examples::VectorOfConstraints &constraints,
ceres::examples::MapOfPoses *poses, ceres::Problem *problem) {
CHECK(poses != nullptr);
@@ -311,6 +323,33 @@
problem->SetParameterBlockConstant(pose_start_iter->second.q.coeffs().data());
}
+void TargetMapper::BuildMapFittingOptimizationProblem(ceres::Problem *problem) {
+ // Setup robot visualization
+ vis_robot_.ClearImage();
+ constexpr int kImageWidth = 1280;
+ constexpr double kFocalLength = 500.0;
+ vis_robot_.SetDefaultViewpoint(kImageWidth, kFocalLength);
+
+ const size_t num_targets = FLAGS_max_target_id - FLAGS_min_target_id;
+ // Translation and rotation error for each target
+ const size_t num_residuals = num_targets * 6;
+ // Set up the only cost function (also known as residual). This uses
+ // auto-differentiation to obtain the derivative (jacobian).
+ ceres::CostFunction *cost_function =
+ new ceres::AutoDiffCostFunction<TargetMapper, ceres::DYNAMIC, 3, 4>(
+ this, num_residuals, ceres::DO_NOT_TAKE_OWNERSHIP);
+
+ ceres::LossFunction *loss_function = new ceres::HuberLoss(2.0);
+ ceres::LocalParameterization *quaternion_local_parameterization =
+ new ceres::EigenQuaternionParameterization;
+
+ problem->AddResidualBlock(cost_function, loss_function,
+ T_frozen_actual_.vector().data(),
+ R_frozen_actual_.coeffs().data());
+ problem->SetParameterization(R_frozen_actual_.coeffs().data(),
+ quaternion_local_parameterization);
+}
+
// Taken from ceres/examples/slam/pose_graph_3d/pose_graph_3d.cc
bool TargetMapper::SolveOptimizationProblem(ceres::Problem *problem) {
CHECK_NOTNULL(problem);
@@ -330,11 +369,39 @@
void TargetMapper::Solve(std::string_view field_name,
std::optional<std::string_view> output_dir) {
- ceres::Problem problem;
- BuildOptimizationProblem(target_constraints_, &target_poses_, &problem);
+ ceres::Problem target_pose_problem;
+ BuildTargetPoseOptimizationProblem(target_constraints_, &target_poses_,
+ &target_pose_problem);
+ CHECK(SolveOptimizationProblem(&target_pose_problem))
+ << "The target pose solve was not successful, exiting.";
- CHECK(SolveOptimizationProblem(&problem))
- << "The solve was not successful, exiting.";
+ ceres::Problem map_fitting_problem;
+ BuildMapFittingOptimizationProblem(&map_fitting_problem);
+ CHECK(SolveOptimizationProblem(&map_fitting_problem))
+ << "The map fitting solve was not successful, exiting.";
+
+ Eigen::Affine3d H_frozen_actual = T_frozen_actual_ * R_frozen_actual_;
+ LOG(INFO) << "H_frozen_actual: "
+ << PoseUtils::Affine3dToPose3d(H_frozen_actual);
+
+ auto H_world_frozen =
+ PoseUtils::Pose3dToAffine3d(target_poses_[FLAGS_frozen_target_id]);
+ auto H_world_frozenactual = H_world_frozen * H_frozen_actual;
+
+ // Offset the solved poses to become the actual ones
+ for (auto &[id, pose] : target_poses_) {
+ // Don't offset targets we didn't solve for
+ if (id < FLAGS_min_target_id || id > FLAGS_max_target_id) {
+ continue;
+ }
+
+ // Take the delta between the frozen target and the solved target, and put
+ // that on top of the actual pose of the frozen target
+ auto H_world_solved = PoseUtils::Pose3dToAffine3d(pose);
+ auto H_frozen_solved = H_world_frozen.inverse() * H_world_solved;
+ auto H_world_actual = H_world_frozenactual * H_frozen_solved;
+ pose = PoseUtils::Affine3dToPose3d(H_world_actual);
+ }
auto map_json = MapToJson(field_name);
VLOG(1) << "Solved target poses: " << map_json;
@@ -366,6 +433,110 @@
{.multi_line = true});
}
+namespace {
+
+// Hacks to extract a double from a scalar, which is either a ceres jet or a
+// double. Only used for debugging and displaying.
+template <typename S>
+double ScalarToDouble(S s) {
+ const double *ptr = reinterpret_cast<double *>(&s);
+ return *ptr;
+}
+
+template <typename S>
+Eigen::Affine3d ScalarAffineToDouble(Eigen::Transform<S, 3, Eigen::Affine> H) {
+ Eigen::Affine3d H_double;
+ for (size_t i = 0; i < H.rows(); i++) {
+ for (size_t j = 0; j < H.cols(); j++) {
+ H_double(i, j) = ScalarToDouble(H(i, j));
+ }
+ }
+ return H_double;
+}
+
+} // namespace
+
+template <typename S>
+bool TargetMapper::operator()(const S *const translation,
+ const S *const rotation, S *residual) const {
+ using Affine3s = Eigen::Transform<S, 3, Eigen::Affine>;
+ Eigen::Quaternion<S> R_frozen_actual(rotation[3], rotation[1], rotation[2],
+ rotation[0]);
+ Eigen::Translation<S, 3> T_frozen_actual(translation[0], translation[1],
+ translation[2]);
+ // Actual target pose in the frame of the fixed pose.
+ Affine3s H_frozen_actual = T_frozen_actual * R_frozen_actual;
+ VLOG(2) << "H_frozen_actual: "
+ << PoseUtils::Affine3dToPose3d(ScalarAffineToDouble(H_frozen_actual));
+
+ Affine3s H_world_frozen =
+ PoseUtils::Pose3dToAffine3d(target_poses_.at(FLAGS_frozen_target_id))
+ .cast<S>();
+ Affine3s H_world_frozenactual = H_world_frozen * H_frozen_actual;
+
+ size_t residual_index = 0;
+ if (FLAGS_visualize_solver) {
+ vis_robot_.ClearImage();
+ }
+
+ for (const auto &[id, solved_pose] : target_poses_) {
+ if (id < FLAGS_min_target_id || id > FLAGS_max_target_id) {
+ continue;
+ }
+
+ Affine3s H_world_ideal =
+ PoseUtils::Pose3dToAffine3d(ideal_target_poses_.at(id)).cast<S>();
+ Affine3s H_world_solved =
+ PoseUtils::Pose3dToAffine3d(solved_pose).cast<S>();
+ // Take the delta between the frozen target and the solved target, and put
+ // that on top of the actual pose of the frozen target
+ auto H_frozen_solved = H_world_frozen.inverse() * H_world_solved;
+ auto H_world_actual = H_world_frozenactual * H_frozen_solved;
+ VLOG(2) << id << ": " << H_world_actual.translation();
+ Affine3s H_ideal_actual = H_world_ideal.inverse() * H_world_actual;
+ auto T_ideal_actual = H_ideal_actual.translation();
+ VLOG(2) << "T_ideal_actual: " << T_ideal_actual;
+ VLOG(2);
+ auto R_ideal_actual = Eigen::AngleAxis<S>(H_ideal_actual.rotation());
+
+ constexpr double kTranslationScalar = 100.0;
+ constexpr double kRotationScalar = 1000.0;
+
+ // Penalize based on how much our actual poses matches the ideal
+ // ones. We've already solved for the relative poses, now figure out
+ // where all of them fit in the world.
+ residual[residual_index++] = kTranslationScalar * T_ideal_actual(0);
+ residual[residual_index++] = kTranslationScalar * T_ideal_actual(1);
+ residual[residual_index++] = kTranslationScalar * T_ideal_actual(2);
+ residual[residual_index++] =
+ kRotationScalar * R_ideal_actual.angle() * R_ideal_actual.axis().x();
+ residual[residual_index++] =
+ kRotationScalar * R_ideal_actual.angle() * R_ideal_actual.axis().y();
+ residual[residual_index++] =
+ kRotationScalar * R_ideal_actual.angle() * R_ideal_actual.axis().z();
+
+ if (FLAGS_visualize_solver) {
+ vis_robot_.DrawFrameAxes(ScalarAffineToDouble(H_world_actual),
+ std::to_string(id), cv::Scalar(0, 255, 0));
+ vis_robot_.DrawFrameAxes(ScalarAffineToDouble(H_world_ideal),
+ std::to_string(id), cv::Scalar(255, 255, 255));
+ }
+ }
+ if (FLAGS_visualize_solver) {
+ cv::imshow("Target maps", vis_robot_.image_);
+ cv::waitKey(0);
+ }
+
+ // Ceres can't handle residual values of exactly zero
+ for (size_t i = 0; i < residual_index; i++) {
+ if (residual[i] == S(0)) {
+ residual[i] = S(1e-9);
+ }
+ }
+
+ return true;
+}
+
} // namespace frc971::vision
std::ostream &operator<<(std::ostream &os, ceres::examples::Pose3d pose) {
diff --git a/frc971/vision/target_mapper.h b/frc971/vision/target_mapper.h
index c782992..ca36866 100644
--- a/frc971/vision/target_mapper.h
+++ b/frc971/vision/target_mapper.h
@@ -7,6 +7,7 @@
#include "ceres/ceres.h"
#include "frc971/vision/ceres/types.h"
#include "frc971/vision/target_map_generated.h"
+#include "frc971/vision/visualize_robot.h"
namespace frc971::vision {
@@ -51,18 +52,36 @@
ceres::examples::MapOfPoses target_poses() { return target_poses_; }
+ // Cost function for the secondary solver finding out where the whole map fits
+ // in the world
+ template <typename S>
+ bool operator()(const S *const translation, const S *const rotation,
+ S *residual) const;
+
private:
// Constructs the nonlinear least squares optimization problem from the
// pose graph constraints.
- void BuildOptimizationProblem(
+ void BuildTargetPoseOptimizationProblem(
const ceres::examples::VectorOfConstraints &constraints,
ceres::examples::MapOfPoses *poses, ceres::Problem *problem);
+ // Constructs the nonlinear least squares optimization problem for the solved
+ // -> actual pose solver.
+ void BuildMapFittingOptimizationProblem(ceres::Problem *problem);
+
// Returns true if the solve was successful.
bool SolveOptimizationProblem(ceres::Problem *problem);
+ ceres::examples::MapOfPoses ideal_target_poses_;
ceres::examples::MapOfPoses target_poses_;
ceres::examples::VectorOfConstraints target_constraints_;
+
+ // Transformation moving the target map we solved for to where it actually
+ // should be in the world
+ Eigen::Translation3d T_frozen_actual_;
+ Eigen::Quaterniond R_frozen_actual_;
+
+ mutable VisualizeRobot vis_robot_;
};
// Utility functions for dealing with ceres::examples::Pose3d structs
diff --git a/frc971/vision/target_mapper_test.cc b/frc971/vision/target_mapper_test.cc
index cd7d18b..1371f89 100644
--- a/frc971/vision/target_mapper_test.cc
+++ b/frc971/vision/target_mapper_test.cc
@@ -9,6 +9,9 @@
#include "glog/logging.h"
#include "gtest/gtest.h"
+DECLARE_int32(min_target_id);
+DECLARE_int32(max_target_id);
+
namespace frc971::vision {
namespace {
@@ -338,6 +341,9 @@
}
TEST(TargetMapperTest, TwoTargetsOneConstraint) {
+ FLAGS_min_target_id = 0;
+ FLAGS_max_target_id = 1;
+
ceres::examples::MapOfPoses target_poses;
target_poses[0] = Make2dPose(5.0, 0.0, M_PI);
target_poses[1] = Make2dPose(-5.0, 0.0, 0.0);
@@ -361,6 +367,9 @@
}
TEST(TargetMapperTest, TwoTargetsTwoConstraints) {
+ FLAGS_min_target_id = 0;
+ FLAGS_max_target_id = 1;
+
ceres::examples::MapOfPoses target_poses;
target_poses[0] = Make2dPose(5.0, 0.0, M_PI);
target_poses[1] = Make2dPose(-5.0, 0.0, -M_PI_2);
@@ -390,6 +399,9 @@
}
TEST(TargetMapperTest, TwoTargetsOneNoisyConstraint) {
+ FLAGS_min_target_id = 0;
+ FLAGS_max_target_id = 1;
+
ceres::examples::MapOfPoses target_poses;
target_poses[0] = Make2dPose(5.0, 0.0, M_PI);
target_poses[1] = Make2dPose(-5.0, 0.0, 0.0);
@@ -549,33 +561,6 @@
.value();
EXPECT_POSE_NEAR(mapper_target_pose, actual_target_pose.pose);
}
-
- //
- // See what happens when we don't start with the
- // correct values
- //
- for (auto [target_id, target_pose] : target_poses) {
- // Skip first pose, since that needs to be correct
- // and is fixed in the solver
- if (target_id != 1) {
- ceres::examples::Pose3d bad_pose{
- Eigen::Vector3d::Zero(),
- PoseUtils::EulerAnglesToQuaternion(Eigen::Vector3d::Zero())};
- target_poses[target_id] = bad_pose;
- }
- }
-
- frc971::vision::TargetMapper mapper_bad_poses(target_poses,
- target_constraints);
- mapper_bad_poses.Solve(kFieldName);
-
- for (auto [target_pose_id, mapper_target_pose] :
- mapper_bad_poses.target_poses()) {
- TargetMapper::TargetPose actual_target_pose =
- TargetMapper::GetTargetPoseById(actual_target_poses, target_pose_id)
- .value();
- EXPECT_POSE_NEAR(mapper_target_pose, actual_target_pose.pose);
- }
}
} // namespace frc971::vision
diff --git a/scouting/scouting_test.cy.js b/scouting/scouting_test.cy.js
index d79bf5a..1d7620a 100644
--- a/scouting/scouting_test.cy.js
+++ b/scouting/scouting_test.cy.js
@@ -99,25 +99,24 @@
cy.get('.badge').eq(89).contains('Final 1 Match 3');
});
- it('should: prefill the match information.', () => {
- headerShouldBe('Matches');
-
- clickSemiFinal2Match3Team5254();
-
+ it('should: be let users enter match information manually.', () => {
+ switchToTab('Entry');
headerShouldBe(' Team Selection ');
- cy.get('#match_number').should('have.value', '3');
- cy.get('#team_number').should('have.value', '5254');
- cy.get('#set_number').should('have.value', '2');
- cy.get('#comp_level').should('have.value', '3: sf');
+
+ setInputTo('#match_number', '3');
+ setInputTo('#team_number', '5254');
+ setInputTo('#set_number', '2');
+ setInputTo('#comp_level', '3: sf');
+
+ clickButton('Next');
+
+ headerShouldBe('5254 Init ');
});
//TODO(FILIP): Verify last action when the last action header gets added.
it('should: be able to submit data scouting.', () => {
clickSemiFinal2Match3Team5254();
- headerShouldBe(' Team Selection ');
- clickButton('Next');
-
// Select Starting Position.
headerShouldBe('5254 Init ');
cy.get('[type="radio"]').first().check();
@@ -150,9 +149,6 @@
it('should: be able to return to correct screen with undo for pick and place.', () => {
clickSemiFinal2Match3Team5254();
- headerShouldBe(' Team Selection ');
- clickButton('Next');
-
// Select Starting Position.
cy.get('[type="radio"]').first().check();
clickButton('Start Match');
diff --git a/scouting/www/app/app.ng.html b/scouting/www/app/app.ng.html
index 526cd61..b7f1873 100644
--- a/scouting/www/app/app.ng.html
+++ b/scouting/www/app/app.ng.html
@@ -72,18 +72,15 @@
*ngSwitchCase="'MatchList'"
></app-match-list>
<app-entry
- (switchTabsEvent)="switchTabTo($event)"
[teamNumber]="selectedTeamInMatch.teamNumber"
[matchNumber]="selectedTeamInMatch.matchNumber"
[setNumber]="selectedTeamInMatch.setNumber"
[compLevel]="selectedTeamInMatch.compLevel"
+ [skipTeamSelection]="navigatedFromMatchList"
*ngSwitchCase="'Entry'"
></app-entry>
<frc971-notes *ngSwitchCase="'Notes'"></frc971-notes>
<app-driver-ranking *ngSwitchCase="'DriverRanking'"></app-driver-ranking>
<shift-schedule *ngSwitchCase="'ShiftSchedule'"></shift-schedule>
- <app-view
- (switchTabsEvent)="switchTabTo($event)"
- *ngSwitchCase="'View'"
- ></app-view>
+ <app-view *ngSwitchCase="'View'"></app-view>
</ng-container>
diff --git a/scouting/www/app/app.ts b/scouting/www/app/app.ts
index f7d2770..c19895a 100644
--- a/scouting/www/app/app.ts
+++ b/scouting/www/app/app.ts
@@ -30,6 +30,9 @@
setNumber: 1,
compLevel: 'qm',
};
+ // Keep track of the match list automatically navigating the user to the
+ // Entry tab.
+ navigatedFromMatchList: boolean = false;
tab: Tab = 'MatchList';
@ViewChild('block_alerts') block_alerts: ElementRef;
@@ -54,7 +57,8 @@
selectTeamInMatch(teamInMatch: TeamInMatch) {
this.selectedTeamInMatch = teamInMatch;
- this.switchTabTo('Entry');
+ this.navigatedFromMatchList = true;
+ this.switchTabTo('Entry', false);
}
switchTabToGuarded(tab: Tab) {
@@ -69,11 +73,16 @@
}
}
if (shouldSwitch) {
- this.switchTabTo(tab);
+ this.switchTabTo(tab, true);
}
}
- private switchTabTo(tab: Tab) {
+ private switchTabTo(tab: Tab, wasGuarded: boolean) {
+ if (wasGuarded) {
+ // When the user navigated between tabs manually, we want to reset some
+ // state.
+ this.navigatedFromMatchList = false;
+ }
this.tab = tab;
}
}
diff --git a/scouting/www/entry/entry.component.ts b/scouting/www/entry/entry.component.ts
index 71c8de2..b56948a 100644
--- a/scouting/www/entry/entry.component.ts
+++ b/scouting/www/entry/entry.component.ts
@@ -97,7 +97,7 @@
templateUrl: './entry.ng.html',
styleUrls: ['../app/common.css', './entry.component.css'],
})
-export class EntryComponent {
+export class EntryComponent implements OnInit {
// Re-export the type here so that we can use it in the `[value]` attribute
// of radio buttons.
readonly COMP_LEVELS = COMP_LEVELS;
@@ -106,11 +106,11 @@
readonly ScoreLevel = ScoreLevel;
section: Section = 'Team Selection';
- @Output() switchTabsEvent = new EventEmitter<string>();
@Input() matchNumber: number = 1;
@Input() teamNumber: number = 1;
@Input() setNumber: number = 1;
@Input() compLevel: CompLevel = 'qm';
+ @Input() skipTeamSelection = false;
actionList: ActionT[] = [];
errorMessage: string = '';
@@ -119,6 +119,12 @@
matchStartTimestamp: number = 0;
+ ngOnInit() {
+ // When the user navigated from the match list, we can skip the team
+ // selection. I.e. we trust that the user clicked the correct button.
+ this.section = this.skipTeamSelection ? 'Init' : 'Team Selection';
+ }
+
addAction(action: ActionT): void {
if (action.type == 'startMatchAction') {
// Unix nanosecond timestamp.
diff --git a/y2023/BUILD b/y2023/BUILD
index c3f7c33..6be5cac 100644
--- a/y2023/BUILD
+++ b/y2023/BUILD
@@ -55,6 +55,7 @@
"//aos/util:foxglove_websocket",
"//y2023/vision:viewer",
"//y2023/vision:localization_verifier",
+ "//y2023/vision:camera_monitor",
"//y2023/vision:aprilrobotics",
"//aos/events:aos_timing_report_streamer",
"//y2023/localizer:localizer_main",
diff --git a/y2023/autonomous/autonomous_actor.cc b/y2023/autonomous/autonomous_actor.cc
index 58c31d0..cd4d4f1 100644
--- a/y2023/autonomous/autonomous_actor.cc
+++ b/y2023/autonomous/autonomous_actor.cc
@@ -475,7 +475,13 @@
splines[1].Start();
std::this_thread::sleep_for(chrono::milliseconds(300));
+ Neutral();
+
+ if (!splines[1].WaitForSplineDistanceTraveled(3.2)) return;
HighCubeScore();
+ AOS_LOG(
+ INFO, "Extending arm %lf s\n",
+ aos::time::DurationInSeconds(aos::monotonic_clock::now() - start_time));
if (!splines[1].WaitForSplineDistanceRemaining(0.08)) return;
AOS_LOG(
@@ -521,10 +527,17 @@
if (!splines[3].WaitForPlan()) return;
splines[3].Start();
+ std::this_thread::sleep_for(chrono::milliseconds(400));
+ Neutral();
+
AOS_LOG(
INFO, "Driving back %lf s\n",
aos::time::DurationInSeconds(aos::monotonic_clock::now() - start_time));
+ if (!splines[3].WaitForSplineDistanceTraveled(3.5)) return;
+ AOS_LOG(
+ INFO, "Extending arm %lf s\n",
+ aos::time::DurationInSeconds(aos::monotonic_clock::now() - start_time));
MidCubeScore();
if (!splines[3].WaitForSplineDistanceRemaining(0.07)) return;
diff --git a/y2023/autonomous/splines/splinecable.0.json b/y2023/autonomous/splines/splinecable.0.json
index 3fdcfbe..3a8a1c8 100644
--- a/y2023/autonomous/splines/splinecable.0.json
+++ b/y2023/autonomous/splines/splinecable.0.json
@@ -1 +1 @@
-{"spline_count": 1, "spline_x": [-6.468141183035714, -6.142335156250001, -5.834629464285714, -2.6851712053571433, -2.2145624999999995, -1.7620541294642855], "spline_y": [-3.493364620535714, -3.493364620535714, -3.4752642857142853, -3.0951572544642856, -3.0770569196428568, -3.0770569196428568], "constraints": [{"constraint_type": "LONGITUDINAL_ACCELERATION", "value": 3.0}, {"constraint_type": "LATERAL_ACCELERATION", "value": 2.0}, {"constraint_type": "VOLTAGE", "value": 10.0}]}
\ No newline at end of file
+{"spline_count": 2, "spline_x": [6.468141183035714, 6.156227508178545, 5.627928372772197, 5.618126908868124, 4.926607785226736, 4.384253622459038, 3.8418994596913407, 3.448710257797334, 2.6702357572801336, 1.9707990587626902, 1.441511105732058], "spline_y": [-3.493364620535714, -3.4921188608837532, -3.431437306632174, -3.364982889102878, -3.381212722774612, -3.3815433034902798, -3.3818738842059477, -3.3663052119655514, -3.293883383936489, -3.2050645805145246, -3.125670625960137], "constraints": [{"constraint_type": "LONGITUDINAL_ACCELERATION", "value": 3.5}, {"constraint_type": "LATERAL_ACCELERATION", "value": 2.0}, {"constraint_type": "VOLTAGE", "value": 12.0}, {"constraint_type": "VELOCITY", "value": 1.5, "start_distance": 1.6, "end_distance": 2.75}]}
\ No newline at end of file
diff --git a/y2023/autonomous/splines/splinecable.1.json b/y2023/autonomous/splines/splinecable.1.json
index b9b4fdd..bf6838d 100644
--- a/y2023/autonomous/splines/splinecable.1.json
+++ b/y2023/autonomous/splines/splinecable.1.json
@@ -1 +1 @@
-{"spline_count": 1, "spline_x": [-1.7620541294642855, -2.2145624999999995, -2.6851712053571433, -5.8165291294642865, -6.124234821428573, -6.450040848214286], "spline_y": [-3.0770569196428568, -3.0770569196428568, -3.0951572544642856, -2.9141539062499993, -2.932254241071428, -2.932254241071428], "constraints": [{"constraint_type": "LONGITUDINAL_ACCELERATION", "value": 3.0}, {"constraint_type": "LATERAL_ACCELERATION", "value": 2.0}, {"constraint_type": "VOLTAGE", "value": 10.0}]}
\ No newline at end of file
+{"spline_count": 2, "spline_x": [1.441511105732058, 2.158787794162399, 2.644327791138681, 3.0017835551344163, 3.810734918966552, 4.3709130304548385, 4.931091141943125, 5.242496001087563, 5.544694674297983, 5.965729060465805, 6.444059695844368], "spline_y": [-3.125670625960137, -3.233263161324085, -3.2028042659537914, -3.139500351375334, -3.125566969972238, -3.126688937308022, -3.127810904643806, -3.143988220718469, -2.9893341041515376, -2.943814113563144, -2.9458417329239843], "constraints": [{"constraint_type": "LONGITUDINAL_ACCELERATION", "value": 3.0}, {"constraint_type": "LATERAL_ACCELERATION", "value": 2.0}, {"constraint_type": "VOLTAGE", "value": 12.0}, {"constraint_type": "VELOCITY", "value": 1.5, "start_distance": 1.9, "end_distance": 10.0}]}
\ No newline at end of file
diff --git a/y2023/autonomous/splines/splinecable.2.json b/y2023/autonomous/splines/splinecable.2.json
index cef0375..1caf288 100644
--- a/y2023/autonomous/splines/splinecable.2.json
+++ b/y2023/autonomous/splines/splinecable.2.json
@@ -1 +1 @@
-{"spline_count": 1, "spline_x": [-6.450040848214286, -6.124234821428573, -5.8165291294642865, -2.9023752232142854, -2.1059604910714285, -1.6353517857142856], "spline_y": [-2.932254241071428, -2.932254241071428, -2.9141539062499993, -3.2218595982142855, -2.6607492187499995, -2.244441517857142], "constraints": [{"constraint_type": "LONGITUDINAL_ACCELERATION", "value": 3.0}, {"constraint_type": "LATERAL_ACCELERATION", "value": 2.0}, {"constraint_type": "VOLTAGE", "value": 10.0}]}
\ No newline at end of file
+{"spline_count": 2, "spline_x": [6.444059695844368, 5.977202894617023, 5.571532532121525, 5.64631894401731, 5.012590627058541, 4.364949107965309, 3.7173075888720764, 3.0557528676443795, 2.3031869947159427, 1.8583292479008144, 1.389061484581065], "spline_y": [-2.9458417329239843, -2.943862750565559, -2.96219050239874, -3.128181973537357, -3.0803143268366417, -3.079651639654858, -3.0789889524730745, -3.1255312248102225, -2.6948472310294784, -2.234117571800885, -1.9039073778940578], "constraints": [{"constraint_type": "LONGITUDINAL_ACCELERATION", "value": 3.0}, {"constraint_type": "LATERAL_ACCELERATION", "value": 2.0}, {"constraint_type": "VOLTAGE", "value": 12.0}, {"constraint_type": "VELOCITY", "value": 1.5, "start_distance": 1.6, "end_distance": 2.55}]}
\ No newline at end of file
diff --git a/y2023/autonomous/splines/splinecable.3.json b/y2023/autonomous/splines/splinecable.3.json
index 60821fe..c49c1e8 100644
--- a/y2023/autonomous/splines/splinecable.3.json
+++ b/y2023/autonomous/splines/splinecable.3.json
@@ -1 +1 @@
-{"spline_count": 1, "spline_x": [-1.6353517857142856, -2.1059604910714285, -2.9023752232142854, -5.8165291294642865, -6.124234821428573, -6.450040848214286], "spline_y": [-2.244441517857142, -2.6607492187499995, -3.2218595982142855, -2.9141539062499993, -2.932254241071428, -2.932254241071428], "constraints": [{"constraint_type": "LONGITUDINAL_ACCELERATION", "value": 3.0}, {"constraint_type": "LATERAL_ACCELERATION", "value": 2.0}, {"constraint_type": "VOLTAGE", "value": 10.0}]}
\ No newline at end of file
+{"spline_count": 2, "spline_x": [1.389061484581065, 2.1138941612201396, 2.2865982233539297, 3.1216159825232483, 3.763030179317507, 4.301019167021117, 4.839008154724727, 5.2735719333376885, 5.59798451182913, 5.734162859875811, 6.443454541551164], "spline_y": [-1.9039073778940578, -2.4139512321954397, -2.680729209318427, -3.1198721507158496, -3.0738965122418085, -3.0726645678919953, -3.071432623542182, -3.1149443733165967, -2.966418143397661, -2.9578217694545557, -2.957336170980663], "constraints": [{"constraint_type": "LONGITUDINAL_ACCELERATION", "value": 3.0}, {"constraint_type": "LATERAL_ACCELERATION", "value": 2.0}, {"constraint_type": "VOLTAGE", "value": 12.0}, {"constraint_type": "VELOCITY", "value": 1.5, "start_distance": 2.5, "end_distance": 3.6}]}
\ No newline at end of file
diff --git a/y2023/vision/BUILD b/y2023/vision/BUILD
index eda123e..ca057fd 100644
--- a/y2023/vision/BUILD
+++ b/y2023/vision/BUILD
@@ -298,3 +298,25 @@
srcs = ["game_pieces_detector_starter.sh"],
visibility = ["//visibility:public"],
)
+
+cc_library(
+ name = "camera_monitor_lib",
+ srcs = ["camera_monitor_lib.cc"],
+ hdrs = ["camera_monitor_lib.h"],
+ deps = [
+ "//aos/events:event_loop",
+ "//aos/starter:starter_rpc_lib",
+ "//frc971/vision:vision_fbs",
+ ],
+)
+
+cc_binary(
+ name = "camera_monitor",
+ srcs = ["camera_monitor.cc"],
+ visibility = ["//visibility:public"],
+ deps = [
+ ":camera_monitor_lib",
+ "//aos:init",
+ "//aos/events:shm_event_loop",
+ ],
+)
diff --git a/y2023/vision/camera_monitor.cc b/y2023/vision/camera_monitor.cc
new file mode 100644
index 0000000..e69fd7c
--- /dev/null
+++ b/y2023/vision/camera_monitor.cc
@@ -0,0 +1,18 @@
+#include "aos/events/shm_event_loop.h"
+#include "aos/init.h"
+#include "y2023/vision/camera_monitor_lib.h"
+
+DEFINE_string(config, "aos_config.json", "Path to the config file to use.");
+
+int main(int argc, char *argv[]) {
+ aos::InitGoogle(&argc, &argv);
+
+ aos::FlatbufferDetachedBuffer<aos::Configuration> config =
+ aos::configuration::ReadConfig(FLAGS_config);
+
+ aos::ShmEventLoop event_loop(&config.message());
+
+ y2023::vision::CameraMonitor monitor(&event_loop);
+
+ event_loop.Run();
+}
diff --git a/y2023/vision/camera_monitor_lib.cc b/y2023/vision/camera_monitor_lib.cc
new file mode 100644
index 0000000..c4ec0dc
--- /dev/null
+++ b/y2023/vision/camera_monitor_lib.cc
@@ -0,0 +1,37 @@
+#include "y2023/vision/camera_monitor_lib.h"
+namespace y2023::vision {
+namespace {
+// This needs to include the amount of time that it will take for the
+// camera_reader to start.
+constexpr std::chrono::seconds kImageTimeout{5};
+} // namespace
+CameraMonitor::CameraMonitor(aos::EventLoop *event_loop)
+ : event_loop_(event_loop), starter_(event_loop_) {
+ event_loop_->MakeNoArgWatcher<frc971::vision::CameraImage>(
+ "/camera", [this]() { SetImageTimeout(); });
+ starter_.SetTimeoutHandler([this]() {
+ LOG(WARNING) << "Failed to restart camera_reader when images timed out.";
+ SetImageTimeout();
+ });
+ starter_.SetSuccessHandler([this]() {
+ LOG(INFO) << "Finished restarting camera_reader.";
+ SetImageTimeout();
+ });
+
+ image_timeout_ = event_loop_->AddTimer([this]() {
+ LOG(INFO) << "Restarting camera_reader due to stale images.";
+ starter_.SendCommands({{aos::starter::Command::RESTART,
+ "camera_reader",
+ {event_loop_->node()}}},
+ /*timeout=*/std::chrono::seconds(3));
+ });
+ // If for some reason camera_reader fails to start up at all, we want to
+ // end up restarting things.
+ event_loop_->OnRun([this]() { SetImageTimeout(); });
+}
+
+void CameraMonitor::SetImageTimeout() {
+ image_timeout_->Setup(event_loop_->context().monotonic_event_time +
+ kImageTimeout);
+}
+} // namespace y2023::vision
diff --git a/y2023/vision/camera_monitor_lib.h b/y2023/vision/camera_monitor_lib.h
new file mode 100644
index 0000000..ddb116e
--- /dev/null
+++ b/y2023/vision/camera_monitor_lib.h
@@ -0,0 +1,21 @@
+#ifndef Y2023_VISION_CAMERA_MONITOR_LIB_H_
+#define Y2023_VISION_CAMERA_MONITOR_LIB_H_
+#include "aos/events/event_loop.h"
+#include "aos/starter/starter_rpc_lib.h"
+#include "frc971/vision/vision_generated.h"
+namespace y2023::vision {
+// This class provides an application that will restart the camera_reader
+// process whenever images stop flowing for too long. This is to mitigate an
+// issue where sometimes we stop getting camera images.
+class CameraMonitor {
+ public:
+ CameraMonitor(aos::EventLoop *event_loop);
+
+ private:
+ void SetImageTimeout();
+ aos::EventLoop *event_loop_;
+ aos::starter::StarterClient starter_;
+ aos::TimerHandler *image_timeout_;
+};
+} // namespace y2023::vision
+#endif // Y2023_VISION_CAMERA_MONITOR_LIB_H_
diff --git a/y2023/vision/target_mapping.cc b/y2023/vision/target_mapping.cc
index ba5f5d9..daba90c 100644
--- a/y2023/vision/target_mapping.cc
+++ b/y2023/vision/target_mapping.cc
@@ -5,7 +5,6 @@
#include "aos/util/mcap_logger.h"
#include "frc971/control_loops/pose.h"
#include "frc971/vision/calibration_generated.h"
-#include "frc971/vision/charuco_lib.h"
#include "frc971/vision/target_mapper.h"
#include "frc971/vision/visualize_robot.h"
#include "opencv2/aruco.hpp"
@@ -49,7 +48,10 @@
DEFINE_bool(solve, true, "Whether to solve for the field's target map.");
DEFINE_string(dump_constraints_to, "/tmp/constraints.txt",
"Write the target constraints to this path");
-DECLARE_uint64(frozen_target_id);
+DECLARE_int32(frozen_target_id);
+DECLARE_int32(min_target_id);
+DECLARE_int32(max_target_id);
+DECLARE_bool(visualize_solver);
namespace y2023 {
namespace vision {
@@ -61,9 +63,6 @@
using frc971::vision::VisualizeRobot;
namespace calibration = frc971::vision::calibration;
-constexpr TargetMapper::TargetId kMinTargetId = 1;
-constexpr TargetMapper::TargetId kMaxTargetId = 8;
-
// Class to handle reading target poses from a replayed log,
// displaying various debug info, and passing the poses to
// frc971::vision::TargetMapper for field mapping.
@@ -203,7 +202,7 @@
});
}
- if (FLAGS_visualize) {
+ if (FLAGS_visualize_solver) {
vis_robot_.ClearImage();
const double kFocalLength = 500.0;
vis_robot_.SetDefaultViewpoint(kImageWidth, kFocalLength);
@@ -229,22 +228,23 @@
for (const auto *target_pose_fbs : *map.target_poses()) {
// Skip detections with invalid ids
- if (target_pose_fbs->id() < kMinTargetId ||
- target_pose_fbs->id() > kMaxTargetId) {
- LOG(WARNING) << "Skipping tag with invalid id of "
- << target_pose_fbs->id();
+ if (static_cast<TargetMapper::TargetId>(target_pose_fbs->id()) <
+ FLAGS_min_target_id ||
+ static_cast<TargetMapper::TargetId>(target_pose_fbs->id()) >
+ FLAGS_max_target_id) {
+ VLOG(1) << "Skipping tag with invalid id of " << target_pose_fbs->id();
continue;
}
// Skip detections with high pose errors
if (target_pose_fbs->pose_error() > FLAGS_max_pose_error) {
- VLOG(1) << " Skipping tag " << target_pose_fbs->id()
+ VLOG(1) << "Skipping tag " << target_pose_fbs->id()
<< " due to pose error of " << target_pose_fbs->pose_error();
continue;
}
// Skip detections with high pose error ratios
if (target_pose_fbs->pose_error_ratio() > FLAGS_max_pose_error_ratio) {
- VLOG(1) << " Skipping tag " << target_pose_fbs->id()
+ VLOG(1) << "Skipping tag " << target_pose_fbs->id()
<< " due to pose error ratio of "
<< target_pose_fbs->pose_error_ratio();
continue;
@@ -274,7 +274,7 @@
.distortion_factor = distortion_factor,
.id = static_cast<TargetMapper::TargetId>(target_pose.id)});
- if (FLAGS_visualize) {
+ if (FLAGS_visualize_solver) {
// If we've already drawn in the current image,
// display it before clearing and adding the new poses
if (drawn_nodes_.count(node_name) != 0) {
@@ -419,10 +419,13 @@
.pose),
.distance_from_camera = kSeedDistanceFromCamera,
.distortion_factor = kSeedDistortionFactor,
- .id = kMinTargetId};
+ .id = FLAGS_frozen_target_id};
- for (TargetMapper::TargetId id = kMinTargetId; id <= kMaxTargetId; id++) {
- if (id == static_cast<TargetMapper::TargetId>(FLAGS_frozen_target_id)) {
+ constexpr TargetMapper::TargetId kAbsMinTargetId = 1;
+ constexpr TargetMapper::TargetId kAbsMaxTargetId = 8;
+ for (TargetMapper::TargetId id = kAbsMinTargetId; id <= kAbsMaxTargetId;
+ id++) {
+ if (id == FLAGS_frozen_target_id) {
continue;
}
diff --git a/y2023/y2023_logger.json b/y2023/y2023_logger.json
index b0ece47..8c7f245 100644
--- a/y2023/y2023_logger.json
+++ b/y2023/y2023_logger.json
@@ -447,6 +447,14 @@
]
},
{
+ "name": "camera_monitor",
+ "executable_name": "camera_monitor",
+ "user": "pi",
+ "nodes": [
+ "logger"
+ ]
+ },
+ {
"name": "game_pieces_detector_starter",
"executable_name": "game_pieces_detector_starter.sh",
"autostart": true,
diff --git a/y2023/y2023_pi_template.json b/y2023/y2023_pi_template.json
index 46678f3..8167ba0 100644
--- a/y2023/y2023_pi_template.json
+++ b/y2023/y2023_pi_template.json
@@ -384,6 +384,14 @@
]
},
{
+ "name": "camera_monitor",
+ "executable_name": "camera_monitor",
+ "user": "pi",
+ "nodes": [
+ "pi{{ NUM }}"
+ ]
+ },
+ {
"name": "foxglove_websocket",
"user": "pi",
"nodes": [