Error NaN al integrar una variable

International Español Software
1

Hola hace unos días realice un post consultando por ayuda sobre como implementar un método de estimación de velocidad angular. Pude sacar andando el mismo (funciona más o menos), pero comprobando valores de la odometría estoy obteniendo un nan en una variable. El código es el siguiente:

//Librerías utilizadas en el programa
#include <ros.h>
#include <geometry_msgs/Twist.h>
#include "DualVNH5019MotorShield.h"
#include <nav_msgs/Odometry.h>
#include <PID_v1.h>
#include <TimerThree.h>
#include <ros/time.h>
#include <tf/transform_broadcaster.h>


//Pines para lectura de datos encoders
const byte pinEnc1a = 21;  // Pin encoder 1, señal A
const byte pinEnc2a = 18; // Pin encoder 2, señal A
const byte pinEnc1b = 20;  // Pin encoder 1, señal B
const byte pinEnc2b = 19; // Pin encoder 2, señal B


//Constantes del robot
float R = 0.035; // Radio ruedas
float L = 0.24;  // Longitud entre ruedas
int pulsos = 1120; //cantidad de pasos del encoder en una revolución

//Velocidades lineal y angular que le vamos a proveer al robot. Velocidades que calculamos para cada motor.
volatile float v = 0;
volatile float w = 0;
double Vr = 0, Vl = 0;
double Vrp = 0, Vlp = 0, Deltavr, Deltavl, vrpp = 0, vlpp = 0; //Estos son los valores que le paso a los PID para lograr un arranque suave

//Declaración de objetos PID y motores
double Input1, Output1;
double pulseg1, pulseg2;
double Input2, Output2;
double Kp = .4;
double Ki = 0.01;
double Kd = 0;
volatile double auxiliar1 = 0;
volatile double auxiliar2 = 0;
volatile double contador1 = 0;
volatile double contador2 = 0;
DualVNH5019MotorShield md;


PID myPID1(&Input1, &Output1, &Vrp, Kp, Ki, Kd, DIRECT);
PID myPID2(&Input2, &Output2, &Vlp, Kp, Ki, Kd, DIRECT);

//Variables para cálculo de la odometría
double x = 0;
double y = 0;
double th = 0;
double vx = 0;
double vy = 0;
double vth = 0;

//Frecuencia a la que deseo publicar la odometría
double f = 10;

//Periodo de la señal de alta frecuencia
double hf = 300 * 0.000001;

//Variables auxiliares
long int cont1, cont2;
double aux;
double tiempo1, tiempo2, deltat = 0;
char base_link[] = "/base_link";
char odom[] = "/odom";

//Crear el nodo de ros nh. El nodo se usa para publicarle al Arduino
ros::NodeHandle nh;

//Función que llamo cada vez que me llega un dato de velocidad desde el ros.
void Cbmotor( const geometry_msgs::Twist& mensaje_motor) {
  v = mensaje_motor.linear.x;
  w = mensaje_motor.angular.z;
  Vl = (v + (w * L) / 2) / R;
  Vr = (v - (w * L) / 2) / R;

  if (Vr == 0) {
    Vrp = 0;
  }
  if (Vl == 0) {
    Vlp = 0;
  }
  if (Vr != vrpp) {
    Deltavr = (Vr - Vrp) * 0.1;
  }
  if (Vl != vlpp) {
    Deltavl = (Vl - Vlp) * 0.1;
  }
}

//Suscripto al tópico cmd_vel. Cada vez que llegue un mensaje de dicho tópico va a llamar a la función Cbmotor.
ros::Subscriber<geometry_msgs::Twist> sub("cmd_vel", &Cbmotor );

//Crear el nodo que va a publicarle la odometría al ROS
nav_msgs::Odometry odometria;
ros::Publisher pub("nav_msgs/Odometry", &odometria);

//Crear el nodo que va a publicarle la transformada al ROS
geometry_msgs::TransformStamped trans_odom;
tf::TransformBroadcaster broadcaster;

void setup() {
  //Declaración de los pines de entrada
  pinMode(pinEnc1a, INPUT);
  attachInterrupt(digitalPinToInterrupt(pinEnc1a), interrupcion1, FALLING);
  pinMode(pinEnc2a, INPUT);
  attachInterrupt(digitalPinToInterrupt(pinEnc2a), interrupcion2, FALLING);
  pinMode(pinEnc1b, INPUT);
  pinMode(pinEnc2b, INPUT);

  aux = 1000000 / f;

  //Inicialización de los motores
  md.init();
  md.setM1Speed(0);
  md.setM2Speed(0);

  //Inicializo la suscripción a ROS
  nh.initNode();
  nh.subscribe(sub);

  //Inicializo la publicación a ROS
  nh.initNode();
  nh.advertise(pub);

  //Inicializo broadcasting a ROS
  nh.initNode();
  broadcaster.init(nh);

  //Comienzo de la comunicación serie
  Serial.begin(57600);

  myPID1.SetMode(AUTOMATIC);
  myPID2.SetMode(AUTOMATIC);
  myPID1.SetOutputLimits(-15.7, 15.7 );
  myPID2.SetOutputLimits(-15.7, 15.7 );

  Timer3.initialize(hf * 1000000);       // Dispara cada 30 us
  Timer3.attachInterrupt(timer_int); // Activa la interrupcion y la asocia a timer_int
  tiempo1 = cont1 = micros();
}

void loop() {
  vrpp = Vr;
  vlpp = Vl;
  cont2 = micros();
  Vlp = Vlp + Deltavl;
  if (0.001 > abs(Vl - Vlp)) {
    Deltavl = 0;
  }

  Vrp = Vrp + Deltavr;
  if (0.001 > abs(Vr - Vrp)) {
    Deltavr = 0;
  }
  myPID1.Compute();
  myPID2.Compute();
  md.setM1Speed((Vrp + Output1) * 400 / 15.7);
  md.setM2Speed(-(Vlp + Output2) * 400 / 15.7);
  
  noInterrupts();
  if (Vr < 0) {
    Input1 = -2 * PI / pulsos / auxiliar1 / hf;
  }
  if (Vr > 0) {
    Input1 = 2 * PI / pulsos / auxiliar1 / hf;
  }
  if (Vr == 0) {
    Input1 = 0;
  }
  if (Vl < 0) {
    Input2 = -2 * PI / pulsos / auxiliar1 / hf;
  }
  if (Vl > 0) {
    Input2 = 2 * PI / pulsos / auxiliar1 / hf;
  }
  if (Vl == 0) {
    Input2 = 0;
  }
  interrupts();
  
  //Cálculo de la odometría
  vx = R / 2 * (Input1 + Input2) * cos((float) th);
  vy = R / 2 * (Input1 + Input2) * sin((float) th);
  vth = R / L * (Input1 - Input2);

  tiempo2 = micros();
  deltat = (tiempo2 - tiempo1) / 1000000;
  tiempo1 = tiempo2;

  //Calculamos las posiciones
  x = x + vx * deltat;
  y = y + vy * deltat;
  th =th + vth * deltat;

 
  Serial.print(vth, 10);
  Serial.print("\n");
  Serial.print(th, 10);
   Serial.print("\n\n");

  //Armo el mensaje de la odometría para enviarlo
  odometria.header.stamp = nh.now();
  odometria.header.frame_id = base_link;
  odometria.pose.pose.position.x = Input1;
  odometria.pose.pose.position.y = Input2;
  odometria.pose.pose.position.z = th;
  odometria.child_frame_id = odom;
  odometria.twist.twist.linear.x = vx;
  odometria.twist.twist.linear.y = vy;
  odometria.twist.twist.angular.z = vth;

  //Armo el mensaje de la transformada para enviarlo
  trans_odom.header.frame_id = base_link;
  trans_odom.child_frame_id = odom;
  trans_odom.transform.translation.x = 0.01;
  trans_odom.transform.translation.y = 0;
  trans_odom.transform.translation.z = 0.1;
  trans_odom.transform.rotation.x = 0.0;
  trans_odom.transform.rotation.y = 0.0;
  trans_odom.transform.rotation.z = 0.0;
  trans_odom.transform.rotation.w = 1.0;
  trans_odom.header.stamp = nh.now();

  //Publico la odometría
  if ((cont2 - cont1) > aux) {
    cont1 = cont2;
    pub.publish(&odometria);
    broadcaster.sendTransform(trans_odom);
  }
  nh.spinOnce();
}


void timer_int()
{
  contador1++;
  contador2++;
}

//Cuento la cantidad de pasos del encoder del motor 1
void interrupcion1()
{
  auxiliar1 = contador1;
  contador1 = 0;
}

//Cuento la cantidad de pasos del encoder del motor 2
void interrupcion2()
{
  auxiliar2 = contador2;
  contador2 = 0;
}

Si observan el bucle principal pueden observar que en un momento cálculo una serie de parametros (x,y,th), y al comprobar con el monitor serie me encontre con que el resultado de th me da NaN, lo que posteriomente implica que x e y tambien sean NaN. Buscando en internet encontre que el resultado de NaN en general surge al hacer una división por cero, o en algunos casos por tener un overflow de la variable, pero ese no es mi caso. Si a la cuenta de th la modifico por th=vth*deltat el problema se soluciona, así que el problema me lo genera el acumular la variable. No creo que se produzca un overflow ya que el deltat es el tiempo de loop (son milisegundos) y el vth en el peor de los casos es como máximo 30.

A alguno se le ocurre porque se puede producir este resultado? Desde ya gracias

Saludos

2

Antes que nada.. en el otro hlo no comentas que lo resolviste y de ser asi tendrias que haber puesto la respuesta.
Yo tenia un código pero como ando escaso de tiempo no lo verifiqué.
Si te interesa lo hago pero cuando termines un hilo le pones la solución y indicas SOLUCIONADO editando el titulo principal y entonces cerramos el hilo.

Imposible seguir tu código si no brindas información completa.

  1. los codigos grandes los adjuntas, no los subdividas en varios post, no tiene sentido.
  2. coloca tmb las librerías usadas o enlaces a dichas librerías, de lo contrario es imposible seguir tu consulta.
3

Perrary:
cálculo una serie de parametros (x,y,th), y al comprobar con el monitor serie me encontre con que el resultado de th me da NaN

En que quedamos que la variable th es double o float ?

double th = 0;

vx = R / 2 * (Input1 + Input2) * cos((float) th);
vy = R / 2 * (Input1 + Input2) * sin((float) th);

th = th + vth * deltat;

Serial.print(th, 10);
4

Surbyte, gracias por la respuesta en ambos topics, ahora voy a probar el código de Nick Gammon. Contesto este primero porque puedo hacerlo desde ya. Hasta el momento no pude resolver el problema, por lo que para no seguir perdiendo el tiempo, quise ir probando como andaba el cálculo de la odometría más allá de tener un error en la medición de velocidad (no vaya a ser que tuviese un error sistemático como después me propuso PeterKantTropus). Es por esto que no adjunte ninguna solución ni lo edité como solucionado.

En segundo lugar, voy a tener en cuenta la apreciación que me hiciste respecto a los códigos grandes y a las librerías utilizadas.

Kike_GL, gracias por responder. La variable th es double. El problema de esa doble declaración, fue que googleando me encontré con que a la función coseno y seno había que pasarle como argumento un float, y yo le estaba pasando un double, y en su momento pensé que esta podía ser la razón por la que obtenía un NaN. Lo verifiqué, y resultó que no era así, por lo que la declaración como float está de más, y se me olvidó borrarla en el código que les adjunté.

Resumiendo. Voy a proceder a comprobar primero la solución que me propuso Surbyte en el otro tópico (Estimacion de velocidad con encoders incrementales (SOLUCIONADO) - Software - Arduino Forum) y después volvemos a este. No se si tengo que editar el título aclarando algo de esto.

Saludos

5

Perrary:
La variable th es double. El problema de esa doble declaración, fue que googleando me encontré con que a la función coseno y seno había que pasarle como argumento un float, y yo le estaba pasando un double, y en su momento pensé que esta podía ser la razón por la que obtenía un NaN. Lo verifiqué, y resultó que no era así, por lo que la declaración como float está de más, y se me olvidó borrarla en el código que les adjunté.

Lo importante de la variable th es que debe estar en radianes en valores entre 0 y TWO_PI y la respuesta sera un float entre -1 y +1.

sin(0);                 //0
sin(HALF_PI/2);    //0.707
sin(HALF_PI);       //1
sin(PI);                //0
sin(PI+HALF_PI)   //-1
sin(TWO_PI);       //0