<commit_before>#ifndef __PRONTO_FRAME_CHECK_TOOLS_HPP__
#define __PRONTO_FRAME_CHECK_TOOLS_HPP__

/*
SCAN has parent: POST_SPINDLE <fixed>
POST_SPINDLE has parent: PRE_SPINDLE <from multisense as PRE_SPINDLE_TO_POST_SPINDLE>
PRE_SPINDLE has parent: CAMERA_LEFT <fixed>
CAMERA_LEFT has parent: head <fixed>
head has parent: body  <from FK in joint2frames as BODY_TO_HEAD>
body has parent: local <by the estimator as POSE_BODY>
*/

class FrameCheckTools
{
public:
  FrameCheckTools (){
    local_to_head_frame_valid_ = false;

  };
  ~FrameCheckTools() {}
  
  
  // Check if the SCAN frames have been updated at all.
  bool isLocalToScanValid(BotFrames* botframes){

    if (local_to_head_frame_valid_){
      return true; 
    }else{ // check each updating frame has been updated already.      
      int64_t last_update_utime_1, last_update_utime_2, last_update_utime_3;

      if (!bot_frames_get_trans_latest_timestamp(botframes, "PRE_SPINDLE", "POST_SPINDLE", &last_update_utime_1)){
        return false; 
      }
      if (!bot_frames_get_trans_latest_timestamp(botframes, "head", "body", &last_update_utime_2)){
        return false; 
      }
      if (!bot_frames_get_trans_latest_timestamp(botframes, "body", "local", &last_update_utime_3)){
        return false; 
      }
      
      std::cout << "PRE_SPINDLE" << " to " << "POST_SPINDLE" << " last updated " << last_update_utime_1 << "\n";
      std::cout << "head" << " to " << "body" << " last updated " << last_update_utime_2 << "\n";
      std::cout << "body" << " to " << "local" << " last updated " << last_update_utime_3 << "\n";
      
      if (( (last_update_utime_1 > 0) && (last_update_utime_2 > 0) )  && (last_update_utime_3 > 0) ){
        local_to_head_frame_valid_ = true;
        return true;
      }else{
        return false;
      }
    }
  }
  
  
  // Check if the SCAN frames are more recent than the scan utime (to avoid extrapolation):
  bool isLocalToScanUpdated(BotFrames* botframes, int64_t scan_utime){
    int64_t last_update_utime_1, last_update_utime_2, last_update_utime_3;
    if (!bot_frames_get_trans_latest_timestamp(botframes, "PRE_SPINDLE", "POST_SPINDLE", &last_update_utime_1)){
      return false; 
    }
    if (!bot_frames_get_trans_latest_timestamp(botframes, "head", "body", &last_update_utime_2)){<commit_msg>hyq does not have a neck. Quick hack to remove this when mapping<commit_after>#ifndef __PRONTO_FRAME_CHECK_TOOLS_HPP__
#define __PRONTO_FRAME_CHECK_TOOLS_HPP__

/*
SCAN has parent: POST_SPINDLE <fixed>
POST_SPINDLE has parent: PRE_SPINDLE <from multisense as PRE_SPINDLE_TO_POST_SPINDLE>
PRE_SPINDLE has parent: CAMERA_LEFT <fixed>
CAMERA_LEFT has parent: head <fixed>
head has parent: body  <from FK in joint2frames as BODY_TO_HEAD>
body has parent: local <by the estimator as POSE_BODY>
*/

class FrameCheckTools
{
public:
  FrameCheckTools (){
    local_to_head_frame_valid_ = false;

  };
  ~FrameCheckTools() {}
  
  
  // Check if the SCAN frames have been updated at all.
  bool isLocalToScanValid(BotFrames* botframes){

    if (local_to_head_frame_valid_){
      return true; 
    }else{ // check each updating frame has been updated already.      
      int64_t last_update_utime_1, last_update_utime_2, last_update_utime_3;

      if (!bot_frames_get_trans_latest_timestamp(botframes, "PRE_SPINDLE", "POST_SPINDLE", &last_update_utime_1)){
        return false; 
      }
      if (!bot_frames_get_trans_latest_timestamp(botframes, "head", "body", &last_update_utime_2)){
        return false; 
      }
      if (!bot_frames_get_trans_latest_timestamp(botframes, "body", "local", &last_update_utime_3)){
        return false; 
      }
      
      std::cout << "PRE_SPINDLE" << " to " << "POST_SPINDLE" << " last updated " << last_update_utime_1 << "\n";
      std::cout << "head" << " to " << "body" << " last updated " << last_update_utime_2 << "\n";
      std::cout << "body" << " to " << "local" << " last updated " << last_update_utime_3 << "\n";
      
      if ( (last_update_utime_1 > 0)   && (last_update_utime_3 > 0) ){
        local_to_head_frame_valid_ = true;
        return true;
      }else{
        return false;
      }
    }
  }
  
  
  // Check if the SCAN frames are more recent than the scan utime (to avoid extrapolation):
  bool isLocalToScanUpdated(BotFrames* botframes, int64_t scan_utime){
    int64_t last_update_utime_1, last_update_utime_2, last_update_utime_3;
    if (!bot_frames_get_trans_latest_timestamp(botframes, "PRE_SPINDLE", "POST_SPINDLE", &last_update_utime_1)){
      return false; 
    }
    if (!bot_frames_get_trans_latest_timestamp(botframes, "head", "body", &last_update_utime_2)){
{'fraction_non_alphanumeric': 0.10867697594501718, 'fraction_numerical': 0.011168384879725086, 'mean_word_length': 3.8409563409563408, 'pattern_counts': {'":': 0, '<': 87, '<?xml version=': 0, '>': 20, 'https://': 0, 'lorem ipsum': 0, 'www.': 0, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 0, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
{'dir': 'git-commits-cleaned', 'id': '3871824', 'n_tokens_mistral': 1692, 'n_tokens_neox': 1636, 'n_words': 365}