コード例 #1
0
  void updateCallback(const ros::TimerEvent& e)
  {
    static bool got_first = false;
    latest_dt = (e.current_real - e.last_real).toSec();
    latest_jitter = (e.current_real - e.current_expected).toSec();
    max_abs_jitter = max(max_abs_jitter, fabs(latest_jitter));
    Result::Enum the_result;
    bool was_new_frame = true;

    if ((not segment_data_enabled) //
        and (pose_pub.getNumSubscribers() > 0 or markers_pub.getNumSubscribers() > 0))
    {
      the_result = MyClient.EnableSegmentData().Result;
      if (the_result != Result::Success)
      {
        ROS_ERROR("Enable segment data call failed");
      }
      else
      {
        ROS_ASSERT(MyClient.IsSegmentDataEnabled().Enabled);
        ROS_INFO("Segment data enabled");
        segment_data_enabled = true;
      }
    }

    if (segment_data_enabled)
    {
      while (was_new_frame and (the_result = MyClient.GetFrame().Result) == Result::Success)
        ;
      {
        now_time = ros::Time::now(); // try to grab as close to getting message as possible
        was_new_frame = process_frame();
        got_first = true;
      }

      if (the_result != Result::NoFrame)
      {
        ROS_ERROR_STREAM("GetFrame() returned " << (int)the_result);
      }

      if (got_first)
      {
        max_period_between_updates = max(max_period_between_updates, latest_dt);
        last_callback_duration = e.profile.last_duration.toSec();
      }
    }
    diag_updater.update();
  }
コード例 #2
0
  bool grab_vicon_pose_callback(vicon_mocap::viconGrabPose::Request& req, vicon_mocap::viconGrabPose::Response& resp)
  {
    ROS_INFO("Got request for a VICON pose");
    updateTimer.stop();
    tf::StampedTransform transform;
    //tf::Quaternion quat;

    if (not segment_data_enabled)
    {
      MyClient.EnableSegmentData();
      ROS_ASSERT(MyClient.IsSegmentDataEnabled().Enabled);
      segment_data_enabled = true;
    }

    // Gather data:
    int N = req.n_measurements;
    double accum_trans[3] = {0, 0, 0};
    double accum_quat[4] = {0, 0, 0, 0};
    Result::Enum the_result;
    int n_success = 0;
    for (int k = 0; k < N; k++)
    {
      ros::Duration(1 / vicon_capture_rate).sleep(); // Wait at least as long as vicon needs to capture the next frame
      if ((the_result = MyClient.GetFrame().Result) == Result::Success)
      {
        try
        {
          Output_GetSegmentGlobalTranslation trans = MyClient.GetSegmentGlobalTranslation(req.subject_name,
                                                                                          req.segment_name);
          Output_GetSegmentGlobalRotationQuaternion quat =
              MyClient.GetSegmentGlobalRotationQuaternion(req.subject_name, req.segment_name);
          if ((!trans.Occluded) && (!quat.Occluded))
          {
            accum_trans[0] += trans.Translation[0] / 1000;
            accum_trans[1] += trans.Translation[1] / 1000;
            accum_trans[2] += trans.Translation[2] / 1000;
            accum_quat[0] += quat.Rotation[3];
            accum_quat[1] += quat.Rotation[0];
            accum_quat[2] += quat.Rotation[1];
            accum_quat[3] += quat.Rotation[2];
            n_success++;
          }
        }
        catch (tf::TransformException ex)
        {
          ROS_ERROR("%s", ex.what());
          resp.success = false;
          return false; // TODO: should we really bail here, or just try again?
        }
      }
      else
      {
        if (the_result != Result::NoFrame)
        {
          ROS_ERROR_STREAM("GetFrame() returned " << (int)the_result);
        }
      }
    }

    ROS_INFO("Averaging %d measurements", n_success);
    // Average the data:
    double normalized_quat[4];
    double quat_norm = sqrt(
                            accum_quat[0] * accum_quat[0] + accum_quat[1] * accum_quat[1] + accum_quat[2]
                                * accum_quat[2] + accum_quat[3] * accum_quat[3]);
    for (int i = 0; i < 4; i++)
    {
      normalized_quat[i] = accum_quat[i] / quat_norm;
    }
    double normalized_vector[3];
    // Copy to inputs:
    for (int i = 0; i < 3; i++)
    {
      normalized_vector[i] = accum_trans[i] / n_success;
    }

    // copy what we used to service call response:
    resp.success = true;
    resp.pose.header.stamp = ros::Time::now();
    resp.pose.header.frame_id = tf_ref_frame_id;
    resp.pose.pose.position.x = normalized_vector[0];
    resp.pose.pose.position.y = normalized_vector[1];
    resp.pose.pose.position.z = normalized_vector[2];
    resp.pose.pose.orientation.w = normalized_quat[0];
    resp.pose.pose.orientation.x = normalized_quat[1];
    resp.pose.pose.orientation.y = normalized_quat[2];
    resp.pose.pose.orientation.z = normalized_quat[3];

    updateTimer.start();
    return true;
  }