From 059000581a0c2beec5b1f3492c86db84bbf4e576 Mon Sep 17 00:00:00 2001 From: Robin Shaun Date: Sun, 19 Jan 2020 19:40:51 +0800 Subject: [PATCH 1/2] darknet_ros --- software/darknet_ros | 1 + 1 file changed, 1 insertion(+) create mode 160000 software/darknet_ros diff --git a/software/darknet_ros b/software/darknet_ros new file mode 160000 index 00000000..c9188638 --- /dev/null +++ b/software/darknet_ros @@ -0,0 +1 @@ +Subproject commit c9188638eb67d13dec842419fb49ab0920a25837 From 0907db3ce29ab5bc4b41e51e7f2eaf4b554dac0f Mon Sep 17 00:00:00 2001 From: Robin Shaun Date: Mon, 20 Jan 2020 21:28:01 +0800 Subject: [PATCH 2/2] add yolo_tracking in tuturiol_6 --- .../6_object_tracking/yolo_tracking.py | 21 +++++++++++++++++ .../6_object_tracking/yolo_tracking_test.py | 23 +++++++++++++++++++ 2 files changed, 44 insertions(+) create mode 100644 demo/tutorial_6/6_object_tracking/yolo_tracking.py create mode 100644 demo/tutorial_6/6_object_tracking/yolo_tracking_test.py diff --git a/demo/tutorial_6/6_object_tracking/yolo_tracking.py b/demo/tutorial_6/6_object_tracking/yolo_tracking.py new file mode 100644 index 00000000..87154ca9 --- /dev/null +++ b/demo/tutorial_6/6_object_tracking/yolo_tracking.py @@ -0,0 +1,21 @@ +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 + 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() diff --git a/demo/tutorial_6/6_object_tracking/yolo_tracking_test.py b/demo/tutorial_6/6_object_tracking/yolo_tracking_test.py new file mode 100644 index 00000000..b508c8d9 --- /dev/null +++ b/demo/tutorial_6/6_object_tracking/yolo_tracking_test.py @@ -0,0 +1,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()