#include<ros/ros.h>
#include<sensor_msgs/LaserScan.h>
#include<geometry_msgs/Twist.h>


float x_pillar;                 //机器人距离物体在X轴上的距离
float y_pillar;                 //机器人距离物体在Y轴上的记录
float alpha_pillar;             //机器人于物体之间的角度
float smallest_distance = 200;  //定义了一个接收激光雷达扫描的距离

geometry_msgs::Twist vel_msg_;
ros::Publisher pub;


void callback(const sensor_msgs::LaserScan::ConstPtr& scan)
{
    int arr_size = floor((scan->angle_max-scan->angle_min)/scan->angle_increment);
    //使用arr_size接收激光雷达扫描一次的激光点数（(最大角度-最小角度)/单位角度 = 激光点的个数）

    for(int i=0;i< arr_size;i++)
    {    
        if(scan->ranges[i]<smallest_distance)
        {    
        smallest_distance = scan->ranges[i];
                //for语句来找到扫描一次后离机器人最近的直线距离的点
        alpha_pillar = (scan->angle_min + i*scan->angle_increment);
        //并计算出角弧度    
        }
    }
    
    x_pillar = smallest_distance*cos(alpha_pillar);
    y_pillar = smallest_distance*sin(alpha_pillar);
    //通过得到的直线距离点，再通过三角函数公式，算出物体在以机器人为原点的X轴和Y轴的偏移量
    
    ROS_INFO("Pillar offset angle(red):[%lf]",alpha_pillar);
    ROS_INFO("Pillar x diatance(m):[%lf]",x_pillar);
    ROS_INFO("Pillar y distance(m):[%lf]",y_pillar);
    //这里我们输出通过我们计算出来的角弧度和X和Y轴的距离
    
    float p_gain_vel = 1;
        float p_gain_ang = 0.4;
    //定义一个线速度和角速度的比值量

        if(x_pillar > 0.3) //当物体在车前时，算出来的x_pillar是大于零的
        {
            if (x_pillar > 5 )
             {     //当物体的距离大于5米时，直接用x和Y距离*速度比值
                 vel_msg_.linear.x = x_pillar * p_gain_vel  ;
                 vel_msg_.angular.z = -(y_pillar * p_gain_ang) ;
        
             }
            else
              {//减速递减
        if(x_pillar > 2 )
        {
         //当物体距离在2米到5米之间时，我们使线速度再*0.2使速度再小一点
                 vel_msg_.linear.x = x_pillar * p_gain_vel * 0.2 ;
                 vel_msg_.angular.z = -(y_pillar * p_gain_ang ) ;
        }
        else
        {
            if(x_pillar < 0.3)
            {     //当物体距离达到0.3米一下时我们让小车停止
                  vel_msg_.linear.x = 0;
                              vel_msg_.angular.z = 0;
            }
        }
 
              }
       }
       else
       {         //当物体距离达到0.3米一下时我们让小车停止
                 vel_msg_.linear.x = 0;
                 vel_msg_.angular.z = 0;
       }
       if(x_pillar < -0.3) //当物体在车子后面时，我们就可以先只给车子一个角速度让车子转过来
       {                   //这样物体又在车子前面了，这样就又执行上面的语句了
         vel_msg_.angular.z = -(y_pillar * p_gain_ang ) ;
       }
    //发布我们计算好的速度指令
        pub.publish(vel_msg_);
    //这条语句是在车子停了以后，当物体再次移动位置时，重新初始化最大距离
        //这样小车就可以继续跟随物体了
    smallest_distance=200;
    
}


int main(int argc,char** argv)
{
    //首先初始化和创建句柄
    ros::init(argc,argv,"HuskyHighlevelController");
    ros::NodeHandle n;

    //订阅机器人的激光雷达的信息
    ros::Subscriber sub = n.subscribe("/scan",1000,callback);
    //定义一个发布者，发布我们处理过激光后，转化成线速度和角速度的信息
    pub=n.advertise<geometry_msgs::Twist>("/husky_velocity_controller/cmd_vel",1000);

    ros::spin();

}