int main(int argc, char** argv) {
ros::init(argc, argv, "multiquad_script_test");
ros::NodeHandle node;
ros::Rate rate(FRAMES_PER_SEC);
// Create quads
Quad quad1("quad01");
Quad quad2("quad02");
// Group quads together
Formation form;
form.add_quad(quad1);
form.add_quad(quad2);
// form.add_script<Takeoff>(new Takeoff(0.0));
ROS_INFO("Boundary checking starting...");
while(node.ok()) {
form.run();
rate.sleep();
}
return 0;
}
gererFormation::gererFormation(Formation& f,QWidget *parent) :
QDialog(parent),
ui(new Ui::gererFormation)
{
nom=f.getNom();
description=f.getDescription();
uvs=f.getUVs();
nbCredits=f.getNbCreditsByCat();
critereValidation=f.getConditions();
nouvelle=false;
ui->setupUi(this);
init();
}
int FormationWorld::formationInteractions( ){
int nInteractions = 0;
for( int i=0; i<formations.size(); i++ ){
Formation * fi = formations[i];
//if( fi != NULL ){
//for( int j=0; j<i; j++ ){ // This will be more complicated to resolve symetrically
for( int j=0; j<formations.size(); j++ ){
if( i==j ){
nInteractions += fi->interactInside( );
}else{
nInteractions += fi->interact( formations[j] );
}
}
//}
}
return nInteractions;
}
//
// bool initCells(n, f)
// Last modified: 28Aug2006
//
// Initializes each cell to the parameterized values,
// returning true if successful, false otherwise.
//
// Returns: true if successful, false otherwise
// Parameters:
// n in the initial number of cells
// f in the initial formation
//
bool Environment::initCells(const GLint n, const Formation f)
{
srand(time(NULL));
for (GLint i = 0; i < n; ++i) if (!addCell()) return false;
// initialize each robot's neighborhood
if (!initNbrs()) return false;
// organizes the cells into an initial formation (default line)
Cell *c = NULL;
for (GLint i = 0; i < getNCells(); ++i)
{
if (!cells.getHead(c)) return false;
c->x = f.getRadius() *
((GLfloat)i - (GLfloat)(getNCells() - 1) / 2.0f);
c->y = 0.0f;
c->setColor(DEFAULT_ROBOT_COLOR);
c->setHeading(f.getHeading());
++cells;
}
return (getNCells() == n) &&
sendMsg(new Formation(f), f.getSeedID(),
ID_OPERATOR, CHANGE_FORMATION);
} // initCells(const GLint, const Formation f)
bool Environment::initRobots(const Formation f)
{
for (GLint i = 0; i < N_CELLS; ++i)
if (!addCell(gXPos[i], gYPos[i], gHeading[i]))
return false;
// initialize each robot's neighborhood
if (!initNbrs()) return false;
setColor(BLACK);
return (getNCells() == N_CELLS) &&
sendMsg(new Formation(f), f.getSeedID(),
ID_OPERATOR, CHANGE_FORMATION);
}
void UnitTrackingMarker::RenderTrackingFighters(VertexShape_3f_4f_1f* vertices)
{
if (!_meleeTarget && !_missileTarget)
{
bool isBlue = _unit->commander->GetTeam() == _battleView->GetCommander()->GetTeam();
glm::vec4 color = isBlue ? glm::vec4(0, 0, 255, 16) / 255.0f : glm::vec4(255, 0, 0, 16) / 255.0f;
glm::vec2 destination = DestinationXXX();
//glm::vec2 orientation = _missileTarget ? _missileTarget->state.center : _orientation;
Formation formation = _unit->formation;
formation.SetDirection(GetFacing());
glm::vec2 frontLeft = formation.GetFrontLeft(destination);
for (Fighter* fighter = _unit->fighters, * end = fighter + _unit->fightersCount; fighter != end; ++fighter)
{
glm::vec2 offsetRight = formation.towardRight * (float)Unit::GetFighterFile(fighter);
glm::vec2 offsetBack = formation.towardBack * (float)Unit::GetFighterRank(fighter);
glm::vec3 p = _battleView->GetSimulator()->GetBattleMap()->GetHeightMap()->GetPosition(frontLeft + offsetRight + offsetBack, 0.5);
vertices->AddVertex(Vertex_3f_4f_1f(p, color, 3.0));
}
}
}
//
// GLfloat rangeSensor(p)
// Last modified: 08Nov2009
//
// Returns with the distance from this robot
// to the position being auctioned.
//
// Returns: the distance from this robot to the position being auctioned
// Parameters:
// p in/out the packet (auction) to be processed
GLfloat Robot::rangeSensor(Packet &p)
{
// unpack the auction from the packet
Push_Auction_Announcement *aa = (Push_Auction_Announcement *)p.msg;
// unpack the formation definition from the state within the auction
Formation f = aa->s_i.formation;
// get the range from the auctioneer to this robot
GLfloat range = env->distanceToRobot(env->getCell(p.fromID), this);
// if the auctioneer is beyond sensor range, do not bid
if (range > SENSOR_RANGE) return -1.0f;
// get the relative bearing from the auctioneer to this robot
GLfloat bearing = bearingSensor(p.fromID);
// get the angle between the auctioneer and the position being auctioned
GLfloat ang = atan2(f.getFunction()(f.getRadius()), f.getRadius()) -
bearing;
// get the vector between the auctioneer and this robot
Vector d = (*(Vector *)this) - (*(Vector *)env->getCell(p.fromID));
// get the vector between the auctioneer and the position being auctioned
Vector e;
e.y = f.getFunction()(f.getRadius());
e.x = sqrt((f.getRadius() * f.getRadius()) - (e.y * e.y));
// get the vector between the position being auctioned and this robot
Vector a;
if (aa->right) // if the position is to the right of the auctioneer
a = d - e;
else
a = d + e;
// range is the magnitude of the vector between
// the position being auctioned and this robot
range = a.magnitude();
//cout << "robot " << getID() << " bidding " << range << endl;
return range;
} // rangeSensor(Packet &)
void FormationTacticsApp::eventHandling ( const SDL_Event& event ){
//printf( "NBodyWorldApp::eventHandling() \n" );
switch( event.type ){
case SDL_KEYDOWN :
switch( event.key.keysym.sym ){
case SDLK_0: formation_view_mode = 0; printf( "view : default\n" ); break;
case SDLK_1: formation_view_mode = VIEW_INJURY; printf( "view : injury\n" ); break;
case SDLK_2: formation_view_mode = VIEW_STAMINA; printf( "view : stamina\n" ); break;
case SDLK_3: formation_view_mode = VIEW_CHARGE; printf( "view : charge\n" ); break;
case SDLK_4: formation_view_mode = VIEW_MORAL; printf( "view : moral\n" ); break;
}
break;
case SDL_MOUSEBUTTONDOWN:
switch( event.button.button ){
case SDL_BUTTON_LEFT:
//printf( "left button pressed !!!! " );
if( currentFaction != NULL ) currentFormation = currentFaction->getFormationAt( { mouse_begin_x, mouse_begin_y } );
break;
case SDL_BUTTON_RIGHT:
//printf( "left button pressed !!!! " );
if( currentFormation != NULL ) currentFormation->setTarget( { mouse_begin_x, mouse_begin_y } );
break;
}
break;
/*
case SDL_MOUSEBUTTONUP:
switch( event.button.button ){
case SDL_BUTTON_LEFT:
//printf( "left button pressed !!!! " );
world.picked = NULL;
break;
}
break;
*/
};
AppSDL2OGL::eventHandling( event );
camStep = zoom*0.05;
}
// OpenGL global variables
GLint g_windowSize[2] = {800, 800}; // window size in pixels
GLfloat g_windowWidth = 2.0f; // resized window width
GLfloat g_windowHeight = 2.0f; // resized window height
// simulation global constants
const GLfloat SELECT_RADIUS = 1.5f * DEFAULT_ROBOT_RADIUS;
const GLint N_CELLS = 0;
const GLint MIDDLE_CELL = 0;//(N_CELLS - 1) / 2;
const Formation DEFAULT_FORMATION = Formation(formations[0],
DEFAULT_ROBOT_RADIUS *
FACTOR_COLLISION_RADIUS,
Vector(), MIDDLE_CELL, 0,
90.0f);
// simulation global variables
Environment *g_env = NULL;
GLint g_nRobots = 0;
GLfloat g_fRadius = DEFAULT_FORMATION.getRadius();
GLint g_sID = DEFAULT_FORMATION.getSeedID();
GLint g_fID = DEFAULT_FORMATION.getFormationID();
GLfloat g_fHeading = DEFAULT_FORMATION.getHeading();
GLint g_fIndex = 0;
GLint g_selectedIndex = g_sID;
GLint g_dt = 50; // time interval (in milliseconds)
bool operator==(Formation f1, Formation f2 ){return (f1.getStrLabel()==f2.getStrLabel());}
int main(int argc, char** argv) {
// ROS initialization stuff
ros::init(argc, argv, "quadscript_example");
ros::NodeHandle node;
// Set the rate to run everything at. FRAMES_PER_SEC is set in
// constants_config.h
ros::Rate rate(FRAMES_PER_SEC);
// Create Quad objects with the namespace corresponding to the
// actual quadcopter, or the type of fake quad for FakeQuads.
FakeQuad fake(WAND_MOVABLE);
Quad quad1("quad01");
Quad quad2("quad02");
// Add quads to a formation to give them access to each other and
// make group movements easier.
Formation form;
form.add_quad(fake);
form.add_quad(quad1);
form.add_quad(quad2);
// Tells the entire formation to takeoff to the specified height.
// Does not do anything for any FakeQuads in the formation.
form.add_script<Takeoff>(new Takeoff(0.5));
// Sets quad1 to the specified position (first 3), with the specified
// orientation quaternion (last 4)
quad1.add_script(new SetPose(-1, -0.7, 0.5, 0, 0, 0, -1));
// Sets quad1's most recently added script to need a wand check, meaning
// it won't move on to the next script until the wand signal is given.
quad1.back()->set_needsWandCheck(true);
// Same for quad2
quad2.add_script(new SetPose(-1, 0.7, 0.5, 0, 0, 0, -1));
quad2.back()->set_needsWandCheck(true);
// Sets quads to follow the 0th index quad in the formation (last arg of ctor),
// with the offset (x, y, z) specified in first 3 args.
// 0th index quad in the formation is a WAND_MOVABLE FakeQuad, so the quads
// will follow the imaginary point set by the FakeQuad.
quad1.add_script(new FollowOffset(0.6, 0, 0, 0));
quad2.add_script(new FollowOffset(-0.6, 0, 0, 0));
// View QuadScripts.h for more details and documentation of how to
// use specific scripts
ROS_INFO("Starting...");
// Loops FRAMES_PER_SEC times per second, running the publisher in each active
// QuadScript every time. Instead of form.run(), can also use quad#.run() for
// individual quads.
while(node.ok()) {
form.run();
rate.sleep();
}
return 0;
}
cocos2d::Vec2 SteeringBehaviors::keepFormation( Formation& aFormation, int posId )
{
return arrive(aFormation.getPositionByPosId(posId));
}
