#include "ros/ros.h" #include "gazebo/ApplyBodyWrench.h" // Necesita incluir la dependencia a gazebo en el manifest.xml #include int main(int argc, char **argv) { ros::init(argc, argv, "aplicar_trottle_yaw_simulado"); if (argc != 1) { ROS_INFO("usage: No hacen falta argumentos, directamente aplicara unas fuerzas de X newton a los rotores de los 4 motores"); return 1; } ros::NodeHandle n; // Creamos servicio del tipo gazebo::ApplyBodyWrench y decimos que vamos a publicar en el servicio /gazebo/apply.. // El objeto client es el que nos permitira enviar objetos del tipo especificado (gazebo::Apply..) al servicio especificado (/gazebo/apply..) // Observacion: La llamada es sincrona en el sentido de que si no encuentra/existe el servicio que indiquemos espera a que lo este. ros::ServiceClient client = n.serviceClient("/gazebo/apply_body_wrench"); // Objeto de la clase de gazebo::ApplyBodyWrench para rellenar y enviar(ver en su carpeta srv sus miembros request y response) gazebo::ApplyBodyWrench srv1; gazebo::ApplyBodyWrench srv2; gazebo::ApplyBodyWrench srv3; gazebo::ApplyBodyWrench srv4; gazebo::ApplyBodyWrench srv5; // SIMULACION DEL YAW: introduzco directamente momento en z // gazebo::ApplyBodyWrench //request //string body_name # Gazebo body to apply wrench (linear force and torque) //# wrench is applied in the gazebo world by default //string reference_frame # wrench is defined in the reference frame of this entity //# use inertial frame if left empty //geometry_msgs/Point reference_point # wrench is defined at this location in the reference frame //geometry_msgs/Wrench wrench # wrench applied to the origin of the body //time start_time # (optional) wrench application start time (seconds) //# if start_time is not specified, or //# start_time < current time, start as soon as possible //duration duration # optional duration of wrench application time (seconds) //# if duration < 0, apply wrench continuously without end //# if duration = 0, do nothing //# if duration < step size, apply wrench //# for one step size //response //bool success # return true if set wrench successful //string status_message # comments if available // Relleno los servicios (Link,fuerza,comienzo,duracion) float F1; // Fuerzas a lee desde el control float F2; float F3; float F4; F1=4.20175; // (4.20175 N = Fuerza por motor de equlibrio) F2=4.20175; F3=4.20175; F4=4.20175; F1=F1+0.1; // 0.1 para que suba y 1e-10 para que gire un poco F2=F2+0.1; F3=F3+0.1; F4=F4+0.1; float M5=0.001; srv1.request.body_name = "quad::Base"; // Cuerpo al que se le ejercen las fuerzas srv1.request.reference_frame= "quad::Base"; // Sistema de referencia en el que se escriben las fuerzas srv1.request.reference_point.x=0.2875; srv1.request.reference_point.y=0; srv1.request.reference_point.z=0; srv1.request.wrench.force.x=0; // geometry_msgs (wrench, vector3, ...) lo incluye ApplyBodyWrench.h srv1.request.wrench.force.y=0; srv1.request.wrench.force.z=F1; srv1.request.wrench.torque.x=0; srv1.request.wrench.torque.y=0; srv1.request.wrench.torque.z=0; srv2.request.body_name = "quad::Base"; srv2.request.reference_frame= "quad::Base"; srv2.request.reference_point.x=0; srv2.request.reference_point.y=0.2875; srv2.request.reference_point.z=0; srv2.request.wrench.force.x=0; srv2.request.wrench.force.y=0; srv2.request.wrench.force.z=F2; srv2.request.wrench.torque.x=0; srv2.request.wrench.torque.y=0; srv2.request.wrench.torque.z=0; srv3.request.body_name = "quad::Base"; srv3.request.reference_frame= "quad::Base"; srv3.request.reference_point.x=-0.2875; srv3.request.reference_point.y=0; srv3.request.reference_point.z=0; srv3.request.wrench.force.x=0; srv3.request.wrench.force.y=0; srv3.request.wrench.force.z=F3; srv3.request.wrench.torque.x=0; srv3.request.wrench.torque.y=0; srv3.request.wrench.torque.z=0; srv4.request.body_name = "quad::Base"; srv4.request.reference_frame= "quad::Base"; srv4.request.reference_point.x=0; srv4.request.reference_point.y=-0.2875; srv4.request.reference_point.z=0; srv4.request.wrench.force.x=0; srv4.request.wrench.force.y=0; srv4.request.wrench.force.z=F4; srv4.request.wrench.torque.x=0; srv4.request.wrench.torque.y=0; srv4.request.wrench.torque.z=0; srv5.request.body_name = "quad::Base"; srv5.request.reference_frame= "quad::Base"; srv5.request.reference_point.x=0; srv5.request.reference_point.y=0; srv5.request.reference_point.z=0; srv5.request.wrench.force.x=0; srv5.request.wrench.force.y=0; srv5.request.wrench.force.z=0; srv5.request.wrench.torque.x=0; srv5.request.wrench.torque.y=0; srv5.request.wrench.torque.z=M5; ros::Time::waitForValid(); // IMPORTANTE: HAY QUE ESPERAR A QUE EL TOPIC /CLOCK EMPIEZE A EMITIR-LLEGAR AL NODO, SINO VALE CERO SEA CUAL SEA EL TIEMPO DE SIMULACION ros::Duration un_seg(1,0); // LLamada al constructor. Devuelve un objeto denominado un_seg de la clase Duration(sec , nsec) ambos unit_32: ros::Duration un_seg(1,0); //ros::Time un_seg(1,0) no porque NO ESTA DEFINIDO TIME+TIME. Por tanto defino un_seg como duration: ros::Time ahora = ros::Time::now(); ros::Time ahora_mas_uno = ahora + un_seg; srv1.request.start_time= ahora_mas_uno; srv2.request.start_time= ahora_mas_uno; srv3.request.start_time= ahora_mas_uno; srv4.request.start_time= ahora_mas_uno; srv5.request.start_time= ahora_mas_uno; ROS_INFO("Comienzo de aplicacion de las fuerzas: %d", ahora.sec); //ROS_INFO("ahora: %d", ahora.nsec); //ROS_INFO("ahora: %d", ahora_mas_uno.sec); //ROS_INFO("ahora: %d", ahora_mas_uno.nsec); srv1.request.duration = ros::Duration(5); srv2.request.duration = ros::Duration(5); srv3.request.duration = ros::Duration(5); srv4.request.duration = ros::Duration(5); srv5.request.duration = ros::Duration(5); bool a=client.call(srv1); bool b=client.call(srv2); bool c=client.call(srv3); bool d=client.call(srv4); bool e=client.call(srv5); // Llamada al servicio, enviando el objeto rellenado if (a) { ROS_INFO("Llamada al servicio correcta 1, fuerza aplicada: %f ", srv1.request.wrench.force.z); } else { ROS_ERROR("Fallo al llamar al servicio apply_body_wrench 1"); return 1; } if (srv1.response.success) { ROS_INFO("Respuesta de confirmacion 1"); } else { ROS_ERROR("No se pudo aplicar la fuerza 1"); return 1; } if (b) { ROS_INFO("Llamada al servicio correcta 2, fuerza aplicada: %f ", srv2.request.wrench.force.z); } else { ROS_ERROR("Fallo al llamar al servicio apply_body_wrench 2"); return 1; } if (srv2.response.success) { ROS_INFO("Respuesta de confirmacion 2"); } else { ROS_ERROR("No se pudo aplicar la fuerza 2"); return 1; } if (c) { ROS_INFO("Llamada al servicio correcta 3, fuerza aplicada: %f ", srv3.request.wrench.force.z); } else { ROS_ERROR("Fallo al llamar al servicio apply_body_wrench 3"); return 1; } if (srv3.response.success) { ROS_INFO("Respuesta de confirmacion 3"); } else { ROS_ERROR("No se pudo aplicar la fuerza 3"); return 1; } if (d) { ROS_INFO("Llamada al servicio correcta 4, fuerza aplicada: %f ", srv4.request.wrench.force.z); } else { ROS_ERROR("Fallo al llamar al servicio apply_body_wrench 4"); return 1; } if (srv4.response.success) { ROS_INFO("Respuesta de confirmacion 4"); } else { ROS_ERROR("No se pudo aplicar la fuerza 4"); return 1; } if (e) { ROS_INFO("Llamada al servicio correcta 5, fuerza aplicada: %f ", srv4.request.wrench.torque.z); } else { ROS_ERROR("Fallo al llamar al servicio apply_body_wrench 5"); return 1; } if (srv5.response.success) { ROS_INFO("Respuesta de confirmacion 5"); } else { ROS_ERROR("NO SE PUDO APLICAR EL MOMENTO"); return 1; } 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  >  pruebas  >  aplicar_trottle_yaw_simulado.cpp