-
Notifications
You must be signed in to change notification settings - Fork 38
TutorialAdvanced
This tutorial goes through all the components of the library with examples on how to develop custom sensors, detector, avoidance strategies and vehicles. It is a demonstration of the library features, but by no means is a representative of how a program must be developed. Custom versions of sensors and vehicles are likely to only be used by people working directly in the library code. Still, this tutorial demonstrates how to customize every part of the program flow.
This tutorial assumes you've been through the Basic Tutorial first. Every custom component developed here can be plugged directly into the flow explained in the Basic Tutorial without the need for replacing all the components at once.
- Writing a custom Sensor
- Writing a custom Vehicle
- Writing a custom Detector
- Writing a custom Avoidance Strategy
- Wrapping it up
For this tutorial we will write a custom depth camera sensor. The base class for depth camera is called DepthCamera and can be found in the common/DepthCamera.hh header. This will be a simulated camera that always return a clear view, but it can be easily modified to return custom data to fit your needs.
First, we need to define our class. There are two methods that must be implemented in every custom DepthCamera: The Constructor and the get_depth_buffer() method. Depending on your camera, you may want to override the get_horizontal_fov() and get_vertical_fov() methods as well, but the class already has a basic implementation that will fit most simple cases.
class MyCamera : public DepthCamera
{
public:
MyCamera(size_t width, size_t height);
vector<uint16_t> &get_depth_buffer() override;
private:
std::vector<uint16_t> depth_buffer;
};The Constructor in our example will be responsible for setting the buffer size and initializing it to an empty buffer. Since an empty buffer represents a clear view reading, all that our get_depth_buffer() method needs to do is return that buffer.
MyCamera::MyCamera(size_t width, size_t height)
{
this->width = width;
this->height = height;
this->depth_buffer.resize(width * height, 0);
}
vector<uint16_t> &MyCamera::get_depth_buffer()
{
return this->depth_buffer;
}For variables and methods available for the DepthCamera class, check the class header.
For our vehicle we will write a custom quad copter. The base class for the that vehicle is the QuadCopter. There are three methods that we need to implement to fill the class interface: target_pose(), which returns the vehicle mission target, vehicle_pose(), which returns the current vehicle pose, and set_target-pose(), which sets a new mission target. First, we declare the class header.
class MyQuad : public QuadCopter
{
public:
MyQuad(Pose initial_pose);
Pose target_pose();
Pose vehicle_pose();
void set_target_pose(Pose pose);
void move_to(Pose pose);
private:
Pose pose;
Pose target;
};For this simple class, we will create a simple move_to() function that sets the vehicle position to the provided destination to simulate movement. All the other methods are straight forward. In a realistic scenario, you will want to use those methods to actuate the vehicle.
MyQuad::MyQuad(Pose initial_pose)
{
this->pose = pose;
}
Pose MyQuad::target_pose()
{
return target;
}
Pose MyQuad::vehicle_pose()
{
return this->pose;
}
void MyQuad::set_target_pose(Pose pose)
{
this->target = pose;
}
void MyQuad::move_to(Pose destination)
{
this->pose = destination;
}For a more elaborate example, check the code of MavQuadCopter, the class responsible for communicating with Mavlink vehicles.
A Detector in the library is a template class that is tied to a type of sensor. It has one core method: const vector<Obstacle> detect(). This method is responsible for reading and analysing the sensor data and return a list of Obstacles that its algorithm detected.
For this tutorial, we will write a detector that can operate on DepthCamera sensors, which allows it to work with our custom sensor and also with other depth cameras already in the library. That is what the detector declaration looks like:
class MyDetector : public Detector<DepthCamera>
{
public:
MyDetector(shared_ptr<DepthCamera> depth_camera);
const vector<Obstacle> &detect() override;
private:
vector<Obstacle> obstacles;
};In this detector we will go through all the depth readings provided the depth camera and if there is a reading that is different than 0 (which mean the camera has detected something), we will create an obstacle right in front of the vehicle with its distance to the vehicle given by that reading. This is a very simple example that is not usable with real data (due to noise in the depth camera readings), but it illustrates the core concepts of developing a detector.
MyDetector::MyDetector(shared_ptr<DepthCamera> depth_camera)
{
this->sensor = depth_camera;
}
const vector<Obstacle> &MyDetector::detect()
{
std::vector<uint16_t> depth_buffer = this->sensor->get_depth_buffer();
this->obstacles.clear();
for (uint16_t val : depth_buffer) {
if (val == 0)
continue;
Obstacle obs;
obs.center = glm::dvec3(val, M_PI/2, M_PI/2);
obstacles.push_back(obs);
break;
}
return obstacles;
}Note that the obstacle uses polar coordinates as described here. For an example of a practical detector, check the source for Obstacle Detector.
As with detectors, Avoidance Strategies are template classes that are tied to a vehicle type. For this tutorial we will write an avoidance strategy that can operate any vehicle of the QuadCopter class. This allow us to use it with our custom vehicle and also other QuadCopter vehicles. Avoidance strategies have one core method: void avoid(const std::vector<Obstacle> &detection). This method receives a list of obstacles (usually directly from a detector's detect() method) and determines if it must actuate on the vehicle to avoid an obstacle.
This is what the declaration of our custom avoidance strategy looks like:
class MyAvoidance : public CollisionAvoidanceStrategy<QuadCopter>
{
public:
MyAvoidance(std::shared_ptr<MyQuad> my_quad);
void avoid(const std::vector<Obstacle> &detection) override;
};For this tutorial, we will check the obstacle list and if there is any obstacle there we will stop the vehicle. We do so by setting the vehicle target to its current position. Doing so will tell the vehicle that it has reached its destination and does not need to keep moving. As with our custom detector, this is a simplified version of a detector to demonstrate its core concepts. A more realistic detector would check the position of the obstacle to determine if it is in a collision course with the vehicle.
MyAvoidance::MyAvoidance(std::shared_ptr<MyQuad> my_quad)
{
this->vehicle = my_quad;
}
void MyAvoidance::avoid(const std::vector<Obstacle> &detection)
{
if (detection.size() != 0) {
this->vehicle->set_target_pose(this->vehicle->vehicle_pose());
}
}For a more complex implementation of an avoidance strategy, check the source for Quadcopter Shift Avoidance.
This is a sample app that puts together all our custom classes an add them to the main() implementation of the Basic Tutorial.
#include <iostream>
#include <memory>
#include <vector>
#include "avoidance/QuadCopterStopAvoidance.hh"
#include "common/common.hh"
#include "common/DepthCamera.hh"
#include "detection/DepthImageObstacleDetector.hh"
#include "sensors/RealSenseCamera.hh"
#include "vehicles/MavQuadCopter.hh"
using namespace std;
/* Custom Senson */
class MyCamera : public DepthCamera
{
public:
MyCamera(size_t width, size_t height);
vector<uint16_t> &get_depth_buffer() override;
private:
std::vector<uint16_t> depth_buffer;
};
MyCamera::MyCamera(size_t width, size_t height)
{
this->width = width;
this->height = height;
this->depth_buffer.resize(width * height, 0);
}
vector<uint16_t> &MyCamera::get_depth_buffer()
{
return this->depth_buffer;
}
/* Custom Vehicle */
class MyQuad : public QuadCopter
{
public:
MyQuad(Pose initial_pose);
Pose target_pose();
Pose vehicle_pose();
void set_target_pose(Pose pose);
void move_to(Pose pose);
private:
Pose pose;
Pose target;
};
MyQuad::MyQuad(Pose initial_pose)
{
this->pose = pose;
}
Pose MyQuad::target_pose()
{
return target;
}
Pose MyQuad::vehicle_pose()
{
return this->pose;
}
void MyQuad::set_target_pose(Pose pose)
{
this->target = pose;
}
void MyQuad::move_to(Pose destination)
{
this->pose = destination;
}
/* Custom Detector */
class MyDetector : public Detector<DepthCamera>
{
public:
MyDetector(shared_ptr<DepthCamera> depth_camera);
const vector<Obstacle> &detect() override;
private:
vector<Obstacle> obstacles;
};
MyDetector::MyDetector(shared_ptr<DepthCamera> depth_camera)
{
this->sensor = depth_camera;
}
const vector<Obstacle> &MyDetector::detect()
{
std::vector<uint16_t> depth_buffer = this->sensor->get_depth_buffer();
this->obstacles.clear();
for (uint16_t val : depth_buffer) {
if (val == 0)
continue;
Obstacle obs;
obs.center = glm::dvec3(val, M_PI/2, M_PI/2);
obstacles.push_back(obs);
break;
}
return obstacles;
}
/* Custom Avoidance */
class MyAvoidance : public CollisionAvoidanceStrategy<QuadCopter>
{
public:
MyAvoidance(std::shared_ptr<MyQuad> my_quad);
void avoid(const std::vector<Obstacle> &detection) override;
};
MyAvoidance::MyAvoidance(std::shared_ptr<MyQuad> my_quad)
{
this->vehicle = my_quad;
}
void MyAvoidance::avoid(const std::vector<Obstacle> &detection)
{
if (detection.size() != 0) {
this->vehicle->set_target_pose(this->vehicle->vehicle_pose());
}
}
int main(int argc, char **argv)
{
cout << "My custom app" << endl;
Pose vehicle_pose, target_pose;
vehicle_pose.pos = glm::dvec3(0, 0, 0);
target_pose.pos = glm::dvec3(10, 10, 10);
shared_ptr<MyQuad> vehicle = make_shared<MyQuad>(vehicle_pose);
shared_ptr<MyCamera> sensor = make_shared<MyCamera>(640, 480);
shared_ptr<Detector<DepthCamera>> detector = make_shared<MyDetector>(sensor);
shared_ptr<CollisionAvoidanceStrategy<QuadCopter>> avoidance = make_shared<MyAvoidance>(vehicle);
vehicle->set_target_pose(target_pose);
while (true) {
avoidance->avoid(detector->detect());
}
return 0;
}