MRPT关于rplidar激光雷达的错误

MRPT1.3.0版本关于rplidar激光雷达的数据处理是错误的，不能等到正确的数据。具体的修改如下：

将void  CRoboPeakLidar::doProcessSimple()的函数内容用一下替换。

void  CRoboPeakLidar::doProcessSimple(

         bool                             &outThereIsObservation,

         mrpt::obs::CObservation2DRangeScan   &outObservation,

         bool                             &hardwareError)

{

#if MRPT_HAS_ROBOPEAK_LIDAR

     outThereIsObservation  =false;

     hardwareError          =false;

 

     // Bound?

     if (!checkCOMMs())

     {

         hardwareError = true;

         return;

     }

 

     rplidar_response_measurement_node_t nodes[360*2];

     size_t   count =sizeof(nodes)/sizeof(nodes[0]);

 

     // Scan:

     const mrpt::system::TTimeStamp tim_scan_start =mrpt::system::now();

     u_result op_result = RPLIDAR_DRV->grabScanData(nodes, count);

     //const mrpt::system::TTimeStamp tim_scan_end =mrpt::system::now();

     //const double scan_duration =mrpt::system::timeDifference(tim_scan_start,tim_scan_end);

 

  const size_t angle_compensate_nodes_count =360;  //change by chen

 

     // Fill in scan data:

     if (op_result == RESULT_OK)

     {

         op_result = RPLIDAR_DRV->ascendScanData(nodes, count);

         if (op_result == RESULT_OK)

         {

              // const size_t angle_compensate_nodes_count =360;  change by chen

              const size_t angle_compensate_multiple = 1;

              int angle_compensate_offset = 0;

              rplidar_response_measurement_node_t angle_compensate_nodes[angle_compensate_nodes_count];

              memset(angle_compensate_nodes, 0,angle_compensate_nodes_count*sizeof(rplidar_response_measurement_node_t));

 

              outObservation.scan.assign(angle_compensate_nodes_count,0); //change by chen

              outObservation.validRange.resize(angle_compensate_nodes_count,0);  //change by chen

 

              for(size_t i=0 ; i < count; i++ )

              {

                   if (nodes[i].distance_q2 != 0)

                   {

                       float angle =(float)((nodes[i].angle_q6_checkbit >>RPLIDAR_RESP_MEASUREMENT_ANGLE_SHIFT)/64.0f);

                       intangle_value = (int)(angle * angle_compensate_multiple);

                       if ((angle_value - angle_compensate_offset)< 0) angle_compensate_offset = angle_value;

                       for (size_t j = 0; j <angle_compensate_multiple; j++)

                       {

                            angle_compensate_nodes[angle_value-angle_compensate_offset+j]= nodes[i];

                       }

                   }

              }

 

              for(size_t i=0 ; i < angle_compensate_nodes_count;i++ ) //change by chen

              {

                   const float read_value = (float)angle_compensate_nodes[i].distance_q2/4.0f/1000;

                   outObservation.scan[i] = read_value;

                   outObservation.validRange[i] = (read_value>0);

              }

         }

         else if (op_result == RESULT_OPERATION_FAIL)

         {

              // All the data is invalid, just publish them

              outObservation.scan.assign(angle_compensate_nodes_count,0); //change by chen

              outObservation.validRange.resize(angle_compensate_nodes_count,0); //change by chen

         }

 

         // Fill common parts of the obs:

         outObservation.timestamp = tim_scan_start;

         outObservation.rightToLeft = false;

         outObservation.aperture = 2.0*M_PI;

         outObservation.maxRange =6.0;

         outObservation.stdError = 0.010f;

         outObservation.sensorPose = m_sensorPose;

         outObservation.sensorLabel = m_sensorLabel;

 

         // Do filter:

         C2DRangeFinderAbstract::filterByExclusionAreas(outObservation );

         C2DRangeFinderAbstract::filterByExclusionAngles(outObservation );

         // Do show preview:

         C2DRangeFinderAbstract::processPreview(outObservation);

 

         outThereIsObservation = true;

     }

     else

     {

         if (op_result==RESULT_OPERATION_TIMEOUT ||op_result==RESULT_OPERATION_FAIL)

         {

              // Error? Retry connection

              this->disconnect();

         }

     }

 

 

#endif

}