Merge "Use 2020 drivetrain in spline GUI"
diff --git a/y2020/BUILD b/y2020/BUILD
index 4aa851f..bfa6ce2 100644
--- a/y2020/BUILD
+++ b/y2020/BUILD
@@ -4,6 +4,9 @@
load("@com_github_google_flatbuffers//:build_defs.bzl", "flatbuffer_cc_library")
robot_downloader(
+ binaries = [
+ ":setpoint_setter",
+ ],
data = [
":config.json",
],
@@ -114,6 +117,7 @@
":joystick_reader.cc",
],
deps = [
+ ":setpoint_fbs",
"//aos:init",
"//aos/actions:action_lib",
"//aos/input:action_joystick_input",
@@ -134,6 +138,7 @@
name = "config",
src = "y2020.json",
flatbuffers = [
+ ":setpoint_fbs",
"//aos/network:message_bridge_client_fbs",
"//aos/network:message_bridge_server_fbs",
"//aos/network:timestamp_fbs",
@@ -173,3 +178,23 @@
"//y2020/www:main_bundle",
],
)
+
+load("@com_github_google_flatbuffers//:build_defs.bzl", "flatbuffer_cc_library")
+
+flatbuffer_cc_library(
+ name = "setpoint_fbs",
+ srcs = [
+ "setpoint.fbs",
+ ],
+ gen_reflections = 1,
+)
+
+cc_binary(
+ name = "setpoint_setter",
+ srcs = ["setpoint_setter.cc"],
+ deps = [
+ ":setpoint_fbs",
+ "//aos:init",
+ "//aos/events:shm_event_loop",
+ ],
+)
diff --git a/y2020/control_loops/superstructure/superstructure.cc b/y2020/control_loops/superstructure/superstructure.cc
index 224a24f..8947f85 100644
--- a/y2020/control_loops/superstructure/superstructure.cc
+++ b/y2020/control_loops/superstructure/superstructure.cc
@@ -172,7 +172,7 @@
if (shooter_.ready() &&
unsafe_goal->shooter()->velocity_accelerator() > 10.0 &&
unsafe_goal->shooter()->velocity_finisher() > 10.0) {
- output_struct.feeder_voltage = 9.0;
+ output_struct.feeder_voltage = 12.0;
} else {
output_struct.feeder_voltage = 0.0;
}
diff --git a/y2020/joystick_reader.cc b/y2020/joystick_reader.cc
index 026a50b..2049e83 100644
--- a/y2020/joystick_reader.cc
+++ b/y2020/joystick_reader.cc
@@ -13,10 +13,11 @@
#include "aos/network/team_number.h"
#include "aos/util/log_interval.h"
#include "frc971/autonomous/base_autonomous_actor.h"
-#include "y2020/control_loops/drivetrain/drivetrain_base.h"
#include "y2020/constants.h"
+#include "y2020/control_loops/drivetrain/drivetrain_base.h"
#include "y2020/control_loops/superstructure/superstructure_goal_generated.h"
#include "y2020/control_loops/superstructure/superstructure_status_generated.h"
+#include "y2020/setpoint_generated.h"
using aos::input::driver_station::ButtonLocation;
using aos::input::driver_station::ControlBit;
@@ -57,8 +58,9 @@
superstructure_goal_sender_(
event_loop->MakeSender<superstructure::Goal>("/superstructure")),
superstructure_status_fetcher_(
- event_loop->MakeFetcher<superstructure::Status>(
- "/superstructure")) {}
+ event_loop->MakeFetcher<superstructure::Status>("/superstructure")),
+ setpoint_fetcher_(event_loop->MakeFetcher<y2020::joysticks::Setpoint>(
+ "/superstructure")) {}
void AutoEnded() override {
AOS_LOG(INFO, "Auto ended, assuming disc and have piece\n");
@@ -71,6 +73,8 @@
return;
}
+ setpoint_fetcher_.Fetch();
+
double hood_pos = constants::Values::kHoodRange().middle();
double intake_pos = -0.89;
double turret_pos = 0.0;
@@ -84,15 +88,26 @@
}
if (data.IsPressed(kHood)) {
- hood_pos = 0.05;
+ hood_pos = 0.45;
+ } else {
+ if (setpoint_fetcher_.get()) {
+ hood_pos = setpoint_fetcher_->hood();
+ } else {
+ hood_pos = 0.58;
+ }
}
if (data.IsPressed(kShootFast)) {
- accelerator_speed = 500.0;
- finisher_speed = 500.0;
+ if (setpoint_fetcher_.get()) {
+ accelerator_speed = setpoint_fetcher_->accelerator();
+ finisher_speed = setpoint_fetcher_->finisher();
+ } else {
+ accelerator_speed = 250.0;
+ finisher_speed = 500.0;
+ }
} else if (data.IsPressed(kShootSlow)) {
- accelerator_speed = 30.0;
- finisher_speed = 30.0;
+ accelerator_speed = 180.0;
+ finisher_speed = 375.0;
}
if (data.IsPressed(kIntakeExtend)) {
@@ -162,6 +177,8 @@
::aos::Sender<superstructure::Goal> superstructure_goal_sender_;
::aos::Fetcher<superstructure::Status> superstructure_status_fetcher_;
+
+ ::aos::Fetcher<y2020::joysticks::Setpoint> setpoint_fetcher_;
};
} // namespace joysticks
diff --git a/y2020/setpoint.fbs b/y2020/setpoint.fbs
new file mode 100644
index 0000000..5f1af72
--- /dev/null
+++ b/y2020/setpoint.fbs
@@ -0,0 +1,11 @@
+namespace y2020.joysticks;
+
+table Setpoint {
+ accelerator:double;
+
+ finisher:double;
+
+ hood:double;
+}
+
+root_type Setpoint;
diff --git a/y2020/setpoint_setter.cc b/y2020/setpoint_setter.cc
new file mode 100644
index 0000000..1cbc518
--- /dev/null
+++ b/y2020/setpoint_setter.cc
@@ -0,0 +1,34 @@
+#include "aos/events/shm_event_loop.h"
+#include "aos/init.h"
+#include "gflags/gflags.h"
+#include "y2020/setpoint_generated.h"
+
+DEFINE_double(accelerator, 250.0, "Accelerator speed");
+DEFINE_double(finisher, 500.0, "Finsher speed");
+DEFINE_double(hood, 0.45, "Hood setpoint");
+
+int main(int argc, char ** argv) {
+ aos::InitGoogle(&argc, &argv);
+ aos::InitNRT();
+
+ aos::FlatbufferDetachedBuffer<aos::Configuration> config =
+ aos::configuration::ReadConfig("config.json");
+
+ aos::ShmEventLoop event_loop(&config.message());
+
+ ::aos::Sender<y2020::joysticks::Setpoint> setpoint_sender =
+ event_loop.MakeSender<y2020::joysticks::Setpoint>("/superstructure");
+
+ aos::Sender<y2020::joysticks::Setpoint>::Builder builder =
+ setpoint_sender.MakeBuilder();
+
+ y2020::joysticks::Setpoint::Builder setpoint_builder =
+ builder.MakeBuilder<y2020::joysticks::Setpoint>();
+
+ setpoint_builder.add_accelerator(FLAGS_accelerator);
+ setpoint_builder.add_finisher(FLAGS_finisher);
+ setpoint_builder.add_hood(FLAGS_hood);
+ builder.Send(setpoint_builder.Finish());
+
+ aos::Cleanup();
+}
diff --git a/y2020/vision/camera_reader.cc b/y2020/vision/camera_reader.cc
index c47e42b..f38a40e 100644
--- a/y2020/vision/camera_reader.cc
+++ b/y2020/vision/camera_reader.cc
@@ -189,12 +189,14 @@
// avoid crashes that only occur when specific features are matched.
CHECK(feature_table->has_field_location());
- const flatbuffers::Vector<float> *const descriptor =
+ const flatbuffers::Vector<uint8_t> *const descriptor =
feature_table->descriptor();
CHECK_EQ(descriptor->size(), 128u) << ": Unsupported feature size";
- cv::Mat(1, descriptor->size(), CV_32F,
- const_cast<void *>(static_cast<const void *>(descriptor->data())))
- .copyTo(features(cv::Range(i, i + 1), cv::Range(0, 128)));
+ const auto in_mat = cv::Mat(
+ 1, descriptor->size(), CV_8U,
+ const_cast<void *>(static_cast<const void *>(descriptor->data())));
+ const auto out_mat = features(cv::Range(i, i + 1), cv::Range(0, 128));
+ in_mat.convertTo(out_mat, CV_32F);
}
matcher_->add(features);
}
@@ -552,13 +554,21 @@
const cv::Mat &descriptors) {
const int number_features = keypoints.size();
CHECK_EQ(descriptors.rows, number_features);
+ CHECK_EQ(descriptors.cols, 128);
std::vector<flatbuffers::Offset<sift::Feature>> features_vector(
number_features);
for (int i = 0; i < number_features; ++i) {
- const auto submat = descriptors(cv::Range(i, i + 1), cv::Range(0, 128));
+ const auto submat =
+ descriptors(cv::Range(i, i + 1), cv::Range(0, descriptors.cols));
CHECK(submat.isContinuous());
- const auto descriptor_offset =
- fbb->CreateVector(reinterpret_cast<float *>(submat.data), 128);
+ flatbuffers::Offset<flatbuffers::Vector<uint8_t>> descriptor_offset;
+ {
+ uint8_t *data;
+ descriptor_offset = fbb->CreateUninitializedVector(128, &data);
+ submat.convertTo(
+ cv::Mat(1, descriptors.cols, CV_8U, static_cast<void *>(data)),
+ CV_8U);
+ }
sift::Feature::Builder feature_builder(*fbb);
feature_builder.add_descriptor(descriptor_offset);
feature_builder.add_x(keypoints[i].pt.x);
diff --git a/y2020/vision/rootfs/target_configure.sh b/y2020/vision/rootfs/target_configure.sh
index 032764a..193f8b5 100755
--- a/y2020/vision/rootfs/target_configure.sh
+++ b/y2020/vision/rootfs/target_configure.sh
@@ -37,7 +37,20 @@
libopencv-videoio3.2 \
libopencv-videostab3.2 \
libopencv-viz3.2 \
- python3-opencv
+ python3-opencv \
+ gstreamer1.0-plugins-bad \
+ gstreamer1.0-plugins-base \
+ gstreamer1.0-plugins-good \
+ gstreamer1.0-plugins-ugly \
+ gstreamer1.0-x \
+ gstreamer1.0-nice \
+ gstreamer1.0-gl \
+ libgstreamer-plugins-bad1.0-0 \
+ libgstreamer-plugins-base1.0-0 \
+ libgstreamer1.0-0 \
+ libgstreamer-gl1.0-0 \
+ libnice10 \
+ libnice-dev
echo 'GOVERNOR="performance"' > /etc/default/cpufrequtils
diff --git a/y2020/vision/sift/demo_sift_training.py b/y2020/vision/sift/demo_sift_training.py
index 3fa33cf..c78a44a 100644
--- a/y2020/vision/sift/demo_sift_training.py
+++ b/y2020/vision/sift/demo_sift_training.py
@@ -26,7 +26,8 @@
for keypoint, descriptor in zip(keypoints, descriptors):
Feature.FeatureStartDescriptorVector(fbb, len(descriptor))
for n in reversed(descriptor):
- fbb.PrependFloat32(n)
+ assert n == round(n)
+ fbb.PrependUint8(int(round(n)))
descriptor_vector = fbb.EndVector(len(descriptor))
Feature.FeatureStart(fbb)
diff --git a/y2020/vision/sift/sift.fbs b/y2020/vision/sift/sift.fbs
index 3e2daaf..24fd64d 100644
--- a/y2020/vision/sift/sift.fbs
+++ b/y2020/vision/sift/sift.fbs
@@ -9,7 +9,8 @@
// Represents a single feature extracted from an image.
table Feature {
- // Contains the descriptor data.
+ // Contains the descriptor data. OpenCV likes to represent them as floats, but
+ // they're really ubytes.
//
// TODO(Brian): These are scaled to be convertible to chars. Should we do
// that to minimize storage space? Or maybe int16?
@@ -17,7 +18,7 @@
// The size of this depends on the parameters. It is width*width*hist_bins.
// Currently we have width=4 and hist_bins=8, which results in a size of
// 4*4*8=128.
- descriptor:[float];
+ descriptor:[ubyte];
// Location of the keypoint.
x:float;
diff --git a/y2020/vision/tools/python_code/camera_param_test.cc b/y2020/vision/tools/python_code/camera_param_test.cc
index 5b959cd..4fdc2fe 100644
--- a/y2020/vision/tools/python_code/camera_param_test.cc
+++ b/y2020/vision/tools/python_code/camera_param_test.cc
@@ -143,12 +143,14 @@
cv::Mat features(training_image->features()->size(), 128, CV_32F);
for (size_t i = 0; i < training_image->features()->size(); ++i) {
const sift::Feature *feature_table = training_image->features()->Get(i);
- const flatbuffers::Vector<float> *const descriptor =
+ const flatbuffers::Vector<uint8_t> *const descriptor =
feature_table->descriptor();
CHECK_EQ(descriptor->size(), 128u) << ": Unsupported feature size";
- cv::Mat(1, descriptor->size(), CV_32F,
- const_cast<void *>(static_cast<const void *>(descriptor->data())))
- .copyTo(features(cv::Range(i, i + 1), cv::Range(0, 128)));
+ const auto in_mat = cv::Mat(
+ 1, descriptor->size(), CV_8U,
+ const_cast<void *>(static_cast<const void *>(descriptor->data())));
+ const auto out_mat = features(cv::Range(i, i + 1), cv::Range(0, 128));
+ in_mat.convertTo(out_mat, CV_32F);
cv::Point2f point_2d = Training2dPoint(train_image_index, i);
point_list_2d_.push_back(point_2d);
diff --git a/y2020/vision/tools/python_code/load_sift_training.py b/y2020/vision/tools/python_code/load_sift_training.py
index 6db3155..bf58e5f 100644
--- a/y2020/vision/tools/python_code/load_sift_training.py
+++ b/y2020/vision/tools/python_code/load_sift_training.py
@@ -71,7 +71,8 @@
# Build the Descriptor vector
Feature.FeatureStartDescriptorVector(fbb, len(descriptor))
for n in reversed(descriptor):
- fbb.PrependFloat32(n)
+ assert n == round(n)
+ fbb.PrependUint8(int(round(n)))
descriptor_vector = fbb.EndVector(len(descriptor))
# Add all the components to the each Feature
diff --git a/y2020/y2020.json b/y2020/y2020.json
index 3d966b8..b40ac87 100644
--- a/y2020/y2020.json
+++ b/y2020/y2020.json
@@ -248,6 +248,12 @@
"num_senders": 2
},
{
+ "name": "/superstructure",
+ "type": "y2020.joysticks.Setpoint",
+ "source_node": "roborio",
+ "num_senders": 2
+ },
+ {
"name": "/drivetrain",
"type": "frc971.IMUValues",
"source_node": "roborio",
@@ -334,7 +340,7 @@
"type": "frc971.vision.sift.ImageMatchResult",
"source_node": "pi1",
"frequency": 25,
- "max_size": 300000
+ "max_size": 1000000
},
{
"name": "/pi1/camera",
@@ -370,7 +376,7 @@
"type": "frc971.vision.sift.ImageMatchResult",
"source_node": "pi2",
"frequency": 25,
- "max_size": 300000
+ "max_size": 1000000
},
{
"name": "/pi2/camera",
@@ -406,7 +412,7 @@
"type": "frc971.vision.sift.ImageMatchResult",
"source_node": "pi3",
"frequency": 25,
- "max_size": 300000
+ "max_size": 1000000
},
{
"name": "/pi3/camera",