Resolve conflicts

launch/track.launch

@@ -31,8 +31,22 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
     <param name="realtime_navdata" value="true" />
   </node>
 
-  <node name="ardrone_tracker" pkg="falkor_ardrone" type="ardrone_tracker.py"  output="screen" >
-    <param name="cascadefile" value="$(find falkor_ardrone)/cascade/lbpcascade_falkorlogobw.xml" />
+  <node name="ros_tld_tracker_node" pkg="tld_tracker" type="ros_tld_tracker_node" >
+    <remap from="image_rgb" to="/ardrone/front/image_raw"/>
+    <remap from="bounding_box" to="tld_gui_bb"/>
+    <remap from="cmds" to="tld_gui_cmds"/>
+    <param name="showOutput" value="true" type="bool"/>
+    <param name="loadModel" value="false" type="bool"/>
+    <param name="autoFaceDetection" value="false" type="bool"/>
+    <param name="modelImportFile" value="$(find falkor_ardrone)/cascade/falkor_tld_model" type="string"/>
+    <param name="exportModelAfterRun" value="true" type="bool"/>
+    <param name="modelExportFile" value="$(find falkor_ardrone)/cascade/falkor_tld_model_new" type="string"/>
+  </node>
+
+  <node name="ros_tld_gui_node" pkg="tld_gui" type="ros_tld_gui_node" >
+    <remap from="image_rgb" to="/ardrone/front/image_raw"/>
+    <remap from="tracked_object" to="tld_tracked_object"/>
+    <remap from="fps_tracker" to="tld_fps"/>
   </node>
 
   <node name="ardrone_follow" pkg="falkor_ardrone" type="ardrone_follow.py" output="screen" />

manifest.xml

@@ -17,6 +17,7 @@
   <depend package="camera_info_manager" />
   <depend package="sensor_msgs"/>
   <depend package="ardrone_autonomy"/>
+  <depend package="tld_msgs" />
 </package>
 
 

nodes/ardrone_follow.py

@@ -39,6 +39,7 @@ from std_msgs.msg import Empty
 from sensor_msgs.msg import Joy, Image
 from ardrone_autonomy.msg import Navdata
 from ardrone_autonomy.srv import LedAnim
+from tld_msgs.msg import BoundingBox
 from ardrone_autonomy.srv import RecordEnable
 import std_srvs.srv
 
@@ -52,12 +53,12 @@ class ArdroneFollow:
         self.record_service = rospy.ServiceProxy( "ardrone/setrecord", RecordEnable )
         self.recording = False
 
-        self.tracker_sub = rospy.Subscriber( "ardrone_tracker/found_point",
-                                             Point, self.found_point_cb )
+        self.tracker_sub = rospy.Subscriber( "tld_tracked_object",
+                                             BoundingBox, self.bounding_box_cb )
         self.goal_vel_pub = rospy.Publisher( "goal_vel", Twist )
         self.found_time = None
 
-        self.tracker_img_sub = rospy.Subscriber( "ardrone_tracker/image",
+        self.tracker_img_sub = rospy.Subscriber( "ardrone/front/image_raw",
                                                  Image, self.image_cb )
         self.tracker_image = None
 
@@ -87,6 +88,9 @@ class ArdroneFollow:
         self.lastAnim = -1
 
         self.found_point = Point( 0, 0, -1 )
+        self.found_bounding_box = BoundingBox()
+        self.image_size = (1,1)
+        self.image_area = 1
         self.old_cmd = self.current_cmd = Twist()
 
         self.joy_sub = rospy.Subscriber( "joy", Joy, self.callback_joy )
@@ -125,8 +129,10 @@ class ArdroneFollow:
             cv_image = self.bridge.imgmsg_to_cv( data, "passthrough" )
         except CvBridgeError, e:
             print e
-        
+
         self.tracker_image = np.asarray( cv_image )
+        self.image_size = self.tracker_image.shape
+        self.image_area = self.image_size[0]*self.image_size[1]
 
     def increase_z_setpt( self ):
         self.zPid.setPointMin *= 1.01
@@ -206,9 +212,15 @@ class ArdroneFollow:
     def reset( self ):
         self.reset_pub.publish( Empty() )
 
-    def found_point_cb( self, data ):
-        self.found_point = data
-        self.found_time = rospy.Time.now()
+    def bounding_box_cb( self, data ):
+        self.found_bounding_box = data
+        if data.confidence < 0.1:
+            self.found_point = Point(0,0,-1.0)
+        else:
+            self.found_point = Point( (data.x+data.width/2 )*100.0/self.image_size[1],
+                                      (data.y+data.height/2)*100.0/self.image_size[0],
+                                      math.sqrt(data.width*data.height/self.image_area)*100 )
+            self.found_time = rospy.Time.now()
 
     def hover( self ):
         hoverCmd = Twist()
@@ -255,6 +267,7 @@ class ArdroneFollow:
                        ( cx + side_max_half, cy + side_max_half ),
                        ( 255, 255, 0 ) )
 
+
         if self.current_cmd.linear.x > 0:
             line_color = ( 0, 255, 0 )
         else:
@@ -265,6 +278,16 @@ class ArdroneFollow:
                   line_color,
                   min( max( int( abs( self.current_cmd.linear.x ) * 255 ), 0 ), 255 ) )
 
+        # Draw detected box
+        if self.found_point.z != -1.0:
+            confidence_value = int(255 * self.found_bounding_box.confidence)
+            confidence_color = (confidence_value,confidence_value,confidence_value)
+            cv2.rectangle( vis,
+                           ( self.found_bounding_box.x, self.found_bounding_box.y ),
+                           ( self.found_bounding_box.x + self.found_bounding_box.width,
+                             self.found_bounding_box.y + self.found_bounding_box.height ),
+                           confidence_color, 3, 8, 0 )
+
         if self.navdata != None:
             self.put_text( vis, 'State: %s' % self.states[ self.navdata.state ],
                            ( 150, 240 ) )