#! /usr/bin/env python

import rospy

import numpy as np

import cv2 as cv

import sys

from time import time

from geometry_msgs.msg import Twist

from cv_bridge import CvBridge, CvBridgeError

from sensor_msgs.msg import Image

import kcftracker

selectingObject = False

initTracking = False

onTracking = False

ix, iy, cx, cy = -1, -1, -1, -1

w, h = 0, 0

inteval = 1

duration = 0.01

class image_converter:

def __init__(self):

self.cmd_pub = rospy.Publisher("/bebop/cmd_vel", Twist,queue_size=1)

self.bridge = CvBridge()

self.image_sub = rospy.Subscriber("/bebop/image_raw", Image, self.callback)

def callback(self, data):

try:

frame = self.bridge.imgmsg_to_cv2(data, "bgr8")

except CvBridgeError as e:

print e

kernel = cv.getStructuringElement(cv.MORPH_RECT, (3, 3))

height, width = frame.shape[0:2]

screen_center = width / 2

screen_center_h = height / 2

offset = 50

offset_h = 30

lower_b = (75, 43, 46)

upper_b = (110, 255, 255)

hsv_frame = cv.cvtColor(frame, cv.COLOR_BGR2HSV)

mask = cv.inRange(hsv_frame, lower_b, upper_b)

mask2 = cv.morphologyEx(mask, cv.MORPH_OPEN, kernel)

mask3 = cv.morphologyEx(mask2, cv.MORPH_CLOSE, kernel)

cv.imshow("mask", mask3)

cv.imshow("Image", frame)

twist = Twist()

if center_x < screen_center - offset:

twist.linear.x = 0.0

twist.linear.y = 0.2

twist.angular.z = 0.2

print "turn left"

elif screen_center - offset <= center_x <= screen_center + offset:

twist.linear.x = 0.0

twist.linear.y = 0.0

twist.angular.z = 0

print "keep"

elif center_x > screen_center + offset:

twist.linear.x = 0.0

twist.linear.y = -0.2

twist.angular.z = -0.2

print "turn right"

else:

twist.linear.x = 0

twist.angular.z = 0

print "stop"

if center_y < screen_center_h - offset_h:

twist.linear.z = 0.2

print "up up up"

elif screen_center_h - offset_h <= center_y <= screen_center_h + offset_h:

twist.linear.z = 0

print "keep"

elif center_y > screen_center_h + offset_h:

twist.linear.z = -0.2

print "down down down"

else:

twist.linear.z = 0

print "stop"

cv.waitKey(3)

try:

self.cmd_pub.publish(twist)

except CvBridgeError as e:

print e

def draw_boundingbox(event, x, y, flags, param):

global selectingObject, initTracking, onTracking, ix, iy, cx,cy, w, h

if event == cv.EVENT_LBUTTONDOWN:

selectingObject = True

onTracking = False

ix, iy = x, y

cx, cy = x, y

elif event == cv.EVENT_MOUSEMOVE:

cx, cy = x, y

elif event == cv.EVENT_LBUTTONUP:

selectingObject = False

if(abs(x-ix)>10 and abs(y-iy)>10):

w, h = abs(x - ix), abs(y - iy)

ix, iy = min(x, ix), min(y, iy)

initTracking = True

else:

onTracking = False

elif event == cv.EVENT_RBUTTONDOWN:

onTracking = False

if(w>0):

ix, iy = x-w/2, y-h/2

initTracking = True

tracker = kcftracker.KCFTracker(True, True, True) # hog, fixed_window, multiscale

#if you use hog feature, there will be a short pause after you draw a first boundingbox, that is due to the use of Numba.

cv.namedWindow('tracking')

cv.setMouseCallback('tracking',draw_boundingbox)

while(1):

if(selectingObject):

cv.rectangle(frame,(ix,iy), (cx,cy), (0,255,255), 1)

elif(initTracking):

cv.rectangle(frame,(ix,iy), (ix+w,iy+h), (0,255,255), 2)

tracker.init([ix,iy,w,h], frame)

initTracking = False

onTracking = True

elif(onTracking):

t0 = time()

boundingbox = tracker.update(frame)

t1 = time()

boundingbox = map(int, boundingbox)

cv.rectangle(frame,(boundingbox[0],boundingbox[1]), (boundingbox[0]+boundingbox[2],boundingbox[1]+boundingbox[3]), (0,255,255), 1)

center_x = int(boundingbox[0] + boundingbox[2] / 2)

center_y = int(boundingbox[1] + boundingbox[3] / 2)

cv.circle(frame, (center_x, center_y), 5, (0, 0, 255), -1)

duration = 0.8*duration + 0.2*(t1-t0)

#duration = t1-t0

cv.putText(frame, 'FPS: '+str(1/duration)[:4].strip('.'), (8,20), cv.FONT_HERSHEY_SIMPLEX, 0.6, (0,0,255), 2)

cv.imshow('tracking', frame)

c = cv.waitKey(inteval) & 0xFF

if c==27 or c==ord('q'):

break

if __name__ == '__main__':

try:

rospy.init_node("cv_bridge_test")

rospy.loginfo("Starting cv_bridge_test node")

image_converter()

except KeyboardInterrupt:

print "Shutting down cv_bridge_test node."

cv.destroyAllWindows()

mysql多商户数据库结构问题

白首方悔读书迟

4个回复