Pattern follower

Teleoperate the real robot: Erle-Copter

Source code



REMINDER: configure your environment before starting this tutorial.


Download and compile the ROS node in your workspace

cd ~/simulation/ros_catkin_ws/src
git clone https://github.com/erlerobot/ros_erle_pattern_follower
cd ..
catkin_make --pkg ros_erle_pattern_follower

This node is able to:

  • Detect markers with the bottom cam of the Erle-Copter
  • Follow the marker

Launch the simulation

source ~/simulation/ros_catkin_ws/devel/setup.bash
cd ~/simulation/ardupilot/ArduCopter
../Tools/autotest/sim_vehicle.sh -j 4 -f Gazebo
# once MAVProxy has launched completely, load the parameters
param load /[path_to_your_home_directory]/simulation/ardupilot/Tools/Frame_params/Erle-Copter.param
# NOTE: replace [path_to_your_home_directory] with the actual path to your home directory.
# Example: param load /home/john/simulation/ardupilot/Tools/Frame_params/Erle-Copter.param

In another terminal

source ~/simulation/ros_catkin_ws/devel/setup.bash
roslaunch ardupilot_sitl_gazebo_plugin erlecopter_mark.launch.launch
Figure 1 - General view of the simulation. The building corresponds to the first floor of Erle Robotics' office.
Figure 2 - Erle-Copter model with the mark model in Gazebo simulator

Make the copter take off. In the first terminal execute:

# in the MAVProxy prompt:
mode GUIDED
arm throttle
takeoff 2
param set SYSID_MYGCS 1

Launch the pattern follower node. This node will detect and follow the marker in the floor.

source ~/simulation/ros_catkin_ws/devel/setup.bash
rosrun  ros_erle_pattern_follower ros_erle_pattern_follower

You should see the following window:

Figure 3 - Bottom cam view of the Erle-Copter simulation

In order to the follower node to start, we need to change the flight mode to LOITER. To do that, execute in the first terminal:

# in the MAVProxy prompt:
mode LOITER

Now is when the fun part starts! Move the mark model with the translation tool (T) around the office and watch the copter following it.

Figure 4 - Execution of the simulation with the mark follower

Get Erle-Copter at

Rather than having to move the marker manually, let's make it move automatically so we can trully test the potential of the algorithm. To do so, compile the following gazebo plugin by running:

catkin_make --pkg mark_plugin

Then, change the <shape> </shape> tag inside ~/.gazebo/models/aruco0/model.sdf with one of the followings:

  • <shape>dot</shape> will make it stay in the place.
  • <shape>square</shape> will follow the shape of a square.
  • <shape>circle</shape> will follow the shape of a circle.

Relaunch the simulation. Or simply insert the mark from the Insert tab in Gazebo.

Here is the code of the node

#include <ros/ros.h>
#include <image_transport/image_transport.h>
#include <opencv2/highgui/highgui.hpp>
#include <cv_bridge/cv_bridge.h>
#include <aruco/aruco.h>
#include <iostream>
#include <mavros/OverrideRCIn.h>
#include <mavros/State.h>

#define FACTOR  0.6

#define MINRC   1100
#define BASERC  1500
#define MAXRC   1900

// Subscriber to bottom camera
image_transport::Subscriber sub;

// Subscriber to flight mode
ros::Subscriber mavros_state_sub;

// RC publisher
ros::Publisher pub;

// Time control
ros::Time lastTime;

// Mark info
float MarkX, MarkY; // Mark center
float lastMarkX, lastMarkY; // Last mark center
double lastMarkVelX, lastMarkVelY; // Last mark velocity

//Image center
float ImageX, ImageY;

double Roll, Pitch;

// Flight mode
std::string mode;
bool guided;
bool armed;

void imageCallback(const sensor_msgs::ImageConstPtr& msg)
{
    try
    {
        // Time since last call
        double timeBetweenMarkers = (ros::Time::now() - lastTime).toSec();
        lastTime = ros::Time::now();

        char tab2[1024];
        strncpy(tab2, mode.c_str(), sizeof(tab2));
        tab2[sizeof(tab2) - 1] = 0;
        ROS_INFO("Marker = (%f , %f) | LastMarker = (%f , %f) \n timeBetweenMarkers = %fs | lastMarkVelX = (%f , %f)\n Roll = %f | Pitch = %f\n Mode = %s \n", MarkX, MarkY, lastMarkX, lastMarkY, timeBetweenMarkers, lastMarkVelX, lastMarkVelY, Roll, Pitch, tab2);

        aruco::MarkerDetector MDetector;
        vector<aruco::Marker> Markers;
        cv::Point2f MarkCenter;

        // Get the msg image
        cv::Mat InImage;
        InImage = cv_bridge::toCvShare(msg, "bgr8")->image;

        // Error between Image and Mark
        float ErX = 0.0;
        float ErY = 0.0;

        // Get the Image center
        ImageX = InImage.cols / 2.0f;
        ImageY = InImage.rows / 2.0f;

        // Detect markers
        MDetector.detect(InImage,Markers);

        // Create RC msg
        mavros::OverrideRCIn msg;

        lastMarkX = MarkX;
        lastMarkY = MarkY;

        // For each marker, draw info ant its coundaries in the image
        for (unsigned int i = 0; i<Markers.size(); i++){
            Markers[i].draw(InImage,cv::Scalar(0,0,255),2);

            // Calculate the error between Image center and Mark center
            MarkCenter = Markers[i].getCenter();
            MarkX = MarkCenter.x;
            MarkY = MarkCenter.y;
            ErX = ImageX - MarkX;
            ErY = ImageY - MarkY;
        }

        // Calculate velocity
        if (timeBetweenMarkers < 1.0){
            lastMarkVelX = (lastMarkX - MarkX)/timeBetweenMarkers;
            lastMarkVelY = (lastMarkY - MarkY)/timeBetweenMarkers;
        } else{
            lastMarkVelX = 0.0;
            lastMarkVelY = 0.0;
        }

        // Calculate Roll and Pitch depending on the mode
        if (mode == "LOITER"){
            Roll = BASERC - ErX * FACTOR;
            Pitch = BASERC - ErY * FACTOR;
        }else if (mode == "ALT_HOLD"){
            Roll = BASERC - (0.5*ErX+0.1*lastMarkVelX);
            Pitch = BASERC - (0.5*ErY+0.1*lastMarkVelY);
        }else{
            Roll = BASERC;
            Pitch = BASERC;
        }  

        // Limit the Roll
        if (Roll > MAXRC)
        {
            Roll = MAXRC;
        } else if (Roll < MINRC)
        {
            Roll = MINRC;
        }

        // Limit the Pitch
        if (Pitch > MAXRC)
        {
            Pitch = MAXRC;
        } else if (Pitch < MINRC)
        {
            Pitch = MINRC;
        }

        msg.channels[0] = Roll;     //Roll
        msg.channels[1] = Pitch;    //Pitch
        msg.channels[2] = BASERC;   //Throttle
        msg.channels[3] = 0;        //Yaw
        msg.channels[4] = 0;
        msg.channels[5] = 0;
        msg.channels[6] = 0;
        msg.channels[7] = 0;

        pub.publish(msg);

        cv::imshow("view", InImage);
        cv::waitKey(30);
    }
    catch (cv_bridge::Exception& e)
    {
        ROS_ERROR("Could not convert from '%s' to 'bgr8'.", msg->encoding.c_str());
    }
}

void mavrosStateCb(const mavros::StateConstPtr &msg)
{
    if(msg->mode == std::string("CMODE(0)"))
        return;
    //ROS_INFO("I heard: [%s] [%d] [%d]", msg->mode.c_str(), msg->armed, msg->guided);
    mode = msg->mode;
    guided = msg->guided==128;
    armed = msg->armed==128;
}

int main(int argc, char **argv)
{
    ros::init(argc, argv, "image_listener");
    ros::NodeHandle nh;
    lastTime = ros::Time::now();
    image_transport::ImageTransport it(nh);
    sub = it.subscribe("/erlecopter/bottom/image_raw", 1, imageCallback);
    mavros_state_sub = nh.subscribe("/mavros/state", 1, mavrosStateCb);
    pub = nh.advertise<mavros::OverrideRCIn>("/mavros/rc/override", 10);;
    ros::spin();
}

Now, let's break the code down.

#include <ros/ros.h>
#include <image_transport/image_transport.h>
#include <opencv2/highgui/highgui.hpp>
#include <cv_bridge/cv_bridge.h>
#include <aruco/aruco.h>
#include <iostream>
#include <mavros/OverrideRCIn.h>
#include <mavros/State.h>

Headers that need to be included.

#define FACTOR 0.6

The factor used to calculate the Roll and Pitch needed to follow the mark.

#define MINRC   1100
#define BASERC  1500
#define MAXRC   1900

Minimum, base and max values for the RC of the Erle-Copter.

// Subscriber to bottom camera
image_transport::Subscriber sub;

// Subscriber to flight mode
ros::Subscriber mavros_state_sub;

// RC publisher
ros::Publisher pub;

First set of global variables. The first one is the subscriber fot the bottom camera of the copter, the second one subscribes to the flight mode of the copter and the last one the publisher that overrides the values of the RC.

// Time control
ros::Time lastTime;

Variable to control the time between loops.


// Mark info
float MarkX, MarkY; // Mark center
float lastMarkX, lastMarkY; // Last mark center
double lastMarkVelX, lastMarkVelY; // Last mark velocity

Information about the mark. First, the X and Y coordenates of the center of the mark. This is where we want the Erle-Copter to be placed. Then, the last coordinates of the mark. And finally, the velocity of the last mark read.

//Image center
float ImageX, ImageY;

X and Y coordenates of the image of the camera. Our objective is to ake this coordenates and the coordenates of the mark equal.

double Roll, Pitch;

Roll and pitch values.

// Flight mode
std::string mode;
bool guided;
bool armed;

Information about the flight mode.

void imageCallback(const sensor_msgs::ImageConstPtr& msg)
{

Callback for the ros topic of the bottom camera of the copter. Everytime a new message arrives, this method will be called.

try
{

    //...

}catch (cv_bridge::Exception& e)
{
      ROS_ERROR("Could not convert from '%s' to 'bgr8'.", msg->encoding.c_str());
}

In case cv_bridge::toCvShare fails to convert the image to bgr8.

// Time since last call
double timeBetweenMarkers = (ros::Time::now() - lastTime).toSec();
lastTime = ros::Time::now();

Calculation of the time since the last call to the callback. This is used later to estimate the velocity of the marker.

aruco::MarkerDetector MDetector;

This is the marker detector implemented by aruco.

vector<aruco::Marker> Markers;

Vector of markers detected by the marker detector.

cv::Point2f MarkCenter;

Point that represents the center of the mark detected.

// Get the msg image
cv::Mat InImage;
InImage = cv_bridge::toCvShare(msg, "bgr8")->image;

Get the image that contains the message and convert it to bgr8.

// Error between Image and Mark
float ErX = 0.0;
float ErY = 0.0;

The error between the image and the mark center. Our objective is to bring it to zero.

// Get the Image center
ImageX = InImage.cols / 2.0f;
ImageY = InImage.rows / 2.0f;

Calculate the center of the image.

// Detect
MDetector.detect(InImage,Markers);

This is where magic happens. This method detects the markers in the image and stores them in Markers, the vector of markers previosly created.

// Create RC msg
mavros::OverrideRCIn msg;

Create the msg to override the RC of the copter via mavros.

lastMarkX = MarkX;
lastMarkY = MarkY;

Ẁe store the position of the last mark. This is used to calculate the velocity later.

// For each marker, draw info ant its coundaries in the image
for (unsigned int i = 0; i<Markers.size(); i++){
    Markers[i].draw(InImage,cv::Scalar(0,0,255),2);

    // Calculate the error between Image center and Mark center
    MarkCenter = Markers[i].getCenter();
    MarkX = MarkCenter.x;
    MarkY = MarkCenter.y;
    ErX = ImageX - MarkX;
    ErY = ImageY - MarkY;
}

For each marker detected, draw it in the image. Then, calculate the error between the center of the mark and the center of the image.

 // Calculate velocity
if (timeBetweenMarkers < 1.0){
    lastMarkVelX = (lastMarkX - MarkX)/timeBetweenMarkers;
    lastMarkVelY = (lastMarkY - MarkY)/timeBetweenMarkers;
} else{
    lastMarkVelX = 0.0;
    lastMarkVelY = 0.0;
}

Calculation of the velocity vector of the marker. Velocity = Distance / Time.

// Calculate Roll and Pitch depending on the mode
if (mode == "LOITER"){
    Roll = BASERC - ErX * FACTOR;
    Pitch = BASERC - ErY * FACTOR;
}else if (mode == "ALT_HOLD"){
    Roll = BASERC - (0.5*ErX+0.1*lastMarkVelX);
    Pitch = BASERC - (0.5*ErY+0.1*lastMarkVelY);
}else{
    Roll = BASERC;
    Pitch = BASERC;
}

We need to adjust the values of Roll and Pitch depending on the fligt mode.

// Limit the Roll
if (Roll > MAXRC)
{
    Roll = MAXRC;
} else if (Roll < MINRC)
{
    Roll = MINRC;
}

// Limit the Pitch
if (Pitch > MAXRC)
{
    Pitch = MAXRC;
} else if (Pitch < MINRC)
{
    Pitch = MINRC;
}

Since Roll and Pitch have a maximum and a minimum value, we need to limit them.

msg.channels[0] = Roll;     //Roll
msg.channels[1] = Pitch;    //Pitch
msg.channels[2] = BASERC;   //Throttle
msg.channels[3] = 0;        //Yaw
msg.channels[4] = 0;
msg.channels[5] = 0;
msg.channels[6] = 0;
msg.channels[7] = 0;

pub.publish(msg);

Store the values of the RC into the message and publish it.

cv::imshow("view", InImage);
cv::waitKey(30);

Show the image on the screen.

void mavrosStateCb(const mavros::StateConstPtr &msg)
{
    if(msg->mode == std::string("CMODE(0)"))
        return;
    //ROS_INFO("I heard: [%s] [%d] [%d]", msg->mode.c_str(), msg->armed, msg->guided);
    mode = msg->mode;
    guided = msg->guided==128;
    armed = msg->armed==128;
}

Callback method for the flight mode of the copter.

int main(int argc, char **argv)
{
    ros::init(argc, argv, "image_listener");
    ros::NodeHandle nh;
    lastTime = ros::Time::now();
    image_transport::ImageTransport it(nh);
    sub = it.subscribe("/erlecopter/bottom/image_raw", 1, imageCallback);
    mavros_state_sub = nh.subscribe("/mavros/state", 1, mavrosStateCb);
    pub = nh.advertise<mavros::OverrideRCIn>("/mavros/rc/override", 10);;
    ros::spin();
}

Main method of the script. It does the usual things that a subscriber node does: initialize ROS, subscribe to a topic (in this case the bottom camera of the copter and its flight mode) and advertise the publisher.

In case you need to adapt this code to your necesities here is a branch of the project that will help you out.