C++ Time::now方法代码示例

/**
*  \brief Publishes the joint angles for the visualization
*
*  \param device0 the robot and its state
*
*  \ingroup ROS
*
*/
void publish_joints(struct robot_device* device0){

    static int count=0;
    static ros::Time t1;
    static ros::Time t2;
    static ros::Duration d;

    if (count == 0){
        t1 = t1.now();
    }
    count ++;
    t2 = t2.now();
    d = t2-t1;

    if (d.toSec()<0.030)
        return;
    t1=t2;

    publish_marker(device0);

    sensor_msgs::JointState joint_state;
    //update joint_state
    joint_state.header.stamp = ros::Time::now();
    joint_state.name.resize(28);
    joint_state.position.resize(28);
//    joint_state.name.resize(14);
//    joint_state.position.resize(14);
    int left, right;
    if (device0->mech[0].type == GOLD_ARM)
    {
        left = 0;
        right = 1;
    }
    else
    {
        left = 1;
        right = 0;
    }
    //======================LEFT ARM===========================
    joint_state.name[0] ="shoulder_L";
    joint_state.position[0] = device0->mech[left].joint[0].jpos + offsets_l.shoulder_off;
    joint_state.name[1] ="elbow_L";
    joint_state.position[1] = device0->mech[left].joint[1].jpos + offsets_l.elbow_off;
    joint_state.name[2] ="insertion_L";
    joint_state.position[2] = device0->mech[left].joint[2].jpos + d4 + offsets_l.insertion_off;
    joint_state.name[3] ="tool_roll_L";
    joint_state.position[3] = device0->mech[left].joint[4].jpos - 45 * d2r + offsets_l.roll_off;
    joint_state.name[4] ="wrist_joint_L";
    joint_state.position[4] = device0->mech[left].joint[5].jpos + offsets_l.wrist_off;
    joint_state.name[5] ="grasper_joint_1_L";
    joint_state.position[5] = device0->mech[left].joint[6].jpos + offsets_l.grasp1_off;
    joint_state.name[6] ="grasper_joint_2_L";
    joint_state.position[6] = device0->mech[left].joint[7].jpos * -1 + offsets_l.grasp2_off;

    //======================RIGHT ARM===========================
    joint_state.name[7] ="shoulder_R";
    joint_state.position[7] = device0->mech[right].joint[0].jpos + offsets_r.shoulder_off;
    joint_state.name[8] ="elbow_R";
    joint_state.position[8] = device0->mech[right].joint[1].jpos + offsets_r.elbow_off;
    joint_state.name[9] ="insertion_R";
    joint_state.position[9] = device0->mech[right].joint[2].jpos + d4 + offsets_r.insertion_off;
    joint_state.name[10] ="tool_roll_R";
    joint_state.position[10] = device0->mech[right].joint[4].jpos + 45 * d2r + offsets_r.roll_off;
    joint_state.name[11] ="wrist_joint_R";
    joint_state.position[11] = device0->mech[right].joint[5].jpos * -1 + offsets_r.wrist_off;
    joint_state.name[12] ="grasper_joint_1_R";
    joint_state.position[12] = device0->mech[right].joint[6].jpos + offsets_r.grasp1_off;
    joint_state.name[13] ="grasper_joint_2_R";
    joint_state.position[13] = device0->mech[right].joint[7].jpos * -1 + offsets_r.grasp2_off;

    //======================LEFT ARM===========================

    joint_state.name[14] ="shoulder_L2";
    joint_state.position[14] = device0->mech[left].joint[0].jpos_d + offsets_l.shoulder_off;
    joint_state.name[15] ="elbow_L2";
    joint_state.position[15] = device0->mech[left].joint[1].jpos_d + offsets_l.elbow_off;
    joint_state.name[16] ="insertion_L2";
    joint_state.position[16] = device0->mech[left].joint[2].jpos_d + d4 + offsets_l.insertion_off;
    joint_state.name[17] ="tool_roll_L2";
    joint_state.position[17] = device0->mech[left].joint[4].jpos_d - 45 * d2r + offsets_l.roll_off;
    joint_state.name[18] ="wrist_joint_L2";
    joint_state.position[18] = device0->mech[left].joint[5].jpos_d + offsets_l.wrist_off;
    joint_state.name[19] ="grasper_joint_1_L2";
    joint_state.position[19] = device0->mech[left].joint[6].jpos_d + offsets_l.grasp1_off;
    joint_state.name[20] ="grasper_joint_2_L2";
    joint_state.position[20] = device0->mech[left].joint[7].jpos_d * -1 + offsets_l.grasp2_off;

    //======================RIGHT ARM===========================
    joint_state.name[21] ="shoulder_R2";
    joint_state.position[21] = device0->mech[right].joint[0].jpos_d + offsets_r.shoulder_off;
    joint_state.name[22] ="elbow_R2";
    joint_state.position[22] = device0->mech[right].joint[1].jpos_d + offsets_r.elbow_off;
//.........这里部分代码省略.........

/**
* \brief Publishes the raven_state message from the robot and currParams structures
*
*   \param dev robot device structure with the current state of the robot
*   \param currParams the parameters being passed from the interfaces
*  \ingroup ROS
*/
void publish_ravenstate_ros(struct robot_device *dev,struct param_pass *currParams){
    static int count=0;
    static raven_state msg_ravenstate;  // satic variables to minimize memory allocation calls
    static ros::Time t1;
    static ros::Time t2;
    static ros::Duration d;

    msg_ravenstate.last_seq = currParams->last_sequence;

    if (count == 0){
        t1 = t1.now();
    }
    count ++;
    t2 = t2.now();
    d = t2-t1;

//    if (d.toSec()<0.01)
//        return;

    msg_ravenstate.dt=d;
    t1=t2;

    publish_joints(dev);

    // Copy the robot state to the output datastructure.
    int numdof=8;
    int j;
    for (int i=0; i<NUM_MECH; i++){
    	j = dev->mech[i].type == GREEN_ARM ? 1 : 0;
        msg_ravenstate.type[j]    = dev->mech[j].type;
        msg_ravenstate.pos[j*3]   = dev->mech[j].pos.x;
        msg_ravenstate.pos[j*3+1] = dev->mech[j].pos.y;
        msg_ravenstate.pos[j*3+2] = dev->mech[j].pos.z;
        msg_ravenstate.pos_d[j*3]   = dev->mech[j].pos_d.x;
        msg_ravenstate.pos_d[j*3+1] = dev->mech[j].pos_d.y;
        msg_ravenstate.pos_d[j*3+2] = dev->mech[j].pos_d.z;
        msg_ravenstate.grasp_d[j] = (float)dev->mech[j].ori_d.grasp/1000;

        for (int orii=0; orii<3; orii++)
        {
            for (int orij=0; orij<3; orij++)
            {
            	msg_ravenstate.ori[j*9 + orii*3+orij] = dev->mech[j].ori.R[orii][orij];
            	msg_ravenstate.ori_d[j*9 + orii*3+orij] = dev->mech[j].ori_d.R[orii][orij];
            }
        }


        for (int m=0; m<numdof; m++){
            int jtype = dev->mech[j].joint[m].type;
            msg_ravenstate.encVals[jtype]    = dev->mech[j].joint[m].enc_val;
            msg_ravenstate.tau[jtype]        = dev->mech[j].joint[m].tau_d;
            msg_ravenstate.mpos[jtype]       = dev->mech[j].joint[m].mpos RAD2DEG;
            msg_ravenstate.jpos[jtype]       = dev->mech[j].joint[m].jpos RAD2DEG;
            msg_ravenstate.mvel[jtype]       = dev->mech[j].joint[m].mvel RAD2DEG;
            msg_ravenstate.jvel[jtype]       = dev->mech[j].joint[m].jvel RAD2DEG;
            msg_ravenstate.jpos_d[jtype]     = dev->mech[j].joint[m].jpos_d RAD2DEG;
            msg_ravenstate.mpos_d[jtype]     = dev->mech[j].joint[m].mpos_d RAD2DEG;
            msg_ravenstate.encoffsets[jtype] = dev->mech[j].joint[m].enc_offset;
            msg_ravenstate.dac_val[jtype]    = dev->mech[j].joint[m].current_cmd;
        }

        //grab jacobian velocities and forces
        float vel[6];
        float f[6];
        j = dev->mech[i].type == GREEN_ARM ? 1 : 0;
        dev->mech[j].r2_jac.get_vel(vel);
        dev->mech[j].r2_jac.get_vel(f);
        for (int k=0; k<6; k++){
        	msg_ravenstate.jac_vel[j*6+k] = vel[k];
        	msg_ravenstate.jac_f[j*6+k] = f[k];
        }
    }
//    msg_ravenstate.f_secs = d.toSec();
    msg_ravenstate.hdr.stamp = msg_ravenstate.hdr.stamp.now();
    msg_ravenstate.runlevel=currParams->runlevel;
    msg_ravenstate.sublevel=currParams->sublevel;

    // Publish the raven data to ROS
    pub_ravenstate.publish(msg_ravenstate);
}