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

void updateHaptics(void)
{
    // reset clock
    simClock.reset();

    // main haptic simulation loop
    while(simulationRunning)
    {
        // stop the simulation clock
        simClock.stop();

        // read the time increment in seconds
        double timeInterval = simClock.getCurrentTimeSeconds();
        if (timeInterval > 0.001) { timeInterval = 0.001; }

        // restart the simulation clock
        simClock.reset();
        simClock.start();

        // read position from haptic device
        cVector3d pos;
        hapticDevice->getPosition(pos);
        pos.mul(workspaceScaleFactor);
        device->setPos(pos);

        // init temp variable
        cVector3d force;
        force.zero();

        // compute reaction forces
        for (int y=0; y<10; y++)
        {
            for (int x=0; x<10; x++)
            {
               cVector3d nodePos = nodes[x][y]->m_pos;
               cVector3d f = computeForce(pos, deviceRadius, nodePos, radius, stiffness);
               cVector3d tmpfrc = cNegate(f);
               nodes[x][y]->setExternalForce(tmpfrc);
               force.add(f);
            }
        }

        // integrate dynamics
        defWorld->updateDynamics(timeInterval);

        // scale force
        force.mul(deviceForceScale);

        // send forces to haptic device
        hapticDevice->setForce(force);
    }

    // exit haptics thread
    simulationFinished = true;
}

//.........这里部分代码省略.........
            if (nextItem->m_useSkeletonModel)
            {
                list<cGELSkeletonNode*>::iterator i;
                for(i = nextItem->m_nodes.begin(); i != nextItem->m_nodes.end(); ++i)
                {
                    cGELSkeletonNode* node = *i;
                    cVector3d nodePos = node->m_pos;
                    double radius = node->m_radius;
                    cVector3d force = cVector3d(-0.01 * nodePos.x, -0.01 * (nodePos.y), 0.0);
                    if ((nodePos.z-radius) < level)
                    {
                        double depth = (nodePos.z-radius) - level;
                        force.add(cVector3d(0,0,-1.0 * depth));
                        node->m_vel.mul(0.95);
                    }
                    node->setExternalForce(force);
                }
            }
        }


        // compute haptic feedback
		for(i = defWorld->m_gelMeshes.begin(); i != defWorld->m_gelMeshes.end(); ++i)
		{
			cGELMesh *nextItem = *i;

			if (nextItem->m_useMassParticleModel)
			{
				int numVertices = nextItem->m_gelVertices.size();
				for (int i=0; i<numVertices; i++)
				{
				cVector3d nodePos = nextItem->m_gelVertices[i].m_massParticle->m_pos;
				cVector3d f = computeForce(pos, deviceRadius, nodePos, radius, stiffness);
				if (f.lengthsq() > 0)
				{
					cVector3d tmpfrc = cNegate(f);
					nextItem->m_gelVertices[i].m_massParticle->setExternalForce(tmpfrc);
				}
				force.add(cMul(1.0, f));
				}
			}

			if (nextItem->m_useSkeletonModel)
			{
				list<cGELSkeletonNode*>::iterator i;
				for(i = nextItem->m_nodes.begin(); i != nextItem->m_nodes.end(); ++i)
				{
					cGELSkeletonNode* node = *i;
					cVector3d nodePos = node->m_pos;
					double radius = node->m_radius;
					cVector3d f = computeForce(pos, deviceRadius, nodePos, radius, stiffness);
					if (f.lengthsq() > 0)
					{
						cVector3d tmpfrc = cNegate(f);
						node->setExternalForce(tmpfrc);
					}
					force.add(cMul(4.0, f));
				}
			}
		}

        // integrate dynamics
		double interval = simClock.stop();
		simClock.reset();
		simClock.start();

        if (interval > 0.001) { interval = 0.001; }
        defWorld->updateDynamics(interval);

        // scale force
        force.mul(0.6 * deviceForceScale);

        // water viscosity
        if ((pos.z - deviceRadius) < level)
        {
            // read damping properties of haptic device
            cHapticDeviceInfo info = hapticDevice->getSpecifications();
            double Kv = 0.8 * info.m_maxLinearDamping;

            // read device velocity
            cVector3d linearVelocity;
            hapticDevice->getLinearVelocity(linearVelocity);

            // compute a scale factor [0,1] proportional to percentage
            // of tool volume immersed in the water
            double val = (level - (pos.z - deviceRadius)) / (2.0 * deviceRadius);
            double scale = cClamp(val, 0.1, 1.0);

            // compute force
            cVector3d forceDamping = cMul(-Kv * scale, linearVelocity);
            force.add(forceDamping);
        }

        // send forces to haptic device
        hapticDevice->setForce(force);
    }

    // exit haptics thread
    simulationFinished = true;
}

void updateHaptics(void)
{
    // reset clock
    simClock.reset();

    // main haptic simulation loop
    while(simulationRunning)
    {
        // compute global reference frames for each object
        world->computeGlobalPositions(true);

        // update position and orientation of tool
        tool->updatePose();

        // compute interaction forces
        tool->computeInteractionForces();

        // send forces to device
        tool->applyForces();

        // stop the simulation clock
        simClock.stop();

        // read the time increment in seconds
        double timeInterval = simClock.getCurrentTimeSeconds();

        // restart the simulation clock
        simClock.reset();
        simClock.start();

        // temp variable to compute rotational acceleration
        cVector3d rotAcc(0,0,0);

        // check if tool is touching an object
        cGenericObject* objectContact = tool->m_proxyPointForceModel->m_contactPoint0->m_object;
        if (objectContact != NULL)
        {
            // retrieve the root of the object mesh
            cGenericObject* obj = objectContact->getSuperParent();

            // get position of cursor in global coordinates
            cVector3d toolPos = tool->m_deviceGlobalPos;

            // get position of object in global coordinates
            cVector3d objectPos = obj->getGlobalPos();

            // compute a vector from the center of mass of the object (point of rotation) to the tool
            cVector3d vObjectCMToTool = cSub(toolPos, objectPos);

            // compute acceleration based on the interaction forces
            // between the tool and the object
            if (vObjectCMToTool.length() > 0.0)
            {
                // get the last force applied to the cursor in global coordinates
                // we negate the result to obtain the opposite force that is applied on the
                // object
                cVector3d toolForce = cNegate(tool->m_lastComputedGlobalForce);

                // compute effective force to take into account the fact the object
                // can only rotate around a its center mass and not translate
                cVector3d effectiveForce = toolForce - cProject(toolForce, vObjectCMToTool);

                // compute the resulting torque
                cVector3d torque = cMul(vObjectCMToTool.length(), cCross( cNormalize(vObjectCMToTool), effectiveForce));

                // update rotational acceleration
                const double OBJECT_INERTIA = 0.4;
                rotAcc = (1.0 / OBJECT_INERTIA) * torque;
            }
        }

        // update rotational velocity
        rotVel.add(timeInterval * rotAcc);

        // set a threshold on the rotational velocity term
        const double ROT_VEL_MAX = 10.0;
        double velMag = rotVel.length();
        if (velMag > ROT_VEL_MAX)
        {
            rotVel.mul(ROT_VEL_MAX / velMag);
        }

        // add some damping too
        const double DAMPING_GAIN = 0.1;
        rotVel.mul(1.0 - DAMPING_GAIN * timeInterval);

        // if user switch is pressed, set velocity to zero
        if (tool->getUserSwitch(0) == 1)
        {
            rotVel.zero();
        }

        // compute the next rotation configuration of the object
        if (rotVel.length() > CHAI_SMALL)
        {
            object->rotate(cNormalize(rotVel), timeInterval * rotVel.length());
        }
    }
    
    // exit haptics thread
//.........这里部分代码省略.........

  /*!
  * \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
//.........这里部分代码省略.........

void hapticsLoop(void* a_pUserData)
{
    // read the position of the haptic device
    cursor->updatePose();

    // compute forces between the cursor and the environment
    cursor->computeForces();

    // stop the simulation clock
    g_clock.stop();

    // read the time increment in seconds
    double increment = g_clock.getCurrentTime() / 1000000.0;

    // restart the simulation clock
    g_clock.initialize();
    g_clock.start();

    // get position of cursor in global coordinates
    cVector3d cursorPos = cursor->m_deviceGlobalPos;

    // compute velocity of cursor;
    timeCounter = timeCounter + increment;
    if (timeCounter > 0.01)
    {
        cursorVel = (cursorPos - lastCursorPos) / timeCounter;
        lastCursorPos = cursorPos;
        timeCounter = 0;
    }

    // get position of torus in global coordinates
    cVector3d objectPos = object->getGlobalPos();

    // compute the velocity of the sphere at the contact point
    cVector3d contactVel = cVector3d(0.0, 0.0, 0.0);
    if (rotVelocity.length() > CHAI_SMALL)
    {
        cVector3d projection = cProjectPointOnLine(cursorPos, objectPos, rotVelocity);
        cVector3d vpc = cursorPos - projection;
        if (vpc.length() > CHAI_SMALL)
        {
            contactVel = vpc.length() * rotVelocity.length() * cNormalize(cCross(rotVelocity, vpc));
        }
    }

    // get the last force applied to the cursor in global coordinates
    cVector3d cursorForce = cursor->m_lastComputedGlobalForce;

    // compute friction force
    cVector3d friction = -40.0 * cursorForce.length() * cProjectPointOnPlane((cursorVel - contactVel), cVector3d(0.0, 0.0, 0.0), (cursorPos - objectPos));

    // add friction force to cursor
    cursor->m_lastComputedGlobalForce.add(friction);

    // update rotational velocity
    if (friction.length() > CHAI_SMALL)
    {
        rotVelocity.add( cMul(-10.0 * increment, cCross(cSub(cursorPos, objectPos), friction)));
    }

    // add some damping...
    //rotVelocity.mul(1.0 - increment);
    
    // compute the next rotation of the torus
    if (rotVelocity.length() > CHAI_SMALL)
    {
        object->rotate(cNormalize(rotVelocity), increment * rotVelocity.length());
    }

    // send forces to haptic device
    cursor->applyForces();
}