linux下pioneer3控制程序

#include <stdio.h> #include <unistd.h> #include <string.h> #include <stdlib.h> #include <pthread.h> #include "Aria.h" #include "robot.h" //#include "ActionGoto.h" #define simulator //#define print pthread_cond_t robotCondDataOut;/*when received data from robot, send this signal*/ pthread_cond_t robotCondSndDataFinish;/*when sent data to robot and return, send this signal*/ pthread_cond_t robotCondDataReady;/*when data is ready to send to robot, send this signal*/ pthread_mutex_t robotMutexControl; pthread_mutex_t robotMutexSndDataFinish; pthread_mutex_t robotMutexHandleRobotMsg; float X_axis; float Y_axis; struct robotData robotDataIn; struct robotData robotDataOut; double eastAngle = 0; //declare functions void handleRobotMoveResult(char *result); void handleRobotBreakCmdResult(char *result); void initRobot(); void initRobotArg(int &argc, char *argv[]); void handleRobotUnkownCmdResult(char *cmd); void robotSetXYDegree(char *cmd, ArRobot *robot); void robotGetXYDegree(ArRobot *robot); void robotSendExecCmdResult(char *cmd, char *result); void handleRobotSetXYDegreeResult(char *result); void handleRobotGetXYDegreeResult(char * result); void handleRobotPositionPkg(char *pkgData); void convertMapCoordinates2Robotcoordinates(double mapX, double mapY, double angle, double *robotX, double *robotY); void convertRobotCoordinates2Mapcoordinates(double *mapX, double *mapY, double angle, double robotX, double robotY); void* controlRobotThread(void *arg); //implement functions void initRobotCtrlInterface(void) { pthread_t handleMsgFromRobotId; pthread_t ControlRobotThrdId; robotDataIn.data = (char *)malloc(robotPkgMaxLen); robotDataOut.data = (char *)malloc(robotPkgMaxLen); robotDataOut.dataType = (char *)malloc(10); robotDataIn.dataType = (char *)malloc(10); memset(robotDataIn.data, 0, robotPkgMaxLen); memset(robotDataOut.data, 0, robotPkgMaxLen); memset(robotDataIn.dataType, 0, 10); memset(robotDataOut.dataType, 0, 10); //pthread_cond_init( &robotCondCmdReady, NULL); //pthread_cond_init( &robotCondFinish, NULL); //pthread_cond_init( &robotCondSndCmdFinish, NULL); pthread_create( &handleMsgFromRobotId, NULL, handleMsgFromRobot, NULL); pthread_create( &ControlRobotThrdId, NULL, controlRobotThread, NULL); } void sendData2Robot(char *packageType, char *packagedata) { for(;;) { pthread_mutex_lock(&robotDataIn.mutex); if(0 == strlen(robotDataIn.data)) { memcpy(robotDataIn.dataType, packageType, strlen(packageType)); memcpy(robotDataIn.data, packagedata, strlen(packagedata)); #ifdef print printf("robotDataIn is ready\n"); #endif break; } #ifdef print printf("robotDataIn isn't ready\n"); #endif pthread_mutex_unlock(&robotDataIn.mutex); usleep(1000); } pthread_mutex_unlock(&robotDataIn.mutex); #ifdef print printf("Send command is ready signal\n"); #endif //pthread_cond_broadcast(&robotCondCmdReady); pthread_cond_signal(&robotCondDataReady); pthread_mutex_lock(&robotMutexSndDataFinish); pthread_cond_wait(&robotCondSndDataFinish, &robotMutexSndDataFinish); pthread_mutex_unlock(&robotMutexSndDataFinish); } void initRobotArg(int *argc, char *argv[]) { argv[0] = (char *)malloc(50); argv[1] = (char *)malloc(50); argv[2] = (char *)malloc(50); argv[3] = (char *)malloc(50); argv[4] = (char *)malloc(50); memcpy(argv[0], "./robotmod", strlen("./robotmod")); #ifdef simulator memcpy(argv[1], "-rh", strlen("-rh")); #define IP "127.0.0.1" memcpy(argv[2], IP, strlen(IP)); memcpy(argv[3], "-remoteRobotTcpPort", strlen("-remoteRobotTcpPort")); memcpy(argv[4], "8101", strlen("8101")); *argc = 5; #endif #ifndef simulator argv[1] = NULL; argv[2] = NULL; argv[3] = NULL; argv[4] = NULL; *argc = 1; #endif } void robotSendExecCmdResult(char *cmd, char *result) { for(;;)/*see if robotDataOut is ready*/ { pthread_mutex_lock(&robotDataOut.mutex); if(0 == strlen(robotDataOut.data)) { break; } //usleep(10000); pthread_mutex_unlock(&robotDataOut.mutex); } //robotDataOut is ready memcpy(robotDataOut.dataType, robotPkgCmdExcuteRet, strlen(robotPkgCmdExcuteRet)); memcpy(robotDataOut.data, cmd, strlen(cmd)); memcpy(robotDataOut.data + strlen(cmd), ",", strlen(",")); memcpy(robotDataOut.data + strlen(cmd) + strlen(","), result, strlen(result)); pthread_mutex_unlock(&robotDataOut.mutex); pthread_cond_broadcast(&robotCondDataOut); } void getAllSonarData(ArRobot *robot, float *sonarData) { for(int i = 0; i < 16; i++) { sonarData[i] = robot->getSonarReading(i)->getRange(); } } void printRobotPosition(ArRobot *robot) { printf("Robot position:%f, %f\n", robot->getX(), robot->getY()); } void *robotMoveDoneThrd(void *arg) { ActionGoto *gotoPoseAction = (ActionGoto *)arg; for(;;) { if(gotoPoseAction->haveAchievedGoal()) { #ifdef print printf("robot move done.\n"); #endif robotSendExecCmdResult(robotCmdMov, robotCmdExecSucceed); break; } if(gotoPoseAction->isCanceled()) { #ifdef print printf("robot move action has been broken\n"); #endif robotSendExecCmdResult(robotCmdMov, robotCmdExecBreaked); break; } //check if robot is move done 10HZ usleep(100000); } return 0; } void convertMapCoordinates2Robotcoordinates(double mapX, double mapY, double angle, double *robotX, double *robotY) { #ifdef simulator *robotX = mapX * 1000; *robotY = mapY * 1000; #endif #ifndef simulator *robotX = mapX * 1000 /cos(angle); *robotY = mapY * 1000 /cos(angle); #endif } void convertRobotCoordinates2Mapcoordinates(double *mapX, double *mapY, double angle, double robotX, double robotY) { #ifdef simulator *mapX = robotX / 1000; *mapY = robotY / 1000; #endif #ifndef simulator *mapX = robotX * cos(angle) / 1000; *mapY = robotY * cos(angle) / 1000; #endif } void robotMove(ActionGoto *gotoPoseAction, pthread_t *robotMoveDoneThrdId, char *cmd) { char *ptr1, *ptr2; /*cmd[robotPkgMaxLen]*/; char x[robotPkgMaxLen], y[robotPkgMaxLen]; double mapX, mapY; double xaxis, yaxis; memset(x, 0, robotPkgMaxLen); memset(y, 0, robotPkgMaxLen); ptr1 = strchr(robotDataIn.data, ','); if(ptr1 == NULL) { robotSendExecCmdResult(robotCmdMov, robotCmdExecParameterErr); return; } ptr1++; ptr2 = strchr(ptr1, ','); if(ptr2 == NULL) { robotSendExecCmdResult(robotCmdMov, robotCmdExecParameterErr); return; } memcpy(x, ptr1, ptr2 - ptr1); ptr1 = ptr2 + 1; ptr2 = strchr(ptr1, 0); if(ptr1 == ptr2) { robotSendExecCmdResult(robotCmdMov, robotCmdExecParameterErr); return; } memcpy(y, ptr1, ptr2 - ptr1); mapX = atof(x); mapY = atof(y); convertMapCoordinates2Robotcoordinates(mapX, mapY, eastAngle, &xaxis, &yaxis); gotoPoseAction->setGoal(ArPose(xaxis, yaxis)); Aria::getRunning(); pthread_create(robotMoveDoneThrdId, NULL, robotMoveDoneThrd, (void *)gotoPoseAction); } void robotBreakMove(ActionGoto *gotoPoseAction) { gotoPoseAction->setCancel(); robotSendExecCmdResult(robotCmdRobotBreak, robotCmdExecSucceed); } void robotBreakCmd(char *cmd, ActionGoto *gotoPoseAction) { char *ptr1, *ptr2; char breakCmd[4]; //extract breakCmd parameter--the commad which would be broken ptr1 = strchr(cmd, ','); if(ptr1 == NULL) { robotSendExecCmdResult(robotCmdRobotBreak, robotCmdExecParameterErr); return; } ptr1++; ptr2 = strchr(ptr1, 0); if(ptr1 == ptr2) { robotSendExecCmdResult(robotCmdRobotBreak, robotCmdExecParameterErr); return; } memset(breakCmd, 0, 4); memcpy(breakCmd, ptr1, ptr2 - ptr1); if(strcmp(breakCmd, robotCmdMov) == 0) { #ifdef print printf("break robot move action\n"); #endif robotBreakMove(gotoPoseAction); } else if(0 == strcmp(breakCmd, robotCmdSetXYDegree)) { #ifdef print printf("Can not break command:%s\n", breakCmd); #endif robotSendExecCmdResult(robotCmdRobotBreak, robotCmdExecForbidBreak); } else if(0 == strcmp(breakCmd, robotCmdGetXYDegree)) { #ifdef print printf("Can not break command:%s\n", breakCmd); #endif robotSendExecCmdResult(robotCmdRobotBreak