From 473c88f4bd9ce267b77515297dc670facc92c6b2 Mon Sep 17 00:00:00 2001
From: Robin Shaun <robin_shaun@foxmail.com>
Date: Fri, 17 Jul 2020 11:11:51 +0800
Subject: [PATCH] =?UTF-8?q?=E4=BF=AE=E6=94=B9yolo=E8=BF=BD=E8=B8=AA?=
 =?UTF-8?q?=E8=84=9A=E6=9C=AC?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 control/yolo_human_tracking.py | 13 +++++++------
 sensing/gimbal/gimbal_demo.py  | 22 ++++++++++++++--------
 2 files changed, 21 insertions(+), 14 deletions(-)

diff --git a/control/yolo_human_tracking.py b/control/yolo_human_tracking.py
index b733245..be36857 100644
--- a/control/yolo_human_tracking.py
+++ b/control/yolo_human_tracking.py
@@ -9,6 +9,7 @@ import math
 
 def darknet_callback(data):
     global twist, cmd
+    find = False
     for target in data.bounding_boxes:
         if(target.id==0):
             print('find human')
@@ -17,17 +18,17 @@ def darknet_callback(data):
             twist.linear.x = Kp_linear*x_error
             twist.angular.z = Kp_angular*math.atan(y_error/x_error)
             cmd = ''
-
-        else:
-            twist.linear.x = 0.0
-            twist.angular.z = 0.0
-            cmd = 'HOVER'
+            find = True
+    if not find:
+        twist.linear.x = 0.0
+        twist.angular.z = 0.0
+        cmd = 'HOVER'
                 
             
 if __name__ == "__main__":  
     twist = Twist()
     cmd = String()
-    Kp_linear=0.05
+    Kp_linear=0.006
     Kp_angular=0.2/math.pi
     x_center=752/2
     y_center=480/2
diff --git a/sensing/gimbal/gimbal_demo.py b/sensing/gimbal/gimbal_demo.py
index a083f5a..9e0b7b7 100644
--- a/sensing/gimbal/gimbal_demo.py
+++ b/sensing/gimbal/gimbal_demo.py
@@ -1,27 +1,33 @@
 import rospy
 from mavros_msgs.msg import MountControl
-from mavros_msgs.srv import SetMode
+from mavros_msgs.srv import MountConfigure
+
+import std_msgs.msg
 # Single vehicle Mount Command
 rospy.init_node('gimbal_control')
 mountCnt = rospy.Publisher('/typhoon_h480_0/mavros/mount_control/command', MountControl, queue_size=10)
-FlightMode = rospy.ServiceProxy("/typhoon_h480_0/mavros/set_mode", SetMode)
+mountConfig = rospy.ServiceProxy("/typhoon_h480_0/mavros/mount_control/configure", MountConfigure)
 i=0
 rate=rospy.Rate(100)
-gimbal_pitch_ = -30
+gimbal_pitch_ = -90
 gimbal_yaw_ = 0.0
 gimbal_roll_ = 0.0
-FlightMode(custom_mode='offboard')
+srvheader=std_msgs.msg.Header()
+srvheader.stamp=rospy.Time.now()
+srvheader.frame_id="map"
+
+mountConfig(header=srvheader,mode=2,stabilize_roll=0,stabilize_pitch=0)
 while not rospy.is_shutdown():
     msg = MountControl()
     msg.header.stamp = rospy.Time.now()
     msg.header.frame_id = "map"
     msg.mode = 2
-    #if i%20 == 0:
-    	#gimbal_yaw_ += 10
-    	#print("yaw increased to", gimbal_yaw_)
+    if i%20 == 0:
+    	gimbal_yaw_ += 0
+    	print("yaw increased to", gimbal_yaw_)
     msg.pitch = gimbal_pitch_
     msg.roll = gimbal_roll_
     msg.yaw = gimbal_yaw_
     mountCnt.publish(msg)
     i+=1
-    rate.sleep()
\ No newline at end of file
+    rate.sleep()