Merge "Add turret queue"
diff --git a/aos/common/scoped_fd.h b/aos/common/scoped_fd.h
index 57ecdd8..29ccf6e 100644
--- a/aos/common/scoped_fd.h
+++ b/aos/common/scoped_fd.h
@@ -1,7 +1,10 @@
+#ifndef _AOS_COMMON_SCOPED_FD_
+#define _AOS_COMMON_SCOPED_FD_
+
#include <unistd.h>
-#include "aos/common/macros.h"
#include "aos/common/logging/logging.h"
+#include "aos/common/macros.h"
namespace aos {
@@ -9,9 +12,7 @@
class ScopedFD {
public:
explicit ScopedFD(int fd = -1) : fd_(fd) {}
- ~ScopedFD() {
- Close();
- }
+ ~ScopedFD() { Close(); }
int get() const { return fd_; }
int release() {
const int r = fd_;
@@ -25,6 +26,7 @@
}
}
operator bool() const { return fd_ != -1; }
+
private:
int fd_;
void Close() {
@@ -38,3 +40,5 @@
};
} // namespace aos
+
+#endif // _AOS_COMMON_SCOPED_FD_
diff --git a/aos/vision/blob/BUILD b/aos/vision/blob/BUILD
index a024aba..8cb4373 100644
--- a/aos/vision/blob/BUILD
+++ b/aos/vision/blob/BUILD
@@ -93,7 +93,6 @@
],
)
-"""
cc_library(
name = 'stream_view',
hdrs = ['stream_view.h'],
@@ -103,4 +102,3 @@
'//aos/vision/image:image_types',
],
)
-"""
diff --git a/aos/vision/debug/BUILD b/aos/vision/debug/BUILD
new file mode 100644
index 0000000..91111b2
--- /dev/null
+++ b/aos/vision/debug/BUILD
@@ -0,0 +1,22 @@
+#!/usr/bin/python3
+package(default_visibility = ["//visibility:public"])
+
+cc_library(
+ name = "overlay",
+ hdrs = ["overlay.h"],
+ deps = [
+ '//aos/vision/math:vector',
+ '//aos/vision/math:segment',
+ '//aos/vision/image:image_types',
+ ],
+)
+
+cc_library(name = "debug_viewer",
+ srcs = ["debug_viewer.cc"],
+ hdrs = ["debug_viewer.h"],
+ deps = [
+ '@usr_repo//:gtk+-3.0',
+ "//aos/vision/image:image_types",
+ ":overlay",
+ ]
+)
diff --git a/aos/vision/debug/debug_viewer.cc b/aos/vision/debug/debug_viewer.cc
new file mode 100644
index 0000000..331f733
--- /dev/null
+++ b/aos/vision/debug/debug_viewer.cc
@@ -0,0 +1,166 @@
+#include "aos/vision/debug/debug_viewer.h"
+
+#include <gdk-pixbuf/gdk-pixbuf.h>
+#include <gtk/gtk.h>
+#include <poll.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <functional>
+#include <memory>
+#include <vector>
+
+#include "aos/vision/image/image_types.h"
+
+namespace aos {
+namespace vision {
+
+template <typename T, gboolean (T::*DrawMethod)(cairo_t *cr)>
+gboolean DrawCallback(GtkWidget *, cairo_t *cr, gpointer data) {
+ return ((*((T *)data)).*DrawMethod)(cr);
+}
+
+template <typename T, gboolean (T::*DrawMethod)(cairo_t *cr)>
+void g_draw_signal_connect(GtkWidget *widget, T *obj) {
+ gboolean (*fnptr)(GtkWidget *, cairo_t *, gpointer) =
+ DrawCallback<T, DrawMethod>;
+ g_signal_connect(widget, "draw", G_CALLBACK(fnptr), obj);
+}
+
+struct DebugViewer::Internals {
+ Internals(bool flip) : flip_(flip) {}
+
+ gboolean Draw(cairo_t *cr) {
+ needs_draw = false;
+ cairo_scale(cr, scale_factor, scale_factor);
+ if (pixbuf != nullptr) {
+ cairo_save(cr);
+ if (flip_) {
+ cairo_translate(cr, ptr.fmt().w, ptr.fmt().h);
+ cairo_scale(cr, -1, -1);
+ }
+ gdk_cairo_set_source_pixbuf(cr, pixbuf, 0.0, 0.0);
+ cairo_paint(cr);
+ cairo_restore(cr);
+ }
+
+ int w = ptr.fmt().w;
+ int h = ptr.fmt().h;
+ if (overlays) {
+ for (const auto &ov : *overlays) {
+ cairo_save(cr);
+ CairoRender render(cr);
+ // move the drawing to match the window size
+ ov->Draw(&render, w, h);
+ cairo_restore(cr);
+ }
+ }
+
+ return FALSE;
+ }
+
+ GdkPixbuf *pixbuf = nullptr;
+ GtkWidget *drawing_area = nullptr;
+ ImagePtr ptr;
+ bool needs_draw = true;
+ GtkWidget *window;
+ std::vector<OverlayBase *> *overlays = nullptr;
+ double scale_factor;
+
+ // flip the image rows on drawing
+ bool flip_ = false;
+
+ // clear per frame
+ bool clear_per_frame_ = true;
+};
+
+void DebugViewer::SetOverlays(std::vector<OverlayBase *> *overlays) {
+ self->overlays = overlays;
+}
+
+void DebugViewer::Redraw() {
+ if (!self->needs_draw) {
+ gtk_widget_queue_draw(self->drawing_area);
+ self->needs_draw = true;
+ }
+}
+
+void DebugViewer::UpdateImage(ImagePtr ptr) {
+ if (ptr.data() != self->ptr.data()) {
+ int w = ptr.fmt().w;
+ int h = ptr.fmt().h;
+ self->pixbuf = gdk_pixbuf_new_from_data(
+ (const unsigned char *)ptr.data(), GDK_COLORSPACE_RGB, FALSE, 8,
+ ptr.fmt().w, ptr.fmt().h, 3 * ptr.fmt().w, NULL, NULL);
+ self->ptr = ptr;
+
+ gtk_window_set_default_size(GTK_WINDOW(self->window), w * scale_factor,
+ h * scale_factor);
+
+ gtk_widget_set_size_request(self->drawing_area, w * scale_factor,
+ h * scale_factor);
+ window_height_ = h;
+ window_width_ = w;
+ }
+}
+
+void DebugViewer::MoveTo(int x, int y) {
+ gtk_window_move(GTK_WINDOW(self->window), x, y);
+}
+
+void DebugViewer::SetScale(double scale_factor_inp) {
+ int w = window_width_;
+ int h = window_height_;
+
+ scale_factor = scale_factor_inp;
+ self->scale_factor = scale_factor;
+
+ gtk_window_resize(GTK_WINDOW(self->window), w * scale_factor,
+ h * scale_factor);
+
+ gtk_widget_set_size_request(self->drawing_area, w * scale_factor,
+ h * scale_factor);
+}
+
+gboolean debug_viewer_key_press_event(GtkWidget * /*widget*/,
+ GdkEventKey *event, gpointer user_data) {
+ auto &key_press_cb =
+ reinterpret_cast<DebugViewer *>(user_data)->key_press_event;
+ if (key_press_cb) key_press_cb(event->keyval);
+ return FALSE;
+}
+
+DebugViewer::DebugViewer(bool flip) : self(new Internals(flip)) {
+ self->scale_factor = scale_factor;
+ GtkWidget *window;
+ auto drawing_area = self->drawing_area = gtk_drawing_area_new();
+ gtk_widget_set_size_request(drawing_area, window_width_ * scale_factor,
+ window_height_ * scale_factor);
+ gtk_widget_add_events(drawing_area, GDK_KEY_PRESS_MASK);
+
+ g_draw_signal_connect<DebugViewer::Internals, &DebugViewer::Internals::Draw>(
+ drawing_area, self.get());
+
+ window = gtk_window_new(GTK_WINDOW_TOPLEVEL);
+ self->window = window;
+ g_signal_connect(window, "key-press-event",
+ G_CALLBACK(debug_viewer_key_press_event), this);
+ gtk_window_set_title(GTK_WINDOW(window), "Window");
+ gtk_window_set_default_size(GTK_WINDOW(window), window_width_ * scale_factor,
+ window_height_ * scale_factor);
+
+ gtk_container_add(GTK_CONTAINER(window), drawing_area);
+ gtk_widget_show_all(window);
+}
+DebugViewer::~DebugViewer() {}
+
+void CairoRender::Text(int x, int y, int /*text_x*/, int /*text_y*/,
+ const std::string &text) {
+ auto *pango_lay = pango_cairo_create_layout(cr_);
+ cairo_move_to(cr_, x, y);
+ pango_layout_set_text(pango_lay, text.data(), text.size());
+ pango_cairo_show_layout(cr_, pango_lay);
+ g_object_unref(pango_lay);
+}
+
+} // namespace vision
+} // namespace aos
diff --git a/aos/vision/debug/debug_viewer.h b/aos/vision/debug/debug_viewer.h
new file mode 100644
index 0000000..3bada28
--- /dev/null
+++ b/aos/vision/debug/debug_viewer.h
@@ -0,0 +1,78 @@
+#ifndef AOS_VISION_DEBUG_DEBUG_VIEWER_H_
+#define AOS_VISION_DEBUG_DEBUG_VIEWER_H_
+
+#include <cairo.h>
+#include <functional>
+#include "aos/vision/debug/overlay.h"
+#include "aos/vision/image/image_types.h"
+
+namespace aos {
+namespace vision {
+
+// Implement Cairo version of RenderInterface.
+class CairoRender : public RenderInterface {
+ public:
+ explicit CairoRender(cairo_t *cr) : cr_(cr) {}
+ virtual ~CairoRender() {}
+
+ void Translate(double x, double y) override { cairo_translate(cr_, x, y); }
+
+ void SetSourceRGB(double r, double g, double b) override {
+ cairo_set_source_rgb(cr_, r, g, b);
+ }
+
+ void MoveTo(double x, double y) override { cairo_move_to(cr_, x, y); }
+
+ void LineTo(double x, double y) override { cairo_line_to(cr_, x, y); }
+
+ void Circle(double x, double y, double r) override {
+ cairo_arc(cr_, x, y, r, 0.0, 2 * M_PI);
+ }
+
+ void Stroke() override { cairo_stroke(cr_); }
+
+ void Text(int x, int y, int text_x, int text_y,
+ const std::string &text) override;
+
+ private:
+ cairo_t *cr_;
+};
+
+// Simple debug view window.
+class DebugViewer {
+ public:
+ struct Internals;
+ explicit DebugViewer(bool flip);
+ ~DebugViewer();
+ // Explicit redraw queuing (Will not double-queue).
+ void Redraw();
+
+ // This will resize the window as well as updating to draw from the
+ // (not owned) ptr. When you change ptr, you should call Redraw();
+ void UpdateImage(ImagePtr ptr);
+
+ // Sets up the window to draw a list of overlays.
+ // See overlay.h for more info.
+ void SetOverlays(std::vector<OverlayBase *> *overlay);
+
+ // Resizes the window.
+ void SetScale(double scale_factor);
+
+ // Move window.
+ void MoveTo(int x, int y);
+
+ // Set to change the key_press behaviour.
+ // The argument type is a constant that looks like: GDK_KEY_#{key_val_name}
+ std::function<void(uint32_t)> key_press_event;
+
+ private:
+ double scale_factor = 1.0;
+ int window_width_ = 100;
+ int window_height_ = 100;
+ std::unique_ptr<Internals> self;
+};
+
+} // namespace vision
+} // namespace aos
+
+#endif // AOS_VISION_DEBUG_DEBUG_VIEWER_H_
diff --git a/aos/vision/debug/overlay.h b/aos/vision/debug/overlay.h
new file mode 100644
index 0000000..fbd6838
--- /dev/null
+++ b/aos/vision/debug/overlay.h
@@ -0,0 +1,198 @@
+#ifndef _AOS_VISION_IMAGE_DEBUG_OVERLAY_H_
+#define _AOS_VISION_IMAGE_DEBUG_OVERLAY_H_
+
+#include <string>
+#include <vector>
+
+#include "aos/vision/image/image_types.h"
+#include "aos/vision/math/segment.h"
+#include "aos/vision/math/vector.h"
+
+namespace aos {
+namespace vision {
+
+// Abstract away rendering to avoid compiling gtk for arm.
+// This should match a reduced cairo rendering api.
+class RenderInterface {
+ public:
+ RenderInterface() {}
+ RenderInterface(RenderInterface &&other) = delete;
+ RenderInterface(const RenderInterface &other) = delete;
+ ~RenderInterface() {}
+
+ virtual void Translate(double x, double y) = 0;
+ virtual void SetSourceRGB(double r, double g, double b) = 0;
+ virtual void MoveTo(double x, double y) = 0;
+ virtual void LineTo(double x, double y) = 0;
+ virtual void Circle(double x, double y, double r) = 0;
+ // negative in x, y, text_x, text_y measures from max in those value
+ virtual void Text(int x, int y, int text_x, int text_y,
+ const std::string &text) = 0;
+ virtual void Stroke() = 0;
+};
+
+// Interface for a list of overlays to be drawn onto a debug image.
+// These will be passed into the running vision algorithms to output debug info,
+// so they must not have costly side-effects.
+class OverlayBase {
+ public:
+ OverlayBase() {}
+ virtual ~OverlayBase() {}
+
+ // Draws this overlay to the given canvas.
+ virtual void Draw(RenderInterface *render, double /* width */,
+ double /* height */) = 0;
+
+ // Clears the entire overlay.
+ virtual void Reset() = 0;
+
+ PixelRef color = {255, 0, 0};
+ double scale = 1.0;
+};
+
+// A lambda that renders directly to the render interface.
+class LambdaOverlay : public OverlayBase {
+ public:
+ std::function<void(RenderInterface *, double, double)> draw_fn;
+ void Draw(RenderInterface *render, double width, double height) override {
+ if (draw_fn) draw_fn(render, width, height);
+ }
+ void Reset() override {}
+};
+
+// Lines rendered in a coordinate system where the origin is the center
+// of the screen
+class LinesOverlay : public OverlayBase {
+ public:
+ LinesOverlay() : OverlayBase() {}
+ ~LinesOverlay() {}
+
+ // build a segment for this line
+ void add_line(Vector<2> st, Vector<2> ed) { add_line(st, ed, color); }
+
+ // build a segment for this line
+ void add_line(Vector<2> st, Vector<2> ed, PixelRef newColor) {
+ lines_.emplace_back(
+ std::pair<Segment<2>, PixelRef>(Segment<2>(st, ed), newColor));
+ }
+
+ void add_point(Vector<2> pt) { add_point(pt, color); }
+
+ // add a new point connected to the last point in the line
+ void add_point(Vector<2> pt, PixelRef newColor) {
+ if (lines_.empty()) {
+ lines_.emplace_back(
+ std::pair<Segment<2>, PixelRef>(Segment<2>(pt, pt), newColor));
+ } else {
+ Vector<2> st = lines_.back().first.B();
+ lines_.emplace_back(
+ std::pair<Segment<2>, PixelRef>(Segment<2>(st, pt), newColor));
+ }
+ }
+
+ void Draw(RenderInterface *render, double w, double h) override {
+ render->Translate(w / 2.0, h / 2.0);
+ for (const auto &ln : lines_) {
+ PixelRef localColor = ln.second;
+ render->SetSourceRGB(localColor.r / 255.0, localColor.g / 255.0,
+ localColor.b / 255.0);
+ render->MoveTo(scale * ln.first.A().x(), -scale * ln.first.A().y());
+ render->LineTo(scale * ln.first.B().x(), -scale * ln.first.B().y());
+ render->Stroke();
+ }
+ }
+
+ // Empting the list will blank the whole overlay
+ void Reset() override { lines_.clear(); }
+
+ private:
+ // lines in this over lay
+ std::vector<std::pair<Segment<2>, PixelRef>> lines_;
+};
+
+// Lines rendered in pixel coordinates (Should match up with the screen.)
+class PixelLinesOverlay : public OverlayBase {
+ public:
+ PixelLinesOverlay() : OverlayBase() {}
+ ~PixelLinesOverlay() {}
+
+ // build a segment for this line
+ void add_line(Vector<2> st, Vector<2> ed) { add_line(st, ed, color); }
+
+ // build a segment for this line
+ void add_line(Vector<2> st, Vector<2> ed, PixelRef newColor) {
+ lines_.emplace_back(
+ std::pair<Segment<2>, PixelRef>(Segment<2>(st, ed), newColor));
+ }
+
+ void start_new_profile() { start_profile = true; }
+
+ // add a new point connected to the last point in the line
+ void add_point(Vector<2> pt, PixelRef newColor) {
+ if (lines_.empty() || start_profile) {
+ lines_.emplace_back(
+ std::pair<Segment<2>, PixelRef>(Segment<2>(pt, pt), newColor));
+ start_profile = false;
+ } else {
+ Vector<2> st = lines_.back().first.B();
+ lines_.emplace_back(
+ std::pair<Segment<2>, PixelRef>(Segment<2>(st, pt), newColor));
+ }
+ }
+
+ void Draw(RenderInterface *render, double /*width*/,
+ double /*hieght*/) override {
+ for (const auto &ln : lines_) {
+ PixelRef localColor = ln.second;
+ render->SetSourceRGB(localColor.r / 255.0, localColor.g / 255.0,
+ localColor.b / 255.0);
+ render->MoveTo(ln.first.A().x(), ln.first.A().y());
+ render->LineTo(ln.first.B().x(), ln.first.B().y());
+ render->Stroke();
+ }
+ }
+
+ // Empting the list will blank the whole overlay.
+ void Reset() override { lines_.clear(); }
+
+ private:
+ // Lines in this overlay.
+ std::vector<std::pair<Segment<2>, PixelRef>> lines_;
+ bool start_profile = false;
+};
+
+// Circles rendered in a coordinate system where the origin is the center
+// of the screen.
+class CircleOverlay : public OverlayBase {
+ public:
+ CircleOverlay() : OverlayBase() {}
+ ~CircleOverlay() {}
+
+ // build a circle as a point and radius
+ std::pair<Vector<2>, double> *add_circle(Vector<2> center, double radius) {
+ circles_.emplace_back(std::pair<Vector<2>, double>(center, radius));
+ return &(circles_.back());
+ }
+
+ void Draw(RenderInterface *render, double w, double h) {
+ render->Translate(w / 2.0, h / 2.0);
+ render->SetSourceRGB(color.r / 255.0, color.g / 255.0, color.b / 255.0);
+ for (const auto &circle : circles_) {
+ render->Circle(scale * circle.first.x(), -scale * circle.first.y(),
+ scale * circle.second);
+ render->Stroke();
+ }
+ }
+
+ // empting the list will blank the whole overlay
+ void Reset() { circles_.clear(); }
+
+ private:
+ // circles in this overlay
+ std::vector<std::pair<Vector<2>, double>> circles_;
+};
+
+} // vision
+} // aos
+
+#endif // _AOS_VISION_IMAGE_DEBUG_OVERLAY_H_
diff --git a/aos/vision/events/BUILD b/aos/vision/events/BUILD
index c3400fe..e003dc9 100644
--- a/aos/vision/events/BUILD
+++ b/aos/vision/events/BUILD
@@ -11,6 +11,14 @@
],
)
+cc_library(name = "socket_types",
+ hdrs = ["socket_types.h"],
+ deps = [
+ "//aos/vision/events:tcp_server",
+ "//aos/vision/image:image_types",
+ ],
+)
+
cc_library(
name = 'intrusive_free_list',
hdrs = ['intrusive_free_list.h'],
diff --git a/aos/vision/events/socket_types.h b/aos/vision/events/socket_types.h
new file mode 100644
index 0000000..2432cb9
--- /dev/null
+++ b/aos/vision/events/socket_types.h
@@ -0,0 +1,78 @@
+#ifndef _AOS_VISION_EVENTS_SOCKET_TYPES_H_
+#define _AOS_VISION_EVENTS_SOCKET_TYPES_H_
+
+#include <poll.h>
+#include <stdint.h>
+
+#include "aos/vision/events/tcp_server.h"
+#include "aos/vision/image/image_types.h"
+#include "google/protobuf/message.h"
+
+namespace aos {
+namespace events {
+
+// Simple TCP client connection that sends messages prefixed by length.
+// Useful to broadcast to a message all connected clients.
+class DataSocket : public events::SocketConnection {
+ public:
+ // Aliasing cast.
+ union data_len {
+ uint32_t len;
+ char buf[4];
+ };
+
+ DataSocket(events::TCPServerBase *serv, int fd)
+ : events::SocketConnection(serv, fd) {}
+
+ ~DataSocket() { printf("Closed connection on descriptor %d\n", fd()); }
+
+ void ReadEvent() override {
+ // Ignore reads, but don't leave them pending.
+ ssize_t count;
+ char buf[512];
+ while (true) {
+ count = read(fd(), &buf, sizeof buf);
+ if (count <= 0) return;
+ }
+ }
+
+ void Emit(const google::protobuf::Message &data) {
+ std::string d;
+ if (data.SerializeToString(&d)) {
+ Emit(d);
+ }
+ }
+
+ void Emit(vision::DataRef data) {
+ data_len len;
+ len.len = data.size();
+ int res = write(fd(), len.buf, sizeof len.buf);
+ if (res == -1) {
+ printf("Emit Error on write\n");
+ }
+ size_t write_count = 0;
+ while (write_count < data.size()) {
+ int len =
+ write(fd(), &data.data()[write_count], data.size() - write_count);
+ if (len == -1) {
+ if (errno == EAGAIN) {
+ struct pollfd waiting;
+ waiting.fd = fd();
+ waiting.events = POLLOUT;
+ poll(&waiting, 1, -1);
+ } else {
+ close(fd());
+ return;
+ }
+ } else {
+ write_count += len;
+ }
+ if (write_count != data.size()) printf("wrote: %d\n", len);
+ }
+ }
+};
+
+} // namespace events
+} // namespace aos
+
+#endif // _AOS_VISION_EVENTS_SOCKET_TYPES_H_
diff --git a/aos/vision/events/udp.cc b/aos/vision/events/udp.cc
index 2d3d1d1..b5367f6 100644
--- a/aos/vision/events/udp.cc
+++ b/aos/vision/events/udp.cc
@@ -5,24 +5,24 @@
#include "aos/common/logging/logging.h"
namespace aos {
-namespace vision {
+namespace events {
-TXUdpSocket::TXUdpSocket(const char *ip_addr, int port)
+TXUdpSocket::TXUdpSocket(const std::string &ip_addr, int port)
: fd_(PCHECK(socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP))) {
sockaddr_in destination_in;
memset(&destination_in, 0, sizeof(destination_in));
destination_in.sin_family = AF_INET;
destination_in.sin_port = htons(port);
- if (inet_aton(ip_addr, &destination_in.sin_addr) == 0) {
- LOG(FATAL, "invalid IP address %s\n", ip_addr);
+ if (inet_aton(ip_addr.c_str(), &destination_in.sin_addr) == 0) {
+ LOG(FATAL, "invalid IP address %s\n", ip_addr.c_str());
}
PCHECK(connect(fd_.get(), reinterpret_cast<sockaddr *>(&destination_in),
sizeof(destination_in)));
}
-int TXUdpSocket::Send(const void *data, int size) {
- return PCHECK(send(fd_.get(), static_cast<const char *>(data), size, 0));
+int TXUdpSocket::Send(const char *data, int size) {
+ return PCHECK(send(fd_.get(), data, size, 0));
}
RXUdpSocket::RXUdpSocket(int port)
@@ -42,5 +42,5 @@
return PCHECK(recv(fd_.get(), static_cast<char *>(data), size, 0));
}
-} // namespace vision
+} // namespace events
} // namespace aos
diff --git a/aos/vision/events/udp.h b/aos/vision/events/udp.h
index a12a8bf..ed4a8e7 100644
--- a/aos/vision/events/udp.h
+++ b/aos/vision/events/udp.h
@@ -1,27 +1,28 @@
-#ifndef AOS_VISION_IMAGE_UDP_H_
-#define AOS_VISION_IMAGE_UDP_H_
+#ifndef AOS_VISION_EVENTS_UDP_H_
+#define AOS_VISION_EVENTS_UDP_H_
#include <arpa/inet.h>
-#include <sys/socket.h>
#include <math.h>
+#include <sys/socket.h>
#include <unistd.h>
+#include <string>
#include <vector>
#include "aos/common/macros.h"
#include "aos/common/scoped_fd.h"
namespace aos {
-namespace vision {
+namespace events {
// Simple wrapper around a transmitting UDP socket.
//
// LOG(FATAL)s for all errors, including from Send.
class TXUdpSocket {
public:
- TXUdpSocket(const char *ip_addr, int port);
+ TXUdpSocket(const std::string &ip_addr, int port);
// Returns the number of bytes actually sent.
- int Send(const void *data, int size);
+ int Send(const char *data, int size);
private:
ScopedFD fd_;
@@ -45,7 +46,7 @@
DISALLOW_COPY_AND_ASSIGN(RXUdpSocket);
};
-} // namespace vision
+} // namespace events
} // namespace aos
-#endif // AOS_VISION_IMAGE_UDP_H_
+#endif // AOS_VISION_EVENTS_UDP_H_
diff --git a/aos/vision/events/udp_test.cc b/aos/vision/events/udp_test.cc
index 87046ba..7e77b68 100644
--- a/aos/vision/events/udp_test.cc
+++ b/aos/vision/events/udp_test.cc
@@ -3,7 +3,7 @@
#include "gtest/gtest.h"
namespace aos {
-namespace vision {
+namespace events {
TEST(UDPTest, SendRecv) {
RXUdpSocket rx(1109);
@@ -19,5 +19,5 @@
EXPECT_EQ(txdata[3], rxdata[3]);
}
-} // namespace vision
+} // namespace events
} // namespace aos
diff --git a/aos/vision/image/image_stream.h b/aos/vision/image/image_stream.h
index ca2d8be..bdfbc31 100644
--- a/aos/vision/image/image_stream.h
+++ b/aos/vision/image/image_stream.h
@@ -9,6 +9,8 @@
namespace aos {
namespace vision {
+// Converts a camera reader into a virtual base class that calls ProcessImage
+// on each new image.
class ImageStreamEvent : public ::aos::events::EpollEvent {
public:
static std::unique_ptr<::camera::Reader> GetCamera(
@@ -17,28 +19,28 @@
using namespace std::placeholders;
std::unique_ptr<::camera::Reader> camread(new ::camera::Reader(
fname,
- std::bind(&ImageStreamEvent::ProcessHelper, obj, _1, _2), params);
+ std::bind(&ImageStreamEvent::ProcessHelper, obj, _1, _2), params));
camread->StartAsync();
return camread;
}
explicit ImageStreamEvent(std::unique_ptr<::camera::Reader> reader)
- : ::aos::events::EpollEvent(reader->fd()), reader_(reader) {}
+ : ::aos::events::EpollEvent(reader->fd()), reader_(std::move(reader)) {}
explicit ImageStreamEvent(const std::string &fname,
camera::CameraParams params)
: ImageStreamEvent(GetCamera(fname, this, params)) {}
- void ProcessHelper(DataRef data, uint64_t timestamp) {
+ void ProcessHelper(DataRef data, aos::monotonic_clock::time_point timestamp) {
if (data.size() < 300) {
- LOG(INFO, "got bad img: %d of size(%lu)\n", (int)timestamp, data.size());
+ LOG(INFO, "got bad img of size(%lu)\n", data.size());
return;
}
ProcessImage(data, timestamp);
}
- virtual void ProcessImage(DataRef data, uint64_t timestamp) = 0;
+ virtual void ProcessImage(DataRef data, aos::monotonic_clock::time_point timestamp) = 0;
- void ReadEvent(Context /*ctx*/) override { reader_->HandleFrame(); }
+ void ReadEvent() override { reader_->HandleFrame(); }
private:
std::unique_ptr<::camera::Reader> reader_;
diff --git a/debian/usr.BUILD b/debian/usr.BUILD
index 7ee1bf3..403d069 100644
--- a/debian/usr.BUILD
+++ b/debian/usr.BUILD
@@ -86,3 +86,61 @@
],
visibility = ['//visibility:public'],
)
+
+cc_library(
+ name = 'gtk+-3.0',
+ hdrs = glob([
+ 'include/gtk-3.0/**/*.h',
+ 'include/at-spi2-atk/2.0/**/*.h',
+ 'include/at-spi-2.0/**/*.h',
+ 'include/dbus-1.0/**/*.h',
+ 'lib/x86_64-linux-gnu/dbus-1.0/include/**/*.h',
+ 'include/gtk-3.0/**/*.h',
+ 'include/gio-unix-2.0/**/*.h',
+ 'include/cairo/**/*.h',
+ 'include/pango-1.0/**/*.h',
+ 'include/harfbuzz/**/*.h',
+ 'include/pango-1.0/**/*.h',
+ 'include/atk-1.0/**/*.h',
+ 'include/pixman-1/**/*.h',
+ 'include/freetype2/**/*.h',
+ 'include/libpng12/**/*.h',
+ 'include/gdk-pixbuf-2.0/**/*.h',
+ 'include/glib-2.0/**/*.h',
+ 'lib/x86_64-linux-gnu/glib-2.0/include/**/*.h',
+ ]),
+ includes = [
+ 'include/gtk-3.0',
+ 'include/at-spi2-atk/2.0',
+ 'include/at-spi-2.0',
+ 'include/dbus-1.0',
+ 'lib/x86_64-linux-gnu/dbus-1.0/include',
+ 'include/gtk-3.0',
+ 'include/gio-unix-2.0/',
+ 'include/cairo',
+ 'include/pango-1.0',
+ 'include/harfbuzz',
+ 'include/pango-1.0',
+ 'include/atk-1.0',
+ 'include/pixman-1',
+ 'include/freetype2',
+ 'include/libpng12',
+ 'include/gdk-pixbuf-2.0',
+ 'include/glib-2.0',
+ 'lib/x86_64-linux-gnu/glib-2.0/include',
+ ],
+ linkopts = [
+ '-lgtk-3',
+ '-lgdk-3',
+ '-lpangocairo-1.0',
+ '-lpango-1.0',
+ '-latk-1.0',
+ '-lcairo-gobject',
+ '-lcairo',
+ '-lgdk_pixbuf-2.0',
+ '-lgio-2.0',
+ '-lgobject-2.0',
+ '-lglib-2.0'
+ ],
+ visibility = ['//visibility:public'],
+)
diff --git a/y2016/vision/BUILD b/y2016/vision/BUILD
index f41ea84..59139e1 100644
--- a/y2016/vision/BUILD
+++ b/y2016/vision/BUILD
@@ -76,6 +76,39 @@
],
)
+cc_binary(name = 'target_sender',
+ srcs = ['target_sender.cc'],
+ deps = [
+ '//aos/common:time',
+ '//aos/common/logging:logging',
+ '//aos/common/logging:implementations',
+ '//aos/vision/image:reader',
+ '//aos/vision/image:jpeg_routines',
+ '//aos/vision/image:image_stream',
+ '//y2016/vision:blob_filters',
+ '//aos/vision/events:udp',
+ '//aos/vision/events:epoll_events',
+ '//aos/vision/events:socket_types',
+ ':stereo_geometry',
+ ':vision_data',
+ ':calibration',
+ ]
+)
+
+cc_library(name = "blob_filters",
+ srcs = ["blob_filters.cc"],
+ hdrs = ["blob_filters.h"],
+ deps = [
+ "//aos/vision/math:vector",
+ "//aos/vision/math:segment",
+ "//aos/vision/blob:range_image",
+ "//aos/vision/blob:threshold",
+ "//aos/vision/blob:find_blob",
+ "//aos/vision/blob:hierarchical_contour_merge",
+ "//aos/vision/blob:codec",
+ ],
+)
+
cc_binary(
name = 'target_receiver',
srcs = [
diff --git a/y2016/vision/blob_filters.cc b/y2016/vision/blob_filters.cc
new file mode 100644
index 0000000..e9f881d
--- /dev/null
+++ b/y2016/vision/blob_filters.cc
@@ -0,0 +1,393 @@
+#include "y2016/vision/blob_filters.h"
+#include <unistd.h>
+
+namespace aos {
+namespace vision {
+
+double CornerFinder::LineScore(Vector<2> A, Vector<2> B, FittedLine line) {
+ Vector<2> st(line.st.x, line.st.y);
+ Vector<2> ed(line.ed.x, line.ed.y);
+
+ double dist_st_a = A.SquaredDistanceTo(st);
+ double dist_st_b = B.SquaredDistanceTo(st);
+
+ if (dist_st_b < dist_st_a) {
+ Vector<2> tmp = B;
+ B = A;
+ A = tmp;
+ }
+
+ return A.SquaredDistanceTo(st) + B.SquaredDistanceTo(ed);
+}
+
+void CornerFinder::EnqueueLine(std::vector<FittedLine> *list, FittedLine line) {
+ if (list->size() < 2) {
+ list->emplace_back(line);
+ return;
+ }
+
+ Vector<2> st(line.st.x, line.st.y);
+ Vector<2> ed(line.ed.x, line.ed.y);
+
+ double llen = st.SquaredDistanceTo(ed);
+ FittedLine ins = line;
+ for (int i = 0; i < (int)list->size(); i++) {
+ Vector<2> ist((*list)[i].st.x, (*list)[i].st.y);
+ Vector<2> ied((*list)[i].ed.x, (*list)[i].ed.y);
+ double ilen = ist.SquaredDistanceTo(ied);
+ if (ilen < llen) {
+ // our new line is longer, so lets use that.
+ // but we still need to check the one we are replacing against the
+ // others.
+ FittedLine tmp = (*list)[i];
+ (*list)[i] = ins;
+ ins = tmp;
+ llen = ilen;
+ }
+ }
+}
+
+Segment<2> CornerFinder::MakeLine(const std::vector<FittedLine> &list) {
+ assert(list.size() == 2);
+ Vector<2> st0(list[0].st.x, list[0].st.y);
+ Vector<2> ed0(list[0].ed.x, list[0].ed.y);
+ Vector<2> st1(list[1].st.x, list[1].st.y);
+ Vector<2> ed1(list[1].ed.x, list[1].ed.y);
+
+ if (ed1.SquaredDistanceTo(st0) < st1.SquaredDistanceTo(st0)) {
+ Vector<2> sttmp = st1;
+ st1 = ed1;
+ ed1 = sttmp;
+ }
+
+ return Segment<2>((st0 + st1), (ed0 + ed1)).Scale(0.5);
+}
+
+std::vector<std::pair<Vector<2>, Vector<2>>> CornerFinder::Find(
+ const std::vector<SelectedBlob> &blobl) {
+ std::vector<std::pair<Vector<2>, Vector<2>>> res;
+ alloc_.reset();
+ for (const SelectedBlob &blob : blobl) {
+ ContourNode *n = RangeImgToContour(blob.blob, &alloc_);
+ std::vector<FittedLine> lines;
+ HierarchicalMerge(n, &lines, merge_rate_, min_len_);
+
+ if (do_overlay_) {
+ for (FittedLine &line : lines) {
+ overlay_->add_line(Vector<2>(line.st.x, line.st.y),
+ Vector<2>(line.ed.x, line.ed.y), {255, 0, 0});
+ }
+ }
+
+ std::vector<FittedLine> leftLine;
+ std::vector<FittedLine> rightLine;
+ std::vector<FittedLine> bottomLine;
+
+ for (auto &line : lines) {
+ double left_score = LineScore(blob.upper_left, blob.lower_left, line);
+ double right_score = LineScore(blob.upper_right, blob.lower_right, line);
+ double bottom_score = LineScore(blob.lower_left, blob.lower_right, line);
+ if (left_score < right_score && left_score < bottom_score) {
+ if (line.st.x > 0 && line.st.y > 0 && line.ed.x > 0 && line.ed.y > 0) {
+ EnqueueLine(&leftLine, line);
+ }
+ } else if (right_score < left_score && right_score < bottom_score) {
+ EnqueueLine(&rightLine, line);
+ } else {
+ EnqueueLine(&bottomLine, line);
+ }
+ }
+
+ if (leftLine.size() == 2 && rightLine.size() == 2 &&
+ bottomLine.size() == 2) {
+ Segment<2> left(MakeLine(leftLine));
+ Segment<2> right(MakeLine(rightLine));
+ Segment<2> bottom(MakeLine(bottomLine));
+
+ if (do_overlay_) {
+ overlay_->add_line(left.A(), left.B(), {155, 0, 255});
+ overlay_->add_line(right.A(), right.B(), {255, 155, 0});
+ overlay_->add_line(bottom.A(), bottom.B(), {255, 0, 155});
+ }
+
+ res.emplace_back(left.Intersect(bottom), right.Intersect(bottom));
+ }
+ }
+ return res;
+}
+
+//*****************************************************************************
+
+std::vector<SelectedBlob> BlobFilterBase::PreFilter(const BlobList &blobl) {
+ std::vector<SelectedBlob> filt;
+ for (const RangeImage &blob : blobl) {
+ int area = blob.calc_area();
+ if (area > min_area_ && area < max_area_) {
+ filt.emplace_back(SelectedBlob(blob));
+ }
+ }
+ return filt;
+}
+
+//*****************************************************************************
+
+bool HistogramBlobFilter::PickClosest(const Vector<2> &goal, const Vector<2> &A,
+ const Vector<2> &B) {
+ double sq_dist_a = goal.SquaredDistanceTo(A);
+ double sq_dist_b = goal.SquaredDistanceTo(B);
+ if (sq_dist_a < sq_dist_b) {
+ // A is closest
+ return true;
+ } else {
+ // B is closest
+ return false;
+ }
+}
+
+namespace {
+void CalcBoundingBox(const RangeImage &rimage, Vector<2> &ul, Vector<2> &ur,
+ Vector<2> &lr, Vector<2> &ll) {
+ const auto &ranges = rimage.ranges();
+ int mini = rimage.mini();
+ double x_min = ranges[0][0].st;
+ double x_max = ranges[0][0].ed;
+ double y_min = mini;
+ double y_max = mini + ranges.size();
+ for (auto &range : ranges) {
+ for (auto &interval : range) {
+ if (interval.st < x_min) {
+ x_min = interval.st;
+ }
+ if (interval.ed > x_max) {
+ x_max = interval.ed;
+ }
+ }
+ }
+ ul = Vector<2>(x_min, y_min);
+ ur = Vector<2>(x_max, y_min);
+ lr = Vector<2>(x_max, y_max);
+ ll = Vector<2>(x_min, y_max);
+}
+} // namespace
+
+std::vector<SelectedBlob> HistogramBlobFilter::PostFilter(
+ std::vector<SelectedBlob> blobl) {
+ std::vector<SelectedBlob> ret;
+ for (int ti = 0; ti < (int)blobl.size(); ti++) {
+ Vector<2> upper_left(1280, 960);
+ Vector<2> upper_right(0.0, 960);
+ Vector<2> lower_right(0.0, 0.0);
+ Vector<2> lower_left(1280, 0.0);
+
+ Vector<2> tul;
+ Vector<2> tur;
+ Vector<2> tlr;
+ Vector<2> tll;
+ CalcBoundingBox(blobl[ti].blob, tul, tur, tlr, tll);
+ for (int j = 0; j < (int)blobl[ti].blob.ranges().size(); j++) {
+ auto &range = blobl[ti].blob.ranges()[j];
+ Vector<2> first(range[0].st, j + blobl[ti].blob.mini());
+ Vector<2> last(range.back().ed, j + blobl[ti].blob.mini());
+ if (!PickClosest(tul, upper_left, first)) {
+ upper_left = first;
+ }
+ if (!PickClosest(tll, lower_left, first)) {
+ lower_left = first;
+ }
+ if (!PickClosest(tur, upper_right, last)) {
+ upper_right = last;
+ }
+ if (!PickClosest(tlr, lower_right, last)) {
+ lower_right = last;
+ }
+ }
+ blobl[ti].upper_left = upper_left;
+ blobl[ti].upper_right = upper_right;
+ blobl[ti].lower_right = lower_right;
+ blobl[ti].lower_left = lower_left;
+
+ double error = CheckHistogram(&blobl[ti], upper_left, upper_right,
+ lower_right, lower_left);
+ const double k_hist_threshold = 0.05;
+ if (error < k_hist_threshold) {
+ ret.emplace_back(blobl[ti]);
+ }
+ }
+
+ return ret;
+}
+
+double HistogramBlobFilter::CheckHistogram(SelectedBlob *blob,
+ const Vector<2> &ul,
+ const Vector<2> &ur,
+ const Vector<2> &lr,
+ const Vector<2> &ll) {
+ // found horiz histogram
+ std::vector<double> hist_lr(hist_size_);
+ // step size along left edge
+ Vector<2> delta_left = (ul - ll) * (hist_step_);
+ // step size along right edge
+ Vector<2> delta_right = (ur - lr) * (hist_step_);
+ // sum each left to right line for the histogram
+ Vector<2> s;
+ Vector<2> e;
+ for (int i = 0; i < hist_size_; i++) {
+ s = ll + (i * delta_left);
+ e = lr + (i * delta_right);
+ hist_lr[i] = calcHistComponent(&blob->blob, s, e);
+ if (do_overlay_) {
+ double a = hist_lr[i];
+ Vector<2> mid = a * s + (1.0 - a) * e;
+ overlay_->add_line(s, mid, {0, 0, 255});
+ overlay_->add_line(mid, e, {255, 255, 0});
+ }
+ }
+ double check_vert_up = L22_dist(hist_size_, vert_hist_, hist_lr);
+ double check_vert_fliped = L22_dist(hist_size_, vert_hist_fliped_, hist_lr);
+
+ // found vert histogram
+ std::vector<double> hist_ub(hist_size_);
+ // step size along bottom edge
+ Vector<2> delta_bottom = (ll - lr) * (hist_step_);
+ // step size along top edge
+ Vector<2> delta_top = (ul - ur) * (hist_step_);
+ // sum each top to bottom line for the histogram
+ for (int i = 0; i < hist_size_; i++) {
+ s = ur + (i * delta_top);
+ e = lr + (i * delta_bottom);
+ hist_ub[i] = calcHistComponent(&blob->blob, s, e);
+ if (do_overlay_) {
+ double a = hist_ub[i];
+ Vector<2> mid = a * s + (1.0 - a) * e;
+ overlay_->add_line(s, mid, {0, 0, 255});
+ overlay_->add_line(mid, e, {255, 255, 0});
+ }
+ }
+ double check_horiz = L22_dist(hist_size_, horiz_hist_, hist_ub);
+
+ if (do_overlay_) {
+ Vector<2> A = blob->upper_left + Vector<2>(-10, 10);
+ Vector<2> B = blob->upper_left - Vector<2>(-10, 10);
+ overlay_->add_line(A, B, {255, 255, 255});
+ A = blob->upper_right + Vector<2>(-10, 10);
+ B = blob->upper_right - Vector<2>(-10, 10);
+ overlay_->add_line(A, B, {255, 255, 255});
+ A = blob->lower_right + Vector<2>(-10, 10);
+ B = blob->lower_right - Vector<2>(-10, 10);
+ overlay_->add_line(A, B, {255, 255, 255});
+ A = blob->lower_left + Vector<2>(-10, 10);
+ B = blob->lower_left - Vector<2>(-10, 10);
+ overlay_->add_line(A, B, {255, 255, 255});
+ }
+ // If this target is better upside down, if it is flip the blob.
+ // The horizontal will not be effected, so we will not change that.
+ double check_vert;
+ if (check_vert_up < check_vert_fliped) {
+ // normal one is better, leave it alone
+ check_vert = check_vert_up;
+ } else {
+ check_vert = check_vert_fliped;
+ blob->Flip(fmt_);
+ }
+ if (do_overlay_) {
+ Vector<2> A = blob->upper_left + Vector<2>(10, 10);
+ Vector<2> B = blob->upper_left - Vector<2>(10, 10);
+ overlay_->add_line(A, B, {255, 0, 255});
+ A = blob->upper_right + Vector<2>(10, 10);
+ B = blob->upper_right - Vector<2>(10, 10);
+ overlay_->add_line(A, B, {255, 0, 255});
+ A = blob->lower_right + Vector<2>(10, 10);
+ B = blob->lower_right - Vector<2>(10, 10);
+ overlay_->add_line(A, B, {255, 0, 255});
+ A = blob->lower_left + Vector<2>(10, 10);
+ B = blob->lower_left - Vector<2>(10, 10);
+ overlay_->add_line(A, B, {255, 0, 255});
+ }
+
+ // average the two distances
+ double check = (check_vert + check_horiz) / (2.0 * hist_size_);
+ return check;
+}
+
+double HistogramBlobFilter::calcHistComponent(const RangeImage *blob,
+ const Vector<2> &start,
+ const Vector<2> &end) {
+ int startx = (int)std::floor(start.x());
+ int endx = (int)std::floor(end.x());
+ int starty = (int)std::floor(start.y()) - blob->mini();
+ int endy = (int)std::floor(end.y()) - blob->mini();
+ int dx = std::abs(endx - startx);
+ int dy = std::abs(endy - starty);
+ int sx = (startx < endx) ? 1 : -1;
+ int sy = (starty < endy) ? 1 : -1;
+ int error = dx - dy;
+
+ int total = 0;
+ int value = 0;
+ int total_error;
+ while (1) {
+ total++;
+ if (starty < 0 || starty >= (int)blob->ranges().size()) {
+ printf("starty (%d) size(%d)\n", starty, (int)blob->ranges().size());
+ fflush(stdout);
+ return 0;
+ }
+ const std::vector<ImageRange> &rangel = blob->ranges()[starty];
+ for (const ImageRange &range : rangel) {
+ if (startx >= range.st && startx <= range.ed) {
+ value++;
+ if (do_imgdbg_) {
+ image_->get_px(startx, starty + blob->mini()) = {255, 255, 255};
+ }
+ }
+ }
+
+ // bresenham logic to move along a line
+ if (startx == endx && starty == endy) break;
+ total_error = 2 * error;
+ if (total_error > -dy) {
+ error -= dy;
+ startx += sx;
+ }
+ if (total_error < dx) {
+ error += dx;
+ starty += sy;
+ }
+ }
+ return (double)value / (double)total;
+}
+
+void HistogramBlobFilter::MakeGoalHist(bool is_90) {
+ // calculate a desired histogram before we start
+ double targ_height = 14.0;
+ double targ_width = 20.0;
+ double tape_width = 2.0;
+ horiz_hist_.resize(hist_size_);
+ vert_hist_fliped_.resize(hist_size_);
+ vert_hist_.resize(hist_size_);
+ int j = 0;
+ for (double i = 0; j < hist_size_; i += hist_step_) {
+ if (is_90) {
+ assert(false);
+ } else {
+ if (i < (tape_width / targ_height)) {
+ vert_hist_[j] = 1.0;
+ } else {
+ vert_hist_[j] = 2 * tape_width / targ_width;
+ }
+
+ if (i < tape_width / targ_width || i > 1.0 - (tape_width / targ_width)) {
+ horiz_hist_[j] = 1.0;
+ } else {
+ horiz_hist_[j] = tape_width / targ_height;
+ }
+ }
+ j++;
+ }
+ for (int i = 0; i < hist_size_; i++) {
+ vert_hist_fliped_[hist_size_ - i - 1] = vert_hist_[i];
+ }
+}
+
+} // namespace vision
+} // namespace aos
diff --git a/y2016/vision/blob_filters.h b/y2016/vision/blob_filters.h
new file mode 100644
index 0000000..85d2c94
--- /dev/null
+++ b/y2016/vision/blob_filters.h
@@ -0,0 +1,202 @@
+#ifndef Y2016_VISION_BLOB_FILTERS_H_
+#define Y2016_VISION_BLOB_FILTERS_H_
+
+#include "aos/vision/blob/codec.h"
+#include "aos/vision/blob/find_blob.h"
+#include "aos/vision/blob/hierarchical_contour_merge.h"
+#include "aos/vision/blob/range_image.h"
+#include "aos/vision/blob/threshold.h"
+#include "aos/vision/debug/overlay.h"
+#include "aos/vision/math/segment.h"
+#include "aos/vision/math/vector.h"
+
+namespace aos {
+namespace vision {
+
+struct SelectedBlob {
+ SelectedBlob(const RangeImage &blob_inp) : blob(blob_inp) {}
+ RangeImage blob;
+ Vector<2> upper_left;
+ Vector<2> upper_right;
+ Vector<2> lower_right;
+ Vector<2> lower_left;
+
+ void Flip(ImageFormat fmt) {
+ auto image_width = fmt.w;
+ auto image_height = fmt.h;
+ blob.Flip(fmt);
+
+ Vector<2> tmp = lower_right;
+ lower_right = upper_left;
+ upper_left = tmp;
+ tmp = lower_left;
+ lower_left = upper_right;
+ upper_right = tmp;
+
+ // now flip the box
+ lower_right = Vector<2>(image_width - lower_right.x(),
+ image_height - lower_right.y());
+ upper_right = Vector<2>(image_width - upper_right.x(),
+ image_height - upper_right.y());
+ lower_left =
+ Vector<2>(image_width - lower_left.x(), image_height - lower_left.y());
+ upper_left =
+ Vector<2>(image_width - upper_left.x(), image_height - upper_left.y());
+ }
+};
+
+class CornerFinder {
+ public:
+ CornerFinder(float merge_rate, int min_line_length)
+ : merge_rate_(merge_rate), min_len_(min_line_length) {}
+
+ CornerFinder() : CornerFinder(1.0, 25) {}
+
+ // score how well the line matches the points. Lower score is better.
+ double LineScore(Vector<2> A, Vector<2> B, FittedLine line);
+
+ // We want to save the "best" two lines. Here we will difine that as the
+ // longest
+ // two we see.
+ void EnqueueLine(std::vector<FittedLine> *list, FittedLine line);
+ // Lines come in as two sides of the tape, we will flip them around so they
+ // have the same orientation then avrage the two end points to get a line in
+ // the center.
+ Segment<2> MakeLine(const std::vector<FittedLine> &list);
+
+ // take the given blob and find lines to represent it, then return the
+ // target
+ // corners from those lines. Left first, then bottom.
+ std::vector<std::pair<Vector<2>, Vector<2>>> Find(
+ const std::vector<SelectedBlob> &blobl);
+
+ // Enable overlay debugging.
+ void EnableOverlay(PixelLinesOverlay *overlay) {
+ do_overlay_ = true;
+ overlay_ = overlay;
+ }
+
+ private:
+ // Parker did some sort of optimization with the memory.
+ AnalysisAllocator alloc_;
+
+ // check if we shuld draw the overlay
+ bool do_overlay_ = false;
+ PixelLinesOverlay *overlay_ = NULL;
+
+ // how smooth do we want the lines
+ float merge_rate_;
+ // how short do we want the lines
+ int min_len_;
+};
+
+class BlobFilterBase {
+ public:
+ BlobFilterBase(int min_blob_area, int max_blob_area)
+ : min_area_(min_blob_area), max_area_(max_blob_area) {}
+
+ std::vector<SelectedBlob> PreFilter(const BlobList &blobl);
+
+ std::vector<SelectedBlob> FilterBlobs(const BlobList &blobl) {
+ return PostFilter(PreFilter(blobl));
+ }
+
+ // Enable overlay debugging.
+ void EnableOverlay(PixelLinesOverlay *overlay) {
+ do_overlay_ = true;
+ overlay_ = overlay;
+ }
+
+ private:
+ virtual std::vector<SelectedBlob> PostFilter(
+ std::vector<SelectedBlob> blobl) = 0;
+
+ protected:
+ // absolute minimum for even looking at a blob.
+ int min_area_;
+ // absolute maximum for even looking at a blob.
+ int max_area_;
+
+ // check if we shuld draw the overlay
+ bool do_overlay_ = false;
+ PixelLinesOverlay *overlay_ = NULL;
+};
+
+class HistogramBlobFilter : public BlobFilterBase {
+ public:
+ HistogramBlobFilter(ImageFormat fmt, int hist_size, int min_blob_area,
+ int max_blob_area)
+ : BlobFilterBase(min_blob_area, max_blob_area),
+ fmt_(fmt),
+ hist_size_(hist_size),
+ hist_step_(1.0 / (double)hist_size) {
+ MakeGoalHist(false);
+ }
+
+ // Enable image debugging.
+ void EnableImageHist(ImagePtr *img) {
+ do_imgdbg_ = true;
+ image_ = img;
+ }
+
+ private:
+ // Returns the point closest to the goal point.
+ bool PickClosest(const Vector<2> &goal, const Vector<2> &A,
+ const Vector<2> &B);
+ // Main filter function.
+ std::vector<SelectedBlob> PostFilter(std::vector<SelectedBlob> blobl);
+
+ // calc and compare the histograms to the desired
+ double CheckHistogram(SelectedBlob *blob, const Vector<2> &ul,
+ const Vector<2> &ur, const Vector<2> &lr,
+ const Vector<2> &ll);
+
+ // sum over values between these two points and normalize
+ // see Bresenham's Line Algorithm for the logic of moving
+ // over all the pixels between these two points.
+ double calcHistComponent(const RangeImage *blob, const Vector<2> &start,
+ const Vector<2> &end);
+
+ void MakeGoalHist(bool is_90);
+
+ // just a distance function
+ double chiSquared(int length, double *histA, double *histB) {
+ double sum = 0;
+ for (int i = 0; i < length; i++) {
+ double diff = *(histB + i) - *(histA + i);
+ sum += (diff * diff) / *(histA + i);
+ }
+ return sum;
+ }
+
+ // squared euclidiean dist function
+ double L22_dist(int length, std::vector<double> &histA,
+ std::vector<double> histB) {
+ double sum = 0;
+ for (int i = 0; i < length; i++) {
+ double diff = histB[i] - histA[i];
+ sum += (diff * diff);
+ }
+ return sum;
+ }
+
+ // size of image
+ ImageFormat fmt_;
+ // Number of elements in eaach histogram.
+ int hist_size_;
+ // Percent step of the above size.
+ double hist_step_;
+ // histogram summing in y.
+ std::vector<double> horiz_hist_;
+ // histogram summing in x direction.
+ std::vector<double> vert_hist_;
+ // histogram summing in x from greatest to least.
+ std::vector<double> vert_hist_fliped_;
+ bool do_imgdbg_ = false;
+ ImagePtr *image_ = NULL;
+};
+
+} // namespace vision
+} // namespace aos
+
+#endif // Y2016_VISION_BLOB_FILTERS_H_
diff --git a/y2016/vision/stereo_geometry.h b/y2016/vision/stereo_geometry.h
index 93f3892..a758f1c 100644
--- a/y2016/vision/stereo_geometry.h
+++ b/y2016/vision/stereo_geometry.h
@@ -3,8 +3,8 @@
#include <string>
-#include "aos/vision/math/vector.h"
#include "aos/common/logging/logging.h"
+#include "aos/vision/math/vector.h"
#include "y2016/vision/calibration.pb.h"
diff --git a/y2016/vision/target_receiver.cc b/y2016/vision/target_receiver.cc
index 7ade8e7..3e4465d 100644
--- a/y2016/vision/target_receiver.cc
+++ b/y2016/vision/target_receiver.cc
@@ -1,5 +1,5 @@
-#include <stdlib.h>
#include <netdb.h>
+#include <stdlib.h>
#include <unistd.h>
#include <array>
@@ -10,19 +10,19 @@
#include <thread>
#include <vector>
-#include "aos/linux_code/init.h"
-#include "aos/common/time.h"
#include "aos/common/logging/logging.h"
#include "aos/common/logging/queue_logging.h"
-#include "aos/vision/events/udp.h"
#include "aos/common/mutex.h"
+#include "aos/common/time.h"
+#include "aos/linux_code/init.h"
+#include "aos/vision/events/udp.h"
#include "frc971/control_loops/drivetrain/drivetrain.q.h"
+#include "y2016/constants.h"
+#include "y2016/vision/stereo_geometry.h"
#include "y2016/vision/vision.q.h"
#include "y2016/vision/vision_data.pb.h"
-#include "y2016/vision/stereo_geometry.h"
-#include "y2016/constants.h"
namespace y2016 {
namespace vision {
@@ -179,11 +179,12 @@
double angle, double last_angle,
::aos::vision::Vector<2> *interpolated_result,
double *interpolated_angle) {
- const double age_ratio =
- chrono::duration_cast<chrono::duration<double>>(
- older.capture_time() - newer.last_capture_time()).count() /
- chrono::duration_cast<chrono::duration<double>>(
- newer.capture_time() - newer.last_capture_time()).count();
+ const double age_ratio = chrono::duration_cast<chrono::duration<double>>(
+ older.capture_time() - newer.last_capture_time())
+ .count() /
+ chrono::duration_cast<chrono::duration<double>>(
+ newer.capture_time() - newer.last_capture_time())
+ .count();
interpolated_result->Set(
newer_center.x() * age_ratio + (1 - age_ratio) * last_newer_center.x(),
newer_center.y() * age_ratio + (1 - age_ratio) * last_newer_center.y());
@@ -226,9 +227,11 @@
status->drivetrain_right_position = before.right;
} else {
const double age_ratio = chrono::duration_cast<chrono::duration<double>>(
- capture_time - before.time).count() /
+ capture_time - before.time)
+ .count() /
chrono::duration_cast<chrono::duration<double>>(
- after.time - before.time).count();
+ after.time - before.time)
+ .count();
status->drivetrain_left_position =
before.left * (1 - age_ratio) + after.left * age_ratio;
status->drivetrain_right_position =
@@ -313,7 +316,7 @@
CameraHandler left;
CameraHandler right;
- ::aos::vision::RXUdpSocket recv(8080);
+ ::aos::events::RXUdpSocket recv(8080);
char rawData[65507];
while (true) {
@@ -358,8 +361,8 @@
double last_angle_left;
double last_angle_right;
SelectTargets(left.last_target(), right.last_target(),
- &last_center_left, &last_center_right,
- &last_angle_left, &last_angle_right);
+ &last_center_left, &last_center_right, &last_angle_left,
+ &last_angle_right);
::aos::vision::Vector<2> filtered_center_left(0.0, 0.0);
::aos::vision::Vector<2> filtered_center_right(0.0, 0.0);
@@ -370,7 +373,8 @@
filtered_angle_left = angle_left;
new_vision_status->target_time =
chrono::duration_cast<chrono::nanoseconds>(
- left.capture_time().time_since_epoch()).count();
+ left.capture_time().time_since_epoch())
+ .count();
CalculateFiltered(left, right, center_right, last_center_right,
angle_right, last_angle_right,
&filtered_center_right, &filtered_angle_right);
@@ -379,7 +383,8 @@
filtered_angle_right = angle_right;
new_vision_status->target_time =
chrono::duration_cast<chrono::nanoseconds>(
- right.capture_time().time_since_epoch()).count();
+ right.capture_time().time_since_epoch())
+ .count();
CalculateFiltered(right, left, center_left, last_center_left,
angle_left, last_angle_left, &filtered_center_left,
&filtered_angle_left);
@@ -393,7 +398,8 @@
new_vision_status->right_image_timestamp =
right.target().image_timestamp();
new_vision_status->left_send_timestamp = left.target().send_timestamp();
- new_vision_status->right_send_timestamp = right.target().send_timestamp();
+ new_vision_status->right_send_timestamp =
+ right.target().send_timestamp();
new_vision_status->horizontal_angle = horizontal_angle;
new_vision_status->vertical_angle = vertical_angle;
new_vision_status->distance = distance;
diff --git a/y2016/vision/target_sender.cc b/y2016/vision/target_sender.cc
new file mode 100644
index 0000000..706c348
--- /dev/null
+++ b/y2016/vision/target_sender.cc
@@ -0,0 +1,242 @@
+#include <stdio.h>
+#include <stdlib.h>
+#include <fstream>
+#include <iostream>
+#include <memory>
+#include <thread>
+#include <vector>
+
+#include "aos/common/logging/implementations.h"
+#include "aos/common/logging/logging.h"
+#include "aos/common/time.h"
+#include "aos/vision/events/socket_types.h"
+#include "aos/vision/events/udp.h"
+#include "aos/vision/image/image_stream.h"
+#include "aos/vision/image/jpeg_routines.h"
+#include "aos/vision/image/reader.h"
+#include "y2016/vision/blob_filters.h"
+#include "y2016/vision/stereo_geometry.h"
+#include "y2016/vision/vision_data.pb.h"
+
+namespace y2016 {
+namespace vision {
+using aos::vision::ImageStreamEvent;
+using aos::vision::DataRef;
+using aos::events::TCPServer;
+using aos::vision::BlobLRef;
+using aos::vision::Vector;
+using aos::vision::Int32Codec;
+using aos::vision::BlobList;
+using aos::vision::RangeImage;
+using aos::vision::PixelRef;
+using aos::vision::ImageValue;
+using aos::vision::HistogramBlobFilter;
+using aos::vision::CornerFinder;
+using aos::vision::Int64Codec;
+using aos::events::TXUdpSocket;
+using aos::events::DataSocket;
+using aos::vision::ImageFormat;
+
+::camera::CameraParams GetCameraParams(const Calibration &calibration) {
+ return ::camera::CameraParams{.width = calibration.camera_image_width(),
+ .height = calibration.camera_image_height(),
+ .exposure = calibration.camera_exposure(),
+ .brightness = calibration.camera_brightness(),
+ .gain = calibration.camera_gain(),
+ .fps = calibration.camera_fps()};
+}
+
+int64_t Nanos(aos::monotonic_clock::duration time_diff) {
+ return std::chrono::duration_cast<std::chrono::nanoseconds>(time_diff)
+ .count();
+}
+
+int64_t NowNanos() {
+ return Nanos(aos::monotonic_clock::now().time_since_epoch());
+}
+
+inline bool FileExist(const std::string &name) {
+ struct stat buffer;
+ return (stat(name.c_str(), &buffer) == 0);
+}
+
+class ImageSender : public ImageStreamEvent {
+ public:
+ ImageSender(int camera_index, camera::CameraParams params,
+ const std::string &fname, const std::string &ipadder, int port)
+ : ImageStreamEvent(fname, params),
+ camera_index_(camera_index),
+ udp_serv_(ipadder, 8080),
+ tcp_serv_(port),
+ blob_filt_(ImageFormat(params.width, params.height), 40, 750, 250000),
+ finder_(0.25, 35) {
+ int index = 0;
+ while (true) {
+ std::string file = "./logging/blob_record_" + std::to_string(index) +
+ "_" + std::to_string(camera_index_) + ".dat";
+ if (FileExist(file)) {
+ index++;
+ } else {
+ printf("Logging to file (%s)\n", file.c_str());
+ ofst_.open(file);
+ assert(ofst_.is_open());
+ break;
+ }
+ }
+ }
+
+ ~ImageSender() { ofst_.close(); }
+
+ void ProcessImage(DataRef data, aos::monotonic_clock::time_point tp) {
+ int64_t timestamp = std::chrono::duration_cast<std::chrono::nanoseconds>(
+ tp.time_since_epoch())
+ .count();
+ DecodeJpeg(data, &image_);
+ auto fmt = image_.fmt();
+
+ RangeImage rimg = DoThreshold(image_.get(), [](PixelRef &px) {
+ return (px.g > 88);
+ });
+
+ // flip the right image as this camera is mount backward
+ if (camera_index_ == 0) {
+ rimg.Flip(fmt.w, fmt.h);
+ }
+
+ BlobList blobl = aos::vision::FindBlobs(rimg);
+ auto whatever = blob_filt_.FilterBlobs(blobl);
+
+ VisionData target;
+ target.set_camera_index(camera_index_);
+ target.set_image_timestamp(timestamp);
+
+ if (!whatever.empty()) {
+ std::vector<std::pair<Vector<2>, Vector<2>>> corners =
+ finder_.Find(whatever);
+
+ if (!corners.empty()) {
+ for (int i = 0; i < (int)corners.size(); i++) {
+ Target *pos = target.add_target();
+ pos->set_left_corner_x(corners[i].first.x());
+ pos->set_left_corner_y(corners[i].first.y());
+ pos->set_right_corner_x(corners[i].second.x());
+ pos->set_right_corner_y(corners[i].second.y());
+ }
+ }
+ }
+ target.set_send_timestamp(NowNanos());
+
+ // always send data
+ std::string dat;
+ if (target.SerializeToString(&dat)) {
+ if (print_once_) {
+ printf("Beginning data streaming camera %d...\n", camera_index_);
+ print_once_ = false;
+ }
+
+ // Send only target over udp.
+ udp_serv_.Send(dat.data(), dat.size());
+ }
+
+ // Write blob to file for logging.
+ int blob_size = CalculateSize(blobl);
+ int tmp_size = blob_size + sizeof(int32_t) + sizeof(uint64_t);
+ char *buf;
+ blob_data_.resize(tmp_size, 0);
+ {
+ buf = Int32Codec::Write(&blob_data_[0], tmp_size);
+ buf = Int64Codec::Write(buf, timestamp);
+ SerializeBlob(blobl, buf);
+ }
+ ofst_.write(&blob_data_[0], tmp_size);
+
+ // Add blob debug
+ bool debug = true;
+ if (debug) {
+ target.set_raw(buf, blob_size);
+ if (target.SerializeToString(&dat)) {
+ tcp_serv_.Broadcast([&](DataSocket *client) { client->Emit(dat); });
+ }
+ }
+
+ bool timing = false;
+ if (timing) {
+ if (n_time > 0) {
+ auto now = aos::monotonic_clock::now();
+ printf("%g, %g\n",
+ (((double)Nanos(now - tstart)) / (double)(n_time)) / 1e6,
+ (double)Nanos(now - tp) / 1e6);
+ } else {
+ tstart = aos::monotonic_clock::now();
+ }
+ ++n_time;
+ }
+ }
+
+ TCPServer<DataSocket> *GetTCPServ() { return &tcp_serv_; }
+
+ // print when we start
+ bool print_once_ = true;
+
+ // left or right camera
+ int camera_index_;
+
+ // udp socket on which to send to robot
+ TXUdpSocket udp_serv_;
+
+ // tcp socket on which to debug to laptop
+ TCPServer<DataSocket> tcp_serv_;
+
+ // our blob processing object
+ HistogramBlobFilter blob_filt_;
+
+ // corner finder to align aiming
+ CornerFinder finder_;
+
+ ImageValue image_;
+ std::string blob_data_;
+ std::ofstream ofst_;
+ aos::monotonic_clock::time_point tstart;
+ int n_time = 0;
+
+ private:
+};
+
+void RunCamera(int camera_index, camera::CameraParams params,
+ const std::string &device, const std::string &ip_addr,
+ int port) {
+ printf("Creating camera %d (%d, %d).\n", camera_index, params.width,
+ params.height);
+ ImageSender strm(camera_index, params, device, ip_addr, port);
+
+ aos::events::EpollLoop loop;
+ loop.Add(strm.GetTCPServ());
+ loop.Add(&strm);
+ printf("Running Camera (%d)\n", camera_index);
+ loop.Run();
+}
+
+} // namespace vision
+} // namespace y2016
+
+int main(int, char **) {
+ using namespace y2016::vision;
+ StereoGeometry stereo("./stereo_rig.calib");
+ ::aos::logging::Init();
+ ::aos::logging::AddImplementation(
+ new ::aos::logging::StreamLogImplementation(stdout));
+ std::thread cam0([stereo]() {
+ RunCamera(0, GetCameraParams(stereo.calibration()),
+ stereo.calibration().right_camera_name(),
+ stereo.calibration().roborio_ip_addr(), 8082);
+ });
+ std::thread cam1([stereo]() {
+ RunCamera(1, GetCameraParams(stereo.calibration()),
+ stereo.calibration().left_camera_name(),
+ stereo.calibration().roborio_ip_addr(), 8082);
+ });
+ cam0.join();
+ cam1.join();
+
+ return EXIT_SUCCESS;
+}
diff --git a/y2017/constants.cc b/y2017/constants.cc
index 33deee3..36cf348 100644
--- a/y2017/constants.cc
+++ b/y2017/constants.cc
@@ -38,18 +38,21 @@
case 1: // for tests
return new Values{
5.0, // drivetrain max speed
+ 0.0, // down error
};
break;
case kCompTeamNumber:
return new Values{
5.0, // drivetrain max speed
+ 0.0, // down error
};
break;
case kPracticeTeamNumber:
return new Values{
5.0, // drivetrain max speed
+ 0.0, // down error
};
break;
diff --git a/y2017/constants.h b/y2017/constants.h
index be29a2a..3ca12c9 100644
--- a/y2017/constants.h
+++ b/y2017/constants.h
@@ -25,6 +25,8 @@
// ///// Dynamic constants. /////
double drivetrain_max_speed;
+
+ double down_error;
};
// Creates (once) a Values instance for ::aos::network::GetTeamNumber() and
diff --git a/y2017/control_loops/drivetrain/BUILD b/y2017/control_loops/drivetrain/BUILD
new file mode 100644
index 0000000..c44da31
--- /dev/null
+++ b/y2017/control_loops/drivetrain/BUILD
@@ -0,0 +1,78 @@
+package(default_visibility = ['//visibility:public'])
+
+load('/aos/build/queues', 'queue_library')
+
+genrule(
+ name = 'genrule_drivetrain',
+ visibility = ['//visibility:private'],
+ cmd = '$(location //y2017/control_loops/python:drivetrain) $(OUTS)',
+ tools = [
+ '//y2017/control_loops/python:drivetrain',
+ ],
+ outs = [
+ 'drivetrain_dog_motor_plant.h',
+ 'drivetrain_dog_motor_plant.cc',
+ 'kalman_drivetrain_motor_plant.h',
+ 'kalman_drivetrain_motor_plant.cc',
+ ],
+)
+
+genrule(
+ name = 'genrule_polydrivetrain',
+ visibility = ['//visibility:private'],
+ cmd = '$(location //y2017/control_loops/python:polydrivetrain) $(OUTS)',
+ tools = [
+ '//y2017/control_loops/python:polydrivetrain',
+ ],
+ outs = [
+ 'polydrivetrain_dog_motor_plant.h',
+ 'polydrivetrain_dog_motor_plant.cc',
+ 'polydrivetrain_cim_plant.h',
+ 'polydrivetrain_cim_plant.cc',
+ ],
+)
+
+cc_library(
+ name = 'polydrivetrain_plants',
+ srcs = [
+ 'polydrivetrain_dog_motor_plant.cc',
+ 'drivetrain_dog_motor_plant.cc',
+ 'kalman_drivetrain_motor_plant.cc',
+ ],
+ hdrs = [
+ 'polydrivetrain_dog_motor_plant.h',
+ 'drivetrain_dog_motor_plant.h',
+ 'kalman_drivetrain_motor_plant.h',
+ ],
+ deps = [
+ '//frc971/control_loops:state_feedback_loop',
+ ],
+)
+
+cc_library(
+ name = 'drivetrain_base',
+ srcs = [
+ 'drivetrain_base.cc',
+ ],
+ hdrs = [
+ 'drivetrain_base.h',
+ ],
+ deps = [
+ ':polydrivetrain_plants',
+ '//frc971/control_loops/drivetrain:drivetrain_config',
+ '//frc971:shifter_hall_effect',
+ '//y2017:constants',
+ ],
+)
+
+cc_binary(
+ name = 'drivetrain',
+ srcs = [
+ 'drivetrain_main.cc',
+ ],
+ deps = [
+ ':drivetrain_base',
+ '//aos/linux_code:init',
+ '//frc971/control_loops/drivetrain:drivetrain_lib',
+ ],
+)
diff --git a/y2017/control_loops/drivetrain/drivetrain_base.cc b/y2017/control_loops/drivetrain/drivetrain_base.cc
new file mode 100644
index 0000000..03e180b
--- /dev/null
+++ b/y2017/control_loops/drivetrain/drivetrain_base.cc
@@ -0,0 +1,49 @@
+#include "y2017/control_loops/drivetrain/drivetrain_base.h"
+
+#include "frc971/control_loops/drivetrain/drivetrain_config.h"
+
+#include "frc971/control_loops/state_feedback_loop.h"
+#include "y2017/control_loops/drivetrain/drivetrain_dog_motor_plant.h"
+#include "y2017/control_loops/drivetrain/polydrivetrain_dog_motor_plant.h"
+#include "y2017/control_loops/drivetrain/kalman_drivetrain_motor_plant.h"
+#include "y2017/constants.h"
+
+using ::frc971::control_loops::drivetrain::DrivetrainConfig;
+
+namespace y2017 {
+namespace control_loops {
+namespace drivetrain {
+
+using ::frc971::constants::ShifterHallEffect;
+
+const ShifterHallEffect kThreeStateDriveShifter{0.0, 0.0, 0.0, 0.0, 0.25, 0.75};
+
+const DrivetrainConfig &GetDrivetrainConfig() {
+ static DrivetrainConfig kDrivetrainConfig{
+ ::frc971::control_loops::drivetrain::ShifterType::HALL_EFFECT_SHIFTER,
+ ::frc971::control_loops::drivetrain::LoopType::CLOSED_LOOP,
+
+ ::y2017::control_loops::drivetrain::MakeDrivetrainLoop,
+ ::y2017::control_loops::drivetrain::MakeVelocityDrivetrainLoop,
+ ::y2017::control_loops::drivetrain::MakeKFDrivetrainLoop,
+
+ drivetrain::kDt,
+ drivetrain::kRobotRadius,
+ drivetrain::kWheelRadius,
+ drivetrain::kV,
+
+ drivetrain::kHighGearRatio,
+ drivetrain::kHighGearRatio,
+ kThreeStateDriveShifter,
+ kThreeStateDriveShifter,
+ true,
+ constants::GetValues().down_error,
+ 0.25,
+ 1.0};
+
+ return kDrivetrainConfig;
+};
+
+} // namespace drivetrain
+} // namespace control_loops
+} // namespace y2017
diff --git a/y2017/control_loops/drivetrain/drivetrain_base.h b/y2017/control_loops/drivetrain/drivetrain_base.h
new file mode 100644
index 0000000..6ef519b
--- /dev/null
+++ b/y2017/control_loops/drivetrain/drivetrain_base.h
@@ -0,0 +1,17 @@
+#ifndef Y2017_CONTROL_LOOPS_DRIVETRAIN_DRIVETRAIN_BASE_H_
+#define Y2017_CONTROL_LOOPS_DRIVETRAIN_DRIVETRAIN_BASE_H_
+
+#include "frc971/control_loops/drivetrain/drivetrain_config.h"
+
+namespace y2017 {
+namespace control_loops {
+namespace drivetrain {
+
+const ::frc971::control_loops::drivetrain::DrivetrainConfig &
+GetDrivetrainConfig();
+
+} // namespace drivetrain
+} // namespace control_loops
+} // namespace y2017
+
+#endif // Y2017_CONTROL_LOOPS_DRIVETRAIN_DRIVETRAIN_BASE_H_
diff --git a/y2017/control_loops/drivetrain/drivetrain_main.cc b/y2017/control_loops/drivetrain/drivetrain_main.cc
new file mode 100644
index 0000000..aab4ea0
--- /dev/null
+++ b/y2017/control_loops/drivetrain/drivetrain_main.cc
@@ -0,0 +1,15 @@
+#include "aos/linux_code/init.h"
+
+#include "frc971/control_loops/drivetrain/drivetrain.h"
+#include "y2017/control_loops/drivetrain/drivetrain_base.h"
+
+using ::frc971::control_loops::drivetrain::DrivetrainLoop;
+
+int main() {
+ ::aos::Init();
+ DrivetrainLoop drivetrain(
+ ::y2017::control_loops::drivetrain::GetDrivetrainConfig());
+ drivetrain.Run();
+ ::aos::Cleanup();
+ return 0;
+}
diff --git a/y2017/control_loops/python/BUILD b/y2017/control_loops/python/BUILD
new file mode 100644
index 0000000..56a9f3d
--- /dev/null
+++ b/y2017/control_loops/python/BUILD
@@ -0,0 +1,39 @@
+package(default_visibility = ['//y2017:__subpackages__'])
+
+py_binary(
+ name = 'drivetrain',
+ srcs = [
+ 'drivetrain.py',
+ ],
+ deps = [
+ '//external:python-gflags',
+ '//external:python-glog',
+ '//frc971/control_loops/python:controls',
+ ],
+)
+
+py_binary(
+ name = 'polydrivetrain',
+ srcs = [
+ 'polydrivetrain.py',
+ 'drivetrain.py',
+ ],
+ deps = [
+ '//external:python-gflags',
+ '//external:python-glog',
+ '//frc971/control_loops/python:controls',
+ ],
+)
+
+py_library(
+ name = 'polydrivetrain_lib',
+ srcs = [
+ 'polydrivetrain.py',
+ 'drivetrain.py',
+ ],
+ deps = [
+ '//external:python-gflags',
+ '//external:python-glog',
+ '//frc971/control_loops/python:controls',
+ ],
+)
diff --git a/y2017/control_loops/python/drivetrain.py b/y2017/control_loops/python/drivetrain.py
new file mode 100755
index 0000000..a9e5101
--- /dev/null
+++ b/y2017/control_loops/python/drivetrain.py
@@ -0,0 +1,356 @@
+#!/usr/bin/python
+
+from frc971.control_loops.python import control_loop
+from frc971.control_loops.python import controls
+import numpy
+import sys
+from matplotlib import pylab
+
+import gflags
+import glog
+
+FLAGS = gflags.FLAGS
+
+gflags.DEFINE_bool('plot', False, 'If true, plot the loop response.')
+
+
+class Drivetrain(control_loop.ControlLoop):
+ def __init__(self, name="Drivetrain", left_low=True, right_low=True):
+ super(Drivetrain, self).__init__(name)
+ # Number of motors per side
+ self.num_motors = 2
+ # Stall Torque in N m
+ self.stall_torque = 2.42 * self.num_motors * 0.60
+ # Stall Current in Amps
+ self.stall_current = 133.0 * self.num_motors
+ # Free Speed in RPM. Used number from last year.
+ self.free_speed = 5500.0
+ # Free Current in Amps
+ self.free_current = 4.7 * self.num_motors
+ # Moment of inertia of the drivetrain in kg m^2
+ self.J = 1.5
+ # Mass of the robot, in kg.
+ self.m = 50
+ # Radius of the robot, in meters (requires tuning by hand)
+ self.rb = 0.6 / 2.0
+ # Radius of the wheels, in meters.
+ self.r = 0.041275
+ # Resistance of the motor, divided by the number of motors.
+ self.resistance = 12.0 / self.stall_current
+ # Motor velocity constant
+ self.Kv = ((self.free_speed / 60.0 * 2.0 * numpy.pi) /
+ (12.0 - self.resistance * self.free_current))
+ # Torque constant
+ self.Kt = self.stall_torque / self.stall_current
+ # Gear ratios
+ self.G_low = 12.0 / 54.0
+ self.G_high = 12.0 / 54.0
+ if left_low:
+ self.Gl = self.G_low
+ else:
+ self.Gl = self.G_high
+ if right_low:
+ self.Gr = self.G_low
+ else:
+ self.Gr = self.G_high
+
+ # Control loop time step
+ self.dt = 0.005
+
+ # These describe the way that a given side of a robot will be influenced
+ # by the other side. Units of 1 / kg.
+ self.msp = 1.0 / self.m + self.rb * self.rb / self.J
+ self.msn = 1.0 / self.m - self.rb * self.rb / self.J
+ # The calculations which we will need for A and B.
+ self.tcl = -self.Kt / self.Kv / (self.Gl * self.Gl * self.resistance * self.r * self.r)
+ self.tcr = -self.Kt / self.Kv / (self.Gr * self.Gr * self.resistance * self.r * self.r)
+ self.mpl = self.Kt / (self.Gl * self.resistance * self.r)
+ self.mpr = self.Kt / (self.Gr * self.resistance * self.r)
+
+ # State feedback matrices
+ # X will be of the format
+ # [[positionl], [velocityl], [positionr], velocityr]]
+ self.A_continuous = numpy.matrix(
+ [[0, 1, 0, 0],
+ [0, self.msp * self.tcl, 0, self.msn * self.tcr],
+ [0, 0, 0, 1],
+ [0, self.msn * self.tcl, 0, self.msp * self.tcr]])
+ self.B_continuous = numpy.matrix(
+ [[0, 0],
+ [self.msp * self.mpl, self.msn * self.mpr],
+ [0, 0],
+ [self.msn * self.mpl, self.msp * self.mpr]])
+ self.C = numpy.matrix([[1, 0, 0, 0],
+ [0, 0, 1, 0]])
+ self.D = numpy.matrix([[0, 0],
+ [0, 0]])
+
+ self.A, self.B = self.ContinuousToDiscrete(
+ self.A_continuous, self.B_continuous, self.dt)
+
+ if left_low or right_low:
+ q_pos = 0.12
+ q_vel = 1.0
+ else:
+ q_pos = 0.14
+ q_vel = 0.95
+
+ # Tune the LQR controller
+ self.Q = numpy.matrix([[(1.0 / (q_pos ** 2.0)), 0.0, 0.0, 0.0],
+ [0.0, (1.0 / (q_vel ** 2.0)), 0.0, 0.0],
+ [0.0, 0.0, (1.0 / (q_pos ** 2.0)), 0.0],
+ [0.0, 0.0, 0.0, (1.0 / (q_vel ** 2.0))]])
+
+ self.R = numpy.matrix([[(1.0 / (12.0 ** 2.0)), 0.0],
+ [0.0, (1.0 / (12.0 ** 2.0))]])
+ self.K = controls.dlqr(self.A, self.B, self.Q, self.R)
+
+ glog.debug('DT q_pos %f q_vel %s %s', q_pos, q_vel, name)
+ glog.debug(str(numpy.linalg.eig(self.A - self.B * self.K)[0]))
+ glog.debug('K %s', repr(self.K))
+
+ self.hlp = 0.3
+ self.llp = 0.4
+ self.PlaceObserverPoles([self.hlp, self.hlp, self.llp, self.llp])
+
+ self.U_max = numpy.matrix([[12.0], [12.0]])
+ self.U_min = numpy.matrix([[-12.0], [-12.0]])
+
+ self.InitializeState()
+
+
+class KFDrivetrain(Drivetrain):
+ def __init__(self, name="KFDrivetrain", left_low=True, right_low=True):
+ super(KFDrivetrain, self).__init__(name, left_low, right_low)
+
+ self.unaugmented_A_continuous = self.A_continuous
+ self.unaugmented_B_continuous = self.B_continuous
+
+ # The practical voltage applied to the wheels is
+ # V_left = U_left + left_voltage_error
+ #
+ # The states are
+ # [left position, left velocity, right position, right velocity,
+ # left voltage error, right voltage error, angular_error]
+ #
+ # The left and right positions are filtered encoder positions and are not
+ # adjusted for heading error.
+ # The turn velocity as computed by the left and right velocities is
+ # adjusted by the gyro velocity.
+ # The angular_error is the angular velocity error between the wheel speed
+ # and the gyro speed.
+ self.A_continuous = numpy.matrix(numpy.zeros((7, 7)))
+ self.B_continuous = numpy.matrix(numpy.zeros((7, 2)))
+ self.A_continuous[0:4,0:4] = self.unaugmented_A_continuous
+ self.A_continuous[0:4,4:6] = self.unaugmented_B_continuous
+ self.B_continuous[0:4,0:2] = self.unaugmented_B_continuous
+ self.A_continuous[0,6] = 1
+ self.A_continuous[2,6] = -1
+
+ self.A, self.B = self.ContinuousToDiscrete(
+ self.A_continuous, self.B_continuous, self.dt)
+
+ self.C = numpy.matrix([[1, 0, 0, 0, 0, 0, 0],
+ [0, 0, 1, 0, 0, 0, 0],
+ [0, -0.5 / self.rb, 0, 0.5 / self.rb, 0, 0, 0]])
+
+ self.D = numpy.matrix([[0, 0],
+ [0, 0],
+ [0, 0]])
+
+ q_pos = 0.05
+ q_vel = 1.00
+ q_voltage = 10.0
+ q_encoder_uncertainty = 2.00
+
+ self.Q = numpy.matrix([[(q_pos ** 2.0), 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
+ [0.0, (q_vel ** 2.0), 0.0, 0.0, 0.0, 0.0, 0.0],
+ [0.0, 0.0, (q_pos ** 2.0), 0.0, 0.0, 0.0, 0.0],
+ [0.0, 0.0, 0.0, (q_vel ** 2.0), 0.0, 0.0, 0.0],
+ [0.0, 0.0, 0.0, 0.0, (q_voltage ** 2.0), 0.0, 0.0],
+ [0.0, 0.0, 0.0, 0.0, 0.0, (q_voltage ** 2.0), 0.0],
+ [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, (q_encoder_uncertainty ** 2.0)]])
+
+ r_pos = 0.0001
+ r_gyro = 0.000001
+ self.R = numpy.matrix([[(r_pos ** 2.0), 0.0, 0.0],
+ [0.0, (r_pos ** 2.0), 0.0],
+ [0.0, 0.0, (r_gyro ** 2.0)]])
+
+ # Solving for kf gains.
+ self.KalmanGain, self.Q_steady = controls.kalman(
+ A=self.A, B=self.B, C=self.C, Q=self.Q, R=self.R)
+
+ self.L = self.A * self.KalmanGain
+
+ unaug_K = self.K
+
+ # Implement a nice closed loop controller for use by the closed loop
+ # controller.
+ self.K = numpy.matrix(numpy.zeros((self.B.shape[1], self.A.shape[0])))
+ self.K[0:2, 0:4] = unaug_K
+ self.K[0, 4] = 1.0
+ self.K[1, 5] = 1.0
+
+ self.Qff = numpy.matrix(numpy.zeros((4, 4)))
+ qff_pos = 0.005
+ qff_vel = 1.00
+ self.Qff[0, 0] = 1.0 / qff_pos ** 2.0
+ self.Qff[1, 1] = 1.0 / qff_vel ** 2.0
+ self.Qff[2, 2] = 1.0 / qff_pos ** 2.0
+ self.Qff[3, 3] = 1.0 / qff_vel ** 2.0
+ self.Kff = numpy.matrix(numpy.zeros((2, 7)))
+ self.Kff[0:2, 0:4] = controls.TwoStateFeedForwards(self.B[0:4,:], self.Qff)
+
+ self.InitializeState()
+
+
+def main(argv):
+ argv = FLAGS(argv)
+ glog.init()
+
+ # Simulate the response of the system to a step input.
+ drivetrain = Drivetrain(left_low=False, right_low=False)
+ simulated_left = []
+ simulated_right = []
+ for _ in xrange(100):
+ drivetrain.Update(numpy.matrix([[12.0], [12.0]]))
+ simulated_left.append(drivetrain.X[0, 0])
+ simulated_right.append(drivetrain.X[2, 0])
+
+ if FLAGS.plot:
+ pylab.plot(range(100), simulated_left)
+ pylab.plot(range(100), simulated_right)
+ pylab.suptitle('Acceleration Test')
+ pylab.show()
+
+ # Simulate forwards motion.
+ drivetrain = Drivetrain(left_low=False, right_low=False)
+ close_loop_left = []
+ close_loop_right = []
+ left_power = []
+ right_power = []
+ R = numpy.matrix([[1.0], [0.0], [1.0], [0.0]])
+ for _ in xrange(300):
+ U = numpy.clip(drivetrain.K * (R - drivetrain.X_hat),
+ drivetrain.U_min, drivetrain.U_max)
+ drivetrain.UpdateObserver(U)
+ drivetrain.Update(U)
+ close_loop_left.append(drivetrain.X[0, 0])
+ close_loop_right.append(drivetrain.X[2, 0])
+ left_power.append(U[0, 0])
+ right_power.append(U[1, 0])
+
+ if FLAGS.plot:
+ pylab.plot(range(300), close_loop_left, label='left position')
+ pylab.plot(range(300), close_loop_right, label='right position')
+ pylab.plot(range(300), left_power, label='left power')
+ pylab.plot(range(300), right_power, label='right power')
+ pylab.suptitle('Linear Move')
+ pylab.legend()
+ pylab.show()
+
+ # Try turning in place
+ drivetrain = Drivetrain()
+ close_loop_left = []
+ close_loop_right = []
+ R = numpy.matrix([[-1.0], [0.0], [1.0], [0.0]])
+ for _ in xrange(100):
+ U = numpy.clip(drivetrain.K * (R - drivetrain.X_hat),
+ drivetrain.U_min, drivetrain.U_max)
+ drivetrain.UpdateObserver(U)
+ drivetrain.Update(U)
+ close_loop_left.append(drivetrain.X[0, 0])
+ close_loop_right.append(drivetrain.X[2, 0])
+
+ if FLAGS.plot:
+ pylab.plot(range(100), close_loop_left)
+ pylab.plot(range(100), close_loop_right)
+ pylab.suptitle('Angular Move')
+ pylab.show()
+
+ # Try turning just one side.
+ drivetrain = Drivetrain()
+ close_loop_left = []
+ close_loop_right = []
+ R = numpy.matrix([[0.0], [0.0], [1.0], [0.0]])
+ for _ in xrange(100):
+ U = numpy.clip(drivetrain.K * (R - drivetrain.X_hat),
+ drivetrain.U_min, drivetrain.U_max)
+ drivetrain.UpdateObserver(U)
+ drivetrain.Update(U)
+ close_loop_left.append(drivetrain.X[0, 0])
+ close_loop_right.append(drivetrain.X[2, 0])
+
+ if FLAGS.plot:
+ pylab.plot(range(100), close_loop_left)
+ pylab.plot(range(100), close_loop_right)
+ pylab.suptitle('Pivot')
+ pylab.show()
+
+ # Write the generated constants out to a file.
+ drivetrain_low_low = Drivetrain(
+ name="DrivetrainLowLow", left_low=True, right_low=True)
+ drivetrain_low_high = Drivetrain(
+ name="DrivetrainLowHigh", left_low=True, right_low=False)
+ drivetrain_high_low = Drivetrain(
+ name="DrivetrainHighLow", left_low=False, right_low=True)
+ drivetrain_high_high = Drivetrain(
+ name="DrivetrainHighHigh", left_low=False, right_low=False)
+
+ kf_drivetrain_low_low = KFDrivetrain(
+ name="KFDrivetrainLowLow", left_low=True, right_low=True)
+ kf_drivetrain_low_high = KFDrivetrain(
+ name="KFDrivetrainLowHigh", left_low=True, right_low=False)
+ kf_drivetrain_high_low = KFDrivetrain(
+ name="KFDrivetrainHighLow", left_low=False, right_low=True)
+ kf_drivetrain_high_high = KFDrivetrain(
+ name="KFDrivetrainHighHigh", left_low=False, right_low=False)
+
+ if len(argv) != 5:
+ print "Expected .h file name and .cc file name"
+ else:
+ namespaces = ['y2017', 'control_loops', 'drivetrain']
+ dog_loop_writer = control_loop.ControlLoopWriter(
+ "Drivetrain", [drivetrain_low_low, drivetrain_low_high,
+ drivetrain_high_low, drivetrain_high_high],
+ namespaces = namespaces)
+ dog_loop_writer.AddConstant(control_loop.Constant("kDt", "%f",
+ drivetrain_low_low.dt))
+ dog_loop_writer.AddConstant(control_loop.Constant("kStallTorque", "%f",
+ drivetrain_low_low.stall_torque))
+ dog_loop_writer.AddConstant(control_loop.Constant("kStallCurrent", "%f",
+ drivetrain_low_low.stall_current))
+ dog_loop_writer.AddConstant(control_loop.Constant("kFreeSpeedRPM", "%f",
+ drivetrain_low_low.free_speed))
+ dog_loop_writer.AddConstant(control_loop.Constant("kFreeCurrent", "%f",
+ drivetrain_low_low.free_current))
+ dog_loop_writer.AddConstant(control_loop.Constant("kJ", "%f",
+ drivetrain_low_low.J))
+ dog_loop_writer.AddConstant(control_loop.Constant("kMass", "%f",
+ drivetrain_low_low.m))
+ dog_loop_writer.AddConstant(control_loop.Constant("kRobotRadius", "%f",
+ drivetrain_low_low.rb))
+ dog_loop_writer.AddConstant(control_loop.Constant("kWheelRadius", "%f",
+ drivetrain_low_low.r))
+ dog_loop_writer.AddConstant(control_loop.Constant("kR", "%f",
+ drivetrain_low_low.resistance))
+ dog_loop_writer.AddConstant(control_loop.Constant("kV", "%f",
+ drivetrain_low_low.Kv))
+ dog_loop_writer.AddConstant(control_loop.Constant("kT", "%f",
+ drivetrain_low_low.Kt))
+ dog_loop_writer.AddConstant(control_loop.Constant("kLowGearRatio", "%f",
+ drivetrain_low_low.G_low))
+ dog_loop_writer.AddConstant(control_loop.Constant("kHighGearRatio", "%f",
+ drivetrain_high_high.G_high))
+
+ dog_loop_writer.Write(argv[1], argv[2])
+
+ kf_loop_writer = control_loop.ControlLoopWriter(
+ "KFDrivetrain", [kf_drivetrain_low_low, kf_drivetrain_low_high,
+ kf_drivetrain_high_low, kf_drivetrain_high_high],
+ namespaces = namespaces)
+ kf_loop_writer.Write(argv[3], argv[4])
+
+if __name__ == '__main__':
+ sys.exit(main(sys.argv))
diff --git a/y2017/control_loops/python/polydrivetrain.py b/y2017/control_loops/python/polydrivetrain.py
new file mode 100755
index 0000000..353618b
--- /dev/null
+++ b/y2017/control_loops/python/polydrivetrain.py
@@ -0,0 +1,501 @@
+#!/usr/bin/python
+
+import numpy
+import sys
+from frc971.control_loops.python import polytope
+from y2017.control_loops.python import drivetrain
+from frc971.control_loops.python import control_loop
+from frc971.control_loops.python import controls
+from frc971.control_loops.python.cim import CIM
+from matplotlib import pylab
+
+import gflags
+import glog
+
+__author__ = 'Austin Schuh (austin.linux@gmail.com)'
+
+FLAGS = gflags.FLAGS
+
+try:
+ gflags.DEFINE_bool('plot', False, 'If true, plot the loop response.')
+except gflags.DuplicateFlagError:
+ pass
+
+def CoerceGoal(region, K, w, R):
+ """Intersects a line with a region, and finds the closest point to R.
+
+ Finds a point that is closest to R inside the region, and on the line
+ defined by K X = w. If it is not possible to find a point on the line,
+ finds a point that is inside the region and closest to the line. This
+ function assumes that
+
+ Args:
+ region: HPolytope, the valid goal region.
+ K: numpy.matrix (2 x 1), the matrix for the equation [K1, K2] [x1; x2] = w
+ w: float, the offset in the equation above.
+ R: numpy.matrix (2 x 1), the point to be closest to.
+
+ Returns:
+ numpy.matrix (2 x 1), the point.
+ """
+ return DoCoerceGoal(region, K, w, R)[0]
+
+def DoCoerceGoal(region, K, w, R):
+ if region.IsInside(R):
+ return (R, True)
+
+ perpendicular_vector = K.T / numpy.linalg.norm(K)
+ parallel_vector = numpy.matrix([[perpendicular_vector[1, 0]],
+ [-perpendicular_vector[0, 0]]])
+
+ # We want to impose the constraint K * X = w on the polytope H * X <= k.
+ # We do this by breaking X up into parallel and perpendicular components to
+ # the half plane. This gives us the following equation.
+ #
+ # parallel * (parallel.T \dot X) + perpendicular * (perpendicular \dot X)) = X
+ #
+ # Then, substitute this into the polytope.
+ #
+ # H * (parallel * (parallel.T \dot X) + perpendicular * (perpendicular \dot X)) <= k
+ #
+ # Substitute K * X = w
+ #
+ # H * parallel * (parallel.T \dot X) + H * perpendicular * w <= k
+ #
+ # Move all the knowns to the right side.
+ #
+ # H * parallel * ([parallel1 parallel2] * X) <= k - H * perpendicular * w
+ #
+ # Let t = parallel.T \dot X, the component parallel to the surface.
+ #
+ # H * parallel * t <= k - H * perpendicular * w
+ #
+ # This is a polytope which we can solve, and use to figure out the range of X
+ # that we care about!
+
+ t_poly = polytope.HPolytope(
+ region.H * parallel_vector,
+ region.k - region.H * perpendicular_vector * w)
+
+ vertices = t_poly.Vertices()
+
+ if vertices.shape[0]:
+ # The region exists!
+ # Find the closest vertex
+ min_distance = numpy.infty
+ closest_point = None
+ for vertex in vertices:
+ point = parallel_vector * vertex + perpendicular_vector * w
+ length = numpy.linalg.norm(R - point)
+ if length < min_distance:
+ min_distance = length
+ closest_point = point
+
+ return (closest_point, True)
+ else:
+ # Find the vertex of the space that is closest to the line.
+ region_vertices = region.Vertices()
+ min_distance = numpy.infty
+ closest_point = None
+ for vertex in region_vertices:
+ point = vertex.T
+ length = numpy.abs((perpendicular_vector.T * point)[0, 0])
+ if length < min_distance:
+ min_distance = length
+ closest_point = point
+
+ return (closest_point, False)
+
+
+class VelocityDrivetrainModel(control_loop.ControlLoop):
+ def __init__(self, left_low=True, right_low=True, name="VelocityDrivetrainModel"):
+ super(VelocityDrivetrainModel, self).__init__(name)
+ self._drivetrain = drivetrain.Drivetrain(left_low=left_low,
+ right_low=right_low)
+ self.dt = 0.005
+ self.A_continuous = numpy.matrix(
+ [[self._drivetrain.A_continuous[1, 1], self._drivetrain.A_continuous[1, 3]],
+ [self._drivetrain.A_continuous[3, 1], self._drivetrain.A_continuous[3, 3]]])
+
+ self.B_continuous = numpy.matrix(
+ [[self._drivetrain.B_continuous[1, 0], self._drivetrain.B_continuous[1, 1]],
+ [self._drivetrain.B_continuous[3, 0], self._drivetrain.B_continuous[3, 1]]])
+ self.C = numpy.matrix(numpy.eye(2))
+ self.D = numpy.matrix(numpy.zeros((2, 2)))
+
+ self.A, self.B = self.ContinuousToDiscrete(self.A_continuous,
+ self.B_continuous, self.dt)
+
+ # FF * X = U (steady state)
+ self.FF = self.B.I * (numpy.eye(2) - self.A)
+
+ self.PlaceControllerPoles([0.67, 0.67])
+ self.PlaceObserverPoles([0.02, 0.02])
+
+ self.G_high = self._drivetrain.G_high
+ self.G_low = self._drivetrain.G_low
+ self.resistance = self._drivetrain.resistance
+ self.r = self._drivetrain.r
+ self.Kv = self._drivetrain.Kv
+ self.Kt = self._drivetrain.Kt
+
+ self.U_max = self._drivetrain.U_max
+ self.U_min = self._drivetrain.U_min
+
+
+class VelocityDrivetrain(object):
+ HIGH = 'high'
+ LOW = 'low'
+ SHIFTING_UP = 'up'
+ SHIFTING_DOWN = 'down'
+
+ def __init__(self):
+ self.drivetrain_low_low = VelocityDrivetrainModel(
+ left_low=True, right_low=True, name='VelocityDrivetrainLowLow')
+ self.drivetrain_low_high = VelocityDrivetrainModel(left_low=True, right_low=False, name='VelocityDrivetrainLowHigh')
+ self.drivetrain_high_low = VelocityDrivetrainModel(left_low=False, right_low=True, name = 'VelocityDrivetrainHighLow')
+ self.drivetrain_high_high = VelocityDrivetrainModel(left_low=False, right_low=False, name = 'VelocityDrivetrainHighHigh')
+
+ # X is [lvel, rvel]
+ self.X = numpy.matrix(
+ [[0.0],
+ [0.0]])
+
+ self.U_poly = polytope.HPolytope(
+ numpy.matrix([[1, 0],
+ [-1, 0],
+ [0, 1],
+ [0, -1]]),
+ numpy.matrix([[12],
+ [12],
+ [12],
+ [12]]))
+
+ self.U_max = numpy.matrix(
+ [[12.0],
+ [12.0]])
+ self.U_min = numpy.matrix(
+ [[-12.0000000000],
+ [-12.0000000000]])
+
+ self.dt = 0.005
+
+ self.R = numpy.matrix(
+ [[0.0],
+ [0.0]])
+
+ self.U_ideal = numpy.matrix(
+ [[0.0],
+ [0.0]])
+
+ # ttrust is the comprimise between having full throttle negative inertia,
+ # and having no throttle negative inertia. A value of 0 is full throttle
+ # inertia. A value of 1 is no throttle negative inertia.
+ self.ttrust = 1.0
+
+ self.left_gear = VelocityDrivetrain.LOW
+ self.right_gear = VelocityDrivetrain.LOW
+ self.left_shifter_position = 0.0
+ self.right_shifter_position = 0.0
+ self.left_cim = CIM()
+ self.right_cim = CIM()
+
+ def IsInGear(self, gear):
+ return gear is VelocityDrivetrain.HIGH or gear is VelocityDrivetrain.LOW
+
+ def MotorRPM(self, shifter_position, velocity):
+ if shifter_position > 0.5:
+ return (velocity / self.CurrentDrivetrain().G_high /
+ self.CurrentDrivetrain().r)
+ else:
+ return (velocity / self.CurrentDrivetrain().G_low /
+ self.CurrentDrivetrain().r)
+
+ def CurrentDrivetrain(self):
+ if self.left_shifter_position > 0.5:
+ if self.right_shifter_position > 0.5:
+ return self.drivetrain_high_high
+ else:
+ return self.drivetrain_high_low
+ else:
+ if self.right_shifter_position > 0.5:
+ return self.drivetrain_low_high
+ else:
+ return self.drivetrain_low_low
+
+ def SimShifter(self, gear, shifter_position):
+ if gear is VelocityDrivetrain.HIGH or gear is VelocityDrivetrain.SHIFTING_UP:
+ shifter_position = min(shifter_position + 0.5, 1.0)
+ else:
+ shifter_position = max(shifter_position - 0.5, 0.0)
+
+ if shifter_position == 1.0:
+ gear = VelocityDrivetrain.HIGH
+ elif shifter_position == 0.0:
+ gear = VelocityDrivetrain.LOW
+
+ return gear, shifter_position
+
+ def ComputeGear(self, wheel_velocity, should_print=False, current_gear=False, gear_name=None):
+ high_omega = (wheel_velocity / self.CurrentDrivetrain().G_high /
+ self.CurrentDrivetrain().r)
+ low_omega = (wheel_velocity / self.CurrentDrivetrain().G_low /
+ self.CurrentDrivetrain().r)
+ #print gear_name, "Motor Energy Difference.", 0.5 * 0.000140032647 * (low_omega * low_omega - high_omega * high_omega), "joules"
+ high_torque = ((12.0 - high_omega / self.CurrentDrivetrain().Kv) *
+ self.CurrentDrivetrain().Kt / self.CurrentDrivetrain().resistance)
+ low_torque = ((12.0 - low_omega / self.CurrentDrivetrain().Kv) *
+ self.CurrentDrivetrain().Kt / self.CurrentDrivetrain().resistance)
+ high_power = high_torque * high_omega
+ low_power = low_torque * low_omega
+ #if should_print:
+ # print gear_name, "High omega", high_omega, "Low omega", low_omega
+ # print gear_name, "High torque", high_torque, "Low torque", low_torque
+ # print gear_name, "High power", high_power, "Low power", low_power
+
+ # Shift algorithm improvements.
+ # TODO(aschuh):
+ # It takes time to shift. Shifting down for 1 cycle doesn't make sense
+ # because you will end up slower than without shifting. Figure out how
+ # to include that info.
+ # If the driver is still in high gear, but isn't asking for the extra power
+ # from low gear, don't shift until he asks for it.
+ goal_gear_is_high = high_power > low_power
+ #goal_gear_is_high = True
+
+ if not self.IsInGear(current_gear):
+ glog.debug('%s Not in gear.', gear_name)
+ return current_gear
+ else:
+ is_high = current_gear is VelocityDrivetrain.HIGH
+ if is_high != goal_gear_is_high:
+ if goal_gear_is_high:
+ glog.debug('%s Shifting up.', gear_name)
+ return VelocityDrivetrain.SHIFTING_UP
+ else:
+ glog.debug('%s Shifting down.', gear_name)
+ return VelocityDrivetrain.SHIFTING_DOWN
+ else:
+ return current_gear
+
+ def FilterVelocity(self, throttle):
+ # Invert the plant to figure out how the velocity filter would have to work
+ # out in order to filter out the forwards negative inertia.
+ # This math assumes that the left and right power and velocity are equal.
+
+ # The throttle filter should filter such that the motor in the highest gear
+ # should be controlling the time constant.
+ # Do this by finding the index of FF that has the lowest value, and computing
+ # the sums using that index.
+ FF_sum = self.CurrentDrivetrain().FF.sum(axis=1)
+ min_FF_sum_index = numpy.argmin(FF_sum)
+ min_FF_sum = FF_sum[min_FF_sum_index, 0]
+ min_K_sum = self.CurrentDrivetrain().K[min_FF_sum_index, :].sum()
+ # Compute the FF sum for high gear.
+ high_min_FF_sum = self.drivetrain_high_high.FF[0, :].sum()
+
+ # U = self.K[0, :].sum() * (R - x_avg) + self.FF[0, :].sum() * R
+ # throttle * 12.0 = (self.K[0, :].sum() + self.FF[0, :].sum()) * R
+ # - self.K[0, :].sum() * x_avg
+
+ # R = (throttle * 12.0 + self.K[0, :].sum() * x_avg) /
+ # (self.K[0, :].sum() + self.FF[0, :].sum())
+
+ # U = (K + FF) * R - K * X
+ # (K + FF) ^-1 * (U + K * X) = R
+
+ # Scale throttle by min_FF_sum / high_min_FF_sum. This will make low gear
+ # have the same velocity goal as high gear, and so that the robot will hold
+ # the same speed for the same throttle for all gears.
+ adjusted_ff_voltage = numpy.clip(throttle * 12.0 * min_FF_sum / high_min_FF_sum, -12.0, 12.0)
+ return ((adjusted_ff_voltage + self.ttrust * min_K_sum * (self.X[0, 0] + self.X[1, 0]) / 2.0)
+ / (self.ttrust * min_K_sum + min_FF_sum))
+
+ def Update(self, throttle, steering):
+ # Shift into the gear which sends the most power to the floor.
+ # This is the same as sending the most torque down to the floor at the
+ # wheel.
+
+ self.left_gear = self.right_gear = True
+ if True:
+ self.left_gear = self.ComputeGear(self.X[0, 0], should_print=True,
+ current_gear=self.left_gear,
+ gear_name="left")
+ self.right_gear = self.ComputeGear(self.X[1, 0], should_print=True,
+ current_gear=self.right_gear,
+ gear_name="right")
+ if self.IsInGear(self.left_gear):
+ self.left_cim.X[0, 0] = self.MotorRPM(self.left_shifter_position, self.X[0, 0])
+
+ if self.IsInGear(self.right_gear):
+ self.right_cim.X[0, 0] = self.MotorRPM(self.right_shifter_position, self.X[0, 0])
+
+ if self.IsInGear(self.left_gear) and self.IsInGear(self.right_gear):
+ # Filter the throttle to provide a nicer response.
+ fvel = self.FilterVelocity(throttle)
+
+ # Constant radius means that angualar_velocity / linear_velocity = constant.
+ # Compute the left and right velocities.
+ steering_velocity = numpy.abs(fvel) * steering
+ left_velocity = fvel - steering_velocity
+ right_velocity = fvel + steering_velocity
+
+ # Write this constraint in the form of K * R = w
+ # angular velocity / linear velocity = constant
+ # (left - right) / (left + right) = constant
+ # left - right = constant * left + constant * right
+
+ # (fvel - steering * numpy.abs(fvel) - fvel - steering * numpy.abs(fvel)) /
+ # (fvel - steering * numpy.abs(fvel) + fvel + steering * numpy.abs(fvel)) =
+ # constant
+ # (- 2 * steering * numpy.abs(fvel)) / (2 * fvel) = constant
+ # (-steering * sign(fvel)) = constant
+ # (-steering * sign(fvel)) * (left + right) = left - right
+ # (steering * sign(fvel) + 1) * left + (steering * sign(fvel) - 1) * right = 0
+
+ equality_k = numpy.matrix(
+ [[1 + steering * numpy.sign(fvel), -(1 - steering * numpy.sign(fvel))]])
+ equality_w = 0.0
+
+ self.R[0, 0] = left_velocity
+ self.R[1, 0] = right_velocity
+
+ # Construct a constraint on R by manipulating the constraint on U
+ # Start out with H * U <= k
+ # U = FF * R + K * (R - X)
+ # H * (FF * R + K * R - K * X) <= k
+ # H * (FF + K) * R <= k + H * K * X
+ R_poly = polytope.HPolytope(
+ self.U_poly.H * (self.CurrentDrivetrain().K + self.CurrentDrivetrain().FF),
+ self.U_poly.k + self.U_poly.H * self.CurrentDrivetrain().K * self.X)
+
+ # Limit R back inside the box.
+ self.boxed_R = CoerceGoal(R_poly, equality_k, equality_w, self.R)
+
+ FF_volts = self.CurrentDrivetrain().FF * self.boxed_R
+ self.U_ideal = self.CurrentDrivetrain().K * (self.boxed_R - self.X) + FF_volts
+ else:
+ glog.debug('Not all in gear')
+ if not self.IsInGear(self.left_gear) and not self.IsInGear(self.right_gear):
+ # TODO(austin): Use battery volts here.
+ R_left = self.MotorRPM(self.left_shifter_position, self.X[0, 0])
+ self.U_ideal[0, 0] = numpy.clip(
+ self.left_cim.K * (R_left - self.left_cim.X) + R_left / self.left_cim.Kv,
+ self.left_cim.U_min, self.left_cim.U_max)
+ self.left_cim.Update(self.U_ideal[0, 0])
+
+ R_right = self.MotorRPM(self.right_shifter_position, self.X[1, 0])
+ self.U_ideal[1, 0] = numpy.clip(
+ self.right_cim.K * (R_right - self.right_cim.X) + R_right / self.right_cim.Kv,
+ self.right_cim.U_min, self.right_cim.U_max)
+ self.right_cim.Update(self.U_ideal[1, 0])
+ else:
+ assert False
+
+ self.U = numpy.clip(self.U_ideal, self.U_min, self.U_max)
+
+ # TODO(austin): Model the robot as not accelerating when you shift...
+ # This hack only works when you shift at the same time.
+ if self.IsInGear(self.left_gear) and self.IsInGear(self.right_gear):
+ self.X = self.CurrentDrivetrain().A * self.X + self.CurrentDrivetrain().B * self.U
+
+ self.left_gear, self.left_shifter_position = self.SimShifter(
+ self.left_gear, self.left_shifter_position)
+ self.right_gear, self.right_shifter_position = self.SimShifter(
+ self.right_gear, self.right_shifter_position)
+
+ glog.debug('U is %s %s', str(self.U[0, 0]), str(self.U[1, 0]))
+ glog.debug('Left shifter %s %d Right shifter %s %d',
+ self.left_gear, self.left_shifter_position,
+ self.right_gear, self.right_shifter_position)
+
+
+def main(argv):
+ argv = FLAGS(argv)
+
+ vdrivetrain = VelocityDrivetrain()
+
+ if not FLAGS.plot:
+ if len(argv) != 5:
+ glog.fatal('Expected .h file name and .cc file name')
+ else:
+ namespaces = ['y2017', 'control_loops', 'drivetrain']
+ dog_loop_writer = control_loop.ControlLoopWriter(
+ "VelocityDrivetrain", [vdrivetrain.drivetrain_low_low,
+ vdrivetrain.drivetrain_low_high,
+ vdrivetrain.drivetrain_high_low,
+ vdrivetrain.drivetrain_high_high],
+ namespaces=namespaces)
+
+ dog_loop_writer.Write(argv[1], argv[2])
+
+ cim_writer = control_loop.ControlLoopWriter("CIM", [CIM()])
+
+ cim_writer.Write(argv[3], argv[4])
+ return
+
+ vl_plot = []
+ vr_plot = []
+ ul_plot = []
+ ur_plot = []
+ radius_plot = []
+ t_plot = []
+ left_gear_plot = []
+ right_gear_plot = []
+ vdrivetrain.left_shifter_position = 0.0
+ vdrivetrain.right_shifter_position = 0.0
+ vdrivetrain.left_gear = VelocityDrivetrain.LOW
+ vdrivetrain.right_gear = VelocityDrivetrain.LOW
+
+ glog.debug('K is %s', str(vdrivetrain.CurrentDrivetrain().K))
+
+ if vdrivetrain.left_gear is VelocityDrivetrain.HIGH:
+ glog.debug('Left is high')
+ else:
+ glog.debug('Left is low')
+ if vdrivetrain.right_gear is VelocityDrivetrain.HIGH:
+ glog.debug('Right is high')
+ else:
+ glog.debug('Right is low')
+
+ for t in numpy.arange(0, 1.7, vdrivetrain.dt):
+ if t < 0.5:
+ vdrivetrain.Update(throttle=0.00, steering=1.0)
+ elif t < 1.2:
+ vdrivetrain.Update(throttle=0.5, steering=1.0)
+ else:
+ vdrivetrain.Update(throttle=0.00, steering=1.0)
+ t_plot.append(t)
+ vl_plot.append(vdrivetrain.X[0, 0])
+ vr_plot.append(vdrivetrain.X[1, 0])
+ ul_plot.append(vdrivetrain.U[0, 0])
+ ur_plot.append(vdrivetrain.U[1, 0])
+ left_gear_plot.append((vdrivetrain.left_gear is VelocityDrivetrain.HIGH) * 2.0 - 10.0)
+ right_gear_plot.append((vdrivetrain.right_gear is VelocityDrivetrain.HIGH) * 2.0 - 10.0)
+
+ fwd_velocity = (vdrivetrain.X[1, 0] + vdrivetrain.X[0, 0]) / 2
+ turn_velocity = (vdrivetrain.X[1, 0] - vdrivetrain.X[0, 0])
+ if abs(fwd_velocity) < 0.0000001:
+ radius_plot.append(turn_velocity)
+ else:
+ radius_plot.append(turn_velocity / fwd_velocity)
+
+ # TODO(austin):
+ # Shifting compensation.
+
+ # Tighten the turn.
+ # Closed loop drive.
+
+ pylab.plot(t_plot, vl_plot, label='left velocity')
+ pylab.plot(t_plot, vr_plot, label='right velocity')
+ pylab.plot(t_plot, ul_plot, label='left voltage')
+ pylab.plot(t_plot, ur_plot, label='right voltage')
+ pylab.plot(t_plot, radius_plot, label='radius')
+ pylab.plot(t_plot, left_gear_plot, label='left gear high')
+ pylab.plot(t_plot, right_gear_plot, label='right gear high')
+ pylab.legend()
+ pylab.show()
+ return 0
+
+if __name__ == '__main__':
+ sys.exit(main(sys.argv))
diff --git a/y2017/control_loops/python/polydrivetrain_test.py b/y2017/control_loops/python/polydrivetrain_test.py
new file mode 100755
index 0000000..434cdca
--- /dev/null
+++ b/y2017/control_loops/python/polydrivetrain_test.py
@@ -0,0 +1,82 @@
+#!/usr/bin/python
+
+import polydrivetrain
+import numpy
+from numpy.testing import *
+import polytope
+import unittest
+
+__author__ = 'Austin Schuh (austin.linux@gmail.com)'
+
+
+class TestVelocityDrivetrain(unittest.TestCase):
+ def MakeBox(self, x1_min, x1_max, x2_min, x2_max):
+ H = numpy.matrix([[1, 0],
+ [-1, 0],
+ [0, 1],
+ [0, -1]])
+ K = numpy.matrix([[x1_max],
+ [-x1_min],
+ [x2_max],
+ [-x2_min]])
+ return polytope.HPolytope(H, K)
+
+ def test_coerce_inside(self):
+ """Tests coercion when the point is inside the box."""
+ box = self.MakeBox(1, 2, 1, 2)
+
+ # x1 = x2
+ K = numpy.matrix([[1, -1]])
+ w = 0
+
+ assert_array_equal(polydrivetrain.CoerceGoal(box, K, w,
+ numpy.matrix([[1.5], [1.5]])),
+ numpy.matrix([[1.5], [1.5]]))
+
+ def test_coerce_outside_intersect(self):
+ """Tests coercion when the line intersects the box."""
+ box = self.MakeBox(1, 2, 1, 2)
+
+ # x1 = x2
+ K = numpy.matrix([[1, -1]])
+ w = 0
+
+ assert_array_equal(polydrivetrain.CoerceGoal(box, K, w, numpy.matrix([[5], [5]])),
+ numpy.matrix([[2.0], [2.0]]))
+
+ def test_coerce_outside_no_intersect(self):
+ """Tests coercion when the line does not intersect the box."""
+ box = self.MakeBox(3, 4, 1, 2)
+
+ # x1 = x2
+ K = numpy.matrix([[1, -1]])
+ w = 0
+
+ assert_array_equal(polydrivetrain.CoerceGoal(box, K, w, numpy.matrix([[5], [5]])),
+ numpy.matrix([[3.0], [2.0]]))
+
+ def test_coerce_middle_of_edge(self):
+ """Tests coercion when the line intersects the middle of an edge."""
+ box = self.MakeBox(0, 4, 1, 2)
+
+ # x1 = x2
+ K = numpy.matrix([[-1, 1]])
+ w = 0
+
+ assert_array_equal(polydrivetrain.CoerceGoal(box, K, w, numpy.matrix([[5], [5]])),
+ numpy.matrix([[2.0], [2.0]]))
+
+ def test_coerce_perpendicular_line(self):
+ """Tests coercion when the line does not intersect and is in quadrant 2."""
+ box = self.MakeBox(1, 2, 1, 2)
+
+ # x1 = -x2
+ K = numpy.matrix([[1, 1]])
+ w = 0
+
+ assert_array_equal(polydrivetrain.CoerceGoal(box, K, w, numpy.matrix([[5], [5]])),
+ numpy.matrix([[1.0], [1.0]]))
+
+
+if __name__ == '__main__':
+ unittest.main()