C++ Vector::clear方法代码示例

bool YarpJointDev::getDOF ( yarp::sig::Vector& curDof ) {

    curDof.clear();

    for ( unsigned i = 0; i < _chain->GetNumberOfJoints(); ++i )
        curDof.push_back ( 1 );

    return true;

}

bool IndexFinger::getPositionHandFrame(yarp::sig::Vector &position, yarp::sig::Vector &fingerEncoders){

    bool ret = true;
    Vector joints;

    int nEncs;

    position.clear();
    position.resize(3); //x,y, z position


    ret = ret && _armEncoder->getAxes(&nEncs);
    Vector encs(nEncs);
    if(! (ret = ret && _armEncoder->getEncoders(encs.data())))
    {
        cerr << _dbgtag << "Failed to read arm encoder data" << endl;
    }


    //cout << encs.toString() << endl;

    ret = ret && _iCubFinger->getChainJoints(encs, joints);


    if(ret == false){
        cout << "failed to get chain joints" << endl;
        return false;
    }


    // Replace the joins with the encoder readings
    joints[0] = 20; // The index fingertip calibration procedure used this value.
    joints[1] = 90 * (1 - (fingerEncoders[0] - _minProximal) / (_maxProximal - _minProximal) );
    joints[2] = 90 * (1 - (fingerEncoders[1] - _minMiddle) / (_maxMiddle - _minMiddle) );
    joints[3] = 90 * (1 - (fingerEncoders[2] - _minDistal) / (_maxDistal - _minDistal) );

    //cout << joints.size() << endl;
    //Convert the joints to radians.
    for (int j = 0; j < joints.size(); j++)
        joints[j] *= DEG2RAD;


    yarp::sig::Matrix tipFrame = _iCubFinger->getH(joints);
    position = tipFrame.getCol(3); // Tip's position in the hand coordinate


    return ret;
}

void YarpJointDev::PosEulerSingle_To_PosAxisAngle ( const std::vector< float >& pos,
        yarp::sig::Vector& xd,
        yarp::sig::Vector& od ) {

    assert ( pos.size() == 6 );

    xd.clear();
    od.clear();

    xd.push_back ( pos[0] );
    xd.push_back ( pos[1] );
    xd.push_back ( pos[2] );

    float xa, ya, za, theta;

    Euler_To_AxisAngle ( pos[3], pos[4], pos[5], xa, ya, za, theta );

    od.push_back ( xa );
    od.push_back ( ya );
    od.push_back ( za );
    od.push_back ( theta );

}

bool JoypadControlClient::getRawTouch(unsigned int touch_id, yarp::sig::Vector& value)
{
    value.clear();
    if(m_rpc_only)
    {
        Bottle cmd, response;

        cmd.addVocab(VOCAB_IJOYPADCTRL);
        cmd.addVocab(VOCAB_GET);
        cmd.addVocab(VOCAB_TOUCH);
        cmd.addVocab(VOCAB_VALUE);
        cmd.addInt32(touch_id);
        m_rpcPort.write(cmd, response);
        if(response.get(0).asVocab() == VOCAB_OK && response .get(1).isFloat64() && response.get(2).isFloat64())
        {
            value.push_back(response.get(1).asFloat64());
            value.push_back(response.get(2).asFloat64());
            return true;
        }
        else
        {
            return false;
        }
    }
    else
    {
        LockGuard l(m_touchPort.mutex);
        if(touch_id < m_touchPort.storage.size()/2)
        {
            value.push_back(m_touchPort.storage[touch_id * 2]);
            value.push_back(m_touchPort.storage[touch_id * 2 + 1]);
            return true;
        }
        else
        {
            return false;
        }
    }
}

bool JoypadControlClient::getRawStick(unsigned int stick_id, yarp::sig::Vector& value, JoypadCtrl_coordinateMode coordinate_mode)
{
    value.clear();
    if(m_rpc_only || coordinate_mode == IJoypadController::JypCtrlcoord_POLAR)
    {
        Bottle cmd, response;
        int    dof, coordmode;

        coordmode = coordinate_mode == IJoypadController::JypCtrlcoord_CARTESIAN ? VOCAB_CARTESIAN : VOCAB_POLAR;
        cmd.addVocab(VOCAB_IJOYPADCTRL);
        cmd.addVocab(VOCAB_GET);
        cmd.addVocab(VOCAB_STICKDOF);
        cmd.addVocab(VOCAB_VALUE);
        cmd.addInt32(stick_id);
        m_rpcPort.write(cmd, response);

        if(response.get(0).asVocab() == VOCAB_OK && response.get(1).isInt32())
        {
            dof = response.get(1).asInt32();
        }
        else
        {
            return false;
        }
        cmd.addVocab(VOCAB_IJOYPADCTRL);
        cmd.addVocab(VOCAB_GET);
        cmd.addVocab(VOCAB_STICK);
        cmd.addVocab(VOCAB_VALUE);
        cmd.addVocab(coordmode);
        cmd.addInt32(stick_id);
        m_rpcPort.write(cmd, response);
        if(response.get(0).asVocab() == VOCAB_OK)
        {
            for(int i = 0; i < dof; i++)
            {
                if(response.get(i).isFloat64())
                {
                    value.push_back(response.get(i).asFloat64());
                }
                else
                {
                    return false;
                }
            }
            return true;
        }
        else
        {
            return false;
        }
    }
    else
    {
        LockGuard l(m_stickPort.mutex);
        int offset = 0;

        unsigned int i;
        if(getStickCount(i), stick_id >= i)
        {
            yError() << "JoypadControlCLient: GetStick() error.. Stick_id out of bound";
            return false;
        }
        for(size_t j = 0; j < stick_id; j++)
        {
            offset += m_stickDof[j];
        }

        for(size_t i = 0; i < m_stickDof[stick_id]; ++i)
        {
            value.push_back(m_stickPort.storage[offset + i]);
        }

        return true;
    }
}