修改yolo追踪脚本

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

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()
+    rate.sleep()