-
Notifications
You must be signed in to change notification settings - Fork 462
Expand file tree
/
Copy pathyolo_tracking_test.py
More file actions
23 lines (23 loc) · 910 Bytes
/
yolo_tracking_test.py
File metadata and controls
23 lines (23 loc) · 910 Bytes
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
import rospy
import sys
sys.path.append('/home/robin/catkin_ws/devel/lib/python2.7/dist-packages')
from darknet_ros_msgs.msg import BoundingBoxes
#from commander import Commander
import time
Kp=0.01
x_center=752/2
y_center=480/2
#con=Commander()
def darknet_callback(data):
if(data.bounding_boxes[0].xmax):
x_error=y_center-(data.bounding_boxes[0].ymax+data.bounding_boxes[0].ymin)/2
y_error=x_center-(data.bounding_boxes[0].xmax+data.bounding_boxes[0].xmin)/2
print('x_error:',x_error)
print('y_error:',y_error)
#con.move(Kp*x_error,Kp*y_error,z=0)
rospy.init_node('yolo_tracking')
rospy.Subscriber("/darknet_ros/bounding_boxes", BoundingBoxes, darknet_callback)
# spin() simply keeps python from exiting until this node is stopped
rate = rospy.Rate(60) #the rate will take care of updating the publisher at a specified speed - 50hz in this case
while(True):
rate.sleep()