From b539476ec800eb1d7804fd8e9b73fa9bbb1f63bd Mon Sep 17 00:00:00 2001 From: Florian Jung Date: Mon, 3 Dec 2012 23:47:10 +0100 Subject: makefile geupdated. könnte mit gnu-make kaputtgehen :/ MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- Makefile | 29 +- mariokart.cpp | 885 ++++++++++++++++++++++++++++++++++++++++++++++++++++++++ mariokart01.cpp | 885 -------------------------------------------------------- 3 files changed, 896 insertions(+), 903 deletions(-) create mode 100644 mariokart.cpp delete mode 100644 mariokart01.cpp diff --git a/Makefile b/Makefile index 98b4d9c..e7a632c 100644 --- a/Makefile +++ b/Makefile @@ -1,29 +1,22 @@ -all: detect_road_borders mariokart01 +.SUFFIXES: .cpp .o +.PHONY: clean -detect_road_borders: detect_road_borders.cpp - g++ `pkg-config --libs --cflags opencv` -g $< -o $@ - -road_thresholder.o: road_thresholder.cpp road_thresholder.h - g++ `pkg-config --cflags opencv` -g road_thresholder.cpp -c +all: detect_road_borders mariokart -horizon_steerer.o: horizon_steerer.cpp horizon_steerer.h - g++ `pkg-config --cflags opencv` -g horizon_steerer.cpp -c +clean: + rm -f *.o mariokart detect_road_borders -naive_steerer.o: naive_steerer.cpp naive_steerer.h - g++ `pkg-config --cflags opencv` -g naive_steerer.cpp -c +.cpp.o: + g++ `pkg-config --cflags opencv` -g -c $< -steer_accumulator.o: steer_accumulator.cpp steer_accumulator.h - g++ `pkg-config --cflags opencv` -g steer_accumulator.cpp -c -util.o: util.cpp util.h - g++ `pkg-config --cflags opencv` -g util.cpp -c +detect_road_borders: detect_road_borders.cpp + g++ `pkg-config --libs --cflags opencv` -g $^ -o $@ test_detect: detect_road_borders ./detect_road_borders test.mpg -mariokart01.o: mariokart01.cpp - g++ `pkg-config --cflags opencv` -lxcb -lpthread -g mariokart01.cpp -c -mariokart01: mariokart01.o road_thresholder.o horizon_steerer.o naive_steerer.o steer_accumulator.o util.o - g++ `pkg-config --libs --cflags opencv` -lxcb -lpthread -g mariokart01.o road_thresholder.o horizon_steerer.o naive_steerer.o steer_accumulator.o util.o -o $@ +mariokart: mariokart.o road_thresholder.o horizon_steerer.o naive_steerer.o steer_accumulator.o util.o + g++ `pkg-config --libs --cflags opencv` -lxcb -lpthread -g $> -o $@ diff --git a/mariokart.cpp b/mariokart.cpp new file mode 100644 index 0000000..dc6eaa9 --- /dev/null +++ b/mariokart.cpp @@ -0,0 +1,885 @@ +/* + * unbenannt.cxx + * + * Copyright 2012 Unknown + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, + * MA 02110-1301, USA. + */ + + +#define FREEBSD +//#define LINUX + +#include +#include +#include +#include +#include +#include +#include +#include + +#ifdef LINUX +#include +#include +#endif + +#include +#include +#include +#include + +#include "util.h" +#include "steer_interface.h" +#include "naive_steerer.h" +#include "road_thresholder_iface.h" +#include "road_thresholder.h" + +#include +#include +#include +#include + +#include + +using namespace std; +using namespace cv; + +class XorgGrabber +{ + public: + XorgGrabber(const char* win_title); + ~XorgGrabber(); + void read(Mat& mat); + + private: + xcb_connection_t* conn; + xcb_window_t grabbed_win; + int grab_width, grab_height; + xcb_screen_t* grab_screen; + xcb_get_image_reply_t* img; + +}; + +XorgGrabber::XorgGrabber(const char* win_title) +{ + conn=xcb_connect(NULL,NULL); + + bool found_win=false; + grab_screen=NULL; + img=NULL; + + /* Find configured screen */ + const xcb_setup_t* setup = xcb_get_setup(conn); + for (xcb_screen_iterator_t i = xcb_setup_roots_iterator(setup); + i.rem > 0; xcb_screen_next (&i)) + { + xcb_screen_t* scr = i.data; + xcb_query_tree_reply_t* reply = xcb_query_tree_reply( conn, xcb_query_tree(conn, scr->root), NULL); + if (reply) + { + int len = xcb_query_tree_children_length(reply); + xcb_window_t* children = xcb_query_tree_children(reply); + xcb_get_window_attributes_cookie_t* cookies = new xcb_get_window_attributes_cookie_t[len]; + for (int i=0; ioverride_redirect && attr->map_state == XCB_MAP_STATE_VIEWABLE) + { + char* title=(char*)(title_reply+1); + cout << title << endl; + if (strstr(title, win_title)) + { + xcb_get_geometry_reply_t* geo; + geo = xcb_get_geometry_reply (conn, xcb_get_geometry (conn, children[i]), NULL); + if (geo) + { + grab_width=geo->width; + grab_height=geo->height; + + free(geo); + + grabbed_win=children[i]; + found_win=true; + + grab_screen=scr; + } + else + { + cerr << "geo==NULL!" << endl; + } + } + } + + free(title_reply); + free(attr); + } + + free(reply); + delete[] cookies; + } + else + { + cout << "xcb_get_setup failed" << endl; + } + } + + if (found_win) + { + xcb_get_image_reply_t* img = xcb_get_image_reply (conn, + xcb_get_image (conn, XCB_IMAGE_FORMAT_Z_PIXMAP, grabbed_win, + 0, 0, grab_width, grab_height, ~0), NULL); + + + + xcb_depth_iterator_t depth_iterator = xcb_screen_allowed_depths_iterator(grab_screen); + + int ndepths=xcb_screen_allowed_depths_length(grab_screen); + + for (int i=0; idepth == img->depth) + { + xcb_visualtype_t* visuals = xcb_depth_visuals(depth_iterator.data); + int nvisuals = xcb_depth_visuals_length(depth_iterator.data); + + for (int j=0;jvisual) + { + assert(visuals[j]._class==XCB_VISUAL_CLASS_TRUE_COLOR || visuals[j]._class==XCB_VISUAL_CLASS_DIRECT_COLOR); + cout << (int)visuals[j]._class << "," << XCB_VISUAL_CLASS_TRUE_COLOR << endl; + cout << visuals[j].red_mask << endl; + cout << visuals[j].green_mask << endl; + cout << visuals[j].blue_mask << endl; + break; + } + } + + break; + } + + xcb_depth_next(&depth_iterator); + } + + + + int nformats = xcb_setup_pixmap_formats_length(setup); + xcb_format_t* formats = xcb_setup_pixmap_formats(setup); + for (int i=0;idepth) + { + cout << (int)formats[i].bits_per_pixel << endl; + cout << (int)formats[i].scanline_pad << endl; + break; + } + } + + cout << grab_width << "x" << grab_height << endl; + + free(img); + + } + else + { + throw string("FATAL: did not find window, exiting."); + } + + +} + +XorgGrabber::~XorgGrabber() +{ + if (img) free(img); + xcb_disconnect(conn); +} + +void XorgGrabber::read(Mat& mat) +{ + if (img) free(img); + + + // mat gets invalid when the next read() is called! + img = xcb_get_image_reply (conn, + xcb_get_image (conn, XCB_IMAGE_FORMAT_Z_PIXMAP, grabbed_win, + 0, 0, grab_width, grab_height, ~0), NULL); + + mat = Mat(grab_height, grab_width, CV_8UC4, xcb_get_image_data(img)); + mat.addref(); +} + + + + +#define THROTTLE_CNT_MAX 10 + + +#ifdef FREEBSD +typedef union { + unsigned int Value; + struct { + unsigned R_DPAD : 1; + unsigned L_DPAD : 1; + unsigned D_DPAD : 1; + unsigned U_DPAD : 1; + unsigned START_BUTTON : 1; + unsigned Z_TRIG : 1; + unsigned B_BUTTON : 1; + unsigned A_BUTTON : 1; + + unsigned R_CBUTTON : 1; + unsigned L_CBUTTON : 1; + unsigned D_CBUTTON : 1; + unsigned U_CBUTTON : 1; + unsigned R_TRIG : 1; + unsigned L_TRIG : 1; + unsigned Reserved1 : 1; + unsigned Reserved2 : 1; + + signed X_AXIS : 8; + signed Y_AXIS : 8; + }; +} BUTTONS; + + +char* pack(const BUTTONS* buttons, char* buf) +{ + buf[0]= (buttons->A_BUTTON ? 1 : 0) + + (buttons->B_BUTTON ? 2 : 0) + + (buttons->L_TRIG ? 4 : 0) + + (buttons->R_TRIG ? 8 : 0) + + (buttons->Z_TRIG ? 16 : 0) + + (buttons->START_BUTTON ? 32 : 0) + + 128; + + buf[1]= (buttons->R_DPAD ? 1 : 0) + + (buttons->L_DPAD ? 2 : 0) + + (buttons->U_DPAD ? 4 : 0) + + (buttons->D_DPAD ? 8 : 0) + + ((buttons->X_AXIS & 128) ? 16 : 0) + + ((buttons->Y_AXIS & 128) ? 32 : 0); + + buf[2]= (buttons->R_CBUTTON ? 1 : 0) + + (buttons->L_CBUTTON ? 2 : 0) + + (buttons->U_CBUTTON ? 4 : 0) + + (buttons->D_CBUTTON ? 8 : 0); + + buf[3]= (buttons->X_AXIS & 127); + + buf[4]= (buttons->Y_AXIS & 127); + + return buf; +} +#endif + +class Joystick +{ + public: + Joystick(); + ~Joystick(); + void steer(float dir, float dead_zone=0.0); + void throttle(float t); + void press_a(bool); + + + void process(); + void reset(); + + private: +#ifdef FREEBSD + BUTTONS buttons; + void send_data(); + int fifo_fd; +#endif +#ifdef LINUX + int fd; +#endif + + float throt; + int throttle_cnt; + +}; + +#ifdef FREEBSD +Joystick::Joystick() +{ + if ((fifo_fd=open("/var/tmp/mupen64plus_ctl", O_WRONLY )) == -1) {throw string(strerror(errno));} + cout << "opened" << endl; + if (fcntl(fifo_fd, F_SETFL, O_NONBLOCK) == -1) throw string("failed to set nonblocking io"); + + reset(); +} + +void Joystick::press_a(bool state) +{ + buttons.A_BUTTON=state; + send_data(); +} + +void Joystick::send_data() +{ + char buf[5]; + pack(&buttons, buf); + write(fifo_fd, buf, 5); +} + + + +void Joystick::steer(float dir, float dead_zone) +{ + if (dir<-1.0) dir=-1.0; + if (dir>1.0) dir=1.0; + + if (fabs(dir)1.0) dir=1.0; + + if (fabs(dir)1.0) t=1.0; + + throt=t; +} + +void Joystick::process() +{ + throttle_cnt++; + if (throttle_cnt>=THROTTLE_CNT_MAX) throttle_cnt=0; + + press_a((throttle_cnt < throt*THROTTLE_CNT_MAX)); +} + + + +#define HIST_SMOOTH 7 + +//#define NO_BRIGHTNESS // will man nicht, nur zu demonstrationszwecken + +#define ERODE_RADIUS_2D 4 + + +#define IMG_HISTORY 3 + + + + + +int crosshair_x=0, crosshair_y=0; + +void mouse_callback(int event, int x, int y, int flags, void* userdata) +{ + if (event==EVENT_LBUTTONDOWN) + { + crosshair_x=x; + crosshair_y=y; + } +} + + + + + +sem_t thread1_go; +sem_t thread1_done; +Mat thread1_img; + +void* thread1_func(void*) +{ + Mat gray; + static Mat gray_prev; + + static std::vector points[2]; + + std::vector status; // status of tracked features + std::vector err; // error in tracking + + + cout << "thread 1 is alive :)" < features; // detected features + + // detect the features + goodFeaturesToTrack(gray, // the image + features, // the output detected features + 3000, // the maximum number of features + 0.2, // quality level + 10); // min distance between two features + + + // add the detected features to + // the currently tracked features + points[0].insert(points[0].end(), features.begin(),features.end()); + } + + + // for first image of the sequence + if(gray_prev.empty()) + gray.copyTo(gray_prev); + + cv::calcOpticalFlowPyrLK( + gray_prev, gray, // 2 consecutive images + points[0], // input point positions in first image + points[1], // output point positions in the 2nd image + status, // tracking success + err); // tracking error + + + + int k=0; + for(int i=0; i < points[1].size(); i++) { + // do we keep this point? + if (status[i]) { + // keep this point in vector + points[0][k] = points[0][i]; + points[1][k++] = points[1][i]; + } + } + // eliminate unsuccesful points + points[0].resize(k); + points[1].resize(k); + + + + // for all tracked points + for(int i= 0; i < points[1].size(); i++ ) { + // draw line and circle + cv::line(thread1_img, + points[0][i], // initial position + points[1][i],// new position + cv::Scalar(255,255,255)); + + cv::circle(thread1_img, points[1][i], 2, + cv::Scalar(255,255,255),-1); + } + + // 4. extrapolate movement + for (int i=0;i(row); + + for (int col=0; col0) + { + data[0]=(int)data[0] * 256 / sum; + data[1]=(int)data[1] * 256 / sum; + data[2]=(int)data[2] * 256 / sum; + } + else + { + data[0]=255/3; + data[1]=255/3; + data[2]=255/3; + } + data+=img.step[1]; + } + } + #endif + + + road_thresholder->process_image(img); + Mat img_thres = road_thresholder->get_road(); + + Mat img_eroded(img_thres.rows, img_thres.cols, img_thres.type()); + Mat img_tmp(img_thres.rows, img_thres.cols, img_thres.type()); + Mat img_thres2(img_thres.rows, img_thres.cols, img_thres.type()); + + erode(img_thres, img_tmp, erode_2d_small); + dilate(img_tmp, img_thres, erode_2d_small); + dilate(img_thres, img_tmp, erode_2d_big); + erode(img_tmp, img_thres2, erode_2d_big); + + + + + + + + + + + assert(img.rows==img_eroded.rows); + assert(img.cols==img_eroded.cols); + + + + + +/* + Mat img_perspective(trans_height, trans_width, img_thres.type()); + warpPerspective(img_thres, img_perspective, transform, img_perspective.size()); + + + threshold(img_perspective, img_perspective, 127, 255, THRESH_BINARY); + Mat img_perspective_temp(img_perspective.rows, img_perspective.cols, img_perspective.type()); + Mat img_perspective_temp2(img_perspective.rows, img_perspective.cols, img_perspective.type()); + erode(img_perspective, img_perspective_temp, circle_mat(7)); + dilate(img_perspective_temp, img_perspective_temp2, circle_mat(7 + 15)); + erode(img_perspective_temp2, img_perspective, circle_mat(15)); + */ + + + img.row(crosshair_y)=Scalar(255,0,0); + img.col(crosshair_x)=Scalar(255,0,0); + img_thres2.row(crosshair_y)=128; + img_thres2.col(crosshair_x)=128; + + steerer->process_image(img_thres2); + double steer_value = steerer->get_steer_data(); + + Mat steer=Mat::zeros(20,1920,CV_8U); + steer.col( steer.cols /2 )=128; + if (steerer->get_confidence()>0) + { + int x = (steer_value/2.0 + 0.5)*steer.cols; + + if (x<1) x=1; + if (x>=steer.cols-1) x=steer.cols-2; + + steer.col(x) = 255; + steer.col(x-1)=240; + steer.col(x+1)=240; + + + joystick.steer( steer_value,0.05); + } + else + joystick.steer(0.0); + + + + + sem_wait(&thread1_done); // wait for thread1 to finish + + + //imshow("orig", img); + imshow("edit", img); + //imshow("perspective", img_perspective); + //imshow("diff", img_diff); + + imshow("thres", img_thres); + imshow("thres2", img_thres2); + imshow("tracked", thread1_img); + imshow("steer", steer); + + + + //img_writer << img_diff; + thres_writer << img_thres; + thres2_writer << img_thres2; + + waitKey(1000/50); + joystick.process(); +} + +} //try +catch(string meh) +{ + cout << "error: "< - * - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 2 of the License, or - * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with this program; if not, write to the Free Software - * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, - * MA 02110-1301, USA. - */ - - -#define FREEBSD -//#define LINUX - -#include -#include -#include -#include -#include -#include -#include -#include - -#ifdef LINUX -#include -#include -#endif - -#include -#include -#include -#include - -#include "util.h" -#include "steer_interface.h" -#include "naive_steerer.h" -#include "road_thresholder_iface.h" -#include "road_thresholder.h" - -#include -#include -#include -#include - -#include - -using namespace std; -using namespace cv; - -class XorgGrabber -{ - public: - XorgGrabber(const char* win_title); - ~XorgGrabber(); - void read(Mat& mat); - - private: - xcb_connection_t* conn; - xcb_window_t grabbed_win; - int grab_width, grab_height; - xcb_screen_t* grab_screen; - xcb_get_image_reply_t* img; - -}; - -XorgGrabber::XorgGrabber(const char* win_title) -{ - conn=xcb_connect(NULL,NULL); - - bool found_win=false; - grab_screen=NULL; - img=NULL; - - /* Find configured screen */ - const xcb_setup_t* setup = xcb_get_setup(conn); - for (xcb_screen_iterator_t i = xcb_setup_roots_iterator(setup); - i.rem > 0; xcb_screen_next (&i)) - { - xcb_screen_t* scr = i.data; - xcb_query_tree_reply_t* reply = xcb_query_tree_reply( conn, xcb_query_tree(conn, scr->root), NULL); - if (reply) - { - int len = xcb_query_tree_children_length(reply); - xcb_window_t* children = xcb_query_tree_children(reply); - xcb_get_window_attributes_cookie_t* cookies = new xcb_get_window_attributes_cookie_t[len]; - for (int i=0; ioverride_redirect && attr->map_state == XCB_MAP_STATE_VIEWABLE) - { - char* title=(char*)(title_reply+1); - cout << title << endl; - if (strstr(title, win_title)) - { - xcb_get_geometry_reply_t* geo; - geo = xcb_get_geometry_reply (conn, xcb_get_geometry (conn, children[i]), NULL); - if (geo) - { - grab_width=geo->width; - grab_height=geo->height; - - free(geo); - - grabbed_win=children[i]; - found_win=true; - - grab_screen=scr; - } - else - { - cerr << "geo==NULL!" << endl; - } - } - } - - free(title_reply); - free(attr); - } - - free(reply); - delete[] cookies; - } - else - { - cout << "xcb_get_setup failed" << endl; - } - } - - if (found_win) - { - xcb_get_image_reply_t* img = xcb_get_image_reply (conn, - xcb_get_image (conn, XCB_IMAGE_FORMAT_Z_PIXMAP, grabbed_win, - 0, 0, grab_width, grab_height, ~0), NULL); - - - - xcb_depth_iterator_t depth_iterator = xcb_screen_allowed_depths_iterator(grab_screen); - - int ndepths=xcb_screen_allowed_depths_length(grab_screen); - - for (int i=0; idepth == img->depth) - { - xcb_visualtype_t* visuals = xcb_depth_visuals(depth_iterator.data); - int nvisuals = xcb_depth_visuals_length(depth_iterator.data); - - for (int j=0;jvisual) - { - assert(visuals[j]._class==XCB_VISUAL_CLASS_TRUE_COLOR || visuals[j]._class==XCB_VISUAL_CLASS_DIRECT_COLOR); - cout << (int)visuals[j]._class << "," << XCB_VISUAL_CLASS_TRUE_COLOR << endl; - cout << visuals[j].red_mask << endl; - cout << visuals[j].green_mask << endl; - cout << visuals[j].blue_mask << endl; - break; - } - } - - break; - } - - xcb_depth_next(&depth_iterator); - } - - - - int nformats = xcb_setup_pixmap_formats_length(setup); - xcb_format_t* formats = xcb_setup_pixmap_formats(setup); - for (int i=0;idepth) - { - cout << (int)formats[i].bits_per_pixel << endl; - cout << (int)formats[i].scanline_pad << endl; - break; - } - } - - cout << grab_width << "x" << grab_height << endl; - - free(img); - - } - else - { - throw string("FATAL: did not find window, exiting."); - } - - -} - -XorgGrabber::~XorgGrabber() -{ - if (img) free(img); - xcb_disconnect(conn); -} - -void XorgGrabber::read(Mat& mat) -{ - if (img) free(img); - - - // mat gets invalid when the next read() is called! - img = xcb_get_image_reply (conn, - xcb_get_image (conn, XCB_IMAGE_FORMAT_Z_PIXMAP, grabbed_win, - 0, 0, grab_width, grab_height, ~0), NULL); - - mat = Mat(grab_height, grab_width, CV_8UC4, xcb_get_image_data(img)); - mat.addref(); -} - - - - -#define THROTTLE_CNT_MAX 10 - - -#ifdef FREEBSD -typedef union { - unsigned int Value; - struct { - unsigned R_DPAD : 1; - unsigned L_DPAD : 1; - unsigned D_DPAD : 1; - unsigned U_DPAD : 1; - unsigned START_BUTTON : 1; - unsigned Z_TRIG : 1; - unsigned B_BUTTON : 1; - unsigned A_BUTTON : 1; - - unsigned R_CBUTTON : 1; - unsigned L_CBUTTON : 1; - unsigned D_CBUTTON : 1; - unsigned U_CBUTTON : 1; - unsigned R_TRIG : 1; - unsigned L_TRIG : 1; - unsigned Reserved1 : 1; - unsigned Reserved2 : 1; - - signed X_AXIS : 8; - signed Y_AXIS : 8; - }; -} BUTTONS; - - -char* pack(const BUTTONS* buttons, char* buf) -{ - buf[0]= (buttons->A_BUTTON ? 1 : 0) + - (buttons->B_BUTTON ? 2 : 0) + - (buttons->L_TRIG ? 4 : 0) + - (buttons->R_TRIG ? 8 : 0) + - (buttons->Z_TRIG ? 16 : 0) + - (buttons->START_BUTTON ? 32 : 0) + - 128; - - buf[1]= (buttons->R_DPAD ? 1 : 0) + - (buttons->L_DPAD ? 2 : 0) + - (buttons->U_DPAD ? 4 : 0) + - (buttons->D_DPAD ? 8 : 0) + - ((buttons->X_AXIS & 128) ? 16 : 0) + - ((buttons->Y_AXIS & 128) ? 32 : 0); - - buf[2]= (buttons->R_CBUTTON ? 1 : 0) + - (buttons->L_CBUTTON ? 2 : 0) + - (buttons->U_CBUTTON ? 4 : 0) + - (buttons->D_CBUTTON ? 8 : 0); - - buf[3]= (buttons->X_AXIS & 127); - - buf[4]= (buttons->Y_AXIS & 127); - - return buf; -} -#endif - -class Joystick -{ - public: - Joystick(); - ~Joystick(); - void steer(float dir, float dead_zone=0.0); - void throttle(float t); - void press_a(bool); - - - void process(); - void reset(); - - private: -#ifdef FREEBSD - BUTTONS buttons; - void send_data(); - int fifo_fd; -#endif -#ifdef LINUX - int fd; -#endif - - float throt; - int throttle_cnt; - -}; - -#ifdef FREEBSD -Joystick::Joystick() -{ - if ((fifo_fd=open("/var/tmp/mupen64plus_ctl", O_WRONLY )) == -1) {throw string(strerror(errno));} - cout << "opened" << endl; - if (fcntl(fifo_fd, F_SETFL, O_NONBLOCK) == -1) throw string("failed to set nonblocking io"); - - reset(); -} - -void Joystick::press_a(bool state) -{ - buttons.A_BUTTON=state; - send_data(); -} - -void Joystick::send_data() -{ - char buf[5]; - pack(&buttons, buf); - write(fifo_fd, buf, 5); -} - - - -void Joystick::steer(float dir, float dead_zone) -{ - if (dir<-1.0) dir=-1.0; - if (dir>1.0) dir=1.0; - - if (fabs(dir)1.0) dir=1.0; - - if (fabs(dir)1.0) t=1.0; - - throt=t; -} - -void Joystick::process() -{ - throttle_cnt++; - if (throttle_cnt>=THROTTLE_CNT_MAX) throttle_cnt=0; - - press_a((throttle_cnt < throt*THROTTLE_CNT_MAX)); -} - - - -#define HIST_SMOOTH 7 - -//#define NO_BRIGHTNESS // will man nicht, nur zu demonstrationszwecken - -#define ERODE_RADIUS_2D 4 - - -#define IMG_HISTORY 3 - - - - - -int crosshair_x=0, crosshair_y=0; - -void mouse_callback(int event, int x, int y, int flags, void* userdata) -{ - if (event==EVENT_LBUTTONDOWN) - { - crosshair_x=x; - crosshair_y=y; - } -} - - - - - -sem_t thread1_go; -sem_t thread1_done; -Mat thread1_img; - -void* thread1_func(void*) -{ - Mat gray; - static Mat gray_prev; - - static std::vector points[2]; - - std::vector status; // status of tracked features - std::vector err; // error in tracking - - - cout << "thread 1 is alive :)" < features; // detected features - - // detect the features - goodFeaturesToTrack(gray, // the image - features, // the output detected features - 3000, // the maximum number of features - 0.2, // quality level - 10); // min distance between two features - - - // add the detected features to - // the currently tracked features - points[0].insert(points[0].end(), features.begin(),features.end()); - } - - - // for first image of the sequence - if(gray_prev.empty()) - gray.copyTo(gray_prev); - - cv::calcOpticalFlowPyrLK( - gray_prev, gray, // 2 consecutive images - points[0], // input point positions in first image - points[1], // output point positions in the 2nd image - status, // tracking success - err); // tracking error - - - - int k=0; - for(int i=0; i < points[1].size(); i++) { - // do we keep this point? - if (status[i]) { - // keep this point in vector - points[0][k] = points[0][i]; - points[1][k++] = points[1][i]; - } - } - // eliminate unsuccesful points - points[0].resize(k); - points[1].resize(k); - - - - // for all tracked points - for(int i= 0; i < points[1].size(); i++ ) { - // draw line and circle - cv::line(thread1_img, - points[0][i], // initial position - points[1][i],// new position - cv::Scalar(255,255,255)); - - cv::circle(thread1_img, points[1][i], 2, - cv::Scalar(255,255,255),-1); - } - - // 4. extrapolate movement - for (int i=0;i(row); - - for (int col=0; col0) - { - data[0]=(int)data[0] * 256 / sum; - data[1]=(int)data[1] * 256 / sum; - data[2]=(int)data[2] * 256 / sum; - } - else - { - data[0]=255/3; - data[1]=255/3; - data[2]=255/3; - } - data+=img.step[1]; - } - } - #endif - - - road_thresholder->process_image(img); - Mat img_thres = road_thresholder->get_road(); - - Mat img_eroded(img_thres.rows, img_thres.cols, img_thres.type()); - Mat img_tmp(img_thres.rows, img_thres.cols, img_thres.type()); - Mat img_thres2(img_thres.rows, img_thres.cols, img_thres.type()); - - erode(img_thres, img_tmp, erode_2d_small); - dilate(img_tmp, img_thres, erode_2d_small); - dilate(img_thres, img_tmp, erode_2d_big); - erode(img_tmp, img_thres2, erode_2d_big); - - - - - - - - - - - assert(img.rows==img_eroded.rows); - assert(img.cols==img_eroded.cols); - - - - - -/* - Mat img_perspective(trans_height, trans_width, img_thres.type()); - warpPerspective(img_thres, img_perspective, transform, img_perspective.size()); - - - threshold(img_perspective, img_perspective, 127, 255, THRESH_BINARY); - Mat img_perspective_temp(img_perspective.rows, img_perspective.cols, img_perspective.type()); - Mat img_perspective_temp2(img_perspective.rows, img_perspective.cols, img_perspective.type()); - erode(img_perspective, img_perspective_temp, circle_mat(7)); - dilate(img_perspective_temp, img_perspective_temp2, circle_mat(7 + 15)); - erode(img_perspective_temp2, img_perspective, circle_mat(15)); - */ - - - img.row(crosshair_y)=Scalar(255,0,0); - img.col(crosshair_x)=Scalar(255,0,0); - img_thres2.row(crosshair_y)=128; - img_thres2.col(crosshair_x)=128; - - steerer->process_image(img_thres2); - double steer_value = steerer->get_steer_data(); - - Mat steer=Mat::zeros(20,1920,CV_8U); - steer.col( steer.cols /2 )=128; - if (steerer->get_confidence()>0) - { - int x = (steer_value/2.0 + 0.5)*steer.cols; - - if (x<1) x=1; - if (x>=steer.cols-1) x=steer.cols-2; - - steer.col(x) = 255; - steer.col(x-1)=240; - steer.col(x+1)=240; - - - joystick.steer( steer_value,0.05); - } - else - joystick.steer(0.0); - - - - - sem_wait(&thread1_done); // wait for thread1 to finish - - - //imshow("orig", img); - imshow("edit", img); - //imshow("perspective", img_perspective); - //imshow("diff", img_diff); - - imshow("thres", img_thres); - imshow("thres2", img_thres2); - imshow("tracked", thread1_img); - imshow("steer", steer); - - - - //img_writer << img_diff; - thres_writer << img_thres; - thres2_writer << img_thres2; - - waitKey(1000/50); - joystick.process(); -} - -} //try -catch(string meh) -{ - cout << "error: "<