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

    double *map = new double [data.GetLength()/sizeof(double)];
    memcpy(map, data.GetDataAddress(), data.GetLength()  );

    static double currentH[6],nextH[6];

    memcpy(nextH,map,sizeof(nextH));

    HexIII.MotionPlanWithKinect(currentH,nextH,*Gait_Calculated_From_Map);

    memcpy(currentH,nextH,sizeof(nextH));

    delete[] map;

    for(int j=0;j<GAIT_ADAPTIVEWALK_LEN;j++)
    {
        for(int i=0;i<GAIT_WIDTH;i++)
        {
            CGait::GaitAdaptiveWalk[j][i]=-(int)(Gait_Calculated_From_Map[j][i]);
        }
    }

    Aris::Core::MSG controldata;
    controldata.SetMsgID(WALKADAPTIVE);
    cs.NRT_PostMsg(controldata);

    return 0;
}
Ejemplo n.º 2
0
Aris::Core::MSG parseSwing(const std::string &cmd, const map<std::string, std::string> &params)
{
    SWING_PARAM  param;

    for(auto &i:params)
    {
        if(i.first=="y")
        {
            param.centreP[1]=stod(i.second);
        }
        else if(i.first=="z")
        {
            param.centreP[2]=stod(i.second);
        }
        else if(i.first=="deg")
        {
            param.swingRad=stod(i.second)/180*PI;//计算身体摆动的弧度
        }
        else
        {
            std::cout<<"parse failed"<<std::endl;
            return MSG{};
        }
    }

    param.periodCount=3000;

    Aris::Core::MSG msg;
    msg.CopyStruct(param);

    std::cout<<"finished parse"<<std::endl;

    return msg;
}
Ejemplo n.º 3
0
Aris::Core::MSG parsePre2Shv(const std::string &cmd, const map<std::string, std::string> &params)
{
    PRE2SHV_PARAM  param;

    for(auto &i:params)
    {
        if(i.first=="distance")
        {
            param.distance=stod(i.second);
        }
        else if(i.first=="height")
        {
            param.height=stod(i.second);
        }
        else
        {
            std::cout<<"parse failed"<<std::endl;
            return MSG{};
        }
    }

    param.periodCount[0]=3000;
    param.periodCount[1]=3000;

    Aris::Core::MSG msg;
    msg.CopyStruct(param);

    std::cout<<"finished parse"<<std::endl;

    return msg;
}
Ejemplo n.º 4
0
int On_VS_Connected(Aris::Core::MSG &msg)
{
    cout<<"Received Connection from Vision System:"<<endl;
    cout<<"   Remote IP is: "<<msg.GetDataAddress()+sizeof(int)<<endl;
    cout<<"   Port is     : "<<*((int*)msg.GetDataAddress()) << endl << endl;

    return 0;
}
Ejemplo n.º 5
0
int show(const Aris::Core::MSG &msg)
{
	cout << "Msg Length:" << msg.GetLength()<<endl;
	cout << "Msg MsgID :" << msg.GetMsgID()<<endl;
	//cout << "Msg Type  :" << msg.GetType()<<endl;
	cout << "Msg Data  :" << msg.GetDataAddress()<<endl<<endl;

	return 0;
}
Ejemplo n.º 6
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;
}
Ejemplo n.º 7
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;
}
Ejemplo n.º 8
0
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;
}
Ejemplo n.º 9
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;
}
Ejemplo n.º 10
0
//MSG call back functions
int On_CS_Connected(Aris::Core::MSG &msg)
{
    cout<<"Received Connection from Control System:"<<endl;
    cout<<"   Remote IP is: "<<msg.GetDataAddress()+sizeof(int)<<endl;
    cout<<"   Port is     : "<<*((int*)msg.GetDataAddress()) << endl << endl;

    Aris::Core::MSG data(0,0);
    ControlSystem.SendData(data);
    return 0;
}
Ejemplo n.º 11
0
int On_CS_DataReceived(Aris::Core::CONN *pConn,Aris::Core::MSG &data)
{
    int cmd=data.GetMsgID()+100;
    Aris::Core::MSG CMD=CS_CMD_Received;
    CMD.SetLength(sizeof(int));
    bool IsCMDExecutable=true;
    CMD.Copy(&cmd,sizeof(int));
    cout<<"received CMD is"<<cmd<<endl;
    PostMsg(CMD);
    LastCMD=data.GetMsgID();
    return 0;
}
Ejemplo n.º 12
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;
}
Ejemplo n.º 13
0
int Aris::RT_CONTROL::ACTUATION::NRT_PostMsg(Aris::Core::MSG &p_data)
{
	int ret;
	//printf("sizeof %d\n",sizeof(p_data));
	//p_data.SetMsgID((int)Aris::RT_CONTROL::CM_CUS_MESSAGE);
	ret=sysBase->NRT_SendDataRaw(p_data._pData,p_data.GetLength()+MSG_HEADER_LENGTH);
	//printf("NRT_SendData is me :%d\n",ret);
	return 0;
}//ok
Ejemplo n.º 14
0
int On_CS_DataReceived(Aris::Core::CONN *pConn, Aris::Core::MSG &data)
{
    int cmd=data.GetMsgID();
    MSG CMD=CS_CMD_Received;
    CMD.SetLength(sizeof(int));
    CMD.Copy(&cmd,sizeof(int));
    cout<<"received CMD is "<<cmd<<endl;
    PostMsg(CMD);
    return 0;
}
Ejemplo n.º 15
0
int On_CS_CMD_Received(Aris::Core::MSG &msg)
{
    int Command_Gait;
    msg.Paste(&Command_Gait,sizeof(int));
    Aris::Core::MSG Gait=Command_Gait;
    cout<<"Gait Needed is: "<<Gait.GetMsgID()<<endl;
    PostMsg(Gait);
    Aris::Core::MSG data(0,0);
    ControlSystem.SendData(data);
    return 0;
}
Ejemplo n.º 16
0
Aris::Core::MSG parseShovel(const std::string &cmd, const map<std::string, std::string> &params)
{
    SHOVEL_PARAM  param;

    param.radius=2;
    param.periodNum = 7;
    for(int i = 0; i < param.periodNum; i++)
    {
        param.periodCount[i]=3000;
    }

    for(auto &i:params)
    {
        if(i.first=="height")
        {
            param.height=stod(i.second);
        }
        else if(i.first=="length")
        {
            param.length=stod(i.second);
        }
        else if(i.first=="degree")
        {
            param.rad=stod(i.second)/180*PI;
        }
        else
        {
            std::cout<<"parse failed"<<std::endl;
            return MSG{};
        }
    }

    Aris::Core::MSG msg;
    msg.CopyStruct(param);

    std::cout<<"finished parse"<<std::endl;

    return msg;
}
Ejemplo n.º 17
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;
}
Ejemplo n.º 18
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;
}
Aris::Core::MSG parseCWF(const std::string &cmd, const std::map<std::string, std::string> &params)
{
    Robots::WALK_PARAM  param;

    for (auto &i : params)
    {
        if (i.first == "totalCount")
        {
            param.totalCount = std::stoi(i.second);
        }
        else if (i.first == "walkDirection")
        {
            param.walkDirection = std::stoi(i.second);
        }
        else if (i.first == "upDirection")
        {
            param.upDirection = std::stoi(i.second);
        }
        else if (i.first == "distance")
        {
            param.d = std::stod(i.second);
        }
        else if (i.first == "height")
        {
            param.h = std::stod(i.second);
        }
    }

    isStoppingCWF=false;

    Aris::Core::MSG msg;

    msg.CopyStruct(param);

    return msg;
}
Ejemplo n.º 20
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:
        data.SetMsgID(BEGINDISCOVER);
        cs.NRT_PostMsg(data);
        break;
    case 15:
        data.SetMsgID(ENDDISCOVER);
        cs.NRT_PostMsg(data);
        break;
    case 16:
        Aris::Core::PostMsg(Aris::Core::MSG(VS_Capture));
        CGait::IsWalkAdaptiveRegistered=true;
        break;
    default:
        cout<<"Do Not Get Validate CMD"<<endl;
        break;
    }
    return CommandID;
}
Ejemplo n.º 21
0
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;
    case 37:
    {
        if(data.GetLength() == sizeof(int))
        {
            Vision_Msg visioncmd = Vision_StepDown;
            Aris::Core::MSG visionmsg;
            visionmsg.SetMsgID(VS_Capture);
            visionmsg.SetLength(sizeof(visioncmd));
            visionmsg.Copy(&visioncmd, sizeof(visioncmd));
            PostMsg(visionmsg);
        }
        else
        {
            CGait::IsStepDown = true;
            cout<<"STEP DOWN!"<<endl;
            double *map = new double [data.GetLength()/sizeof(double)];
            memcpy(map, data.GetDataAddress(), data.GetLength());

            static double StepDown_currentH[6] = {-0.85, -0.85, -0.85, -0.85, -0.85, -0.85};
            static double StepDown_nextH[6];

            memcpy(StepDown_nextH,map,sizeof(StepDown_nextH));

            if(StepDown_currentH[0] == -1.05&&StepDown_currentH[1] == -1.05&&StepDown_currentH[2] == -1.05
                    &&StepDown_currentH[3] == -1.05&&StepDown_currentH[4] == -1.05&&StepDown_currentH[5] == -1.05
                    &&StepDown_nextH[0] == -1.05&&StepDown_nextH[1] == -1.05&&StepDown_nextH[2] == -1.05
                    &&StepDown_nextH[3] == -1.05&&StepDown_nextH[4] == -1.05&&StepDown_nextH[5] == -1.05)
            {
                delete[] map;
                CGait::IsStepDown = false;
                cout<<"STEPDOWN FINISHED!"<<endl;
                cout<<"ENDISCOVER!"<<endl;
                CGait::IsEndDiscoverStart = true;
                Aris::Core::MSG controlcmd;
                controlcmd.SetMsgID(ENDDISCOVER);
                cout<<"Send ENDDISCOVER Message to CS"<<endl;
                cs.NRT_PostMsg(controlcmd);
            }
            else
            {
                double StepDownLen = 0.325;

                HexIII.RobotStepDown(&StepDownLen,StepDown_currentH,StepDown_nextH,*Gait_StepDown__Map);

                memcpy(StepDown_currentH,StepDown_nextH,sizeof(StepDown_nextH));

                delete[] map;

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

                Aris::Core::MSG controldata;
                controldata.SetMsgID(STEPDOWN);
                cs.NRT_PostMsg(controldata);
            }
        }
    }
        break;
    case 38:
    {
        if(data.GetLength() == sizeof(int))
        {
            Vision_Msg visioncmd = Vision_StepOver;
            Aris::Core::MSG visionmsg;
            visionmsg.SetMsgID(VS_Capture);
            visionmsg.SetLength(sizeof(visioncmd));
            visionmsg.Copy(&visioncmd, sizeof(visioncmd));
            PostMsg(visionmsg);
        }
        else
        {
            cout<<"STEPOVER!"<<endl;
            CGait::IsStepOver = true;
            double *stepover_data = new double [data.GetLength()/sizeof(double)];
            memcpy(stepover_data, data.GetDataAddress(), data.GetLength());

            int count = int(stepover_data[0]);
            if(count == 5)
            {
                CGait::IsStepOver = false;
                cout<<"STEPOVER 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);
            }
            else
            {
                double move_data[3];
                move_data[0] = stepover_data[1];
                move_data[1] = stepover_data[2];
                move_data[2] = stepover_data[3];

                cout<<"Move Data From Sensor"<<endl;
                cout<<"Move X: "<<move_data[0]<<endl;
                cout<<"Move Y: "<<move_data[1]<<endl;
                cout<<"Move Z: "<<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 39:
    {
        cout<<"BEGINDISCOVER!"<<endl;
        CGait::IsBeginDiscoverStart = true;
        Aris::Core::MSG controlcmd;
        controlcmd.SetMsgID(BEGINDISCOVER);
        cout<<"Send BEGINDISCOVER Message to CS"<<endl;
        cs.NRT_PostMsg(controlcmd);
    }
        break;
    case 40:
    {
        cout<<"Vision Control Singnal Recieved, Beging Walk Avoid!"<<endl;

        int command;
        memcpy(&command, data.GetDataAddress(), data.GetLength());

        Aris::Core::MSG controlcmd;

        switch (command)
        {
        case 0:
            CGait::IsWalkAvoidRegistered = false;
            break;
        case 1:
        {
            controlcmd.SetMsgID(FORWARD);
            cout<<"Send GAIT_MOVE Message to CS"<<endl;
            cs.NRT_PostMsg(controlcmd);
        }
            break;
        case 2:
        {
            controlcmd.SetMsgID(BACKWARD);
            cout<<"Send GAIT_MOVE_BACK Message to CS"<<endl;
            cs.NRT_PostMsg(controlcmd);
        }
            break;
        case 3:
        {
            controlcmd.SetMsgID(TURNLEFT);
            cout<<"Send GAIT_TURN_LEFT Message to CS"<<endl;
            cs.NRT_PostMsg(controlcmd);
        }
            break;
        case 4:
        {
            controlcmd.SetMsgID(TURNRIGHT);
            cout<<"Send GAIT_TURN_RIGHT Message to CS"<<endl;
            cs.NRT_PostMsg(controlcmd);
        }
            break;
        default:
            break;
        }
    }
        break;

    default:
        break;
    }
    return 0;
}
Ejemplo n.º 22
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;
}
Ejemplo n.º 23
0
	int SendRequest(int argc, char *argv[], const char *xmlFileName)
	{
		/*需要去除命令名的路径和扩展名*/
		std::string cmdName(argv[0]);
		
#ifdef PLATFORM_IS_WINDOWS
		if (cmdName.rfind('\\'))
		{
			cmdName = cmdName.substr(cmdName.rfind('\\') + 1, cmdName.npos);
		}
#endif
#ifdef PLATFORM_IS_LINUX
		if (cmdName.rfind('/'))
		{
			cmdName = cmdName.substr(cmdName.rfind('/') + 1, cmdName.npos);
		}
#endif
		
		if (cmdName.rfind('.'))
		{
			cmdName = cmdName.substr(0, cmdName.rfind('.'));
		}

		/*添加命令的所有参数*/
		for (int i = 1; i < argc; ++i)
		{
			cmdName = cmdName + " " + argv[i];
		}



		/*构造msg,这里需要先copy命令名称,然后依次copy各个参数*/
		Aris::Core::MSG msg;
		msg.Copy(cmdName.c_str());
		


		/*连接并发送msg*/
		Aris::Core::DOCUMENT doc;

		if (doc.LoadFile(xmlFileName) != 0)
			throw std::logic_error("failed to read configuration xml file");

		std::string ip = doc.RootElement()->FirstChildElement("Server")->FirstChildElement("Connection")->Attribute("IP");
		std::string port = doc.RootElement()->FirstChildElement("Server")->FirstChildElement("Connection")->Attribute("Port");

		Aris::Core::CONN conn;

		while (true)
		{
			try 
			{
				conn.Connect(ip.c_str(), port.c_str());
				break;
			}
			catch (std::exception &)
			{
				std::cout << "failed to connect server, will retry in 1 second" << std::endl;
				Aris::Core::Sleep(1000);
			}
			
		}
		
		Aris::Core::MSG ret = conn.SendRequest(msg);

		/*错误处理*/
		if (ret.GetLength() > 0)
		{
			std::cout << "cmd has fault, please regard to following information:" << std::endl;
			std::cout << "    " << ret.GetDataAddress() << std::endl;
		}
		else
		{
			std::cout << "send command successful" << std::endl;
		}

		return 0;
	}
Ejemplo n.º 24
0
int tg(CMachineData& machineData,RT_MSG& msg)
{
    const int MapAbsToPhy[18]=
    {
        10,	11,	9,
        12,	14,	13,
        17,	15,	16,
        6,	8,	7,
        3,	5,	4,
        0,	2,	1
    };
    const int MapPhyToAbs[18]=
    {
        15,	17,	16,
        12,	14,	13,
        9,	11,	10,
        2,	0,	1,
        3,	5,	4,
        7,	8,	6
    };

    int CommandID;
    CommandID=msg.GetMsgID();

    switch(CommandID)
    {
    case NOCMD:
        for(int i=0;i<18;i++)
        {
            machineData.motorsCommands[i]=EMCMD_NONE;
        }
        rt_printf("NONE Command Get in NRT\n" );
        break;
    case ENABLE:
        for(int i=0;i<18;i++)
        {
            machineData.motorsCommands[i]=EMCMD_ENABLE;
        }
        rt_printf("ENABLE Command Get in NRT\n" );
        break;
    case POWEROFF:
        for(int i=0;i<18;i++)
        {
            machineData.motorsCommands[i]=EMCMD_POWEROFF;
            gait.IfReadytoSetGait(false,i);
        }
        rt_printf("POWEROFF Command Get in NRT\n" );
        break;
    case STOP:
        for(int i=0;i<18;i++)
        {
            machineData.motorsCommands[i]=EMCMD_STOP;
        }
        rt_printf("STOP Command Get in NRT\n" );
        break;
    case RUNNING:
        for(int i=0;i<18;i++)
        {
            machineData.motorsCommands[i]=EMCMD_RUNNING;
            gait.IfReadytoSetGait(true,i);
        }
        rt_printf("RUNNING Command Get in NRT\n" );
        break;
    case GOHOME_1:
        machineData.motorsCommands[MapAbsToPhy[0]]=EMCMD_GOHOME;
        machineData.motorsCommands[MapAbsToPhy[1]]=EMCMD_GOHOME;
        machineData.motorsCommands[MapAbsToPhy[2]]=EMCMD_GOHOME;
        machineData.motorsCommands[MapAbsToPhy[6]]=EMCMD_GOHOME;
        machineData.motorsCommands[MapAbsToPhy[7]]=EMCMD_GOHOME;
        machineData.motorsCommands[MapAbsToPhy[8]]=EMCMD_GOHOME;
        machineData.motorsCommands[MapAbsToPhy[12]]=EMCMD_GOHOME;
        machineData.motorsCommands[MapAbsToPhy[13]]=EMCMD_GOHOME;
        machineData.motorsCommands[MapAbsToPhy[14]]=EMCMD_GOHOME;

        gaitcmd[MapAbsToPhy[0]]=EGAIT::GAIT_HOME;
        gaitcmd[MapAbsToPhy[1]]=EGAIT::GAIT_HOME;
        gaitcmd[MapAbsToPhy[2]]=EGAIT::GAIT_HOME;
        gaitcmd[MapAbsToPhy[6]]=EGAIT::GAIT_HOME;
        gaitcmd[MapAbsToPhy[7]]=EGAIT::GAIT_HOME;
        gaitcmd[MapAbsToPhy[8]]=EGAIT::GAIT_HOME;
        gaitcmd[MapAbsToPhy[12]]=EGAIT::GAIT_HOME;
        gaitcmd[MapAbsToPhy[13]]=EGAIT::GAIT_HOME;
        gaitcmd[MapAbsToPhy[14]]=EGAIT::GAIT_HOME;

        rt_printf("GOHOME_1 Command Get in NRT\n" );
        break;
    case GOHOME_2:
        machineData.motorsCommands[MapAbsToPhy[3]]=EMCMD_GOHOME;
        machineData.motorsCommands[MapAbsToPhy[4]]=EMCMD_GOHOME;
        machineData.motorsCommands[MapAbsToPhy[5]]=EMCMD_GOHOME;
        machineData.motorsCommands[MapAbsToPhy[9]]=EMCMD_GOHOME;
        machineData.motorsCommands[MapAbsToPhy[10]]=EMCMD_GOHOME;
        machineData.motorsCommands[MapAbsToPhy[11]]=EMCMD_GOHOME;
        machineData.motorsCommands[MapAbsToPhy[15]]=EMCMD_GOHOME;
        machineData.motorsCommands[MapAbsToPhy[16]]=EMCMD_GOHOME;
        machineData.motorsCommands[MapAbsToPhy[17]]=EMCMD_GOHOME;

        gaitcmd[MapAbsToPhy[3]]=EGAIT::GAIT_HOME;
        gaitcmd[MapAbsToPhy[4]]=EGAIT::GAIT_HOME;
        gaitcmd[MapAbsToPhy[5]]=EGAIT::GAIT_HOME;
        gaitcmd[MapAbsToPhy[9]]=EGAIT::GAIT_HOME;
        gaitcmd[MapAbsToPhy[10]]=EGAIT::GAIT_HOME;
        gaitcmd[MapAbsToPhy[11]]=EGAIT::GAIT_HOME;
        gaitcmd[MapAbsToPhy[15]]=EGAIT::GAIT_HOME;
        gaitcmd[MapAbsToPhy[16]]=EGAIT::GAIT_HOME;
        gaitcmd[MapAbsToPhy[17]]=EGAIT::GAIT_HOME;

        rt_printf("GOHOME_2 Command Get in NRT\n" );
        break;
    case HOME2START_1:
        if(gait.m_gaitState[MapAbsToPhy[0]]==GAIT_STOP)
        {
            for(int i=0;i<18;i++)
            {
                machineData.motorsModes[i]=EOperationMode::OM_CYCLICVEL;
            }
            gaitcmd[MapAbsToPhy[0]]=EGAIT::GAIT_HOME2START;
            gaitcmd[MapAbsToPhy[1]]=EGAIT::GAIT_HOME2START;
            gaitcmd[MapAbsToPhy[2]]=EGAIT::GAIT_HOME2START;
            gaitcmd[MapAbsToPhy[6]]=EGAIT::GAIT_HOME2START;
            gaitcmd[MapAbsToPhy[7]]=EGAIT::GAIT_HOME2START;
            gaitcmd[MapAbsToPhy[8]]=EGAIT::GAIT_HOME2START;
            gaitcmd[MapAbsToPhy[12]]=EGAIT::GAIT_HOME2START;
            gaitcmd[MapAbsToPhy[13]]=EGAIT::GAIT_HOME2START;
            gaitcmd[MapAbsToPhy[14]]=EGAIT::GAIT_HOME2START;

            rt_printf("HOME2START_1 Command Get in NRT\n" );
        }
        break;
    case HOME2START_2:
        if(gait.m_gaitState[MapAbsToPhy[3]]==GAIT_STOP)
        {
            for(int i=0;i<18;i++)
            {
                machineData.motorsModes[i]=EOperationMode::OM_CYCLICVEL;
            }
            gaitcmd[MapAbsToPhy[3]]=EGAIT::GAIT_HOME2START;
            gaitcmd[MapAbsToPhy[4]]=EGAIT::GAIT_HOME2START;
            gaitcmd[MapAbsToPhy[5]]=EGAIT::GAIT_HOME2START;
            gaitcmd[MapAbsToPhy[9]]=EGAIT::GAIT_HOME2START;
            gaitcmd[MapAbsToPhy[10]]=EGAIT::GAIT_HOME2START;
            gaitcmd[MapAbsToPhy[11]]=EGAIT::GAIT_HOME2START;
            gaitcmd[MapAbsToPhy[15]]=EGAIT::GAIT_HOME2START;
            gaitcmd[MapAbsToPhy[16]]=EGAIT::GAIT_HOME2START;
            gaitcmd[MapAbsToPhy[17]]=EGAIT::GAIT_HOME2START;

            rt_printf("HOME2START_2 Command Get in NRT\n" );
        }
        break;
    case FORWARD:
        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_MOVE;
                rt_printf("FORWARD Command Get in NRT\n" );
            }
        }
        break;
    case BACKWARD:
        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_MOVE_BACK;
                rt_printf("BACKWARD Command Get in NRT\n" );
            }
        }
        break;
    case TURNLEFT:
        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_TURN_LEFT;
                rt_printf("TURNLEFT Command Get in NRT\n" );
            }
        }
        break;
    case TURNRIGHT:
        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_TURN_RIGHT;
                rt_printf("TURNRIGHT Command Get in NRT\n" );
            }
        }
        break;
    case LEGUP:
        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_LEGUP;
                rt_printf("LEGUP Command Get in NRT\n" );

            }
        }
        break;
    case MOVE:
        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_MOVE_MAP;
                rt_printf("MOVE_MAP Command Get in NRT\n" );

            }
        }
        break;
    case TURN:
        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_TURN_MAP;
                rt_printf("TURN_MAP Command Get in NRT\n" );

            }
        }
        break;
    case BEGINDISCOVER:
        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_BEGIN_DISCOVER;
                rt_printf("BEGINDISCOVER Command Get in NRT\n" );
            }
        }
        break;
    case ENDDISCOVER:
        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_END_DISCOVER;
                rt_printf("ENDDISCOVER Command Get in NRT\n" );
            }
        }
        break;
    case STEPUP:
        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_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)
        {
            cout<<"ENDDISCOVER 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);
            CGait::IsEndDiscoverStart = false;
            CGait::IsEndDiscoverStart = false;
        }

        if(CGait::IsStepUp == true&&CGait::IsStepUpstep == true)
        {
            CGait::IsStepUpstep = false;
            Vision_Msg visioncmd = Vision_StepUp;
            Aris::Core::MSG visionmsg;
            visionmsg.SetMsgID(VS_Capture);
            visionmsg.SetLength(sizeof(visioncmd));
            visionmsg.Copy(&visioncmd, sizeof(visioncmd));
            PostMsg(visionmsg);
        }

        if(CGait::IsStepDown == true&&CGait::IsStepDownstep == true)
        {
            CGait::IsStepDownstep = false;
            Vision_Msg visioncmd = Vision_StepDown;
            Aris::Core::MSG visionmsg;
            visionmsg.SetMsgID(VS_Capture);
            visionmsg.SetLength(sizeof(visioncmd));
            visionmsg.Copy(&visioncmd, sizeof(visioncmd));
            PostMsg(visionmsg);
        }
        if(CGait::IsStepOver == true&&CGait::IsStepOverstep == true)
        {
            CGait::IsStepOverstep = false;
            Vision_Msg visioncmd = Vision_StepOver;
            Aris::Core::MSG visionmsg;
            visionmsg.SetMsgID(VS_Capture);
            visionmsg.SetLength(sizeof(visioncmd));
            visionmsg.Copy(&visioncmd, sizeof(visioncmd));
            PostMsg(visionmsg);
        }
    }

    return 0;
}
Ejemplo n.º 25
0
Aris::Core::MSG parseMove2(const std::string &cmd, const map<std::string, std::string> &params)
{
    MOVES_PARAM  param;

    double targetPos[3]; //移动目标位置

    for(auto &i:params)
    {
        if(i.first=="component")
        {
            if(i.second=="lf")
            {
                param.comID=0;
            }
            else if(i.second=="lm")
            {
                param.comID=1;
            }
            else if(i.second=="lr")
            {
                param.comID=2;
            }
            else if(i.second=="rf")
            {
                param.comID=3;
            }
            else if(i.second=="rm")
            {
                param.comID=4;
            }
            else if(i.second=="rr")
            {
                param.comID=5;
            }
            else if(i.second=="bd")
            {
                param.comID=6;
            }
            else
            {
                std::cout<<"parse failed"<<std::endl;
                return MSG{};
            }
        }
        //绝对坐标移动
        else if(i.first=="x")
        {
            targetPos[0]=stod(i.second);
            param.isAbsolute=true;
        }
        else if(i.first=="y")
        {
            targetPos[1]=stod(i.second);
            param.isAbsolute=true;
        }
        else if(i.first=="z")
        {
            targetPos[2]=stod(i.second);
            param.isAbsolute=true;
        }
        //相对坐标移动
        else if(i.first=="u")
        {
            targetPos[0]=stod(i.second);
        }
        else if(i.first=="v")
        {
            targetPos[1]=stod(i.second);
        }
        else if(i.first=="w")
        {
            targetPos[2]=stod(i.second);
        }
        else
        {
            std::cout<<"parse failed"<<std::endl;
            return MSG{};
        }
    }

    if(param.comID==6)
    {
        std::copy_n(targetPos, 3, param.targetBodyPE);
    }
    else
    {
        std::copy_n(targetPos, 3, &param.targetPee[3*param.comID]);
        param.legNum=1;
        param.motorNum=3;
        param.legID[0]=param.comID;
        int motors[3] = { 3*param.comID, 3*param.comID+1, 3*param.comID+2 };
        std::copy_n(motors, 9, param.motorID);
    }

    param.periodCount=3000;

    Aris::Core::MSG msg;
    msg.CopyStruct(param);

    std::cout<<"finished parse"<<std::endl;

    return msg;
}
Ejemplo n.º 26
0
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;
}