Merge "Redid intrinsic calibration for comp robot cameras pi-971-1 to pi-971-4"
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/db/db.go b/scouting/db/db.go
index ca1af9e..031f54c 100644
--- a/scouting/db/db.go
+++ b/scouting/db/db.go
@@ -37,7 +37,9 @@
LowConesAuto, MiddleConesAuto, HighConesAuto, ConesDroppedAuto int32
LowCubes, MiddleCubes, HighCubes, CubesDropped int32
LowCones, MiddleCones, HighCones, ConesDropped int32
+ SuperchargedPieces int32
AvgCycle int64
+ Mobility bool
DockedAuto, EngagedAuto, BalanceAttemptAuto bool
Docked, Engaged, BalanceAttempt bool
@@ -64,7 +66,7 @@
Notes string
GoodDriving bool
BadDriving bool
- SolidPickup bool
+ SolidPlacing bool
SketchyPlacing bool
GoodDefense bool
BadDefense bool
@@ -308,7 +310,7 @@
Notes: data.Notes,
GoodDriving: data.GoodDriving,
BadDriving: data.BadDriving,
- SolidPickup: data.SolidPickup,
+ SolidPlacing: data.SolidPlacing,
SketchyPlacing: data.SketchyPlacing,
GoodDefense: data.GoodDefense,
BadDefense: data.BadDefense,
diff --git a/scouting/db/db_test.go b/scouting/db/db_test.go
index b0d34d8..3c345b0 100644
--- a/scouting/db/db_test.go
+++ b/scouting/db/db_test.go
@@ -150,8 +150,8 @@
LowConesAuto: 1, MiddleConesAuto: 0, HighConesAuto: 2,
ConesDroppedAuto: 0, LowCubes: 1, MiddleCubes: 2,
HighCubes: 1, CubesDropped: 0, LowCones: 0,
- MiddleCones: 2, HighCones: 1, ConesDropped: 1,
- AvgCycle: 0, DockedAuto: true, EngagedAuto: false,
+ MiddleCones: 2, HighCones: 1, ConesDropped: 1, SuperchargedPieces: 0,
+ AvgCycle: 0, Mobility: true, DockedAuto: true, EngagedAuto: false,
BalanceAttemptAuto: false, Docked: false, Engaged: false,
BalanceAttempt: false, CollectedBy: "emma",
},
@@ -162,8 +162,8 @@
LowConesAuto: 2, MiddleConesAuto: 0, HighConesAuto: 0,
ConesDroppedAuto: 1, LowCubes: 1, MiddleCubes: 0,
HighCubes: 0, CubesDropped: 1, LowCones: 0,
- MiddleCones: 0, HighCones: 1, ConesDropped: 0,
- AvgCycle: 0, DockedAuto: false, EngagedAuto: false,
+ MiddleCones: 0, HighCones: 1, ConesDropped: 0, SuperchargedPieces: 1,
+ AvgCycle: 0, Mobility: false, DockedAuto: false, EngagedAuto: false,
BalanceAttemptAuto: true, Docked: true, Engaged: true,
BalanceAttempt: false, CollectedBy: "tyler",
},
@@ -174,8 +174,8 @@
LowConesAuto: 0, MiddleConesAuto: 2, HighConesAuto: 1,
ConesDroppedAuto: 1, LowCubes: 0, MiddleCubes: 0,
HighCubes: 2, CubesDropped: 1, LowCones: 1,
- MiddleCones: 1, HighCones: 0, ConesDropped: 1,
- AvgCycle: 0, DockedAuto: false, EngagedAuto: false,
+ MiddleCones: 1, HighCones: 0, ConesDropped: 1, SuperchargedPieces: 0,
+ AvgCycle: 0, Mobility: true, DockedAuto: false, EngagedAuto: false,
BalanceAttemptAuto: false, Docked: false, Engaged: false,
BalanceAttempt: true, CollectedBy: "isaac",
},
@@ -186,8 +186,8 @@
LowConesAuto: 1, MiddleConesAuto: 0, HighConesAuto: 0,
ConesDroppedAuto: 0, LowCubes: 0, MiddleCubes: 1,
HighCubes: 2, CubesDropped: 1, LowCones: 0,
- MiddleCones: 1, HighCones: 0, ConesDropped: 0,
- AvgCycle: 0, DockedAuto: true, EngagedAuto: true,
+ MiddleCones: 1, HighCones: 0, ConesDropped: 0, SuperchargedPieces: 0,
+ AvgCycle: 0, Mobility: false, DockedAuto: true, EngagedAuto: true,
BalanceAttemptAuto: true, Docked: false, Engaged: false,
BalanceAttempt: true, CollectedBy: "will",
},
@@ -198,8 +198,8 @@
LowConesAuto: 0, MiddleConesAuto: 1, HighConesAuto: 1,
ConesDroppedAuto: 1, LowCubes: 1, MiddleCubes: 0,
HighCubes: 0, CubesDropped: 2, LowCones: 1,
- MiddleCones: 1, HighCones: 0, ConesDropped: 1,
- AvgCycle: 0, DockedAuto: true, EngagedAuto: false,
+ MiddleCones: 1, HighCones: 0, ConesDropped: 1, SuperchargedPieces: 0,
+ AvgCycle: 0, Mobility: true, DockedAuto: true, EngagedAuto: false,
BalanceAttemptAuto: false, Docked: true, Engaged: false,
BalanceAttempt: false, CollectedBy: "unkown",
},
@@ -248,8 +248,8 @@
LowConesAuto: 1, MiddleConesAuto: 0, HighConesAuto: 2,
ConesDroppedAuto: 0, LowCubes: 1, MiddleCubes: 2,
HighCubes: 1, CubesDropped: 0, LowCones: 0,
- MiddleCones: 2, HighCones: 1, ConesDropped: 1,
- AvgCycle: 0, DockedAuto: true, EngagedAuto: false,
+ MiddleCones: 2, HighCones: 1, ConesDropped: 1, SuperchargedPieces: 0,
+ AvgCycle: 0, Mobility: true, DockedAuto: true, EngagedAuto: false,
BalanceAttemptAuto: false, Docked: false, Engaged: false,
BalanceAttempt: false, CollectedBy: "emma",
},
@@ -260,8 +260,8 @@
LowConesAuto: 2, MiddleConesAuto: 0, HighConesAuto: 0,
ConesDroppedAuto: 1, LowCubes: 1, MiddleCubes: 0,
HighCubes: 0, CubesDropped: 1, LowCones: 0,
- MiddleCones: 0, HighCones: 1, ConesDropped: 0,
- AvgCycle: 0, DockedAuto: true, EngagedAuto: true,
+ MiddleCones: 0, HighCones: 1, ConesDropped: 0, SuperchargedPieces: 1,
+ AvgCycle: 0, Mobility: false, DockedAuto: true, EngagedAuto: true,
BalanceAttemptAuto: true, Docked: false, Engaged: false,
BalanceAttempt: false, CollectedBy: "tyler",
},
@@ -272,8 +272,8 @@
LowConesAuto: 1, MiddleConesAuto: 0, HighConesAuto: 2,
ConesDroppedAuto: 0, LowCubes: 1, MiddleCubes: 2,
HighCubes: 1, CubesDropped: 0, LowCones: 0,
- MiddleCones: 2, HighCones: 1, ConesDropped: 1,
- AvgCycle: 0, DockedAuto: true, EngagedAuto: false,
+ MiddleCones: 2, HighCones: 1, ConesDropped: 1, SuperchargedPieces: 0,
+ AvgCycle: 0, Mobility: true, DockedAuto: true, EngagedAuto: false,
BalanceAttemptAuto: false, Docked: true, Engaged: false,
BalanceAttempt: true, CollectedBy: "emma",
},
@@ -329,8 +329,8 @@
LowConesAuto: 0, MiddleConesAuto: 2, HighConesAuto: 0,
ConesDroppedAuto: 1, LowCubes: 0, MiddleCubes: 1,
HighCubes: 2, CubesDropped: 1, LowCones: 1,
- MiddleCones: 0, HighCones: 1, ConesDropped: 2,
- AvgCycle: 58, DockedAuto: false, EngagedAuto: false,
+ MiddleCones: 0, HighCones: 1, ConesDropped: 2, SuperchargedPieces: 0,
+ AvgCycle: 58, Mobility: true, DockedAuto: false, EngagedAuto: false,
BalanceAttemptAuto: true, Docked: true, Engaged: true,
BalanceAttempt: false, CollectedBy: "unknown",
},
@@ -341,8 +341,8 @@
LowConesAuto: 0, MiddleConesAuto: 1, HighConesAuto: 0,
ConesDroppedAuto: 0, LowCubes: 2, MiddleCubes: 0,
HighCubes: 1, CubesDropped: 0, LowCones: 0,
- MiddleCones: 2, HighCones: 1, ConesDropped: 0,
- AvgCycle: 34, DockedAuto: true, EngagedAuto: true,
+ MiddleCones: 2, HighCones: 1, ConesDropped: 0, SuperchargedPieces: 1,
+ AvgCycle: 34, Mobility: true, DockedAuto: true, EngagedAuto: true,
BalanceAttemptAuto: false, Docked: true, Engaged: false,
BalanceAttempt: false, CollectedBy: "simon",
},
@@ -353,8 +353,8 @@
LowConesAuto: 1, MiddleConesAuto: 0, HighConesAuto: 0,
ConesDroppedAuto: 1, LowCubes: 0, MiddleCubes: 2,
HighCubes: 0, CubesDropped: 0, LowCones: 2,
- MiddleCones: 0, HighCones: 1, ConesDropped: 1,
- AvgCycle: 50, DockedAuto: false, EngagedAuto: false,
+ MiddleCones: 0, HighCones: 1, ConesDropped: 1, SuperchargedPieces: 0,
+ AvgCycle: 50, Mobility: true, DockedAuto: false, EngagedAuto: false,
BalanceAttemptAuto: true, Docked: false, Engaged: false,
BalanceAttempt: false, CollectedBy: "eliza",
},
@@ -365,8 +365,8 @@
LowConesAuto: 0, MiddleConesAuto: 2, HighConesAuto: 0,
ConesDroppedAuto: 1, LowCubes: 0, MiddleCubes: 1,
HighCubes: 2, CubesDropped: 1, LowCones: 0,
- MiddleCones: 2, HighCones: 1, ConesDropped: 1,
- AvgCycle: 49, DockedAuto: true, EngagedAuto: false,
+ MiddleCones: 2, HighCones: 1, ConesDropped: 1, SuperchargedPieces: 0,
+ AvgCycle: 49, Mobility: false, DockedAuto: true, EngagedAuto: false,
Docked: false, Engaged: false, CollectedBy: "isaac",
},
Stats2023{
@@ -376,8 +376,8 @@
LowConesAuto: 1, MiddleConesAuto: 1, HighConesAuto: 0,
ConesDroppedAuto: 1, LowCubes: 1, MiddleCubes: 2,
HighCubes: 0, CubesDropped: 0, LowCones: 1,
- MiddleCones: 1, HighCones: 1, ConesDropped: 0,
- AvgCycle: 70, DockedAuto: true, EngagedAuto: true,
+ MiddleCones: 1, HighCones: 1, ConesDropped: 0, SuperchargedPieces: 0,
+ AvgCycle: 70, Mobility: false, DockedAuto: true, EngagedAuto: true,
BalanceAttemptAuto: false, Docked: false, Engaged: false,
BalanceAttempt: true, CollectedBy: "sam",
},
@@ -391,8 +391,8 @@
LowConesAuto: 1, MiddleConesAuto: 0, HighConesAuto: 0,
ConesDroppedAuto: 1, LowCubes: 0, MiddleCubes: 2,
HighCubes: 0, CubesDropped: 0, LowCones: 2,
- MiddleCones: 0, HighCones: 1, ConesDropped: 1,
- AvgCycle: 50, DockedAuto: false, EngagedAuto: false,
+ MiddleCones: 0, HighCones: 1, ConesDropped: 1, SuperchargedPieces: 0,
+ AvgCycle: 50, Mobility: true, DockedAuto: false, EngagedAuto: false,
BalanceAttemptAuto: true, Docked: false, Engaged: false,
BalanceAttempt: false, CollectedBy: "eliza",
},
@@ -403,8 +403,8 @@
LowConesAuto: 1, MiddleConesAuto: 1, HighConesAuto: 0,
ConesDroppedAuto: 1, LowCubes: 1, MiddleCubes: 2,
HighCubes: 0, CubesDropped: 0, LowCones: 1,
- MiddleCones: 1, HighCones: 1, ConesDropped: 0,
- AvgCycle: 70, DockedAuto: true, EngagedAuto: true,
+ MiddleCones: 1, HighCones: 1, ConesDropped: 0, SuperchargedPieces: 0,
+ AvgCycle: 70, Mobility: false, DockedAuto: true, EngagedAuto: true,
BalanceAttemptAuto: false, Docked: false, Engaged: false,
BalanceAttempt: true, CollectedBy: "sam",
},
@@ -683,8 +683,8 @@
LowConesAuto: 0, MiddleConesAuto: 0, HighConesAuto: 2,
ConesDroppedAuto: 1, LowCubes: 1, MiddleCubes: 2,
HighCubes: 1, CubesDropped: 0, LowCones: 2,
- MiddleCones: 0, HighCones: 2, ConesDropped: 1,
- AvgCycle: 51, DockedAuto: true, EngagedAuto: true,
+ MiddleCones: 0, HighCones: 2, ConesDropped: 1, SuperchargedPieces: 0,
+ AvgCycle: 51, Mobility: true, DockedAuto: true, EngagedAuto: true,
BalanceAttemptAuto: false, Docked: false, Engaged: false,
BalanceAttempt: true, CollectedBy: "isaac",
},
@@ -695,8 +695,8 @@
LowConesAuto: 1, MiddleConesAuto: 1, HighConesAuto: 0,
ConesDroppedAuto: 0, LowCubes: 2, MiddleCubes: 2,
HighCubes: 1, CubesDropped: 0, LowCones: 1,
- MiddleCones: 0, HighCones: 2, ConesDropped: 1,
- AvgCycle: 39, DockedAuto: false, EngagedAuto: false,
+ MiddleCones: 0, HighCones: 2, ConesDropped: 1, SuperchargedPieces: 1,
+ AvgCycle: 39, Mobility: false, DockedAuto: false, EngagedAuto: false,
BalanceAttemptAuto: true, Docked: false, Engaged: false,
BalanceAttempt: false, CollectedBy: "jack",
},
@@ -707,8 +707,8 @@
LowConesAuto: 2, MiddleConesAuto: 0, HighConesAuto: 0,
ConesDroppedAuto: 1, LowCubes: 2, MiddleCubes: 2,
HighCubes: 0, CubesDropped: 0, LowCones: 1,
- MiddleCones: 2, HighCones: 1, ConesDropped: 1,
- AvgCycle: 45, DockedAuto: true, EngagedAuto: false,
+ MiddleCones: 2, HighCones: 1, ConesDropped: 1, SuperchargedPieces: 0,
+ AvgCycle: 45, Mobility: false, DockedAuto: true, EngagedAuto: false,
BalanceAttemptAuto: true, Docked: false, Engaged: false,
BalanceAttempt: true, CollectedBy: "martin",
},
@@ -719,8 +719,8 @@
LowConesAuto: 2, MiddleConesAuto: 0, HighConesAuto: 0,
ConesDroppedAuto: 1, LowCubes: 2, MiddleCubes: 2,
HighCubes: 0, CubesDropped: 0, LowCones: 2,
- MiddleCones: 2, HighCones: 1, ConesDropped: 1,
- AvgCycle: 34, DockedAuto: true, EngagedAuto: false,
+ MiddleCones: 2, HighCones: 1, ConesDropped: 1, SuperchargedPieces: 0,
+ AvgCycle: 34, Mobility: true, DockedAuto: true, EngagedAuto: false,
BalanceAttemptAuto: false, Docked: true, Engaged: false,
BalanceAttempt: false, CollectedBy: "unknown",
},
@@ -875,11 +875,11 @@
expected := []string{"Note 1", "Note 3"}
- err := fixture.db.AddNotes(NotesData{TeamNumber: 1234, Notes: "Note 1", GoodDriving: true, BadDriving: false, SolidPickup: false, SketchyPlacing: true, GoodDefense: false, BadDefense: true, EasilyDefended: true})
+ err := fixture.db.AddNotes(NotesData{TeamNumber: 1234, Notes: "Note 1", GoodDriving: true, BadDriving: false, SolidPlacing: false, SketchyPlacing: true, GoodDefense: false, BadDefense: true, EasilyDefended: true})
check(t, err, "Failed to add Note")
- err = fixture.db.AddNotes(NotesData{TeamNumber: 1235, Notes: "Note 2", GoodDriving: false, BadDriving: true, SolidPickup: false, SketchyPlacing: true, GoodDefense: false, BadDefense: false, EasilyDefended: false})
+ err = fixture.db.AddNotes(NotesData{TeamNumber: 1235, Notes: "Note 2", GoodDriving: false, BadDriving: true, SolidPlacing: false, SketchyPlacing: true, GoodDefense: false, BadDefense: false, EasilyDefended: false})
check(t, err, "Failed to add Note")
- err = fixture.db.AddNotes(NotesData{TeamNumber: 1234, Notes: "Note 3", GoodDriving: true, BadDriving: false, SolidPickup: false, SketchyPlacing: true, GoodDefense: true, BadDefense: false, EasilyDefended: true})
+ err = fixture.db.AddNotes(NotesData{TeamNumber: 1234, Notes: "Note 3", GoodDriving: true, BadDriving: false, SolidPlacing: false, SketchyPlacing: true, GoodDefense: true, BadDefense: false, EasilyDefended: true})
check(t, err, "Failed to add Note")
actual, err := fixture.db.QueryNotes(1234)
diff --git a/scouting/scouting_test.cy.js b/scouting/scouting_test.cy.js
index 8ac1880..1d7620a 100644
--- a/scouting/scouting_test.cy.js
+++ b/scouting/scouting_test.cy.js
@@ -20,6 +20,17 @@
cy.get(fieldSelector).type('{selectAll}' + value);
}
+// Click on a random team in the Match list. The exact details here are not
+// important, but we need to know what they are. This could as well be any
+// other team from any other match.
+function clickSemiFinal2Match3Team5254() {
+ // On the 87th row of matches (index 86) click on the second team
+ // (index 1) which resolves to team 5254 in semi final 2 match 3.
+ cy.get('button.match-item')
+ .eq(86 * 6 + 1)
+ .click();
+}
+
// Moves the nth slider left or right. A positive "adjustBy" value moves the
// slider to the right. A negative value moves the slider to the left.
//
@@ -88,29 +99,26 @@
cy.get('.badge').eq(89).contains('Final 1 Match 3');
});
- it('should: prefill the match information.', () => {
- headerShouldBe('Matches');
+ it('should: be let users enter match information manually.', () => {
+ switchToTab('Entry');
+ headerShouldBe(' Team Selection ');
- // On the 87th row of matches (index 86) click on the second team
- // (index 1) which resolves to team 5254 in semi final 2 match 3.
- cy.get('button.match-item')
- .eq(86 * 6 + 1)
- .click();
+ setInputTo('#match_number', '3');
+ setInputTo('#team_number', '5254');
+ setInputTo('#set_number', '2');
+ setInputTo('#comp_level', '3: sf');
- 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');
+ 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.', () => {
- switchToTab('Data Entry');
- headerShouldBe('Team Selection');
- clickButton('Next');
+ clickSemiFinal2Match3Team5254();
// Select Starting Position.
+ headerShouldBe('5254 Init ');
cy.get('[type="radio"]').first().check();
clickButton('Start Match');
@@ -132,16 +140,14 @@
cy.get('[type="checkbox"]').check();
clickButton('End Match');
- headerShouldBe('Review and Submit');
+ headerShouldBe('5254 Review and Submit ');
clickButton('Submit');
- headerShouldBe('Success');
+ headerShouldBe('5254 Success ');
});
it('should: be able to return to correct screen with undo for pick and place.', () => {
- switchToTab('Data Entry');
- headerShouldBe('Team Selection');
- clickButton('Next');
+ clickSemiFinal2Match3Team5254();
// Select Starting Position.
cy.get('[type="radio"]').first().check();
@@ -154,13 +160,13 @@
clickButton('UNDO');
// User should be back on pickup screen.
- headerShouldBe('Pickup');
+ headerShouldBe('5254 Pickup ');
// Check the same thing but for undoing place.
clickButton('CUBE');
clickButton('MID');
clickButton('UNDO');
- headerShouldBe('Place');
+ headerShouldBe('5254 Place ');
});
it('should: submit note scouting for multiple teams', () => {
diff --git a/scouting/webserver/requests/debug/cli/cli_test.py b/scouting/webserver/requests/debug/cli/cli_test.py
index 0c2beab..b944564 100644
--- a/scouting/webserver/requests/debug/cli/cli_test.py
+++ b/scouting/webserver/requests/debug/cli/cli_test.py
@@ -105,7 +105,7 @@
"notes": "A very inspiring and useful comment",
"good_driving": True,
"bad_driving": False,
- "solid_pickup": False,
+ "solid_placing": False,
"sketchy_placing": True,
"good_defense": False,
"bad_defense": False,
@@ -128,7 +128,7 @@
Notes: (string) (len=35) "A very inspiring and useful comment",
GoodDriving: (bool) true,
BadDriving: (bool) false,
- SolidPickup: (bool) false,
+ SolidPlacing: (bool) false,
SketchyPlacing: (bool) true,
GoodDefense: (bool) false,
BadDefense: (bool) false,
diff --git a/scouting/webserver/requests/messages/request_2023_data_scouting_response.fbs b/scouting/webserver/requests/messages/request_2023_data_scouting_response.fbs
index b472c58..e532ffe 100644
--- a/scouting/webserver/requests/messages/request_2023_data_scouting_response.fbs
+++ b/scouting/webserver/requests/messages/request_2023_data_scouting_response.fbs
@@ -24,8 +24,11 @@
middle_cones:int (id:16);
high_cones:int (id:17);
cones_dropped:int (id:18);
+ supercharged_pieces:int (id:29);
// Time in nanoseconds.
avg_cycle: int64 (id:19);
+ // Did the robot leave its community during auto.
+ mobility: bool (id:30);
docked_auto: bool (id:20);
engaged_auto: bool (id:23);
balance_attempt_auto: bool (id:27);
diff --git a/scouting/webserver/requests/messages/request_all_notes_response.fbs b/scouting/webserver/requests/messages/request_all_notes_response.fbs
index 983ff62..70504d3 100644
--- a/scouting/webserver/requests/messages/request_all_notes_response.fbs
+++ b/scouting/webserver/requests/messages/request_all_notes_response.fbs
@@ -5,7 +5,7 @@
notes:string (id: 1);
good_driving:bool (id: 2);
bad_driving:bool (id: 3);
- solid_pickup:bool (id: 4);
+ solid_placing:bool (id: 4);
sketchy_placing:bool (id: 5);
good_defense:bool (id: 6);
bad_defense:bool (id: 7);
diff --git a/scouting/webserver/requests/messages/submit_actions.fbs b/scouting/webserver/requests/messages/submit_actions.fbs
index d8aa98d..a0f7913 100644
--- a/scouting/webserver/requests/messages/submit_actions.fbs
+++ b/scouting/webserver/requests/messages/submit_actions.fbs
@@ -12,7 +12,12 @@
enum ScoreLevel: short {
kLow,
kMiddle,
- kHigh
+ kHigh,
+ kSupercharged,
+}
+
+table MobilityAction {
+ mobility:bool (id:0);
}
table AutoBalanceAction {
@@ -43,6 +48,7 @@
}
union ActionType {
+ MobilityAction,
AutoBalanceAction,
StartMatchAction,
PickupObjectAction,
diff --git a/scouting/webserver/requests/messages/submit_notes.fbs b/scouting/webserver/requests/messages/submit_notes.fbs
index cee4dee..845a601 100644
--- a/scouting/webserver/requests/messages/submit_notes.fbs
+++ b/scouting/webserver/requests/messages/submit_notes.fbs
@@ -5,7 +5,7 @@
notes:string (id: 1);
good_driving:bool (id: 2);
bad_driving:bool (id: 3);
- solid_pickup:bool (id: 4);
+ solid_placing:bool (id: 4);
sketchy_placing:bool (id: 5);
good_defense:bool (id: 6);
bad_defense:bool (id: 7);
diff --git a/scouting/webserver/requests/requests.go b/scouting/webserver/requests/requests.go
index 8646a30..25bb6b7 100644
--- a/scouting/webserver/requests/requests.go
+++ b/scouting/webserver/requests/requests.go
@@ -355,7 +355,7 @@
Notes: string(request.Notes()),
GoodDriving: bool(request.GoodDriving()),
BadDriving: bool(request.BadDriving()),
- SolidPickup: bool(request.SolidPickup()),
+ SolidPlacing: bool(request.SolidPlacing()),
SketchyPlacing: bool(request.SketchyPlacing()),
GoodDefense: bool(request.GoodDefense()),
BadDefense: bool(request.BadDefense()),
@@ -380,7 +380,7 @@
stat := db.Stats2023{TeamNumber: string(submitActions.TeamNumber()), MatchNumber: submitActions.MatchNumber(), SetNumber: submitActions.SetNumber(), CompLevel: string(submitActions.CompLevel()),
StartingQuadrant: 0, LowCubesAuto: 0, MiddleCubesAuto: 0, HighCubesAuto: 0, CubesDroppedAuto: 0,
LowConesAuto: 0, MiddleConesAuto: 0, HighConesAuto: 0, ConesDroppedAuto: 0, LowCubes: 0, MiddleCubes: 0, HighCubes: 0,
- CubesDropped: 0, LowCones: 0, MiddleCones: 0, HighCones: 0, ConesDropped: 0, AvgCycle: 0, CollectedBy: string(submitActions.CollectedBy()),
+ CubesDropped: 0, LowCones: 0, MiddleCones: 0, HighCones: 0, ConesDropped: 0, SuperchargedPieces: 0, AvgCycle: 0, CollectedBy: string(submitActions.CollectedBy()),
}
// Loop over all actions.
for i := 0; i < submitActions.ActionsListLength(); i++ {
@@ -397,6 +397,13 @@
var startMatchAction submit_actions.StartMatchAction
startMatchAction.Init(actionTable.Bytes, actionTable.Pos)
stat.StartingQuadrant = startMatchAction.Position()
+ } else if action_type == submit_actions.ActionTypeMobilityAction {
+ var mobilityAction submit_actions.MobilityAction
+ mobilityAction.Init(actionTable.Bytes, actionTable.Pos)
+ if mobilityAction.Mobility() {
+ stat.Mobility = true
+ }
+
} else if action_type == submit_actions.ActionTypeAutoBalanceAction {
var autoBalanceAction submit_actions.AutoBalanceAction
autoBalanceAction.Init(actionTable.Bytes, actionTable.Pos)
@@ -460,6 +467,8 @@
stat.HighConesAuto += 1
} else if object == 1 && level == 2 && auto == false {
stat.HighCones += 1
+ } else if level == 3 {
+ stat.SuperchargedPieces += 1
} else {
return db.Stats2023{}, errors.New(fmt.Sprintf("Got unknown ObjectType/ScoreLevel/Auto combination"))
}
@@ -541,7 +550,9 @@
MiddleCones: stat.MiddleCones,
HighCones: stat.HighCones,
ConesDropped: stat.ConesDropped,
+ SuperchargedPieces: stat.SuperchargedPieces,
AvgCycle: stat.AvgCycle,
+ Mobility: stat.Mobility,
DockedAuto: stat.DockedAuto,
EngagedAuto: stat.EngagedAuto,
BalanceAttemptAuto: stat.BalanceAttemptAuto,
@@ -737,7 +748,7 @@
Notes: note.Notes,
GoodDriving: note.GoodDriving,
BadDriving: note.BadDriving,
- SolidPickup: note.SolidPickup,
+ SolidPlacing: note.SolidPlacing,
SketchyPlacing: note.SketchyPlacing,
GoodDefense: note.GoodDefense,
BadDefense: note.BadDefense,
diff --git a/scouting/webserver/requests/requests_test.go b/scouting/webserver/requests/requests_test.go
index ac644ea..c2bc750 100644
--- a/scouting/webserver/requests/requests_test.go
+++ b/scouting/webserver/requests/requests_test.go
@@ -129,8 +129,8 @@
LowConesAuto: 1, MiddleConesAuto: 2, HighConesAuto: 1,
ConesDroppedAuto: 0, LowCubes: 1, MiddleCubes: 1,
HighCubes: 2, CubesDropped: 1, LowCones: 1,
- MiddleCones: 2, HighCones: 0, ConesDropped: 1,
- AvgCycle: 34, DockedAuto: true, EngagedAuto: true,
+ MiddleCones: 2, HighCones: 0, ConesDropped: 1, SuperchargedPieces: 0,
+ AvgCycle: 34, Mobility: false, DockedAuto: true, EngagedAuto: true,
BalanceAttemptAuto: false, Docked: false, Engaged: false,
BalanceAttempt: false, CollectedBy: "alex",
},
@@ -141,8 +141,8 @@
LowConesAuto: 0, MiddleConesAuto: 0, HighConesAuto: 0,
ConesDroppedAuto: 1, LowCubes: 0, MiddleCubes: 0,
HighCubes: 1, CubesDropped: 0, LowCones: 0,
- MiddleCones: 2, HighCones: 1, ConesDropped: 1,
- AvgCycle: 53, DockedAuto: true, EngagedAuto: false,
+ MiddleCones: 2, HighCones: 1, ConesDropped: 1, SuperchargedPieces: 0,
+ AvgCycle: 53, Mobility: true, DockedAuto: true, EngagedAuto: false,
BalanceAttemptAuto: false, Docked: false, Engaged: false,
BalanceAttempt: true, CollectedBy: "bob",
},
@@ -214,8 +214,8 @@
LowConesAuto: 1, MiddleConesAuto: 2, HighConesAuto: 1,
ConesDroppedAuto: 0, LowCubes: 1, MiddleCubes: 1,
HighCubes: 2, CubesDropped: 1, LowCones: 1,
- MiddleCones: 2, HighCones: 0, ConesDropped: 1,
- AvgCycle: 34, DockedAuto: true, EngagedAuto: false,
+ MiddleCones: 2, HighCones: 0, ConesDropped: 1, SuperchargedPieces: 0,
+ AvgCycle: 34, Mobility: false, DockedAuto: true, EngagedAuto: false,
BalanceAttemptAuto: false, Docked: false, Engaged: false,
BalanceAttempt: true, CollectedBy: "isaac",
},
@@ -226,8 +226,8 @@
LowConesAuto: 0, MiddleConesAuto: 0, HighConesAuto: 0,
ConesDroppedAuto: 1, LowCubes: 0, MiddleCubes: 0,
HighCubes: 1, CubesDropped: 0, LowCones: 0,
- MiddleCones: 2, HighCones: 1, ConesDropped: 1,
- AvgCycle: 53, DockedAuto: false, EngagedAuto: false,
+ MiddleCones: 2, HighCones: 1, ConesDropped: 1, SuperchargedPieces: 0,
+ AvgCycle: 53, Mobility: false, DockedAuto: false, EngagedAuto: false,
BalanceAttemptAuto: true, Docked: false, Engaged: false,
BalanceAttempt: true, CollectedBy: "unknown",
},
@@ -255,8 +255,8 @@
LowConesAuto: 1, MiddleConesAuto: 2, HighConesAuto: 1,
ConesDroppedAuto: 0, LowCubes: 1, MiddleCubes: 1,
HighCubes: 2, CubesDropped: 1, LowCones: 1,
- MiddleCones: 2, HighCones: 0, ConesDropped: 1,
- AvgCycle: 34, DockedAuto: true, EngagedAuto: false,
+ MiddleCones: 2, HighCones: 0, ConesDropped: 1, SuperchargedPieces: 0,
+ AvgCycle: 34, Mobility: false, DockedAuto: true, EngagedAuto: false,
BalanceAttemptAuto: false, Docked: false, Engaged: false,
BalanceAttempt: true, CollectedBy: "isaac",
},
@@ -267,8 +267,8 @@
LowConesAuto: 0, MiddleConesAuto: 0, HighConesAuto: 0,
ConesDroppedAuto: 1, LowCubes: 0, MiddleCubes: 0,
HighCubes: 1, CubesDropped: 0, LowCones: 0,
- MiddleCones: 2, HighCones: 1, ConesDropped: 1,
- AvgCycle: 53, DockedAuto: false, EngagedAuto: false,
+ MiddleCones: 2, HighCones: 1, ConesDropped: 1, SuperchargedPieces: 0,
+ AvgCycle: 53, Mobility: false, DockedAuto: false, EngagedAuto: false,
BalanceAttemptAuto: true, Docked: false, Engaged: false,
BalanceAttempt: true, CollectedBy: "unknown",
},
@@ -336,6 +336,15 @@
},
{
ActionTaken: &submit_actions.ActionTypeT{
+ Type: submit_actions.ActionTypeMobilityAction,
+ Value: &submit_actions.MobilityActionT{
+ Mobility: true,
+ },
+ },
+ Timestamp: 2200,
+ },
+ {
+ ActionTaken: &submit_actions.ActionTypeT{
Type: submit_actions.ActionTypeAutoBalanceAction,
Value: &submit_actions.AutoBalanceActionT{
Docked: true,
@@ -368,6 +377,27 @@
},
{
ActionTaken: &submit_actions.ActionTypeT{
+ Type: submit_actions.ActionTypePickupObjectAction,
+ Value: &submit_actions.PickupObjectActionT{
+ ObjectType: submit_actions.ObjectTypekCube,
+ Auto: false,
+ },
+ },
+ Timestamp: 3500,
+ },
+ {
+ ActionTaken: &submit_actions.ActionTypeT{
+ Type: submit_actions.ActionTypePlaceObjectAction,
+ Value: &submit_actions.PlaceObjectActionT{
+ ObjectType: submit_actions.ObjectTypekCube,
+ ScoreLevel: submit_actions.ScoreLevelkSupercharged,
+ Auto: false,
+ },
+ },
+ Timestamp: 3900,
+ },
+ {
+ ActionTaken: &submit_actions.ActionTypeT{
Type: submit_actions.ActionTypeEndMatchAction,
Value: &submit_actions.EndMatchActionT{
Docked: true,
@@ -375,7 +405,7 @@
BalanceAttempt: true,
},
},
- Timestamp: 4000,
+ Timestamp: 4200,
},
},
}).Pack(builder))
@@ -394,8 +424,8 @@
LowConesAuto: 0, MiddleConesAuto: 0, HighConesAuto: 0,
ConesDroppedAuto: 0, LowCubes: 0, MiddleCubes: 0,
HighCubes: 0, CubesDropped: 0, LowCones: 0,
- MiddleCones: 0, HighCones: 1, ConesDropped: 0,
- AvgCycle: 1100, DockedAuto: true, EngagedAuto: true,
+ MiddleCones: 0, HighCones: 1, ConesDropped: 0, SuperchargedPieces: 1,
+ AvgCycle: 950, Mobility: true, DockedAuto: true, EngagedAuto: true,
BalanceAttemptAuto: false, Docked: true, Engaged: false,
BalanceAttempt: true, CollectedBy: "katie",
}
@@ -418,7 +448,7 @@
Notes: "Notes",
GoodDriving: true,
BadDriving: false,
- SolidPickup: true,
+ SolidPlacing: true,
SketchyPlacing: false,
GoodDefense: true,
BadDefense: false,
@@ -436,7 +466,7 @@
Notes: "Notes",
GoodDriving: true,
BadDriving: false,
- SolidPickup: true,
+ SolidPlacing: true,
SketchyPlacing: false,
GoodDefense: true,
BadDefense: false,
@@ -456,7 +486,7 @@
Notes: "Notes",
GoodDriving: true,
BadDriving: false,
- SolidPickup: true,
+ SolidPlacing: true,
SketchyPlacing: false,
GoodDefense: true,
BadDefense: false,
@@ -684,7 +714,7 @@
Notes: "Notes",
GoodDriving: true,
BadDriving: false,
- SolidPickup: true,
+ SolidPlacing: true,
SketchyPlacing: false,
GoodDefense: true,
BadDefense: false,
@@ -695,7 +725,7 @@
Notes: "More Notes",
GoodDriving: false,
BadDriving: false,
- SolidPickup: false,
+ SolidPlacing: false,
SketchyPlacing: true,
GoodDefense: false,
BadDefense: true,
@@ -723,7 +753,7 @@
Notes: "Notes",
GoodDriving: true,
BadDriving: false,
- SolidPickup: true,
+ SolidPlacing: true,
SketchyPlacing: false,
GoodDefense: true,
BadDefense: false,
@@ -734,7 +764,7 @@
Notes: "More Notes",
GoodDriving: false,
BadDriving: false,
- SolidPickup: false,
+ SolidPlacing: false,
SketchyPlacing: true,
GoodDefense: false,
BadDefense: true,
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..8992ed8 100644
--- a/scouting/www/entry/entry.component.ts
+++ b/scouting/www/entry/entry.component.ts
@@ -15,6 +15,7 @@
ScoreLevel,
SubmitActions,
StartMatchAction,
+ MobilityAction,
AutoBalanceAction,
PickupObjectAction,
PlaceObjectAction,
@@ -54,6 +55,11 @@
position: number;
}
| {
+ type: 'mobilityAction';
+ timestamp?: number;
+ mobility: boolean;
+ }
+ | {
type: 'autoBalanceAction';
timestamp?: number;
docked: boolean;
@@ -97,7 +103,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 +112,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 +125,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.
@@ -199,22 +211,18 @@
startMatchActionOffset
);
break;
-
- case 'pickupObjectAction':
- const pickupObjectActionOffset =
- PickupObjectAction.createPickupObjectAction(
- builder,
- action.objectType,
- action.auto || false
- );
+ case 'mobilityAction':
+ const mobilityActionOffset = MobilityAction.createMobilityAction(
+ builder,
+ action.mobility
+ );
actionOffset = Action.createAction(
builder,
BigInt(action.timestamp || 0),
- ActionType.PickupObjectAction,
- pickupObjectActionOffset
+ ActionType.MobilityAction,
+ mobilityActionOffset
);
break;
-
case 'autoBalanceAction':
const autoBalanceActionOffset =
AutoBalanceAction.createAutoBalanceAction(
@@ -231,6 +239,20 @@
);
break;
+ case 'pickupObjectAction':
+ const pickupObjectActionOffset =
+ PickupObjectAction.createPickupObjectAction(
+ builder,
+ action.objectType,
+ action.auto || false
+ );
+ actionOffset = Action.createAction(
+ builder,
+ BigInt(action.timestamp || 0),
+ ActionType.PickupObjectAction,
+ pickupObjectActionOffset
+ );
+ break;
case 'placeObjectAction':
const placeObjectActionOffset =
PlaceObjectAction.createPlaceObjectAction(
diff --git a/scouting/www/entry/entry.ng.html b/scouting/www/entry/entry.ng.html
index e304f2a..98e427b 100644
--- a/scouting/www/entry/entry.ng.html
+++ b/scouting/www/entry/entry.ng.html
@@ -1,5 +1,8 @@
<div class="header" #header>
- <h2>{{section}}</h2>
+ <h2>
+ <span *ngIf="section != 'Team Selection'">{{teamNumber}}</span>
+ {{section}}
+ </h2>
</div>
<ng-container [ngSwitch]="section">
<div
@@ -89,7 +92,13 @@
<h6 class="text-muted">
Last Action: {{actionList[actionList.length - 1].type}}
</h6>
- <div class="d-grid gap-5">
+ <!--
+ Decrease distance between buttons during auto to make space for auto balancing
+ selection and keep all buttons visible without scrolling on most devices.
+ -->
+ <div
+ [ngClass]="{'d-grid': true, 'gap-3': autoPhase === true, 'gap-5': autoPhase === false}"
+ >
<button class="btn btn-secondary" (click)="undoLastAction()">UNDO</button>
<button
class="btn btn-danger"
@@ -109,6 +118,13 @@
>
CUBE
</button>
+ <button
+ *ngIf="autoPhase"
+ class="btn btn-light"
+ (click)="addAction({type: 'mobilityAction', mobility: true});"
+ >
+ Mobility
+ </button>
<!-- 'Balancing' during auto. -->
<div *ngIf="autoPhase" class="d-grid gap-2">
<label>
@@ -126,14 +142,14 @@
<br />
<button
class="btn btn-info"
- (click)="addAction({type: 'autoBalanceAction', docked: dockedValue, engaged: engagedValue, balanceAttempt: attempted.checked});"
+ (click)="addAction({type: 'autoBalanceAction', docked: docked.checked, engaged: engaged.checked, balanceAttempt: attempted.checked});"
>
Submit Balancing
</button>
</div>
<button
*ngIf="autoPhase"
- class="btn btn-info"
+ class="btn btn-dark"
(click)="autoPhase = false; addAction({type: 'endAutoPhase'});"
>
Start Teleop
@@ -151,7 +167,13 @@
<h6 class="text-muted">
Last Action: {{actionList[actionList.length - 1].type}}
</h6>
- <div class="d-grid gap-5">
+ <!--
+ Decrease distance between buttons during auto to make space for auto balancing
+ selection and keep all buttons visible without scrolling on most devices.
+ -->
+ <div
+ [ngClass]="{'d-grid': true, 'gap-3': autoPhase === true, 'gap-5': autoPhase === false}"
+ >
<button class="btn btn-secondary" (click)="undoLastAction()">UNDO</button>
<button
class="btn btn-danger"
@@ -177,8 +199,24 @@
>
LOW
</button>
+ <button
+ *ngIf="autoPhase"
+ class="btn btn-light"
+ (click)="addAction({type: 'mobilityAction', mobility: true});"
+ >
+ Mobility
+ </button>
+ <!-- Impossible to place supercharged pieces in auto. -->
+ <div *ngIf="autoPhase == false" class="d-grid gap-2">
+ <button
+ class="btn btn-dark"
+ (click)="changeSectionTo('Pickup'); addAction({type: 'placeObjectAction', scoreLevel: ScoreLevel.kSupercharged});"
+ >
+ SUPERCHARGED
+ </button>
+ </div>
<!-- 'Balancing' during auto. -->
- <div *ngIf="autoPhase" class="d-grid gap-2">
+ <div *ngIf="autoPhase" class="d-grid gap-1">
<label>
<input #docked type="checkbox" />
Docked
@@ -194,14 +232,14 @@
<br />
<button
class="btn btn-info"
- (click)="addAction({type: 'autoBalanceAction', docked: dockedValue, engaged: engagedValue, balanceAttempt: attempted.checked});"
+ (click)="addAction({type: 'autoBalanceAction', docked: docked.checked, engaged: engaged.checked, balanceAttempt: attempted.checked});"
>
Submit Balancing
</button>
</div>
<button
*ngIf="autoPhase"
- class="btn btn-info"
+ class="btn btn-dark"
(click)="autoPhase = false; addAction({type: 'endAutoPhase'});"
>
Start Teleop
diff --git a/scouting/www/notes/notes.component.ts b/scouting/www/notes/notes.component.ts
index 5d1c4a2..2786717 100644
--- a/scouting/www/notes/notes.component.ts
+++ b/scouting/www/notes/notes.component.ts
@@ -40,7 +40,7 @@
interface Keywords {
goodDriving: boolean;
badDriving: boolean;
- solidPickup: boolean;
+ solidPlacing: boolean;
sketchyPlacing: boolean;
goodDefense: boolean;
badDefense: boolean;
@@ -56,7 +56,7 @@
const KEYWORD_CHECKBOX_LABELS = {
goodDriving: 'Good Driving',
badDriving: 'Bad Driving',
- solidPickup: 'Solid Pickup',
+ solidPlacing: 'Solid Placing',
sketchyPlacing: 'Sketchy Placing',
goodDefense: 'Good Defense',
badDefense: 'Bad Defense',
@@ -115,7 +115,7 @@
keywordsData: {
goodDriving: false,
badDriving: false,
- solidPickup: false,
+ solidPlacing: false,
sketchyPlacing: false,
goodDefense: false,
badDefense: false,
@@ -152,7 +152,7 @@
dataFb,
this.newData[i].keywordsData.goodDriving,
this.newData[i].keywordsData.badDriving,
- this.newData[i].keywordsData.solidPickup,
+ this.newData[i].keywordsData.solidPlacing,
this.newData[i].keywordsData.sketchyPlacing,
this.newData[i].keywordsData.goodDefense,
this.newData[i].keywordsData.badDefense,
diff --git a/scouting/www/view/view.component.ts b/scouting/www/view/view.component.ts
index f7d6ce8..561ae08 100644
--- a/scouting/www/view/view.component.ts
+++ b/scouting/www/view/view.component.ts
@@ -171,8 +171,8 @@
if (entry.badDriving()) {
parsedKeywords += 'Bad Driving ';
}
- if (entry.solidPickup()) {
- parsedKeywords += 'Solid Pickup ';
+ if (entry.solidPlacing()) {
+ parsedKeywords += 'Solid Placing ';
}
if (entry.sketchyPlacing()) {
parsedKeywords += 'Sketchy Placing ';
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/control_loops/python/graph_edit.py b/y2023/control_loops/python/graph_edit.py
index 99b09af..f3035f1 100644
--- a/y2023/control_loops/python/graph_edit.py
+++ b/y2023/control_loops/python/graph_edit.py
@@ -283,6 +283,8 @@
# Lets you only view selected path
self.view_current = False
+ self.previous_segment = None
+
self.editing = True
self.x_offset = 0
@@ -539,6 +541,13 @@
self.theta_version = not self.theta_version
self.init_extents()
+ def undo(self):
+ if self.previous_segment is not None:
+ if self.edit_control1:
+ self.segments[self.index].control1 = self.previous_segment
+ else:
+ self.segments[self.index].control2 = self.previous_segment
+
def do_key_press(self, event):
keyval = Gdk.keyval_to_lower(event.keyval)
print("Gdk.KEY_" + Gdk.keyval_name(keyval))
@@ -562,6 +571,9 @@
if self.segments:
print(repr(self.segments[self.index].ToThetaPoints()))
+ elif keyval == Gdk.KEY_u:
+ self.undo()
+
elif keyval == Gdk.KEY_p:
if self.index > 0:
self.index -= 1
@@ -628,11 +640,16 @@
return
self.now_segment_pt = np.array(shift_angles(pt_theta))
-
if self.editing:
if self.edit_control1:
+ if (self.now_segment_pt !=
+ self.segments[self.index].control1).any():
+ self.previous_segment = self.segments[self.index].control1
self.segments[self.index].control1 = self.now_segment_pt
else:
+ if (self.now_segment_pt !=
+ self.segments[self.index].control2).any():
+ self.previous_segment = self.segments[self.index].control2
self.segments[self.index].control2 = self.now_segment_pt
print('Clicked at theta: np.array([%s, %s])' %
@@ -700,6 +717,9 @@
label="Toggle Control Point Indicators")
self.indicator_button.connect('clicked', self.on_button_click)
+ self.undo_button = Gtk.Button(label="Undo")
+ self.undo_button.connect('clicked', self.on_button_click)
+
self.box = Gtk.Box(spacing=6)
self.grid.attach(self.box, 0, 0, 1, 1)
@@ -713,6 +733,7 @@
self.box.pack_start(self.theta_button, False, False, 0)
self.box.pack_start(self.editing_button, False, False, 0)
self.box.pack_start(self.indicator_button, False, False, 0)
+ self.box.pack_start(self.undo_button, False, False, 0)
def on_combo_changed(self, combo):
iter = combo.get_active_iter()
@@ -743,6 +764,8 @@
self.arm_draw.editing = not self.arm_draw.editing
elif self.indicator_button == button:
self.arm_draw.show_indicators = not self.arm_draw.show_indicators
+ elif self.undo_button == button:
+ self.arm_draw.undo()
self.arm_draw.queue_draw()
diff --git a/y2023/control_loops/python/graph_paths.py b/y2023/control_loops/python/graph_paths.py
index 93134c7..1efcd93 100644
--- a/y2023/control_loops/python/graph_paths.py
+++ b/y2023/control_loops/python/graph_paths.py
@@ -329,7 +329,7 @@
))
points['HPPickupBackConeUp'] = to_theta_with_circular_index_and_roll(
- -1.1200539, 1.330, np.pi / 2.0, circular_index=0)
+ -1.1200539, 1.325, np.pi / 2.0, circular_index=0)
named_segments.append(
ThetaSplineSegment(
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_imu.json b/y2023/y2023_imu.json
index 5aae0a9..1982c6e 100644
--- a/y2023/y2023_imu.json
+++ b/y2023/y2023_imu.json
@@ -7,7 +7,11 @@
"frequency": 100,
"logger": "LOCAL_AND_REMOTE_LOGGER",
"logger_nodes": [
- "imu"
+ "logger",
+ "pi1",
+ "pi2",
+ "pi3",
+ "pi4"
],
"destination_nodes": [
{
@@ -121,67 +125,16 @@
"name": "/imu/aos",
"type": "aos.starter.Status",
"source_node": "imu",
- "logger": "LOCAL_AND_REMOTE_LOGGER",
"frequency": 50,
"num_senders": 20,
- "max_size": 2048,
- "logger_nodes": [
- "logger"
- ],
- "destination_nodes": [
- {
- "name": "logger",
- "priority": 5,
- "time_to_live": 5000000,
- "timestamp_logger": "LOCAL_AND_REMOTE_LOGGER",
- "timestamp_logger_nodes": [
- "imu"
- ]
- }
- ]
- },
- {
- "name": "/imu/aos/remote_timestamps/roborio/imu/aos/aos-starter-Status",
- "type": "aos.message_bridge.RemoteMessage",
- "frequency": 100,
- "source_node": "imu",
- "max_size": 208
- },
- {
- "name": "/imu/aos/remote_timestamps/logger/imu/aos/aos-starter-Status",
- "type": "aos.message_bridge.RemoteMessage",
- "frequency": 100,
- "source_node": "imu",
- "max_size": 208
+ "max_size": 2048
},
{
"name": "/imu/aos",
"type": "aos.starter.StarterRpc",
"source_node": "imu",
"frequency": 10,
- "num_senders": 2,
- "logger": "LOCAL_AND_REMOTE_LOGGER",
- "logger_nodes": [
- "logger"
- ],
- "destination_nodes": [
- {
- "name": "logger",
- "priority": 5,
- "time_to_live": 5000000,
- "timestamp_logger": "LOCAL_AND_REMOTE_LOGGER",
- "timestamp_logger_nodes": [
- "imu"
- ]
- }
- ]
- },
- {
- "name": "/imu/aos/remote_timestamps/logger/imu/aos/aos-starter-StarterRpc",
- "type": "aos.message_bridge.RemoteMessage",
- "frequency": 20,
- "source_node": "imu",
- "max_size": 208
+ "num_senders": 2
},
{
"name": "/imu/aos",
@@ -252,64 +205,6 @@
"max_size": 208
},
{
- "name": "/logger/aos",
- "type": "aos.starter.StarterRpc",
- "source_node": "logger",
- "logger": "LOCAL_AND_REMOTE_LOGGER",
- "logger_nodes": [
- "imu"
- ],
- "destination_nodes": [
- {
- "name": "imu",
- "priority": 5,
- "timestamp_logger": "LOCAL_AND_REMOTE_LOGGER",
- "timestamp_logger_nodes": [
- "logger"
- ],
- "time_to_live": 5000000
- }
- ]
- },
- {
- "name": "/logger/aos/remote_timestamps/imu/logger/aos/aos-starter-StarterRpc",
- "type": "aos.message_bridge.RemoteMessage",
- "source_node": "logger",
- "logger": "NOT_LOGGED",
- "frequency": 20,
- "num_senders": 2,
- "max_size": 200
- },
- {
- "name": "/logger/aos",
- "type": "aos.starter.Status",
- "source_node": "logger",
- "logger": "LOCAL_AND_REMOTE_LOGGER",
- "logger_nodes": [
- "imu"
- ],
- "destination_nodes": [
- {
- "name": "imu",
- "priority": 5,
- "timestamp_logger": "LOCAL_AND_REMOTE_LOGGER",
- "timestamp_logger_nodes": [
- "logger"
- ],
- "time_to_live": 5000000
- }
- ]
- },
- {
- "name": "/logger/aos/remote_timestamps/imu/logger/aos/aos-starter-Status",
- "type": "aos.message_bridge.RemoteMessage",
- "source_node": "logger",
- "logger": "NOT_LOGGED",
- "frequency": 50,
- "num_senders": 2,
- "max_size": 200
- },
- {
"name": "/roborio/aos",
"type": "aos.starter.StarterRpc",
"source_node": "roborio",
diff --git a/y2023/y2023_logger.json b/y2023/y2023_logger.json
index b0ece47..47977ad 100644
--- a/y2023/y2023_logger.json
+++ b/y2023/y2023_logger.json
@@ -53,86 +53,6 @@
"max_size": 200
},
{
- "name": "/pi1/aos",
- "type": "aos.message_bridge.Timestamp",
- "source_node": "pi1",
- "logger": "LOCAL_AND_REMOTE_LOGGER",
- "logger_nodes": [
- "logger"
- ],
- "destination_nodes": [
- {
- "name": "logger",
- "priority": 1,
- "time_to_live": 5000000,
- "timestamp_logger": "LOCAL_AND_REMOTE_LOGGER",
- "timestamp_logger_nodes": [
- "pi1"
- ]
- }
- ]
- },
- {
- "name": "/pi2/aos",
- "type": "aos.message_bridge.Timestamp",
- "source_node": "pi2",
- "logger": "LOCAL_AND_REMOTE_LOGGER",
- "logger_nodes": [
- "logger"
- ],
- "destination_nodes": [
- {
- "name": "logger",
- "priority": 1,
- "time_to_live": 5000000,
- "timestamp_logger": "LOCAL_AND_REMOTE_LOGGER",
- "timestamp_logger_nodes": [
- "pi2"
- ]
- }
- ]
- },
- {
- "name": "/pi3/aos",
- "type": "aos.message_bridge.Timestamp",
- "source_node": "pi3",
- "logger": "LOCAL_AND_REMOTE_LOGGER",
- "logger_nodes": [
- "logger"
- ],
- "destination_nodes": [
- {
- "name": "logger",
- "priority": 1,
- "timestamp_logger": "LOCAL_AND_REMOTE_LOGGER",
- "timestamp_logger_nodes": [
- "pi3"
- ],
- "time_to_live": 5000000
- }
- ]
- },
- {
- "name": "/pi4/aos",
- "type": "aos.message_bridge.Timestamp",
- "source_node": "pi4",
- "logger": "LOCAL_AND_REMOTE_LOGGER",
- "logger_nodes": [
- "logger"
- ],
- "destination_nodes": [
- {
- "name": "logger",
- "priority": 1,
- "timestamp_logger": "LOCAL_AND_REMOTE_LOGGER",
- "timestamp_logger_nodes": [
- "pi4"
- ],
- "time_to_live": 5000000
- }
- ]
- },
- {
"name": "/logger/aos",
"type": "aos.timing.Report",
"source_node": "logger",
@@ -194,50 +114,10 @@
"logger": "LOCAL_AND_REMOTE_LOGGER",
"logger_nodes": [
"imu",
- "pi1",
- "pi2",
- "pi3",
- "pi4",
"roborio"
],
"destination_nodes": [
{
- "name": "pi1",
- "priority": 1,
- "time_to_live": 5000000,
- "timestamp_logger": "LOCAL_AND_REMOTE_LOGGER",
- "timestamp_logger_nodes": [
- "logger"
- ]
- },
- {
- "name": "pi2",
- "priority": 1,
- "time_to_live": 5000000,
- "timestamp_logger": "LOCAL_AND_REMOTE_LOGGER",
- "timestamp_logger_nodes": [
- "logger"
- ]
- },
- {
- "name": "pi3",
- "priority": 1,
- "time_to_live": 5000000,
- "timestamp_logger": "LOCAL_AND_REMOTE_LOGGER",
- "timestamp_logger_nodes": [
- "logger"
- ]
- },
- {
- "name": "pi4",
- "priority": 1,
- "time_to_live": 5000000,
- "timestamp_logger": "LOCAL_AND_REMOTE_LOGGER",
- "timestamp_logger_nodes": [
- "logger"
- ]
- },
- {
"name": "imu",
"priority": 1,
"time_to_live": 5000000,
@@ -267,42 +147,6 @@
"max_size": 200
},
{
- "name": "/logger/aos/remote_timestamps/pi1/logger/aos/aos-message_bridge-Timestamp",
- "type": "aos.message_bridge.RemoteMessage",
- "source_node": "logger",
- "logger": "NOT_LOGGED",
- "frequency": 20,
- "num_senders": 2,
- "max_size": 200
- },
- {
- "name": "/logger/aos/remote_timestamps/pi2/logger/aos/aos-message_bridge-Timestamp",
- "type": "aos.message_bridge.RemoteMessage",
- "source_node": "logger",
- "logger": "NOT_LOGGED",
- "frequency": 20,
- "num_senders": 2,
- "max_size": 200
- },
- {
- "name": "/logger/aos/remote_timestamps/pi3/logger/aos/aos-message_bridge-Timestamp",
- "type": "aos.message_bridge.RemoteMessage",
- "source_node": "logger",
- "logger": "NOT_LOGGED",
- "frequency": 20,
- "num_senders": 2,
- "max_size": 200
- },
- {
- "name": "/logger/aos/remote_timestamps/pi4/logger/aos/aos-message_bridge-Timestamp",
- "type": "aos.message_bridge.RemoteMessage",
- "source_node": "logger",
- "logger": "NOT_LOGGED",
- "frequency": 20,
- "num_senders": 2,
- "max_size": 200
- },
- {
"name": "/logger/aos/remote_timestamps/roborio/logger/aos/aos-message_bridge-Timestamp",
"type": "aos.message_bridge.RemoteMessage",
"source_node": "logger",
@@ -333,22 +177,6 @@
"num_senders": 18
},
{
- "name": "/localizer",
- "type": "frc971.IMUValuesBatch",
- "source_node": "imu",
- "logger": "LOCAL_AND_REMOTE_LOGGER",
- "logger_nodes": [
- "logger"
- ],
- "destination_nodes": [
- {
- "name": "logger",
- "priority": 3,
- "time_to_live": 500000000
- }
- ]
- },
- {
"name": "/logger/constants",
"type": "y2023.Constants",
"source_node": "logger",
@@ -447,6 +275,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,
@@ -468,21 +304,9 @@
"port": 9971
},
{
- "name": "pi1"
- },
- {
- "name": "pi2"
- },
- {
- "name": "pi3"
- },
- {
"name": "imu"
},
{
- "name": "pi4"
- },
- {
"name": "roborio"
}
]
diff --git a/y2023/y2023_pi_template.json b/y2023/y2023_pi_template.json
index 46678f3..eee4ebb 100644
--- a/y2023/y2023_pi_template.json
+++ b/y2023/y2023_pi_template.json
@@ -21,51 +21,14 @@
"source_node": "pi{{ NUM }}",
"frequency": 50,
"num_senders": 20,
- "max_size": 2000,
- "logger": "LOCAL_AND_REMOTE_LOGGER",
- "logger_nodes": [
- "logger"
- ],
- "destination_nodes": [
- {
- "name": "logger",
- "priority": 5,
- "time_to_live": 5000000,
- "timestamp_logger": "LOCAL_AND_REMOTE_LOGGER",
- "timestamp_logger_nodes": [
- "pi{{ NUM }}"
- ]
- }
- ]
+ "max_size": 2000
},
{
"name": "/pi{{ NUM }}/aos",
"type": "aos.starter.StarterRpc",
"source_node": "pi{{ NUM }}",
"frequency": 10,
- "num_senders": 2,
- "logger": "LOCAL_AND_REMOTE_LOGGER",
- "logger_nodes": [
- "logger"
- ],
- "destination_nodes": [
- {
- "name": "logger",
- "priority": 5,
- "time_to_live": 5000000,
- "timestamp_logger": "LOCAL_AND_REMOTE_LOGGER",
- "timestamp_logger_nodes": [
- "pi{{ NUM }}"
- ]
- }
- ]
- },
- {
- "name": "/pi{{ NUM }}/aos/remote_timestamps/logger/pi{{ NUM }}/aos/aos-starter-StarterRpc",
- "type": "aos.message_bridge.RemoteMessage",
- "frequency": 20,
- "source_node": "pi{{ NUM }}",
- "max_size": 208
+ "num_senders": 2
},
{
"name": "/pi{{ NUM }}/aos",
@@ -178,8 +141,7 @@
"max_size": 1024,
"logger": "LOCAL_AND_REMOTE_LOGGER",
"logger_nodes": [
- "imu",
- "logger"
+ "imu"
],
"destination_nodes": [
{
@@ -190,15 +152,6 @@
"pi{{ NUM }}"
],
"time_to_live": 5000000
- },
- {
- "name": "logger",
- "priority": 4,
- "timestamp_logger": "LOCAL_AND_REMOTE_LOGGER",
- "timestamp_logger_nodes": [
- "pi{{ NUM }}"
- ],
- "time_to_live": 5000000
}
]
},
@@ -210,87 +163,6 @@
"max_size": 208
},
{
- "name": "/pi{{ NUM }}/aos/remote_timestamps/logger/pi{{ NUM }}/camera/frc971-vision-TargetMap",
- "type": "aos.message_bridge.RemoteMessage",
- "frequency": 80,
- "source_node": "pi{{ NUM }}",
- "max_size": 208
- },
- {
- "name": "/logger/aos",
- "type": "aos.starter.StarterRpc",
- "source_node": "logger",
- "logger": "LOCAL_AND_REMOTE_LOGGER",
- "logger_nodes": [
- "pi{{ NUM }}"
- ],
- "destination_nodes": [
- {
- "name": "pi{{ NUM }}",
- "priority": 5,
- "timestamp_logger": "LOCAL_AND_REMOTE_LOGGER",
- "timestamp_logger_nodes": [
- "logger"
- ],
- "time_to_live": 5000000
- }
- ]
- },
- {
- "name": "/logger/aos/remote_timestamps/pi{{ NUM }}/logger/aos/aos-starter-StarterRpc",
- "type": "aos.message_bridge.RemoteMessage",
- "source_node": "logger",
- "logger": "NOT_LOGGED",
- "frequency": 20,
- "num_senders": 2,
- "max_size": 200
- },
- {
- "name": "/logger/aos",
- "type": "aos.starter.Status",
- "source_node": "logger",
- "logger": "LOCAL_AND_REMOTE_LOGGER",
- "logger_nodes": [
- "pi{{ NUM }}"
- ],
- "destination_nodes": [
- {
- "name": "pi{{ NUM }}",
- "priority": 5,
- "timestamp_logger": "LOCAL_AND_REMOTE_LOGGER",
- "timestamp_logger_nodes": [
- "logger"
- ],
- "time_to_live": 5000000
- }
- ]
- },
- {
- "name": "/pi{{ NUM }}/aos/remote_timestamps/logger/pi{{ NUM }}/aos/aos-starter-Status",
- "type": "aos.message_bridge.RemoteMessage",
- "frequency": 100,
- "source_node": "pi{{ NUM }}",
- "max_size": 208
- },
- {
- "name": "/pi{{ NUM }}/aos/remote_timestamps/logger/pi{{ NUM }}/aos/aos-message_bridge-Timestamp",
- "type": "aos.message_bridge.RemoteMessage",
- "source_node": "pi{{ NUM }}",
- "logger": "NOT_LOGGED",
- "frequency": 15,
- "num_senders": 2,
- "max_size": 200
- },
- {
- "name": "/logger/aos/remote_timestamps/pi{{ NUM }}/logger/aos/aos-starter-Status",
- "type": "aos.message_bridge.RemoteMessage",
- "source_node": "logger",
- "logger": "NOT_LOGGED",
- "frequency": 50,
- "num_senders": 2,
- "max_size": 200
- },
- {
"name": "/pi{{ NUM }}/constants",
"type": "y2023.Constants",
"source_node": "pi{{ NUM }}",
@@ -384,6 +256,14 @@
]
},
{
+ "name": "camera_monitor",
+ "executable_name": "camera_monitor",
+ "user": "pi",
+ "nodes": [
+ "pi{{ NUM }}"
+ ]
+ },
+ {
"name": "foxglove_websocket",
"user": "pi",
"nodes": [
@@ -448,9 +328,6 @@
"port": 9971
},
{
- "name": "logger"
- },
- {
"name": "imu"
}
]