/*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);
}
