激光漫游——避障——ros——lms1xx

这个程序主要利用激光数据进行自动避障（漫游）；
代码 有点乱；
使用husky gazebo仿真；
程序在小车平行面向障碍物避障的时候，check_front_obstacle() 检测失败；
各位帮忙看看问题出在哪儿？

可能出错的地方
1。激光angle_min； angle_max ; 0 ° 对应的坐标 x y z是怎么样的（和车一样吗）？
2。lms ros包 总共多少个点 ？1081 ？
代码如下：

/*
 * laserobstacleavoid.cc
 *
 * a simple obstacle avoidance demo
 *
 */

#include "ros/ros.h"
#include <stdio.h>
#include <iostream>
#include <geometry_msgs/Twist.h>
#include <sensor_msgs/LaserScan.h>
#include <math.h>

#define RAYS 1081      
#define PI 3.1415
//lms111 的数据点是1081 
//hokuyo utm-30lx 貌似也是
using namespace std;

float min_left_dist;
float min_right_dist;
double l,r;
double newspeed = 0;
double newturnrate = 0;
geometry_msgs::Twist cmd_vel;
ros::Publisher cmd_vel_pub;

float offset_degrees = 45; // degrees to ignore at the sides
float offset_samples = offset_degrees*4; // 4 samples per degree
//为什么*4,因为激光都是0.25°分辨率

//float max_distance = 100.0;
float max_distance = 2.0;

float force_turning_right_difference = 1.0;

float max_speed = 0.1; // m/s
float speed_ratio = max_distance*2/max_speed;

float max_turning_speed_rad = 2.5; // rad/sec
float turning_ratio_rad = (100+max_distance)/max_turning_speed_rad;


double limit(double n, double min, double max)
{
    if(n < min)
        return min;
    else if(n > max)
        return max;
    else return n;
}

// degrees to radians
float dtor(double degrees)
{
    return degrees * PI / 180;
}

float get_min_distance_left(sensor_msgs::LaserScan laser)
{
    float min_left_dist = 2;

    for(int i=laser.ranges.size()/2; i<laser.ranges.size()-offset_samples; i++)
    {
        if(laser.ranges[i] < min_left_dist)
            min_left_dist = laser.ranges[i];
        //  std::cout << min_left_dist << std::endl;
    }
    return min_left_dist;
}

float get_min_distance_right(sensor_msgs::LaserScan laser)
{
    float min_right_dist = 2.0;

    for(int i=offset_samples; i<laser.ranges.size()/2; i++)
    {
        if(laser.ranges[i] < min_right_dist)
            min_right_dist = laser.ranges[i];
        //  std::cout << min_right_dist << std::endl;
    }
    return min_right_dist;
}

bool check_front_obstacle(sensor_msgs::LaserScan laser, float x_region, float y_region)
{
    int offset = 90 * 4; // 4 samples per degree

    for(int i=offset; i<laser.ranges.size()-offset; i++)
    {
        // limit readings
        if (laser.ranges[i] > 0.1 && laser.ranges[i] < 3.0) 
        {
            double y = laser.ranges[i] * sin(laser.angle_min + i * laser.angle_increment);
            double x = laser.ranges[i] * cos(laser.angle_min + i * laser.angle_increment);
            //cout << "x: " << x << " ,y: " << y << " ,dist: " << laser.ranges[i] << endl;
            if ((  x < x_region) && (fabs(y) < y_region)) 
            {
                std::cout << "found obstacle" << std::endl;
                return true;
            }
        }
    }
    return false;
}
// TODO: doesn´t work
bool check_front_obstacle_semicircle(sensor_msgs::LaserScan laser, float radius)
{
    int offset = 90 * 4; // 4 samples per degree
    float x_center = 0.5;
    float y_center = 0.0;
    float dist_hokuyo_to_center = 0.25;

    for(int i=offset; i<laser.ranges.size()-offset; i++)
    {
        // limit readings
        if (laser.ranges[i] > 0.05 && laser.ranges[i] < 1.0) 
        {
            double x = laser.ranges[i] * sin(laser.angle_min + i * laser.angle_increment);
            double y = laser.ranges[i] * cos(laser.angle_min + i * laser.angle_increment);

            x = x + dist_hokuyo_to_center;
            cout << "x: " << x << " ,y: " << y << " ,dist: " << laser.ranges[i] << endl;
            return true;
            float dist = sqrt(x*x + y*y);
            if(dist < radius)
            {
                cout << "distance: " << dist << endl;
                return true;
            }

        }
    }
    return false;
}

void autonomous_behave(const sensor_msgs::LaserScan &laser)
{
    newspeed = newturnrate = 0.0;

    min_left_dist = get_min_distance_left(laser);
    min_right_dist = get_min_distance_right(laser);

    cout << "----------------------------" << endl;



    cout << "Minimum distance to the left: " << min_left_dist << endl;
    cout << "Minimum distance to the right: " << min_right_dist << endl;


    //l = (1e5*min_right_dist)/500-max_distance; 
  //r = (1e5*min_left_dist)/500-max_distance;

    l = min_left_dist;
    r = min_right_dist;

  //cout << "l: " << l << endl;
  //cout << "r: " << r << endl;

    //if(abs(l - r) < 1000)
    //  cout << "Same distance" << endl;

    // limit max distance
    if (l > max_distance)
        l = max_distance;
  if (r > max_distance)
        r = max_distance;


  cout << "l (limited): " << l << endl;
  cout << "r(limited): " << r << endl;

    /*
    if(l<max_distance || r<max_distance)
    {
        // TODO: force turning to one or another side depending on the sign of the difference
        if(abs(l-r) < force_turning_right_difference) 
        {
            cout << "difference: " << abs(l - r) << endl;
            cout << "Force turning" << endl;
            r = r-100; //turn to the other side

        }
    }
    */

        if(!check_front_obstacle(laser,2.5,1.5))
            newspeed = (r+l)/speed_ratio;
        else 
        {
            cout << "colision detected!!" << endl;
            newspeed = 0.0;
        }
        newspeed = limit(newspeed, -max_speed, max_speed);




    //newturnrate = (r-l);
    newturnrate =-1 * ( l - r );
    std::cout << "newturnrate= l - r " << newturnrate << std::endl;
    //cout << "turn rate: " << newturnrate << endl;

    // degrees to radians
    //newturnrate = dtor(newturnrate);

    //newturnrate = newturnrate/(2.15/max_turning_speed_rad); // 2.15/max_turn_rate_rad

    //cout << "turn rate (rad): " << newturnrate << endl;

    // limit angular speed
    newturnrate = limit(newturnrate, -max_turning_speed_rad, max_turning_speed_rad);

    float limit_robot_stuck = 0.5;

    // check if robot is stuck
    if(newspeed == 0.0 && newturnrate < limit_robot_stuck)
    {
            if(newturnrate > 0)
                newturnrate = limit_robot_stuck;
            else if(newturnrate < 0)
                newturnrate = -limit_robot_stuck;
        }


    cout << "turn rate (rad limited): " << newturnrate << endl;

    //cout << "speed: " << newspeed << "turn: " << newturnrate << endl;



    cmd_vel.linear.x = newspeed;
    cmd_vel.linear.y = 0.0;
    cmd_vel.linear.z = 0.0;
    cmd_vel.angular.x = 0.0;
    cmd_vel.angular.y = 0.0;
    cmd_vel.angular.z = newturnrate;

    cmd_vel_pub.publish(cmd_vel);


}



int main(int argc, char **argv)
{
        ros::init(argc, argv, "laser_obstacle_avoidance");
    ros::NodeHandle nh;
    cmd_vel_pub = nh.advertise<geometry_msgs::Twist>("/cmd_vel", 1);
    ros::Subscriber laser_sub = nh.subscribe("/scan", 10, autonomous_behave);

    ros::spin();
}