[*] Update doc

README.md

@@ -1,18 +1,52 @@
 # ardrone_carrier
 
-ROS project fro an ardrone landing on mobile platform.
+ROS project for an ardrone landing on mobile platform, for course ROB314 at ENSTA ParisTech.
 
-## TODO
+Autors : N. Cadart, B. Sarthou, P. Antoine
 
-### navigation_node (and pid)
+Date : September 2018 - March 2019
 
-- new behavior : when receiving a new NavigationGoal msg, if quaternion is null, ignore rotation and simply fly to given position.
-- bug to fix : convert new NavigationGoal into right frame
-- test node on drone
+- [ardrone_carrier](#ardrone_carrier)
+  - [Description du projet](#description-du-projet)
+  - [Usage](#usage)
+  - [Descriptions des fichiers](#descriptions-des-fichiers)
+  - [Ar Track Alvar](#ar-track-alvar)
+    - [Mode d'emploi](#mode-demploi)
+    - [Observation des détections individuelles et *tf*](#observation-des-détections-individuelles-et-tf)
+    - [Observation des détections multiples (*bundles*)](#observation-des-détections-multiples-bundles)
+  - [Tum Ardrone](#tum-ardrone)
+  - [Calibration de la caméra](#calibration-de-la-caméra)
 
-### flight_manager_node
+## Description du projet
 
-- test node on drone with real navigation
+Ce package ROS a pour but d'utiliser un drone Parrot Ardrone de manière autonome afin de décoller, voler, détecter une cible, la suivre, puis atterrir dessus.
+
+L'interface avec le drone est faite au moyen de [ardrone_autonomy](https://ardrone-autonomy.readthedocs.io/en/latest/). La cible est assimilée à un ensemble de QR codes, détectés par [ar_track_alvar](http://wiki.ros.org/ar_track_alvar).
+
+## Usage
+
+Pour lancer l'interface de communication et de commande du drone, ainsi que la correction des TF erronées :
+```
+roslaunch ardrone_carrier ardrone_driver.launch
+```
+
+Pour lancer la détection de la cible par **ar_track_alvar**
+```
+roslaunch ardrone_carrier track_alvar_ardrone_bundles.launch
+```
+
+Pour lancer l'aperçu avec RViz :
+```
+roslaunch ardrone_carrier rviz.launch
+```
+
+## Descriptions des fichiers
+
+- `config/` : répertoire contenant les fichiers de calibrations des caméras (Ardrone 1 et 2), ainsi que la configuration de RViz pour ce projet.
+- `doc/` : répertoire contenant des images pour la documentation ainsi que la définition du bundle utilisé comme cible.
+- `launch/` : ROS launch files pour lancer les différents noeuds utilisés
+- `msg/` : ROS msgs définis dans ce package
+- `src/` : répertoire contenant le code source des noeuds développés pour ce projet.
 
 
 ## Ar Track Alvar
@@ -101,34 +135,32 @@ Launchfile à utiliser : `roslaunch ardrone_carrier track_alvar_ardrone_bundles.
 L'idée est de définir un "objet" rigide composé de plusieurs QR codes. On définit dans un fichier XML la position de ces QR codes pa rapport à la position d'un QR code *master*, ou principal. Seront publiés alors sur `/ar_pose_marker` la position du QR code *master* de ce bundle.
 La détection multiple permet d'être plus robuste aux occlusions, et de fournir une meilleure estimation de la position de l'objet total.
 
-## Tum Ardrone
-
 
-### Setting target using the mouse from video feed
+## Tum Ardrone
 
-Clicking on the video window will generate way points, which are sent to drone_autopilot (if running):
+Package ROS utilisant l'algorithme PTAM pour une meilleure estimation de la position du drone. Implémente un paquet complet de navigation pour l'Ardrone.
+Plus d'infos [ici](http://wiki.ros.org/tum_ardrone).
 
-- left-click: fly (x,y,0)m relative to the current position. The image-center is (0,0), borders are 2m respectively.
-- right-click: fly (0,0,y)m and rotate yaw by x degree. The Image-center is (0,0), borders are 2m and 90 degree respectively.
+Note : nécessite l'utilisation de la caméra frontale du drone.
 
 
 ## Calibration de la caméra
 
-Voir tutoriel [camera_calibration](http://wiki.ros.org/camera_calibration/Tutorials/MonocularCalibration).
+Pour que la détection des markers par **ar_track_alvar** soit efficace, il est nécessaire de calibrer les caméras du drone. Voir tutoriel [camera_calibration](http://wiki.ros.org/camera_calibration/Tutorials/MonocularCalibration).
 
 Paramètres:
-- `size`: taille en mètres des carrés de la mire
-- `cam_topic`: nom du topic où sont publiées les images de la caméra à calibrer
-- `cam_name`: nom de la caméra à calibrer
+- `size` : taille en mètres des carrés de la mire
+- `cam_topic` : nom du topic où sont publiées les images de la caméra à calibrer
+- `cam_name` : nom de la caméra à calibrer
 
 ```
 size=0.1085
-cam_topic=/ardrone/front/image_raw
-cam_info_topic=/ardrone/front/camera_info
+cam_topic=/ardrone/bottom/image_raw
+cam_name=/ardrone/bottom
 rosrun camera_calibration cameracalibrator.py --size 8x6 --square $size image:=$cam_topic camera:=$cam_name
 ```
 
-1. Lancer les commandes ci-dessus
-2. Bouger la mire dans le champ de vision de la caméra jusqu'à obtenir les jauges vertes
+1. Lancer les commandes ci-dessus.
+2. Bouger la mire dans le champ de vision de la caméra jusqu'à obtenir les jauges vertes.
 3. Cliquer sur "Calibrate" UNE SEULE FOIS. Le calcul du modèle de transformation peut prendre jusqu'à 1 min.
-4. Cliquer sur "Commit" pour enregistrer la calibration dans `/home/<username>/.ros/camera_info/`, ou sur "Save" pour écrire les résultats dans une archive `/tmp/calibrationdata.tar.gz`
+4. Cliquer sur "Commit" pour enregistrer la calibration dans `/home/<username>/.ros/camera_info/`, ou sur "Save" pour écrire les résultats dans une archive `/tmp/calibrationdata.tar.gz`.

launch/ardrone_driver.launch

@@ -16,7 +16,7 @@
     </node>
 
     <!-- Correct TF of bottom camera -->
-    <node pkg="tf2_ros" type="static_transform_publisher" name="ardrone_base_bottomcam_new_tf_pub" args="0 -0.02 0 -1.571 0 3.142 ardrone_base_link ardrone_base_bottomcam_new"/>
+    <node name="ardrone_base_bottomcam_new_tf_pub" pkg="tf2_ros" type="static_transform_publisher" args="0 -0.02 0 -1.571 0 3.142 ardrone_base_link ardrone_base_bottomcam_new"/>
     <node name="bottomcam_converter" pkg="ardrone_carrier" type="bottomcam_converter_node.py" output="screen" respawn="true" />
 
 </launch>

src/bottomcam_converter_node.py

@@ -1,4 +1,9 @@
 #!/usr/bin/env python
+"""
+@brief Convert bottom camera to corrected bottom_new fake camera.
+@author N. Cadart
+@date February 2019
+"""
 import rospy
 from sensor_msgs.msg import CameraInfo, Image
 

src/flight_manager_node.py

@@ -1,18 +1,22 @@
 #!/usr/bin/env python
+"""
+@brief Flight manager node, the brain of the drone : decides the behavior of the
+       drone to fulfill its user defined mission.
+@author N. Cadart
+@date December 2018
+"""
 from __future__ import division, print_function
 from enum import IntEnum
 import numpy as np
 
 import rospy
 import tf2_ros
 from std_msgs.msg import Empty
-# from geometry_msgs.msg import PoseStamped, PointStamped
 from geometry_msgs.msg import Point
 from tf2_geometry_msgs import PoseStamped, PointStamped
 from ar_track_alvar_msgs.msg import AlvarMarkers
 from ardrone_autonomy.msg import Navdata
 from ardrone_autonomy.srv import LedAnim, CamSelect
-
 from ardrone_carrier.msg import NavigationGoal, ArdroneCommand
 
 
@@ -30,18 +34,17 @@ class STATE(IntEnum):
 BUNDLE_ID = 3  # id of the master marker in the bundle
 FRAME_TARGET = "ar_marker_{}".format(BUNDLE_ID)  # target bundle to follow/land on
 FRAME_DRONE = "ardrone_base_link"  # drone
-FRAME_WORLD = "odom"  # base frame
+FRAME_WORLD = "odom"  # static frame
 
 # time parameters
 LOOP_RATE = 20.  # [Hz] rate of the ROS node loop
 BUNDLE_DETECTION_TIMEOUT = 1.  # [s] if a bundle detection is older than this, we go back to FINDING state
 
-BUNDLE_FINDING_DISTANCE_FACTOR = 0.10  # [m] how much we increase distance from approximate target position at each step
-
 # Flight parameters
 TAKEOFF_ALLOWED = True  # if False, no takeoff order will be sent (Test mode)
 FLIGHT_ALTITUDE = 1.20  # [m] general altitude of flight for the drone
 FLIGHT_PRECISION = 0.20  # [m] tolerance to reach specified target
+BUNDLE_FINDING_DISTANCE_FACTOR = 0.10  # [m] how much we increase distance from approximate target position at each step
 
 LANDING_FACTOR = 0.7  # at each iteration, the drone multiply its distance to target by this factor
 LANDING_MIN_ALTITUDE = 0.50  # [m] below this altitude, the drone stops flying and tries to land
@@ -53,7 +56,7 @@ def __init__(self):
         rospy.loginfo("Waiting for 'ardrone_autonomy' node...")
         rospy.on_shutdown(self.on_shutdown)
 
-        # ROS subscribers and publishers
+        # ROS services, subscribers and publishers
         rospy.wait_for_service("/ardrone/setcamchannel")
         self.cam_srv = rospy.ServiceProxy("/ardrone/setcamchannel", CamSelect)
         self.led_srv = rospy.ServiceProxy("/ardrone/setledanimation", LedAnim)
@@ -328,13 +331,14 @@ def _change_state(self, new_state, warn=False, previous=None):
         self.state = new_state
 
     def send_nav_null_order(self):
-        """ Send null command to navigation node to disable it """
+        """ Send null command to navigation node to disable it. """
         nav_target = NavigationGoal()
         nav_target.header.frame_id = FRAME_DRONE
         nav_target.mode = NavigationGoal.RELATIVE
         self.nav_pub.publish(nav_target)
 
     def on_shutdown(self):
+        """ When ROS node is killed, stop navigation controller and send land order. """
         # land on exit
         self.send_nav_null_order()
         self.land_pub.publish()
@@ -451,5 +455,4 @@ def _command_callback(self, msg):
         flight_manager.run()
     except rospy.ROSInterruptException:
         pass
-
     print("Shutting down flight manager node...")

src/navigation_node.py

@@ -1,5 +1,9 @@
 #!/usr/bin/python
-# coding=utf-8
+"""
+@brief Navigation node to fly the ardrone to a specific pose
+@author N. Cadart & B. Sarthou
+@date October 2018
+"""
 import numpy as np
 
 import rospy
@@ -230,4 +234,5 @@ def pose_goal_callback(self, msg_pose):
         navigation_node = ArdroneNav()
         navigation_node.run()
     except rospy.ROSInterruptException:
-        print("Shutting down drone navigation node...")
+        pass
+    print("Shutting down drone navigation node...")

src/pid.py

@@ -1,8 +1,17 @@
-class PID:
+"""
+@brief PID controller with conditional integration anti-windup.
+@author N. Cadart
+@date October 2018
+"""
 
+class PID:
+    """
+    Proportionnal, Integrative, Derivative Controller.
+    Anti-windup scheme done by conditional integration.
+    """
     def __init__(self, kp, ki, kd, dt):
         """
-        Initialise a PID control loop
+        Initialise PID controller
         """
         # Set proportional, integral, derivative factor, and control period
         self.kp = kp
@@ -24,18 +33,18 @@ def __init__(self, kp, ki, kd, dt):
 
     def activate_command_saturation(self, low_saturation, high_saturation):
         """
-        @brief: Constrain the output PID command between min and max values,
+        @brief Constrain the output PID command between min and max values,
                 and activate conditonal integration anti-windup.
-        @param: lowSat : low saturation threshold (= minimum value of command)
-        @param: highSat : high saturation threshold (= maximum value of command)
+        @param lowSat : low saturation threshold (= minimum value of command)
+        @param highSat : high saturation threshold (= maximum value of command)
         """
         self.saturation_activation = True
         self.low_saturation = low_saturation
         self.high_saturation = high_saturation
 
     def deactivate_command_saturation(self):
         """
-        @brief: Deactivate PID command saturation and conditonal integration anti-windup.
+        @brief Deactivate PID command saturation and conditonal integration anti-windup.
         """
         self.saturation_activation = False
         self.low_saturation = 0.0
@@ -44,11 +53,11 @@ def deactivate_command_saturation(self):
 
     def anti_windup(self, error, saturation):
         """
-        @brief: Run conditional anti-windup to prevent integrator term explosion.
+        @brief Run conditional anti-windup to prevent integrator term explosion.
                 If the PID output command is saturated, the anti-windup will stop error
                 integration to limit overshoot or divergence.
-        @param: error : value of the error (= goalValue - currentValue)
-        @param: saturation : value of the PID command saturation (= command - saturatedCommand)
+        @param error : value of the error (= goalValue - currentValue)
+        @param saturation : value of the PID command saturation (= command - saturatedCommand)
         """
         if saturation == 0 or error * saturation < 0:
             self.integrate_error = True
@@ -57,9 +66,9 @@ def anti_windup(self, error, saturation):
 
     def compute_command(self, error):
         """
-        @brief: Run the PID control
-        @param: error : value of the error (= goalValue - currentValue)
-        @return: computed PID control value
+        @brief Run the PID control
+        @param error : value of the error (= goalValue - currentValue)
+        @return computed PID control value
         """
         # compute PID command
         command = self.kp * error + \
@@ -70,10 +79,8 @@ def compute_command(self, error):
         if self.saturation_activation:
             # compute saturated command
             saturatedCommand = max(min(command, self.high_saturation), self.low_saturation)
-
-            # Use anti-windup
+            # Perform anti-windup
             self.anti_windup(error, command - saturatedCommand)
-
             # Update command with saturated
             command = saturatedCommand