#include "ros/ros.h" /// Incluye los funciones comunes de Ros y de STD (math.h,Services.h,....) #include "modelquad/trayectoria.h" /// Incluimos archivo de cabecera en el que se define la trayectoria a seguir #include "nav_msgs/Odometry.h" /// Tipo de mensaje de Odometria (common_msg) #include "geometry_msgs/Point.h" /// Tipo de mensaje para representacion de posicion (x,y,z) #include "modelquad/EnvioPosDeseada.h" /// Tipo mensaje del servicio para enviar puntos al control /// Posicion actual del quadrotor. Global para poder ser llamada desde main y posicion actual geometry_msgs::Point pos_actual; // Point=(x,y,z) /// Funcion Topic /Odometria void posicion_actual(const nav_msgs::Odometry odom) { /// POSICION (Respecto al sistema de referencia del plano (fijo)) pos_actual = odom.pose.pose.position; } ///Funcion auxiliar: devuelve el radio equivalente float radio_equiv(float x, float y) { float result = sqrt( pow( x - pos_actual.x , 2) + pow( y - pos_actual.y , 2) ); return result; } int main(int argc, char **argv) { ROS_INFO("Entra generador de trayectorias"); /// Creacion del nodo con su nombre y etiqueta nodehandle ros::init(argc, argv, "gen_trayectoria"); ros::NodeHandle ng; /// Espera a que empieze a emitir /clock (PLAY GAZEBO) ros::Time::waitForValid(); /// Subscripcion al topic por el que se reciben los mensajes del sensor de odometria del quadrotor.urdf ros::Subscriber Sub_pos_actual = ng.subscribe("/Odometria", 1, posicion_actual); /// Cliente al servicio EnvioPosDeseada: ros::ServiceClient client =ng.serviceClient("envio_pos_deseada"); modelquad::EnvioPosDeseada srv; /// Publicamos por un topic los puntos de la trayectoria enviados al control para poder sacarlos por grafica ros::Publisher Pub_pos_comandada = ng.advertise("gen_trayectoria/pos_comandada", 1000); geometry_msgs::Point pos_comandada; /// Tamanio del vector que contiene la trayectoria int n=sizeof(tray_x)/sizeof(float); /// Frecuencia del ciclo (while) a 10 Hz, necesario incluir loop.sleep() ros::Rate loop(1); /// Indice al final de la iteracion int k = 0; while(ros::ok()) { /// Lectura de la posicion actual = Llamada a la funcion posicion_actual ros::spinOnce(); /// Calculo punto de la trayectoria a enviar al control de posicion del quad ///El indice de busqueda comienza a partir del punto donde se quedo la ultima iteracion int i = k; float R = 0; /// Calculo nuevo punto a enviar /// En el caso de que el indice exceda el maximo (n) sera la ultima componente (n+1) do{ R = radio_equiv( tray_x[i], tray_y[i]); i = i+1; } while( (R <= L) && (i < n) ); /// guardamos indice del punto que se envia para partir de el en la siguiente busqueda k = i-1; /// Una vez que salgamos del do_while i-1 sera el indice del punto de la trayectoria mas alejado del quadrotor a una distancia menor que R a partir del punto de la trayectoria que enviamos en el ciclo anterior /// Enviamos el punto al control de posicion mediante llamada al servicio srv.request.pos_des.posicion.x = tray_x[i-1]; srv.request.pos_des.posicion.y = tray_y[i-1]; srv.request.pos_des.posicion.z = tray_z[i-1]; srv.request.pos_des.orientacion.x = 0; srv.request.pos_des.orientacion.y = 0; srv.request.pos_des.orientacion.z = 0; if (client.call(srv)) ///Llamada al servicio { ROS_INFO("Llamada correcta al control\nPosicion enviada: [%f, %f, %f]", srv.request.pos_des.posicion.x, srv.request.pos_des.posicion.y, srv.request.pos_des.posicion.z); } else { ROS_INFO("Error en la llamada al servicio del control del quadrotor"); } /// Caso fin de trayectoria if(i==n) { ROS_INFO("Fin de la trayectoria"); return 0; } /// Publicacion por el topic de la posicion comandada pos_comandada.x = tray_x[i-1]; pos_comandada.y = tray_y[i-1]; pos_comandada.z = tray_z[i-1]; Pub_pos_comandada.publish(pos_comandada); loop.sleep(); // Espera fin de ciclo (10 Hz) } return 0; } e-REdING. Biblioteca de la Escuela Superior de Ingenieros de Sevilla.


MODELADO, CONTROL Y SIMULACIÓN DE UN QUADROTOR EQUIPADO CON UN BRAZO MANIPULADOR ROBÓTICO

: RAGEL DE LA TORRE, RICARDO
: Ingeniería Industrial
Contenido del proyecto:
Directorio raíz  >  ros_workspace  >  modelquad  >  src  >  control  >  gen_trayectoria.cpp