How to set states for robot to search for a gate

Tutorial

How to set states for robot to search for a gate

Dates
  • Creation: 03/23/2020
  • Update: 08/26/2020
Members
unity-rosbridge2020-unity2ros-tut1


Tutorial Context




In this tutorial, we are given gate corners on a camera video feed and we use this video feed to determine whether the drone is approaching the gate, whether it has missed the gate and other possible conditions.



  • Trajectory calculation is the subject of a different tutorial, to be published next month.
  • Visual odometry is the subject of a different tutorial linked below: "How to make a robot know its position using only gate corners".


# Understanding the context of this code: a drone race

As you saw in the video, this code forms part of a larger PID control code for a virtual drone to fly through 6 gates in a drone race.


Fortunately, the drone knows a few key elements:

#define NUM_GATES 6
	int gate_sequence[NUM_GATES] =  {10, 21, 2, 13, 23, 6};
  • Code above: the order of the gates to be followed.
extern double gate_nominal_position[NUM_GATES][4][3];
  • The nominal position of each gate. This position is approximate and helps insofar as allowing the drone to rotate toward the gate.
extern std::vector<flightgoggles::IRMarkerArray> ir_markersVec;
  • The corner information is extracted from the Unity3D simulator.
extern bool collision_flag;

The collision_flag goes up when the drone hits an obstacle. And then everything is reinitialised. For the sake of this tutorial, it sets our drone to search mode.



Finally we will be building our code on this foundation. As passing gates is our primary objective, let's see how we will process gate information.


# Gate information

The gate information updates everytime the corners are detected on the image. To do so, we first define gate parameters.

Please pay attention to these parameters: they govern the rest of the code.


typedef struct {  
  int target_index;  //the number of the current gate
  VECTOR2 midpoint, lgm, corner[4], lgc[4]; 
  //using predefined VECTOR2 type ie. (x,y) coordinates
  //midpoint of gate on screen
  //lgm: last good midpoint, in case we lose it
  //corner[4]: all 4 corners
  //lgc[4]: all 4 last good corners in case we lose it
  cv::Mat transform;  //to store 3D position of gate
  double length, distance;  
  //length: the side length between 2 adjescent corners. 
  bool found;  
  //TRUE when corners are detected AND belong to the correct gate.
  //FALSE otherwise.
  bool passed;
  //TRUE when a gate length existed but now has disappeared: it's a way of saying we passed the gate!
  //FALSE otherwise.
} GATE_INFO; 


Tada! Now the logic that governs this system is clear.

Now we are ready to follow the drone through its race and discover the code as soon as it is processed.



# Extracting the corner information

All our code stems from the corner information. These corners are given in the simulator that we use: Unity3D.

Corners are found in the ir_markers vector that publishes to our code (through a rosbridge, the subject of a tutorial linked below).

By extracting corner information, we also take this opportunity to set the boolean gate_info-> found. Remember as above: a gate is found if the corners do in fact belong to the gate.

//first we reinitialise all the variables
//but we want to skip everything if there's no markers present.
if(!ir_markersVec.size())
	    return;
  for (int i = 0; i < 4; i++)
	    gate_info->corner[i] = {0, 0};
  gate_info->midpoint = {0, 0};
  int n_corners = 0;
  sprintf(buffer, "Gate%d", gate_sequence[gate_info->target_index]); // this adds the current gate to buffer
  gate_info->found = false;
  
  for (int i = 0; i < ir_markersVec.back().markers.size(); i++) {    
  if (strcmp(ir_markersVec.back().markers[i].landmarkID.data.c_str(), buffer) == 0) {  
//the buffer previously stores the gate number: here we are checking that corners belong to the correct gate.    
      gate_info->found = true;    
  }  
}
  

We also set our midpoint as you can see: by summing up the coordinates and averaging them out. Not too clean I agree - but it sure is a fast solution.

//within the corner loop seen above
	      gate_info->midpoint.x += gate_info->corner[k].x = ir_markersVec.back().markers[i].x;
	      gate_info->midpoint.y += gate_info->corner[k].y = ir_markersVec.back().markers[i].y;
	      n_corners++;
//outside of the loop
	  if (n_corners) {
	    gate_info->midpoint.x /= n_corners;
	    gate_info->midpoint.y /= n_corners;
	  }

Now we have the corners and the current (averaged) midpoint.



Depending on whether these corners stay in the video feed, we should tell if the correct gate has been found, lost, not yet found, or even just stopping this process altogether at the end of the race! These are states.


# Alternating the gate_state

We choose 3 states for the drone: GATE_LOOKING, GATE_FOUND and RACE_FINISHED. These are defined in an enum and they update every time the drone passes a gate:


do {
	    gate_info.target_index = gate_target;
	    gateInfo(&gate_info); //gets corners, orders them and determines if a gate was passed
	    getGatePose(&gate_info); //finds the drone position with respect to the gate
        if ((gate_state == GATE_LOOKING) && (gate_info.found)) {      
gate_state = GATE_FOUND; //now we will navigate towards the gate          
        } else if ((gate_state == GATE_FOUND) && (gate_info.passed)) {           
        num_passed_gates++;            
          if (++gate_target >= NUM_GATES) {        
          gate_state = RACE_FINISHED;              
          } else {        
          gate_state = GATE_LOOKING;              
          }    
        } else if ((gate_state == GATE_FOUND) && (!gate_info.found)) {      
        gate_state = GATE_LOOKING;          
        }  
} while (gate_info.passed);


Did you see the functions GatePose() and gateInfo()? They are crucial to understanding the movement of the gate in time. We will get back to them once we're done with our states.


# RACE_FINISHED state

Easy: this ends the code.


# GATE_LOOKING state


When the drone is in this state, it searchs for the next obstacle. We only need it for a few gates.

typedef struct {
		double r, p, y, t;
	} RATE_THRUST;


4 parameters define the flight of a drone: Roll, pitch, yaw and thrust. You can find them here with their + direction of rotation:



We set default command values since (if you remember!) we are given a nominal position for the gates.

RATE_THRUST gate_looking_default[NUM_GATES] = {
	  0, 0, 0, 0, // Gate10
	  0, 0, 0, 0, // Gate21
	  -0.80, 0.0, 1.5, 10,  // Gate2
	  -0.5, 0, 1, 10, // Gate13
	  1.3, 0, -2.6, 12, // Gate23 from 9
	  1.1, 0, -2.2, 15  // Gate6
	};

double roll, pitch, yaw, thrust;  
if (gate_state == GATE_FOUND) {
//do x
  } else {    // Default suggestion when gate is not found  	
roll = gate_looking_default[gate_target].r;  	
pitch = gate_looking_default[gate_target].p;  	
yaw = gate_looking_default[gate_target].y;  	
thrust = gate_looking_default[gate_target].t;  }


# GATE_FOUND state

Now we drive towards the gate. And for the moment being, all the commands will be defaulted to their standard format: velocity = GAIN x (error)

Where (error) is the difference betwen the point the drone is aiming towards with its current settings and the actual midpoint of the gate on the screen.

// Set our control values  
double roll, pitch, yaw, thrust;
// Set a default gain for each gate.
double lrnY[NUM_GATES] = {5,5,5,5,7.5,5};
// Set an initial target point: screen center
VECTOR2 target_point = {screen.x/2, screen.y/2};
  
if (gate_state == GATE_FOUND) {    
  // Initial learned settings    
  yaw = lrnY[gate_target] * (target_point.x - gate_info.midpoint.x) / screen.x;
   
  // Default    
  roll = -yaw/2;
  
  pitch = 0.1 - 0.5 * (target_point.y - gate_info.midpoint.y) / screen.y;    
  pitch = std::max(pitch, -0.2);

  // Initial learned settings    
  thrust = 9.9 + 1 * (target_point.y - gate_info.midpoint.y) / screen.y;
  }


So, GATE_LOOKING and GATE_FOUND have been coded. There's a condition that arises: what if we are flying towards a gate in plain view, and then we just miss it?



# The 'skirt' condition

This condition triggers when the drone has just missed an obstacle. It sets the state of the drone to GATE_LOOKING.

  //Determine a centerpoint from the center of the screen with a pre-defined shift
double centerpoint = screen.x/2-fudgefactor[gate_target];
// Make sure we didn't skirt the edge  
if (((gate_info->lgc[0].x > 1) && (gate_info->lgc[0].x > centerpoint)) || 
  ((gate_info->lgc[3].x > 1) && (gate_info->lgc[3].x > centerpoint)) ||      
  ((gate_info->lgc[1].x > 1) && (gate_info->lgc[1].x < centerpoint)) || 
  ((gate_info->lgc[2].x > 1) && (gate_info->lgc[2].x < centerpoint))) {  
    
  if (gate_info->passed)      
  ROS_INFO("Skirts!  %f %f %f %f %f", screen.x / 2,        gate_info->lgc[0].x, gate_info->lgc[1].x, gate_info->lgc[2].x, gate_info->lgc[3].x);    
  gate_info->passed = false;  
  }


Then there's this condition:

# The 'don't hit the wall' condition


Don't land on the wall. This actually gives rise to a new section of code: setting gate_info -> passed before you pass the gate, and measuring out the right distance so that the momentum will keep the drone flying.


  gate_info->passed = ((gate_info->length == -1) && (prev_length != -1) // -1 is used to encode 'no-info' so it indicates no two corners existed in the current frame 
//(gate_info->length) but did in the previous one (prev_length)
	      && (prev_length > 200)); 
	  // Gate 2 & 6 hack
	  if (((gate_info->target_index == 1) || (gate_info->target_index == 5))
	  		&& (n_corners <= 2) && (prev_length > 200)) {
	  	gate_info->passed = true;
	  }

In this tutorial, we have understood the different states managed by a drone.


How do we calculate gate length? And how do we keep the length from the previous frame? See the tutorial below: 'How to make a robot know its position using only gate corners'.