コード例 #1
0
  bool process_frame()
  {
    static ros::Time lastTime;
    Output_GetFrameNumber OutputFrameNum = MyClient.GetFrameNumber();
    if ((OutputFrameNum.Result != Result::Success) and (OutputFrameNum.Result != Result::NoFrame))
    {
      ROS_ERROR("GetFrameNumber() returned %d", int(OutputFrameNum.Result));
      return false;
    }
    if (frame_datum == 0 or (fabs(latest_time_bias) > time_bias_reset)) // this is the first frame we've gotten
    {
      frame_datum = OutputFrameNum.FrameNumber;
      time_datum = now_time;
      ROS_INFO("Time bias reset (bias was %f): marking VICON frame number %d as datum time %d.%d", latest_time_bias, frame_datum, time_datum.sec,
               time_datum.nsec);
      time_bias_reset_count++;
    }

    //frameCount++;
    //    ROS_INFO_STREAM("Grabbed a frame: " << OutputFrameNum.FrameNumber);
    int frameDiff = 0;
    if (lastFrameNumber != 0)
    {
      frameDiff = OutputFrameNum.FrameNumber - lastFrameNumber;
      frameCount += frameDiff;
      if ((frameDiff) > 1)
      {
        droppedFrameCount += frameDiff - 1;
        double droppedFramePct = (double)droppedFrameCount / frameCount * 100;
        ROS_DEBUG_STREAM(
                         (frameDiff - 1) << " more (total " << droppedFrameCount << "/" << frameCount << ", "
                             << droppedFramePct << "%) frame(s) dropped. Consider adjusting rates.");
      }
    }
    lastFrameNumber = OutputFrameNum.FrameNumber;

    if (frameDiff == 0)
    {
      return false;
    }
    else
    {
      freq_status.tick();
      //double latencyInMs = MyClient.GetLatencyTotal().Total * 1000;
      ros::Time frame_time = time_datum + ros::Duration((lastFrameNumber - frame_datum) / vicon_capture_rate);
      latest_time_bias = (frame_time - now_time).toSec();

      process_subjects(frame_time);
      process_markers(frame_time, lastFrameNumber);

      lastTime = now_time;
      return true;
    }
  }
コード例 #2
0
  bool process_frame()
  {
    static ros::Time lastTime;
    Output_GetFrameNumber OutputFrameNum = msvcbridge::GetFrameNumber();

    //frameCount++;
    //ROS_INFO_STREAM("Grabbed a frame: " << OutputFrameNum.FrameNumber);
    int frameDiff = 0;
    if (lastFrameNumber != 0)
    {
      frameDiff = OutputFrameNum.FrameNumber - lastFrameNumber;
      frameCount += frameDiff;
      if ((frameDiff) > 1)
      {
        droppedFrameCount += frameDiff;
        double droppedFramePct = (double)droppedFrameCount / frameCount * 100;
        ROS_DEBUG_STREAM(frameDiff << " more (total " << droppedFrameCount << "/" << frameCount << ", "
            << droppedFramePct << "%) frame(s) dropped. Consider adjusting rates.");
      }
    }
    lastFrameNumber = OutputFrameNum.FrameNumber;

    if (frameDiff == 0)
    {
      return false;
    }
    else
    {
      freq_status_.tick();
      ros::Duration vicon_latency(msvcbridge::GetLatencyTotal().Total);

      process_subjects(now_time - vicon_latency);
      process_markers(now_time - vicon_latency, lastFrameNumber);

      lastTime = now_time;
      return true;
    }
  }