void Data2StringStream::toStringStream(const Route &r) {
m_sstr << r.toString();
}
void PortCoreInputUnit::run() {
running = true;
phase.post();
Route route;
bool wasNoticed = false;
bool posted = false;
bool done = false;
yAssert(ip!=nullptr);
PortCommand cmd;
bool ok = true;
if (!reversed) {
ip->open(getName().c_str());
}
if (!ok) {
YARP_DEBUG(Logger::get(), std::string("new input connection to ")+
getOwner().getName()+ " is broken");
done = true;
} else {
route = ip->getRoute();
// just before going official, tag any lurking inputs from
// the same source as undesired
if (Name(route.getFromName()).isRooted()) {
YARP_SPRINTF3(Logger::get(),
debug,
"Port %s starting up, flushing routes %s->*->%s",
getOwner().getName().c_str(),
route.getFromName().c_str(),
route.getToName().c_str());
getOwner().removeIO(Route(route.getFromName(),
route.getToName(), "*"), true);
}
officialRoute = route;
setMode();
getOwner().reportUnit(this, true);
std::string msg = std::string("Receiving input from ") +
route.getFromName() + " to " + route.getToName() +
" using " +
route.getCarrierName();
if (Name(route.getFromName()).isRooted()) {
if (reversed||ip->getConnection().isPush()) {
YARP_INFO(Logger::get(), msg);
posted = true;
} else {
YARP_DEBUG(Logger::get(), msg);
}
} else {
YARP_DEBUG(Logger::get(), msg);
}
// Report the new connection
PortInfo info;
info.message = msg;
info.tag = yarp::os::PortInfo::PORTINFO_CONNECTION;
info.incoming = true;
info.created = true;
info.sourceName = route.getFromName().c_str();
info.targetName = route.getToName().c_str();
info.portName = info.targetName;
info.carrierName = route.getCarrierName().c_str();
if (info.sourceName!="admin"&&info.sourceName!="null") {
getOwner().report(info);
wasNoticed = true;
}
}
if (!reversed) {
if (!ip->getConnection().isPush()) {
/* IP=OP */
OutputProtocol *op = &(ip->getOutput());
Route r = op->getRoute();
// reverse route
r.swapNames();
op->rename(r);
getOwner().addOutput(op);
ip = nullptr;
done = true;
}
}
if (closing) {
done = true;
}
void *id = (void *)this;
if (ip!=nullptr && !ip->getConnection().canEscape()) {
InputStream *is = &ip->getInputStream();
is->setReadEnvelopeCallback(envelopeReadCallback, this);
}
while (!done) {
if(!ip) break;
ConnectionReader& br = ip->beginRead();
if (br.getReference()!=nullptr) {
//printf("HAVE A REFERENCE\n");
if (localReader!=nullptr) {
bool ok = localReader->read(br);
if (!br.isActive()) { break; }
if (!ok) continue;
} else {
PortManager& man = getOwner();
bool ok = man.readBlock(br, id, nullptr);
if (!br.isActive()) { break; }
if (!ok) continue;
}
//printf("DONE WITH A REFERENCE\n");
if (ip!=nullptr) {
ip->endRead();
}
continue;
}
if (ip->getConnection().canEscape()) {
bool ok = cmd.read(br);
if (!br.isActive()) { break; }
if (!ok) continue;
} else {
cmd = PortCommand('d', "");
if (!ip->isOk()) { break; }
}
if (closing||isDoomed()) {
break;
}
char key = cmd.getKey();
//printf("Port command is [%c:%d/%s]\n",
// (key>=32)?key:'?', key, cmd.getText().c_str());
PortManager& man = getOwner();
OutputStream *os = nullptr;
if (br.isTextMode()) {
os = &(ip->getOutputStream());
}
switch (key) {
case '/':
YARP_SPRINTF3(Logger::get(),
debug,
"Port command (%s): %s should add connection: %s",
route.toString().c_str(),
getOwner().getName().c_str(),
cmd.getText().c_str());
man.addOutput(cmd.getText(), id, os);
break;
case '!':
YARP_SPRINTF3(Logger::get(),
debug,
"Port command (%s): %s should remove output: %s",
route.toString().c_str(),
getOwner().getName().c_str(),
cmd.getText().c_str());
man.removeOutput(cmd.getText().substr(1, std::string::npos), id, os);
break;
case '~':
YARP_SPRINTF3(Logger::get(),
debug,
"Port command (%s): %s should remove input: %s",
route.toString().c_str(),
getOwner().getName().c_str(),
cmd.getText().c_str());
man.removeInput(cmd.getText().substr(1, std::string::npos), id, os);
break;
case '*':
man.describe(id, os);
break;
case 'D':
case 'd':
{
if (key=='D') {
ip->suppressReply();
}
std::string env = cmd.getText();
if (env.length()>2) {
//YARP_ERROR(Logger::get(),
//"***** received an envelope! [%s]", env.c_str());
std::string env2 = env.substr(2, env.length());
man.setEnvelope(env2);
ip->setEnvelope(env2);
}
if (localReader) {
localReader->read(br);
if (!br.isActive()) { done = true; break; }
} else {
if (ip->getReceiver().acceptIncomingData(br)) {
ConnectionReader* cr = &(ip->getReceiver().modifyIncomingData(br));
yarp::os::impl::PortDataModifier& modifier = getOwner().getPortModifier();
modifier.inputMutex.lock();
if (modifier.inputModifier) {
if (modifier.inputModifier->acceptIncomingData(*cr)) {
cr = &(modifier.inputModifier->modifyIncomingData(*cr));
modifier.inputMutex.unlock();
man.readBlock(*cr, id, os);
}
else {
modifier.inputMutex.unlock();
skipIncomingData(*cr);
}
}
else {
modifier.inputMutex.unlock();
man.readBlock(*cr, id, os);
}
}
else
skipIncomingData(br);
if (!br.isActive()) { done = true; break; }
}
}
break;
case 'a':
{
man.adminBlock(br, id, os);
}
break;
case 'r':
/*
In YARP implementation, OP=IP.
(This information is used rarely, and when used
is tagged with OP=IP keyword)
If it were not true, memory alloc would need to
reorganized here
*/
{
OutputProtocol *op = &(ip->getOutput());
ip->endRead();
Route r = op->getRoute();
// reverse route
r.swapNames();
op->rename(r);
getOwner().addOutput(op);
ip = nullptr;
done = true;
}
break;
case 'q':
done = true;
break;
#if !defined(NDEBUG)
case 'i':
printf("Interrupt requested\n");
//yarp::os::impl::kill(0, 2); // SIGINT
//yarp::os::impl::kill(Logger::get().getPid(), 2); // SIGINT
yarp::os::impl::kill(Logger::get().getPid(), 15); // SIGTERM
break;
#endif
case '?':
case 'h':
if (os!=nullptr) {
BufferedConnectionWriter bw(true);
bw.appendLine("This is a YARP port. Here are the commands it responds to:");
bw.appendLine("* Gives a description of this port");
bw.appendLine("d Signals the beginning of input for the port's owner");
bw.appendLine("do The same as \"d\" except replies should be suppressed (\"data-only\")");
bw.appendLine("q Disconnects");
#if !defined(NDEBUG)
bw.appendLine("i Interrupt parent process (unix only)");
#endif
bw.appendLine("r Reverse connection type to be a reader");
bw.appendLine("/port Requests to send output to /port");
bw.appendLine("!/port Requests to stop sending output to /port");
bw.appendLine("~/port Requests to stop receiving input from /port");
bw.appendLine("a Signals the beginning of an administrative message");
bw.appendLine("? Gives this help");
bw.write(*os);
}
break;
default:
if (os!=nullptr) {
BufferedConnectionWriter bw(true);
bw.appendLine("Port command not understood.");
bw.appendLine("Type d to send data to the port's owner.");
bw.appendLine("Type ? for help.");
bw.write(*os);
}
break;
}
if (ip!=nullptr) {
ip->endRead();
}
if (ip==nullptr) {
break;
}
if (closing||isDoomed()||(!ip->isOk())) {
break;
}
}
setDoomed();
YARP_DEBUG(Logger::get(), "PortCoreInputUnit closing ip");
access.wait();
if (ip!=nullptr) {
ip->close();
}
access.post();
YARP_DEBUG(Logger::get(), "PortCoreInputUnit closed ip");
std::string msg = std::string("Removing input from ") +
route.getFromName() + " to " + route.getToName();
if (Name(route.getFromName()).isRooted()) {
if (posted) {
YARP_INFO(Logger::get(), msg);
}
} else {
YARP_DEBUG(Logger::get(), "PortCoreInputUnit (unrooted) shutting down");
}
getOwner().reportUnit(this, false);
if (wasNoticed) {
// Report the disappearing connection
PortInfo info;
info.message = msg.c_str();
info.tag = yarp::os::PortInfo::PORTINFO_CONNECTION;
info.incoming = true;
info.created = false;
info.sourceName = route.getFromName().c_str();
info.targetName = route.getToName().c_str();
info.portName = info.targetName;
info.carrierName = route.getCarrierName().c_str();
if (info.sourceName!="admin") {
getOwner().report(info);
}
}
if (localReader!=nullptr) {
delete localReader;
localReader = nullptr;
}
running = false;
finished = true;
// it would be nice to get my entry removed from the port immediately,
// but it would be a bit dodgy to delete this object and join this
// thread within and from themselves
}
void
Rib::insert(const Name& prefix, const Route& route)
{
RibTable::iterator ribIt = m_rib.find(prefix);
// Name prefix exists
if (ribIt != m_rib.end()) {
shared_ptr<RibEntry> entry(ribIt->second);
RibEntry::iterator entryIt;
bool didInsert = false;
std::tie(entryIt, didInsert) = entry->insertRoute(route);
if (didInsert) {
// The route was new and we successfully inserted it.
m_nItems++;
afterAddRoute(RibRouteRef{entry, entryIt});
// Register with face lookup table
m_faceMap[route.faceId].push_back(entry);
}
else {
// Route exists, update fields
// First cancel old scheduled event, if any, then set the EventId to new one
if (static_cast<bool>(entryIt->getExpirationEvent())) {
NFD_LOG_TRACE("Cancelling expiration event for " << entry->getName() << " "
<< (*entryIt));
scheduler::cancel(entryIt->getExpirationEvent());
}
// No checks are required here as the iterator needs to be updated in all cases.
entryIt->setExpirationEvent(route.getExpirationEvent());
entryIt->flags = route.flags;
entryIt->cost = route.cost;
entryIt->expires = route.expires;
}
}
else {
// New name prefix
shared_ptr<RibEntry> entry = make_shared<RibEntry>();
m_rib[prefix] = entry;
m_nItems++;
entry->setName(prefix);
RibEntry::iterator routeIt = entry->insertRoute(route).first;
// Find prefix's parent
shared_ptr<RibEntry> parent = findParent(prefix);
// Add self to parent's children
if (parent != nullptr) {
parent->addChild(entry);
}
RibEntryList children = findDescendants(prefix);
for (const auto& child : children) {
if (child->getParent() == parent) {
// Remove child from parent and inherit parent's child
if (parent != nullptr) {
parent->removeChild(child);
}
entry->addChild(child);
}
}
// Register with face lookup table
m_faceMap[route.faceId].push_back(entry);
// do something after inserting an entry
afterInsertEntry(prefix);
afterAddRoute(RibRouteRef{entry, routeIt});
}
}
void FlightController::run() {
double yUpperLimit = 9300;
double lowerLimit = 1500;
double xUpperLimit = 8100;
Route myRoute;
myRoute.initRoute(true);
ros::Rate loop_rate(LOOP_RATE);
setStraightFlight(true);
bool firstIteration = true;
takeOff();
bool turning = true;
double amountTurned = 0;
double turnStepSize = 30;
hoverDuration(3);
navData->resetRawRotation();
hoverDuration(2);
cmd.linear.z = 0.5;
pub_control.publish(cmd);
while (navData->getPosition().z < 1200)
ros::Rate(LOOP_RATE).sleep();
hoverDuration(1);
Vector3 pos;
double starting_orientation = navData->getRawRotation();
//ROS_INFO("Start while");
while (!dronePossision.positionLocked) {
//ROS_INFO("in while");
if (turning) {
turnDegrees(turnStepSize);
double orientation = navData->getRawRotation();
amountTurned += angleDifference(starting_orientation,orientation);
//ROS_INFO("AMOUNTTURNED: %f",amountTurned);
starting_orientation = orientation;
if (amountTurned >= 360)
turning = false;
} else {
navData->resetRaw();
flyForward(0.4);
turning = true;
amountTurned = 0;
}
}
lookingForQR = false;
cmd.linear.z = -0.5;
pub_control.publish(cmd);
while (navData->getPosition().z > 900)
ros::Rate(LOOP_RATE).sleep();
//ROS_INFO("end while");
navData->resetToPosition(dronePossision.x * 10, dronePossision.y * 10, dronePossision.heading);
int startWall = dronePossision.wallNumber;
turnDegrees(-dronePossision.angle);
if(startWall == 0) {
while (navData->getPosition().y - 700 > lowerLimit) {
flyBackward(0.7);
navData->addToY(-700);
hoverDuration(3);
}
strafe(1, 0.8);
navData->addToX(1000);
hoverDuration(2);
while (navData->getPosition().y + 700 < yUpperLimit) {
flyForward(0.5);
navData->addToX(700);
hoverDuration(3);
}
} else if(startWall == 2){
while (navData->getPosition().y + 700 < yUpperLimit) {
flyForward(0.5);
navData->addToX(700);
hoverDuration(3);
}
strafe(1, 0.8);
navData->addToX(-1000);
hoverDuration(2);
while (navData->getPosition().y - 700 > lowerLimit) {
flyBackward(0.7);
navData->addToY(-700);
hoverDuration(3);
}
} else if(startWall == 3) {
while (navData->getPosition().x + 700 < xUpperLimit){
flyBackward(0.7);
navData->addToX(700);
hoverDuration(3);
}
strafe(1, 0.8);
navData->addToY(1000);
hoverDuration(2);
while (navData->getPosition().x - 700 > xUpperLimit){
flyBackward(0.7);
navData->addToX(-700);
hoverDuration(3);
}
} else if(startWall == 1){
while (navData->getPosition().x + 700 < xUpperLimit){
flyBackward(0.7);
navData->addToX(700);
hoverDuration(3);
}
strafe(1, 0.8);
navData->addToY(-1000);
hoverDuration(2);
while (navData->getPosition().x - 700 > xUpperLimit){
flyBackward(0.7);
navData->addToX(-700);
hoverDuration(3);
}
}
/* else if(dronePossision.wallNumber == 1){
while (navData->getPosition().y - 1000 > 1500) {
flyForward(0.7);
navData->addToY(-1000);
hoverDuration(3);
cvHandler->swapCam(false);
cvHandler->checkCubes();
cvHandler->swapCam(true);
}
}*/
/*double rotation = navData->getRotation();
double target;
int wallNumber = dronePossision.wallNumber;
ROS_INFO("WALL NUMBER RECEIVED START: %d", wallNumber);
switch(dronePossision.wallNumber) {
case 0:
target = 180;
break;
case 1:
target = 270;
break;
case 2:
target = 0;
break;
case 3:
target = 90;
break;
}
ROS_INFO("Target = %f", target);*/
/*int i = 0;
while(i < 4) {
double difference = angleDifference(rotation, target);
int direction = angleDirection(rotation, target);
ROS_INFO("Difference = %f", difference);
ROS_INFO("Direction = %f", direction);
//turnDegrees(difference*direction);
cmd.angular.z = 1;
pub_control.publish(cmd);
while(navData->getRotation() > 190 || navData->getRotation() < 170 )
ros::Rate(50).sleep();
//ROS_INFO("Difference = %f", difference);
//ROS_INFO("Direction = %f", direction);
//turnTowardsAngle(target, 0);
/* lookingForQR = true;
flyForward(0.7);
cvHandler->swapCam(false);
hoverDuration(3);
cvHandler->checkCubes();
cvHandler->swapCam(true);
flyForward(0.7);
cvHandler->swapCam(false);
hoverDuration(3);
cvHandler->checkCubes();
cvHandler->swapCam(true);
flyForward(0.7);
cvHandler->swapCam(false);
hoverDuration(3);
cvHandler->checkCubes();
cvHandler->swapCam(true);
flyForward(0.7);
cvHandler->swapCam(false);
hoverDuration(3);
cvHandler->checkCubes();
cvHandler->swapCam(true);
lookingForQR = true;
strafe(0.7, -1);
cvHandler->swapCam(false);
hoverDuration(3);
cvHandler->checkCubes();
cvHandler->swapCam(true);
strafe(0.7, -1);
cvHandler->swapCam(false);
hoverDuration(3);
cvHandler->checkCubes();
cvHandler->swapCam(true);
strafe(0.7, -1);
cvHandler->swapCam(false);
hoverDuration(3);
cvHandler->checkCubes();
cvHandler->swapCam(true);
strafe(0.7, -1);
cvHandler->swapCam(false);
hoverDuration(3);
cvHandler->checkCubes();
cvHandler->swapCam(true);
if(target == 0)
while(navData->getPosition().y +1000 < 9300) {
flyForward(0.7);
navData->addToY(1000);
hoverDuration(4);
cvHandler->swapCam(false);
cvHandler->checkCubes();
cvHandler->swapCam(true);
if (navData->getRotation() != target) {
difference = angleDifference(rotation, target);
direction = angleDirection(rotation, target);
ROS_INFO("Difference = %f", difference);
ROS_INFO("Direction = %f", direction);
turnDegrees(difference * direction);
}
}
else if (target == 180)
while (navData->getPosition().y - 1000 > 1500) {
flyForward(0.7);
navData->addToY(-1000);
hoverDuration(4);
cvHandler->swapCam(false);
cvHandler->checkCubes();
cvHandler->swapCam(true);
if (navData->getRotation() != target) {
difference = angleDifference(rotation, target);
direction = angleDirection(rotation, target);
ROS_INFO("Difference = %f", difference);
ROS_INFO("Direction = %f", direction);
turnDegrees(difference * direction);
}
}
}
*/
land();
return;
}
bool TcpRosCarrier::expectSenderSpecifier(ConnectionState& proto) {
Route route = proto.getRoute();
route.setFromName("tcpros");
proto.setRoute(route);
dbg_printf("Trying for tcpros header\n");
ManagedBytes m(headerLen1);
Bytes mrem(m.get()+4,m.length()-4);
NetInt32 ni = headerLen2;
memcpy(m.get(),(char*)(&ni), 4);
dbg_printf("reading %d bytes\n", (int)mrem.length());
int res = proto.is().readFull(mrem);
dbg_printf("read %d bytes\n", res);
if (res!=(int)mrem.length()) {
if (res>=0) {
fprintf(stderr,"TCPROS header failure, expected %d bytes, got %d bytes\n",
(int)mrem.length(),res);
} else {
fprintf(stderr,"TCPROS connection has gone terribly wrong\n");
}
return false;
}
RosHeader header;
header.readHeader(string(m.get(),m.length()));
dbg_printf("Got header %s\n", header.toString().c_str());
std::string rosname;
if (header.data.find("type")!=header.data.end()) {
rosname = header.data["type"];
}
std::string rtyp = getRosType(proto);
if (rtyp!="") {
rosname = rtyp;
header.data["type"] = rosname;
header.data["md5sum"] = (md5sum!="")?md5sum:"*";
if (message_definition!="") {
header.data["message_definition"] = message_definition;
}
}
dbg_printf("<outgoing> Type of data is %s\n", rosname.c_str());
route = proto.getRoute();
if (header.data.find("callerid")!=header.data.end()) {
route.setFromName(header.data["callerid"]);
} else {
route.setFromName("tcpros");
}
proto.setRoute(route);
// Let's just ignore everything that is sane and holy, and
// send the same header right back.
// **UPDATE** Oh, ok, let's modify the callerid. Begrudgingly.
NestedContact nc(proto.getRoute().getToName());
header.data["callerid"] = nc.getNodeName();
string header_serial = header.writeHeader();
string header_len(4,'\0');
char *at = (char*)header_len.c_str();
RosHeader::appendInt32(at,header_serial.length());
dbg_printf("Writing %s -- %d bytes\n",
RosHeader::showMessage(header_len).c_str(),
(int)header_len.length());
Bytes b1((char*)header_len.c_str(),header_len.length());
proto.os().write(b1);
dbg_printf("Writing %s -- %d bytes\n",
RosHeader::showMessage(header_serial).c_str(),
(int)header_serial.length());
Bytes b2((char*)header_serial.c_str(),header_serial.length());
proto.os().write(b2);
if (header.data.find("probe")!=header.data.end()) {
dbg_printf("================PROBE===============\n");
return false;
}
if (!isService) {
isService = (header.data.find("service")!=header.data.end());
}
if (rosname!="" && (user_type != wire_type || user_type == "")) {
if (wire_type!="sensor_msgs/Image") { // currently using a custom method for images
kind = TcpRosStream::rosToKind(rosname.c_str());
TcpRosStream::configureTwiddler(twiddler,kind.c_str(),rosname.c_str(),true,true);
translate = TCPROS_TRANSLATE_TWIDDLER;
}
} else {
rosname = "";
}
sender = isService;
processRosHeader(header);
if (isService) {
auto* stream = new TcpRosStream(proto.giveStreams(),
sender,
false,
isService,
raw,
rosname.c_str());
if (stream==nullptr) { return false; }
proto.takeStreams(stream);
return proto.is().isOk();
}
return true;
}
bool TcpRosCarrier::expectReplyToHeader(ConnectionState& proto) {
RosHeader header;
char mlen[4];
Bytes mlen_buf(mlen,4);
int res = proto.is().readFull(mlen_buf);
if (res<4) {
printf("Fail %s %d\n", __FILE__, __LINE__);
return false;
}
int len = NetType::netInt(mlen_buf);
dbg_printf("Len %d\n", len);
if (len>10000) {
printf("not ready for serious messages\n");
return false;
}
ManagedBytes m(len);
res = proto.is().readFull(m.bytes());
if (res!=len) {
printf("Fail %s %d\n", __FILE__, __LINE__);
return false;
}
header.readHeader(string(m.get(),m.length()));
dbg_printf("Message header: %s\n", header.toString().c_str());
std::string rosname;
if (header.data.find("type")!=header.data.end()) {
rosname = header.data["type"];
}
dbg_printf("<incoming> Type of data is [%s]s\n", rosname.c_str());
if (header.data.find("callerid")!=header.data.end()) {
string name = header.data["callerid"];
dbg_printf("<incoming> callerid is %s\n", name.c_str());
dbg_printf("Route was %s\n", proto.getRoute().toString().c_str());
Route route = proto.getRoute();
route.setToName(name);
proto.setRoute(route);
dbg_printf("Route is now %s\n", proto.getRoute().toString().c_str());
}
if (!isService) {
isService = (header.data.find("request_type")!=header.data.end());
}
if (rosname!="" && (user_type != wire_type || user_type == "")) {
kind = TcpRosStream::rosToKind(rosname.c_str());
TcpRosStream::configureTwiddler(twiddler,kind.c_str(),rosname.c_str(),false,false);
translate = TCPROS_TRANSLATE_TWIDDLER;
} else {
rosname = "";
}
dbg_printf("tcpros %s mode\n", isService?"service":"topic");
// we may be a pull stream
sender = isService;
processRosHeader(header);
auto* stream = new TcpRosStream(proto.giveStreams(),
sender,
sender,
isService,
raw,
rosname.c_str());
if (stream==nullptr) { return false; }
dbg_printf("Getting ready to hand off streams...\n");
proto.takeStreams(stream);
return proto.is().isOk();
}
void SceneGraph::initialize(void (*callbackFn)(int nNode, void *info), void *callbackFnInfo)
{
Node *node;
int nNode = 0;
for (node = Parser::getNodes(); node; node = node->nextTraversal()) {
node->setSceneGraph(this);
if (node->isInstanceNode() == false)
node->initialize();
nNode++;
if (callbackFn)
callbackFn(nNode, callbackFnInfo);
}
// Convert from InstanceNode into DEFNode
node = Parser::getNodes();
while(node != NULL) {
Node *nextNode = node->nextTraversal();
if (node->isInstanceNode() == true && node->isDEFNode() == false) {
Node *referenceNode = node->getReferenceNode();
Node *parentNode = node->getParentNode();
Node *defNode;
defNode = referenceNode->createDEFNode();
if (parentNode != NULL)
parentNode->addChildNode(defNode, false);
else
addNode(defNode, false);
node->remove();
delete node;
nextNode = defNode->nextTraversal();
}
node = nextNode;
}
// Convert from DEFNode into InstanceNode
node = Parser::getNodes();
while(node != NULL) {
Node *nextNode = node->nextTraversal();
if (node->isDEFNode() == true) {
Node *defNode = findNode(node->getName());
assert(defNode);
if (defNode) {
Node *instanceNode = defNode->createInstanceNode();
Node *parentNode = node->getParentNode();
if (parentNode != NULL)
parentNode->moveChildNode(instanceNode);
else
moveNode(instanceNode);
node->remove();
delete node;
}
}
node = nextNode;
}
recomputeBoundingBox();
for (Route *route = Parser::getRoutes(); route; route = route->next())
route->initialize();
}
int main()
{
PairWiseMove move;
Dimension atvsize(1,1);
Dimension coptersize(1,1);
Dimension searchSize(2,2);
VirtualQuadCopter copter(coptersize,searchSize,4,4);
VirtualATV atv(atvsize, 0, 0);
Map mapp;
Route atvRoute;
//atvRoute.push_back(WayPoint(1, 1));
Route * result = move.generatePairRoute(atvRoute, atv, copter, mapp )->first;
bool error = false;
if(result->getSize() == 1){ //Quad must move te atv
if(!(result->getWaypoint(0)->x == 0 && result->getWaypoint(0)->y == 0)){
error = true;
std::cout << "Quad must move to atv waypoint ERROR" << std::endl;
}
std::cout << "Quad must move te atv SUCCES" << std::endl;
}
else{
std::cout << "Quad must move te atv size ERROR" << std::endl;
}
copter = VirtualQuadCopter(coptersize,searchSize,0,0);
atv = VirtualATV(atvsize, 0, 0);
atvRoute.clearRoute();
atvRoute.pushWayPoint( new WayPoint(1, 1));
result = move.generatePairRoute(atvRoute, atv, copter, mapp)->first;
if(!(result->getSize() == 0)){ //Atv is just in range of quadsight
error = true;
std::cout << "Atv is just in range of quadsight ERROR"<< std::endl;
}else{
std::cout << "Atv is just in range of quadsight SUCCES"<< std::endl;
}
atvRoute.clearRoute();
atvRoute.pushWayPoint( new WayPoint(2, 2));
result = move.generatePairRoute(atvRoute, atv, copter, mapp)->first;
if(result->getSize() == 1){ //Atv is just out range of quadsight
if(!(result->getWaypoint(0)->x == 2 && result->getWaypoint(0)->y == 2)){
error = true;
std::cout << "Atv is just out range of quadsight waypoint ERROR"<< std::endl;
}
std::cout << "Atv is just out range of quadsight SUCCES" << std::endl;
}
else{
std::cout << "Atv is just out range of quadsight size ERROR"<< std::endl;
}
atvRoute.clearRoute();
result = move.generatePairRoute(atvRoute, atv, copter, mapp)->first;
if(!result->getSize() == 0){//Empty atv route returns...
error = true;
std::cout << "Empty atv route returns empty pair route ERROR"<< std::endl;
}else{
std::cout << "Empty atv route returns empty pair route SUCCES"<< std::endl;
}
copter.goTo(1, 4);
result = move.generatePairRoute(atvRoute, atv, copter, mapp)->first;
if(!result->getSize() == 1){//Empty atv route returns...
error = true;
std::cout << "Empty atv route returns sync waypoint size ERROR"<< std::endl;
}
else{
if(result->getWaypoint(0)->x == 0 && result->getWaypoint(0)->y == 0){
std::cout << "Empty atv route returns sync waypoint SUCCES"<< std::endl;
}
else{
error = true;
std::cout << "Empty atv route returns sync waypoint waypoint ERROR"<< std::endl;
}
}
copter.goTo(0,0);
atvRoute.pushWayPoint(new WayPoint(1, 2));
atvRoute.pushWayPoint(new WayPoint(2, 3));
atvRoute.pushWayPoint(new WayPoint(4, 2));
atvRoute.pushWayPoint(new WayPoint(5, 3));
atvRoute.pushWayPoint(new WayPoint(-1, -1));
result = move.generatePairRoute(atvRoute, atv, copter, mapp)->first;
if(result->getSize() == 3){//Longer route
std::cout << "Long route size: Good, SUCCES" << std::endl;
if(!(result->getWaypoint(0)->x == 1 && result->getWaypoint(0)->y == 2)){
error = true;
std::cout << " Point 1,2 ERROR" << std::endl;
}
else{
std::cout << " Point 1,2 SUCCES" << std::endl;
}
if(!(result->getWaypoint(1)->x == 4 && result->getWaypoint(1)->y == 2)){
error = true;
std::cout << " Point 4,2 ERROR" << std::endl;
}
else{
std::cout << " Point 4,2 SUCCES" << std::endl;
}
if(!(result->getWaypoint(2)->x == -1 && result->getWaypoint(2)->y == -1)){
error = true;
std::cout << " Point -1,-1 ERROR" << std::endl;
}
else{
std::cout << " Point -1,-1 SUCCES" << std::endl;
}
}
else{
error = true;
std::cout << "Long route size: fault, ERROR" << std::endl;
}
return error;
}
