unsigned long RADAR::ping(int pan, int tilt)
{
setPan(pan);
setTilt(tilt);
while (!ready());
return ping();
}
void PTUInterface::parseResponse(QByteArray barr){
if(barr.size() < 1)
return;
int n;
double m;
switch(barr[0]){
case 8: // change level word
if(barr.size() < 6)
break;
n = barr.at(3) * 256 + barr.at(4);
if(barr.at(2) == 5){ // pan
m = 1.0 + n * 358.0 / 65535.0; // remap to [1, 359] from [0, 65535]
setPan(m);
} else if(barr.at(2) == 6){ // tilt
m = 90.0 + n * 180.0 / 65535.0; // remap to [90, 270] from [0, 65535]
setTilt(m);
}
break;
case 2: // change level byte
if(barr.size() < 5)
break;
n = barr.at(3);
if(barr.at(2) == 0){ // pan
m = 1.0 + n * 358.0 / 256.0; // remap to [1, 359] from [0, 256]
setPan(m);
} else if(barr.at(2) == 1){ // tilt
m = 90.0 + n * 180.0 / 256.0; // remap to [90, 270] from [0, 256]
setTilt(m);
}
break;
default:
// do nothing, for now
break;
}
}
void home()
{
setPan(_homePan);
setTilt(_homeTilt);
}
//--------------------------------------------------------------
void testApp::draw() {
ofSetColor(255, 255, 255);
// draw from the live kinect
//kinect.drawDepth(10, 10, 400, 300);
//kinect.draw(420, 10, 400, 300);
grayImage.draw(10, 10);
ofNoFill();
for(int i = 0; i < contourFinder.nBlobs; i++) {
ofxCvBlob* blob;
blob = &contourFinder.blobs.at(i);
ofRectangle r = blob->boundingRect;
r.x += 10; r.y += 10;
ofSetLineWidth(1);
ofSetColor(200);
if (blob == trackedBlob) {
ofSetLineWidth(3);
ofSetColor(255);
}
ofRect(r);
}
if (trackedBlob) {
int x = ofMap(trackedBlob->centroid.x - 70, 0, 600, 150, 0);
int y = 5;
int z = ofMap(trackedBlob->centroid.y, 250, 400, -30, 12);
//int z = 20;
//cout << trackedBlob->centroid.y << endl;
/*
int x = mouseX- 320;
int y = mouseY - 240;
int z = 400;
*/
/*
int x = mouseX - 320;
int y = 0;
int z = mouseY-300;*/
if (x != 0) {
ofVec3f vec = ofVec3f(x,y,z);
ofVec3f lamp = ofVec3f(0,-200,0);
vec = vec-lamp;
// vec.normalize();
float panAngle = atan2(vec.z, vec.x);
float tiltAngle = asin(vec.y/vec.length());
// cout<<tiltAngle<<endl;
tiltAngle = HALF_PI - (tiltAngle - HALF_PI);
panAngle += PI;
//cout<<panAngle<<endl;
//panAngle += PI;
ofPushMatrix();
ofTranslate(50, 50);
ofRotate(panAngle * RAD_TO_DEG, 0, 0, 1);
ofRect(0, 0, 100, 10);
ofPopMatrix();
ofPushMatrix();
ofTranslate(50, 150);
ofRotate(tiltAngle * RAD_TO_DEG, 0, 0, 1);
ofRect(0, 0, 100, 10);
ofPopMatrix();
pan = ofMap(panAngle, 0, TWO_PI, 0, 255);
tilt = ofMap(tiltAngle, 0, PI, 30, 255-30);
//printf("%i, %i pan: %f tilt: %f\n", pan, tilt, atan2(z, x),acos(y/vec.length()));
/*vec_set(temp, your.x)
vec_sub(temp, my.x)
vec_to_angle(my.pan, temp)*/
/*
pan = ofMap(mouseX, 0, ofGetWidth(), 0, 255);
tilt = ofMap(mouseY, 0, ofGetHeight(), 0, 255);
cout<<tilt<<endl;*/
//tilt = ofMap
//pan = 63;
//tilt = 255;
setPan();
setTilt();
}
}
//contourFinder.draw(10, 320, 400, 300);
#ifdef USE_TWO_KINECTS
kinect2.draw(420, 320, 400, 300);
#endif
// draw instructions
stringstream reportStream;
ofSetColor(255, 255, 255);
reportStream << "Pan:" << pan << ", tilt: " << tilt << endl;
if (trackedBlob) {
reportStream << "Tracking blob at x: " << trackedBlob->centroid.x << " y: " << trackedBlob->centroid.y << "accel is: " << ofToString(kinect.getMksAccel().x, 2) << " / "
<< ofToString(kinect.getMksAccel().y, 2) << " / "
<< ofToString(kinect.getMksAccel().z, 2) << endl
<< "using opencv threshold = " << bThreshWithOpenCV <<" (press spacebar)" << endl
<< "set near threshold " << nearThreshold << " (press: + -)" << endl
<< "set far threshold " << farThreshold << " (press: < >) num blobs found " << contourFinder.nBlobs
<< ", fps: " << ofGetFrameRate() << endl
<< "press c to close the connection and o to open it again, connection is: " << kinect.isConnected() << endl
<< "press UP and DOWN to change the tilt angle: " << angle << " degrees" << endl;
}
ofDrawBitmapString(reportStream.str(),20,652);
}
// prime_server stuff
worker_t::result_t Paparazzi::work (const std::list<zmq::message_t>& job, void* request_info){
//false means this is going back to the client, there is no next stage of the pipeline
worker_t::result_t result{false};
//this type differs per protocol hence the void* fun
auto& info = *static_cast<http_request_t::info_t*>(request_info);
// Try to generate a response
http_response_t response;
try {
// double start_call = getTime();
//TODO:
// - actually use/validate the request parameters
auto request = http_request_t::from_string(static_cast<const char*>(job.front().data()), job.front().size());
if (request.path == "/check") {
// ELB check
response = http_response_t(200, "OK", "OK", headers_t{CORS, TXT_MIME});
} else {
// SCENE
// ---------------------
auto scene_itr = request.query.find("scene");
if (scene_itr == request.query.cend() || scene_itr->second.size() == 0) {
// If there is NO SCENE QUERY value
if (request.body.empty())
// if there is not POST body content return error...
throw std::runtime_error("scene is required punk");
// ... other whise load content
setSceneContent(request.body);
// The size of the custom scene is unique enough
result.heart_beat = std::to_string(request.body.size());
}
else {
// If there IS a SCENE QUERRY value load it
setScene(scene_itr->second.front());
result.heart_beat = scene_itr->second.front();
}
bool size_and_pos = true;
float pixel_density = 1.0f;
// SIZE
// ---------------------
auto width_itr = request.query.find("width");
if (width_itr == request.query.cend() || width_itr->second.size() == 0)
size_and_pos = false;
auto height_itr = request.query.find("height");
if (height_itr == request.query.cend() || height_itr->second.size() == 0)
size_and_pos = false;
auto density_itr = request.query.find("density");
if (density_itr != request.query.cend() && density_itr->second.size() > 0)
pixel_density = fmax(1.,std::stof(density_itr->second.front()));
// POSITION
// ---------------------
auto lat_itr = request.query.find("lat");
if (lat_itr == request.query.cend() || lat_itr->second.size() == 0)
size_and_pos = false;
auto lon_itr = request.query.find("lon");
if (lon_itr == request.query.cend() || lon_itr->second.size() == 0)
size_and_pos = false;
auto zoom_itr = request.query.find("zoom");
if (zoom_itr == request.query.cend() || zoom_itr->second.size() == 0)
size_and_pos = false;
if (size_and_pos) {
// Set Map and OpenGL context size
setSize(std::stoi(width_itr->second.front()), std::stoi(height_itr->second.front()), pixel_density);
setPosition(std::stod(lon_itr->second.front()), std::stod(lat_itr->second.front()));
setZoom(std::stof(zoom_itr->second.front()));
} else {
const std::regex re("\\/(\\d*)\\/(\\d*)\\/(\\d*)\\.png");
std::smatch match;
if (std::regex_search(request.path, match, re) && match.size() == 4) {
setSize(256,256, pixel_density);
int tile_coord[3] = {0,0,0};
for (int i = 0; i < 3; i++) {
std::istringstream cur(match.str(i+1));
cur >> tile_coord[i];
}
futile_coord_s tile;
tile.z = tile_coord[0];
setZoom(tile.z);
tile.x = tile_coord[1];
tile.y = tile_coord[2];
futile_bounds_s bounds;
futile_coord_to_bounds(&tile, &bounds);
setPosition(bounds.minx + (bounds.maxx-bounds.minx)*0.5,bounds.miny + (bounds.maxy-bounds.miny)*0.5);
}
else {
throw std::runtime_error("not enought data to construct image");
}
}
// OPTIONAL tilt and rotation
// ---------------------
auto tilt_itr = request.query.find("tilt");
if (tilt_itr != request.query.cend() && tilt_itr->second.size() != 0) {
// If TILT QUERRY is provided assigned ...
setTilt(std::stof(tilt_itr->second.front()));
}
else {
// othewise use default (0.)
setTilt(0.0f);
}
auto rotation_itr = request.query.find("rotation");
if (rotation_itr != request.query.cend() && rotation_itr->second.size() != 0) {
// If ROTATION QUERRY is provided assigned ...
setRotation(std::stof(rotation_itr->second.front()));
}
else {
// othewise use default (0.)
setRotation(0.0f);
}
// Time to render
// ---------------------
std::string image;
if (m_map) {
update();
// Render Tangram Scene
m_aab->bind();
m_map->render();
m_aab->unbind();
// Once the main FBO is draw take a picture
m_aab->getPixelsAsString(image);
// double total_time = getTime()-start_call;
// LOG("TOTAL CALL: %f", total_time);
// LOG("TOTAL speed: %f millisec per pixel", (total_time/((m_width * m_height)/1000.0)));
}
response = http_response_t(200, "OK", image, headers_t{CORS, PNG_MIME});
}
