Hello. I need help. I'm trying to implement a fuzzy controller for avoiding obstacles. I could integrate it in ROS and I want to do it with the Turtlebot. As input, I've got the distance and angle position, and as output, the fuzzy controller tells me the linear and angular velocities.

As the Turtlebot I'm working with uses a Kinect, I suppose that it gives me distance measures from a minimum angle to a maximum angle. What I do is to check the minimum distance and obstacle positions point-per-point, and according to what I read in some forums, Turtlebot's Kinect only works properly within -28 to 28 degrees. Thus, the loop should be within that range.

My problem is the following. When the robot is near an obstacle, it spins to a side, but suddenly it spins to the other side successively (like if the robot was saying "no" with its body). I think there's some other way to do it, but I need your advice. Here's the code:

/*
 * Stand-alone codes for fuzzy inference systems.
 * J.-S. Roger Jang, 1994.
 * Copyright 1994-2002 The MathWorks, Inc.
 * $Revision: 1.12 $  $Date: 2002/06/17 12:47:24 $
 */


#define DEBUG 1
/* Start of the regular fismain.c */

#include "fis.h" //Libreria Controlador Difuso.
#include "ros/ros.h" //Libreria Estandar ROS.
#include <stdio.h>   //Libreria Estandar c++ 
#include <stdarg.h> // Libreria Estandar c++
#include <sensor_msgs/LaserScan.h> //Libreria "LaserScan del package "sensor_msgs" 
#include <geometry_msgs/Twist.h>  //Librería "Twist" del package "geometry_msgs"

/* Main routine 
 **********************************************************************/
/*
    Variables globales del programa. Se utilizan variables globales para
    hacer posible y eficiente el uso del controlador difuso sin tener que
    cargarlo en cada nueva evaluación del mismo.
*/
        FIS *fis;
    DOUBLE **dataMatrix, **fisMatrix, **outputMatrix;
    int fis_row_n, fis_col_n;






/***********************************************************************
                Procedimiento para inicialización
 **********************************************************************/
int inicializaSistemaDifuso(char *fis_file)
{
    /* obtain data matrix and FIS matrix */
    fisMatrix = returnFismatrix(fis_file, &fis_row_n, &fis_col_n);
        /* build FIS data structure */
    fis = (FIS *)fisCalloc(1, sizeof(FIS));
    fisBuildFisNode(fis, fisMatrix, fis_col_n, MF_POINT_N);
    dataMatrix = (DOUBLE **)fisCreateMatrix(10, 10, sizeof(DOUBLE));
        outputMatrix = (DOUBLE **)fisCreateMatrix(10, 10, sizeof(DOUBLE));
   return 1; 
}

/***********************************************************************
                Procedimiento para finalización
 **********************************************************************/
int finalizaSistemaDifuso(void)
{
    /* clean up memory */
    fisFreeFisNode(fis);
    fisFreeMatrix((void **)fisMatrix, fis_row_n);
    fisFreeMatrix((void**)dataMatrix,10);
    fisFreeMatrix((void**)outputMatrix,10);
    return 1;
}

int presentarInformacion(FIS *fis)
{
    fisPrintData(fis);
    return 1;
}

/***********************************************************************
                Procedimiento para usar el controlador
 **********************************************************************/
int evaluarFisOnLine(FIS *fis, DOUBLE **dataMatrix, DOUBLE **outputMatrix )
{
    getFisOutput(dataMatrix[0], fis, outputMatrix[0]);
    return 1; 
}

/*************************************************************************
     Procedimiento para realizar las lecturas sensoriales del Robot
**************************************************************************/
void scanCallback (const sensor_msgs::LaserScan::ConstPtr& scan_msg)
 {

    for (unsigned int i=0;i< scan_msg->ranges.size();i++){   
                   if (scan_msg->ranges[i]<dataMatrix[0][0]){
    /*Distancia-->*/   dataMatrix[0][0]=scan_msg->ranges[i];  
    /*Angulo-->*/      dataMatrix[0][1] = ((double)i*scan_msg->angle_increment)+scan_msg->angle_min;
              //      std::cout << dataMatrix[0][0] <<" ------ "<< dataMatrix[0][1] << std::endl;          



                      }                
     }

}

int main(int argc, char **argv)
{  
  ros::init(argc, argv, "fuzzyros");   //Creación del nodo "fuzzyros"
  int ret;
  dataMatrix[0][0]=10;
  ret=inicializaSistemaDifuso("AvoidObstacleFuzzy.fis");
    if ( ret != 1 ){
        printf("Ha ocurrido un error al inicializar el sistema difuso. No se puede continuar");
       printf("\n\nPulse una tecla para terminar ...");
        getchar();
        exit(0);
    }
   presentarInformacion(fis);

   ros::NodeHandle n;
   ros::Publisher vel_pub_=n.advertise<geometry_msgs::Twist>("cmd_vel", 1);
   ros::Subscriber scan_sub = n.subscribe("scan", 10, scanCallback);


   ros::param::set("/kinect_laser/angle_min",-0.4884);          
   ros::param::set("/kinect_laser/angle_max", 0.4884);         


   geometry_msgs::Twist vel;
   ros::Rate loop_rate(8);
   ret = evaluarFisOnLine(fis, dataMatrix, outputMatrix);

 while (n.ok())  // Bucle mientras no se reciba "Ctrl+C"
        {

                   vel.linear.x=outputMatrix[0][0];        
                   vel.angular.z=outputMatrix[0][1];
                   vel_pub_.publish(vel);
                   dataMatrix[0][0]=10; //Reactualizamos el valor de distancia.
                   ros::spinOnce();
                   loop_rate.sleep();      

             ret = evaluarFisOnLine(fis, dataMatrix, outputMatrix);



 }
 // Liberacion de Recuersos
   ret = finalizaSistemaDifuso();
        if ( ret != 1 ){
          printf("Ha ocurrido un error al finalizar el sistema difuso. No se puede continuar");
          printf("\n\nPulse una tecla para terminar ...");
          getchar();
          exit(0);
        }


 return 0;
}