//-------------------------------------------------------------------------------------
GUIManager::GUIManager(Ogre::Root *root, TankWarWorld *world, Ogre::RenderWindow *window, Ogre::SceneManager *sceneMgr, InputHandler* inputHandler, TankFactory* tankFactory, CollisionManager *mCollisionManager):
mRoot(root), mWorld(world), mWindow(window), mSceneMgr(sceneMgr), mInputHandler(inputHandler), tankFactory(tankFactory), mCollisionManager(mCollisionManager)
{
setupVariables();
initGUI();
setupMenuGUI();
miniMap();
}
//--------------------------------------------------------------
void ofApp::setup()
{
ofSetFrameRate(FRAMERATE);
// Setup Variables
setupVariables();
// Looks for masks inside of the Masks folder
setupMasks();
// Setup the GUI
setupGUI();
// Setup the Timer
setupTimer();
// Setup Custom openCV Class
openCV.setup(CAM_WIDTH,CAM_HEIGHT,FRAMERATE);
}
int main(int argc, char **argv)
{
//set up variables (using names inside flockVars and forceVars structs instead of the arrays because they're easier to remember)
//temporarily assume worst case
setupVariables(param_32plane_500m);
//standard ROS startup
ros::init(argc, argv, "collisionAvoidance");
ros::NodeHandle n;
//subscribe to telemetry outputs and create clients for the goToWaypoint and requestWaypointInfo services
ros::Subscriber sub = n.subscribe("telemetry", 1000, telemetryCallback);
goToWaypointClient = n.serviceClient<AU_UAV_ROS::GoToWaypoint>("go_to_waypoint");
requestWaypointInfoClient = n.serviceClient<AU_UAV_ROS::RequestWaypointInfo>("request_waypoint_info");
//initialize counting
count = 0;
//needed for ROS to wait for callbacks
ros::spin();
return 0;
}
void telemetryCallback(const AU_UAV_ROS::TelemetryUpdate::ConstPtr &msg)
{
AU_UAV_ROS::GoToWaypoint goToWaypointSrv;
AU_UAV_ROS::RequestWaypointInfo requestWaypointInfoSrv;
int planeID = msg->planeID;
/* request this plane's current normal destination */
requestWaypointInfoSrv.request.planeID = planeID;
requestWaypointInfoSrv.request.isAvoidanceWaypoint = false;
requestWaypointInfoSrv.request.positionInQueue = 0;
if (!requestWaypointInfoClient.call(requestWaypointInfoSrv)){
ROS_ERROR("Did not receive a response from the coordinator");
return;
}
destLatArray[planeID] = requestWaypointInfoSrv.response.latitude;
destLonArray[planeID] = requestWaypointInfoSrv.response.longitude;
// check for the total number of planes present to use the correct parameters
if (planeID+1 > maxPlanesFound) maxPlanesFound++;
// check the positions of all the waypoints and planes to see the field size for the correct parameters
// it IS possible for a larger scenario to seem like a smaller scenario by chance, but that should be okay
if (planeID == maxPlanesFound-1)
{
fieldSize = 500;
for (unsigned int i = 0; i < maxPlanesFound; i += 1)
{
// check waypoints
if (destLatArray[i] < (SOUTH_MOST_LATITUDE_500M - LATITUDE_EPSILON) ||
destLonArray[i] > (EAST_MOST_LONGITUDE_500M + LONGITUDE_EPSILON))
{
fieldSize = 1000;
break;
}
}
// use the plane and waypoint information to find the correct parameters (update once per cycle)
if (fieldSize == 500){
if (maxPlanesFound <= 4){
setupVariables(param_4plane_500m);
}
else if (maxPlanesFound <= 8){
setupVariables(param_8plane_500m);
}
else if (maxPlanesFound <= 16){
setupVariables(param_16plane_500m);
}
else if (maxPlanesFound <= 32){
setupVariables(param_32plane_500m);
}
else
{
std::cout << "plane number error: more than 32 UAVs found- using 32 plane case" << std::endl;
setupVariables(param_32plane_500m);
}
}
else if (fieldSize == 1000){
if (maxPlanesFound <= 4){
setupVariables(param_4plane_1000m);
}
else if (maxPlanesFound <= 8){
setupVariables(param_8plane_1000m);
}
else if (maxPlanesFound <= 16){
setupVariables(param_16plane_1000m);
}
else if (maxPlanesFound <= 32){
setupVariables(param_32plane_1000m);
}
else
{
setupVariables(param_32plane_1000m);
std::cout << "plane number error: more than 32 UAVs found- using 32 plane case" << std::endl;
}
}
else std::cout << "field size error: invalid field size determined (this should never happen)" << std::endl;
std::cout << "Using parameters for a " << fieldSize << "m field and " << maxPlanesFound << " UAVs" << std::endl;
}
/*
Remove any planes that have been involved in a collision.
Note: This function is made for use with the evaluator node, and may not work optimally in the field.
To check for collisions, it compares the current time with the last update time of each of the UAVs.
If the values differ by more than three seconds, it is assumed the plane has been deleted. However,
packet losses / network latency may render issues in the real world.
*/
checkCollisions();
/*
if (plane has reached current destination waypoint)
move on to next normal destination waypoint in queue
*/
if (findDistance(msg->currentLatitude, msg->currentLongitude,
requestWaypointInfoSrv.response.latitude,
requestWaypointInfoSrv.response.longitude) < COLLISION_THRESHOLD){
/* request next normal destination */
requestWaypointInfoSrv.request.positionInQueue = 1;
if (!requestWaypointInfoClient.call(requestWaypointInfoSrv)){
ROS_ERROR("Did not recieve a response from the coordinator");
return;
}
}
/*
Pseudocode for the following code lines.
if (this plane is not in the map of planes and the telemetry update indicates that the plane is or has been moving towards a destination)
if (the plane had been flying but we previously detected a collision and removed it)
return - the plane is dead; the simulator is lagging behind
else
the plane has registered a new TelemetryUpdate
else
return - the plane has just been initialized but it is not moving torwards a waypoint as of now
*/
if (pobjects.find(planeID) == pobjects.end() && msg->currentWaypointIndex != -1){
if (msg->currentWaypointIndex > 0){
/* This plane is dead; it had previously been moving but was in a collision.
The simulator is lagging behind and still reporting a telemetry update */
return;
}
else{
/*
a new plane has registered a TelemetryUpdate where the destination is not (0, 0)
create new PlaneObject, collision radius is set to the distance traveled in one second
*/
AU_UAV_ROS::PlaneObject newPlane(MPS_SPEED, *msg); /* */
pobjects[planeID] = newPlane; /* put the new plane into the map */
/* update the destination of the PlaneObject with the value found with the requestWaypointInfoSrv call */
AU_UAV_ROS::waypoint newDest;
newDest.latitude = requestWaypointInfoSrv.response.latitude;
newDest.longitude = requestWaypointInfoSrv.response.longitude;
newDest.altitude = requestWaypointInfoSrv.response.altitude;
pobjects[planeID].setDestination(newDest);
}
}
else if (pobjects.find(planeID) == pobjects.end()) /* new plane without waypoint set */
return;
/*
Note: The requestWaypointInfo service returns a waypoint of -1000, -1000 when the UAV it cannot retrieve a destination from
queue.
Pseudocode:
if (the plane has no destination){
- for simulations, silence any force from this UAV so it does not affect flight paths by giving it an impossible location
- update with the new time of latest update to avoid a false detection of a collision
}
else{
update the plane with the new telemetry information
if (the plane's destination has changed)
update the map entry of the plane with this information
}
*/
if (requestWaypointInfoSrv.response.latitude == -1000){ /* plane has no waypoints to go to */
/*
useful for simulations, remove in real flights;
set location of finished planes to -1000, -1000 so no repulsive force is felt from this plane
*/
pobjects[planeID].setLatitude(-1000);
pobjects[planeID].setLongitude(-1000);
pobjects[planeID].update(); /* update the time of last update for this plane to acknowledge it is still in the air */
return;
}
else{
pobjects[planeID].update(*msg); /* update plane with new position */
/* if (destination has changed)
update pobjects[planeID] with new destination */
if (((pobjects[planeID].getDestination().latitude - requestWaypointInfoSrv.response.latitude) > EPSILON)
|| ((pobjects[planeID].getDestination().longitude - requestWaypointInfoSrv.response.longitude) > EPSILON)
|| ((pobjects[planeID].getDestination().latitude - requestWaypointInfoSrv.response.latitude) < EPSILON)
|| ((pobjects[planeID].getDestination().longitude - requestWaypointInfoSrv.response.longitude) < EPSILON)){
AU_UAV_ROS::waypoint newDest;
newDest.latitude = requestWaypointInfoSrv.response.latitude;
newDest.longitude = requestWaypointInfoSrv.response.longitude;
newDest.altitude = requestWaypointInfoSrv.response.altitude;
pobjects[planeID].setDestination(newDest);
}
}
//form flocks
formFlocks();
//organize flock information
checkFlocks();
// give the UAVs their flock numbers
for (unsigned int i = 0; i < flocks.size(); i += 1)
{
for (unsigned int j = 0; j < flocks[i].size(); j += 1)
{
flocks[i][j]->setFlock(i);
}
}
/* Find the force acting on this UAV. The plane is attracted to its waypoint or leader, and repelled from other UAVs */
AU_UAV_ROS::mathVector force = calculateForces(pobjects[planeID], pobjects, flocks, forceVars);
/*
Create a goToWaypoint service to send to the coordinator based on the force vector just calculated. The destination will be one
second away from the current position.
*/
goToWaypointSrv = updatePath(msg, force);
goToWaypointSrv.request.isAvoidanceManeuver = true;
goToWaypointSrv.request.isNewQueue = true;
if (goToWaypointClient.call(goToWaypointSrv)){
count++;
//ROS_INFO("Received response from service request %d", (count-1));
}
else{
ROS_ERROR("Did not receive response");
}
}
//------------------------------------------------------------------------------
int __attribute__((OS_main)) main(void)
{
setupA();
setupB();
setupVariables();
dnaUsbInit();
INSTALL_MORLOCK_DEFAULTS;
saveEEPROMConstants();
loadEEPROMConstants();
// 12000000 / (64 * 94) = 1994.680 interrupts per second
// accomplished by using timer0 in waveform generation mode 2
TCCR0B = 1<<CS01 | 1<<CS00; // set 8-bit timer prescaler to div/64
OCR0A = 94;
TCCR0A = 1<<WGM01;
TIMSK0 = 1<<OCIE0A; // mode 2, reset counter when it reaches OCROA
TCCR1B = 1<<WGM12 | 1<<CS12;// CTC for OCR1A, clock select
// set up A2D
ADMUX = 1<<MUX0; // A1, Vcc ref
ADCSRA = 1<<ADEN | 1<<ADPS2; // enable A2D, x16
DIDR0 = ADC1D; // disable all digital function on A1
ADCSRB = 1<<ADLAR; // knock off lower two bits, implementation is not that accurate
PRR = 1<<PRUSI; // not using USI (yet)
rnaInit();
sei();
_delay_ms(2); // let state settle, and make sure housekeeping ISR runs
if( !triggerState && !consts.locked )
{
timesToBlinkLight = 1;
inProgramMode = true;
}
for(;;)
{
PRR |= 1<<PRTIM1; // power down timer, don't waste power
// spin until a fire condition is triggered from the ISR
while( !startFireCycle )
{
sampleEye = false;
if ( rnaRequestsConfigData )
{
usbRNAPacket[0] = RNATypeSetConfigData;
for( unsigned char c=0; c<sizeof(consts); c++ )
{
usbRNAPacket[c+1] = ((unsigned char *)&consts)[c];
}
rnaSend( rnaRequestsConfigData, usbRNAPacket, sizeof(consts) + 1 );
rnaRequestsConfigData = 0;
}
if ( !millisecondCountBox && consts.eyeEnabled )
{
millisecondCountBox--;
digitizeEye();
isLedOn = eyeBlocked ? true : false;
}
if ( eepromConstantsDirty )
{
isLedOn = true;
setLedOn();
eepromConstantsDirty = false;
saveEEPROMConstants();
isLedOn = false;
}
if ( rnaPacketAvail )
{
isLedOn = true;
setLedOn();
rnaPacketAvail = false;
_delay_ms( 50 );
rnaSend( usbRNATo, usbRNAPacket, usbRNAPacketExpected );
isLedOn = false;
}
}
sampleEye = true;
// set up the timer
PRR &= ~(1<<PRTIM1); // timer back on
TCNT1 = 0; // reset the timer
TIFR1 |= 1<<OCF1A; // reset compare match
if ( shotsInString < consts.rampEnableCount )
{
shotsInString++;
}
// setup complete, cycle the marker
if ( consts.singleSolenoid )
{
cycleSingleSolenoid();
}
else
{
cycleDoubleSolenoid();
}
// bursting? if so count it down
if ( burstCount && --burstCount )
{
startFireCycle = true;
}
else if ( currentFireMode != ceFullAuto )
{
startFireCycle = false;
}
isLedOn = false;
// make sure at least this much time elapses at the end of a fire cycle
millisecondCountBox = consts.shortCyclePreventionInterval;
while( millisecondCountBox );
while( !(TIFR1 & 1<<OCF1A) ); // wait for end of fire cycle
if ( currentFireMode == ceRamp && startFireCycle && (consts.rampTopMode != ceSemi) ) // officially hit top rate, blow guts out
{
// a shot was scheduled at the maximum rate it could be, go
// ahead and shift up to whatever top mode the user wanted
currentFireMode = consts.rampTopMode;
scheduleShotBox = 0;
scheduleShotRate = 0;
}
}
}
Result iterate() {
setupVariables();
// for now we use a iteration counter to protect us from infinite loops
// TODO< implement loop detection scheme, basic idea
// * we manage a fixed sized array as a sliding window or the hashes of the operations
// * if we detect a loop we apply bland's rule to resolve it (is the rule detection realy neccesary?)
//
//
// * every operation (pivot column, pivot row, number of pivot element itself) gets an hash
// * at each iteration we subtract the previous hash and add the current hash
// * if the hash doesn't change in n iteration(where n == 2 for the simple loop) we are looping
// >
// TODO< implement https://en.wikipedia.org/wiki/Bland's_rule >
unsigned iterationCounter = 0;
const unsigned MaximalIterationCounter = 128;
for(;;) {
iterationCounter++;
if( iterationCounter > MaximalIterationCounter ) {
return Result(Result::EnumSolverState::TOOMANYITERATIONS);
}
bool foundMaximumColumn;
const int pivotColumnIndex = searchMinimumColumn(foundMaximumColumn);
// all values in the target value row are < 0.0, done
if( !foundMaximumColumn ) {
return Result(Result::EnumSolverState::FOUNDSOLUTION);
}
//std::cout << "min column " << pivotColumnIndex << std::endl;
if( areAllEntriesOfPivotColumnNegativeOrZero(pivotColumnIndex) ) {
// solution is unbounded
return Result(Result::EnumSolverState::UNBOUNDEDSOLUTION);
}
// divide numbers of pivot column with right side and store in temporary vector
Eigen::Matrix<Type, Eigen::Dynamic, 1> minRatioVector = divideRightSideWithPivotColumnWhereAboveZero(pivotColumnIndex);
//std::cout << "temporary vector" << std::endl;
//std::cout << minRatioVector << std::endl;
const int minIndexOfTargetFunctionCoefficient = getMinIndexOfMinRatioVector(minRatioVector);
const bool positiveMinRatioExists = doesPositiveMinRatioExist(minRatioVector);
if( !positiveMinRatioExists ) {
// solution is unbounded
return Result(Result::EnumSolverState::UNBOUNDEDSOLUTION);
}
const int pivotRowIndex = minIndexOfTargetFunctionCoefficient;
//std::cout << "pivotRowIndex " << pivotRowIndex << std::endl;
Type pivotElement = matrix(pivotRowIndex, pivotColumnIndex);
// divide the pivot row with the pivot element
matrix.block(pivotRowIndex,0, 1, matrix.cols()) /= pivotElement;
// TODO< assert that pivot elemnt is roughtly 1.0 >
// do pivot operation
for(int pivotRowIteration = 0; pivotRowIteration < matrix.rows(); pivotRowIteration++ ) {
if( pivotRowIteration == pivotRowIndex ) {
continue;
}
Type iterationElementAtPivotColumn = matrix(pivotRowIteration, pivotColumnIndex);
matrix.block(pivotRowIteration, 0, 1, matrix.cols()) -= (matrix.block(pivotRowIndex, 0, 1, matrix.cols()) * iterationElementAtPivotColumn);
}
// set the variable identifier that we know which row of the matrix is for which variable
variables[pivotRowIndex] = Variable(Variable::EnumType::NAMEME, pivotColumnIndex);
//std::cout << matrix << std::endl;
}
}
