Help- implement a fuzzy logic controller in ROS for avoiding Obstacles [closed]

asked 2013-03-27 07:27:54 -0600

Bernardo gravatar image

updated 2013-04-29 17:20:27 -0600

tfoote gravatar image

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 ...
(more)
edit retag flag offensive reopen merge delete

Closed for the following reason question is off-topic or not relevant. Please see http://wiki.ros.org/Support for more details. by tfoote
close date 2013-04-29 17:22:11

Comments

This does not appear to be a ROS question. Please see www.ros.org/wiki/Support for guidelines on asking questions.

tfoote gravatar imagetfoote ( 2013-04-29 17:21:56 -0600 )edit