C++ MSG::SetLength方法代码示例

int autoMoveBack(Aris::Core::MSG &msg)
{
    EGAIT cmd=GAIT_MOVE_BACK;
    Aris::Core::MSG data;
    data.SetLength(sizeof(cmd));
    data.Copy(&cmd,sizeof(cmd));
    cs.NRT_SendData(data);
    return 0;
}

int autoEndDiscover(Aris::Core::MSG &msg)
{
    EGAIT cmd=GAIT_END_DISCOVER;
    Aris::Core::MSG data;
    data.SetLength(sizeof(cmd));
    data.Copy(&cmd,sizeof(cmd));
    cs.NRT_SendData(data);
    return 0;
}

int autoTurnRight(Aris::Core::MSG &msg)
{
    EGAIT cmd=GAIT_TURN_RIGHT;
    Aris::Core::MSG data;
    data.SetLength(sizeof(cmd));
    data.Copy(&cmd,sizeof(cmd));
    cs.NRT_SendData(data);
    return 0;
}

int On_VS_ConnectionReceived(Aris::Core::CONN *pConn, const char* addr,int port)
{
    Aris::Core::MSG msg;
    msg.SetMsgID(VS_Connected);
    msg.SetLength(sizeof(port));
    msg.Copy(&port,sizeof(port));
    msg.CopyMore(addr,strlen(addr));
    PostMsg(msg);
    return 0;
}

int autoStart(Aris::Core::MSG &msg)
{

    EGAIT cmd=GAIT_HOME2START;
    Aris::Core::MSG data;

    data.SetLength(sizeof(cmd));
    data.Copy(&cmd,sizeof(cmd));
    cs.NRT_SendData(data);

    return 0;
}

int OnVisualSystemDataNeeded(Aris::Core::MSG &msg)
{
    static int i = 0;

    cout<<"Capture "<<i<<" begin!!!"<<endl;

    visionsensor.capture(&i);

    while(1)
    {
        if(i>40)
        {
            Kinect::ControlCommand = NoValidCommand;
            Aris::Core::MSG data;
            data.SetLength(1 * sizeof(int));
            memset(data.GetDataAddress(), 0, data.GetLength());
            data.Copy(&Kinect::ControlCommand, sizeof(i));
            pVisualSystem->SendData(data);
            break;
        }
        else
        {
            if(Kinect::IsCaptureEnd == true)
            {
                Kinect::IsCaptureEnd = false;
                Aris::Core::MSG data;
                data.SetLength(1 * sizeof(int));
                memset(data.GetDataAddress(), 0, data.GetLength());
                data.Copy(&Kinect::ControlCommand, sizeof(i));
                pVisualSystem->SendData(data);
                break;
            }
        }
    }
    return 0;
}

int On_CS_DataReceived(Aris::Core::CONN *pConn, Aris::Core::MSG &data)
{
    int cmd_id=data.GetMsgID();
    Aris::Core::MSG CMD;
    CMD.SetMsgID(CS_CMD_Received);
    CMD.SetLength(sizeof(int));
    CMD.Copy(&cmd_id,sizeof(int));
    
    // if the cmd has data, it should be repost together
    CMD.CopyMore(data.GetDataAddress(), data.GetLength());

    cout<<"received CMD is "<<cmd_id<<endl;

    PostMsg(CMD);
    return 0;
}

//.........这里部分代码省略.........
                machineData.motorsModes[i]=EOperationMode::OM_CYCLICVEL;
                gaitcmd[MapAbsToPhy[i]]=EGAIT::GAIT_STEPUP_MAP;
                rt_printf("STEPUP_MAP Command Get in NRT\n" );
            }
        }
        break;
    case STEPDOWN:
        if(gait.m_gaitState[MapAbsToPhy[0]]== GAIT_STOP)
        {
            for(int i=0;i<18;i++)
            {
                machineData.motorsModes[i]=EOperationMode::OM_CYCLICVEL;
                gaitcmd[MapAbsToPhy[i]]=EGAIT::GAIT_STEPDOWN_MAP;
                rt_printf("STEPDOWN_MAP Command Get in NRT\n" );
            }
        }
        break;
    default:
        break;
    }

    gait.RunGait(gaitcmd,machineData);

    if(gait.IsGaitFinished())
    {
        if(CGait::IsMove == true&&CGait::IsMoveEnd == true)
        {
            CGait::IsMove = false;
            CGait::IsMoveEnd = false;
            cout<<"MOVE FINISHED!"<<endl;
            Vision_Msg visioncmd = Vision_UpperControl;
            Aris::Core::MSG visionmsg;
            visionmsg.SetMsgID(VS_Capture);
            visionmsg.SetLength(sizeof(visioncmd));
            visionmsg.Copy(&visioncmd, sizeof(visioncmd));
            PostMsg(visionmsg);
        }

        if(CGait::IsTurn == true&&CGait::IsTurnEnd == true)
        {
            CGait::IsTurn = false;
            CGait::IsTurnEnd = false;
            cout<<"TURN FINISHED!"<<endl;
            Vision_Msg visioncmd = Vision_UpperControl;
            Aris::Core::MSG visionmsg;
            visionmsg.SetMsgID(VS_Capture);
            visionmsg.SetLength(sizeof(visioncmd));
            visionmsg.Copy(&visioncmd, sizeof(visioncmd));
            PostMsg(visionmsg);
        }

        if(CGait::IsBeginDiscoverStart == true && CGait::IsBeginDiscoverEnd == true)
        {
            cout<<"BEGINDISCOVER FINISHED!"<<endl;
            Vision_Msg visioncmd = Vision_StepUp;
            Aris::Core::MSG visionmsg;
            visionmsg.SetMsgID(VS_Capture);
            visionmsg.SetLength(sizeof(visioncmd));
            visionmsg.Copy(&visioncmd, sizeof(visioncmd));
            PostMsg(visionmsg);
            CGait::IsBeginDiscoverStart = false;
            CGait::IsBeginDiscoverStart = false;
        }

        if(CGait::IsEndDiscoverStart == true && CGait::IsEndDiscoverEnd == true)
        {

int On_VS_DataReceived(Aris::Core::CONN *pConn, Aris::Core::MSG &data)
{

    cout<<"Calibration Singnal Recieved"<<endl;
    int num;
    memcpy(&num,data.GetDataAddress(),data.GetLength());
    //stop num >= 54
    if (num >= 62 )
    {
        cout<<"Calibration finished"<<endl;
        CGait::IsAutoCalibrateRegistered = false;
    }
    else
    {
        ifstream inf;
        stringstream filenum;
        string filename;
        filenum<<(num-1);
        filename = "./calibration_txt/"+filenum.str() + "F.txt";
        inf.open(filename);
        int Line_Num;
        double temp;
        for(int i=0;i<GAIT_AUTOCALIB_LEN/2;i++)
        {
            inf>>Line_Num;
            for(int j=0;j<GAIT_WIDTH;j++)
            {
                inf>>temp;
                CGait::GaitAutoCalib[i][j]=-(int)temp;
            }
        }

        inf.close();
        filenum.str("");
        filename.clear();

        filenum<<num;
        filename = "./calibration_txt/"+filenum.str()+".txt";
        inf.open(filename);
        for(int i=GAIT_AUTOCALIB_LEN/2;i<GAIT_AUTOCALIB_LEN;i++)
        {
            inf>>Line_Num;
            for(int j=0;j<GAIT_WIDTH;j++)
            {
                inf>>temp;
                CGait::GaitAutoCalib[i][j]=-(int)temp;
            }
        }
        inf.close();

        cout<<"Calibration "<<num<<" begin"<<endl;
        cout<<CGait::GaitAutoCalib[3999][0]<<endl;
        EGAIT cmd=GAIT_AUTO_CALIBRATION;
        Aris::Core::MSG data;
        data.SetLength(sizeof(cmd));
        data.Copy(&cmd,sizeof(cmd));
        cout<<"Send Message to CS"<<endl;
        cs.NRT_SendData(data);
    }
    return 0;
}

int OnGetControlCommand(MSG &msg)
{
    int CommandID;
    msg.Paste(&CommandID,sizeof(int));
    Aris::Core::MSG data;

    switch(CommandID)
    {
    case 1:
        data.SetMsgID(POWEROFF);
        cs.NRT_PostMsg(data);
        break;
    case 2:
        data.SetMsgID(STOP);
        cs.NRT_PostMsg(data);
        break;
    case 3:
        data.SetMsgID(ENABLE);
        cs.NRT_PostMsg(data);
        break;
    case 4:
        data.SetMsgID(RUNNING);
        cs.NRT_PostMsg(data);
        break;
    case 5:
        data.SetMsgID(GOHOME_1);
        cs.NRT_PostMsg(data);
        break;
    case 6:
        data.SetMsgID(GOHOME_2);
        cs.NRT_PostMsg(data);
        break;
    case 7:
        data.SetMsgID(HOME2START_1);
        cs.NRT_PostMsg(data);
        break;
    case 8:
        data.SetMsgID(HOME2START_2);
        cs.NRT_PostMsg(data);
        break;
    case 9:
        data.SetMsgID(FORWARD);
        cs.NRT_PostMsg(data);
        break;
    case 10:
        data.SetMsgID(BACKWARD);
        cs.NRT_PostMsg(data);
        break;
    case 11:
        data.SetMsgID(TURNLEFT);
        cs.NRT_PostMsg(data);
        break;
    case 12:
        data.SetMsgID(TURNRIGHT);
        cs.NRT_PostMsg(data);
        break;
    case 13:
        data.SetMsgID(LEGUP);
        cs.NRT_PostMsg(data);
        break;
    case 14:
    {
        CGait::IsVisionWalk = true;
        Vision_Msg visioncmd = Vision_UpperControl;
        Aris::Core::MSG visionmsg;
        visionmsg.SetMsgID(VS_Capture);
        visionmsg.SetLength(sizeof(visioncmd));
        visionmsg.Copy(&visioncmd, sizeof(visioncmd));
        PostMsg(visionmsg);
    }
        break;
    case 15:
    {
        CGait::IsWalkAvoidRegistered = true;
        Vision_Msg visioncmd = Walk_Avoid;
        Aris::Core::MSG visionmsg;
        visionmsg.SetMsgID(VS_Capture);
        visionmsg.SetLength(sizeof(visioncmd));
        visionmsg.Copy(&visioncmd, sizeof(visioncmd));
        PostMsg(visionmsg);
    }
        break;
    default:
        cout<<"Do Not Get Validate CMD"<<endl;
        break;
    }
    return CommandID;
}

int On_VS_DataReceived(Aris::Core::CONN *pConn, Aris::Core::MSG &data)
{
    cout<<"receive data from visual system"<<endl;

    switch (data.GetMsgID())
    {
    case 34:
    {
        cout<<"MOVE!"<<endl;
        CGait::IsMove = true;
        double *move_data = new double [data.GetLength()/sizeof(double)];
        memcpy(move_data, data.GetDataAddress(), data.GetLength());
        cout<<move_data[0]<<" "<<move_data[1]<<" "<<move_data[2]<<endl;
        HexIII.RobotMove(move_data, *Gait_Move_Map);
        for(int j=0;j<GAIT_MOVE_MAP_LEN;j++)
        {
            for(int i=0;i<GAIT_WIDTH;i++)
            {
                CGait::GaitMoveMap[j][i]=-(int)(Gait_Move_Map[j][i]);
            }
        }
        Aris::Core::MSG controlcmd;
        controlcmd.SetMsgID(MOVE);
        cout<<"Send MOVE Message to CS"<<endl;
        cs.NRT_PostMsg(controlcmd);
    }
        break;
    case 35:
    {
        cout<<"TURN!"<<endl;
        CGait::IsTurn = true;
        double *turn_data = new double [data.GetLength()/sizeof(double)];
        memcpy(turn_data, data.GetDataAddress(), data.GetLength());
        *turn_data = *turn_data*180/M_PI;
        cout<<*turn_data<<endl;
        HexIII.RobotTurn(turn_data, *Gait_Turn_Map);
        for(int j=0;j<GAIT_TURN_MAP_LEN;j++)
        {
            for(int i=0;i<GAIT_WIDTH;i++)
            {
                CGait::GaitTurnMap[j][i]=-(int)(Gait_Turn_Map[j][i]);
            }
        }
        Aris::Core::MSG controlcmd;
        controlcmd.SetMsgID(TURN);
        cout<<"Send TURN Message to CS"<<endl;
        cs.NRT_PostMsg(controlcmd);
    }
        break;
    case 36:
    {
        cout<<"STEP UP!"<<endl;
        CGait::IsStepUp = true;
        double *map = new double [data.GetLength()/sizeof(double)];
        memcpy(map, data.GetDataAddress(), data.GetLength());

        static double StepUP_currentH[6] = {-1.05, -1.05, -1.05, -1.05, -1.05, -1.05};
        static double StepUP_nextH[6];

        memcpy(StepUP_nextH,map,sizeof(StepUP_nextH));

        if(StepUP_currentH[0] == -0.85&&StepUP_currentH[1] == -0.85&&StepUP_currentH[2] == -0.85
                &&StepUP_currentH[3] == -0.85&&StepUP_currentH[4] == -0.85&&StepUP_currentH[5] == -0.85
                &&StepUP_nextH[0] == -0.85&&StepUP_nextH[1] == -0.85&&StepUP_nextH[2] == -0.85
                &&StepUP_nextH[3] == -0.85&&StepUP_nextH[4] == -0.85&&StepUP_nextH[5] == -0.85)
        {
            cout<<"STEPUP FINISHED"<<endl;
            CGait::IsStepUp = false;
            delete[] map;
            Vision_Msg visioncmd = Vision_UpperControl;
            Aris::Core::MSG visionmsg;
            visionmsg.SetMsgID(VS_Capture);
            visionmsg.SetLength(sizeof(visioncmd));
            visionmsg.Copy(&visioncmd, sizeof(visioncmd));
            PostMsg(visionmsg);
        }
        else
        {
            double StepUpLen = 0.325;

            HexIII.RobotStepUp(&StepUpLen,StepUP_currentH,StepUP_nextH,*Gait_StepUp__Map);

            memcpy(StepUP_currentH,StepUP_nextH,sizeof(StepUP_nextH));

            delete[] map;

            for(int j=0;j<GAIT_STEPUPANDDOWN_LEN;j++)
            {
                for(int i=0;i<GAIT_WIDTH;i++)
                {
                    CGait::GaitStepUpMap[j][i]=-(int)(Gait_StepUp__Map[j][i]);
                }
            }

            Aris::Core::MSG controldata;
            controldata.SetMsgID(STEPUP);
            cs.NRT_PostMsg(controldata);
        }
    }
        break;
//.........这里部分代码省略.........