Got vision working with Ben.
diff --git a/vision/OpenCVWorkTask.cpp b/vision/OpenCVWorkTask.cpp
index 5c420d3..df4d450 100644
--- a/vision/OpenCVWorkTask.cpp
+++ b/vision/OpenCVWorkTask.cpp
@@ -14,6 +14,7 @@
#include "aos/atom_code/camera/Buffers.h"
#include "aos/externals/libjpeg/include/jpeglib.h"
#include "aos/atom_code/init.h"
+#include "aos/common/logging/logging.h"
#include "vision/OpenCVWorkTask.h"
#include "vision/CameraProcessor.h"
@@ -22,7 +23,6 @@
#include "vision/JPEGRoutines.h"
-
namespace frc971 {
namespace vision {
@@ -33,6 +33,27 @@
frc971::vision::BinaryServer test(8020,notify);
}
+namespace {
+void SaveImageToFile(IplImage *image, const char *filename) {
+ FILE *file = fopen(filename, "w");
+
+ fputs("P3\n320 240\n255\n", file);
+ ::cv::Mat img(image);
+ for (int i = 0; i < img.rows; i++) {
+ for (int j = 0; j < img.cols; j++) {
+ // You can now access the pixel value with cv::Vec3b
+ fprintf(file, "%d %d %d ",
+ img.at<cv::Vec3b>(i,j)[0],
+ img.at<cv::Vec3b>(i,j)[1],
+ img.at<cv::Vec3b>(i,j)[2]);
+
+ }
+ fputs("\n", file);
+ }
+ fclose(file);
+}
+}
+
#include "frc971/queues/CameraTarget.q.h"
using frc971::vision::targets;
@@ -48,59 +69,69 @@
ProcessorData processor(size.width, size.height, false);
printf("started sender main\n");
+ LOG(INFO, "Camera server started\n");
while(true){
//usleep(7500);
size_t data_size;
timeval timestamp_timeval;
const void *image = buffers.GetNext(
- true, &data_size, ×tamp_timeval, NULL);
- ::aos::time::Time timestamp(timestamp_timeval);
+ true, &data_size, ×tamp_timeval, NULL);
+ ::aos::time::Time timestamp(timestamp_timeval);
//TODO(pschuh): find proper way to cast away const for this: :(
- // parker you prolly want const_cast<type>(var);
- printf("\nNew Image Recieved\n");
- std::cout << timestamp << std::endl;
+ // parker you prolly want const_cast<type>(var);
+ LOG(INFO, "Got new image\n");
unsigned char *buffer = (unsigned char *)notify->GetBuffer();
- frc971::vision::process_jpeg(buffer, (unsigned char *)image, data_size);
+ frc971::vision::process_jpeg(buffer, (unsigned char *)image, data_size);
+
// build the headers on top of the buffer
- cvSetData(processor.src_header_image,
- buffer,
- ::aos::camera::Buffers::Buffers::kWidth * 3);
+ cvSetData(processor.src_header_image,
+ buffer,
+ ::aos::camera::Buffers::Buffers::kWidth * 3);
- // transform the buffer into targets
- processor.threshold(buffer);
- processor.getContours();
- processor.filterToTargets();
+ // Reset.
+ processor.clear();
+ // transform the buffer into targets
+ processor.threshold(buffer);
- // could be used for debug ie drawContours
- //std::vector<std::vector<cv::Point> > target_contours;
- //std::vector<std::vector<cv::Point> > best_contours;
- std::vector<Target>::iterator target_it;
- Target *best_target = NULL;
- // run through the targets
- for(target_it = processor.target_list.begin();
- target_it != processor.target_list.end(); target_it++){
- //target_contours.push_back(*(target_it->this_contour));
- // select the highest target
- if (best_target == NULL) {
- best_target = &*target_it;
- } else {
- if (target_it->height < best_target->height) {
- best_target = &*target_it;
- }
- }
- }
- // if we found one then send it on
- if (best_target != NULL) {
- targets.MakeWithBuilder()
- .percent_azimuth_off_center(
- best_target->rect.centroid.y / (double)::aos::camera::Buffers::kHeight - 0.5)
- .percent_elevation_off_center(
- best_target->rect.centroid.x / (double)::aos::camera::Buffers::kWidth - 0.5)
- .timestamp(timestamp.ToNSec())
- .Send();
+ processor.getContours();
+ processor.filterToTargets();
+
+ // could be used for debug ie drawContours
+ //std::vector<std::vector<cv::Point> > target_contours;
+ //std::vector<std::vector<cv::Point> > best_contours;
+ std::vector<Target>::iterator target_it;
+ Target *best_target = NULL;
+ // run through the targets
+ LOG(INFO, "Found %u targets\n", processor.target_list.size());
+ for(target_it = processor.target_list.begin();
+ target_it != processor.target_list.end(); target_it++){
+ //target_contours.push_back(*(target_it->this_contour));
+ // select the highest target
+ if (best_target == NULL) {
+ best_target = &*target_it;
+ } else {
+ if (target_it->height < best_target->height) {
+ best_target = &*target_it;
+ }
+ }
+ }
+ // if we found one then send it on
+ if (best_target != NULL) {
+ LOG(INFO, "Target is %f\n", best_target->rect.centroid.x);
+ targets.MakeWithBuilder()
+ .percent_azimuth_off_center(
+ best_target->rect.centroid.y / (double)::aos::camera::Buffers::kHeight - 0.5)
+ .percent_elevation_off_center(
+ best_target->rect.centroid.x / (double)::aos::camera::Buffers::kWidth - 0.5)
+ .timestamp(timestamp.ToNSec())
+ .Send();
}
notify->Notify();
+ //static int counter = 0;
+ //if (++counter > 2) {
+ //break;
+ //}
}
::aos::Cleanup();
}