├── LICENSE ├── PredictSteeringAngle.cpp ├── README.md ├── RecordSteeringData.cpp └── steering_wheel_image.jpg /LICENSE: -------------------------------------------------------------------------------- 1 | The MIT License (MIT) 2 | 3 | Copyright (c) 2016 Sully Chen 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /PredictSteeringAngle.cpp: -------------------------------------------------------------------------------- 1 | //This is a modified version of BVLC Caffe's "classification.cpp" 2 | 3 | // Created by Sully Chen 4 | // Copyright © 2015 Sully Chen. All rights reserved. 5 | 6 | //This software is for video demonstration purposes only. Please DO NOT use this to pilot a car! 7 | 8 | #include 9 | #include 10 | #define USE_OPENCV = 1; 11 | #ifdef USE_OPENCV 12 | #include "opencv2/opencv.hpp" 13 | #include 14 | #include 15 | #include 16 | #endif // USE_OPENCV 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include 24 | 25 | #include /* Standard input/output definitions */ 26 | #include /* Standard types */ 27 | #include /* UNIX standard function definitions */ 28 | #include /* File control definitions */ 29 | #include /* Error number definitions */ 30 | #include /* POSIX terminal control definitions */ 31 | #include 32 | #include 33 | 34 | #include "SFML/Graphics.hpp" 35 | 36 | const int width = 240; //SFML window width 37 | const int height = 240; //SFML window height 38 | 39 | #ifdef USE_OPENCV 40 | using namespace caffe; // NOLINT(build/namespaces) 41 | using std::string; 42 | 43 | char buf[256]; //where the serial messages received are stored 44 | 45 | void usage(void); 46 | int serialport_init(const char* serialport, int baud); 47 | int serialport_writebyte(int fd, uint8_t b); 48 | int serialport_write(int fd, const char* str); 49 | int serialport_read_until(int fd, char* buf, char until); 50 | 51 | void thread1() //messages are read in a separate thread to fix timing issues with OpenCV 52 | { 53 | int fd = 0; 54 | char serialport[256]; 55 | int baudrate = B115200; // default 56 | fd = serialport_init("/dev/cu.usbmodem1421", baudrate); //open serial port of arduino 57 | if(fd == -1) 58 | std::cout << "Error opening port!" << std::endl; 59 | while (true) 60 | serialport_read_until(fd, buf, '\n'); 61 | } 62 | 63 | /* Pair (label, confidence) representing a prediction. */ 64 | typedef std::pair Prediction; 65 | 66 | class Classifier { 67 | public: 68 | Classifier(const string& model_file, 69 | const string& trained_file, 70 | const string& mean_file, 71 | const string& label_file); 72 | 73 | std::vector Classify(const cv::Mat& img, int N = 5); 74 | 75 | private: 76 | void SetMean(const string& mean_file); 77 | 78 | std::vector Predict(const cv::Mat& img); 79 | 80 | void WrapInputLayer(std::vector* input_channels); 81 | 82 | void Preprocess(const cv::Mat& img, 83 | std::vector* input_channels); 84 | 85 | private: 86 | shared_ptr > net_; 87 | cv::Size input_geometry_; 88 | int num_channels_; 89 | cv::Mat mean_; 90 | std::vector labels_; 91 | }; 92 | 93 | Classifier::Classifier(const string& model_file, 94 | const string& trained_file, 95 | const string& mean_file, 96 | const string& label_file) { 97 | #ifdef CPU_ONLY 98 | Caffe::set_mode(Caffe::CPU); 99 | #else 100 | Caffe::set_mode(Caffe::GPU); 101 | #endif 102 | 103 | /* Load the network. */ 104 | net_.reset(new Net(model_file, TEST)); 105 | net_->CopyTrainedLayersFrom(trained_file); 106 | 107 | CHECK_EQ(net_->num_inputs(), 1) << "Network should have exactly one input."; 108 | CHECK_EQ(net_->num_outputs(), 1) << "Network should have exactly one output."; 109 | 110 | Blob* input_layer = net_->input_blobs()[0]; 111 | num_channels_ = input_layer->channels(); 112 | CHECK(num_channels_ == 3 || num_channels_ == 1) 113 | << "Input layer should have 1 or 3 channels."; 114 | input_geometry_ = cv::Size(input_layer->width(), input_layer->height()); 115 | 116 | /* Load the binaryproto mean file. */ 117 | SetMean(mean_file); 118 | 119 | /* Load labels. */ 120 | std::ifstream labels(label_file.c_str()); 121 | CHECK(labels) << "Unable to open labels file " << label_file; 122 | string line; 123 | while (std::getline(labels, line)) 124 | labels_.push_back(string(line)); 125 | 126 | Blob* output_layer = net_->output_blobs()[0]; 127 | CHECK_EQ(labels_.size(), output_layer->channels()) 128 | << "Number of labels is different from the output layer dimension."; 129 | } 130 | 131 | static bool PairCompare(const std::pair& lhs, 132 | const std::pair& rhs) { 133 | return lhs.first > rhs.first; 134 | } 135 | 136 | /* Return the indices of the top N values of vector v. */ 137 | static std::vector Argmax(const std::vector& v, int N) { 138 | std::vector > pairs; 139 | for (size_t i = 0; i < v.size(); ++i) 140 | pairs.push_back(std::make_pair(v[i], i)); 141 | std::partial_sort(pairs.begin(), pairs.begin() + N, pairs.end(), PairCompare); 142 | 143 | std::vector result; 144 | for (int i = 0; i < N; ++i) 145 | result.push_back(pairs[i].second); 146 | return result; 147 | } 148 | 149 | /* Return the top N predictions. */ 150 | std::vector Classifier::Classify(const cv::Mat& img, int N) { 151 | std::vector output = Predict(img); 152 | 153 | N = std::min(labels_.size(), N); 154 | std::vector maxN = Argmax(output, N); 155 | std::vector predictions; 156 | for (int i = 0; i < N; ++i) { 157 | int idx = maxN[i]; 158 | predictions.push_back(std::make_pair(labels_[idx], output[idx])); 159 | } 160 | 161 | return predictions; 162 | } 163 | 164 | /* Load the mean file in binaryproto format. */ 165 | void Classifier::SetMean(const string& mean_file) { 166 | BlobProto blob_proto; 167 | ReadProtoFromBinaryFileOrDie(mean_file.c_str(), &blob_proto); 168 | 169 | /* Convert from BlobProto to Blob */ 170 | Blob mean_blob; 171 | mean_blob.FromProto(blob_proto); 172 | CHECK_EQ(mean_blob.channels(), num_channels_) 173 | << "Number of channels of mean file doesn't match input layer."; 174 | 175 | /* The format of the mean file is planar 32-bit float BGR or grayscale. */ 176 | std::vector channels; 177 | float* data = mean_blob.mutable_cpu_data(); 178 | for (int i = 0; i < num_channels_; ++i) { 179 | /* Extract an individual channel. */ 180 | cv::Mat channel(mean_blob.height(), mean_blob.width(), CV_32FC1, data); 181 | channels.push_back(channel); 182 | data += mean_blob.height() * mean_blob.width(); 183 | } 184 | 185 | /* Merge the separate channels into a single image. */ 186 | cv::Mat mean; 187 | cv::merge(channels, mean); 188 | 189 | /* Compute the global mean pixel value and create a mean image 190 | * filled with this value. */ 191 | cv::Scalar channel_mean = cv::mean(mean); 192 | mean_ = cv::Mat(input_geometry_, mean.type(), channel_mean); 193 | } 194 | 195 | std::vector Classifier::Predict(const cv::Mat& img) { 196 | Blob* input_layer = net_->input_blobs()[0]; 197 | input_layer->Reshape(1, num_channels_, 198 | input_geometry_.height, input_geometry_.width); 199 | /* Forward dimension change to all layers. */ 200 | net_->Reshape(); 201 | 202 | std::vector input_channels; 203 | WrapInputLayer(&input_channels); 204 | 205 | Preprocess(img, &input_channels); 206 | 207 | net_->Forward(); 208 | 209 | /* Copy the output layer to a std::vector */ 210 | Blob* output_layer = net_->output_blobs()[0]; 211 | const float* begin = output_layer->cpu_data(); 212 | const float* end = begin + output_layer->channels(); 213 | return std::vector(begin, end); 214 | } 215 | 216 | /* Wrap the input layer of the network in separate cv::Mat objects 217 | * (one per channel). This way we save one memcpy operation and we 218 | * don't need to rely on cudaMemcpy2D. The last preprocessing 219 | * operation will write the separate channels directly to the input 220 | * layer. */ 221 | void Classifier::WrapInputLayer(std::vector* input_channels) { 222 | Blob* input_layer = net_->input_blobs()[0]; 223 | 224 | int width = input_layer->width(); 225 | int height = input_layer->height(); 226 | float* input_data = input_layer->mutable_cpu_data(); 227 | for (int i = 0; i < input_layer->channels(); ++i) { 228 | cv::Mat channel(height, width, CV_32FC1, input_data); 229 | input_channels->push_back(channel); 230 | input_data += width * height; 231 | } 232 | } 233 | 234 | void Classifier::Preprocess(const cv::Mat& img, 235 | std::vector* input_channels) { 236 | /* Convert the input image to the input image format of the network. */ 237 | cv::Mat sample; 238 | if (img.channels() == 3 && num_channels_ == 1) 239 | cv::cvtColor(img, sample, cv::COLOR_BGR2GRAY); 240 | else if (img.channels() == 4 && num_channels_ == 1) 241 | cv::cvtColor(img, sample, cv::COLOR_BGRA2GRAY); 242 | else if (img.channels() == 4 && num_channels_ == 3) 243 | cv::cvtColor(img, sample, cv::COLOR_BGRA2BGR); 244 | else if (img.channels() == 1 && num_channels_ == 3) 245 | cv::cvtColor(img, sample, cv::COLOR_GRAY2BGR); 246 | else 247 | sample = img; 248 | 249 | cv::Mat sample_resized; 250 | if (sample.size() != input_geometry_) 251 | cv::resize(sample, sample_resized, input_geometry_); 252 | else 253 | sample_resized = sample; 254 | 255 | cv::Mat sample_float; 256 | if (num_channels_ == 3) 257 | sample_resized.convertTo(sample_float, CV_32FC3); 258 | else 259 | sample_resized.convertTo(sample_float, CV_32FC1); 260 | 261 | cv::Mat sample_normalized; 262 | cv::subtract(sample_float, mean_, sample_normalized); 263 | 264 | /* This operation will write the separate BGR planes directly to the 265 | * input layer of the network because it is wrapped by the cv::Mat 266 | * objects in input_channels. */ 267 | cv::split(sample_normalized, *input_channels); 268 | 269 | CHECK(reinterpret_cast(input_channels->at(0).data) 270 | == net_->input_blobs()[0]->cpu_data()) 271 | << "Input channels are not wrapping the input layer of the network."; 272 | } 273 | 274 | int main(int argc, char** argv) { 275 | if (argc != 5) { 276 | std::cerr << "Usage: " << argv[0] 277 | << " deploy.prototxt network.caffemodel" 278 | << " mean.binaryproto labels.txt" << std::endl; 279 | return 1; 280 | } 281 | 282 | //SFML things to draw steering wheel 283 | sf::String title_string = "Predicted"; 284 | sf::String title_string2 = "Actual"; 285 | sf::RenderWindow window(sf::VideoMode(width, height), title_string); 286 | sf::RenderWindow window2(sf::VideoMode(width, height), title_string2); 287 | window.setFramerateLimit(30); 288 | window2.setFramerateLimit(30); 289 | 290 | ::google::InitGoogleLogging(argv[0]); 291 | 292 | string model_file = argv[1]; 293 | string trained_file = argv[2]; 294 | string mean_file = argv[3]; 295 | string label_file = argv[4]; 296 | Classifier classifier(model_file, trained_file, mean_file, label_file); 297 | 298 | cv::namedWindow("img",1); 299 | 300 | //open camera 301 | cv::VideoCapture cap(0); 302 | if(!cap.isOpened()) 303 | return -1; 304 | 305 | //load the steering wheel image 306 | sf::Texture steeringWheelTexture; 307 | steeringWheelTexture.loadFromFile("steering_wheel_image.jpg"); 308 | 309 | //create steering wheel sprite 310 | sf::Sprite steeringWheelSprite; 311 | steeringWheelSprite.setTexture(steeringWheelTexture); 312 | steeringWheelSprite.setPosition(120, 120); 313 | steeringWheelSprite.setRotation(30.0f); 314 | steeringWheelSprite.setOrigin(120, 120); 315 | 316 | double smoothed_angle = 0.0f; //Used as the final steering output 317 | 318 | int actual_angle = 0; 319 | 320 | std::thread t(thread1); 321 | 322 | while (true) 323 | { 324 | int actual_angle = (int)::atof(buf); 325 | if (cv::waitKey(10) == 'q') break; // break the loop if the key "q" is pressed 326 | cv::Mat img; 327 | cap >> img; // get a new frame from camera 328 | 329 | //crop the camera image 330 | cv::Rect myROI(280, 0, 720, 720); 331 | cv::Mat croppedImage = img(myROI); 332 | 333 | resize(croppedImage, img, cv::Size(), 256.0f/720.0f, 256.0f/720.0f, cv::INTER_LANCZOS4); //resize to 256x256 334 | 335 | cv::imshow("img", img); //show the image 336 | 337 | //canny edge filtering 338 | Canny(img, img, 50, 200, 3); 339 | 340 | //Preprocessing to remove highly cluttered areas from the image 341 | cv::Mat mask; 342 | GaussianBlur(img, mask, cv::Size(35, 35), 10, 10); 343 | threshold(mask, mask, 60, 255, cv::THRESH_BINARY_INV); 344 | bitwise_and(img, mask, img); 345 | 346 | cv::imshow("filtered img", img); //show the image 347 | 348 | std::vector predictions = classifier.Classify(img); //Use BVLC Caffe to classify the image 349 | 350 | system("clear"); //clear the console for neatness 351 | 352 | std::vector angle_categories; //This vector stores all of the possible angle outputs from the classifier 353 | std::vector outputs; //This vector stores all of the corresponding prediction outputs from the classifier 354 | 355 | //process classifier outputs 356 | for (int i = 0; i < predictions.size(); i++) 357 | { 358 | Prediction p = predictions[i]; 359 | int angle_category; 360 | 361 | //if the category is just "0", then the angle_category is 0 362 | if (p.first == "0") 363 | angle_category = 0; 364 | else //otherwise, do further processing 365 | { 366 | //categories are in the format "posXXX" or "negXXX", where XXX is an angle measure 367 | angle_category = std::stoi(p.first.substr(3, p.first.length() - 3)); //convert the string to an integer, ignoring the first three characters which are likely "pos" or "neg" 368 | 369 | //if the first three letters are "neg", multiply the category by -1 to make the category negative 370 | if (p.first.substr(0, 3) == "neg") 371 | angle_category *= -1; 372 | } 373 | angle_categories.push_back(angle_category); //store the possible categories in the categories vector 374 | outputs.push_back(p.second); //store the corresponding output in the outputs vector 375 | } 376 | 377 | double output_angle = 0.0f; //this is used for a processing step before the final angle 378 | 379 | //do a weighted sum of the classifier outputs 380 | for (int i = 0; i < angle_categories.size(); i++) 381 | output_angle += angle_categories[i] * outputs[i]; 382 | 383 | //make smooth angle transitions by turning the steering wheel based on the difference of the current angle 384 | //and the predicted angle 385 | smoothed_angle += 1.0f * pow(std::abs((output_angle - smoothed_angle)), 2.0f / 3.0f) 386 | * (output_angle - smoothed_angle) / std::abs(output_angle - smoothed_angle); 387 | 388 | //print the angle 389 | if (smoothed_angle < 0) 390 | std::cout << std::fixed << std::setprecision(2) << -1.0f * smoothed_angle << " degrees left" << std::endl; 391 | else if (smoothed_angle > 0) 392 | std::cout << std::fixed << std::setprecision(2) << smoothed_angle << " degrees right" << std::endl; 393 | else 394 | std::cout << "0 degrees" << std::endl; 395 | 396 | //update SFML things 397 | steeringWheelSprite.setRotation(smoothed_angle); 398 | 399 | window.clear(sf::Color::White); 400 | window2.clear(sf::Color::White); 401 | 402 | window.draw(steeringWheelSprite); 403 | 404 | steeringWheelSprite.setRotation(actual_angle); 405 | window2.draw(steeringWheelSprite); 406 | 407 | window.display(); 408 | window2.display(); 409 | } 410 | } 411 | #else 412 | int main(int argc, char** argv) { 413 | LOG(FATAL) << "This example requires OpenCV; compile with USE_OPENCV."; 414 | } 415 | #endif // USE_OPENCV 416 | 417 | int serialport_writebyte( int fd, uint8_t b) 418 | { 419 | int n = write(fd,&b,1); 420 | if( n!=1) 421 | return -1; 422 | return 0; 423 | } 424 | 425 | int serialport_write(int fd, const char* str) 426 | { 427 | int len = strlen(str); 428 | int n = write(fd, str, len); 429 | if( n!=len ) 430 | return -1; 431 | return 0; 432 | } 433 | 434 | int serialport_read_until(int fd, char* buf, char until) 435 | { 436 | char b[1]; 437 | int i=0; 438 | do { 439 | int n = read(fd, b, 1); // read a char at a time 440 | if( n==-1) return -1; // couldn't read 441 | if( n==0 ) { 442 | usleep( 10 * 1000 ); // wait 10 msec try again 443 | continue; 444 | } 445 | buf[i] = b[0]; i++; 446 | } while( b[0] != until ); 447 | 448 | buf[i] = 0; // null terminate the string 449 | return 0; 450 | } 451 | 452 | // takes the string name of the serial port (e.g. "/dev/tty.usbserial","COM1") 453 | // and a baud rate (bps) and connects to that port at that speed and 8N1. 454 | // opens the port in fully raw mode so you can send binary data. 455 | // returns valid fd, or -1 on error 456 | int serialport_init(const char* serialport, int baud) 457 | { 458 | struct termios toptions; 459 | int fd; 460 | 461 | //fprintf(stderr,"init_serialport: opening port %s @ %d bps\n", 462 | // serialport,baud); 463 | 464 | fd = open(serialport, O_RDWR | O_NOCTTY | O_NDELAY); 465 | if (fd == -1) { 466 | perror("init_serialport: Unable to open port "); 467 | return -1; 468 | } 469 | 470 | if (tcgetattr(fd, &toptions) < 0) { 471 | perror("init_serialport: Couldn't get term attributes"); 472 | return -1; 473 | } 474 | speed_t brate = baud; // let you override switch below if needed 475 | switch(baud) { 476 | case 4800: brate=B4800; break; 477 | case 9600: brate=B9600; break; 478 | #ifdef B14400 479 | case 14400: brate=B14400; break; 480 | #endif 481 | case 19200: brate=B19200; break; 482 | #ifdef B28800 483 | case 28800: brate=B28800; break; 484 | #endif 485 | case 38400: brate=B38400; break; 486 | case 57600: brate=B57600; break; 487 | case 115200: brate=B115200; break; 488 | } 489 | cfsetispeed(&toptions, brate); 490 | cfsetospeed(&toptions, brate); 491 | 492 | // 8N1 493 | toptions.c_cflag &= ~PARENB; 494 | toptions.c_cflag &= ~CSTOPB; 495 | toptions.c_cflag &= ~CSIZE; 496 | toptions.c_cflag |= CS8; 497 | // no flow control 498 | toptions.c_cflag &= ~CRTSCTS; 499 | 500 | toptions.c_cflag |= CREAD | CLOCAL; // turn on READ & ignore ctrl lines 501 | toptions.c_iflag &= ~(IXON | IXOFF | IXANY); // turn off s/w flow ctrl 502 | 503 | toptions.c_lflag &= ~(ICANON | ECHO | ECHOE | ISIG); // make raw 504 | toptions.c_oflag &= ~OPOST; // make raw 505 | 506 | // see: http://unixwiz.net/techtips/termios-vmin-vtime.html 507 | toptions.c_cc[VMIN] = 0; 508 | toptions.c_cc[VTIME] = 20; 509 | 510 | if( tcsetattr(fd, TCSANOW, &toptions) < 0) { 511 | perror("init_serialport: Couldn't set term attributes"); 512 | return -1; 513 | } 514 | 515 | return fd; 516 | } 517 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Caffe-Autopilot 2 | 3 | **This software is for video demonstration purposes only. Please _DO_ _NOT_ use this to pilot a car!** 4 | 5 | A car autopilot system developed using C++, [BVLC Caffe](https://github.com/BVLC/caffe), [OpenCV](http://opencv.org/), and [SFML](http://www.sfml-dev.org/) 6 | 7 | **The model in this repository has _only_ been trained on roads with a visible, solid yellow line divider. More models are being trained for different road types and driving conditions. These models will be uploaded when they are ready.** 8 | 9 | [Video here](https://www.youtube.com/watch?v=fSbWnQ_wzvM) 10 | 11 | [Datasets and models](https://drive.google.com/open?id=0B-KJCaaF7ellNFVFSUpVWGlTUWM) 12 | 13 | ## How it works 14 | 15 | Deep convolutional neural nets are powerful. 16 | 17 | First, a dataset is collected using a webcam and an Arduino with a CANBUS shield to read the steering wheel angles from the OBD-II port of the car. 18 | 19 | The images are preprocessed with Canny edge filtering and a certain threshold operation as they are collected. 20 | The purpose of the preprocessing is to make less work for the convolutional neural network by extracting the important features with image processing algorithms ahead of time. 21 | 22 | Then, [BVLC Caffe](https://github.com/BVLC/caffe) is used to train a deep convolutional neural network (specifically AlexNet). 23 | 24 | To predict steering angles based on input images, a weighted sum of the BVLC Caffe classifier outputs is used to calculate the final angle. Some smoothing algorithms are also used to smooth the motion of the predicted steering wheel angle. 25 | -------------------------------------------------------------------------------- /RecordSteeringData.cpp: -------------------------------------------------------------------------------- 1 | //This code uses arduino serial communication code by Tod E. Kurt, tod@todbot.com http://todbot.com/blog/ 2 | 3 | // Created by Sully Chen 4 | // Copyright © 2015 Sully Chen. All rights reserved. 5 | 6 | #include /* Standard input/output definitions */ 7 | #include 8 | #include /* Standard types */ 9 | #include /* String function definitions */ 10 | #include /* UNIX standard function definitions */ 11 | #include /* File control definitions */ 12 | #include /* Error number definitions */ 13 | #include /* POSIX terminal control definitions */ 14 | #include 15 | #include 16 | #include "opencv2/opencv.hpp" 17 | #include 18 | #include 19 | 20 | using namespace std; 21 | using namespace cv; 22 | 23 | void usage(void); 24 | int serialport_init(const char* serialport, int baud); 25 | int serialport_writebyte(int fd, uint8_t b); 26 | int serialport_write(int fd, const char* str); 27 | int serialport_read_until(int fd, char* buf, char until); 28 | 29 | void usage(void) { 30 | printf("Usage: arduino-serial -p [OPTIONS]\n" 31 | "\n" 32 | "Options:\n" 33 | " -h, --help Print this help message\n" 34 | " -p, --port=serialport Serial port Arduino is on\n" 35 | " -b, --baud=baudrate Baudrate (bps) of Arduino\n" 36 | " -s, --send=data Send data to Arduino\n" 37 | " -r, --receive Receive data from Arduino & print it out\n" 38 | " -n --num=num Send a number as a single byte\n" 39 | " -d --delay=millis Delay for specified milliseconds\n" 40 | "\n" 41 | "Note: Order is important. Set '-b' before doing '-p'. \n" 42 | " Used to make series of actions: '-d 2000 -s hello -d 100 -r' \n" 43 | " means 'wait 2secs, send 'hello', wait 100msec, get reply'\n" 44 | "\n"); 45 | } 46 | 47 | char buf[256]; //where the serial messages received are stored 48 | 49 | void thread1() //messages are read in a separate thread to fix timing issues with OpenCV 50 | { 51 | int fd = 0; 52 | char serialport[256]; 53 | int baudrate = B115200; // default 54 | fd = serialport_init("/dev/cu.usbmodem1421", baudrate); //open serial port of arduino 55 | if(fd == -1) 56 | std::cout << "Error opening port!" << std::endl; 57 | while (true) 58 | serialport_read_until(fd, buf, '\n'); 59 | } 60 | 61 | int main(int argc, char *argv[]) 62 | { 63 | //generate data directories 64 | for (int i = -18; i <= 18; i++) 65 | { 66 | std::string s; 67 | if (i == 0) 68 | s = "mkdir 0"; 69 | else if (i < 0) 70 | s = "mkdir neg" + std::to_string(-i * 5); 71 | else 72 | s = "mkdir pos" + std::to_string(i * 5); 73 | std::cout << s << std::endl; 74 | system(s.c_str()); 75 | } 76 | 77 | //open camera 78 | VideoCapture cap(0); 79 | if(!cap.isOpened()) 80 | return -1; 81 | Mat edges; 82 | namedWindow("frame",1); 83 | int i = 0; 84 | std::cout << "Input starting index: "; 85 | cin >> i; 86 | cout << "\n"; 87 | std::thread t(thread1); 88 | while (true) 89 | { 90 | int key_press = waitKey(10); 91 | if (key_press != 's') 92 | { 93 | Mat frame; 94 | cap >> frame; // get a new frame from camera 95 | 96 | //crop and resize 97 | Rect myROI(280, 0, 720, 720); 98 | Mat croppedImage = frame(myROI); 99 | resize(croppedImage, frame, Size(), 256.0f/720.0f, 256.0f/720.0f, cv::INTER_LANCZOS4); 100 | 101 | //canny edge filtering 102 | Canny(frame, frame, 50, 200, 3); 103 | 104 | //Preprocessing to remove highly cluttered areas from the image 105 | Mat mask; 106 | GaussianBlur(frame, mask, Size(35, 35), 10, 10); 107 | threshold(mask, mask, 60, 255, THRESH_BINARY_INV); 108 | bitwise_and(frame, mask, frame); 109 | 110 | //get steering angle from the serial data 111 | int angle = (int)::atof(buf); 112 | 113 | if (angle <= 90 && angle >= -90) 114 | { 115 | std::string s; 116 | angle = (angle / 5) * 5; 117 | if (angle == 0) 118 | s = "0"; 119 | else if (angle < 0) 120 | s = "neg" + std::to_string(-angle); 121 | else 122 | s = "pos" + std::to_string(angle); 123 | std::cout << s << std::endl; 124 | imwrite(s + "/" + std::to_string(i) + ".jpg", frame); 125 | if (key_press == 'q') 126 | break; 127 | i++; 128 | } 129 | imshow("frame", frame); 130 | } else while (waitKey() != 'c'); 131 | } 132 | exit(EXIT_SUCCESS); 133 | } 134 | 135 | int serialport_writebyte( int fd, uint8_t b) 136 | { 137 | int n = write(fd,&b,1); 138 | if( n!=1) 139 | return -1; 140 | return 0; 141 | } 142 | 143 | int serialport_write(int fd, const char* str) 144 | { 145 | int len = strlen(str); 146 | int n = write(fd, str, len); 147 | if( n!=len ) 148 | return -1; 149 | return 0; 150 | } 151 | 152 | int serialport_read_until(int fd, char* buf, char until) 153 | { 154 | char b[1]; 155 | int i=0; 156 | do { 157 | int n = read(fd, b, 1); // read a char at a time 158 | if( n==-1) return -1; // couldn't read 159 | if( n==0 ) { 160 | usleep( 10 * 1000 ); // wait 10 msec try again 161 | continue; 162 | } 163 | buf[i] = b[0]; i++; 164 | } while( b[0] != until ); 165 | 166 | buf[i] = 0; // null terminate the string 167 | return 0; 168 | } 169 | 170 | // takes the string name of the serial port (e.g. "/dev/tty.usbserial","COM1") 171 | // and a baud rate (bps) and connects to that port at that speed and 8N1. 172 | // opens the port in fully raw mode so you can send binary data. 173 | // returns valid fd, or -1 on error 174 | int serialport_init(const char* serialport, int baud) 175 | { 176 | struct termios toptions; 177 | int fd; 178 | 179 | //fprintf(stderr,"init_serialport: opening port %s @ %d bps\n", 180 | // serialport,baud); 181 | 182 | fd = open(serialport, O_RDWR | O_NOCTTY | O_NDELAY); 183 | if (fd == -1) { 184 | perror("init_serialport: Unable to open port "); 185 | return -1; 186 | } 187 | 188 | if (tcgetattr(fd, &toptions) < 0) { 189 | perror("init_serialport: Couldn't get term attributes"); 190 | return -1; 191 | } 192 | speed_t brate = baud; // let you override switch below if needed 193 | switch(baud) { 194 | case 4800: brate=B4800; break; 195 | case 9600: brate=B9600; break; 196 | #ifdef B14400 197 | case 14400: brate=B14400; break; 198 | #endif 199 | case 19200: brate=B19200; break; 200 | #ifdef B28800 201 | case 28800: brate=B28800; break; 202 | #endif 203 | case 38400: brate=B38400; break; 204 | case 57600: brate=B57600; break; 205 | case 115200: brate=B115200; break; 206 | } 207 | cfsetispeed(&toptions, brate); 208 | cfsetospeed(&toptions, brate); 209 | 210 | // 8N1 211 | toptions.c_cflag &= ~PARENB; 212 | toptions.c_cflag &= ~CSTOPB; 213 | toptions.c_cflag &= ~CSIZE; 214 | toptions.c_cflag |= CS8; 215 | // no flow control 216 | toptions.c_cflag &= ~CRTSCTS; 217 | 218 | toptions.c_cflag |= CREAD | CLOCAL; // turn on READ & ignore ctrl lines 219 | toptions.c_iflag &= ~(IXON | IXOFF | IXANY); // turn off s/w flow ctrl 220 | 221 | toptions.c_lflag &= ~(ICANON | ECHO | ECHOE | ISIG); // make raw 222 | toptions.c_oflag &= ~OPOST; // make raw 223 | 224 | // see: http://unixwiz.net/techtips/termios-vmin-vtime.html 225 | toptions.c_cc[VMIN] = 0; 226 | toptions.c_cc[VTIME] = 20; 227 | 228 | if( tcsetattr(fd, TCSANOW, &toptions) < 0) { 229 | perror("init_serialport: Couldn't set term attributes"); 230 | return -1; 231 | } 232 | 233 | return fd; 234 | } 235 | -------------------------------------------------------------------------------- /steering_wheel_image.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/SullyChen/Caffe-Autopilot/886ff759b5d7f32f86727a7190f534868098636b/steering_wheel_image.jpg --------------------------------------------------------------------------------