Revision history [back]

SOLVED. Had -nan values somewhere in my 90,000

 if (isinf(pose.pose.orientation.x) || isnan(pose.pose.orientation.x) ||
      isinf(pose.pose.orientation.y) || isnan(pose.pose.orientation.y) || 
      isinf(pose.pose.orientation.z) || isnan(pose.pose.orientation.z) ||
      isinf(pose.pose.orientation.w) || isnan(pose.pose.orientation.w) ){
      ROS_WARN("Point %d [%0.5f, %0.5f, %0.5f | %0.5f, %0.5f, %0.5f, %0.5f] ", i, pose.pose.position.x, pose.pose.position.y, pose.pose.position.z, pose.pose.orientation.x, pose.pose.orientation.y, pose.pose.orientation.z, pose.pose.orientation.w);
      continue;
}

SOLVED. Had -nan values somewhere in my 90,000 points.

 if (isinf(pose.pose.orientation.x) || isnan(pose.pose.orientation.x) ||
      isinf(pose.pose.orientation.y) || isnan(pose.pose.orientation.y) || 
      isinf(pose.pose.orientation.z) || isnan(pose.pose.orientation.z) ||
      isinf(pose.pose.orientation.w) || isnan(pose.pose.orientation.w) ){
      ROS_WARN("Point %d [%0.5f, %0.5f, %0.5f | %0.5f, %0.5f, %0.5f, %0.5f] ", i, pose.pose.position.x, pose.pose.position.y, pose.pose.position.z, pose.pose.orientation.x, pose.pose.orientation.y, pose.pose.orientation.z, pose.pose.orientation.w);
      continue;
}