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;
}
