void Sistema::volarYSensar(const Drone & d)
{
unsigned int i = 0;
int indiceDrone;
/*Hay que buscar un dron equivalente al que nos dieron en el enjambre del
sistema. Por invariante y requiere deberia siempre encontrarlo y ser unico.
*/
while (i < enjambreDrones().size()){
if (enjambreDrones()[i].id() == d.id()){
/*Como queremos modificarlo tenemos que usar _enjambre, no enjambreDeDrones()
porque enajambreDeDrones() devuelve un const.
Asignamos por referencia para poder modificar el drone en cuestion*/
indiceDrone = i;
}
i++;
}
Drone& drone = _enjambre[indiceDrone];
int posX = drone.posicionActual().x;
int posY = drone.posicionActual().y;
bool seMovio = false;
Posicion targetPos;
if (drone.bateria() >0 && enRangoCultivableLibre(posX + 1, posY)){
targetPos.x = posX + 1;
targetPos.y = posY;
drone.moverA(targetPos);
drone.setBateria(drone.bateria() - 1);
seMovio = true;
}
else if (drone.bateria() >0 && enRangoCultivableLibre(posX - 1, posY)){
targetPos.x = posX - 1;
targetPos.y = posY;
drone.moverA(targetPos);
drone.setBateria(drone.bateria() - 1);
seMovio = true;
}
else if (drone.bateria() >0 && enRangoCultivableLibre(posX, posY + 1)){
targetPos.x = posX;
targetPos.y = posY + 1;
drone.moverA(targetPos);
drone.setBateria(drone.bateria() - 1);
seMovio = true;
}
else if (drone.bateria() >0 && enRangoCultivableLibre(posX, posY - 1)){
targetPos.x = posX;
targetPos.y = posY - 1;
drone.moverA(targetPos);
drone.setBateria(d.bateria() - 1);
seMovio = true;
}
/*Si la parcela esta noSensada se le puede poner cualquier verdura
(eh, entienden? Cualquier verdura para cultivar!)*/
if(seMovio == true){
modificarCultivoYDrone(targetPos, drone);
}
}
void Sistema::cargar(std::istream & is)
{
std::string non;
//adelanto hasta el campo
getline(is, non, '{');
getline(is, non, '{');
is.putback('{');
//cargo el campo
_campo.cargar(is);
// cargo todos los drones
// adelanto hasta encontrar donde empieza cada drone ({) o hasta llegar al ] que
// es donde termina la lista de drones
char c;
while (c != ']') {
is.get(c);
if (c == '{') {
is.putback('{');
Drone d;
d.cargar(is);
_enjambre.push_back(d);
}
}
// despues de los drones obtengo todo hasta el final que son los estados de cultivos
getline(is, non);
Secuencia<std::string> estadosCultivosFilas;
// con el substr saco el " " inicial y el } final!
estadosCultivosFilas = cargarLista(non.substr(1, non.length()-2), "[", "]");
// este vector va a tener todos los estados pero "aplanado", todos en hilera
// y cuando los asigne voy accediendolo como todosEstadosCultivos[n_fila*ancho + posicion en Y]
Secuencia<std::string> todosEstadosCultivos;
int n = 0;
while (n < estadosCultivosFilas.size()) {
Secuencia<string> tmp = splitBy(estadosCultivosFilas[n].substr(1, estadosCultivosFilas[n].length() - 2), ",");
todosEstadosCultivos.insert(todosEstadosCultivos.end(), tmp.begin(), tmp.end());
n++;
}
// creo la grilla de estados con las dimensiones correctas
Dimension dim;
dim.ancho = estadosCultivosFilas.size();
dim.largo = todosEstadosCultivos.size() / estadosCultivosFilas.size();
_estado = Grilla<EstadoCultivo>(dim);
// asigno todos los estados dentro de la grilla
int x = 0;
while (x < dim.ancho) {
int y = 0;
while (y < dim.largo) {
_estado.parcelas[x][y] = dameEstadoCultivoDesdeString(todosEstadosCultivos[x*dim.ancho+y]);
y++;
}
x++;
}
//gane :)
}
int main(int argc, char** argv)
{
ROS_INFO("Using subscriptions demo");
ros::init(argc, argv, "UsingSubscriptionsDemo");
Drone d;
d.addNavdataDemoCallback(navdata_demo_callback);
d.addNavdataVisionDetectCallback(navdata_vd_callback);
ros::spin(); //checks subscriptions forever
return 0;
}
void runJoystickControlled(Drone& drone, const string& joystickDevice)
{
Joystick js(drone.io_service(), joystickDevice);
boost::asio::signal_set signals(drone.io_service());
signals.add(SIGINT);
signals.add(SIGTERM);
#ifdef SIGQUIT
signals.add(SIGQUIT);
#endif
JoystickDroneHandler joyHandler(drone, js);
signals.async_wait(boost::bind(&JoystickDroneHandler::stop, &joyHandler));
drone.io_service().run();
}
Drone* DroneDBE::createDrone(){
if (!mDll){
throw("Drone class should never be instanciated without an associated DLL");
}else{
typedef Shmoulette::Drone* (__cdecl* DLLDroneCallback)();
DLLDroneCallback dllFunc = (DLLDroneCallback)::GetProcAddress(mDll, "getInstance");
Drone* d = dllFunc();
d->init(this);
if (mPythonCallback != ""){
d->setPythonCallback(mPythonCallback);
}
return d;
}
}
void runKeyboardControlled(Drone& drone)
{
show_control_tips();
Keyboard keyboard(drone.io_service());
boost::asio::signal_set signals(drone.io_service());
signals.add(SIGINT);
signals.add(SIGTERM);
#ifdef SIGQUIT
signals.add(SIGQUIT);
#endif
KeybaordDroneHandler handler(drone, keyboard);
keyboard.start();
signals.async_wait(boost::bind(&KeybaordDroneHandler::stop, &handler));
drone.io_service().run();
}
int main(int argc, char *argv[])
{
int exitCode = 0;
dpdk_init(argc, argv);
QCoreApplication app(argc, argv);
Drone *drone = new Drone();
OstProtocolManager = new ProtocolManager();
app.setApplicationName(drone->objectName());
// TODO: command line options
// -v (--version)
// -h (--help)
// -p (--portnum)
if (argc > 1)
myport = atoi(argv[2]);
if (!drone->init())
{
exitCode = -1;
goto _exit;
}
qDebug("Version: %s", version);
qDebug("Revision: %s", revision);
#ifdef Q_OS_UNIX
struct sigaction sa;
memset(&sa, 0, sizeof(sa));
sa.sa_handler = cleanup;
if (sigaction(SIGTERM, &sa, NULL))
qDebug("Failed to install SIGTERM handler. Cleanup may not happen!!!");
if (sigaction(SIGINT, &sa, NULL))
qDebug("Failed to install SIGINT handler. Cleanup may not happen!!!");
#endif
exitCode = app.exec();
_exit:
delete drone;
delete OstProtocolManager;
google::protobuf::ShutdownProtobufLibrary();
return exitCode;
}
int main (int argc, char **argv)
{
ros::init(argc, argv, "qc_node");
ros::NodeHandle n;
Drone qc;
eagle_one_test::Pid_send pid;
pid.error = 32;
pid.last_time = 0;
pid.current_time = 2;
geometry_msgs::Twist twist_msg;
twist_msg.linear.x = 0.0;
twist_msg.linear.y = 0.0;
twist_msg.linear.z = 0.0;
twist_msg.angular.x = 0.0;
twist_msg.angular.y = 0.0;
twist_msg.angular.z = 0.0;
ros::Publisher qc_publisher = n.advertise<eagle_one_test::Pid_send>("pid_send", 5);
ros::Subscriber qc_subscriber = n.subscribe("pid_receive", 5, &Drone::get_pid_update, &qc);
ros::Subscriber tag_info = n.subscribe("/ardrone/navdata", 5, &Drone::set_navdata, &qc);
ros::Publisher follow = n.advertise<geometry_msgs::Twist>("/cmd_vel", 5);
ros::Rate rate(10);
while(ros::ok())
{
pid.error = qc.calcTagDistanceX(qc.getTagX());
pid.current_time = (double) ros::Time::now().toSec();
twist_msg.linear.x = qc.update;
qc_publisher.publish(pid);
follow.publish(twist_msg);
ros::spinOnce();
rate.sleep();
}
return 0;
}
JoystickDroneHandler(Drone& drone, Joystick& joystick)
: drone_(drone),
joystick_(joystick),
deadline_(drone.io_service()),
heigthAdj_(0.0),
yawAdj_(0.0)
{
joystick.setButtonHandler(boost::bind(&JoystickDroneHandler::handleButton, this, _1, _2));
joystick.setAxisHandler(boost::bind(&JoystickDroneHandler::handleAxis, this, _1, _2));
joystick.start();
deadline_.async_wait(boost::bind(&JoystickDroneHandler::checkDeadLine, this));
deadline_.expires_from_now(boost::posix_time::milliseconds(50));
}
void *Drone_new(size_t size, Drone proto, int *id)
{
// setup the default functions in case they aren't set
if(!proto.init) proto.init = Drone_init;
if(!proto.boot) proto.boot = Drone_boot;
if(!proto.stand_by) proto.stand_by = Drone_stand_by;
if(!proto.navigate) proto.navigate = Drone_navigate;
if(!proto.pick_up) proto.pick_up = Drone_pick_up;
if(!proto.lift_off) proto.lift_off = Drone_lift_off;
if(!proto.deliver) proto.deliver= Drone_deliver;
if(!proto.return_home) proto.return_home = Drone_return_home;
if(!proto.land) proto.land = Drone_land;
if(!proto.state) proto.state = Drone_state;
// this seems weird, but we can make a struct of one size,
// then point a different pointer at it to "cast" it
Drone *el = calloc(1, size);
*el = proto;
// copy the id over
el->id = *id;
el->curr_state = 0;
el->busy = 0;
el->curr_x = 0;
el->curr_y = 0;
// initialize it with whatever init we were given
if(!el->init(el)) {
// looks like it didn't initialize properly
el->destroy(el);
return NULL;
} else {
// all done, we made an object of any type
return el;
}
}
bool Drone::operator==(const Drone & otroDrone) const
{
if (id() != otroDrone.id()) return false;
if (bateria() != otroDrone.bateria()) return false;
if (enVuelo() != otroDrone.enVuelo()) return false;
if (vueloRealizado() != otroDrone.vueloRealizado()) return false;
if (!mismosProductos(productosDisponibles(), otroDrone.productosDisponibles())) return false;
if (!(posicionActual() == otroDrone.posicionActual())) return false;
return true;
}
void Sistema::modificarCultivoYDrone(Posicion pos, Drone &d){
EstadoCultivo estado = estadoDelCultivo(pos);
if (estado == NoSensado){
_estado.parcelas[pos.x][pos.y] = RecienSembrado;
}
else if ((estado == RecienSembrado || estado == EnCrecimiento) &&
tieneUnProducto(d.productosDisponibles(), Fertilizante)) {
_estado.parcelas[pos.x][pos.y] = ListoParaCosechar;
d.sacarProducto(Fertilizante);
//Verificar si fertilizar gasta bateria.
//Verificar si queda listo para cosechar cuando esta EnCrecimiento y RecienSembrado
}
else if (estado == ConPlaga){
if (d.bateria() >=10 && tieneUnProducto(d.productosDisponibles(), Plaguicida)){
_estado.parcelas[pos.x][pos.y] = RecienSembrado;
d.sacarProducto(Plaguicida);
d.setBateria(d.bateria() - 10);
}
else if (d.bateria() >=5 && tieneUnProducto(d.productosDisponibles(), PlaguicidaBajoConsumo)){
_estado.parcelas[pos.x][pos.y] = RecienSembrado;
d.sacarProducto(PlaguicidaBajoConsumo);
d.setBateria(d.bateria() - 5);
}
}
else if (estado == ConMaleza){
if (d.bateria() >=5 && tieneUnProducto(d.productosDisponibles(), Herbicida)){
_estado.parcelas[pos.x][pos.y] = RecienSembrado;
d.sacarProducto(Herbicida);
d.setBateria(d.bateria() - 5);
}
else if (d.bateria() >=5 && tieneUnProducto(d.productosDisponibles(), HerbicidaLargoAlcance)){
_estado.parcelas[pos.x][pos.y] = RecienSembrado;
d.sacarProducto(HerbicidaLargoAlcance);
d.setBateria(d.bateria() - 5);
}
}
}
int Sistema::pasosIzquierdaPosibles(int y){
Drone d;
//Buscamos el drone
int i = 0;
while (i < enjambreDrones().size()){
if (enjambreDrones()[i].posicionActual().y == y){
d = enjambreDrones()[i];
}
i++;
}
/*La cantidad de pasos posibles pueden depender de la cantidad de
fertilizante, de bateria, a que distancia se encuentra del limite
del campo o de una casa o granero. Evaluamos cada caso por separado
y luego los comparamos.*/
int posX = d.posicionActual().x;
//Empezamos a buscar en posX y nos movemos hacia la izquierda (restamos)
i = posX;
/*Los inicializamos en -1 para que en caso que no haya ni G ni C en la fila
se pueda usar como limite*/
int xGranero = -1;
int xCasa = -1;
while (i >= 0){
Posicion pos;
pos.x = i;
pos.y = y;
if(campo().contenido(pos) == Casa){
xCasa = i;
}
if(campo().contenido(pos) == Granero){
xGranero = i;
}
i--;
}
//Vemos cuanto fertilizante tenemos disponible
int fertilizante = 0;
i = 0;
while(i < d.productosDisponibles().size()){
if(d.productosDisponibles()[i] == Fertilizante){
fertilizante++;
}
i++;
}
//Vemos cuantos pasos se pueden dar con la cantidad disponible de fertilizante
i = posX;
int pasosFert = 0;
while(i > mayor(xCasa, xGranero)){
Posicion pos;
pos.x = i;
pos.y = y;
if((estadoDelCultivo(pos) != EnCrecimiento && estadoDelCultivo(pos) != RecienSembrado) &&
fertilizante > 0){
pasosFert++;
}
if((estadoDelCultivo(pos) == EnCrecimiento || estadoDelCultivo(pos) == RecienSembrado) &&
fertilizante > 0){
pasosFert++;
fertilizante--;
}
i--;
}
return menor(pasosFert, menor(d.bateria(), menor(posX - xGranero, posX - xCasa)));
}
void Sistema::despegar(const Drone & d)
{
Posicion pos;
bool seMueve = false;
/*Siempre que se hace una consulta sobre parcelas vecinas es importante
ver que esten en rango.*/
if (enRangoCultivableLibre(d.posicionActual().x - 1, d.posicionActual().y)){
pos.x = d.posicionActual().x - 1;
pos.y = d.posicionActual().y;
seMueve = true;
}
else if (enRangoCultivableLibre(d.posicionActual().x, d.posicionActual().y + 1)){
pos.x = d.posicionActual().x;
pos.y = d.posicionActual().y + 1;
seMueve = true;
}
else if (enRangoCultivableLibre(d.posicionActual().x, d.posicionActual().y - 1)){
pos.x = d.posicionActual().x;
pos.y = d.posicionActual().y - 1;
seMueve = true;
}
else if (enRangoCultivableLibre(d.posicionActual().x + 1, d.posicionActual().y)){
pos.x = d.posicionActual().x + 1;
pos.y = d.posicionActual().y;
seMueve = true;
}
unsigned int i = 0;
while (i < _enjambre.size()){
if (seMueve && (_enjambre[i].id() == d.id())){
_enjambre[i].moverA(pos);
}
i++;
}
}
void
TacticalAI::CheckTarget()
{
SimObject* tgt = ship_ai->GetTarget();
if (!tgt) return;
if (tgt->GetRegion() != ship->GetRegion()) {
ship_ai->DropTarget();
return;
}
if (tgt->Type() == SimObject::SIM_SHIP) {
Ship* target = (Ship*) tgt;
// has the target joined our side?
if (target->GetIFF() == ship->GetIFF() && !target->IsRogue()) {
ship_ai->DropTarget();
return;
}
// is the target already jumping/breaking/dying?
if (target->InTransition()) {
ship_ai->DropTarget();
return;
}
// have we been ordered to pursue the target?
if (directed_tgtid) {
if (directed_tgtid != target->Identity()) {
ship_ai->DropTarget();
}
return;
}
// can we catch the target?
if (target->Design()->vlimit <= ship->Design()->vlimit ||
ship->Velocity().length() <= ship->Design()->vlimit)
return;
// is the target now out of range?
WeaponDesign* wep_dsn = ship->GetPrimaryDesign();
if (!wep_dsn)
return;
// compute the "give up" range:
double drop_range = 3 * wep_dsn->max_range;
if (drop_range > 0.75 * ship->Design()->commit_range)
drop_range = 0.75 * ship->Design()->commit_range;
double range = Point(target->Location() - ship->Location()).length();
if (range < drop_range)
return;
// is the target closing or separating?
Point delta = (target->Location() + target->Velocity()) -
(ship->Location() + ship->Velocity());
if (delta.length() < range)
return;
ship_ai->DropTarget();
}
else if (tgt->Type() == SimObject::SIM_DRONE) {
Drone* drone = (Drone*) tgt;
// is the target still a threat?
if (drone->GetEta() < 1 || drone->GetTarget() == 0)
ship_ai->DropTarget();
}
}
int main(int argc, char *argv[])
{
int exitCode = 0;
QCoreApplication app(argc, argv);
Drone *drone;
// TODO: command line options
// -v (--version)
// -h (--help)
// -p (--portnum)
if (argc > 1)
myport = atoi(argv[1]);
app.setApplicationName("Drone");
app.setOrganizationName("Ostinato");
/* (Portable Mode) If we have a .ini file in the same directory as the
executable, we use that instead of the platform specific location
and format for the settings */
QString portableIni = QCoreApplication::applicationDirPath()
+ "/drone.ini";
if (QFile::exists(portableIni))
appSettings = new QSettings(portableIni, QSettings::IniFormat);
else
appSettings = new QSettings(QSettings::IniFormat,
QSettings::UserScope,
app.organizationName(),
app.applicationName().toLower());
drone = new Drone();
OstProtocolManager = new ProtocolManager();
if (!drone->init())
{
exitCode = -1;
goto _exit;
}
qDebug("Version: %s", version);
qDebug("Revision: %s", revision);
#ifdef Q_OS_UNIX
struct sigaction sa;
memset(&sa, 0, sizeof(sa));
sa.sa_handler = cleanup;
if (sigaction(SIGTERM, &sa, NULL))
qDebug("Failed to install SIGTERM handler. Cleanup may not happen!!!");
if (sigaction(SIGINT, &sa, NULL))
qDebug("Failed to install SIGINT handler. Cleanup may not happen!!!");
#endif
exitCode = app.exec();
_exit:
delete drone;
delete OstProtocolManager;
google::protobuf::ShutdownProtobufLibrary();
return exitCode;
}
