[*] Several changes. Code used to generate POC videos.

config/rviz_config.rviz

@@ -1,17 +1,16 @@
 Panels:
   - Class: rviz/Displays
-    Help Height: 78
+    Help Height: 0
     Name: Displays
     Property Tree Widget:
       Expanded:
-        - /Global Options1
-        - /Status1
         - /TF1
-        - /TF1/Frames1
-        - /Image1
-        - /Pose1
+        - /TF1/Tree1
+        - /TF1/Tree1/odom1
+        - /TF1/Tree1/odom1/ardrone_base_link1
+        - /TF1/Tree1/odom1/ardrone_base_link1/ardrone_base_bottomcam_new1
       Splitter Ratio: 0.719444454
-    Tree Height: 361
+    Tree Height: 457
   - Class: rviz/Selection
     Name: Selection
   - Class: rviz/Tool Properties
@@ -30,7 +29,7 @@ Panels:
     Experimental: false
     Name: Time
     SyncMode: 0
-    SyncSource: Image
+    SyncSource: Camera
 Visualization Manager:
   Class: ""
   Displays:
@@ -52,36 +51,17 @@ Visualization Manager:
       Plane Cell Count: 10
       Reference Frame: <Fixed Frame>
       Value: true
-    - Alpha: 1
-      Buffer Length: 1
-      Class: rviz/Path
-      Color: 255; 0; 0
-      Enabled: true
-      Head Diameter: 0.300000012
-      Head Length: 0.200000003
-      Length: 0.300000012
-      Line Style: Lines
-      Line Width: 0.0299999993
-      Name: Path
-      Offset:
-        X: 0
-        Y: 0
-        Z: 0
-      Pose Color: 255; 85; 255
-      Pose Style: None
-      Radius: 0.0299999993
-      Shaft Diameter: 0.100000001
-      Shaft Length: 0.100000001
-      Topic: /rviz/navigation/traj
-      Unreliable: false
-      Value: true
     - Class: rviz/TF
       Enabled: true
       Frame Timeout: 15
       Frames:
         All Enabled: false
+        ar_marker_3:
+          Value: true
         ardrone_base_bottomcam:
           Value: false
+        ardrone_base_bottomcam_new:
+          Value: false
         ardrone_base_frontcam:
           Value: false
         ardrone_base_link:
@@ -98,39 +78,48 @@ Visualization Manager:
           ardrone_base_link:
             ardrone_base_bottomcam:
               {}
+            ardrone_base_bottomcam_new:
+              ar_marker_3:
+                {}
             ardrone_base_frontcam:
               {}
       Update Interval: 0
       Value: true
-    - Class: rviz/Image
-      Enabled: true
-      Image Topic: /ardrone/bottom/image_raw
-      Max Value: 1
-      Median window: 5
-      Min Value: 0
-      Name: Image
-      Normalize Range: true
-      Queue Size: 2
-      Transport Hint: raw
-      Unreliable: false
-      Value: true
     - Class: rviz/Marker
       Enabled: true
       Marker Topic: /visualization_marker
       Name: Marker
       Namespaces:
-        {}
+        basic_shapes: true
+        main_shapes: true
       Queue Size: 100
       Value: true
+    - Class: rviz/Camera
+      Enabled: true
+      Image Rendering: background and overlay
+      Image Topic: /ardrone/bottom_new/image_raw
+      Name: Camera
+      Overlay Alpha: 0.5
+      Queue Size: 2
+      Transport Hint: raw
+      Unreliable: false
+      Value: true
+      Visibility:
+        Grid: true
+        Marker: true
+        Pose Goal: true
+        TF: true
+        Value: true
+      Zoom Factor: 1
     - Alpha: 1
       Axes Length: 0.200000003
-      Axes Radius: 0.0299999993
+      Axes Radius: 0.0250000004
       Class: rviz/Pose
       Color: 255; 25; 0
       Enabled: true
       Head Length: 0.300000012
       Head Radius: 0.100000001
-      Name: Pose
+      Name: Pose Goal
       Shaft Length: 1
       Shaft Radius: 0.0500000007
       Shape: Axes
@@ -162,35 +151,35 @@ Visualization Manager:
   Views:
     Current:
       Class: rviz/Orbit
-      Distance: 5.80267429
+      Distance: 3.01293278
       Enable Stereo Rendering:
         Stereo Eye Separation: 0.0599999987
         Stereo Focal Distance: 1
         Swap Stereo Eyes: false
         Value: false
       Focal Point:
-        X: 0.286439419
-        Y: -0.00250601768
-        Z: 0.000181674957
+        X: 0.656870425
+        Y: -0.0427799486
+        Z: 0.409981489
       Focal Shape Fixed Size: true
       Focal Shape Size: 0.0500000007
       Invert Z Axis: false
       Name: Current View
       Near Clip Distance: 0.00999999978
-      Pitch: 0.669797122
+      Pitch: 0.424797416
       Target Frame: <Fixed Frame>
       Value: Orbit (rviz)
-      Yaw: 0.648460448
+      Yaw: 2.34346151
     Saved: ~
 Window Geometry:
+  Camera:
+    collapsed: false
   Displays:
     collapsed: false
   Height: 1026
   Hide Left Dock: false
   Hide Right Dock: true
-  Image:
-    collapsed: false
-  QMainWindow State: 000000ff00000000fd0000000400000000000001b200000378fc020000000dfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006100fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000028000001f7000000d600fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d00610067006501000002250000017b0000001600fffffffb0000000a0049006d00610067006501000002aa000000f60000000000000000fb0000000a0049006d00610067006501000002d7000000c90000000000000000fb0000000c00430061006d00650072006100000002c1000000160000000000000000fb0000000a0049006d00610067006501000002d2000000ce0000000000000000000000010000010f00000378fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000002800000378000000ac00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000073f0000003efc0100000002fb0000000800540069006d006501000000000000073f0000030000fffffffb0000000800540069006d00650100000000000004500000000000000000000005870000037800000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  QMainWindow State: 000000ff00000000fd00000004000000000000025200000378fc020000000ffb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006100fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000002800000209000000d600fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d00610067006500000001af000000bb0000000000000000fb0000000a0049006d00610067006501000002aa000000f60000000000000000fb0000000a0049006d00610067006501000002d7000000c90000000000000000fb0000000c00430061006d0065007200610100000237000001690000001600fffffffb0000000a0049006d00610067006501000002d2000000ce0000000000000000fb0000000c00430061006d00650072006101000002640000013c0000000000000000fb0000000a0049006d006100670065000000027d000001230000000000000000000000010000010f00000378fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000002800000378000000ac00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000073f0000003efc0100000002fb0000000800540069006d006501000000000000073f0000030000fffffffb0000000800540069006d00650100000000000004500000000000000000000004e70000037800000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
   Selection:
     collapsed: false
   Time:

launch/ardrone_driver.launch

@@ -17,6 +17,6 @@
 
     <!-- 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="bottomcam_converter_node" pkg="ardrone_carrier" type="bottomcam_converter_node" output="screen" respawn="true" />
+    <node name="bottomcam_converter_node" pkg="ardrone_carrier" type="bottomcam_converter_node.py" output="screen" respawn="true" />
 
 </launch>

src/flight_manager_node.py

@@ -33,34 +33,35 @@ class STATE(IntEnum):
 FRAME_WORLD = "odom"  # base frame
 
 # time parameters
-LOOP_RATE = 10.  # [Hz] rate of the ROS node loop
+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.  # [m] general altitude of flight for the drone
+FLIGHT_ALTITUDE = 1.20  # [m] general altitude of flight for the drone
 FLIGHT_PRECISION = 0.20  # [m] tolerance to reach specified target
 
-LANDING_FACTOR = 0.8  # at each iteration, the drone multiply its distance to target by this factor
-LANDING_MIN_ALTITUDE = 0.20  # [m] below this altitude, the drone stops flying and tries to land
+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
 
 
 class FlightManager:
     def __init__(self):
         """ Object constructor. """
         rospy.loginfo("Waiting for 'ardrone_autonomy' node...")
+        rospy.on_shutdown(self.on_shutdown)
 
         # ROS 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)
         self.takeoff_pub = rospy.Publisher("/ardrone/takeoff", Empty, queue_size=1)
         self.land_pub = rospy.Publisher("/ardrone/land", Empty, queue_size=1)
-        self.nav_pub = rospy.Publisher("/pose_goal", NavigationGoal, queue_size=1)
+        self.nav_pub = rospy.Publisher("/pose_goal", NavigationGoal, queue_size=5)
         self.command_sub = rospy.Subscriber("/command", ArdroneCommand, self._command_callback, queue_size=1)
-        self.bundle_sub = rospy.Subscriber("/ar_pose_marker", AlvarMarkers, self._bundle_callback, queue_size=5)
+        self.bundle_sub = rospy.Subscriber("/ar_pose_marker", AlvarMarkers, self._bundle_callback, queue_size=10)
         self.navdata_sub = rospy.Subscriber("/ardrone/navdata", Navdata, self._navdata_callback, queue_size=5)
 
         # TF management
@@ -85,13 +86,11 @@ def __init__(self):
         self.bundle_pose_received = False
 
         # ensure bottom camera is selected
+        rospy.sleep(rospy.Duration(0.5))
         self.cam_srv(1)
 
         # send null pose to navigation
-        nav_target = NavigationGoal()
-        nav_target.header.frame_id = FRAME_DRONE
-        nav_target.mode = NavigationGoal.RELATIVE
-        self.nav_pub.publish(nav_target)
+        self.send_nav_null_order()
 
         rospy.loginfo("Flight manager successfully initialized and ready.")
 
@@ -119,9 +118,6 @@ def run(self):
 
             rate.sleep()
 
-        # land on exit
-        self.land_pub.publish()
-
     # =======================  States loops =======================
 
     def off_loop(self):
@@ -148,6 +144,13 @@ def reaching_loop(self):
         """
         Fly to approximate target where bundle is supposed to be located.
         """
+        # if bundle detection has been received, target has been found : abort reaching loop
+        if self.bundle_pose_received and self.command in {ArdroneCommand.TRACK, ArdroneCommand.LAND}:
+            self.bundle_pose_received = False
+            self.send_nav_null_order()
+            self._change_state(STATE.TRACKING)
+            return
+
         # if new target has been received, send its pose to navigation node
         if self.target_point_received:
             self.target_point_received = False
@@ -178,11 +181,7 @@ def finding_loop(self):
         # if new bundle detection has been received, target has been found !
         if self.bundle_pose_received:
             self.bundle_pose_received = False
-            # send null pose to navigation
-            nav_target = NavigationGoal()
-            nav_target.header.frame_id = FRAME_DRONE
-            nav_target.mode = NavigationGoal.RELATIVE
-            self.nav_pub.publish(nav_target)
+            self.send_nav_null_order()
             # if command was only to find target, reset order and standby
             if self.command == ArdroneCommand.FIND:
                 self.command = ArdroneCommand.STANDBY
@@ -277,8 +276,10 @@ def landing_loop(self):
             nav_target.header = above_bundle_pose.header
             nav_target.pose.position = above_bundle_pose.pose.position
             nav_target.mode = NavigationGoal.ABSOLUTE
-            self.target_point.point = nav_target.pose.position
             self.nav_pub.publish(nav_target)
+            self.target_point.point.x = nav_target.pose.position.x
+            self.target_point.point.y = nav_target.pose.position.y
+            self.target_point.point.z = nav_target.pose.position.z + FLIGHT_ALTITUDE - LANDING_FACTOR * distance
 
         # if drone is landing or has landed, go to OFF state
         if self.drone_state in {0, 1, 2, 8}:
@@ -311,11 +312,7 @@ def _change_state(self, new_state, warn=False, previous=None):
 
         # GREEN when drone is ready and landed
         if new_state == STATE.OFF:
-            # send null pose to navigation
-            nav_target = NavigationGoal()
-            nav_target.header.frame_id = FRAME_DRONE
-            nav_target.mode = NavigationGoal.RELATIVE
-            self.nav_pub.publish(nav_target)
+            self.send_nav_null_order()
             self.led_srv(type=8, freq=1.0, duration=5)
 
         # BLINK_GREEN_RED when we are looking for target
@@ -330,6 +327,18 @@ def _change_state(self, new_state, warn=False, previous=None):
         # change state
         self.state = new_state
 
+    def send_nav_null_order(self):
+        """ 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):
+        # land on exit
+        self.send_nav_null_order()
+        self.land_pub.publish()
+
     # =======================  ROS callbacks =======================
 
     def _bundle_callback(self, msg):
@@ -441,4 +450,6 @@ def _command_callback(self, msg):
         flight_manager = FlightManager()
         flight_manager.run()
     except rospy.ROSInterruptException:
-        print("Shutting down flight manager node...")
+        pass
+
+    print("Shutting down flight manager node...")

src/navigation_node.py

@@ -67,8 +67,8 @@ def __init__(self):
                               'rot_z': 0.7}
 
         # init PID gains
-        self.pid_gains = {'trans_x': {'P': 0.15, 'I': 0.01, 'D': 0.05},
-                          'trans_y': {'P': 0.15, 'I': 0.01, 'D': 0.05},
+        self.pid_gains = {'trans_x': {'P': 0.2, 'I': 0.01, 'D': 0.03},
+                          'trans_y': {'P': 0.2, 'I': 0.01, 'D': 0.03},
                           'trans_z': {'P': 3.,  'I': 0.01, 'D': 0.05},
                           'rot_z':   {'P': 0.,  'I': 0., 'D': 0.}}
 
@@ -137,10 +137,10 @@ def update(self):
         # otherwise, update order with computed PID commands
         else:
             # TODO : check units (m/s or mm/s, rad/s or deg/s)
-            cmd_vel.linear.x = command['trans_x'] # if abs(command['trans_x']) > 0.1 else 0.
-            cmd_vel.linear.y = command['trans_y'] # if abs(command['trans_y']) > 0.1 else 0.
-            cmd_vel.linear.z = command['trans_z'] # if abs(command['trans_z']) > 0.1 else 0.
-            cmd_vel.angular.z = command['rot_z'] # if abs(command['rot_z']) > 0.1 else 0.
+            cmd_vel.linear.x = command['trans_x']
+            cmd_vel.linear.y = command['trans_y']
+            cmd_vel.linear.z = command['trans_z']
+            cmd_vel.angular.z = command['rot_z']
 
         # publish the command msg to drone
         self.pub_command.publish(cmd_vel)