6003xuan
/
XTDrone
forked from xtdrone/XTDrone
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

视觉伺服跟踪

This commit is contained in:
Robin Shaun 2020-07-21 23:23:27 +08:00
parent 288b4b3e4b
commit 4330e124a9
4 changed files with 17 additions and 21 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
control/yolo_human_tracking.py
View File

@ -12,12 +12,10 @@ def darknet_callback(data):
find = False
for target in data.bounding_boxes:
if(target.id==0):
#if not target_height_mask:
#target_height = height
#target_height_mask = True
print('find human')
x_error=(y_center-(target.ymax+target.ymin)/2)*height/fy
y_error=(x_center-(target.xmax+target.xmin)/2)*height/fx
z = height / math.cos(math.radians(45))
y_error=(x_center-(target.xmax+target.xmin)/2)*z/(fx+x_center)
x_error=(y_center-(target.ymax+target.ymin)/2-y_center*y_error/z)*z/fy
print(x_error,y_error,height)
twist.linear.x = Kp_xy*x_error
twist.linear.y = Kp_xy*y_error
@ -38,14 +36,14 @@ def local_pose_callback(data):
if __name__ == "__main__":
height = 0
target_height = 7
target_height = 5
twist = Twist()
cmd = String()
x_center=752/2
y_center=480/2
fx = 240
fy = 240
Kp_xy = 1
Kp_xy = 0.8
Kp_z = 1
#target_height_mask = False
vehicle_type = sys.argv[1]

16
sensing/gimbal/gimbal_demo.py → sensing/gimbal/run.py
View File

@ -1,15 +1,17 @@
import rospy
from mavros_msgs.msg import MountControl
from mavros_msgs.srv import MountConfigure
import sys
import std_msgs.msg
# Single vehicle Mount Command
vehicle_type = sys.argv[1]
vehicle_id = sys.argv[2]
rospy.init_node('gimbal_control')
mountCnt = rospy.Publisher('/typhoon_h480_0/mavros/mount_control/command', MountControl, queue_size=10)
mountConfig = rospy.ServiceProxy("/typhoon_h480_0/mavros/mount_control/configure", MountConfigure)
i=0
mountCnt = rospy.Publisher(vehicle_type+'_'+vehicle_id+'/mavros/mount_control/command', MountControl, queue_size=10)
mountConfig = rospy.ServiceProxy(vehicle_type+'_'+vehicle_id+'/mavros/mount_control/configure', MountConfigure)
rate=rospy.Rate(100)
gimbal_pitch_ = -90
gimbal_pitch_ = -45
gimbal_yaw_ = 0.0
gimbal_roll_ = 0.0
srvheader=std_msgs.msg.Header()
@ -22,12 +24,8 @@ while not rospy.is_shutdown():
msg.header.stamp = rospy.Time.now()
msg.header.frame_id = "map"
msg.mode = 2
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()

2
sensing/object_detection_and_tracking/YOLO/darknet_ros/darknet_ros/config/px4_tracking.yaml
View File

@ -1,7 +1,7 @@
subscribers:
camera_reading:
topic: /iris_0/stereo_camera/left/image_raw
topic: /cgo3_camera/image_raw
queue_size: 1
actions:

8
sitl_config/launch/outdoor1.launch
View File

@ -11,9 +11,9 @@
<arg name="Y" default="0"/>
<!-- vehicle model and world -->
<arg name="est" default="ekf2"/>
<arg name="vehicle" default="iris"/>
<arg name="vehicle" default="typhoon_h480"/>
<arg name="world" default="$(find mavlink_sitl_gazebo)/worlds/outdoor1.world"/>
<arg name="sdf" default="$(find mavlink_sitl_gazebo)/models/iris_stereo_camera/iris_stereo_camera.sdf"/>
<arg name="sdf" default="$(find mavlink_sitl_gazebo)/models/typhoon_h480/typhoon_h480.sdf"/>
<!-- gazebo configs -->
<arg name="gui" default="true"/>
@ -44,8 +44,8 @@
<arg name="paused" value="$(arg paused)"/>
<arg name="respawn_gazebo" value="$(arg respawn_gazebo)"/>
</include>
<group ns="typhoon_h480_0">
<!-- MAVROS -->
<group ns = "iris_0">
<include file="$(find mavros)/launch/px4.launch">
<!-- GCS link is provided by SITL -->
<arg name="gcs_url" value=""/>
@ -53,4 +53,4 @@
<arg name="respawn_mavros" value="$(arg respawn_mavros)"/>
</include>
</group>
</launch>
</launch>