got vision code actually working (and somewhat tuned) but it crashes
diff --git a/aos/atom_code/camera/Buffers.cpp b/aos/atom_code/camera/Buffers.cpp
index 9cd1a7d..d0d20f5 100644
--- a/aos/atom_code/camera/Buffers.cpp
+++ b/aos/atom_code/camera/Buffers.cpp
@@ -23,18 +23,15 @@
} addr;
const int r = socket(AF_UNIX, SOCK_STREAM, 0);
if (r == -1) {
- LOG(ERROR, "socket(AF_UNIX, SOCK_STREAM, 0) failed with %d: %s\n",
+ LOG(FATAL, "socket(AF_UNIX, SOCK_STREAM, 0) failed with %d: %s\n",
errno, strerror(errno));
- return -1;
}
addr.un.sun_family = AF_UNIX;
memset(addr.un.sun_path, 0, sizeof(addr.un.sun_path));
strcpy(addr.un.sun_path, kFDServerName.c_str());
if (bind_connect(r, &addr.addr, sizeof(addr.un)) == -1) {
- LOG(ERROR, "bind_connect(=%p)(%d, %p, %zd) failed with %d: %s\n",
+ LOG(FATAL, "bind_connect(=%p)(%d, %p, %zd) failed with %d: %s\n",
bind_connect, r, &addr.addr, sizeof(addr.un), errno, strerror(errno));
- close(r); // what are we going to do about an error?
- return -1;
}
return r;
}
diff --git a/aos/atom_code/camera/Reader.cpp b/aos/atom_code/camera/Reader.cpp
index 95f6128..125fde1 100644
--- a/aos/atom_code/camera/Reader.cpp
+++ b/aos/atom_code/camera/Reader.cpp
@@ -391,7 +391,7 @@
}
if (FD_ISSET(fd_, &fds)) {
- LOG(INFO, "Got a frame\n");
+ LOG(DEBUG, "Got a frame\n");
ReadFrame();
}
if (FD_ISSET(server_fd_, &fds)) {
diff --git a/frc971/atom_code/scripts/start_list.txt b/frc971/atom_code/scripts/start_list.txt
index 1c4b259..82358de 100644
--- a/frc971/atom_code/scripts/start_list.txt
+++ b/frc971/atom_code/scripts/start_list.txt
@@ -11,6 +11,5 @@
gyro_sensor_receiver
CameraReader
CameraHTTPStreamer
-CameraServer
OpenCVWorkTask
GoalMaster
diff --git a/vision/BinaryServer.cpp b/vision/BinaryServer.cpp
index 29deee9..18cf85b 100644
--- a/vision/BinaryServer.cpp
+++ b/vision/BinaryServer.cpp
@@ -18,7 +18,7 @@
namespace frc971 {
namespace vision {
-static void echo_read_cb(struct bufferevent *bev, void * /*ctx*/){
+static void echo_read_cb(struct bufferevent *bev, void * /*ctx*/) {
struct evbuffer *input = bufferevent_get_input(bev);
struct evbuffer *output = bufferevent_get_output(bev);
@@ -32,94 +32,98 @@
evbuffer_add_buffer(output, input);
}
-void BinaryServer::ErrorEvent(struct bufferevent *bev, short events){
- if (events & BEV_EVENT_ERROR)
- perror("Error from bufferevent");
+void BinaryServer::ErrorEvent(struct bufferevent *bev, short events) {
+ if (events & BEV_EVENT_ERROR) perror("Error from bufferevent");
if (events & (BEV_EVENT_EOF | BEV_EVENT_ERROR)) {
have_id = false;
bufferevent_free(bev);
}
}
-void BinaryServer::Accept(struct evconnlistener *listener,
- evutil_socket_t fd, struct sockaddr * /*address*/, int /*socklen*/){
+void BinaryServer::Accept(struct evconnlistener *listener, evutil_socket_t fd,
+ struct sockaddr * /*address*/, int /*socklen*/) {
struct event_base *base = evconnlistener_get_base(listener);
- if(!have_id){
- struct bufferevent *bev = bufferevent_socket_new(base, fd, BEV_OPT_CLOSE_ON_FREE);
+ if (!have_id) {
+ struct bufferevent *bev =
+ bufferevent_socket_new(base, fd, BEV_OPT_CLOSE_ON_FREE);
_output = bufferevent_get_output(bev);
_bufev = bev;
have_id = true;
int no_delay_flag = 1;
- setsockopt(fd, IPPROTO_TCP, TCP_NODELAY, &no_delay_flag, sizeof(no_delay_flag));
+ setsockopt(fd, IPPROTO_TCP, TCP_NODELAY, &no_delay_flag,
+ sizeof(no_delay_flag));
bufferevent_setcb(bev, echo_read_cb, NULL, StaticErrorEvent, this);
bufferevent_enable(bev, EV_READ | EV_WRITE);
}
}
-static void accept_error_cb(struct evconnlistener *listener, void * /*ctx*/)
-{
+static void accept_error_cb(struct evconnlistener *listener, void * /*ctx*/) {
struct event_base *base = evconnlistener_get_base(listener);
int err = EVUTIL_SOCKET_ERROR();
fprintf(stderr, "Got an error %d (%s) on the listener. "
- "Shutting down.\n", err, evutil_socket_error_to_string(err));
+ "Shutting down.\n",
+ err, evutil_socket_error_to_string(err));
event_base_loopexit(base, NULL);
}
-void BinaryServer::StartServer(uint16_t port){
+void BinaryServer::StartServer(uint16_t port) {
_fd = socket(AF_INET, SOCK_STREAM, 0);
struct sockaddr_in sin;
memset(&sin, 0, sizeof(sin));
sin.sin_family = AF_INET;
- sin.sin_port = htons( port );
+ sin.sin_port = htons(port);
sin.sin_addr.s_addr = inet_addr("0.0.0.0");
- listener = evconnlistener_new_bind(_base, StaticAccept, this,
- LEV_OPT_CLOSE_ON_FREE | LEV_OPT_REUSEABLE, -1,
- (struct sockaddr *) (void *)&sin, sizeof(sin));
+ listener = evconnlistener_new_bind(
+ _base, StaticAccept, this, LEV_OPT_CLOSE_ON_FREE | LEV_OPT_REUSEABLE, -1,
+ (struct sockaddr *)(void *)&sin, sizeof(sin));
if (!listener) {
- fprintf(stderr,"%s:%d: Couldn't create listener\n", __FILE__, __LINE__);
+ fprintf(stderr, "%s:%d: Couldn't create listener\n", __FILE__, __LINE__);
exit(-1);
}
evconnlistener_set_error_cb(listener, accept_error_cb);
}
-void BinaryServer::Notify(int fd,short /*what*/){
+void BinaryServer::Notify(int fd, short /*what*/) {
char notes[4096];
- int count = read(fd,notes,4096);
- if(count == 0){
+ int count = read(fd, notes, 4096);
+ if (count == 0) {
close(fd);
- fprintf(stderr,"%s:%d: Error No cheeze from OpenCV task!!!\n",__FILE__,__LINE__);
+ fprintf(stderr, "%s:%d: Error No cheeze from OpenCV task!!!\n", __FILE__,
+ __LINE__);
exit(-1);
}
- printf("notified!: %d\n",count);
- if(have_id){
+ printf("notified!: %d\n", count);
+ if (have_id) {
printf("got someone to read my stuff!\n");
char *binary_data;
size_t len;
- if(_notify->GetData(&binary_data,&len)){
+ if (_notify->GetData(&binary_data, &len)) {
printf("here is for sending\n");
- evbuffer_add_reference(_output, binary_data, len, PacketNotifier::StaticDataSent,_notify);
- printf("this is how sending went %d\n",bufferevent_flush(_bufev,EV_WRITE, BEV_FLUSH));
+ evbuffer_add_reference(_output, binary_data, len,
+ PacketNotifier::StaticDataSent, _notify);
+ printf("this is how sending went %d\n",
+ bufferevent_flush(_bufev, EV_WRITE, BEV_FLUSH));
}
}
}
-//Constructor
-BinaryServer::BinaryServer(uint16_t port,
- frc971::vision::PacketNotifier *notify) :
- _base(event_base_new()) {
- have_id = false;
- StartServer(port);
- _notify = notify;
- frame_notify = event_new(_base, notify->RecieverFD(),
- EV_READ|EV_PERSIST, StaticNotify, this);
- event_add(frame_notify,NULL);
- event_base_dispatch(_base);
- }
+// Constructor
+BinaryServer::BinaryServer(uint16_t port,
+ frc971::vision::PacketNotifier *notify)
+ : _base(event_base_new()) {
+ have_id = false;
+ StartServer(port);
+ _notify = notify;
+ frame_notify = event_new(_base, notify->RecieverFD(), EV_READ | EV_PERSIST,
+ StaticNotify, this);
+ event_add(frame_notify, NULL);
+ event_base_dispatch(_base);
+}
} // namespace vision
} // namespace frc971
diff --git a/vision/CameraProcessor.cpp b/vision/CameraProcessor.cpp
index 56bd36e..856e12b 100644
--- a/vision/CameraProcessor.cpp
+++ b/vision/CameraProcessor.cpp
@@ -459,7 +459,7 @@
raw_contour, rect, check, is_90));
}
if (contour.size() == 4 && cullObvious(rect, contourArea(contour))) {
- LOG(INFO, "check= %.2f\n", check);
+ LOG(DEBUG, "check= %.2f\n", check);
}
}
}
diff --git a/vision/GoalMaster.cpp b/vision/GoalMaster.cpp
index 3155cf7..5c8fad8 100644
--- a/vision/GoalMaster.cpp
+++ b/vision/GoalMaster.cpp
@@ -46,7 +46,8 @@
kMetersToShooterAngles,
meters);
- LOG(DEBUG, "think target is %f meters away Speed %f Angle %f\n",
+ LOG(DEBUG, "%+f=> think target is %f meters away Speed %f Angle %f\n",
+ targets->percent_elevation_off_center,
meters, shooter_speed, shooter_angle);
target_angle.MakeWithBuilder()
diff --git a/vision/OpenCVWorkTask.cpp b/vision/OpenCVWorkTask.cpp
index 9d68065..f17803e 100644
--- a/vision/OpenCVWorkTask.cpp
+++ b/vision/OpenCVWorkTask.cpp
@@ -4,11 +4,13 @@
#include <sys/mman.h>
#include <errno.h>
#include <string.h>
-#include <vector>
#include <netinet/in.h>
#include <netinet/tcp.h>
#include <arpa/inet.h>
+#include <vector>
+#include <iostream>
+
#include "aos/common/time.h"
#include "aos/atom_code/camera/Buffers.h"
#include "aos/externals/libjpeg/include/jpeglib.h"
@@ -17,8 +19,6 @@
#include "vision/OpenCVWorkTask.h"
#include "vision/CameraProcessor.h"
-#include "vision/BinaryServer.h"
-#include "vision/PacketNotifier.h"
#include "vision/JPEGRoutines.h"
@@ -28,10 +28,6 @@
} // namespace vision
} // namespace frc971
-void reciever_main(frc971::vision::PacketNotifier *notify){
- frc971::vision::BinaryServer test(8020,notify);
-}
-
namespace {
void SaveImageToFile(IplImage *image, const char *filename) {
FILE *file = fopen(filename, "w");
@@ -56,13 +52,13 @@
#include "frc971/queues/CameraTarget.q.h"
using frc971::vision::targets;
-#include <iostream>
-void sender_main(frc971::vision::PacketNotifier *notify){
- ::aos::InitNRT();
+void sender_main(){
::aos::camera::Buffers buffers;
CvSize size;
size.width = ::aos::camera::Buffers::Buffers::kWidth;
size.height = ::aos::camera::Buffers::Buffers::kHeight;
+ unsigned char buffer[::aos::camera::Buffers::Buffers::kWidth *
+ ::aos::camera::Buffers::Buffers::kHeight * 3];
// create processing object
ProcessorData processor(size.width, size.height, false);
@@ -73,17 +69,15 @@
//usleep(7500);
size_t data_size;
timeval timestamp_timeval;
+ LOG(DEBUG, "getting new image\n");
const void *image = buffers.GetNext(
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);
- LOG(INFO, "Got new image\n");
- unsigned char *buffer = (unsigned char *)notify->GetBuffer();
- frc971::vision::process_jpeg(buffer,
- (unsigned char *)const_cast<void *>(image),
- data_size);
+ LOG(DEBUG, "Got new image\n");
+ frc971::vision::process_jpeg(
+ buffer, static_cast<unsigned char *>(const_cast<void *>(image)),
+ data_size);
// build the headers on top of the buffer
cvSetData(processor.src_header_image,
@@ -104,7 +98,7 @@
std::vector<Target>::iterator target_it;
Target *best_target = NULL;
// run through the targets
- LOG(INFO, "Found %u targets\n", processor.target_list.size());
+ LOG(DEBUG, "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));
@@ -119,7 +113,7 @@
}
// if we found one then send it on
if (best_target != NULL) {
- LOG(INFO, "Target is %f\n", best_target->rect.centroid.x);
+ LOG(DEBUG, "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)
@@ -128,29 +122,17 @@
.timestamp(timestamp.ToNSec())
.Send();
}
- notify->Notify();
//static int counter = 0;
//if (++counter > 2) {
//break;
//}
}
- ::aos::Cleanup();
}
int main(int /*argc*/, char** /*argv*/){
- frc971::vision::PacketNotifier *notify = frc971::vision::PacketNotifier::MMap(
- ::aos::camera::Buffers::kHeight * ::aos::camera::Buffers::kWidth * 3);
- pid_t pid = fork();
- if(pid < 0){
- fprintf(stderr,"%s:%d: Error in fork()\n",__FILE__,__LINE__);
- }
- if(pid == 0){
- notify->RegisterSender();
- sender_main(notify);
- }else{
- notify->RegisterReciever();
- reciever_main(notify);
- }
+ ::aos::InitNRT();
+ sender_main();
+ ::aos::Cleanup();
}
diff --git a/vision/SensorProcessor.h b/vision/SensorProcessor.h
index c219245..daf24be 100644
--- a/vision/SensorProcessor.h
+++ b/vision/SensorProcessor.h
@@ -10,27 +10,31 @@
} Interpolation;
static const Interpolation kPixelsToMeters[] = {
- {43.0 / 320.0, 12.573},
- {98.0 / 320.0, 6.604},
- {145.75 / 320.0, 4.420},
- {216.75 / 320.0, 2.794},
+ {-0.050781, 4.7498},
+ {-0.0375, 4.318},
+ {0.028125, 3.9878},
+ {0.080469, 3.51},
+ {0.126563, 3.1496},
+ {0.131, 2.9972},
+ {0.144, 2.921},
+ {0.196, 3.2258},
+ // Below here is junk because it starts coming off of the tower base.
+ {0.296875, 2.667},
+ {0.351562, 2.3876},
};
// Must be in reverse order in meters.
static const Interpolation kMetersToShooterSpeeds[] = {
- {12.5, 375.0},
- {21.0, 360.0},
- {25.0, 375.0},
+ {2.0, 375.0},
+ {3.0, 360.0},
+ {4.5, 375.0},
};
static const Interpolation kMetersToShooterAngles[] = {
- {0.0, 0.7267},
- {12.5, 0.7267},
- {16.5, 0.604},
- {18.0, 0.587},
- {20.0, 0.576},
- {21.0, 0.550},
- {23.0, 0.540},
+ {3.0, 0.68},
+ {3.7, 0.635},
+ {4.15, 0.58},
+ {5.0, 0.51},
};
double interpolate(int num_interp_vals,
diff --git a/vision/vision.gyp b/vision/vision.gyp
index 8fe50a4..df27045 100644
--- a/vision/vision.gyp
+++ b/vision/vision.gyp
@@ -6,8 +6,8 @@
'sources': [
'OpenCVWorkTask.cpp',
'CameraProcessor.cpp',
- 'BinaryServer.cpp',
- 'PacketNotifier.cpp',
+ #'BinaryServer.cpp',
+ #'PacketNotifier.cpp',
'JPEGRoutines.cpp',
],
'dependencies': [