void pix_opencv_patreco :: processGrayImage(imageStruct &image)
{
Mat imgMat( image.ysize, image.xsize, CV_8UC1, image.data, image.csize*image.xsize);
m_detectedPattern.clear();
m_detector->detect(imgMat, m_cameraMatrix, m_distortions, m_patternLibrary, m_detectedPattern);
//~ std::cout << "Found : " << m_detectedPattern.size() << " pattern(s)" << endl;
t_atom pattern[9];
//~ t_atom pattern_list[512]; // TODO set to smthg like PD_MESS_MAX_SIZE
//~ for ( int i = 0 ; i < m_patternLibrary.size() ; i++ ){
//~ SETFLOAT(&pattern_list[i], 0.);
//~ }
for (unsigned int i = 0 ; i < m_detectedPattern.size() ; i++ ){
SETFLOAT(&pattern[0], m_detectedPattern.at(i).id);
//~ SETFLOAT(&pattern_list[m_detectedPattern.at(i).id - 1], 1);
for ( int j = 0 ; j < 4 ; j++ ) {
SETFLOAT(&pattern[j*2+1], float(m_detectedPattern.at(i).vertices.at(j).x/image.xsize));
SETFLOAT(&pattern[j*2+2], float(m_detectedPattern.at(i).vertices.at(j).y/image.ysize));
}
outlet_anything( m_dataout, gensym("pattern_pos"), 9, pattern);
}
//~ if ( m_patternLibrary.size() > 0 )
//~ outlet_anything( m_dataout, gensym("pattern_list"), m_patternLibrary.size(), pattern_list);
}
int main(int argc, char **argv)
{
if(argc != 6) {
std::cout << "wrong args" << std::endl;
return 1;
}
const char *sensorDeviceName = argv[1];
const char *actuatorDeviceName = argv[2];
const int cameraDeviceNum = atoi(argv[3]);
const int port = atoi(argv[4]);
const char *mode = argv[5];
bool showThermal = false;
bool showRGB = false;
if(mode[0] == 't') {
showThermal = true;
} else if(mode[0] == 'r') {
showRGB = true;
} else {
throw "wtf";
}
std::cout << "blah" << std::endl;
std::shared_ptr<ApplicationCore> core = ApplicationCore::instantiate();
auto sc = std::make_shared<ThermalSensorController>(core, sensorDeviceName, 115200);
auto rc = std::make_shared<RgbController>(core, cameraDeviceNum);
auto ac = std::make_shared<ActuatorController>("/dev/tty.usbserial-A9S3VTXD");
auto ns = std::make_shared<NetService>(core);
sc->init();
rc->init();
ac->init();
ns->init(port);
boost::asio::deadline_timer timer(*core->getIOService());
std::function<void(const boost::system::error_code&)> captureStuff;
GyroReading gyroReading;
ThermoReading thermoReading;
captureStuff = [&](const boost::system::error_code& /*e*/) {
//
cv::Vec2d pos = ac->getCurrentPosition();
rc->captureFrame();
auto rgbFrame = rc->popFrame();
if(showRGB && rgbFrame->rows > 0) {
rapidjson::Document doc;
auto &aloc = doc.GetAllocator();
doc.SetObject();
cv::Mat imgMat(rgbFrame->rows, rgbFrame->cols, CV_8UC4, cv::Scalar::all(0.0));
cv::cvtColor(*rgbFrame, imgMat, CV_BGR2RGBA, 4);
cv::Size size(rgbFrame->cols*0.2, rgbFrame->rows*0.2);
cv::resize(imgMat, imgMat, size);
std::string imgDataB64 = tobase64(imgMat.data, imgMat.total()*4*sizeof(byte));
rapidjson::Value val;
val.SetString(imgDataB64.c_str(), doc.GetAllocator());
doc.AddMember("data", val, aloc);
doc.AddMember("type", "rgb_data", aloc);
doc.AddMember("yaw", -pos[0], aloc);
doc.AddMember("pitch", -pos[1], aloc);
doc.AddMember("dataWidth", imgMat.cols, aloc);
doc.AddMember("dataHeight", imgMat.rows, aloc);
doc.AddMember("yawSize", 63.625, aloc);
doc.AddMember("pitchSize", 35.789, aloc);
ns->sendWSDoc(doc);
}
/*if(sc->popGyroReading(gyroReading)) {
printf("Roll: %f, Pitch: %f, Yaw: %f.\n",
gyroReading.roll, gyroReading.pitch, gyroReading.yaw
);
}*/
sc->requestThermoReading();
std::cout << "tick: " << timer.expires_at() << std::endl;
if(showThermal && sc->popThermoReading(thermoReading)){
rapidjson::Document doc;
auto &aloc = doc.GetAllocator();
doc.SetObject();
doc.AddMember("type", "thermo_data", aloc);
doc.AddMember("yaw", -pos[0], aloc);
doc.AddMember("pitch", -pos[1], aloc);
cv::Mat imgMat(4, 16, CV_8UC4, cv::Scalar::all(0.0));
cv::Mat mat = thermoReading.img;
for(int i = 0; i < mat.total(); i++) {
int y = 3-(i%4);
int x = i/4;
double temp = mat.at<float>(0, i);
if(
(x == 11 && y == 2)
|| (x == 11 && y == 3)
|| (x == 12 && y == 2)
) {
temp += 10.0;
}
//std::cout << (int)temp << " ";
cv::Vec4b col = hsv(
300-300.0*(std::max(temp, 14.0)-14.0)/(40.0-14.0),
1, 1
);
if(temp <= 11.0) {
col = cv::Vec4b(30, 30, 50, 255);
} else if(temp > 40.0) {
col = cv::Vec4b(255, 255, 255, 255);
}
imgMat.at<cv::Vec4b>(y, x) = col;
//std::cout << std::endl;
}
std::string imgDataB64 = tobase64(imgMat.data, imgMat.total()*4*sizeof(byte));
rapidjson::Value val;
val.SetString(imgDataB64.c_str(), doc.GetAllocator());
doc.AddMember("data", val, aloc);
ns->sendWSDoc(doc);
}
timer.expires_from_now(boost::posix_time::milliseconds(interval));
timer.async_wait(captureStuff);
};
timer.expires_from_now(boost::posix_time::milliseconds(interval));
timer.async_wait(captureStuff);
ns->registerCallback("move_actuator", [&](const rapidjson::Document &doc) {
ac->stop();
ActuatorMoveOrder order;
order.posDeg = cv::Vec2d(
std::max(-150.0, std::min(150.0, -doc["yaw" ].GetDouble()/M_PI*180)),
std::max(- 90.0, std::min( 90.0, -doc["pitch"].GetDouble()/M_PI*180))
);
order.duration = 3.5;
ac->queueMove(order);
});
std::cout << "run" << std::endl;
core->run();
}
