Flip driver image if wrist is backwards
Change-Id: Id303283baf449ffe88c6286ff13e7cfde91d3a9d
diff --git a/y2019/BUILD b/y2019/BUILD
index 7c30dad..b17e538 100644
--- a/y2019/BUILD
+++ b/y2019/BUILD
@@ -114,6 +114,7 @@
],
deps = [
":status_light",
+ ":vision_proto",
"//aos:init",
"//aos/actions:action_lib",
"//aos/input:action_joystick_input",
@@ -130,6 +131,7 @@
"//frc971/control_loops/drivetrain:drivetrain_queue",
"//y2019/control_loops/drivetrain:drivetrain_base",
"//y2019/control_loops/superstructure:superstructure_queue",
+ "@com_google_protobuf//:protobuf",
],
)
diff --git a/y2019/image_streamer/BUILD b/y2019/image_streamer/BUILD
index dc6c45f..4885da0 100644
--- a/y2019/image_streamer/BUILD
+++ b/y2019/image_streamer/BUILD
@@ -4,6 +4,7 @@
name = "image_streamer",
srcs = ["image_streamer.cc"],
deps = [
+ ":flip_image",
"//aos/logging",
"//aos/logging:implementations",
"//aos/vision/blob:codec",
@@ -16,3 +17,19 @@
"@com_github_gflags_gflags//:gflags",
],
)
+
+cc_library(
+ name = "flip_image",
+ srcs = ["flip_image.cc"],
+ hdrs = ["flip_image.h"],
+ copts = [
+ "-Wno-format-nonliteral",
+ "-Wno-cast-align",
+ "-Wno-cast-qual",
+ "-Wno-error=type-limits",
+ ],
+ deps = [
+ "//third_party/cimg:CImg",
+ "//third_party/libjpeg",
+ ],
+)
diff --git a/y2019/image_streamer/flip_image.cc b/y2019/image_streamer/flip_image.cc
new file mode 100644
index 0000000..6ff00ed
--- /dev/null
+++ b/y2019/image_streamer/flip_image.cc
@@ -0,0 +1,15 @@
+#include "flip_image.h"
+
+#define cimg_display 0
+#define cimg_use_jpeg
+#define cimg_plugin "plugins/jpeg_buffer.h"
+#include "third_party/cimg/CImg.h"
+
+void flip_image(const char *input, const int input_size, JOCTET *buffer,
+ unsigned int *buffer_size) {
+ ::cimg_library::CImg<unsigned char> image;
+ image.load_jpeg_buffer((JOCTET *)(input), input_size);
+ image.mirror("xy");
+
+ image.save_jpeg_buffer(buffer, *buffer_size, 80);
+}
diff --git a/y2019/image_streamer/flip_image.h b/y2019/image_streamer/flip_image.h
new file mode 100644
index 0000000..6a59e96
--- /dev/null
+++ b/y2019/image_streamer/flip_image.h
@@ -0,0 +1,12 @@
+#ifndef Y2019_IMAGE_STREAMER_FLIP_IMAGE_H_
+#define Y2019_IMAGE_STREAMER_FLIP_IMAGE_H_
+
+#include <stddef.h>
+#include <stdio.h>
+#include "third_party/libjpeg/jerror.h"
+#include "third_party/libjpeg/jpeglib.h"
+
+void flip_image(const char *input, const int input_size, JOCTET *buffer,
+ unsigned int *buffer_size);
+
+#endif // Y2019_IMAGE_STREAMER_FLIP_IMAGE_H
diff --git a/y2019/image_streamer/image_streamer.cc b/y2019/image_streamer/image_streamer.cc
index cd83a4e..8ee82f3 100644
--- a/y2019/image_streamer/image_streamer.cc
+++ b/y2019/image_streamer/image_streamer.cc
@@ -12,6 +12,7 @@
#include "aos/vision/events/udp.h"
#include "aos/vision/image/reader.h"
#include "gflags/gflags.h"
+#include "y2019/image_streamer/flip_image.h"
#include "y2019/vision.pb.h"
using ::aos::events::DataSocket;
@@ -195,7 +196,7 @@
fprintf(stderr, "wrong sized buffer\n");
exit(-1);
}
- LOG(INFO, "Frame size in bytes: data.size() = %zu\n",data.size());
+ LOG(INFO, "Frame size in bytes: data.size() = %zu\n", data.size());
output_buffer_.push_back(aos::vision::DataRef(data_header_tmp_, n_written));
output_buffer_.push_back(data);
output_buffer_.push_back("\r\n\r\n");
@@ -255,6 +256,8 @@
void set_active(bool active) { active_ = active; }
+ void set_flip(bool flip) { flip_ = flip; }
+
bool active() const { return active_; }
void ProcessImage(DataRef data,
@@ -276,8 +279,18 @@
sampling = 0;
}
+ std::string image_out;
+
+ if (flip_) {
+ unsigned int out_size = image_buffer_out_.size();
+ flip_image(data.data(), data.size(), &image_buffer_out_[0], &out_size);
+ image_out.assign(&image_buffer_out_[0], &image_buffer_out_[out_size]);
+ } else {
+ image_out = std::string(data);
+ }
+
if (active_) {
- auto frame = std::make_shared<Frame>(Frame{std::string(data)});
+ auto frame = std::make_shared<Frame>(Frame{image_out});
tcp_server_->Broadcast(
[frame](MjpegDataSocket *event) { event->NewFrame(frame); });
}
@@ -290,6 +303,8 @@
::std::unique_ptr<BlobLog> log_;
::std::function<void()> frame_callback_;
bool active_ = false;
+ bool flip_ = false;
+ std::array<JOCTET, 100000> image_buffer_out_;
};
int main(int argc, char **argv) {
@@ -343,7 +358,9 @@
ProtoUdpClient<VisionControl> udp_client(
5000, [&camera0, &camera1](const VisionControl &vision_control) {
bool cam0_active = false;
+ camera0->set_flip(vision_control.flip_image());
if (camera1) {
+ camera1->set_flip(vision_control.flip_image());
cam0_active = !vision_control.high_video();
camera0->set_active(!vision_control.high_video());
camera1->set_active(vision_control.high_video());
diff --git a/y2019/joystick_reader.cc b/y2019/joystick_reader.cc
index 26a8ce1..59684ed 100644
--- a/y2019/joystick_reader.cc
+++ b/y2019/joystick_reader.cc
@@ -11,7 +11,10 @@
#include "aos/input/joystick_input.h"
#include "aos/logging/logging.h"
#include "aos/logging/logging.h"
+#include "aos/network/team_number.h"
#include "aos/util/log_interval.h"
+#include "aos/vision/events/udp.h"
+#include "external/com_google_protobuf/src/google/protobuf/stubs/stringprintf.h"
#include "frc971/autonomous/auto.q.h"
#include "frc971/autonomous/base_autonomous_actor.h"
#include "frc971/control_loops/drivetrain/drivetrain.q.h"
@@ -19,17 +22,21 @@
#include "y2019/control_loops/drivetrain/drivetrain_base.h"
#include "y2019/control_loops/superstructure/superstructure.q.h"
#include "y2019/status_light.q.h"
+#include "y2019/vision.pb.h"
using ::y2019::control_loops::superstructure::superstructure_queue;
using ::aos::input::driver_station::ButtonLocation;
using ::aos::input::driver_station::ControlBit;
using ::aos::input::driver_station::JoystickAxis;
using ::aos::input::driver_station::POVLocation;
+using ::aos::events::ProtoTXUdpSocket;
namespace y2019 {
namespace input {
namespace joysticks {
+using google::protobuf::StringPrintf;
+
const ButtonLocation kSuctionBall(3, 13);
const ButtonLocation kSuctionHatch(3, 12);
const ButtonLocation kDeployStilt(3, 8);
@@ -104,10 +111,13 @@
: ::aos::input::ActionJoystickInput(
event_loop,
::y2019::control_loops::drivetrain::GetDrivetrainConfig()) {
+ const uint16_t team = ::aos::network::GetTeamNumber();
superstructure_queue.goal.FetchLatest();
if (superstructure_queue.goal.get()) {
grab_piece_ = superstructure_queue.goal->suction.grab_piece;
}
+ video_tx_.reset(new ProtoTXUdpSocket<VisionControl>(
+ StringPrintf("10.%d.%d.179", team / 100, team % 100), 5000));
}
void HandleTeleop(const ::aos::input::driver_station::Data &data) {
@@ -263,6 +273,8 @@
new_superstructure_goal->suction.gamepiece_mode = 1;
}
+ vision_control_.set_flip_image(elevator_wrist_pos_.wrist < 0);
+
new_superstructure_goal->suction.grab_piece = grab_piece_;
new_superstructure_goal->elevator.unsafe_goal =
@@ -273,6 +285,8 @@
if (!new_superstructure_goal.Send()) {
LOG(ERROR, "Sending superstructure goal failed.\n");
}
+
+ video_tx_->Send(vision_control_);
}
private:
@@ -282,6 +296,9 @@
bool switch_ball_ = false;
bool stilts_was_above_ = false;
+
+ VisionControl vision_control_;
+ ::std::unique_ptr<ProtoTXUdpSocket<VisionControl>> video_tx_;
};
} // namespace joysticks
diff --git a/y2019/vision.proto b/y2019/vision.proto
index fb1eeec..6a78805 100644
--- a/y2019/vision.proto
+++ b/y2019/vision.proto
@@ -4,6 +4,7 @@
message VisionControl {
optional bool high_video = 1;
+ optional bool flip_image = 2;
}
message VisionStatus {