The new Lidar Lite V3 sensor may be useful for height control of a quadcopter. We want to integrate one of this sensors into our new Asctec Firefly quadcopter for safe indoor flight. The first step of the integration is a complete sensor characterization in order to determine its parameters.

Experimental setup

Materials:

• Arduino UNO
• Electrolytic Capacitor with the value stated on the manual (or with more capacity).
• Lidar Lite v3
• Bosch professional GLM 50 C laser measure device (as ground truth)
• Ruler on the ground

We connected the Lidar Lite v3 to the Arduino UNO using the example configuration found in the manual, we then fixed everything to a board for convenient manipulation. In the software side, we used as reference the example found in the Arduino Library for the Lidar Lite V3 provided by Garmin, we tested that everything was working with our configuration and finally we integrated this code with ROS Serial in order to be able to conveniently process the information in ROS. The final code used in the Arduino with a small tutorial for using with ROS can be found in our Github Repository: TODO!.

A ruler was attached to the ground which allows us to measure distances from 0 to 4 meters in our laboratory. The board with the laser was fixed at position 0 of the ruler.

Note: The base of the Lidar Lite V3 is the base distance reference for all the measurements provided by the laser.

We used for testing another board with a black diffuse material since it gives worse performance than a clear white paper. This board was moved from the maximum distance of the ruler (4m) until the closest distance possible to the laser in intervals of 10 centimeters, for each distance the board was situated perpendicular to the laser and 1000 measurements were obtained for each distance. We developed for this experiment a live histogram tool in ROS (TODO) which connect to the Laser distance topic and then creates a histogram with the data and provides as well the mean value and variance. Additional measurement every 5 centimeters were taken from 0 to 1.5m due to some unexpected non linear effects found on the first pass. The ground truth of the distance was measured using the high precision Bosch professional GLM 50 C device.

The results obtained can be seen in the following graph:

A zoom of the problematic area (distances between 0 and 1.5m):

Relative error (as a percentage of the measured distance):

Histogram at a distance of 3.95m:

Histogram at a distance of 1m:

Histogram at a distance of 50cm:

Histogram at a distance of 30cm:

Histogram at a distance of 10cm:

TODO: Result analysis

First let’s start with some basic assumptions.  We define a 2D environment of size x_max, y_max. We can easily represent this environment in Python as a Grid of size (x_max, y_max). The resolution of this Grid is not important for now, since later it is possible to convert Grid values to meters for example.  We want to simulate an obstacle in a particular position of this environment, let’s say that the obstacle position will be p_obs = (x_obs, y_obs) and that the complete shape of this obstacle can be fitted inside a circle of radius = 10 grid units.

One way of representing this obstacle for potential field navigation is by using a Gaussian repulsor field in the center of the obstacle. The following code creates a Grid and then calculates the potential field for the given obstacle position.

def gaussian_obstacle(X,Y, x_obs, y_obs, size_robot, size_obstacle):
    #We extend the size of the obstacle with the size of the robot (border expansion)
    ext_size = size_robot + size_obstacle
    sigma_x = (ext_size/2)/2.3548
    sigma_y = (ext_size/2)/2.3548
    theta = 0
    A = 100 #Weight of the Gaussian
    Z = zeros_like(X)
    a = cos(theta)**2/2/sigma_x**2 + sin(theta)**2/2/sigma_y**2
    b = -sin(2*theta)/4/sigma_x**2 + sin(2*theta)/4/sigma_y**2
    c = sin(theta)**2/2/sigma_x**2 + cos(theta)**2/2/sigma_y**2
    Z[:] = Z[:]+A*exp( - (a*(X-x_obs)**2 + 2*b*(X-x_obs)*(Y-y_obs) + c*(Y-y_obs)**2))
    return Z

Z = gaussian_obstacle(X, Y, 50, 50, 10, 10)

We then create a gradient field based on the scalar field:

def calculate_vector_field(Z):
    U, V = gradient(Z,1,1)
    U = -U
    V = -V
    return U, V

U, V = calculate_vector_field(Z)

Which will produce this vectors centered in the position of the obstacle

Finally to obtain a vector reference (velocity) for a robot that is currently at position (41,41), you may use the following function:

def get_velocity_command(U, V, x_robot, y_robot):
return U[x_robot,y_robot], V[x_robot,y_robot]

Vx_robot, Vy_robot = get_velocity_command(U, V, 41, 41)

If you have many moving obstacles and a big environment it may be too computationally expensive to do the calculation for the whole Grid, a possible solution is to do the calculation only in the surrounding area of the robot and obtain only the reference vector for the current position of the robot:

p_robot = (rob_x, rob_y)
#We create a minimal grid around the position of our robot
X, Y = mgrid[(rob_x-1):(rob_x+1)+1,(rob_y-1):(rob_y+1)+1]

Complete code with plots using Mayavi from enthought.

from pylab import *
import matplotlib.pyplot as plt
from mayavi import mlab

x_min = 0
y_min = 0
x_max = 100
y_max = 100
X, Y = mgrid[ x_min:x_max+1:1,  y_min: y_max+1:1]

def gaussian_obstacle(X,Y, x_obs, y_obs, size_robot, size_obstacle):
    #We extend the size of the obstacle with the size of the robot (border expansion)
    ext_size = size_robot + size_obstacle
    sigma_x = (ext_size/2)/2.3548
    sigma_y = (ext_size/2)/2.3548
    theta = 0
    A = 100 #Weight of the Gaussian
    Z = zeros_like(X)
    a = cos(theta)**2/2/sigma_x**2 + sin(theta)**2/2/sigma_y**2
    b = -sin(2*theta)/4/sigma_x**2 + sin(2*theta)/4/sigma_y**2
    c = sin(theta)**2/2/sigma_x**2 + cos(theta)**2/2/sigma_y**2
    Z[:] = Z[:]+A*exp( - (a*(X-x_obs)**2 + 2*b*(X-x_obs)*(Y-y_obs) + c*(Y-y_obs)**2))
    return Z

def calculate_vector_field(Z):
    U, V = gradient(Z,1,1)
    U = -U
    V = -V
    return U, V

Z = gaussian_obstacle(X, Y, 50, 50, 10, 10)
U, V = calculate_vector_field(Z)


def get_velocity_command(U, V, x_robot, y_robot):
    return U[x_robot,y_robot], V[x_robot,y_robot]
    
Vx_robot, Vy_robot = get_velocity_command(U, V, 40, 40)

# Plot
mlab.figure(size=(800, 600))
mlab.quiver3d(X, Y, zeros_like(X), U, V,zeros_like(U))
mlab.mesh(X, Y, Z)
mlab.show()

The robotic platform Robotino is a nice omnidirectional Robot manufactured by the company Festo. It was surprising for me to find out that the Robot Simulator VREP doesn’t have a working model of this robot,  even though there exist an opensource model of this robot for the simulator Gazebo.

Just like I did before for a Quadcopter, now I implemented a model of the Robotino for Vrep based on the URDF model from Gazebo. The simulation of the omnidirectional wheels was based on the Kuka Youbot available in Vrep. Additionally I programmed a control script using velocity Twist reference messages from ROS, so just like the quadrotor it is possible to control this robot from ROS using a Joystick (or your custom Node).

Robotino Model in Vrep

This is the control script that I programmed in the Robotino Model:

-- DO NOT WRITE CODE OUTSIDE OF THE if-then-end SECTIONS BELOW!! (unless the code is a function definition)
if (sim_call_type==sim_childscriptcall_initialization) then
-------- START OF ROS INITIALIZATION --------------------------------
 -- Check if the required plugin is there (libv_repExtRos.so or libv_repExtRos.dylib):
 local moduleName=0
 local moduleVersion=0
 local index=0
 local pluginNotFound=true
 while moduleName do
 moduleName,moduleVersion=simGetModuleName(index)
 if (moduleName=='Ros') then
 pluginNotFound=false
 end
 index=index+1
 end
 if (pluginNotFound) then
 -- Display an error message if the plugin was not found:
 simDisplayDialog('Error','ROS plugin was not found.&&nSimulation will not run properly',sim_dlgstyle_ok,false,nil,{0.8,0,0,0,0,0},{0.5,0,0,1,1,1})
 else
 -- Enable topic subscription:
 simExtROS_enableSubscriber("/robotino_cmd_twist",1,simros_strmcmd_set_twist_command,-1,-1,'Command_Twist')
 end

-------- END OF ROS INITIALIZATION --------------------------------
 -- Put some initialization code here

 -- Make sure we have version 2.4.12 or above (the omni-wheels are not supported otherwise)
 v=simGetIntegerParameter(sim_intparam_program_version)
 if (v<20412) then
 simDisplayDialog('Warning','The YouBot model is only fully supported from V-REP version 2.4.12 and above.&&nThis simulation will not run as expected!',sim_dlgstyle_ok,false,'',nil,{0.8,0,0,0,0,0})
 end

 wheelJoints={-1,-1,-1}
 wheelJoints[1]=simGetObjectHandle('wheel0_joint')
 wheelJoints[2]=simGetObjectHandle('wheel1_joint')
 wheelJoints[3]=simGetObjectHandle('wheel2_joint')
 robotino_handle = simGetObjectHandle('robotino')

 -- Make sure you read the section on "Accessing general-type objects programmatically"
 -- For instance, if you wish to retrieve the handle of a scene object, use following instruction:
 --
 -- handle=simGetObjectHandle('sceneObjectName')
 --
 -- Above instruction retrieves the handle of 'sceneObjectName' if this script's name has no '#' in it
 --
 -- If this script's name contains a '#' (e.g. 'someName#4'), then above instruction retrieves the handle of object 'sceneObjectName#4'
 -- This mechanism of handle retrieval is very convenient, since you don't need to adjust any code when a model is duplicated!
 -- So if the script's name (or rather the name of the object associated with this script) is:
 --
 -- 'someName', then the handle of 'sceneObjectName' is retrieved
 -- 'someName#0', then the handle of 'sceneObjectName#0' is retrieved
 -- 'someName#1', then the handle of 'sceneObjectName#1' is retrieved
 -- ...
 --
 -- If you always want to retrieve the same object's handle, no matter what, specify its full name, including a '#':
 --
 -- handle=simGetObjectHandle('sceneObjectName#') always retrieves the handle of object 'sceneObjectName'
 -- handle=simGetObjectHandle('sceneObjectName#0') always retrieves the handle of object 'sceneObjectName#0'
 -- handle=simGetObjectHandle('sceneObjectName#1') always retrieves the handle of object 'sceneObjectName#1'
 -- ...
 --
 -- Refer also to simGetCollisionhandle, simGetDistanceHandle, simGetIkGroupHandle, etc.
 --
 -- Following 2 instructions might also be useful: simGetNameSuffix and simSetNameSuffix
 old_velx = 0
 old_vely = 0
 old_vel_angz = 0
 acc_x = 0
 acc_y = 0
 acc_omega = 0

end

if (sim_call_type==sim_childscriptcall_actuation) then

 -- Put your main ACTUATION code here
 -- From CMDVEL
 -- Signals from ROS
 local packedData=simGetStringSignal('Command_Twist')
 if (packedData) then
 local ref_twist=simUnpackFloats(packedData)
 -- Process the twist data
 -- X and Y velocities (related to the heading of the robotino)
 ref_velx = ref_twist[1]*0.18 --reference values between 0 and 1, Max Vel = 0.18 m/s
 ref_vely = ref_twist[2]*0.18

 -- Angular velocity in Z (this changes the heading of the robotino)
 ref_angz = ref_twist[6]
 end

 -- Robotino Model (Open Loop control)
 omega = ref_angz
 vx = ref_velx
 vy = ref_vely
 -- Sets the distance from robot center to wheel center.
 rb = 0.13
 -- Sets the radius of the wheels.
 rw = 0.040
 -- Sets the gear.
 gear = 0.11 --revisar este numero
 -- Projection matrix
 v0 = { -0.5 * math.sqrt(3), 0.5 }
 v1 = { 0.0 , -1.0 }
 v2 = { 0.5 * math.sqrt( 3.0 ), 0.5 }
 -- Scale omega with the radius of the robot
 vOmegaScaled = rb * omega
 --Convert from m/s to RPM
 k = 60.0 * gear / ( 2.0 * math.pi * rw )
 --Compute the desired velocity
 m1 = (v0[1] * vx + v0[2] * vy + vOmegaScaled ) * k
 m2 = (v1[1] * vx + v1[2] * vy + vOmegaScaled ) * k
 m3 = (v2[1] * vx + v2[2] * vy + vOmegaScaled ) * k 

 -- Transformation to obtain velocities related to the quadrotor heading
 --------------------------------------------------------------------------
 -- Orientation of the quadcopter related to the World
 euler_robotino = simGetObjectOrientation(robotino_handle,-1)
 -- We create a Transformation Matrix from this orientation
 pos_robotino=simGetObjectPosition(robotino_handle,-1)
 m_heading = simBuildMatrix(pos_robotino,euler_robotino)

 -- We create a Transformation Matrix for converting the global referenced velocities
 -- to a reference frame based on the heading of the quadrotor
 global_pos = {0,0,0}
 m_global = simBuildMatrix(global_pos,euler_robotino)
 m = simGetInvertedMatrix(m_global)

 vel_global, vel_angz = simGetObjectVelocity(robotino_handle)

 vel_local=simMultiplyVector(m,vel_global)

 -- Velx Control
 error_x = ref_velx - vel_local[1]
 velxControl = error_x*0.1 +acc_x*0.01 + (vel_local[1]-old_velx)*0.001
 old_velx = vel_local[1]
 acc_x = error_x+acc_x

 -- Vely Control
 error_y = ref_vely - vel_local[2]
 velyControl = error_y*0.1 +acc_y*0.01 + (vel_local[2]-old_vely)*0.001
 old_vely = vel_local[2]
 acc_y = error_y+acc_y

 -- Omega Velocity Control
 error_omega = ref_angz - vel_angz[3]
 rotControl = error_omega*0.05 +acc_omega*0.01 -- (vel_local[3]-old_vel_angz)*1.1
 old_vel_angz = vel_angz[3]
 acc_omega = error_omega+acc_omega

 -- Robotino Model (Closed Loop control)
 -- Sets the distance from robot center to wheel center.
 rb = 0.13
 -- Sets the radius of the wheels.
 rw = 0.040
 -- Sets the gear.
 gear = 1.0 --revisar este numero
 -- Projection matrix
 v0 = { -0.5 * math.sqrt(3), 0.5 }
 v1 = { 0.0 , -1.0 }
 v2 = { 0.5 * math.sqrt( 3.0 ), 0.5 }
 -- Scale omega with the radius of the robot
 vOmegaScaled = rb * rotControl
 --Convert from m/s to RPM
 k = 60.0 * gear / ( 2.0 * math.pi * rw )
 --Compute the desired velocity
 m1 = (v0[1] * velxControl + v0[2] * velyControl + vOmegaScaled ) * k
 m2 = (v1[1] * velxControl + v1[2] * velyControl + vOmegaScaled ) * k
 m3 = (v2[1] * velxControl + v2[2] * velyControl + vOmegaScaled ) * k
 --m1 = 1000
 --m2 = 000
 --m3 = -1000
 --vel_global, vel_ang = simGetObjectVelocity(robotino_handle)
 --print(vel_global[1],vel_global[2],vel_global[3],vel_ang[3])
 print('Global',vel_global[1],vel_global[2],vel_global[3],vel_angz[3])
 print('Ref',ref_velx,ref_vely,ref_angz)
 print('Local',vel_local[1],vel_local[2],vel_angz[3])
 print('Motors',m1,m2,m3)

 -- For example:
 --
 -- local position=simGetObjectPosition(handle,-1)
 -- position[1]=position[1]+0.001
 -- simSetObjectPosition(handle,-1,position)
 simSetJointTargetVelocity(wheelJoints[1],m1)
 simSetJointTargetVelocity(wheelJoints[2],m2)
 simSetJointTargetVelocity(wheelJoints[3],m3)

end

if (sim_call_type==sim_childscriptcall_sensing) then

 -- Put your main SENSING code here

end

if (sim_call_type==sim_childscriptcall_cleanup) then

 -- Put some restoration code here

end

Here you can download the model. Any feedback will be greatly appreciated.

Download Robotino model

The following is a guide on how to start using QtCreator in order to build projects in ROS Indigo (Ubuntu 14.04) with the new Catkin Tools.

1) Install Qt Creator:

sudo apt-get install qtcreator

2) Install the new Catkin Tools

sudo apt-get install python-catkin-tools

3)  Create a catkin workspace with catkin init. It will be assumed that the workspace is configured in ~/catkin_ws

3.1) Before adding your project to qtcreator it is recommended to first clean your workspace a build everything using Catkin tools

catkin clean
catkin build

4) Start QtCreator from a new terminal with the ROS environment already configured in its .bashrc.

qtcreator

5) Select the menu File -> “Open File or Project…”, then select ~/catkin_ws/src/YOUR_PACKAGE_NAME/CMakeLists.txt

6) Set the build dir to your catkin workspace location:
~/catkin_ws/build/YOUR_PACKAGE_NAME

7) Press Configure

8) Now in the Menu on the left of QtCreator select Projects -> Build Settings and add the following CMake arguments:

-DCMAKE_INSTALL_PREFIX=~/catkin_ws/install -DCATKIN_DEVEL_PREFIX=~/catkin_ws/devel

9) Save all an close QtCreator

10) In a new terminal start Qtcreator again
qtcreator

10) Now your project should be available and ready to navigate and build.

I also add some lines to my CMakeLists.txt file for Qtcreator to detected the proper location of my header files:

##################
# FOR QT CREATOR
##################
FILE(GLOB_RECURSE QT_MOC RELATIVE ${CMAKE_CURRENT_SOURCE_DIR} FOLLOW_SYMLINKS include/YOUR_PACKAGE_NAME/*.hpp)
FILE(GLOB_RECURSE INC_ALL "include/YOUR_PACKAGE_NAME/*.hpp")
FILE(GLOB_RECURSE ${CMAKE_CURRENT_SOURCE_DIR} FOLLOW_SYMLINKS src/*.cpp)