void htmInterface::getDepth() { size_t depth = getInteger(); if(depth > HTMMAXDEPTH) throw SpatialInterfaceError("htmInterface:getDepth: Depth too large: Max is HTMMAXDEPTH"); changeDepth(depth); }
void BuoyTask::execute() { HSVFilter green(std::stoi(settings->getProperty("g1")), std::stoi(settings->getProperty("g2")), std::stoi(settings->getProperty("g3")), std::stoi(settings->getProperty("g4")), std::stoi(settings->getProperty("g5")), std::stoi(settings->getProperty("g6"))); HSVFilter red(std::stoi(settings->getProperty("r1")), std::stoi(settings->getProperty("r2")), std::stoi(settings->getProperty("r3")), std::stoi(settings->getProperty("r4")), std::stoi(settings->getProperty("r5")), std::stoi(settings->getProperty("r6"))); //only look for 1 circle ShapeFilter sf (2, 1); //assuming the sub is in the correct position //first look for red, and then hit it //then look and hit green bool hitGreen = false; bool hitRed = false; int retreat = 0; int moveDist = std::stoi(settings->getProperty("moveDist")); int rotateAng = std::stoi(settings->getProperty("rotateAng")); int movementDelay = std::stoi(settings->getProperty("movementDelay")); int deltaDist = 0; float deltaT = 0; bool done = false; while (!done) { std::string s = "raw"; ImgData* data = dynamic_cast<ImgData*> (dynamic_cast<CameraState*> (camModel->getState())->getDeepState(s)); imgWidth = data->getImg().size().width; imgHeight = data->getImg().size().height; // cv::imshow("hsv",data->getImg()); //filter for a color depending if the other color is hit or not if (!hitRed){ println("Filtering red"); red.filter(data); } else if (!hitGreen){ //println("Filtering green"); //green.filter(data); done = true; continue; } else if (hitGreen && hitRed) { done = true; println("Done task"); continue; } //after hitting a color, move the sub back to look for the other one //TODO: CALIBRATE THIS STEP if (sf.filter(data)){ retreat = false; cv::Point2f cent = sf.getCenter()[0]; if (std::abs(cent.x - imgWidth/2) < imgWidth/20){ //in the middle 20% of the screen horizontally if (std::abs(cent.y - imgHeight / 2) < imgHeight / 20) { //in the middle 20% vertically float d = calcDistance(sf.getRad()[0]) * 1.2; float t = d/moveSpeed; println("Moving " + std::to_string(d) + "cm in " + std::to_string(t) + "s"); move(moveSpeed); usleep(std::stoi(settings->getProperty("MOVE_TIME"))); if (!hitRed){ hitRed = true; println("Hit red"); retreat = true; } else { hitGreen = true; println("Hit green"); retreat = true; } logger->info("Stopping"); move(0); /* //if the radius of the circle is huge, so the sub will hit it if (sf.getRad()[0] > imgWidth*imgHeight/3){ if (!hitGreen){ hitGreen = true; println("Hit green"); }else{ hitRed = true; println("Hit red"); } //return the sub back to its orginal position move(0); retreat = true; println("Retreat enabled"); }else{ //move straight and hit it float dist = calcDistance(sf.getRad()[0]); deltaDist = dist*1.2; println("Moving forward " + std::to_string(deltaDist) + "cm to hit buoy"); move(deltaDist); }*/ } else { float deltaY; if (cent.y > imgHeight/2) { deltaY = -0.1; //rise a bit } else { deltaY = 0.1; //sink a bit } //float deltaY = (cent.y - imgHeight/2)/sf.getRad()[0]; println("Rising " + std::to_string(deltaY*100)+"cm"); changeDepth(deltaY); } } else { println("Center: " + std::to_string(cent.x) + " " + std::to_string(imgWidth/2)); //float ang = atan2(cent.x-imgWidth/2, 0) * 180 / M_PI; //float dX = cent.x-imgWidth/2; //dX * 23/sf.getRad()[0]; //float ang = atan(dX/calcDistance(sf.getRad()[0])) * 180 / M_PI; //println("Rotating " + std::to_string(ang) + " degrees"); //rotate(ang); float dir = cent.x-imgWidth/2; println("Rotating 10 degrees " + std::to_string(dir)); dir /= std::abs(dir); rotate (5*dir); /* float scale = 23/sf.getRad()[0]; float dist = sf.getCenter()[0].x - imgWidth/2; //slide 2/3 of the way float deltaX = dist*scale/3*2; println("Sliding " + std::to_string(deltaX)+"cm"); slide(deltaX);*/ } } else { ///CIRCLES NOT FOUND ///ROTATE/MOVE SUB //rotate(rotateAng); println("Circle not found, moving forward"); //move(moveDist); if (retreat) { if (moveWithSpeed){ println("Retreating"); move(-moveSpeed); usleep(40000000); //usleep(deltaT * 500); move(0); rotate(-deltaAngle); usleep(5000000); } else { println("Retreating " + std::to_string(-deltaDist - 20) + "cm"); move(-deltaDist - 20); //move 20cm more than i needed //sleep(movementDelay); //move the sub back to the middle if (deltaAngle != -1){ println("Retreat rotating " + std::to_string(deltaAngle) + " degrees"); rotate(deltaAngle); println("Retreat to middle by " + std::to_string(deltaDist-20) + "cm"); move (deltaDist-20); //move 20cm less than i need rotate(-deltaAngle); } } retreat = false; // continue; } else { ///tries to look for any colors and move towards it logger->info("Looking for coloured masses to turn towards"); std::vector<cv::Point2f> mc = sf.findMassCenter(data); if (mc.size() > 0) { logger->info("Found coloured mass"); float dir = mc[0].x - imgWidth/2; dir /= std::abs(dir); rotate(5 * dir); } else { ///if it dosnt see any colors, move forwards logger->debug("No coloured masses found. Moving forward"); move(moveSpeed); } } } delete data; data = 0; usleep(33000); } }
int sageDisplayManager::parseMessage(sageMessage *msg) { if (!msg) { sage::printLog("sageDisplayManager::parseMessage : message is NULL"); return -1; } switch (msg->getCode()) { case RCV_INIT : { //std::cout << "rcv init : " << (char *)msg->getData() << std::endl; if (init((char *)msg->getData()) < 0) rcvEnd = true; break; } case RCV_UPDATE_DISPLAY : { updateDisplay((char *)msg->getData()); break; } case RCV_CLEAR_DISPLAY : { clearDisplay(atoi((char *)msg->getData())); break; } case SHUTDOWN_RECEIVERS : { shutdownApp(-1); //std::cout << "all apps are shutdown" << std::endl; //sage::sleep(1); rcvEnd = true; //delete shared->displayObj; //_exit(0); break; } case RCV_CHANGE_BGCOLOR : { changeBGColor(msg); break; } case RCV_CHANGE_DEPTH : { changeDepth(msg); break; } case RCV_SHUTDOWN_APP : { shutdownApp(atoi((char *)msg->getData())); break; } case RCV_PERF_INFO_REQ : { startPerformanceReport(msg); break; } case RCV_PERF_INFO_STOP : { stopPerformanceReport(msg); break; } case ADD_OBJECT : { shared->displayObj->addDrawObjectInstance((char *)msg->getData()); break; } case UPDATE_OBJECT_POSITION : { shared->displayObj->updateObjectPosition((char *)msg->getData()); break; } case REMOVE_OBJECT : { shared->displayObj->removeDrawObject((char *)msg->getData()); break; } case OBJECT_MESSAGE : { shared->displayObj->forwardObjectMessage((char *)msg->getData()); break; } case SHOW_OBJECT : { shared->displayObj->showObject((char *)msg->getData()); break; } } msg->destroy(); delete msg; return 0; }