Inconsistency in performance of AR Drone 2.0

A

Ambreen

New Member
Hi,

Hope you are fine. I am Ambreen Mustafa, Research Assistant from the Hong Kong Polytechnic University. Currently I am working on “Path Following of Parrot AR Drone 2.0 by using a camera based indoor localization system (Whycon localization system: https://github.com/lrse/whycon )”. We are facing some issues regarding the performance of AR Drone.

Inconsistency in performance: We have implemented a PID loop and provided way points for drone to follow. Such that when it reaches one point camera detects it and commands are forwarded to drone to reach next point. However with all the conditions kept same, drone sometimes follow a predefined path accurately and sometime its starts drifting randomly and does not respond to commands. Currently we have implemented it using ROS.

Square formation:

E-Formation:

With this message I have attached the source code (please check arNavigate function). I will be thankful if you could provide us with some solution to this problem or point out some possible reasons for this inconsistency.

Thanks a lot, looking forward to your reply soon!

Regards
Ambreen Mustafa
Research Assistant (The Hong Kong Polytechnic University)

Code: 
#include <iostream>
#include <unistd.h>
#include <math.h>

#include "ros/ros.h"
#include "std_msgs/Empty.h"
#include "geometry_msgs/Twist.h"
#include "geometry_msgs/PoseArray.h"
#include "ardrone_autonomy/Navdata.h"

#include <opencv2/core/core.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/calib3d/calib3d.hpp>
#include <opencv2/highgui/highgui.hpp>

#define posRange 0.1
#define minStay 1
#define pathPoints 4

struct pos {
    float x,y;
};

int64 tnow, tprev;
double dtime;
pos current, dest, errorNow, errorPrev, sum, diff, path[4];
int pos_count = 0, stay_count = 0;

float PID[3], dPID[3];

std_msgs::Empty emp;
geometry_msgs::Twist ARcommand_pos;

void callBack_navdata(const ardrone_autonomy::Navdata::ConstPtr& data) {
    return;
}

void callBack_poses(const geometry_msgs::PoseArray::ConstPtr& data) {
    current.x = data->poses[0].position.x;
    current.y = data->poses[0].position.y;
    tnow = (data->header.stamp).toNSec();
    int tdiff = (tnow - tprev);
    dtime = ((double)tdiff)/(1000000000);
    tprev = tnow;
    return;
}

void ARcommandNull(geometry_msgs::Twist& ARcommand) {
    ARcommand.linear.x = 0;
    ARcommand.linear.y = 0;
    ARcommand.linear.z = 0;
    ARcommand.angular.x = 0;
    ARcommand.angular.y = 0;
    ARcommand.angular.z = 0;
    return;
}

void init() {
    ARcommandNull(ARcommand_pos);
    path[0].x = 0.2;    path[0].y = 0.2;
    path[1].x = 0.8;    path[1].y = 0.2;
    path[2].x = 0.8;    path[2].y = 0.8;
    path[3].x = 0.2;    path[3].y = 0.8;

    PID[0] = 0.15, PID[1] = 0.25, PID[2] = 0.15;
    dPID[0] = 0.05, dPID[1] = 0.05, dPID[2] = 0.05;
    return;
}

int mod (int a, int b)
{
    int ret = a % b;
    if(ret < 0) {
        ret += b;
    }
    return ret;
}

void ARnavigate() {
    errorPrev.x = errorNow.x;
    errorPrev.y = errorNow.y;
    errorNow.x = current.x - dest.x;
    errorNow.y = current.y - dest.y;

    sum.x = (errorNow.x + errorPrev.x)*dtime/2;
    sum.y = (errorNow.y + errorPrev.y)*dtime/2;

    diff.x = (errorNow.x - errorPrev.x)/dtime;
    diff.y = (errorNow.y - errorPrev.y)/dtime;

    if(current.x > dest.x+posRange || current.x < dest.x-posRange) {
        ARcommand_pos.linear.y = +PID[0]*(errorNow.x) + PID[1]*sum.x + PID[2]*diff.x;
    }
    else {
        ARcommand_pos.linear.y = 0;
    }

    if(current.y > dest.y+posRange || current.y < dest.y-posRange) {
        ARcommand_pos.linear.x = -PID[0]*(errorNow.y) - PID[1]*sum.y - PID[2]*diff.y;
    }
    else {
        ARcommand_pos.linear.x = 0;
    }
    return;
}

void stayCounter() {
    if(ARcommand_pos.linear.x == 0 && ARcommand_pos.linear.y == 0 && ARcommand_pos.linear.z == 0 && ARcommand_pos.angular.z == 0) {
        if(stay_count < minStay) {
            stay_count++;
        }
        else {
            stay_count = 0;
            if(pos_count < (pathPoints - 1)) {
                pos_count++;
            }
            else {
                pos_count = 0;
            }        }
    }
}

int main(int argc, char **argv)
{
    ros::init(argc, argv, "whycon_ardrone");

    ros::NodeHandle ardrone;

    init();

    cv::Mat image = cv::imread("/home/ameerhamzakhan/image.jpg");
    cv::imshow("Image", image);

    //usleep(2000000);

    ros::Publisher pub_ARtakeoff = ardrone.advertise<std_msgs::Empty>("/ardrone/takeoff", 1000);
    ros::Publisher pub_ARland = ardrone.advertise<std_msgs::Empty>("/ardrone/land", 1000);
    ros::Publisher pub_ARreset = ardrone.advertise<std_msgs::Empty>("/ardrone/reset", 1000);
    ros::Publisher pub_ARcommand = ardrone.advertise<geometry_msgs::Twist>("/cmd_vel", 1000);

    ros::Subscriber sub_ARnavdata = ardrone.subscribe<ardrone_autonomy::Navdata>("/ardrone/navdata", 10, callBack_navdata);
    ros::Subscriber sub_whyconPoses = ardrone.subscribe<geometry_msgs::PoseArray>("/whycon/trans_poses", 100, callBack_poses);

    ros::Rate loop_rate(25);

    char commandKey = 0;
    int sig_t = 0;

    while(ros::ok()) {
        ros::spinOnce();

        if(commandKey == 'r') {
            pub_ARreset.publish(emp);
            std::cout << commandKey << std::endl;
        }
        else if(commandKey == 't') {
            pub_ARtakeoff.publish(emp);
            usleep(4000000);
            sig_t = 1;
            std::cout << commandKey << std::endl;
        }
        else if(commandKey == 'l') {
            pub_ARland.publish(emp);
            ARcommandNull(ARcommand_pos);
            sig_t = 0;
            std::cout << commandKey << std::endl;
        }

        std::cout << " ===================================================== " << std::endl;
        std::cout << "Position      x: " << ARcommand_pos.linear.x << "     y: " << ARcommand_pos.linear.y << std::endl;
        std::cout << "stay_count: " << stay_count << std::endl;
        std::cout << dest.x << "  " << dest.y << std::endl;
        std::cout << "Position count:  " << pos_count << std::endl;
        std::cout << "time:  " << tnow << "   " << tprev << "   " << dtime << std::endl;
        std::cout << "PID   :" << PID[0] << "   " << PID[1] << "   " << PID[2] << std::endl;
        std::cout << "dPID  :" << dPID[0] << "   " << dPID[1] << "   " << dPID[2] << std::endl;

        if(pos_count == 1)    sigStartGoodness = 1;

        dest.x = path[pos_count].x;
        dest.y = path[pos_count].y;

        if(sig_t) {
            ARnavigate();
            stayCounter();
        }

        pub_ARcommand.publish(ARcommand_pos);

        loop_rate.sleep();

        commandKey = cv::waitKey(1);
    }

    ros::spin();

    return 0;
}
 

Bartman

Bartman

Welcome to MultiRotorForums.com!!
@Ambreen
I understand you are trying to get answers to your questions but please stop sending your question to everyone at the site via email, PM's, etc.

Thanks,
Bart
 


You must log in or register to reply here.
Top