/*Main function*/ int main(int argc, char **argv){ /*Objects*/ ros::init(argc, argv, "i90_sensor");//Create "i90_sensor" node ros::NodeHandle n;//Create nodehandler to modify features of the node ros::Publisher targetPub = n.advertise<i90_sensor_board::pos>("i90_target_pos", 1);//Publisher for target position ros::Subscriber translationSub = n.subscribe("i90_current_pos", 1, recalculateTarget);//Subscriber for current position /*Prepare the serial ports*/ if(iPort == -1){ printf("Error opening the port\n\r");//Inform user on the terminal } else{ printf("Sensor board serial port is OPEN\n\r");//Inform user on the terminal } setInterfaceAttribs (iPort, B115200, 0);//Set parameters tcflush(iPort, TCIFLUSH);//Empty the buffer if(iDronePort == -1){ printf("Error opening the drone port\n\r");//Inform user on the terminal } else{ printf("Drone PC serial port is OPEN\n\r");//Inform user on the terminal } setInterfaceAttribs (iDronePort, B9600, 0);//Set parameters tcflush(iDronePort, TCIFLUSH);//Empty the buffer if(iMbedPort == -1){ printf("Error opening the mbed port\n\r");//Inform user on the terminal } else{ printf("Home beacon serial port is OPEN\n\r");//Inform user on the terminal } setInterfaceAttribs (iMbedPort, B115200, 0);//Set parameters tcflush(iMbedPort, TCIFLUSH);//Empty the buffer /*Reset flags*/ bCalculation = false; bBeaconFound = false; bTurnRight = false; bTurnLeft = false; fMaxIrAngle = 45.00; printf("Searching for the beacon.\n\r"); while (ros::ok()){ if(bCalculation == true){ /*Publish the target*/ targetPub.publish(targetPos);//Publish calculated position bCalculation = false; } usleep(200000);//Wait for 200ms (5Hz) ros::spinOnce(); } return 0; }
int DLS::setFD (int fd) { fd_ = fd; setInterfaceAttribs (B115200, 0); // set speed to 115,200 bps, 8n1 (no parity) setBlocking (0); // set no blocking return EXIT_SUCCESS; }
/* * Setup serial port and the block on read. * Reads from the serial and writes to the socket. */ void *serialThread(void *data) { char buf[2]; // Read one char at a time into this buffer. char inBuffer[32]; char msgBuffer[32]; int n; int i; /* * if(globals->getDebug()) { * printf("\nserialThread\n"); } */ // // set speed to 115,200 bps, 8n1 (no parity) // setInterfaceAttribs (globals->getSerial (), globals->getBaudRate (), 0); // // set blocking // setBlocking (globals->getSerial (), 1); while (globals->getRunFlag ()) { n = read(globals->getSerial(), buf, 1); /* * if(globals->getDebug()) { * // putchar(buf[0]); * fflush(stdout); } */ if (buf[0] > 0x20 && buf[0] < 0x80) { sprintf(msgBuffer, "Out %02x:%c\n", buf[0], buf[0]); } else { sprintf(msgBuffer, "Out %02x: \n", buf[0]); } globals->writeLog (msgBuffer); n = write(globals->getSocket (), buf, 1); } /* terminate the thread */ globals->writeLog ((char *) "Thread done\n"); pthread_exit (NULL); }
/*Main function*/ int main(int argc, char **argv) { ros::init(argc, argv, "drone_movement"); //Create node called drone_movement ros::NodeHandle n; //Create nodehandle to alter node parameters ros::Duration d = ros::Duration(2,0); //Duration object to pause node /*Publishers*/ ros::Publisher landPub = n.advertise<std_msgs::Empty>("/ardrone/land",1); //Publisher to send landing commands ros::Publisher takeoffPub = n.advertise<std_msgs::Empty>("/ardrone/takeoff",1); //Publisher to send takeoff commands ros::Publisher drone_pub = n.advertise<std_msgs::String>("tum_ardrone/com",50); //Publisher to send commands to Ar-Drone /*Subscribers*/ ros::Subscriber targetSub = n.subscribe("i90_current_pos", 1, getTargetPos); //To collect the target position published by i90_sensor_board ros::Subscriber patternSub = n.subscribe("/ardrone/navdata", 1, getPattern); //To collect patter recognition information ros::Subscriber actualPosSub = n.subscribe("/ardrone/predictedPose", 1, getActualPos);//To collect actual robot position d.sleep(); //Wait for 2 seconds to allow publishers & subscribers to get ready std_msgs::String s; std::string c; static pthread_mutex_t send_CS; /*Initilize flags*/ iInitilizeFlag = 0; iReturnFlag = 0; iPatternFlag = 0; iCounter = 0; iLaunchFlag = 0; //CHANGE!!! 0 iTimerFlag = 0; /*Check serial port is open*/ if(iI90Port == -1){ ROS_INFO("Error opening the port");//Inform user on the terminal } else{ ROS_INFO("I90 PC serial port is open"); ROS_INFO("Waiting for sync signal"); } setInterfaceAttribs (iI90Port, B9600, 0); while(ros::ok()){ while(iLaunchFlag == 0){ /*Read serial until special character is received from i90*/ read(iI90Port, cpData, 1);//Read 1 bytes from buffer if(cData[0] == 'a'){ tcflush(iI90Port, TCIFLUSH); //Empty buffer write(iI90Port,"b",1); //Send acknowledgement ROS_INFO("Target position request sent"); usleep(5000000); //Wait until all information is received read(iI90Port, cpData, 4); //Read 4 bytes from buffer fTargetPosX = cData[0] + cData[1] / 100.00; fTargetPosY = cData[2] + cData[3] / 100.00; ROS_INFO("Target received: [%f][%f]", fTargetPosX, fTargetPosY); float fPassingPoint1[2] = {2,0},fPassingPoint2[2] = {0,2}; double dDistance1, dDistance2; dDistance1 = sqrt(((fPassingPoint1[0] - fTargetPosX) * (fPassingPoint1[0] - fTargetPosX)) + ((fPassingPoint1[1] - fTargetPosY) * (fPassingPoint1[1] - fTargetPosY))); dDistance2 = sqrt(((fPassingPoint2[0] - fTargetPosX) * (fPassingPoint2[0] - fTargetPosX)) + ((fPassingPoint2[1] - fTargetPosY) * (fPassingPoint2[1] - fTargetPosY))); if(dDistance1 <= dDistance2){ fPassingX = 1; fPassingY = 0; } else{ fPassingX = 0; fPassingY = 1; } fTargetPosX -= 0.5; if(fTargetPosX < 3) fTargetPosX -= fErrorCompCloseX; else if(fTargetPosX < 6) fTargetPosX -= fErrorCompX; else fTargetPosX -= fErrorCompX + 0.25; if(fTargetPosY < 3) fTargetPosY -= fErrorCompCloseY; else if(fTargetPosY < 6) fTargetPosY -= fErrorCompY; else fTargetPosY -= fErrorCompY + 0.25; /*Change flags and display target*/ iLaunchFlag = 1; iInitilizeFlag = 1; } } /*Sample for tag and display current position at 2Hz*/ usleep(500000); if(iTimerFlag == 1){ iCounter++; ROS_INFO("Time left: [%i]s", iCounter/2); if(iCounter > 180){ iReturnFlag = 1; ROS_INFO("Mission not successful!"); write(iI90Port,"e",1); iTimerFlag = 0; } } /*Once target coordinates are received launch and move towards target*/ if(iInitilizeFlag == 1){ //MODIFIED && iReturnFlag == 0 ROS_INFO("Current pos: X = [%f] Y = [%f]", fActualPosX, fActualPosY); /////CHECK!!! /*Set origin in actual position*/ c = "c setReference $POSE$"; //Set origin in drone's actual position s.data = c.c_str(); pthread_mutex_lock(&send_CS); drone_pub.publish(s); pthread_mutex_unlock(&send_CS); ROS_INFO("setReference!"); d.sleep(); /*Takeoff drone*/ takeoffPub.publish(std_msgs::Empty()); ROS_INFO("Taking off!"); d.sleep(); d.sleep(); d.sleep(); /*Set initial parameters*/ c = "c autoTakeOver 500 800"; s.data = c.c_str(); pthread_mutex_lock(&send_CS); drone_pub.publish(s); pthread_mutex_unlock(&send_CS); ROS_INFO("Current pos: X = [%f] Y = [%f]", fActualPosX, fActualPosY); /////CHECK!!! c = "c setMaxControl 0.1"; //Limit AR-drone high speed s.data = c.c_str(); pthread_mutex_lock(&send_CS); drone_pub.publish(s); pthread_mutex_unlock(&send_CS); c = "c setInitialReachDist 0.25"; //Reach target within 0.1m s.data = c.c_str(); pthread_mutex_lock(&send_CS); drone_pub.publish(s); pthread_mutex_unlock(&send_CS); //ROS_INFO("setInitialReachDist!"); c = "c setStayWithinDist 0.25"; //Stay in target location within 0.1m s.data = c.c_str(); pthread_mutex_lock(&send_CS); drone_pub.publish(s); pthread_mutex_unlock(&send_CS); c = "c setStay 1"; //Stay in target location for 3s s.data = c.c_str(); pthread_mutex_lock(&send_CS); drone_pub.publish(s); pthread_mutex_unlock(&send_CS); c = "c lockScaleFP"; //Fix PTAM s.data = c.c_str(); drone_pub.publish(s); d.sleep(); /*Select passing point as starting point*/ c = " "; sprintf(&c[0],"c goto %.2f %.2f 0.75 %.2f", fPassingX * 1.5, fPassingY * 1.5, fYawAngle); s.data = c.c_str(); pthread_mutex_lock(&send_CS); drone_pub.publish(s); pthread_mutex_unlock(&send_CS); ROS_INFO("Moving to passing point: [%f][%f]", fPassingX * 1.5, fPassingY * 1.5); /*Move towards target*/ c = " "; sprintf(&c[0],"c goto %.2f %.2f 1.0 %.2f", fTargetPosX, fTargetPosY, fYawAngle); s.data = c.c_str(); pthread_mutex_lock(&send_CS); drone_pub.publish(s); pthread_mutex_unlock(&send_CS); ROS_INFO("Moving to target: [%f][%f]", fTargetPosX, fTargetPosY); /*Search tag in square shape*/ c = " "; sprintf(&c[0],"c goto %.2f %.2f 1.0 %.2f", fTargetPosX + fPassingY - fSearchDistance, fTargetPosY + fPassingX - fSearchDistance, fYawAngle); s.data = c.c_str(); pthread_mutex_lock(&send_CS); drone_pub.publish(s); pthread_mutex_unlock(&send_CS); c = " "; sprintf(&c[0],"c goto %.2f %.2f 1.0 %.2f", fTargetPosX + fPassingY - fSearchDistance, fTargetPosY + fPassingY - fSearchDistance, fYawAngle); s.data = c.c_str(); pthread_mutex_lock(&send_CS); drone_pub.publish(s); pthread_mutex_unlock(&send_CS); c = " "; sprintf(&c[0],"c goto %.2f %.2f 1.0 %.2f", fTargetPosX + fPassingX - fSearchDistance, fTargetPosY + fPassingY - fSearchDistance, fYawAngle); s.data = c.c_str(); pthread_mutex_lock(&send_CS); drone_pub.publish(s); pthread_mutex_unlock(&send_CS); c = " "; sprintf(&c[0],"c goto %.2f %.2f 1.0 %.2f", fTargetPosX + fPassingX - fSearchDistance, fTargetPosY + fPassingX - fSearchDistance, fYawAngle); s.data = c.c_str(); pthread_mutex_lock(&send_CS); drone_pub.publish(s); pthread_mutex_unlock(&send_CS); c = " "; sprintf(&c[0],"c goto %.2f %.2f 1.0 %.2f", fTargetPosX + fPassingY - fSearchDistance, fTargetPosY + fPassingX - fSearchDistance, fYawAngle); s.data = c.c_str(); pthread_mutex_lock(&send_CS); drone_pub.publish(s); pthread_mutex_unlock(&send_CS); /*Return home in case that target is not found*/ /*Return to appropiate passing point*/ c = " "; sprintf(&c[0],"c goto %.2f %.2f 0.75 %.2f", fPassingX * 1.5, fPassingY * 1.5, fYawAngle); s.data = c.c_str(); pthread_mutex_lock(&send_CS); drone_pub.publish(s); pthread_mutex_unlock(&send_CS); /*Return origin*/ c = " "; c = "c goto -0.25 -0.25 0.25 0"; s.data = c.c_str(); drone_pub.publish(s); iInitilizeFlag = 0; iTimerFlag = 1; } /*Return home once pattern is found*/ if(iPatternFlag == 1){ //MODIFIED && iReturnFlag == 0 ROS_INFO("Target pattern idetified"); /*Clear commands*/ c = " "; c = "c clearCommands"; s.data = c.c_str(); drone_pub.publish(s); /*Move towards target*/ c = " "; sprintf(&c[0],"c goto %.2f %.2f 1.0 %.2f", fTargetPosX, fTargetPosY, fYawAngle); s.data = c.c_str(); pthread_mutex_lock(&send_CS); drone_pub.publish(s); pthread_mutex_unlock(&send_CS); /*Return to appropiate passing point*/ c = " "; sprintf(&c[0],"c goto %.2f %.2f 0.75 %.2f", fPassingX * 1.5, fPassingY * 1.5, fYawAngle); s.data = c.c_str(); pthread_mutex_lock(&send_CS); drone_pub.publish(s); pthread_mutex_unlock(&send_CS); /*Return origin*/ c = " "; c = "c goto -0.25 -0.25 0.25 0"; s.data = c.c_str(); drone_pub.publish(s); iPatternFlag = 0; iReturnFlag = 1; } /*Land once drone is in home area*/ if(iReturnFlag == 1 && (fActualPosX < 0.2 && fActualPosY < 0.2)){ ROS_INFO("Reach home"); c = " "; landPub.publish(std_msgs::Empty()); ROS_INFO("Land"); iTimerFlag = 0; iReturnFlag = 0; } ros::spinOnce(); } return 0; }
int main( int argc, char *argv[]) { int opt; int debug = 1; int verbose = 1; char *serialPort=(char *)NULL; int ser; char outBuffer[255]; char inBuffer[255]; char fileName[PATH_MAX]; bool fileNameSet=false; bool exitFlag = false; char *ptr; FILE *toSend; int len; char test[3]; uint8_t idx=0; while ((opt = getopt(argc, argv, "df:h?s:v")) != -1) { switch(opt) { case 'd': debug=1; break; case 'f': strcpy(fileName,optarg); fileNameSet=true; break; case 'v': verbose=1; printf("\nVerbose mode on.\n"); break; case 'h': case '?': usage(); break; case 's': serialPort = (char *)malloc(strlen(optarg)+1); if( (char *)NULL == serialPort) { perror("Failed to allocate memory for serial port"); exit(1); } strcpy(serialPort, optarg); break; default: break; } } if( false == fileNameSet) { fprintf(stderr,"You need to specify a file to send\n"); exit(-1); } toSend=fopen(fileName,"r"); if(!toSend) { fprintf(stderr,"Failed top open file %s\n", fileName); perror("Send File"); exit(-1); } if(verbose) { printf("\nSerial Port:%s\n",serialPort); } ser = open (serialPort, O_RDWR | O_NOCTTY | O_SYNC); if( 0 > ser) { printf("Failed to open serial port:%d\n",errno); perror("failed"); exit(2); } setInterfaceAttribs (ser, B19200, 0); setBlocking (ser, 1); write(ser, "\r", 1); idx=0; while( exitFlag == false ) { // looking for '>> ' read( ser, outBuffer, 1); if( outBuffer[0] == 0) { exitFlag = true; } if( outBuffer[0] != '>' && outBuffer[0] != ' ') { idx=0; } if( outBuffer[0] == '>' ) { idx++; } if( outBuffer[0] == ' ' ) { idx++; } if( idx == 3 ) { exitFlag = true; } } ptr = outBuffer; while( ptr != NULL) { memset( outBuffer,0,sizeof(outBuffer)); ptr = fgets( outBuffer, 255, toSend ); if( strlen(outBuffer) > 1 ) { fprintf(stderr,"%s", ptr); usleep(5000); // wait 1 ms len = write( ser, outBuffer, strlen( outBuffer )); write( ser,"\r",1 ); read( ser, inBuffer, len ); read( ser, outBuffer, 1); waitForEol( ser ); } } write( ser,"\x1a",1 ); return(0); }