C++ cPrecisionClock::timeoutOccurred方法代码示例

  /*!
  * \brief Callback for receiving a point cloud.
  *
  * This description is displayed lower in the doxygen as an extended description along with
  * the above brief description.
  */
  void cloudCallback(const sensor_msgs::PointCloud2ConstPtr& input)
  {

//    const std::string& publisher_name = event.getPublisherName();
//    ros::M_string connection_header = event.getConnectionHeader();
//    ros::Time receipt_time = event.getReceiptTime();
//    const std::string topic = connection_header["topic"];

//    const sensor_msgs::PointCloud2& input = event.getMessage();

    std::string topic = "none";

    int cloud_size = input->width*input->height;
    ROS_DEBUG_NAMED("haptics", "Got a cloud on topic %s in frame %s with %d points!",
               topic.c_str(),
               input->header.frame_id.c_str(),
               cloud_size);

    if(cloud_size == 0) return;

    pcl::fromROSMsg(*input, last_cloud_);
    
    if(last_cloud_.header.frame_id.compare("/tool_frame"))
    {
//        ROS_INFO("Transforming cloud with %d points from %s to %s.",
//                 (int)last_cloud_.points.size(), last_cloud_.header.frame_id.c_str(),
//                 "/world");
        if(!tfl_.waitForTransform("/tool_frame", last_cloud_.header.frame_id,
                                  last_cloud_.header.stamp, ros::Duration(2.0)) )
        {
            ROS_ERROR("Couldn't get transform for cloud, returning FAILURE!");
            return;
        }
        pcl_ros::transformPointCloud("/tool_frame", last_cloud_, last_cloud_, tfl_);
    }

    //ROS_INFO("m_shape is %d", m_shape);
    //if(new_object->m_shape == hviz::Haptics_CLOUD)
    {
      boost::mutex::scoped_lock lock(mutex_);
      if(object->m_shape == hviz::Haptics_CLOUD)
        object->createFromCloud(last_cloud_);
    }

    if(!got_first_cloud_){
      got_first_cloud_ = true;
      ROS_INFO("Got first cloud!");
    }

    static bool firstTime = true;
    static int counter = 0;
    static cPrecisionClock pclock;
    float sample_period = 2.0;

    if(firstTime) // start a clock to estimate the rate
    {
        pclock.setTimeoutPeriodSeconds(sample_period);
        pclock.start(true);
        firstTime = false;
    }

    // estimate the refresh rate and publish
    ++counter;
    if (pclock.timeoutOccurred()) {
        pclock.stop();
        float rate = counter/sample_period;
        counter = 0;
        pclock.start(true);
        std_msgs::String msg;
        char status_string[256];
        sprintf(status_string, "Cloud rate: %.3f",
                rate );
        msg.data = status_string;
        pub_status_.publish(msg);
    }

  }

  /*!
  * \brief The timer callback sends graphical output to rviz.
  *
  * This description is displayed lower in the doxygen as an extended description along with
  * the above brief description.
  */
  void timerCallback()
  {
    if(!tool) return;

    ros::Time time_now = ros::Time::now(); // use single time for all output

    static bool firstTime = true;
    static int counter = 0;
    static cPrecisionClock pclock;

    if(firstTime) // start a clock to estimate the rate
    {
        pclock.setTimeoutPeriodSeconds(1.0);
        pclock.start(true);
        firstTime = false;
    }

    // estimate the refresh rate and publish
    ++counter;
    if (pclock.timeoutOccurred()) {
        pclock.stop();
        graphicRateEstimate = counter;
        counter = 0;
        pclock.start(true);
        std_msgs::String msg;
        char status_string[256];
        sprintf(status_string, "Haptic rate: %.3f Graphics Rate: %.3f",
                rateEstimate, graphicRateEstimate);
        msg.data = status_string;
        pub_status_.publish(msg);
    }

    // Transmit the visualizations of the tool/proxy
    float proxy_radius = config_.tool_radius;
    cVector3d pos =  object->m_interactionProjectedPoint; //tool->getDeviceGlobalPos();
    cVector3d HIP = tool->getDeviceGlobalPos();
    cMatrix3d tool_rotation = tool->m_deviceGlobalRot;

    if(false && device_info.m_sensedRotation)
    {
      object_manipulator::shapes::Mesh mesh;
      mesh.dims = tf::Vector3(0.5, 0.5, 0.5);
      mesh.frame.setRotation(chai_tools::cMatrixToTFQuaternion(tool_rotation));
      mesh.frame.setOrigin(tf::Vector3(pos.x, pos.y, pos.z));
      mesh.header.stamp = time_now;
      mesh.header.frame_id = "/tool_frame";
      mesh.use_embedded_materials = true;
      mesh.mesh_resource = std::string("package://pr2_description/meshes/gripper_v0/gripper_palm.dae");
//      std::string proximal_finger_string("package://pr2_description/meshes/gripper_v0/l_finger.dae");
//      std::string distal_finger_string("package://pr2_description/meshes/gripper_v0/l_finger_tip.dae");
      object_manipulator::drawMesh(pub_marker_, mesh, "gripper", 0, ros::Duration(), object_manipulator::msg::createColorMsg(1.0, 0.3, 0.7, 1.0));
    }
    else
    {
      object_manipulator::shapes::Sphere sphere;
      sphere.dims = tf::Vector3(2*proxy_radius, 2*proxy_radius, 2*proxy_radius);
      sphere.frame = tf::Transform(tf::Quaternion(0,0,0,1), tf::Vector3(pos.x, pos.y, pos.z));
      sphere.header.frame_id = "/tool_frame";
      sphere.header.stamp = time_now;
      object_manipulator::drawSphere(pub_marker_, sphere, "proxy", 0, ros::Duration(), object_manipulator::msg::createColorMsg(1.0, 0.3, 0.7, 1.0));

      //object_manipulator::shapes::Sphere sphere;
      sphere.dims = tf::Vector3(1.9*proxy_radius, 1.9*proxy_radius, 1.9*proxy_radius);
      sphere.frame = tf::Transform(tf::Quaternion(0,0,0,1), tf::Vector3(HIP.x, HIP.y, HIP.z));
      sphere.header.frame_id = "/tool_frame";
      sphere.header.stamp = time_now;
      object_manipulator::drawSphere(pub_marker_, sphere, "HIP", 0, ros::Duration(), object_manipulator::msg::createColorMsg(1.0, 0.0, 0.0, 0.6));
    }


    object_manipulator::shapes::Cylinder box;
    tf::Quaternion quat = chai_tools::cMatrixToTFQuaternion(object->tPlane->getRot());
    box.frame.setRotation(quat);
    box.frame.setOrigin(chai_tools::cVectorToTF(object->tPlane->getPos()) - 0.5*proxy_radius*box.frame.getBasis().getColumn(2));
    box.dims = tf::Vector3(5*proxy_radius, 5*proxy_radius, 0.0015);
    box.header.frame_id = "/tool_frame";
    box.header.stamp = time_now;
    if(object->m_interactionInside){
      object_manipulator::drawCylinder(pub_marker_, box, "tPlane", 0, ros::Duration(), object_manipulator::msg::createColorMsg(0.2, 0.6, 1.0, 0.8), false);
    }
    else
      object_manipulator::drawCylinder(pub_marker_, box, "tPlane", 0, ros::Duration(), object_manipulator::msg::createColorMsg(0.2, 0.6, 1.0, 0.8), true);


    boost::mutex::scoped_lock lock(mutex_);
    if(config_.publish_cloud)
    {
//      if(object->last_normals->points.size() == object->last_points->points.size())
//      {
//        visualization_msgs::Marker marker;
//        marker.header = object->last_points->header;
//        marker.ns = "cloud";
//        marker.id = 0;
//        marker.type = visualization_msgs::Marker::SPHERE_LIST; // CUBE, SPHERE, ARROW, CYLINDER
//.........这里部分代码省略.........