AMCL laser skipping scan

edit

I am doing amcl on a quadrotor, as the quadrotor move along and yaw pitch roll might affect the laser scan thus the scan I received might end up confuse my amcl and get the wrong localization. To overcome this problem, my partner has try to compensate laser scan value(within certain range) by creating this node. Let just call it LaserCompensation.

Here's some of the files.

LaserCompensation.cpp

#include <stdexcept>
#include <termios.h>
#include <string>
#include <stdlib.h>
#include <vector>
#include <stdint.h>
#include <stdint.h>
#include <limits>
#include "Rotator.cpp"
#include "sensor_msgs/LaserScan.h"
#include "../../px-ros-pkg/px_comm/msg_gen/cpp/include/px_comm/OpticalFlow.h"
#include "../../flight/msg_gen/cpp/include/flight/flightFeedback.h"
#include <ros/ros.h>
#include <cmath>
#define PI 3.141592654
double Flowtime1 ;
double FlowAltitude1;
double Flowtime2;
double FlowAltitude2;
double altitudedifference;
double timedifference;
double pitchAngle;
double rollAngle;
sensor_msgs::LaserScan receivedscan;
sensor_msgs::LaserScan publishscan;
Rotator *rotator;

void altitudeParse(const px_comm::OpticalFlow::ConstPtr& optFlowMsg)
{
    Flowtime2 = Flowtime1;
    Flowtime1 = optFlowMsg->header.stamp.toSec();
    FlowAltitude2 = FlowAltitude1;
    FlowAltitude1 = optFlowMsg->ground_distance;
    altitudedifference = FlowAltitude1-FlowAltitude2;
    timedifference = Flowtime1 - Flowtime2;
}

void angleParse(const flight::flightFeedback::ConstPtr& msg2)
{
            pitchAngle = msg2->pitchAngle;
            rollAngle = msg2->rollAngle;
}
void Transform()//sensor_msgs::LaserScan& scan_msg)
{

    publishscan.angle_min = -2.35619443;
    publishscan.angle_max = 2.35619443;
    publishscan.angle_increment = (1.25/180*PI);
    publishscan.time_increment = receivedscan.time_increment;
    publishscan.scan_time = receivedscan.scan_time;
    publishscan.range_min = receivedscan.range_min;
    publishscan.range_max = receivedscan.range_max;
    publishscan.header.stamp = receivedscan.header.stamp;
    publishscan.header.frame_id = "test1";
    rotator->set(rollAngle, pitchAngle);
    float x = 0.0;
    float y = 0.0;
    float z = 0.0;
    float xt = 0.0;
    float yt = 0.0;
    float zt = 0.0;
    float r = 0.0;
    float rt = 0.0;
    float indexdifference[216];
    for (int o=0;o<216;o++)
    {
       indexdifference[o]=360.0;
    }

    uint g=0;
    for(int i = 0; i < 1080; i++){
           r = receivedscan.ranges[i];
           x = r*cos(((0.25*i)-45)/180*PI);
           z = r*sin(((0.25*i)-45)/180*PI);
           rotator->unrotate(x, y ,z, xt, yt, zt);
           float d=-90.0;
           if((xt!=0.0)||(zt!=0.0))
           {
               d=atan2(zt,xt)*180.0/PI+45.0;
           }
           if(d<0.0){
               d+=360.0;
           }
           float e= (d*215/270)+0.5;
           int h = e;
           float absolutedifference= std::abs (e-h);
           rt = sqrt(zt*zt+xt*xt);

           if(h>=0.0)
           {
              if(absolutedifference<indexdifference[h])
              {
                 indexdifference[h]=absolutedifference;
                 publishscan.ranges[h]=rt;

              }
           }
           /*if(i<2 && absolutedifference<angle[0])
           {
              angle[0]=d;
              publishscan.ranges[0]=rt; 
           }
           else if(absolutedifference<angle[(i-3/5)+1] && i>2)
           {
              angle[(i-3/5)+1]=d;
              publishscan.ranges[i/5]=rt;
           }*/
    }
}


void parseScan(const sensor_msgs::LaserScan::ConstPtr& scan_msg)
{
    receivedscan.angle_min = scan_msg->angle_min;
    receivedscan.angle_max = scan_msg->angle_max;
    receivedscan.angle_increment = scan_msg->angle_increment;
    receivedscan.time_increment = scan_msg->time_increment;
    receivedscan.scan_time = scan_msg->scan_time;
    receivedscan.range_min = scan_msg->range_min;
    receivedscan.range_max = scan_msg->range_max;
    receivedscan.ranges = scan_msg->ranges;
    receivedscan.header.stamp = scan_msg->header.stamp;
    receivedscan.header.frame_id = scan_msg->header.frame_id;

    Transform();
}

int main(int argc, char **argv)
   {
     rotator = new Rotator();
     ros::init(argc, argv, "LaserCompensation");
     ros::NodeHandle n;
     publishscan.ranges.resize(216);
     int count = 0;
     ros::Subscriber altitude= n.subscribe( "/px4flow/opt_flow", 1, &altitudeParse ...