void process_subjects(const ros::Time& frame_time)
  {
    if (pose_pub.getNumSubscribers() > 0)
    {

      // We know the subject and segment names a priori, so don't bother enumerating, just grab the data:
      // Flyer:
      Output_GetSegmentGlobalTranslation trans = MyClient.GetSegmentGlobalTranslation(SubjectName, SegmentName);
      Output_GetSegmentGlobalRotationQuaternion quat = MyClient.GetSegmentGlobalRotationQuaternion(SubjectName,
                                                                                                   SegmentName);
      if (trans.Result == Result::Success and quat.Result == Result::Success)
      {
        if ((not trans.Occluded) and (not quat.Occluded))
        {
          flyer_transform.setOrigin(
                                    tf::Vector3(trans.Translation[0] / 1000, trans.Translation[1] / 1000,
                                                trans.Translation[2] / 1000));
          flyer_transform.setRotation(
                                      tf::Quaternion(quat.Rotation[0], quat.Rotation[1], quat.Rotation[2],
                                                     quat.Rotation[3]));
          //ros::Time thisTime = now_time - ros::Duration(latencyInMs / 1000);
          tf::StampedTransform transform = tf::StampedTransform(flyer_transform, frame_time, tf_ref_frame_id,
                                                                tf_tracked_frame_id);
          if (enable_tf_broadcast)
            tf_broadcast.sendTransform(transform);

          geometry_msgs::TransformStamped pose_msg;
          tf::transformStampedTFToMsg(transform, pose_msg);
          pose_pub.publish(pose_msg);

        }
        else
        {
          ROS_WARN_STREAM("occluded, not publishing...");
        }
      }
      else
      {
        ROS_WARN("GetSegmentGlobalTranslation/Rotation failed (result = %d, %d), not publishing...",
                 (int)(trans.Result), (int)(quat.Result));
      }
    }
  }
  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;
  }
Ejemplo n.º 3
0
// mocap position thread
void DataStreamClient::run(void)
{

  while (true) //TODO: exit cleanly?
  {

    // get frame
    while (_vicon_client.GetFrame().Result != Result::Success) {
      dbg("Couldn't GetFrame()\n");
      usleep(1000);
    }

    // get timecode and populate subject message
    //    Output_GetTimecode time_code = _vicon_client.GetTimecode();
    vicon::body_t msg;
    msg.utime = _timestamp_now();

    vicon::vicon_t system;

    // get subject count
    system.nummodels = _vicon_client.GetSubjectCount().SubjectCount;

    // Uh oh, models disappeared - realloc the system
    if (system.nummodels < system.models.size())
    {
        system.models.clear();
        system.models.reserve(system.nummodels);
    }

    if (system.nummodels == 0)
    {
        std::cout << "no models detected" << std::endl;
        continue;
    }

    // loop through subjects
    for (uint subject_index = 0; subject_index < system.nummodels; subject_index++) {
      ////////////////////////////////////////////////////////////////////////
      /// Marker
      vicon::model_t& model = findModel(system.models, _vicon_client.GetSubjectName(subject_index).SubjectName);

      model.nummarkers = _vicon_client.GetMarkerCount(model.name).MarkerCount;
      if (model.nummarkers < model.markers.size())
      {
        model.markers.clear();
        model.markers.reserve(model.nummarkers);
      }

      for(int32_t j = 0; j < model.nummarkers; j++)
      {
        vicon::marker_t& marker = findMarker(model.markers, _vicon_client.GetMarkerName(model.name, j).MarkerName);
        Output_GetMarkerGlobalTranslation translation = _vicon_client.GetMarkerGlobalTranslation(model.name, marker.name);
        marker.o = translation.Occluded;
        memcpy(marker.xyz, translation.Translation, 3*sizeof(double));
      }

      ////////////////////////////////////////////////////////////////////////
      /// Segments

      // get number of segments
      model.numsegments = _vicon_client.GetSegmentCount(model.name).SegmentCount;
      if (model.numsegments < model.segments.size())
      {
          model.segments.clear();
          model.segments.reserve(model.numsegments);
      }

      // get subject name
      std::string subject_name = _vicon_client.GetSubjectName(subject_index).SubjectName;

      // get root segment name
      std::string root_segment_name = _vicon_client.GetSubjectRootSegmentName(subject_name).SegmentName;

      // loop through segments
      for (uint segment_index = 0; segment_index < model.numsegments; segment_index++) {
        // get segment name
        std::string segment_name = _vicon_client.GetSegmentName(subject_name, segment_index).SegmentName;

        vicon::segment_t& segment = findSegment(model.segments, segment_name);
        Output_GetSegmentGlobalRotationEulerXYZ A = _vicon_client.GetSegmentGlobalRotationEulerXYZ(model.name, segment.name);
        Output_GetSegmentGlobalTranslation T = _vicon_client.GetSegmentGlobalTranslation(model.name, segment.name);
        Output_GetSegmentLocalRotationEulerXYZ ba = _vicon_client.GetSegmentLocalRotationEulerXYZ(model.name, segment.name);
        Output_GetSegmentLocalTranslation bt = _vicon_client.GetSegmentLocalTranslation(model.name, segment.name);
        memcpy(segment.A, A.Rotation, 3*sizeof(double));
        memcpy(segment.T, T.Translation, 3*sizeof(double));
        memcpy(segment.ba, ba.Rotation, 3*sizeof(double));
        memcpy(segment.bt, bt.Translation, 3*sizeof(double));

        // check if root segment
        if (segment_name == root_segment_name) { //TODO: handle articulated bodies
          // get segment translation
          // Output_GetSegmentStaticTranslation segment_translation = _vicon_client.GetSegmentStaticTranslation(subject_name, segment_name);
          Output_GetSegmentGlobalTranslation segment_translation = _vicon_client.GetSegmentGlobalTranslation(
              subject_name, segment_name);

          // get segment rotation
          //Output_GetSegmentStaticRotationQuaternion segment_rotation = _vicon_client.GetSegmentStaticRotationQuaternion(subject_name, segment_name);
          Output_GetSegmentGlobalRotationQuaternion segment_rotation =
              _vicon_client.GetSegmentGlobalRotationQuaternion(subject_name, segment_name);
          //           Output_GetSegmentGlobalRotationEulerXYZ   segment_rotation = _vicon_client.GetSegmentGlobalRotationEulerXYZ(subject_name, segment_name);

          // populate message with position
          for (int i = 0; i < 3; i++)
            msg.trans[i] = segment_translation.Translation[i]/1000.0; //vicon data is in mm
	  msg.quat[0] = segment_rotation.Rotation[3]; //vicon is x,y,z,w
	  msg.quat[1] = segment_rotation.Rotation[0];
	  msg.quat[2] = segment_rotation.Rotation[1];
	  msg.quat[3] = segment_rotation.Rotation[2];


          std::string channel = "VICON_" + subject_name;
          _lcm.publish(channel.c_str(), &msg);

          // break from segment for loop
          break;
        } // root segment
      } // for each segment (model.numsegments)
    } // for each subject (system.nummodels)

    _lcm.publish("VICON_MARKERS", &system);

  }

  // disconnect client
  dbg("Subject position thread terminated properly, disconnecting client\n");
  _vicon_client.Disconnect();
}