def camera(): rospy.init_node('camera') image_topic = rospy.get_param('~image', '/camera/image') image_pub = rospy.Publisher(image_topic, Image, queue_size=5) bridge = CvBridge() width = rospy.get_param('width', 640) height = rospy.get_param('height', 480) camera = Camera(pixel_clock=50, exposure_time=8) #camera.configure(width=width, height=height) while not rospy.is_shutdown(): try: frame = camera.capture() stamp = rospy.Time.now() if len(frame.shape) == 2: image_msg = bridge.cv2_to_imgmsg(frame, encoding='mono8') else: image_msg = bridge.cv2_to_imgmsg(frame, encoding='bgr8') image_msg.header.stamp = stamp image_pub.publish(image_msg) except CvBridgeError, e: print e
class image_publisher: def __init__(self): self.camera = PiCamera() self.camera.resolution = (1024, 768) #(1920, 1080) self.camera.ISO = 100 self.camera.sa = 100 self.camera.awb = "flash" self.camera.co = 100 self.rawCapture = PiRGBArray(self.camera) time.sleep(0.1) self.image_pub = rospy.Publisher("image_topic",Image) self.small_image_pub = rospy.Publisher("small_image_topic",Image) self.bridge = CvBridge() def publishImage(self): for frame in self.camera.capture_continuous(self.rawCapture, format="bgr", use_video_port=True): try: cv_image = frame.array except CvBridgeError as e: print(e) #cv_image = (cv_image * 0.5.astype(umpy.uint8) self.rawCapture.truncate(0) try: self.image_pub.publish(self.bridge.cv2_to_imgmsg(cv_image, "bgr8")) small = cv2.resize(cv_image, (0,0), fx=0.5, fy=0.5) self.small_image_pub.publish(self.bridge.cv2_to_imgmsg(small, "bgr8")) except CvBridgeError as e: print("problem") print(e)
def test_numpy_types(self): import cv2 import numpy as np bridge_ = CvBridge() self.assertRaises(TypeError, lambda: bridge_.cv2_to_imgmsg(1, "rgba8")) if hasattr(cv2, 'cv'): self.assertRaises(TypeError, lambda: bridge_.cv2_to_imgmsg(cv2.cv(), "rgba8"))
def imagePublisher(): front_pub = rospy.Publisher('center_cam', Image, queue_size=1) left_pub = rospy.Publisher('left_cam', Image, queue_size =1) right_pub = rospy.Publisher('right_cam', Image, queue_size = 1) rospy.init_node('camera_node', anonymous=True) #rate=rospy.Rate(30)#10hz bridge = CvBridge() while not rospy.is_shutdown(): _, front_img = cam_front.read() _, left_img = cam_left.read() _, right_img = cam_right.read() cv2.imshow('image', front_img) # for debugging purposes cv2.imshow('left', left_img) cv2.imshow('right', right_img) front_img = bridge.cv2_to_imgmsg(front_img, "bgr8") left_img = bridge.cv2_to_imgmsg(left_img, "bgr8") #change to grayscale? # right_img = bridge.cv2_to_imgmsg(left_img, "bgr8") rospy.loginfo("images sent") # for debugging purposes, remove if cv2.imshow('imgae', img) is deleted k = cv2.waitKey(1) & 0xFF if k ==27: break front_pub.publish(front_img) left_pub.publish(left_img) # right_pub.publish(right_img) cv2.destroyAllWindows() cam_front.release() cam_left.release()
class ProcessDepth: def __init__(self): # Allows conversion between numpy arrays and ROS sensor_msgs/Image self.bridge = CvBridge() # Allow our topics to be dynamic. self.input_camera_topic = rospy.get_param('~input_camera_topic', '/camera/depth/image_rect') self.output_camera_topic = rospy.get_param('~output_camera_topic', '/processed') # WE are going to publish a debug image as it comes in. self.pub = rospy.Publisher(self.output_camera_topic, Image,queue_size=10) rospy.Subscriber(self.input_camera_topic, Image, self._process_depth_img) # run the node self._run() # Keep the node alive def _run(self): rospy.spin() def _process_depth_img(self,input): # convert our image to CV2 numpy format from ROS format latest_image = self.bridge.imgmsg_to_cv2(input) if( latest_image is not None ): try: # convert the image to SimpleCV # The input image is single channel float and we want rgb uint8 # it is also full of nasty nans. We get the min and max and scale # the image from [0,flt_max] to [0,255] dmin = np.nanmin(latest_image) dmax = np.nanmax(latest_image) latest_image = latest_image - dmin sf = 255.0/(dmax-dmin) latest_image = sf*latest_image # Convert to uint8 temp = latest_image.astype(np.uint8) # move to SimpleCV RGB img = scv.Image(temp, cv2image=True, colorSpace=scv.ColorSpace.RGB) # get values less than 128 lt = img.threshold(128).invert() # get values greater than 64 gt = img.threshold(64) # do the logical and of the two depths range = lt*gt # apply the mask to the input image blobs = img.findBlobsFromMask(mask=range) # draw the results. if( blobs ): blobs.draw(color=scv.Color.RED,width=-1) img = img.applyLayers() # convert SimpleCV to CV2 Numpy Format cv2img = img.getNumpyCv2() # Convert Cv2 numpy to ROS format img_msg = self.bridge.cv2_to_imgmsg(cv2img, "bgr8") # publish the topic. self.pub.publish(self.bridge.cv2_to_imgmsg(cv2img, "bgr8")) except CvBridgeError, e: rospy.logwarn("PROCESSING EXCEPTION {0}".format(e))
class VirtualMirror: def __init__(self, target_img="cone.png"): self.node_name = "Cone Detector" self.thread_lock = threading.Lock() self.sub_image = rospy.Subscriber("~image_raw", CompressedImage, self.cbImage, queue_size=1) self.pub_image_mirror = rospy.Publisher("~virtual_mirror_img", Image, queue_size=1) self.pub_image_regular = rospy.Publisher("~virtual_regular_img", Image, queue_size=1) self.pub_image_average = rospy.Publisher("~virtual_regular_img_avg", Image, queue_size=1) self.bridge = CvBridge() self.image_prev = None # Subscribers rospy.loginfo("[%s] Initialized." %(self.node_name)) def cbImage(self,image_msg): # Start a daemon thread to process the image thread = threading.Thread(target=self.processImage,args=(image_msg,)) thread.setDaemon(True) thread.start() # Returns rightaway def processImage(self, image_msg): if not self.thread_lock.acquire(False): # Return immediately if the thread is locked return #self.sub_image.unregister() np_arr = np.fromstring(image_msg.data, np.uint8) image_cv = cv2.imdecode(np_arr, cv2.IMREAD_COLOR) img = image_cv.copy() img_avg = cv2.flip(image_cv, 1) (H,W,_) = image_cv.shape """ H -= 1 W -= 1 for u in range(H): for v in range(W): img[u][v] = image_cv[u, W-v] if self.image_prev !=None: curr = list(image_cv[u][v]) prev = list(self.image_prev[u][v]) avg = [ (curr[i] + prev[i])/2 for i in range(3)] img_avg[u][v] = avg """ try: self.pub_image_mirror.publish(self.bridge.cv2_to_imgmsg(img, "bgr8")) self.pub_image_regular.publish(self.bridge.cv2_to_imgmsg(image_cv, "bgr8")) if self.image_prev != None: self.pub_image_average.publish(self.bridge.cv2_to_imgmsg(img_avg, "bgr8")) except CvBridgeError as e: print(e) self.image_prev = image_cv self.thread_lock.release()
class PersonInfoNode: def __init__(self): #self._people_pub = rospy.Publisher('track_people', TrackPeople, queue_size=10) self._image_pub = rospy.Publisher('~/people_track_img', Image, queue_size=10) self._new_body = False self._new_image = False self._body_lock = Lock() self._img_lock = Lock() self.bridge = CvBridge() self.bodies = None self.img = None def body_handler(self, body_array): with self._body_lock: self.bodies = body_array.bodies self._new_body = True def image_handler(self, data): with self._img_lock: try: self.img = self.bridge.imgmsg_to_cv2(data, "bgr8") except CvBridgeError as e: rospy.logwarn(e) self._new_image = True def pub_msgs(self): with self._body_lock: with self._img_lock: if not self._new_body or not self._new_image: return self._new_body = False self._new_image = False track_people = TrackPeople() show_img = array(self.img, copy=True) rows, cols, _ = self.img.shape for body in self.bodies: person = TrackPerson() person.trackingId = body.trackingId person.pos = average_position(body) person.header = body.header rospy.loginfo("Bounding box") from_x = max(min(body.fromX, body.toX), 0) to_x = min(max(body.fromX, body.toX), cols) from_y = max(min(body.fromY, body.toY), 0) to_y = min(max(body.fromY, body.toY), rows) print(from_x, from_y) print(to_x, to_y) person.image = self.bridge.cv2_to_imgmsg( self.img[from_y:to_y, from_x:to_x], 'bgr8') track_people.people.append(person) cv2.rectangle(show_img, (body.fromX, body.fromY), (body.toX, body.toY), (255, 0, 0), 3) self._image_pub.publish( self.bridge.cv2_to_imgmsg(show_img, 'bgr8'))
class ImageProcessingExample: def __init__(self): # Allows conversion between numpy arrays and ROS sensor_msgs/Image self.bridge = CvBridge() # Allow our topics to be dynamic. self.input_camera_topic = rospy.get_param('~input_camera_topic', '/camera/image_rect_color') self.output_camera_topic = rospy.get_param('~output_camera_topic', '/camera/color_stuff') # WE are going to publish a debug image as it comes in. self.pub = rospy.Publisher(self.output_camera_topic, Image,queue_size=10) # We also publish the position of the thing we found. self.point_pub = rospy.Publisher("/position",Point,queue_size=10) # we get our input from a topic, it is of type image, call _show_colored_stuff. rospy.Subscriber(self.input_camera_topic, Image, self._show_colored_stuff) # run the node self._run() # Keep the node alive def _run(self): rospy.spin() def _show_colored_stuff(self,input): # convert our image to CV2 numpy format from ROS format latest_image = self.bridge.imgmsg_to_cv2(input) if( latest_image is not None ): try: # convert the image to SimpleCV img = scv.Image(latest_image, cv2image=True, colorSpace=scv.ColorSpace.RGB) # create a writeable copy src = img.copy() # Do our image processing get hue, threshold, morphology, connected components. mask = src.hueDistance(180).invert().threshold(240).erode(2).dilate(3) blobs = src.findBlobsFromMask(mask=mask) # if we find something if( blobs ): # draw its convex hull with alpha blobs[-1].drawHull(width=-1,color=scv.Color.YELLOW,alpha=128) # and publish a position update pt = Point(x=blobs[-1].x,y=blobs[-1].y,z=blobs[-1].area()) self.point_pub.publish(pt) src = src.applyLayers() # convert SimpleCV to CV2 Numpy Format cv2img = src.getNumpyCv2() # Convert Cv2 numpy to ROS format img_msg = self.bridge.cv2_to_imgmsg(cv2img, "bgr8") # publish the topic. self.pub.publish(self.bridge.cv2_to_imgmsg(cv2img, "bgr8")) except CvBridgeError, e: print e
def convertData(self, bagfile): seq = 0 bridge = CvBridge() while(True): # Capture frame-by-frame ret, cvImage = self.capture.read() try: imageMsg = bridge.cv2_to_imgmsg(cvImage, "bgr8") # TODO: format spec as cmd option? except CvBridgeError, e: print e # creating ros message seq = seq + 1 imageMsg.header.seq = seq # TODO: temporary hack, time sync/source is needed imageMsg.header.stamp = rospy.Time.from_sec(time.time()) # write message to bag file bagfile.write(self.topic, imageMsg, imageMsg.header.stamp) # this is not so important for conversion if self.showFrames == True: cv2.imshow('frame', cvImage) if cv2.waitKey(1) & 0xFF == ord('q'): break
def processFrame(image): global pub1, pub2 conVerter = CvBridge() #Convert from a ROS image to a CV image try: image = conVerter.imgmsg_to_cv2(image, "bgr8") except CvBridgeError as e: print (e) result = bT.computeSpheres(image) msgBallCoords = ballcoords() msgModImage = Image() if result == None: return msgBallCoords.color= "RED" msgBallCoords.x = result[0] msgBallCoords.y = result[1] msgBallCoords.radius = result[2] # convert from a CV image to a ROS image resImage = result[3] #resImage = conVerter.cv2_to_imgmsg(resImage , "bgr8") msgModImage = conVerter.cv2_to_imgmsg(resImage , "bgr8") rospy.loginfo(msgBallCoords) pub1.publish(msgBallCoords) pub2.publish(msgModImage)
def spin(self): time.sleep(1.0) started = time.time() counter = 0 cvim = numpy.zeros((480, 640, 1), numpy.uint8) ball_xv = 10 ball_yv = 10 ball_x = 100 ball_y = 100 cvb = CvBridge() while not rospy.core.is_shutdown(): cvim.fill(0) cv2.circle(cvim, (ball_x, ball_y), 10, 255, -1) ball_x += ball_xv ball_y += ball_yv if ball_x in [10, 630]: ball_xv = -ball_xv if ball_y in [10, 470]: ball_yv = -ball_yv self.pub.publish(cvb.cv2_to_imgmsg(cvim)) time.sleep(0.03)
def tachyon(): rospy.init_node('tachyon') image_topic = rospy.get_param('~image', '/tachyon/image') image_pub = rospy.Publisher(image_topic, Image, queue_size=5) bridge = CvBridge() mode = rospy.get_param('~mode', 'mono8') config_file = rospy.get_param('~config', 'tachyon.yml') path = rospkg.RosPack().get_path('mashes_tachyon') config_filename = os.path.join(path, 'config', config_file) tachyon = Tachyon(config=config_filename) tachyon.connect() tachyon.calibrate(24) while not rospy.is_shutdown(): try: frame, header = tachyon.read_frame() frame = tachyon.process_frame(frame) if mode == 'rgb8': frame = LUT_IRON[frame] else: frame = np.uint8(frame >> 2) image_msg = bridge.cv2_to_imgmsg(frame, encoding=mode) image_msg.header.stamp = rospy.Time.now() image_pub.publish(image_msg) except CvBridgeError, e: print e
def sendImg(): global FPS global DEV global last_fpsGlobal rospy.init_node('camera_publisher_node', anonymous=True) image_pub = rospy.Publisher("/camera_publisher/output" + str(DEV) + "_" + str(FPS), Image, queue_size=10) bridge = CvBridge() cap = cv2.VideoCapture(DEV) cap.set(cv2.cv.CV_CAP_PROP_FPS, FPS) threading.Timer(1, printFPS).start () while not rospy.is_shutdown(): start = datetime.now() # Capture frame-by-frame ret, frame = cap.read() fpsGlobal = 1000000 / (datetime.now() - start).microseconds last_fpsGlobal = (fpsGlobal * 0.1) + (last_fpsGlobal * 0.9) if GUI: #gray = cv2.cvtColor(frame, cv2.COLOR_BGR2HSV) cv2.imshow("output" + str(DEV) + "_" + str(FPS), frame) if cv2.waitKey(1) & 0xFF == ord('q'): break try: image_pub.publish(bridge.cv2_to_imgmsg(frame, "bgr8")) except: pass
def send_camera_data(rgb_image, thresh_image): #global cv_depth_thresh bridge = CvBridge() cd = CameraData() cd.rgb_image = rgb_image cd.thresh_image = bridge.cv2_to_imgmsg(thresh_image, "passthrough") return cd
class DetectionPublisher(object): """ Publish ROS detection messages """ def __init__(self): self.DetPub = rospy.Publisher('Detection', Detection, queue_size=10) self._bridge = CvBridge() def publish(self, boxes, scores, classes, num, category_index, masks=None, fps=0): # init detection message msg = Detection() for i in range(boxes.shape[0]): if scores[i] > 0.5: if classes[i] in category_index.keys(): class_name = category_index[classes[i]]['name'] else: class_name = 'N/A' ymin, xmin, ymax, xmax = tuple(boxes[i].tolist()) box = RegionOfInterest() box.x_offset = xmin + (xmax-xmin)/2.0 box.y_offset = ymin + (ymax-ymin)/2.0 box.height = ymax - ymin box.width = xmax - xmin # fill detection message with objects obj = Object() obj.box = box obj.class_name = class_name obj.score = int(100*scores[i]) if masks is not None: obj.mask = self._bridge.cv2_to_imgmsg(masks[i], encoding="passthrough") msg.objects.append(obj) msg.fps = fps # publish detection message self.DetPub.publish(msg)
class VirtualMirror(object): def __init__(self): self.node_name = rospy.get_name() self.bridge = CvBridge() self.flip_direction = self.setupParam("~flip_direction","horz") # default horz self.pub_img = rospy.Publisher("~image/flipped",Image,queue_size=1) self.last_stamp = rospy.Time.now() self.sub_img = rospy.Subscriber("~img_in",CompressedImage, self.flipImage) self.param_timer = rospy.Timer(rospy.Duration.from_sec(1.0),self.cbParamTimer) def cbParamTimer(self,event): self.flip_direction = rospy.get_param("~flip_direction", "horz") def setupParam(self,param_name,default_value): value = rospy.get_param(param_name,default_value) rospy.set_param(param_name,value) #Write to parameter server for transparancy rospy.loginfo("[%s] %s = %s " %(self.node_name,param_name,value)) return value def flipImage(self,msg): if self.flip_direction == "vert": int_direction = 0 else: int_direction = 1 # Default horz np_arr = np.fromstring(msg.data, np.uint8) cv_image = cv2.imdecode(np_arr, cv2.CV_LOAD_IMAGE_COLOR) img_msg = cv2.flip(cv_image,int_direction) img_msg = self.bridge.cv2_to_imgmsg(img_msg, "bgr8") self.pub_img.publish(img_msg)
class Node(): def __init__(self): self.image_pub = rospy.Publisher('/image_raw', Image, queue_size=1) self.bridge = CvBridge() self.sensor_sub = rospy.Subscriber('/sensors', PendulumPose, self.pose_callback) self.cmd_sub = rospy.Subscriber('/cmd', Cmd, self.cmd_callback) self.rate = rospy.Rate(rospy.get_param('pendulum/gui_update_frequency')) self.cmd = Cmd() self.pose = PendulumPose() parameters = rospy.get_param('pendulum') self.visualizer = Visualizer(track_length = parameters['track_length']) def cmd_callback(self, cmd): self.cmd = cmd def pose_callback(self, pose): self.pose = pose def update(self): try: image = self.visualizer.draw(self.pose.x, self.pose.theta*pi/180, self.pose.xDot, self.pose.thetaDot*pi/180, self.cmd.cmd) ros_image = self.bridge.cv2_to_imgmsg(image, "rgb8") self.cmd = Cmd() self.image_pub.publish(ros_image) except CvBridgeError as e: print(e)
class SegmentationPublisher(object): """ Publish ROS detection messages """ def __init__(self): self.SegPub = rospy.Publisher('Segmentation', Segmentation, queue_size=10) self._bridge = CvBridge() def publish(self, boxes, classes, labels, seg_map, fps=0): # init detection message msg = Segmentation() boxes = [] for i in range(boxes.shape[0]): class_name = label[i] ymin, xmin, ymax, xmax = tuple(boxes[i].tolist()) box = RegionOfInterest() box.x_offset = xmin + (xmax-xmin)/2.0 box.y_offset = ymin + (ymax-ymin)/2.0 box.height = ymax - ymin box.width = xmax - xmin # fill segmentation message msg.boxes.append(box) msg.class_names.append(class_name) msg.seg_map = self._bridge.cv2_to_imgmsg(seg_map, encoding="passthrough") msg.fps = fps # publish detection message self.SegPub.publish(msg)
class Mirror(object): """ """ def __init__(self): """ """ self.node_name = rospy.get_name() self.bridge = CvBridge() self.pub_mirror = rospy.Publisher( "image/mirror", Image, queue_size=1) self.sub_compressed_img = rospy.Subscriber( "camera_node/image/compressed" , CompressedImage, self.callback, queue_size=1) def callback(self,msg): """ """ # Load message #cv_img = self.bridge.imgmsg_to_cv2( msg.data ) np_arr = np.fromstring(msg.data, np.uint8) cv_img = cv2.imdecode(np_arr, cv2.CV_LOAD_IMAGE_COLOR) # Mirror cv_img_mirror = cv_img.copy() cv_img_mirror = cv2.flip(cv_img,1) # Publish Message img_msg = self.bridge.cv2_to_imgmsg( cv_img_mirror , "bgr8") img_msg.header.stamp = msg.header.stamp img_msg.header.frame_id = msg.header.frame_id self.pub_mirror.publish(img_msg)
class image_converter: def __init__(self): rospy.init_node('image_converter', anonymous=True) self.image_pub = rospy.Publisher("analyzed_image", Image, queue_size=10) self.bridge = CvBridge() self.image_sub = rospy.Subscriber("image_raw", Image, self.callback) self.stabilizer = VideoStabilizer([[409, 250], [300, 762], [1123, 848], [1600, 238]]) def callback(self, data): try: cv_image = self.bridge.imgmsg_to_cv2(data, "bgr8") except CvBridgeError as e: print(e) #cv_image = self.analyze_image(cv_image) #self.showImage(cv_image) try: self.image_pub.publish(self.bridge.cv2_to_imgmsg(cv_image, "bgr8")) except CvBridgeError as e: print(e) def analyze_image(self, image): image = self.stabilizer.stabilize_frame(image) return image def showImage(self, image): cv2.imshow("Image window", image) cv2.waitKey(3)
class Echo: def __init__(self): self.node_name = "BlobDetector" self.thread_lock = threading.Lock() self.sub_image = rospy.Subscriber("/camera/rgb/image_rect_color",\ Image, self.cbImage, queue_size=1) self.pub_image = rospy.Publisher("/blob_image",\ Image, queue_size=1) self.pub_data = rospy.Publisher("/blob_detections", BlobDetections, queue_size = 1) self.bridge = CvBridge() rospy.loginfo("[%s] Initialized." %(self.node_name)) def cbImage(self,image_msg): thread = threading.Thread(target=self.processImage,args=(image_msg,)) thread.setDaemon(True) thread.start() def processImage(self, image_msg): if not self.thread_lock.acquire(False): return image_cv = self.bridge.imgmsg_to_cv2(image_msg) msg, img = process(image_cv) try: self.pub_image.publish(self.bridge.cv2_to_imgmsg(img, "bgr8")) self.pub_data.publish(msg) except CvBridgeError as e: print(e) self.thread_lock.release()
def test_target_image(self): """Tests posting telemetry data through client.""" # Set up test data. url = "http://interop" client_args = (url, "testuser", "testpass", 1.0) target_id = 1 width = 40 height = 30 nparr = np.random.randint(0, 256, (width, height, 3)).astype(np.uint8) bridge = CvBridge() ros_img = bridge.cv2_to_imgmsg(nparr) img = TargetImageSerializer.from_msg(ros_img) with InteroperabilityMockServer(url) as server: # Setup mock server. server.set_root_response() server.set_login_response() server.set_post_target_image_response(target_id) server.set_get_target_image_response(target_id, img, "image/png") server.set_delete_target_image_response(target_id) # Connect client. client = InteroperabilityClient(*client_args) client.wait_for_server() client.login() client.post_target_image(target_id, ros_img) client.get_target_image(target_id) client.delete_target_image(target_id)
class image_converter: def __init__(self): self.image_pub = rospy.Publisher("image_topic_2",Image) cv2.namedWindow("Image window", 1) self.bridge = CvBridge() self.image_sub = rospy.Subscriber("image_topic",Image,self.callback) def callback(self,data): try: cv_image = self.bridge.imgmsg_to_cv2(data, "bgr8") except CvBridgeError as e: print(e) (rows,cols,channels) = cv_image.shape if cols > 60 and rows > 60 : cv2.circle(cv_image, (50,50), 10, 255) cv2.imshow("Image window", cv_image) cv2.waitKey(3) try: self.image_pub.publish(self.bridge.cv2_to_imgmsg(cv_image, "bgr8")) except CvBridgeError as e: print(e)
class circle_detect: def __init__(self): self.image_pub = rospy.Publisher("circle_detection", Image, queue_size = 10) self.bridge = CvBridge() self.image_sub = rospy.Subscriber("/rgb_image", Image, self.callback) def callback(self, data): try: cv_image = self.bridge.imgmsg_to_cv2(data, "bgr8") except CvBridgeError as e: print(e) x0 = 320 y0 = 240 idx = None minC = 800 numC = None img = cv2.medianBlur(cv_image,5) imgg = cv2.cvtColor(img, cv2.COLOR_RGB2GRAY) cimg = cv2.cvtColor(imgg, cv2.COLOR_GRAY2BGR) circles = cv2.HoughCircles(imgg,cv2.cv.CV_HOUGH_GRADIENT, 1.2, 100, param1 = 200, param2 = 30, minRadius = 5, maxRadius = 20) if circles is None: cv2.imshow("Image Windows", cv_image) rospy.loginfo('No circles detected') #circles = np.uint16(np.around(circles)) # .astype("int") # convert the (x,y) coordinates and radius of the circles to integers #numC = len(circles[0,:]) else: print circles.shape # rospy.loginfo('circles found, number of circles {}'.format(numC)) # for i in circles: # dist[i] = distance(x0,y0, i[0],i[1]) # if dist[i] < minC: # minC = dist[i] # idx = i #for i in circles[0,:]: # print idx # cv2.circle(cv_image, (idx[0],idx[1]),idx[2], (0,255,0),1) # draw the outer cirlce # cv2.circle(cv_image, (idx[0],idx[1]), 2, (0,0,255), 3) # draw the center of the circle for i in circles[0,:]: cv2.circle(cv_image, (i[0],i[1]),i[2], (0,255,0),1) # draw the outer cirlce cv2.circle(cv_image, (i[0],i[1]), 2, (0,0,255), 3) # draw the center of the circle cv2.imshow("Image Windows", cv_image) cv2.waitKey(3) try: self.image_pub.publish(self.bridge.cv2_to_imgmsg(cv_image, "bgr8")) except CvBridgeError as e: print(e)
def run(): global right_image global left_image # Init node rospy.init_node("stereo_vision"); # Subscribe for two "eyes" rospy.Subscriber("/stereo/right/image_raw", Image, right_image_callback) rospy.Subscriber("/stereo/left/image_raw", Image, left_image_callback) stereo_publisher = rospy.Publisher("/vision/stereo", Image, queue_size=1000) # Action print "Start processing..." bridge = CvBridge() worker = StereoVision() # rate = rospy.Rate(10) while not rospy.is_shutdown(): if right_image != None and left_image != None: cv_right_image = bridge.imgmsg_to_cv2(right_image, desired_encoding="bgr8") cv_left_image = bridge.imgmsg_to_cv2(left_image, desired_encoding="bgr8") cv_stereo_image = worker.create_stereo_image( cv_right_image, cv_left_image ) stereo_image = bridge.cv2_to_imgmsg(cv_stereo_image, encoding="bgr8") stereo_publisher.publish( stereo_image )
class afisare: def __init__(self): self.bridge = CvBridge() self.cap = cv2.VideoCapture(0) self.cap2 = rospy.Subscriber("/optris/image_color", Image, self.callback) self.pub = rospy.Publisher("topic1", Image, queue_size = 5) def callback(self, data): try: self.cv_image = self.bridge.imgmsg_to_cv2(data, "bgr8") gray = cv2.cvtColor(self.cv_image,cv2.COLOR_BGR2GRAY) except CvBridgeError as e: print (e) try: self.pub.publish(self.bridge.cv2_to_imgmsg(gray, '8UC1')) except CvBridgeError as e: print (e) cv2.imshow("Fereastra1", gray) cv2.waitKey(3)
def gen_blog_entry(self, roi_id, obj_id, objs): print 'Region: ' + self.get_roi_name(roi_id) time = dt.fromtimestamp(int(rospy.get_time())) body = '### CHANGE DETECTION REPORT\n\n' body += '- **Region:** ' + self.get_roi_name(roi_id) + '\n\n' #body += '- **Object ID:** ' + str(obj_id) + '\n\n' body += '- **Time:** ' + str(time) + '\n\n' # Create some blog entries msg_store = MessageStoreProxy(collection=self.blog_collection) robblog_path = roslib.packages.get_pkg_dir('soma_utils') for idx,obj in enumerate(objs): bridge = CvBridge() im = bridge.imgmsg_to_cv2(obj.image_mask, desired_encoding="bgr8") imgray = cv2.cvtColor(im,cv2.COLOR_BGR2GRAY) ret,thresh = cv2.threshold(imgray,1,1,1) #cv2.threshold(imgray,127,255,0) contours, hierarchy = cv2.findContours(thresh,cv2.RETR_TREE,cv2.CHAIN_APPROX_SIMPLE) bridge = CvBridge() cv_image = bridge.imgmsg_to_cv2(obj.image_full, desired_encoding="bgr8") cv2.drawContours(cv_image,contours,-1,(0,0,255),2) full_scene_contour = bridge.cv2_to_imgmsg(cv_image) img_id = msg_store.insert(full_scene_contour) body += '<font color="red">Detected change (' + str(idx+1) + '/'+ str(len(objs)) + '):</font>\n\n![My helpful screenshot](ObjectID(%s))\n\n' % img_id e = RobblogEntry(title=str(time) + " Change Detection - " + self.roi_name[roi_id], body= body ) msg_store.insert(e)
def test_encode_decode_cv2(self): import cv2 import numpy as np fmts = [cv2.CV_8U, cv2.CV_8S, cv2.CV_16U, cv2.CV_16S, cv2.CV_32S, cv2.CV_32F, cv2.CV_64F] cvb_en = CvBridge() cvb_de = CvBridge() for w in range(100, 800, 100): for h in range(100, 800, 100): for f in fmts: for channels in ([], 1, 2, 3, 4, 5): if channels == []: original = np.uint8(np.random.randint(0, 255, size=(h, w))) else: original = np.uint8(np.random.randint(0, 255, size=(h, w, channels))) rosmsg = cvb_en.cv2_to_imgmsg(original) newimg = cvb_de.imgmsg_to_cv2(rosmsg) self.assert_(original.dtype == newimg.dtype) if channels == 1: # in that case, a gray image has a shape of size 2 self.assert_(original.shape[:2] == newimg.shape[:2]) else: self.assert_(original.shape == newimg.shape) self.assert_(len(original.tostring()) == len(newimg.tostring()))
def send_test_messages(self, filename): self.msg_received = False # Publish the camera info TODO make this a field in the annotations file to dictate the source calibration file msg_info = CameraInfo() msg_info.height = 480 msg_info.width = 640 msg_info.distortion_model = "plumb_bob" msg_info.D = [-0.28048157543793056, 0.05674481026365553, -0.000988764087143394, -0.00026869128565781613, 0.0] msg_info.K = [315.128501, 0.0, 323.069638, 0.0, 320.096636, 218.012581, 0.0, 0.0, 1.0] msg_info.R = [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0] msg_info.P = [217.48876953125, 0.0, 321.3154072384932, 0.0, 0.0, 250.71084594726562, 202.30416165274983, 0.0, 0.0, 0.0, 1.0, 0.0] msg_info.roi.do_rectify = False # Publish the test image img = cv2.imread(filename) cvb = CvBridge() msg_raw = cvb.cv2_to_imgmsg(img, encoding="bgr8") self.pub_info.publish(msg_info) self.pub_raw.publish(msg_raw) # Wait for the message to be received timeout = rospy.Time.now() + rospy.Duration(10) # Wait at most 5 seconds for the node to reply while not self.msg_received and not rospy.is_shutdown() and rospy.Time.now() < timeout: rospy.sleep(0.1) self.assertLess(rospy.Time.now(), timeout, "Waiting for apriltag detection timed out.")
class ImageAvNode(object): def __init__(self): self.node_name = "Image Av" self.sub_image = rospy.Subscriber("/ernie/camera_node/image/compressed", CompressedImage,self.flipImageNP) self.pub_image = rospy.Publisher("~mirror_image/compressed", CompressedImage, queue_size=1) self.bridge = CvBridge() def flipImageNP(self,msg): np_array = np.fromstring(msg.data, np.uint8) image_np = cv2.imdecode(np_array, cv2.CV_LOAD_IMAGE_COLOR) #flip_arr = np.fliplr(np_arr) imageflip_np=cv2.flip(image_np,1) flip_im = CompressedImage() flip_im.data = np.array(cv2.imencode('.jpg', imageflip_np)[1]).tostring() #flip_im.data = flip_arr.tostring() flip_im.header.stamp = msg.header.stamp self.pub_image.publish(flip_im) def flipImage(self,msg): # Decode from compressed image # with OpenCV cv_image = self.bridge.imgmsg_to_cv2(msg, desired_encoding="bgr8") #image_cv = cv2.imdecode(np.fromstring(msg.data, np.uint8), cv2.CV_LOAD_IMAGE_COLOR) hei_original = image_cv.shape[0] wid_original = image_cv.shape[1] #reverseimage = image_cv[:,:,-1] reverseimg=cv2.flip(cv_image,1) image_msg_out = self.bridge.cv2_to_imgmsg(reverseimage, "bgr8") image_msg_out.header.stamp = image_msg.header.stamp self.pub_image.publish(image_msg_out)
class BaselineOrangeFinder: def __init__(self, image_topic='/d400/color/image_raw', depth_topic='/d400/aligned_depth_to_color/image_raw', gcop_topic="/gcop_odom"): print(image_topic, depth_topic) self.image_sub = message_filters.Subscriber(image_topic, Image) self.depth_sub = message_filters.Subscriber(depth_topic, Image) self.odom_node = message_filters.Subscriber(gcop_topic, Odometry) self.__params() self.__bridge = CvBridge() self.__segmodel = SegmentationNet() self.__loadModel() torch.no_grad() self.__loadMeanImages() self.__pointcloud_publisher = rospy.Publisher("/pointcloud_topic", PointCloud, queue_size=2) self.__pointcloud_publisher_extra = rospy.Publisher("/pointcloud_topic_extra", PointCloud, queue_size=2) self.__pointcloud_publisher_extra_norange = rospy.Publisher("/pointcloud_topic_extra_norange", PointCloud, queue_size=2) self.__goalpoint_publisher = rospy.Publisher("/ros_tracker", PoseArray, queue_size=100) self.__normal_vec_publisher = rospy.Publisher("/normal_tracker", PoseArray, queue_size=100) self.__seg_image_publisher = rospy.Publisher("/seg_image", Image, queue_size=5) rot = Rotations() self.world2orange = None #np.eye(3)#np.matmul(rot.rotz(0), np.matmul(rot.rotx(np.pi/2),np.matmul(rot.rotz(np.pi/2), rot.roty(np.pi/2)))) #self.world2orange = np.matmul(rot.rotz(np.pi/2), rot.roty(np.pi/2)) self.listener = tf.TransformListener() self.br = tf.TransformBroadcaster() self.world2orange_pos = None self.t = 0 self.alpha = 0.5 self.min_alpha = 0.05 self.max_steps = 50 self.stamp_now = None print("Setup complete") def __loadModel(self): if os.path.isfile(self.__segload): print("Loading Model: ", self.__segload) if not self.__gpu is None: checkpoint = torch.load(self.__segload,map_location=torch.device('cuda')) else: checkpoint = torch.load(self.__segload) self.__segmodel.load_state_dict(checkpoint) self.__segmodel.eval() print("Model Loaded.") else: print("No checkpoint found at: ", self.__segload) exit(0) if not self.__gpu is None: self.__device = torch.device('cuda:'+str(self.__gpu)) self.__segmodel = self.__segmodel.to(self.__device) else: self.__device = torch.device('cpu') self.__segmodel = self.__segmodel.to(self.__device) self.__segmodel.eval() def __loadMeanImages(self): if not (os.path.exists(self.__mean_image_loc)): print('mean image file not found', self.__mean_image_loc) exit(0) else: print('mean image file found') self.__mean_image = np.load(self.__mean_image_loc) def __params(self, gpu=True, relative=True): self.__num_images = 1 self.__segload = "/home/gabe/repos/aerial_autonomy_ws/src/orange_picking/useful_models/seg/model_seg145.pth.tar" self.__mean_image_loc = "/home/gabe/repos/aerial_autonomy_ws/src/orange_picking/useful_models/seg/mean_image.npy" if torch.cuda.is_available() and gpu: self.__gpu = 0 else: self.__gpu = None self.__h = 480 self.__w = 640 def __rosmsg2np(self,data): try: image_arr = self.__bridge.imgmsg_to_cv2(data,"rgb8") except CvBridgeError as e: print(e) return image_arr def __meanSubtract(self, cv_image): cv_image = cv2.resize(cv_image,(self.__w,self.__h)) cv2.imwrite("test_pre.png", cv_image) cv_image = cv_image/255.0 #print(cv_image.shape) if self.__mean_image is None: mean_subtracted = (cv_image) print("ISSUEE: NO MEAN!!! ") else: mean_subtracted = (cv_image-self.__mean_image) return mean_subtracted def __process4model(self, image_arr): image_arr = self.__meanSubtract(image_arr) image_arr = image_arr.transpose(2,0,1) image_arr = image_arr.reshape((1,3,self.__h,self.__w)) image_tensor = torch.tensor(image_arr) image_tensor = image_tensor.to(self.__device,dtype=torch.float) return image_tensor def __segmentationInference(self, image_tensor): seglogits = self.__segmodel(image_tensor) seglogits = seglogits.view(-1,2,self.__segmodel.h,self.__segmodel.w) segimages = (torch.max(seglogits, 1).indices).to('cpu') #.type(torch.FloatTensor).to(device) seg_np = np.array(segimages[0,:,:]) pub_np = (255 * seg_np.copy()).astype(np.uint8).reshape((self.__h, self.__w)) image_message = self.__bridge.cv2_to_imgmsg(pub_np, encoding="passthrough") self.__seg_image_publisher.publish(image_message) cv2.imwrite('test.png', pub_np) return seg_np def __depthnpFromImage(self, msg): #Stolen from https://github.com/eric-wieser/ros_numpy/blob/master/src/ros_numpy/image.py if not msg.encoding in name_to_dtypes: raise TypeError('Unrecognized encoding {}'.format(msg.encoding)) dtype_class, channels = name_to_dtypes[msg.encoding] dtype = np.dtype(dtype_class) dtype = dtype.newbyteorder('>' if msg.is_bigendian else '<') shape = (msg.height, msg.width, channels) data = np.fromstring(msg.data, dtype=dtype).reshape(shape) data.strides = ( msg.step, dtype.itemsize * channels, dtype.itemsize ) if channels == 1: data = data[...,0] return data.astype(np.float64) # dtype_class = np.uint16 # channels = 1 # dtype = np.dtype(dtype_class) # dtype = dtype.newbyteorder('>' if image.is_bigendian else '<') # shape = (image.height, image.width, channels) # data = np.fromstring(image.data, dtype=dtype).reshape(shape) # data.strides = ( # image.step, # dtype.itemsize * channels, # dtype.itemsize # ) # data = data[:,:,0].astype(np.float64) # cm_from_pixel = 0.095 # return data #*cm_from_pixel def __getCentroid(self, seg_image): centroid = ndimage.measurements.center_of_mass(seg_np) if np.isnan(centroid[0]) or np.isnan(centroid[1]): return None #if (c[0] < 100) or (c[1] < 100): # PIL.Image.fromarray((seg_np*255).astype('uint8')).show() return centroid def __convert_depth_frame_to_pointcloud(self, depth_image, camera_intrinsics, area=None): #stolen from https://github.com/IntelRealSense/librealsense/blob/master/wrappers/python/examples/box_dimensioner_multicam/helper_functions.py#L151 """ Convert the depthmap to a 3D point cloud Parameters: ----------- depth_frame : rs.frame() The depth_frame containing the depth map camera_intrinsics : The intrinsic values of the imager in whose coordinate system the depth_frame is computed Return: ---------- x : array The x values of the pointcloud in meters y : array The y values of the pointcloud in meters z : array The z values of the pointcloud in meters """ if area is None: [height, width] = depth_image.shape nx = np.linspace(0, width-1, width) ny = np.linspace(0, height-1, height) u, v = np.meshgrid(nx, ny) else: u = area[:, 1] v = area[:, 0] x = (u.flatten() - camera_intrinsics.ppx)/camera_intrinsics.fx y = (v.flatten() - camera_intrinsics.ppy)/camera_intrinsics.fy if area is None: z = depth_image.flatten() / 1000 else: z = depth_image[area[:,0], area[:,1]].flatten() / 1000 x = np.multiply(x,z) y = np.multiply(y,z) x = x[np.nonzero(z)] y = y[np.nonzero(z)] z = z[np.nonzero(z)] return x, y, z def __publishPointCloud(self, x, y, z, publ, step=100): pointcloud = PointCloud() header = std_msgs.msg.Header() header.stamp = self.stamp_now #rospy.Time.now() header.frame_id = 'camera_depth_optical_frame' #header.frame_id = 'camera_link' pointcloud.header = header for i in range(0, len(x), step): pointcloud.points.append(Point32(x[i], y[i], z[i])) publ.publish(pointcloud) def __rotate_frame(self, x, y, z): x = x.reshape((x.shape[0], 1)) y = y.reshape((y.shape[0], 1)) z = z.reshape((z.shape[0], 1)) points = np.concatenate((x, y, z), axis=1) #print(x.shape, points.shape) r = Rotations() points = (np.matmul(r.rotz(np.pi/2), np.matmul(r.rotx(np.pi/2), points.T))).T x = points[:,0] y = points[:,1] z = points[:,2] return x, y, z def __orange_orientation(self, trans, rot, mean_pos, orientation=None): if self.alpha != self.min_alpha: delta = (self.alpha - self.min_alpha)/self.max_steps self.alpha -= delta print(self.alpha) #decaying alpha in future rot = R.from_quat(np.array(rot)) #fixed_transform = R.from_quat(np.array([-0.500, 0.500, 0.500, 0.500])).as_dcm() temp_world2orange_pos = np.matmul(rot.as_dcm(), mean_pos) + np.array(trans) if self.world2orange_pos is None: self.world2orange_pos = temp_world2orange_pos.copy() else: self.world2orange_pos = self.world2orange_pos + self.alpha*(temp_world2orange_pos-self.world2orange_pos) if orientation is not None: rot_obj = Rotations() orientation = R.from_dcm(np.matmul(np.matmul(rot.as_dcm(),R.from_quat(orientation).as_dcm()), rot_obj.rotx(np.pi/2))).as_quat() if self.world2orange is None: self.world2orange = R.from_quat(orientation).as_dcm() else: new_rot = orientation old_rot = R.from_dcm(self.world2orange).as_quat() key_times = [0, 1] key_rots = np.array([old_rot, new_rot]) key_rots = R.from_quat(key_rots) slerp = Slerp(key_times, key_rots) times = [self.alpha] mean_rot = slerp(times) mean_rot = mean_rot[0] self.world2orange = mean_rot.as_dcm() self.br.sendTransform((self.world2orange_pos[0], self.world2orange_pos[1], self.world2orange_pos[2]), R.from_dcm(self.world2orange).as_quat(), self.stamp_now, #rospy.Time.now(), "orange", "world") Rot = R.from_dcm(np.matmul(np.linalg.inv(rot.as_dcm()), self.world2orange)).as_quat() Trans = np.matmul(np.linalg.inv(rot.as_dcm()), (self.world2orange_pos - np.array(trans))) return Rot, Trans def _publishGoalPoints(self, x, y, z, trans=None, rot=None, orientation=None, odom_data=None, num_points=1): if trans is None or rot is None: try: # (trans,rot) = self.listener.lookupTransform('world', 'camera_depth_optical_frame_filtered', rospy.Time(0)) (trans,rot) = self.listener.lookupTransform('world', 'camera_depth_optical_frame', rospy.Time(0)) except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException): return if np.any(np.isnan(trans)) or np.any(np.isnan(rot)) or np.any(np.isnan(x)) or np.any(np.isnan(y)) or np.any(np.isnan(z)): print("NANANNANANANANANNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNANANANANANANANANNANNANANA") return mean_x = np.mean(x) mean_y = np.mean(y) mean_z = np.mean(z) # if odom_data is not None: # rot = np.array([odom_data.pose.pose.orientation.x, odom_data.pose.pose.orientation.y, odom_data.pose.pose.orientation.z, odom_data.pose.pose.orientation.w]) orientation, mean_pos = self.__orange_orientation(trans, rot, np.array([mean_x, mean_y, mean_z]), orientation) mean_x, mean_y, mean_z = mean_pos[0], mean_pos[1], mean_pos[2] header = std_msgs.msg.Header() header.stamp = self.stamp_now #rospy.Time.now() header.frame_id = 'camera_depth_optical_frame_filtered' header.frame_id = 'camera_depth_optical_frame' #header.frame_id = 'camera_link' goalarray_msg = PoseArray() goalarray_msg.header = header for i in range(num_points): goal = Pose() goal.position.x = mean_x goal.position.y = mean_y goal.position.z = mean_z goal.orientation.x = orientation[0] goal.orientation.y = orientation[1] goal.orientation.z = orientation[2] goal.orientation.w = orientation[3] goalarray_msg.poses.append(goal) # print("Publishing") self.__goalpoint_publisher.publish(goalarray_msg) def __skew(self, vec): return np.array([[0, -vec[2], vec[1]], [vec[2], 0, -vec[0]], [-vec[1], vec[0], 0]]) def __find_rot(self, vector): vector = np.array(vector) vector = vector/np.linalg.norm(vector) vector2 = np.array([1, 0, 0]) v = np.cross(vector, vector2) s = np.linalg.norm(v) c = np.dot(vector, vector2) rot = np.eye(3) + self.__skew(v) + (np.matmul(self.__skew(v), self.__skew(v))*(1-c)/(s*s)) return R.from_dcm(rot).as_quat() def debugPlanePlotter(self, pts, plane, orientation, position): x = np.linspace(-2.,2.,20) y = np.linspace(-2.,2.,20) X,Y = np.meshgrid(x,y) Z=(plane[0]*X + plane[1]*Y + plane[3])/-plane[2] fig = plt.figure() ax = fig.gca(projection='3d') #surf = ax.plot_surface(X, Y, Z) ax.scatter3D(X, Y, Z, c='b') ax.scatter3D(pts[:,0], pts[:,1], pts[:,2], c='r') # ax.set_aspect('equal') ax.set_zlabel("x") ax.set_xlabel("-y") ax.set_ylabel("-z") pos = np.array(position) + np.array(plane[:3]) print("Pos: ", pos) ax.scatter3D(pos[0], pos[1], pos[2], c='g') ax.scatter3D(pos[0], position[1], pos[2], c='k') ax.scatter3D(position[0], position[1], position[2], c='y') R_m = R.from_quat(orientation).as_dcm() plot_basis(ax, R_m, np.array(position), alpha=1) ax.set_xlim(-2. + position[0], 2.+ position[0]) ax.set_ylim(-2.+ position[1], 2.+ position[1]) ax.set_zlim(-2.+ position[2], 2.+ position[2]) ax.set_aspect('equal', 'box') plt.show() def __find_plane(self, x, y, z, Trans, Rot, mean_pt, debug=False): #mean_pt = mean_pt/np.linalg.norm(mean_pt) pts = np.array([x, y, z]).T print(pts.shape) t_size = np.min((500, pts.shape[0])) t_pts = np.random.choice(np.arange(pts.shape[0]), size=t_size, replace=False) pts = pts[t_pts] print(pts.shape) all_points = open3d.utility.Vector3dVector(pts) pc = geometry.PointCloud(all_points) plane, success_pts = pc.segment_plane(0.05, int(pts.shape[0]*0.7), 1000) plane = np.array(plane) # print("Plane:", pts.shape, len(success_pts)) if debug: print("Plane: ", plane) print("Mean pt: ", mean_pt) plane_debug = plane.copy() d = np.dot(mean_pt, plane[:3]) if d > 0: plane = -plane plane_ = plane[:3] #np.matmul(R.from_quat(Rot).as_dcm(), plane[:3].T) plane_[1] = 0 plane_ /= np.linalg.norm(plane_) y = np.array([0, -1, 0]) z = np.cross(plane_, y) rot = np.array([plane_, y, z]).T # print(rot, np.linalg.det(rot), R.from_dcm(rot).as_euler('ZYX', degrees=True)) orientation = R.from_dcm(rot).as_quat() if debug: self.debugPlanePlotter(pts, plane, orientation, position=mean_pt) normal_vec = Pose() normal_vec.position.x = plane_[0] normal_vec.position.y = plane_[1] # normal_vec.position.z = plane[2] #orientation = [0, 0, 0, 1]#self.__find_rot(plane[0:3]) #normal_vec.orientation.x = orientation[0] #normal_vec.orientation.y= orientation[1] #normal_vec.orientation.z= orientation[2] #normal_vec.orientation.w = orientation[3] vecs = PoseArray() vecs.poses.append(normal_vec) header = std_msgs.msg.Header() header.stamp = self.stamp_now #rospy.Time.now() header.frame_id = "orange" vecs.header = header self.__normal_vec_publisher.publish(vecs) # print(plane) return orientation def publishData(self, depth_image, camera_intrinsics, area, publ, Trans, Rot, mean_pt = None, norm_tracker=False, tracker=False): if area.shape[0] > 30: # print(area.shape) x, y, z = self.__convert_depth_frame_to_pointcloud(depth_image, camera_intrinsics, area) #print(x.shape, y.shape, z.shape) if np.any(np.isnan(x)) or np.any(np.isnan(y)) or np.any(np.isnan(z)): return # print(np.mean(x), np.mean(y), np.mean(z)) # print(np.min(x),np.max(x),np.min(y),np.max(y), np.min(z),np.max(z)) # print("\n\n\n") filter = True if filter: pts = np.array([x, y, z]).T pts = self.reject_outliers3d(pts) x, y, z = pts[:, 0], pts[:, 1], pts[:, 2] total_points = 1000 step = int(x.shape[0]/total_points) step = np.max((1, step)) #z -= 0.02 self.__publishPointCloud(x, y, z, publ, step=step) if x.shape[0] < 10: return None orientation = None if norm_tracker: if mean_pt is None: print("ISSUUEE") exit(0) #mean_pt = np.array([np.mean(x), np.mean(y), np.mean(z)]) orientation = self.__find_plane(x, y, z, Trans, Rot, mean_pt) if tracker: if orientation is None or mean_pt is None: print("This should not happen!") exit(0) self._publishGoalPoints(mean_pt[0], mean_pt[1], mean_pt[2], orientation=orientation, trans=Trans, rot=Rot, odom_data=None) else: pass #self._publishGoalPoints(x, y, z, orientation=orientation, odom_data=None) return np.array([np.mean(x), np.mean(y), np.mean(z)]) else: print("Failed in Publish Data check") def reject_outliers(self, data, m=1.5): return data[np.multiply(np.abs(data[:, 0] - np.mean(data[:, 0])) < m * np.std(data[:, 0]), np.abs(data[:,1] - np.mean(data[:,1])) < m * np.std(data[:,1]))] def reject_outliers3d(self, data, m=3): return data[np.multiply(np.multiply(np.abs(data[:, 0] - np.mean(data[:, 0])) < m * np.std(data[:, 0]), np.abs(data[:,1] - np.mean(data[:,1])) < m * np.std(data[:,1])), np.abs(data[:,2] - np.mean(data[:,2])) < m * np.std(data[:,2]))] def reject_outliers3dMedian(self, data, m=3): return data[np.multiply(np.multiply(np.abs(data[:, 0] - np.median(data[:, 0])) < m * np.std(data[:, 0]), np.abs(data[:,1] - np.median(data[:,1])) < m * np.std(data[:,1])), np.abs(data[:,2] - np.median(data[:,2])) < m * np.std(data[:,2]))] def callback(self, image_data, depth_image_data): print("Reaching callbazck") self.t += 1 t1 = time.time() self.stamp_now = image_data.header.stamp image = self.__rosmsg2np(image_data) depth_image = self.__depthnpFromImage(depth_image_data) image_tensor = self.__process4model(image) seg_np = self.__segmentationInference(image_tensor) #centroid = self.__getCentroid(seg_np) camera_intrinsics = CameraIntrinsics() area = np.argwhere(seg_np == 1) pre_area = area.copy() # print("Orignal shape: ", area.shape) if area.shape[0] < 30: print("Early return??", area.shape) return area = self.reject_outliers(area) # print("No outlier shape: ", area.shape) mean_x, mean_y = np.mean(area, axis=0) min_ = np.min(area, axis=0) max_ = np.max(area, axis=0) min_x, min_y = min_[0], min_[1] max_x, max_y = max_[0], max_[1] size_x = np.abs(max_x - min_x) size_y = np.abs(max_y - min_y) size_ = np.max((size_x, size_y)) # mult_x, mult_y = 1.5, 1.5 #extra_min_x, extra_max_x = np.max((0, min_x-int(mult_x*size_x))), np.min((seg_np.shape[0], max_x+int(mult_x*size_))) #extra_min_y, extra_max_y = np.max((0, min_y-int(mult_y*size_y))), np.min((seg_np.shape[1], max_y+int(mult_y*size_))) mult_x, mult_y = 2., 2. extra_min_x, extra_max_x = np.max((0, mean_x-int(mult_x*size_x))), np.min((seg_np.shape[0], mean_x+int(mult_x*size_))) extra_min_y, extra_max_y = np.max((0, mean_y-int(mult_y*size_y))), np.min((seg_np.shape[1], mean_y+int(mult_y*size_))) nx = np.linspace(extra_min_x, extra_max_x-1, extra_max_x - extra_min_x, dtype=np.int32) ny = np.linspace(extra_min_y, extra_max_y-1, extra_max_y - extra_min_y, dtype=np.int32) extra_area = np.transpose([np.tile(nx, len(ny)), np.repeat(ny, len(nx))]) trans, rot = None, None try: # (trans,rot) = self.listener.lookupTransform('world', 'camera_depth_optical_frame_filtered', rospy.Time(0)) (trans,rot) = self.listener.lookupTransform('world', 'camera_depth_optical_frame', rospy.Time(0)) except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException): return if trans is None or rot is None: print("tf None") return if area.shape[0] > 30: # print("In orange: ") mean_pt = self.publishData(depth_image, camera_intrinsics, area, self.__pointcloud_publisher, Trans=trans, Rot=rot, tracker=False) if mean_pt is None: return # print("In extra orange ") self.publishData(depth_image, camera_intrinsics, extra_area, self.__pointcloud_publisher_extra, Trans=trans, Rot=rot, mean_pt=mean_pt, norm_tracker=True, tracker=True) # print(area.shape) # x, y, z = self.__convert_depth_frame_to_pointcloud(depth_image, camera_intrinsics, area) # #print(x.shape, y.shape, z.shape) # if np.any(np.isnan(x)) or np.any(np.isnan(y)) or np.any(np.isnan(z)): # return # print(np.mean(x), np.mean(y), np.mean(z)) # # print(np.min(x),np.max(x),np.min(y),np.max(y), np.min(z),np.max(z)) # # print("\n\n\n") # self.__publishPointCloud(x, y, z, step=20) # #self.__find_plane(x, y, z) # self._publishGoalPoints(x, y, z, odom_data=None) print(time.time()-t1) else: print("Left pre-area check", area.shape, pre_area.shape)
class ArucoLocalization(): def __init__(self): # Load params from launch file param = rospy.search_param("subscribed_image_topic") sub_img_topic = rospy.get_param(param) param = rospy.search_param("aruco_image_topic") aruco_image_topic = rospy.get_param(param) param = rospy.search_param("qualcomm_pose_topic") qualcomm_pose_topic = rospy.get_param(param) param = rospy.search_param("cam_calib_path") cam_calibration_path = rospy.get_param(param) param = rospy.search_param("marker_map_path") marker_map_path = rospy.get_param(param) param = rospy.search_param("aruco_dictionary") aruco_dict = rospy.get_param(param) if aruco_dict == "4x4" or aruco_dict == "4X4" or str( aruco_dict) == "4": aruco_dictionary = cv2.aruco.DICT_4X4_250 elif aruco_dict == "5x5" or aruco_dict == "5X5" or str( aruco_dict) == "5": aruco_dictionary = cv2.aruco.DICT_5X5_250 elif aruco_dict == "6x6" or aruco_dict == "6X6" or str( aruco_dict) == "6": aruco_dictionary = cv2.aruco.DICT_6X6_250 else: print("Invalid (or not included) aruco marker dictionary") print(aruco_dictionary) # Initialize CV bridge self.bridge = CvBridge() # Initialize publisher self.pose_pub = rospy.Publisher(qualcomm_pose_topic, PoseStamped, queue_size=10) self.im_pub = rospy.Publisher(aruco_image_topic, Image, queue_size=1) # Initialize aruco details self.cam_mtx, self.distort_mtx = self.loadCoefficients( cam_calibration_path) self.marker_set, self.marker_dict = self.load_markers(marker_map_path) self.dictionary = cv2.aruco.Dictionary_get(aruco_dictionary) self.parameters = cv2.aruco.DetectorParameters_create() # Initialize transform from camera to qualcomm tmat = translation_matrix((0.1, 0, 0.2)) qmat = quaternion_matrix(quaternion_from_euler(-1.57, 0, -1.57)) self.tf_cam2baselink = inverse_matrix(np.dot(tmat, qmat)) # Initialize subscriber to image stream rospy.Subscriber(sub_img_topic, Image, self.img_callback) #, queue_size = 1) rospy.sleep(0.5) # Delay briefly to allow subscribers to find messages def img_callback(self, msg): try: image = self.bridge.imgmsg_to_cv2(msg, desired_encoding="mono8") image = cv2.resize( image, (640, 480)) # Temporary until camera is calibrated properly ids, corners = self.detectMarkersInMap(image) pose, image = self.updatePoseFromMarkers(ids, corners, image) self.im_pub.publish(self.bridge.cv2_to_imgmsg( image, "passthrough")) if pose: qualcomm_pose = PoseStamped() qualcomm_pose.pose = pose qualcomm_pose.header.frame_id = "map" qualcomm_pose.header.stamp = rospy.Time.now() self.pose_pub.publish(qualcomm_pose) except CvBridgeError as e: print(e) def updatePoseFromMarkers(self, ids, corners, image): if ids: xs, ys, yaws = [], [], [] for i, idx in enumerate(ids): rvec, tvec = cv2.aruco.estimatePoseSingleMarkers( corners[i], self.marker_dict[str(idx[0])].len, self.cam_mtx, self.distort_mtx) rvec = rvec[0][0] tvec = tvec[0][0] tmat = translation_matrix((tvec[0], tvec[1], tvec[2])) qmat = np.zeros((4, 4)) qmat[:3, :3], _ = cv2.Rodrigues(rvec) qmat[3, 3] = 1. tf_cam2mark = np.dot(tmat, qmat) tf_mark2cam = inverse_matrix(tf_cam2mark) marker = self.marker_dict[str(idx[0])] tf_map2cam = np.dot(marker.tf_mat, tf_cam2mark) # trans = translation_from_matrix(tf_map2cam) rot = euler_from_quaternion(quaternion_from_matrix(tf_map2cam)) tf_map2baselink = np.dot(tf_map2cam, self.tf_cam2baselink) baselink_pose = Pose() baselink_pose.position = Vector3( *translation_from_matrix(tf_map2baselink)) baselink_pose.orientation = Quaternion( *quaternion_from_matrix(tf_map2baselink)) print("detected marker", marker.id) print("cam pose", baselink_pose) # Draw axis on marker and publish image image = cv2.cvtColor(image, cv2.COLOR_GRAY2BGR) image = cv2.aruco.drawAxis(image, self.cam_mtx, self.distort_mtx, rvec, tvec, 0.1) """ NOTE Still need to take average of poses when multiple markers are detected """ return baselink_pose, image else: return None, image def detectMarkersInMap(self, image): """ Detects all markers in camera view and returns lists of ids and corners of those that are included in the marker_map.yaml """ try: corners, ids, _ = cv2.aruco.detectMarkers( image, self.dictionary, parameters=self.parameters) map_ids = [] map_corners = [] for i, idx in enumerate(ids): if idx[0] in self.marker_set: map_ids.append(idx) map_corners.append(corners[i]) return map_ids, map_corners except: print("No aruco markers detected") return None, None @staticmethod def load_markers(path): """ Loads a yaml file containing marker definitions Returns a list of marker ids and a dictionary of corresponding Marker() objects """ marker_dict = {} marker_list = [] with open(path) as f: raw_markers = yaml.load(f) for marker in raw_markers: id_ = raw_markers[marker][0] len_ = raw_markers[marker][1] x = raw_markers[marker][2][0] y = raw_markers[marker][2][1] z = raw_markers[marker][2][2] rot_x = raw_markers[marker][3][0] rot_y = raw_markers[marker][3][1] rot_z = raw_markers[marker][3][2] marker_list.append(id_) marker_dict[str(id_)] = Marker(id_, len_, x, y, z, rot_x, rot_y, rot_z) return set(marker_list), marker_dict @staticmethod def loadCoefficients(path): """ Loads a yaml file containing camera calibration values generated by calibration/camera_calibration.py """ with open(path) as f: calibration_data = yaml.load(f) #Camera matrix and distortion cam_mtx = calibration_data.get("camera_matrix") distort_mtx = calibration_data.get("dist_coeff") cam_mtx = np.asarray(cam_mtx) distort_mtx = np.asarray(distort_mtx) return cam_mtx, distort_mtx
class LineDetectorNode(object): def __init__(self): self.node_name = "LineDetectorNode" # Thread lock self.thread_lock = threading.Lock() # Constructor of line detector self.bridge = CvBridge() self.active = True self.stats = Stats() # Only be verbose every 10 cycles self.intermittent_interval = 100 self.intermittent_counter = 0 # color correction self.ai = AntiInstagram() # these will be added if it becomes verbose self.pub_edge = None self.pub_colorSegment = None self.detector = None self.verbose = None self.updateParams(None) # Publishers self.pub_lines = rospy.Publisher("~segment_list", SegmentList, queue_size=1) self.pub_image = rospy.Publisher("~image_with_lines", Image, queue_size=1) # Subscribers self.sub_image = rospy.Subscriber("~image", CompressedImage, self.cbImage, queue_size=1) self.sub_transform = rospy.Subscriber("~transform", AntiInstagramTransform, self.cbTransform, queue_size=1) self.sub_switch = rospy.Subscriber("~switch", BoolStamped, self.cbSwitch, queue_size=1) rospy.loginfo("[%s] Initialized (verbose = %s)." % (self.node_name, self.verbose)) rospy.Timer(rospy.Duration.from_sec(2.0), self.updateParams) def updateParams(self, _event): old_verbose = self.verbose self.verbose = rospy.get_param('~verbose', True) # self.loginfo('verbose = %r' % self.verbose) if self.verbose != old_verbose: self.loginfo('Verbose is now %r' % self.verbose) self.image_size = rospy.get_param('~img_size') self.top_cutoff = rospy.get_param('~top_cutoff') if self.detector is None: c = rospy.get_param('~detector') assert isinstance(c, list) and len(c) == 2, c # if str(self.detector_config) != str(c): self.loginfo('new detector config: %s' % str(c)) self.detector = instantiate(c[0], c[1]) # self.detector_config = c if self.verbose and self.pub_edge is None: self.pub_edge = rospy.Publisher("~edge", Image, queue_size=1) self.pub_colorSegment = rospy.Publisher("~colorSegment", Image, queue_size=1) def cbSwitch(self, switch_msg): self.active = switch_msg.data def cbImage(self, image_msg): self.stats.received() if not self.active: return # Start a daemon thread to process the image thread = threading.Thread(target=self.processImage, args=(image_msg, )) thread.setDaemon(True) thread.start() # Returns rightaway def cbTransform(self, transform_msg): self.ai.shift = transform_msg.s[0:3] self.ai.scale = transform_msg.s[3:6] self.loginfo("AntiInstagram transform received") def loginfo(self, s): rospy.loginfo('[%s] %s' % (self.node_name, s)) def intermittent_log_now(self): return self.intermittent_counter % self.intermittent_interval == 1 def intermittent_log(self, s): if not self.intermittent_log_now(): return self.loginfo('%3d:%s' % (self.intermittent_counter, s)) def processImage(self, image_msg): if not self.thread_lock.acquire(False): self.stats.skipped() # Return immediately if the thread is locked return try: self.processImage_(image_msg) finally: # Release the thread lock self.thread_lock.release() def processImage_(self, image_msg): self.stats.processed() if self.intermittent_log_now(): self.intermittent_log(self.stats.info()) self.stats.reset() tk = TimeKeeper(image_msg) self.intermittent_counter += 1 # Decode from compressed image with OpenCV try: image_cv = image_cv_from_jpg(image_msg.data) except ValueError as e: self.loginfo('Could not decode image: %s' % e) return tk.completed('decoded') # Resize and crop image hei_original, wid_original = image_cv.shape[0:2] if self.image_size[0] != hei_original or self.image_size[ 1] != wid_original: # image_cv = cv2.GaussianBlur(image_cv, (5,5), 2) image_cv = cv2.resize(image_cv, (self.image_size[1], self.image_size[0]), interpolation=cv2.INTER_NEAREST) image_cv = image_cv[self.top_cutoff:, :, :] tk.completed('resized') # apply color correction: AntiInstagram image_cv_corr = self.ai.applyTransform(image_cv) image_cv_corr = cv2.convertScaleAbs(image_cv_corr) tk.completed('corrected') # Set the image to be detected self.detector.setImage(image_cv_corr) # Detect lines and normals white = self.detector.detectLines('white') yellow = self.detector.detectLines('yellow') red = self.detector.detectLines('red') tk.completed('detected') # SegmentList constructor segmentList = SegmentList() segmentList.header.stamp = image_msg.header.stamp # Convert to normalized pixel coordinates, and add segments to segmentList arr_cutoff = np.array((0, self.top_cutoff, 0, self.top_cutoff)) arr_ratio = np.array( (1. / self.image_size[1], 1. / self.image_size[0], 1. / self.image_size[1], 1. / self.image_size[0])) if len(white.lines) > 0: lines_normalized_white = ((white.lines + arr_cutoff) * arr_ratio) segmentList.segments.extend( self.toSegmentMsg(lines_normalized_white, white.normals, Segment.WHITE)) if len(yellow.lines) > 0: lines_normalized_yellow = ((yellow.lines + arr_cutoff) * arr_ratio) segmentList.segments.extend( self.toSegmentMsg(lines_normalized_yellow, yellow.normals, Segment.YELLOW)) if len(red.lines) > 0: lines_normalized_red = ((red.lines + arr_cutoff) * arr_ratio) segmentList.segments.extend( self.toSegmentMsg(lines_normalized_red, red.normals, Segment.RED)) self.intermittent_log( '# segments: white %3d yellow %3d red %3d' % (len(white.lines), len(yellow.lines), len(red.lines))) tk.completed('prepared') # Publish segmentList self.pub_lines.publish(segmentList) tk.completed('--pub_lines--') # VISUALIZATION only below if self.verbose: # Draw lines and normals image_with_lines = np.copy(image_cv_corr) drawLines(image_with_lines, white.lines, (0, 0, 0)) drawLines(image_with_lines, yellow.lines, (255, 0, 0)) drawLines(image_with_lines, red.lines, (0, 255, 0)) tk.completed('drawn') # Publish the frame with lines image_msg_out = self.bridge.cv2_to_imgmsg(image_with_lines, "bgr8") image_msg_out.header.stamp = image_msg.header.stamp self.pub_image.publish(image_msg_out) tk.completed('pub_image') # if self.verbose: colorSegment = color_segment(white.area, red.area, yellow.area) edge_msg_out = self.bridge.cv2_to_imgmsg(self.detector.edges, "mono8") colorSegment_msg_out = self.bridge.cv2_to_imgmsg( colorSegment, "bgr8") self.pub_edge.publish(edge_msg_out) self.pub_colorSegment.publish(colorSegment_msg_out) tk.completed('pub_edge/pub_segment') self.intermittent_log(tk.getall()) def onShutdown(self): self.loginfo("Shutdown.") def toSegmentMsg(self, lines, normals, color): segmentMsgList = [] for x1, y1, x2, y2, norm_x, norm_y in np.hstack((lines, normals)): segment = Segment() segment.color = color segment.pixels_normalized[0].x = x1 segment.pixels_normalized[0].y = y1 segment.pixels_normalized[1].x = x2 segment.pixels_normalized[1].y = y2 segment.normal.x = norm_x segment.normal.y = norm_y segmentMsgList.append(segment) return segmentMsgList
class DetectLine: def __init__(self): # 创建cv_bridge,声明图像的发布者和订阅者 self.image_pub = rospy.Publisher("cv_bridge_image", Image, queue_size=1) self.bridge = CvBridge() self.image_sub = rospy.Subscriber("/usb_cam/image_raw", Image, self.callback) self.rgb_image = None self.tile_pub = rospy.Publisher("/tile_uv", tileuv, queue_size=10) def callback(self, data): try: pass # video_capture=self.bridge.imgmsg_to_cv2(data,"bgr8") # self.rgb_image = video_capture.copy() # self.rgb_image = cv2.imread('/data/ros/ur_ws_yue/src/tile_robot/Impedance_control/1.jpg') except CvBridgeError as e: print e def segment_by_angle_kmeans(self, lines, k=2, **kwargs): """Groups lines based on angle with k-means. Uses k-means on the coordinates of the angle on the unit circle to segment `k` angles inside `lines`. """ # Define criteria = (type, max_iter, epsilon) default_criteria_type = cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER criteria = kwargs.get('criteria', (default_criteria_type, 10, 1.0)) flags = kwargs.get('flags', cv2.KMEANS_PP_CENTERS) attempts = kwargs.get('attempts', 10) # returns angles in [0, pi] in radians angles = np.array([line[0][1] for line in lines]) print "angles", angles # multiply the angles by two and find coordinates of that angle pts = np.array( [[np.cos(2 * angle), np.sin(2 * angle)] for angle in angles], dtype=np.float32) # run kmeans on the coords # print pts # labels1 = np.zeros(k) labels, centers = cv2.kmeans(pts, k, None, criteria, attempts, flags)[1:] print "labels", labels, centers labels = labels.reshape(-1) # transpose to row vec print "labels", labels # segment lines based on their kmeans label segmented = defaultdict(list) segmented0 = defaultdict(list) segmented1 = defaultdict(list) for i, line in zip(range(len(lines)), lines): print i, line segmented[labels[i]].append(line) print "before segmented", segmented segmented = list(segmented.values()) segmented0 = list(segmented[0]) segmented1 = list(segmented[1]) print "after segmented\n", segmented return segmented, segmented0, segmented1 def convert_hsv(self, image): return cv2.cvtColor(image, cv2.COLOR_RGB2HSV) def convert_hls(self, image): return cv2.cvtColor(image, cv2.COLOR_RGB2HLS) # image is expected be in RGB color space def select_rgb_white_yellow(self, image): # white color mask lower = np.uint8([200, 200, 200]) upper = np.uint8([255, 255, 255]) white_mask = cv2.inRange(image, lower, upper) # yellow color mask lower = np.uint8([190, 190, 0]) upper = np.uint8([255, 255, 255]) yellow_mask = cv2.inRange(image, lower, upper) # combine the mask mask = cv2.bitwise_or(white_mask, yellow_mask) masked = cv2.bitwise_and(image, image, mask=mask) return masked def convert_gray_scale(self, image): return cv2.cvtColor(image, cv2.COLOR_RGB2GRAY) def apply_smoothing(self, image, kernel_size=15): """ kernel_size must be postivie and odd """ return cv2.GaussianBlur(image, (kernel_size, kernel_size), 0) def detect_edges(self, image, low_threshold=50, high_threshold=150): return cv2.Canny(image, low_threshold, high_threshold) def select_white(self, image): converted = self.convert_hls(image) # converted = self.convert_hsv(image) # print "con-------",converted # white color mask #white lower = np.uint8([0, 200, 0]) upper = np.uint8([255, 255, 255]) # lower = np.uint8([10, 0, 100]) # upper = np.uint8([40, 255, 255]) white_mask = cv2.inRange(converted, lower, upper) #yellow color mask lower = np.uint8([0, 200, 0]) upper = np.uint8([255, 255, 255]) yellow_mask = cv2.inRange(converted, lower, upper) # combine the mask mask = cv2.bitwise_or(white_mask, yellow_mask) return cv2.bitwise_and(image, image, mask=mask) def select_yellow(self, image): converted = self.convert_hls(image) # converted = self.convert_hsv(image) lower = np.uint8([10, 0, 100]) upper = np.uint8([40, 255, 255]) white_mask = cv2.inRange(converted, lower, upper) #yellow color mask lower = np.uint8([10, 0, 100]) upper = np.uint8([40, 255, 255]) yellow_mask = cv2.inRange(converted, lower, upper) # combine the mask mask = cv2.bitwise_or(white_mask, yellow_mask) return cv2.bitwise_and(image, image, mask=mask) def select_white_yellow(self, image): converted = self.convert_hls(image) # converted = self.convert_hsv(image) # print "con-------",converted # white color mask #white lower = np.uint8([0, 200, 0]) upper = np.uint8([255, 255, 255]) white_mask = cv2.inRange(converted, lower, upper) #yellow color mask lower = np.uint8([10, 0, 100]) upper = np.uint8([40, 255, 255]) yellow_mask = cv2.inRange(converted, lower, upper) # combine the mask mask = cv2.bitwise_or(white_mask, yellow_mask) return cv2.bitwise_and(image, image, mask=mask) def filter_region(self, image, vertices): """ Create the mask using the vertices and apply it to the input image """ mask = np.zeros_like(image) if len(mask.shape) == 2: cv2.fillPoly(mask, vertices, 255) else: cv2.fillPoly(mask, vertices, (255, ) * mask.shape[2] ) # in case, the input image has a channel dimension return cv2.bitwise_and(image, mask) def select_region(self, image, bottom_left_cols1, bottom_left_rows1, top_left_cols1, top_left_rows1, bottom_right_cols1, bottom_right_rows1, top_right_cols1, top_right_rows1): """ It keeps the region surrounded by the `vertices` (i.e. polygon). Other area is set to 0 (black). bottom_left_cols1=0.53 bottom_left_rows1=0.70 top_left_cols1=0.53 top_left_rows1=0.28 bottom_right_cols1=0.95 bottom_right_rows1=0.70 top_right_cols1=0.99 top_right_rows1=0.28 """ # first, define the polygon by vertices rows, cols = image.shape[:2] bottom_left = [cols * bottom_left_cols1, rows * bottom_left_rows1] top_left = [cols * top_left_cols1, rows * top_left_rows1] bottom_right = [cols * bottom_right_cols1, rows * bottom_right_rows1] top_right = [cols * top_right_cols1, rows * top_right_rows1] # the vertices are an array of polygons (i.e array of arrays) and the data type must be integer vertices = np.array([[bottom_left, top_left, top_right, bottom_right]], dtype=np.int32) return self.filter_region(image, vertices) def hough_lines(self, image): """ `image` should be the output of a Canny transform. Returns hough lines (not the image with lines) """ return cv2.HoughLinesP(image, rho=1, theta=np.pi / 180, threshold=20, minLineLength=20, maxLineGap=300) def Draw_triangle(self, contours, rgb, tile_id, obj_desire): ################## DELAY = 0.02 USE_CAM = 1 IS_FOUND = 0 count = 0 #count feature tile numbers cnt = 0 central_list = [] uvuv = uv() tile_uv = tileuv() ################## _width = 480.0 _height = 640.0 _margin = 0.0 corners = np.array([ [[_margin, _margin]], [[_margin, _height + _margin]], [[_width + _margin, _height + _margin]], [[_width + _margin, _margin]], ]) pts_dst = np.array(corners, np.float32) for cont in contours: resultuv = [] """ #1,num,2,centeral point 3,for angular point uv ,4,clockwise direction #caculating Area for tile selected just one tile """ if cv2.contourArea(cont) > 5000 and cv2.contourArea(cont) < 60000: # print "cont----------", cont # 获取轮廓长度 arc_len = cv2.arcLength(cont, True) # 多边形拟合 approx = cv2.approxPolyDP(cont, 0.1 * arc_len, True) if (len(approx) == 4): IS_FOUND = 1 M = cv2.moments(cont) # 获取图像质心坐标 cX = int(M["m10"] / M["m00"]) cY = int(M["m01"] / M["m00"]) cv2.putText(rgb, obj_desire, (cX, cY), cv2.FONT_HERSHEY_SIMPLEX, 1.0, (0, 0, 255), 3) print "CX,CY", [cX, cY] central_list.append([cX, cY]) pts_src = np.array(approx, np.float32) # print "pts_src", pts_src cv2.circle(rgb, (cX, cY), 5, (0, 0, 0), -1) print approx.tolist() angular_point = [] for i in range(len(approx.tolist())): if i == 0: cv2.circle(rgb, (approx.tolist()[i][0][0], approx.tolist()[i][0][1]), 5, (20, 60, 220), -1) print "first point x,y,others use clockwise---", approx.tolist()[i][0][0], \ approx.tolist()[i][0][1] angular_point.append([ approx.tolist()[i][0][0], approx.tolist()[i][0][1] ]) cv2.putText(rgb, str(i), (approx.tolist()[i][0][0], approx.tolist()[i][0][1]), cv2.FONT_HERSHEY_COMPLEX_SMALL, 1, (0, 0, 0), 2) else: cv2.circle(rgb, (approx.tolist()[i][0][0], approx.tolist()[i][0][1]), 5, (0, 255, 0), -1) print "x,y", approx.tolist( )[i][0][0], approx.tolist()[i][0][1] angular_point.append([ approx.tolist()[i][0][0], approx.tolist()[i][0][1] ]) print "chr(i)", str(i) cv2.putText(rgb, str(i), (approx.tolist()[i][0][0], approx.tolist()[i][0][1]), cv2.FONT_HERSHEY_COMPLEX_SMALL, 1, (0, 0, 0), 2) resultuv.append([[tile_id], [cX, cY], angular_point]) # draw trangle in image h, status = cv2.findHomography(pts_src, pts_dst) out = cv2.warpPerspective(rgb, h, (int(_width + _margin * 2), int(_height + _margin * 2))) cv2.drawContours(rgb, [approx], -1, (0, 255, 255), 3) print "all info for tile------", resultuv print "Now tile id", tile_id tile_uv.tile_id = tile_id tile_uv.obj_desire = obj_desire tile_uv.cen_uv.uvinfo = [cX, cY] tile_uv.f1th_uv.uvinfo = angular_point[0] tile_uv.s2th_uv.uvinfo = angular_point[1] tile_uv.t3th_uv.uvinfo = angular_point[2] tile_uv.f4th_uv.uvinfo = angular_point[3] self.tile_pub.publish(tile_uv) else: pass # count += 1 # cnt += 11 return rgb.copy() def pub_empty_uv_info(self, tile_id, obj_desire): uvuv = uv() tile_uv = tileuv() tile_uv.tile_id = tile_id tile_uv.obj_desire = obj_desire tile_uv.cen_uv.uvinfo = [0, 0] tile_uv.f1th_uv.uvinfo = [0, 0] tile_uv.s2th_uv.uvinfo = [0, 0] tile_uv.t3th_uv.uvinfo = [0, 0] tile_uv.f4th_uv.uvinfo = [0, 0] self.tile_pub.publish(tile_uv) def intersection(self, line1, line2): """Finds the intersection of two lines given in Hesse normal form. Returns closest integer pixel locations. See https://stackoverflow.com/a/383527/5087436 """ rho1, theta1 = line1[0] rho2, theta2 = line2[0] A = np.array([[np.cos(theta1), np.sin(theta1)], [np.cos(theta2), np.sin(theta2)]]) b = np.array([[rho1], [rho2]]) x0, y0 = np.linalg.solve(A, b) x0, y0 = int(np.round(x0)), int(np.round(y0)) return [[x0, y0]] def draw_line(self, img, segmented, color): for ii in xrange(len(segmented)): rho, theta = np.array(segmented[ii][0]) a = np.cos(theta) b = np.sin(theta) x0 = a * rho y0 = b * rho x1 = int(x0 + 1000 * (-b)) y1 = int(y0 + 1000 * (a)) x2 = int(x0 - 1000 * (-b)) y2 = int(y0 - 1000 * (a)) cv2.line(img, (x1, y1), (x2, y2), color, 6) def segmented_intersections(self, lines): """Finds the intersections between groups of lines.""" intersections = [] for i, group in enumerate(lines[:-1]): for next_group in lines[i + 1:]: for line1 in group: for line2 in next_group: intersections.append(self.intersection(line1, line2)) return intersections def process_rgb_image(self, rgb_image): MORPH = 7 CANNY = 250 ################## rgb = rgb_image if rgb_image is not None: """' Select White Desire position tile_id=0,fixed point obj_desire="d" """ WHLS = self.select_white(rgb) Y_gray = self.convert_gray_scale(WHLS) # blur = cv2.medianBlur(Y_gray, 1) Y_smooth = self.apply_smoothing(Y_gray, 3) Y_edges = self.detect_edges(Y_smooth) # adapt_type = cv2.ADAPTIVE_THRESH_GAUSSIAN_C # thresh_type = cv2.THRESH_BINARY_INV # bin_img = cv2.adaptiveThreshold(blur, 255, adapt_type, thresh_type, 7, 2) # rho, theta, thresh = 2, np.pi / 180, 400 # lines = cv2.HoughLines(bin_img, rho, theta, thresh) lines = self.hough_lines(Y_edges) segmented, segmented0, segmented1 = self.segment_by_angle_kmeans( lines) print lines # print np.array(segmented) # intersections = self.segmented_intersections(segmented) # print intersections self.draw_line(rgb_image, segmented0, (0, 0, 255)) self.draw_line(rgb_image, segmented1, (100, 0, 255)) # for i in intersections: # cv2.circle(rgb_image, (i[0][0], i[0][1]), 5, (255, 0, 0), -1) """ HLS SPACE """ HLSDOUBLE = self.convert_hls(rgb) cv2.namedWindow('HLSDOUBLE_Space', cv2.WINDOW_NORMAL) cv2.imshow('HLSDOUBLE_Space', HLSDOUBLE) cv2.namedWindow('White_HLS_Space', cv2.WINDOW_NORMAL) cv2.imshow('White_HLS_Space', WHLS) # # cv2.namedWindow( 'White_tile_edges', cv2.WINDOW_NORMAL ) # cv2.imshow( 'White_tile_edges', W_edges ) # # cv2.namedWindow( 'Yellow_HLS_Space', cv2.WINDOW_NORMAL ) # cv2.imshow( 'Yellow_HLS_Space', YHLS ) # # cv2.namedWindow( 'Yellow_tile_edges', cv2.WINDOW_NORMAL ) # cv2.imshow( 'Yellow_tile_edges', Y_edges ) # cv2.namedWindow('tile_pixel_frame', cv2.WINDOW_NORMAL) cv2.imshow('tile_pixel_frame', rgb_image) cv2.waitKey(8) # # 再将opencv格式额数据转换成ros image格式的数据发布 try: self.image_pub.publish( self.bridge.cv2_to_imgmsg(rgb_image, "bgr8")) except CvBridgeError as e: print e # return central_list def papers_alog(self, rgb_image): self.process_rgb_image(rgb_image)
def receiver(filename=['left.yaml','right.yaml'],CLIENT_PORT=(50677,50678)): global image_size image_size=[640,320] CLIENT_IP='10.42.0.1' sockets=[] sock = socket.socket(socket.AF_INET,socket.SOCK_DGRAM) sockets.append(sock) sock = socket.socket(socket.AF_INET,socket.SOCK_DGRAM) sockets.append(sock) sockets[0].bind(('', CLIENT_PORT[0])) sockets[1].bind(('', CLIENT_PORT[1])) pic=['',''] temp=[-1,-1] # ROS stuf image_pub=[] image_pub.append(rospy.Publisher("/camera/left/image_raw",Image,queue_size=10)) image_pub.append(rospy.Publisher("/camera/right/image_raw",Image,queue_size=10)) cameraInfo_pub=[] cameraInfo_pub.append(rospy.Publisher("/camera/left/camera_info",CameraInfo,queue_size=10)) cameraInfo_pub.append(rospy.Publisher("/camera/right/camera_info",CameraInfo,queue_size=10)) rospy.init_node('camera_streamer',anonymous=True) bridge=CvBridge() camera_info=[CameraInfo()]*2 parse_yaml(filename[0],camera_info[0]) parse_yaml(filename[1],camera_info[1]) ################################################################################ while True: temp=[-1,-1] flag=[0]*2 data=['',''] addr=['',''] seq=['',''] img=['',''] res=['',''] pic=['',''] for index in xrange(2): while True: data[index], addr[index] = sockets[index].recvfrom(image_size[0]*image_size[1]) seq[index]=struct.unpack('I',data[index][0:4])[0] try: if (struct.unpack('I',data[index][4:8])[0]==9999): #print str(index)+' broke' break except: pass if seq[index]-1 > temp[index]: print str(index)+' error' flag[index]=1 temp[index]=seq[index] pic[index]=pic[index]+data[index][4:] for index in xrange(2): if seq[index] != temp[index]+1: flag[index]=1 print str(index)+ " flag error $!" try: if flag[index]==0: img[index] = cv2.imdecode(np.asarray(bytearray(pic[index]),dtype=np.uint8), 1) res[index] = cv2.resize(img[index],None,fx=1, fy=1, interpolation = cv2.INTER_CUBIC) except: print str(index)+ " flag error %@" flag[index]=1 try: if flag[0]==0 and flag[1]==0: cv2.imshow('left_frame',res[0]) cv2.imshow('right_frame',res[1]) rosTime=rospy.Time.from_sec(time.time()) camera_info[0].header.stamp=rosTime camera_info[1].header.stamp=rosTime cameraInfo_pub[0].publish(camera_info[0]) cameraInfo_pub[1].publish(camera_info[1]) image_pub[0].publish(bridge.cv2_to_imgmsg(img[0], "bgr8")) image_pub[1].publish(bridge.cv2_to_imgmsg(img[1], "bgr8")) if cv2.waitKey(1) & 0xFF == ord('q'): break; except: pass pic=['','']
class Converter(object): def __init__(self): ''' Publisher initializations ''' self.out_right = rospy.Publisher('omni_right/image_raw',Image, queue_size = 10) self.out_left = rospy.Publisher('omni_left/image_raw',Image , queue_size = 10) self.rect_right = rospy.Publisher('omni_right/rect_image' , Image , queue_size = 10) self.rect_left = rospy.Publisher('omni_left/rect_image' , Image , queue_size = 10) self.left_cam_calib_info = rospy.Publisher('omni_left_info' , Omni, queue_size = 10) self.right_cam_calib_info = rospy.Publisher('omni_right_info' , Omni, queue_size = 10) ''' Omni msg type object ''' self.msg_left = Omni() self.msg_right = Omni() self.bridge = CvBridge() ## Cvbridge object ''' Reading the calibration results ''' self.right_path = '../Results_OcamCalib/calib_results_right.txt' self.left_path = '../Results_OcamCalib/calib_results_left.txt' self.ocam_model_right = get_ocam_model(self.right_path) self.ocam_model_left = get_ocam_model(self.left_path) self.msg_left.len_pol = self.ocam_model_left['length_pol'] self.msg_left.pols = self.ocam_model_left['pols'] self.msg_left.len_invpol = self.ocam_model_left['length_invpol'] self.msg_left.invpols = self.ocam_model_left['inv_pols'] self.msg_left.cx = self.ocam_model_left['xc'] self.msg_left.cy = self.ocam_model_left['yc'] self.msg_left.height = self.ocam_model_left['height'] self.msg_left.width = self.ocam_model_left['width'] self.msg_right.len_pol = self.ocam_model_right['length_pol'] self.msg_right.pols = self.ocam_model_right['pols'] self.msg_right.len_invpol = self.ocam_model_right['length_invpol'] self.msg_right.invpols = self.ocam_model_right['inv_pols'] self.msg_right.cx = self.ocam_model_right['xc'] self.msg_right.cy = self.ocam_model_right['yc'] self.msg_right.height = self.ocam_model_right['height'] self.msg_right.width = self.ocam_model_right['width'] self.scalingFactor = 3.2 (self.mapx_right , self.mapy_right) = create_perspective_undistortion_LUT(self.ocam_model_right , self.scalingFactor) (self.mapx_left , self.mapy_left) = create_perspective_undistortion_LUT(self.ocam_model_left , self.scalingFactor) self.mapx_right = self.mapx_right.astype("float32") self.mapy_right = self.mapy_right.astype("float32") self.mapx_left = self.mapx_left.astype("float32") self.mapy_left = self.mapy_left.astype("float32") rospy.loginfo("Omni Cam Node Starting Publishing") ## logging info stating the node has started publishing self.image_sub = rospy.Subscriber('/omni_cam/image_raw',Image,self.callback) ## subscribes to topic mentioned def callback(self,data): ## callback function of image_sub time = data.header.stamp ## storing timestamp of orig image, to be used for left and right image header time stamp ''' Conversion of Image msg to cv, also rotating the image, assuming the came in upright, if not remove the rotation part ''' cv_image = self.bridge.imgmsg_to_cv2(data, "bgr8") right , left = self.separate(cv_image) ## rotation part for left image scale = 1.0 rows , cols = left.shape[:2] M1 = cv2.getRotationMatrix2D((cols/2, rows/2) , -90 , scale) cos = abs(M1[0,0]) sin = abs(M1[0,1]) nw = int((rows*sin) + (cols*cos)) nh = int((rows*cos) + (cols*sin)) M1[0,2] += (nw/2) - cols/2 M1[1,2] += (nh/2) - rows/2 left = cv2.warpAffine(left , M1 , (nw , nh)) ## rotation part for right image rows , cols = right.shape[:2] M2 = cv2.getRotationMatrix2D((cols/2 , rows/2) , 90 , scale) cos = abs(M2[0,0]) sin = abs(M2[0,1]) nw = int((rows*sin) + (cols*cos)) nh = int((rows*cos) + (cols*sin)) M2[0,2] += (nw/2) - cols/2 M2[1,2] += (nh/2) - rows/2 right = cv2.warpAffine(right , M2 , (cols , rows)) try: ''' Sending the Image msg ''' omni_left = self.bridge.cv2_to_imgmsg(left,"bgr8") omni_left.header.stamp = time omni_left.header.frame_id = "omni_left" omni_right = self.bridge.cv2_to_imgmsg(right,"bgr8") omni_right.header.stamp = omni_left.header.stamp omni_right.header.frame_id = "omni_right" self.out_right.publish(omni_right) self.out_left.publish(omni_left) except CvBridgeError as e: print e ## conversion to grayscale for remapping with mapx,mapy obtained from the undistortion method gray_right = cv2.cvtColor(right , cv2.COLOR_BGR2GRAY) gray_left = cv2.cvtColor(left , cv2.COLOR_BGR2GRAY) rect_left = cv2.remap(gray_left , self.mapx_left , self.mapy_left , cv2.INTER_LINEAR) rect_right = cv2.remap(gray_right , self.mapx_right , self.mapy_right , cv2.INTER_LINEAR) try: ''' conversion to BGR, it does not change the gray image to color, just adds the same value to all 3 channels, image view node does not accept the format if not converted to BGR. ''' color_left = cv2.cvtColor(rect_left , cv2.COLOR_GRAY2BGR) color_right = cv2.cvtColor(rect_right , cv2.COLOR_GRAY2BGR) rectified_left = self.bridge.cv2_to_imgmsg(color_left,"bgr8") rectified_left.header.stamp = time rectified_left.header.frame_id = "omni_rect_left" rectified_right = self.bridge.cv2_to_imgmsg(left,"bgr8") rectified_right.header.stamp = time rectified_right.header.frame_id = "omni_rect_right" self.rect_left.publish(rectified_left) self.rect_right.publish(rectified_right) except CvBridgeError as e: print e ''' publishing the camera info, affine parameters and co-efficients ''' self.left_cam_calib_info.publish(self.msg_left) self.right_cam_calib_info.publish(self.msg_right) ## method implements separation of images def separate(self,img): height,width = img.shape[:2] start_row , start_col = int(0) , int(0) end_row , end_col = int(height) , int(width * .5) cropped_left = img[start_row : end_row , start_col : end_col] start_row_right , start_col_right = int(0) , int(width * .5) end_row_right , end_col_right = int(height) , int(width) cropped_right = img[start_row_right : end_row_right , start_col_right : end_col_right] return cropped_right,cropped_left
class line_follower: def __init__(self): # publish to "image_topic_2" topic using message type Image self.image_pub = rospy.Publisher("image_topic_2", Image, queue_size=1) self.bridge = CvBridge() # subscribe to raw image using message type Image; do callback when there is an image self.image_sub = rospy.Subscriber("/rrbot/camera1/image_raw", Image, self.callback) self.move_pub = rospy.Publisher('/cmd_vel', Twist, queue_size=1) def callback(self, data): global lastError global lineOnRight global lineOnLeft global x global z try: # convert ROS image message into grayscale image # mono8 for grayscale, passthrough for no change cv_image = self.bridge.imgmsg_to_cv2(data, 'mono8') except CvBridgeError as e: print(e) (rows, cols) = cv_image.shape centx = int(cols / 2) centy = rows - 50 cv2.circle(cv_image, (int(cols / 2), rows - 50), 10, 255) #print(cv_image[50,50]) # show encoded image in image window cv2.imshow("Image window", cv_image) cv2.waitKey(3) #loop twice a second #rate = rospy.Rate(2) colour_arr = [] i = 0 while (i < 16): colour_arr.append(cv_image[750, i * 50]) i += 1 num = 0 denom = 0 for i in range(len(colour_arr)): num += i * 100 * colour_arr[i] denom += colour_arr[i] position = num / denom print(position) print(colour_arr) error = position - 750 kP = 0.1 kD = 0.06 speedAdjustment = kP * error + kD * (error - lastError) lastError = error baseSpeed = 1.5 onWhite = 1 # if the line is lost: for i in colour_arr: if i < 100: onWhite = 0 break if onWhite == 1: if lineOnLeft == 1: x = 0 z = 2 print("searching, left") elif lineOnRight == 1: x = 0 z = -2 print("searching, right") else: x = baseSpeed z = baseSpeed + speedAdjustment lav = int((colour_arr[0] + colour_arr[1] + colour_arr[2] + colour_arr[3] + colour_arr[4]) / 5) rav = int((colour_arr[15] + colour_arr[14] + colour_arr[13] + colour_arr[12] + colour_arr[11]) / 5) if rav < 110: print("rav") print(rav) lineOnRight = 1 lineOnLeft = 0 elif lav < 110: print("lav") print(lav) lineOnLeft = 1 lineOnRight = 0 else: lineOnRight = 0 lineOnLeft = 0 if (z > 8): z = 8 if (z < -8): z = -8 move = Twist() move.linear.x = x move.angular.z = z print(z) #print(height, width) try: self.image_pub.publish( self.bridge.cv2_to_imgmsg(cv_image, "passthrough")) self.move_pub.publish(move) except CvBridgeError as e: print(e)
class pointCloud: def __init__(self): self.bridge = CvBridge() #self.image_sub = rospy.Subscriber("/rtabmap/cloud_map",PointCloud2,self.callback) self.image_sub = rospy.Subscriber("/orb_slam2_mono/map_points",PointCloud2,self.callback) self.pose_sub = rospy.Subscriber("/orb_slam2_mono/pose",PoseStamped,self.callback_pose) self.bebop_state_sub= rospy.Subscriber('/bebop/state', String, self.bebop_state_callback) self.image_pub = rospy.Publisher("/bebop/image_map",Image, queue_size=1) self.start_pub = rospy.Publisher("/bebop/start_map",Point, queue_size=1) self.goal_pub = rospy.Publisher("/bebop/goal_map",Point, queue_size=1) self.fig = None self.z=0 self.width =1080 self.height=1080 self.pose = [] self.bebop_state = 0 self.goalx = 0 self.goaly = 0 self.goalz = 0 self.startx = 0 self.starty = 0 self.startz = 0 def rotX(self, cloud, theta): cos = np.cos(theta) sin = np.sin(theta) MatrixX = np.array([[1 ,0 ,0 , 0], [0 ,cos,-sin, 0], [0 ,-sin, cos,0], [0 ,0 ,0 ,1]]) return MatrixX.dot(cloud.T).T def rotY(self, cloud, theta): cos = np.cos(theta) sin = np.sin(theta) MatrixY = np.array([[cos ,0 ,sin ,0], [0 ,1 ,0 ,0], [-sin,0 ,cos ,0], [0 ,0 ,0 ,1]]) return MatrixY.dot(cloud.T).T def rotZ(self, cloud, theta): cos = np.cos(theta) sin = np.sin(theta) MatrixZ = np.array([[cos ,-sin,0 ,0], [sin ,cos ,0 ,0], [0 ,0 ,1 ,0], [0 ,0 ,0 ,1]]) return MatrixZ.dot(cloud.T).T def tran(self, cloud, vec): MatrixT = np.array([[1 ,0 ,0 ,vec[0]], [0 ,1 ,0 ,vec[1]], [0 ,0 ,1 ,vec[2]], [0 ,0 ,0 ,1 ]]) return MatrixT.dot(cloud.T).T def pinhole(self, cloud): d=1 width = self.width height = self.height MatrixP = np.array([[width ,0 ,width/2 ,0 ], [0 ,width ,height/2 ,0 ], [0 ,0 ,-1/d ,0 ]]) return MatrixP.dot(cloud.T).T def ortho(self, cloud): d=1 width = self.width height = self.height MatrixP = np.array([[1 ,0 ,width/2 ,0 ], [0 ,1 ,height/2 ,0 ], [0 ,0 ,0 ,1]]) return MatrixP.dot(cloud.T).T def eucDist(self, point): return np.sqrt(point[0]**2 + point[1]**2 + point[2]**2) def send_image_map(self, image): img = image.astype(np.uint8) image_message = self.bridge.cv2_to_imgmsg(img, "mono8") try: self.image_pub.publish(image_message) except rospy.ROSInterruptException as ros_e : print(ros_e) #self.rate.sleep() def send_start(self, p): #self.rate.sleep() point = Point() point.x = p[0] point.y = p[1] point.z = 0 try: self.start_pub.publish(point) except rospy.ROSInterruptException as ros_e : print(ros_e) def send_goal(self, p): #self.rate.sleep() point = Point() point.x = p[0] point.y = p[1] point.z = 0 try: self.goal_pub.publish(point) except rospy.ROSInterruptException as ros_e : print(ros_e) def bebop_state_callback(self, data): self.bebop_state = int(data.data) #print(self.goalx,self.goaly) def callback_pose(self,data): self.pose = data.pose #print(self.bebop_state) if self.bebop_state == 4: self.goalx = 1 self.goaly = self.pose.position.y self.goalz = self.pose.position.z #print(data.pose) if self.bebop_state == 5 and self.goalx == 0 and self.goaly == 0: self.goalx = 1 self.goaly = self.pose.position.y self.goalz = self.pose.position.z #print(data.pose) self.startx = 1 self.starty = self.pose.position.y self.startz = self.pose.position.z def callback(self,data): if self.goalx == 0: return cloud_points = list(pc2.read_points(data, skip_nans=True, field_names = ("x", "y", "z" ))) cloud_points = np.array(cloud_points) cloud_points = np.insert(cloud_points, 3, 1, axis = 1) save('data_points_ddr.npy', cloud_points) #cloud_points = load('data_points_ddr.npy') goal = np.array([[self.goalx ,self.goaly ,self.goalz,1]]) start = np.array([[self.startx,self.starty,self.startz,1]]) width = self.width height = self.height self.z = -1.5 theta = np.pi * (0.5) cloud_points = self.rotY(cloud_points, theta) goal = self.rotY(goal, theta) start = self.rotY(start, theta) theta = np.pi * (0.5) cloud_points = self.rotZ(cloud_points, theta) goal = self.rotZ(goal, theta) start = self.rotZ(start, theta) meanx = np.mean(cloud_points.T[0]) meany = np.mean(cloud_points.T[1]) meanz = np.mean(cloud_points.T[2]) #print(cloud_points[0]) #print(meanz) vec =[-meanx,-meany,self.z] # #vec =[0,0.0,self.z] # cloud_points = self.tran(cloud_points, vec) goal = self.tran(goal, vec) start = self.tran(start, vec) cloud_points = self.pinhole(cloud_points) goal = self.pinhole(goal) start = self.pinhole(start) #cloud_points = self.ortho(cloud_points) cloud_points = cloud_points.T goal = goal.T start = start.T #print(goal) indextoremove = [] # for i in range(len(cloud_points[2])): # print(i) # if cloud_points[2][i] < -5: # indextoremove.append(i) # if cloud_points[2][i] > 5: # indextoremove.append(i) #cloud_points = np.delete(cloud_points,indextoremove,1) xy = np.array([np.around(cloud_points[0]/cloud_points[2] + width ), np.around(cloud_points[1]/cloud_points[2] + height), cloud_points[2] ]) goal_image = np.array([np.around(goal[0]/goal[2] + width ), np.around(goal[1]/goal[2] + height), goal[2] ]) start_image = np.array([np.around(start[0]/start[2] + width ), np.around(start[1]/start[2] + height), start[2] ]) #print(goal_image.T) #print(int(start_image[0])) #xy = cloud_points indextoremove =[] # for i in range(len(xy[2])): # #print(i) # if xy[2][i] < 4: # indextoremove.append(i) # if xy[2][i] > 5: # indextoremove.append(i) #xy = np.delete(xy,indextoremove,1) uv = np.ones((height, width, 1))*100 xy = xy.T for point in xy: if point[0] < width and point[0] >= 0 and point[1] < height and point[1] > 0: v = int(point[0]) u = int(point[1]) if uv[u,v,0] > point[2]: uv[u,v,0] = point[2] # print("points draw") #print(np.mean(xy.T[2])) mean = np.mean(xy.T[2]) #print(mean - (mean * .8)) #print(mean + (mean * .05)) #print("") uv[:,:,0][uv[:,:,0] >= mean + (mean * .05)] = 0 uv[:,:,0][uv[:,:,0] < mean - (mean * .9) ] = 0 uv[:,:,0][uv[:,:,0] != 0 ] = 255 uv = np.flip(uv,0) start_image[0] = int(start_image[0]) start_image[1] = int(1080 - start_image[1]) goal_image[0] = int(goal_image[0]) goal_image[1] = int(1080 - goal_image[1]) # uv[int(start_image[0])][int(start_image[1])] = 2000 # uv[int(goal_image[0] )][int(goal_image[1] )] = 1000 kernel = cv2.getStructuringElement(cv2.MORPH_ELLIPSE,(10,10)) uv = cv2.dilate(uv,kernel,iterations = 2) kernel = cv2.getStructuringElement(cv2.MORPH_ELLIPSE,(5,5)) uv = cv2.erode(uv,kernel,iterations = 2) # print(uv[int(start_image[0])][int(start_image[1])]) # print(start_image[0], start_image[1]) self.send_goal(goal_image) self.send_start(start_image) self.send_image_map(uv) # plt.imshow(np.reshape(uv,(height,width)),origin='lower') # plt.pause(.001) # plt.cla() #print(np.array(data.data).shape) pass
def processSegments(self, segment_list_msg): if not self.active: return t_start = rospy.get_time() if self.use_propagation: self.propagateBelief() self.t_last_update = rospy.get_time() # initialize measurement likelihood measurement_likelihood = np.zeros(self.d.shape) for segment in segment_list_msg.segments: if segment.color != segment.WHITE and segment.color != segment.YELLOW: continue if segment.points[0].x < 0 or segment.points[1].x < 0: continue d_i, phi_i, l_i = self.generateVote(segment) if d_i > self.d_max or d_i < self.d_min or phi_i < self.phi_min or phi_i > self.phi_max: continue if self.use_max_segment_dist and (l_i > self.max_segment_dist): continue i = floor((d_i - self.d_min) / self.delta_d) j = floor((phi_i - self.phi_min) / self.delta_phi) if self.use_distance_weighting: dist_weight = self.dwa * l_i**3 + self.dwb * l_i**2 + self.dwc * l_i + self.zero_val if dist_weight < 0: continue measurement_likelihood[ i, j] = measurement_likelihood[i, j] + dist_weight else: measurement_likelihood[ i, j] = measurement_likelihood[i, j] + 1 / (l_i) if np.linalg.norm(measurement_likelihood) == 0: return measurement_likelihood = measurement_likelihood / np.sum( measurement_likelihood) if self.use_propagation: self.updateBelief(measurement_likelihood) else: self.beliefRV = measurement_likelihood # TODO entropy test: #print self.beliefRV.argmax() maxids = np.unravel_index(self.beliefRV.argmax(), self.beliefRV.shape) # rospy.loginfo('maxids: %s' % maxids) self.lanePose.header.stamp = segment_list_msg.header.stamp self.lanePose.d = self.d_min + maxids[0] * self.delta_d self.lanePose.phi = self.phi_min + maxids[1] * self.delta_phi self.lanePose.status = self.lanePose.NORMAL # publish the belief image bridge = CvBridge() belief_img = bridge.cv2_to_imgmsg( (255 * self.beliefRV).astype('uint8'), "mono8") belief_img.header.stamp = segment_list_msg.header.stamp max_val = self.beliefRV.max() self.lanePose.in_lane = max_val > self.min_max and len( segment_list_msg.segments) > self.min_segs and np.linalg.norm( measurement_likelihood) != 0 self.pub_lane_pose.publish(self.lanePose) self.pub_belief_img.publish(belief_img) # print "time to process segments:" # print rospy.get_time() - t_start # Publish in_lane according to the ent in_lane_msg = BoolStamped() in_lane_msg.header.stamp = segment_list_msg.header.stamp in_lane_msg.data = self.lanePose.in_lane # ent = entropy(self.beliefRV) # if (ent < self.max_entropy): # in_lane_msg.data = True # else: # in_lane_msg.data = False self.pub_in_lane.publish(in_lane_msg)
class DetectLane(): def __init__(self): self.sub_image_original = rospy.Subscriber('/camera/image', Image, self.cbFindLane, queue_size=1) self.pub_image_detect = rospy.Publisher('/detect/lane', Image, queue_size=1) self.pub_center_white_lane = rospy.Publisher('/control/white_lane', Float64, queue_size=1) self.pub_center_yellow_lane = rospy.Publisher('/control/yellow_lane', Float64, queue_size=1) self.cvBridge = CvBridge() self.counter = 1 self.hue_white_l = 0 self.hue_white_h = 179 self.saturation_white_l = 0 self.saturation_white_h = 30 self.lightness_white_l = 221 self.lightness_white_h = 255 self.hue_yellow_l = 26 self.hue_yellow_h = 34 self.saturation_yellow_l = 43 self.saturation_yellow_h = 255 self.lightness_yellow_l = 46 self.lightness_yellow_h = 255 def cbFindLane(self, image_msg): # Change the frame rate by yourself. Now, it is set to 1/3 (10fps). # Unappropriate value of frame rate may cause huge delay on entire recognition process. # This is up to your computer's operating power. if self.counter % 3 != 0: self.counter += 1 return else: self.counter = 1 cv_image = self.cvBridge.imgmsg_to_cv2(image_msg, "bgr8") # find White and Yellow Lanes self.maskLane(cv_image) # yellow_fraction, cv_yellow_lane = self.maskYellowLane(cv_image) def maskLane(self, image): # convert image to hsv hsv = cv2.cvtColor(image, cv2.COLOR_BGR2HSV) # param of irange hsv(white & yellow) Hue_white_h = self.hue_white_l Hue_white_l = self.hue_white_h Saturation_white_h = self.saturation_white_l Saturation_white_l = self.saturation_white_h Lightness_white_h = self.lightness_white_h Lightness_white_l = self.lightness_white_l Hue_yellow_h = self.hue_yellow_l Hue_yellow_l = self.hue_yellow_h Saturation_yellow_h = self.saturation_yellow_l Saturation_yellow_l = self.saturation_yellow_h Lightness_yellow_h = self.lightness_yellow_h Lightness_yellow_l = self.lightness_yellow_l # define range of white color in HSV lower_white = np.array( [Hue_white_h, Saturation_white_h, Lightness_white_l]) upper_white = np.array( [Hue_white_l, Saturation_white_l, Lightness_white_h]) lower_yellow = np.array( [Hue_yellow_h, Saturation_yellow_h, Lightness_yellow_l]) upper_yellow = np.array( [Hue_yellow_l, Saturation_yellow_l, Lightness_yellow_h]) # Threshold the HSV image to get only white colors mask_white = cv2.inRange(hsv, lower_white, upper_white) mask_yellow = cv2.inRange(hsv, lower_yellow, upper_yellow) kernel = np.ones((5, 5)) erosion_white = cv2.erode(mask_white, kernel) erosion_yellow = cv2.erode(mask_yellow, kernel) Gaussian_white = cv2.GaussianBlur(erosion_white, (5, 5), 0) Gaussian_yellow = cv2.GaussianBlur(erosion_yellow, (5, 5), 0) # findContours of image contours_white, hierarchy_white = cv2.findContours( Gaussian_white, cv2.RETR_TREE, cv2.CHAIN_APPROX_NONE) contours_yellow, hierarchy_yellow = cv2.findContours( Gaussian_yellow, cv2.RETR_TREE, cv2.CHAIN_APPROX_NONE) # print contours # print(contours_white) # print(contours_yellow) # draw the contours in origin image cv2.drawContours(image, contours_white, -1, (139, 104, 0), 3) cv2.drawContours(image, contours_yellow, -1, (139, 104, 0), 3) # center of white and yellow lane # white_center = Center() # yellow_center = Center() # try: white_center = self.calculate_average(contours_white[0]) print("white: ", white_x, white_y) is_detect_white = 1 # except: # is_detect_white = 0 # print(contours_white[0]) # print("The Camera Can`t Catch The White Lane.") # try: yellow_center = self.calculate_average(contours_yellow[0]) print("yellow: ", yellow_x, yellow_y) is_detect_yellow = 1 # except: # is_detect_yellow = 0 # print(contours_yellow[0]) # print("The Camera Can`t Catch The Yellow Lane.") # print(contours_white[0]) # yellow_x, yellow_y = self.calculate_average(contours_yellow[0]) # Publish Image self.pub_image_detect.publish( self.cvBridge.cv2_to_imgmsg(image, 'bgr8')) # Publish Center self.pub_center_white_lane.publish(white_center) self.pub_center_yellow_lane.publish(yellow_center) def calculate_average(self, input): sum_x = 0 for i in input: sum_x += i[0][0] return sum_x / len(input) def main(self): rospy.spin()
cap = cv2.VideoCapture(0) rospy.init_node('lepton_driver', anonymous=True) image_pub = rospy.Publisher("/flir_lepton/image", Image, queue_size=1) while (True and not rospy.is_shutdown()): time.sleep(0.1) # Capture frame-by-frame ret, frame = cap.read() frame = frame[:, :, 0] frame.astype('int') gray = cv2.cvtColor(frame, cv2.COLOR_GRAY2BGR) print(gray.shape) image_message = bridge.cv2_to_imgmsg(frame, encoding="passthrough") image_pub.publish(image_message) # Our operations on the frame come here # Display the resulting frame # cv2.imshow('frame',frame) # if cv2.waitKey(1) & 0xFF == ord('q'): # break # When everything done, release the capture cap.release() cv2.destroyAllWindows()
class SensorStreaming(sensor_streaming_pb2_grpc.SensorStreamingServicer): def __init__(self, camera_pubs, lidar_pub, radar_pub, clock_pub): print("creating") self.bridge = CvBridge() self.camera_pubs = camera_pubs self.lidar_pub = lidar_pub self.radar_pub = radar_pub self.clock_pub = clock_pub def StreamCameraSensor(self, request, context): """ Takes in a gRPC SensorStreamingRequest containing all the data needed to create and publish a sensor_msgs/Image ROS message. """ img_string = request.data cv_image = np.fromstring(img_string, np.uint8) # NOTE, the height is specifiec as a parameter before the width cv_image = cv_image.reshape(request.height, request.width, 3) cv_image = cv2.flip(cv_image, 0) bgr_image = cv2.cvtColor(cv_image, cv2.COLOR_RGB2BGR) msg = Image() header = std_msgs.msg.Header() try: # RGB # msg = self.bridge.cv2_to_imgmsg(cv_image, 'rgb8') # BGR msg = self.bridge.cv2_to_imgmsg(bgr_image, 'bgr8') header.stamp = rospy.Time.from_sec(request.timeStamp) msg.header = header except CvBridgeError as e: print(e) camera_pubs[request.frame_id].publish(msg) return sensor_streaming_pb2.CameraStreamingResponse(success=True) def StreamLidarSensor(self, request, context): """ Takes in a gRPC LidarStreamingRequest containing all the data needed to create and publish a PointCloud2 ROS message. """ pointcloud_msg = PointCloud2() header = std_msgs.msg.Header() header.stamp = rospy.Time.from_sec(request.timeInSeconds) header.frame_id = "velodyne" pointcloud_msg.header = header pointcloud_msg.height = request.height pointcloud_msg.width = request.width fields = request.fields # Set PointCloud[] fields in pointcloud_msg for i in range(len(fields)): pointcloud_msg.fields.append(PointField()) pointcloud_msg.fields[i].name = fields[i].name pointcloud_msg.fields[i].offset = fields[i].offset pointcloud_msg.fields[i].datatype = fields[i].datatype pointcloud_msg.fields[i].count = fields[i].count pointcloud_msg.is_bigendian = request.isBigEndian pointcloud_msg.point_step = request.point_step pointcloud_msg.row_step = request.row_step pointcloud_msg.data = request.data pointcloud_msg.is_dense = request.is_dense self.lidar_pub.publish(pointcloud_msg) # TODO: This does not belong in this RPC implementation, should be # moved to own or something like that. sim_clock = Clock() sim_clock.clock = rospy.Time.from_sec(request.timeInSeconds) self.clock_pub.publish(sim_clock) return sensor_streaming_pb2.LidarStreamingResponse(success=True) def StreamRadarSensor(self, request, context): """ Takes in a gRPC RadarStreamingRequest containing all the data needed to create and publish a RadarSpoke ROS message. """ number_of_spokes = request.numSpokes for i in range(number_of_spokes): radar_spoke_msg = RadarSpoke() # Header header = std_msgs.msg.Header() header.frame_id = "milliampere_radar" header.stamp = rospy.Time.from_sec(request.timeInSeconds[i]) radar_spoke_msg.azimuth = request.azimuth[i] radar_spoke_msg.intensity = request.radarSpokes[i * request.numSamples : i * request.numSamples + request.numSamples] radar_spoke_msg.range_start = request.rangeStart radar_spoke_msg.range_increment = request.rangeIncrement radar_spoke_msg.min_intensity = request.minIntensity radar_spoke_msg.max_intensity = request.maxIntensity radar_spoke_msg.num_samples = request.numSamples self.radar_pub.publish(radar_spoke_msg) return sensor_streaming_pb2.RadarStreamingResponse(success=True)
print("No more frames") break _, leftFrame = left.retrieve() leftFrame = cv2.resize(leftFrame, None,fx=0.5, fy=0.66, interpolation = cv2.INTER_AREA) # if not (right.grab()): # print("No more frames") # break # _, rightFrame = right.retrieve() # rightFrame = cv2.resize(rightFrame, None,fx=0.5, fy=0.66, interpolation = cv2.INTER_AREA) print("Got Image ", frameId) if frameId % 5 == -1: cv2.imshow('left', leftFrame) # cv2.imshow('right', rightFrame) left_image_pub.publish(BRIDGE.cv2_to_imgmsg(leftFrame, "bgr8")) # right_image_pub.publish(BRIDGE.cv2_to_imgmsg(rightFrame, "bgr8")) if cv2.waitKey(15) & 0xFF == ord('q'): break frameId += 1 left.release() # right.release() cv2.destroyAllWindows()
class Bridge(object): def __init__(self, conf, server): rospy.init_node('styx_server') self.server = server self.vel = 0. self.yaw = None self.angular_vel = 0. self.bridge = CvBridge() self.callbacks = { '/vehicle/steering_cmd': self.callback_steering, '/vehicle/throttle_cmd': self.callback_throttle, '/vehicle/brake_cmd': self.callback_brake, } self.subscribers = [ rospy.Subscriber(e.topic, TYPE[e.type], self.callbacks[e.topic]) for e in conf.subscribers ] self.publishers = { e.name: rospy.Publisher(e.topic, TYPE[e.type], queue_size=1) for e in conf.publishers } def create_light(self, x, y, z, yaw, state): light = TrafficLight() light.header = Header() light.header.stamp = rospy.Time.now() light.header.frame_id = '/world' light.pose = self.create_pose(x, y, z, yaw) light.state = state return light def create_pose(self, x, y, z, yaw=0.): pose = PoseStamped() pose.header = Header() pose.header.stamp = rospy.Time.now() pose.header.frame_id = '/world' pose.pose.position.x = x pose.pose.position.y = y pose.pose.position.z = z q = tf.transformations.quaternion_from_euler(0., 0., math.pi * yaw / 180.) pose.pose.orientation = Quaternion(*q) return pose def create_float(self, val): fl = Float() fl.data = val return fl def create_twist(self, velocity, angular): tw = TwistStamped() tw.twist.linear.x = velocity tw.twist.angular.z = angular return tw def create_steer(self, val): st = SteeringReport() st.steering_wheel_angle_cmd = val * math.pi / 180. st.enabled = True st.speed = self.vel return st def calc_angular(self, yaw): angular_vel = 0. if self.yaw is not None: angular_vel = (yaw - self.yaw) / (rospy.get_time() - self.prev_time) self.yaw = yaw self.prev_time = rospy.get_time() return angular_vel def create_point_cloud_message(self, pts): header = Header() header.stamp = rospy.Time.now() header.frame_id = '/world' cloud_message = pcl2.create_cloud_xyz32(header, pts) return cloud_message def broadcast_transform(self, name, position, orientation): br = tf.TransformBroadcaster() br.sendTransform(position, orientation, rospy.Time.now(), name, "world") def publish_odometry(self, data): pose = self.create_pose(data['x'], data['y'], data['z'], data['yaw']) position = (data['x'], data['y'], data['z']) orientation = tf.transformations.quaternion_from_euler( 0, 0, math.pi * data['yaw'] / 180.) self.broadcast_transform("base_link", position, orientation) self.publishers['current_pose'].publish(pose) self.vel = data['velocity'] * 0.44704 self.angular = self.calc_angular(data['yaw'] * math.pi / 180.) self.publishers['current_velocity'].publish( self.create_twist(self.vel, self.angular)) def publish_controls(self, data): steering, throttle, brake = data['steering_angle'], data[ 'throttle'], data['brake'] self.publishers['steering_report'].publish(self.create_steer(steering)) self.publishers['throttle_report'].publish(self.create_float(throttle)) self.publishers['brake_report'].publish(self.create_float(brake)) def publish_obstacles(self, data): for obs in data['obstacles']: pose = self.create_pose(obs[0], obs[1], obs[2]) self.publishers['obstacle'].publish(pose) header = Header() header.stamp = rospy.Time.now() header.frame_id = '/world' cloud = pcl2.create_cloud_xyz32(header, data['obstacles']) self.publishers['obstacle_points'].publish(cloud) def publish_lidar(self, data): self.publishers['lidar'].publish( self.create_point_cloud_message( zip(data['lidar_x'], data['lidar_y'], data['lidar_z']))) def publish_traffic(self, data): x, y, z = data['light_pos_x'], data['light_pos_y'], data[ 'light_pos_z'], yaw = [ math.atan2(dy, dx) for dx, dy in zip(data['light_pos_dx'], data['light_pos_dy']) ] status = data['light_state'] lights = TrafficLightArray() header = Header() header.stamp = rospy.Time.now() header.frame_id = '/world' lights.lights = [ self.create_light(*e) for e in zip(x, y, z, yaw, status) ] self.publishers['trafficlights'].publish(lights) def publish_dbw_status(self, data): self.publishers['dbw_status'].publish(Bool(data)) def publish_camera(self, data): imgString = data["image"] image = PIL_Image.open(BytesIO(base64.b64decode(imgString))) image_array = np.asarray(image) image_message = self.bridge.cv2_to_imgmsg(image_array, encoding="rgb8") self.publishers['image'].publish(image_message) def callback_steering(self, data): self.server( 'steer', data={'steering_angle': str(data.steering_wheel_angle_cmd)}) def callback_throttle(self, data): #rospy.loginfo("Pedal command: " + str(data.pedal_cmd)) self.server('throttle', data={'throttle': str(data.pedal_cmd)}) def callback_brake(self, data): #rospy.loginfo("Break command: " + str(data.pedal_cmd)) self.server('brake', data={'brake': str(data.pedal_cmd)})
class image_processor: """ This class takes image messages from the USB Camera and converts them to a cv2 format subsequently it converts the image to grayscale and performs a perspective and hanning transform. Finally, it outputs a delta value indicating the offset of the vehicle from the center of the lane """ def __init__(self): global display_image, publish_image, calibrate_transform global calibration_pts #Create ROS Interfaces self.offset_lane_pub = rospy.Publisher("lane_offset", Float32, queue_size=10) self.cvfail_pub = rospy.Publisher("cv_abort", Int32, queue_size=10) self.image_pub = rospy.Publisher("cv_image", Image, queue_size=10) self.bridge = CvBridge() self.image_sub = rospy.Subscriber("/barc_cam/image_raw", Image, self.callback_image) #Get Launch File Parameters and configure node calibrate_transform = rospy.get_param( "/image_processing/calibrate_transform") display_image = rospy.get_param("/image_processing/display_image") publish_image = rospy.get_param("/image_processing/publish_image") global image_calibrated # Projection Transform Parameters self.x_offset = 100 self.x_width = 400 self.y_height = 400 if calibrate_transform: image_calibrated = False cv2.namedWindow("Calibrate Image Transform") cv2.setMouseCallback("Calibrate Image Transform", print_point_callback) else: image_calibrated = True calibration_pts = np.float32( ([rospy.get_param("/image_processing/upperLeftX"), rospy.get_param("/image_processing/upperLeftY")], \ [rospy.get_param("/image_processing/upperRightX"), rospy.get_param("/image_processing/upperRightY")], \ [rospy.get_param("/image_processing/lowerRightX"), rospy.get_param("/image_processing/lowerRightY")], \ [rospy.get_param("/image_processing/lowerLeftX"), rospy.get_param("/image_processing/lowerLeftY")])) # Apply Projection Transform # ref points [TOPLEFT, TOPRIGHT, BOTTOMRIGHT, BOTTOMLEFT] ref_pts = np.float32([[self.x_offset,0], \ [self.x_width + self.x_offset,0], \ [self.x_width + self.x_offset, self.y_height], \ [self.x_offset, self.y_height]]) self.projection_dim = (self.x_width + self.x_offset * 2, self.y_height) self.projection_transform = cv2.getPerspectiveTransform( calibration_pts, ref_pts) self.minimumContourSize = 10 self.image_threshold = 20 self.y_offset_pixels_cg = 70 self.num_nonvalid_img = 0 self.num_failed_img_before_abort = 30 self.half_vehicle_width_pixels = (260 // 16) * 6 def callback_image(self, data): """ Callback for incoming image message """ # Global Variables global display_image, publish_image, image_calibrated # Convert ROS Image Message to CV2 image try: cv_image = self.bridge.imgmsg_to_cv2(data, "bgr8") except CvBridgeError as e: print(e) (rows, cols, channels) = cv_image.shape # Convert Color Image to Grayscale gray_image = cv2.cvtColor(cv_image, cv2.COLOR_BGR2GRAY) if image_calibrated: kernel_size = 7 #blur_gray = cv2.GaussianBlur(gray_image, (kernel_size, kernel_size), 0) #cv2.imshow("blurred Image", blur_gray) # Perform Line Detection #edges = cv2.Canny(gray_image,1,15) #in MPC lab, 13 15 100 edges = cv2.Canny(gray_image, 15, 300) #cv2.imshow("Normal Image", cv_image) # imshape = edges0.shape # vertices = np.array([[(100,imshape[0]), (440, 325), (550, 325), (imshape[1],imshape[0])]], dtype=np.int32) # edges = region_of_interest(edges0, vertices) #cv2.imshow("After Canny", edges) # rho = 2 # theta = np.pi/360 # threshold = 20#210 # min_line_length = 250 # max_line = 200 # # lines = cv2.HoughLinesP(edges, rho, theta , threshold, np.array([]), minLineLength = min_line_length , maxLineGap = max_line ) line_img = np.zeros(cv_image.shape, dtype=np.uint8) offset = draw_lines(line_img, edges) pub = rospy.Publisher('chatter', String, queue_size=10) rate = rospy.Rate(100) # 10hz rospy.loginfo(str(offset)) pub.publish(str(offset)) rate.sleep() alpha = .6 beta = 1. gamma = 0. LinesDrawn2 = cv2.addWeighted(cv_image, alpha, line_img, beta, gamma) cv2.rectangle(LinesDrawn2, (400, 100), (640, 0), (255, 255, 255), thickness=-1, lineType=8, shift=0) fontFace = cv2.FONT_HERSHEY_SCRIPT_SIMPLEX cv2.putText(LinesDrawn2, "information box", (465, 20), fontFace, .5, (0, 0, 0)) cv2.putText(LinesDrawn2, "steering angle: " + str(offset) + " deg", (440, 50), fontFace, .5, (0, 0, 0)) #cv2.putText(LinesDrawn2, "developed by Parsa/MPC Lab", (410, 80), fontFace, .5,(0,0,0)) # Find Contours #contours, heirarchy = cv2.findContours(edges, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE) #contours = [contour for contour in contours if (cv2.contourArea(contour) < self.minimumContourSize)] #cv2.drawContours(edges, contours, -1, color = 0) #cv2.imshow("After Canny", edges) # # Remove small contours (noise) in edge detected image #(rows2,cols2,channels2) = LinesDrawn.shape #M = cv2.getRotationMatrix2D((cols2/2,rows2/2),90,1) #LinesDrawn2 = cv2.warpAffine(LinesDrawn,M,(cols2,rows2)) #vertices = vertices.reshape((-1,1,2)) #cv2.polylines(LinesDrawn2, [vertices], True, (0, 255, 255)) # # # # # # # # # Apply Projection Transform # edges = cv2.warpPerspective(edges, self.projection_transform, self.projection_dim) # # # Apply Thresholding # #ret, edges = cv2.threshold(edges, self.image_threshold, 255, cv2.THRESH_BINARY) # # # Convert to RGB to visualize lane detection points # if display_image or publish_image: # backtorgb = cv2.cvtColor(edges,cv2.COLOR_GRAY2RGB) # # ## Lane Detection # height, width = edges.shape # # # We assume that our vehicle is placed in the center of the lane initially # offset = 0 # # y_base = 200 #20 # this represents 3" in front of the car # # # #for i in range(height // y_increment): # index_y = height - y_base # index_x = (width)//2 # if index_x >= width: # index_x = width - 1 # if index_x < 0: # index_x = 0 # x_left, x_right = find_offset_in_lane(edges, index_x, index_y, width) # # midpoint = (x_right + x_left)//2 # offset = midpoint - width//2 # # ~~~~ FILTERING ~~~~ # # perform median filter to remove extremities if display_image or publish_image: # cv2.circle(backtorgb, (x_right, index_y), 3, (0,255,255), -1) # cv2.circle(backtorgb, (x_left, index_y), 3, (0,255,255), -1) # cv2.circle(backtorgb, (midpoint, index_y), 3, (0,255,0),-1) # cv2.circle(backtorgb, (width//2, index_y-5), 3, (0,0,255),-1) # # fontFace = cv2.FONT_HERSHEY_SCRIPT_SIMPLEX # cv2.putText(LinesDrawn2, str(offset), (180, 255), fontFace, .5,(0,0,255)) # if offset > 0: # cv2.arrowedLine(LinesDrawn2, (220, 250), (270, 250), (255,0,0), 2,8, 0,0.5) # elif offset < 0: # cv2.arrowedLine(LinesDrawn2, (170, 250), (120, 250), (255,0,0), 2,8, 0,0.5) # else: # cv2.arrowedLine(LinesDrawn2, (180, 230), (180, 180), (255,0,0), 2,8, 0,0.5) # # # # ## Make Steering Calculations # angle_adjacent = 20; #experimentally determined ## Publish vehicle steering directions self.offset_lane_pub.publish(Float32(offset)) if display_image: #cv2.imshow("Final", backtorgb) cv2.imshow("Advanced Lane Detection", LinesDrawn2) if publish_image: try: self.image_pub.publish( self.bridge.cv2_to_imgmsg(LinesDrawn2, "bgr8")) except CvBridgeError as e: print(e) else: if display_image: cv2.imshow("Calibrate Image Transform", gray_image) if publish_image: try: self.image_pub.publish( self.bridge.cv2_to_imgmsg(gray_image, "bgr8")) except CvBridgeError as e: print(e) if display_image: cv2.waitKey(3)
class viewer: def __init__(self, classifier): self.package_directory = subprocess.check_output( ["rospack", "find", "drone_cam"]).decode().strip('\n') self.filename = self.package_directory + '/src/office_drone_b.mp4' self.cap = cv2.VideoCapture(self.filename) self.frame_count = self.cap.get(cv2.CAP_PROP_FRAME_COUNT) self.frames = 0 self.bridge = CvBridge() self.frame_copy = [] self.topic = '/drone_b/camera/image' # self.pub = rospy.Publisher(self.topic, drone_feed, queue_size=10) self.pubimg = rospy.Publisher(self.topic + '_Image', Image, queue_size=10) self.rate = rospy.Rate(30) # self.img = drone_feed() self.imgmsg = Image() self.classifiers = { 'haarcascades': cv2.CascadeClassifier( '/home/lv/openCV/opencv/data/haarcascades/haarcascade_frontalface_alt.xml' ), 'lbpcascades': cv2.CascadeClassifier( '/home/lv/openCV/opencv/data/lbpcascades/lbpcascade_frontalface.xml' ) } self.classifier = self.classifiers[classifier] def detectFaces(self, frame): self.frame_copy = frame.copy() grayscale = cv2.cvtColor(self.frame_copy, cv2.COLOR_BGR2GRAY) grayscale = cv2.equalizeHist(grayscale) faces = self.classifier.detectMultiScale(grayscale, scaleFactor=1.2, minNeighbors=5) for (x, y, w, h) in faces: cv2.rectangle(self.frame_copy, (x, y), (x + w, y + h), (0, 255, 0), 2) def pubcam(self): while not rospy.is_shutdown(): try: self.frames += 1 if self.frames == self.frame_count: self.cap = cv2.VideoCapture(self.filename) self.frames = 0 ret, frame = self.cap.read() self.detectFaces(frame) self.imgmsg = self.bridge.cv2_to_imgmsg( self.frame_copy, "bgr8") # self.img.name = self.topic # self.pub.publish(self.img) # self.imgmsg = self.img.image self.pubimg.publish(self.imgmsg) self.rate.sleep() except CvBridgeError as e: rospy.loginfo(e)
class DetectBucket(object): x0, y0, z0 = 0, 0, 0 roll0, pitch0, yaw0 = 0, 0, 0 odom_received = False #make birdeye heatmap with size 50, 25, ppm=2, init_pos=0.7, 25 birdeye_heatmap = np.zeros((50, 100), dtype=np.uint8) init_pos = 0.7, 25 ppm = 2 heatmaps = np.zeros((2, 50, 100), dtype=np.uint8) def __init__(self, nodename, drive=None): rospy.init_node(nodename, anonymous=False) self.bridge = CvBridge() self.init_markers() #sub to front cam # rospy.Subscriber("/logi_c920/usb_cam_node/image_raw", Image, self.front_img_callback, queue_size = 1) rospy.Subscriber("/front/image_rect_color", Image, self.front_img_callback, queue_size=1) #sub to downward cam # rospy.Subscriber("/logi_c310/usb_cam_node/image_raw", Image, self.down_img_callback, queue_size = 1) rospy.Subscriber("/down/image_rect_color", Image, self.down_img_callback, queue_size=1) #sub to odom rospy.Subscriber('/visual_odom', Odometry, self.odom_callback, queue_size=1) #publish detection imgs self.img_pub = rospy.Publisher('/bucket_img', Image, queue_size=1) self.down_img_pub = rospy.Publisher('/down_bucket_img', Image, queue_size=1) self.birdeye_heatmap_pub = rospy.Publisher('/birdeye_bucket', Image, queue_size=1) rate = rospy.Rate(10) while not rospy.is_shutdown(): rate.sleep() def down_img_callback(self, msg): font = cv2.FONT_HERSHEY_SIMPLEX color = (0, 0, 255) fov_w, fov_h = 48 * math.pi / 180, 36 * math.pi / 180 px_W, px_H = 640, 480 img = self.bridge.imgmsg_to_cv2(msg, "bgr8") img = self.img_correction(img) h, w = img.shape[:2] output = np.zeros_like(img) blur = cv2.GaussianBlur(img, (7, 7), 0) #red, then blue boundaries = [ ([50, 50, 100], [155, 155, 255], [0, 0, 255]), ([150, 50, 0], [255, 150, 100], [255, 0, 0]) # ([25, 146, 190], [62, 174, 250]), # ([103, 86, 65], [145, 133, 128]) ] combined_mask = np.zeros((h, w), dtype=np.uint8) i = 0 for (lower, upper, color) in boundaries: # create NumPy arrays from the boundaries lower = np.array(lower, dtype="uint8") upper = np.array(upper, dtype="uint8") # find the colors within the specified boundaries and apply # the mask mask = cv2.inRange(img, lower, upper) # minLineLength=300 # lines = cv2.HoughLinesP(image=mask,rho=1,theta=np.pi/180,\ # threshold=50,lines=np.array([]), minLineLength=minLineLength, maxLineGap=12) # if lines is not None: # for line in lines: # #find two major gradients # x1, y1, x2, y2=line[0][0], line[0][1], line[0][2], line[0][3] # cv2.line(mask, (x1, y1), (x2, y2), 0, 5, cv2.LINE_AA) kernel = np.ones((5, 5), np.uint8) contour_mask = cv2.morphologyEx(mask, cv2.MORPH_OPEN, kernel) # opening=mask # opening = cv2.erode(mask, None, iterations=4) opening = cv2.dilate(mask, None, iterations=2) # Hough circle function parameters (_, contours, _) = cv2.findContours(contour_mask, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_NONE) contour_mask = cv2.cvtColor(contour_mask, cv2.COLOR_GRAY2BGR) areas = [] r = [] for contour in contours: rect = cv2.boundingRect(contour) area = rect[2] * rect[3] ar = float(rect[3]) / rect[2] # print(ar) px_count = np.sum(opening[rect[1]:rect[1] + rect[3], rect[0]:rect[0] + rect[2]]) / 255 # print(px_count/area) if ar < 1.5 and ar > 0.6 and area > 10000 and px_count / area > 0.5: cv2.rectangle(img, (rect[0], rect[1]), (rect[2] + rect[0], rect[3] + rect[1]), color, 2) del_px = w / 2 - (rect[0] + rect[2] / 2) del_py = h / 2 - (rect[1] + rect[3] / 2) thetax = fov_w * del_px / (w / 2) thetay = fov_h * del_py / (h / 2) del_x = self.z0 * math.tan(thetax) del_y = self.z0 * math.tan(thetay) x = self.x0 - del_y y = self.y0 - del_x ind_x = int(self.birdeye_heatmap.shape[0] - (self.init_pos[0] + x) * self.ppm) ind_y = int((self.init_pos[1] - y) * self.ppm) ind_x = np.clip(ind_x, 0, self.birdeye_heatmap.shape[1]) ind_y = np.clip(ind_y, 0, self.birdeye_heatmap.shape[0]) self.heatmaps[i, ind_x, ind_y] += 1 combined_mask = cv2.bitwise_or(combined_mask, mask) i += 1 combined_mask = cv2.cvtColor(combined_mask, cv2.COLOR_GRAY2BGR) self.down_img_pub.publish( self.bridge.cv2_to_imgmsg(np.hstack([img, combined_mask]), "bgr8")) def front_img_callback(self, msg): font = cv2.FONT_HERSHEY_SIMPLEX color = (0, 0, 255) start_time = time.time() img = self.bridge.imgmsg_to_cv2(msg, "bgr8") img = self.img_correction(img) h, w = img.shape[:2] output = np.zeros_like(img) blur = cv2.GaussianBlur(img, (7, 7), 0) #red, then blue boundaries = [ ([0, 0, 0], [125, 105, 255], [0, 0, 255]), ([0, 31, 4], [255, 128, 50], [255, 0, 0]) # ([25, 146, 190], [62, 174, 250]), # ([103, 86, 65], [145, 133, 128]) ] combined_mask = np.zeros((h, w), dtype=np.uint8) i = 0 for (lower, upper, color) in boundaries: # create NumPy arrays from the boundaries lower = np.array(lower, dtype="uint8") upper = np.array(upper, dtype="uint8") # find the colors within the specified boundaries and apply # the mask mask = cv2.inRange(img, lower, upper) kernel = np.ones((5, 5), np.uint8) # opening = cv2.morphologyEx(mask, cv2.MORPH_OPEN, kernel) # opening=mask opening = cv2.erode(mask, None, iterations=4) # opening = cv2.dilate(mask, None, iterations=2) (_, contours, _) = cv2.findContours(opening, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_NONE) for contour in contours: #if too low not valid if self.z0 < 0.7: break rect = cv2.boundingRect(contour) area = int(rect[3] * rect[2]) ar = float(rect[3]) / rect[2] # print(ar) px_count = np.sum(opening[rect[1]:rect[1] + rect[3], rect[0]:rect[0] + rect[2]]) / 255 # print(px_count/area) if ar < 1 and ar > 0.5 and area > 1000 and area < 120000 and px_count / area > 0.5: # print(px_count/area) cv2.rectangle(img, (rect[0], rect[1]), (rect[2] + rect[0], rect[3] + rect[1]), color, 2) depth = self.predict_depth(max(rect[2], rect[3])) x, y = self.compute_xy(rect[0] + rect[2] / 2, rect[1] + rect[3] / 2, depth, img) print(depth, x, y) text = "x, y: " + str(round(x, 2)) + "m " + str(round( y, 2)) + "m" cv2.putText(img, text, (int(rect[0]) + 10, int(rect[1]) - 20), font, 0.5, color, 1, cv2.LINE_AA) #update heatmap ind_x = int(self.birdeye_heatmap.shape[0] - (self.init_pos[0] + x) * self.ppm) ind_y = int((self.init_pos[1] - y) * self.ppm) ind_x = np.clip(ind_x, 0, self.birdeye_heatmap.shape[1]) ind_y = np.clip(ind_y, 0, self.birdeye_heatmap.shape[0]) # self.heatmaps[i, ind_x, ind_y]+=1 #threshold->cluster heatmap to get members combined_mask = cv2.bitwise_or(combined_mask, mask) i += 1 self.birdeye_heatmap = np.sum(self.heatmaps, axis=0).astype(np.uint8) max_val = 10 #np.amax(self.birdeye_heatmap) self.birdeye_heatmap = np.clip(self.birdeye_heatmap, 0, max_val) if max_val > 0: birdeye_heatmap_img = cv2.applyColorMap( self.birdeye_heatmap * int(255 / max_val), cv2.COLORMAP_JET) else: birdeye_heatmap_img = cv2.applyColorMap(self.birdeye_heatmap, cv2.COLORMAP_JET) self.birdeye_heatmap_pub.publish( self.bridge.cv2_to_imgmsg(birdeye_heatmap_img, "bgr8")) combined_mask = cv2.cvtColor(combined_mask, cv2.COLOR_GRAY2BGR) self.img_pub.publish( self.bridge.cv2_to_imgmsg(np.hstack([img, combined_mask]), "bgr8")) def compute_xy(self, px, py, depth, img): #compute real position of gate in x,y,z #first x and y refers to side and height fov_w, fov_h = 62 * math.pi / 180, 46 * math.pi / 180 px_W, px_H = 640, 480 pd = (px_W / 2) / math.tan(fov_w / 2) del_px = (-px + img.shape[1] / 2.0) del_py = (-py + img.shape[0] / 2.0) del_x = depth * del_px / pd del_y = depth * del_py / pd del_real_x = del_x * math.cos(self.roll0) - del_y * math.sin( self.roll0) del_real_y = del_x * math.sin(self.roll0) + del_y * math.cos( self.roll0) + depth * math.tan(self.pitch0) x = self.x0 + depth * math.cos(self.yaw0) - del_real_x * math.sin( self.yaw0) y = self.y0 + depth * math.sin(self.yaw0) + del_real_x * math.cos( self.yaw0) z = self.z0 + del_real_y return x, y def predict_depth(self, l): fov_w, fov_h = 62 * math.pi / 180, 46 * math.pi / 180 px_W, px_H = 640, 480 #the longest edge is independent of perspective #real length of pole in metres real_l = 0.55 ppm = l / real_l H = px_H / ppm depth = H / (2 * math.tan(fov_h / 2)) # print(depth) return depth def img_correction(self, img): clahe = cv2.createCLAHE(clipLimit=3.0, tileGridSize=(6, 6)) res = np.zeros_like(img) for i in range(3): res[:, :, i] = clahe.apply(img[:, :, i]) return res def odom_callback(self, msg): self.x0 = msg.pose.pose.position.x self.y0 = msg.pose.pose.position.y self.z0 = msg.pose.pose.position.z # print(self.z0) self.roll0, self.pitch0, self.yaw0 = euler_from_quaternion( (msg.pose.pose.orientation.x, msg.pose.pose.orientation.y, msg.pose.pose.orientation.z, msg.pose.pose.orientation.w)) self.odom_received = True def printMarker(self, gate_pos): #markerList store points wrt 2D world coordinate self.markers.points = [] p = Point() self.markers.points.append(Point(0, 0, 0)) p.x = gate_pos[0] p.y = gate_pos[1] p.z = 0.75 q_angle = quaternion_from_euler(0, 0, 0) q = Quaternion(*q_angle) self.markers.pose = Pose(p, q) self.marker_pub.publish(self.markers) def init_markers(self): # Set up our waypoint markers marker_scale = 0.2 marker_lifetime = 0 # 0 is forever marker_ns = 'frontiers' marker_id = 0 marker_color = {'r': 1.0, 'g': 0.7, 'b': 1.0, 'a': 1.0} # Define a marker publisher. self.marker_pub = rospy.Publisher('gate_markers', Marker, queue_size=5) # Initialize the marker points list. self.markers = Marker() self.markers.ns = marker_ns self.markers.id = marker_id # self.markers.type = Marker.ARROW self.markers.type = Marker.SPHERE_LIST self.markers.action = Marker.ADD self.markers.lifetime = rospy.Duration(marker_lifetime) self.markers.scale.x = marker_scale self.markers.scale.y = marker_scale self.markers.scale.z = marker_scale self.markers.color.r = marker_color['r'] self.markers.color.g = marker_color['g'] self.markers.color.b = marker_color['b'] self.markers.color.a = marker_color['a'] self.markers.header.frame_id = 'map' self.markers.header.stamp = rospy.Time.now() self.markers.points = list()
def callback(data): list_obj = listobj() list_obj.single_obj_info = [] brocili_list = broclist() brocili_list.broc_uv_list = [] temp_list_broc = [] global temp_single_obj #print(data.objects_vector[0].id) # print(data.objects_vector[0].classname) # print(data.objects_vector[0].score) #print("---"*20) single_obj = sglobj() #print("single_obj",single_obj) single_obj.element_data_temp = [] single_obj.classname = [] single_obj.score = [] get_msg = data.objects_vector[0] bridge = CvBridge() cv_image = bridge.imgmsg_to_cv2(data.rgb_img, "bgr8") cv_depth_image = bridge.imgmsg_to_cv2(data.depth_img, "passthrough") rgb_img_to_pos = cv_image.copy() depth_img_to_pos = cv_depth_image.copy() plate_score = [0, 0] pan_score = [0] vegetablebowl_score = [0] broccoli_score = [0, 0, 0] souppothandle_score = [0] panhandle_score = [0] beef_score = [0] nethandle_score = [0] seasoningbottle_score = [0, 0] seasoningbowl_score = [0] for i in range(len(get_msg.id)): if (get_msg.classname[i] == "pan"): pan_score.append(get_msg.score[i]) if (get_msg.classname[i] == "beef"): beef_score.append(get_msg.score[i]) if (get_msg.classname[i] == "plate"): plate_score.append(get_msg.score[i]) if (get_msg.classname[i] == "vegetablebowl"): vegetablebowl_score.append(get_msg.score[i]) if (get_msg.classname[i] == "broccoli"): broccoli_score.append(get_msg.score[i]) if (get_msg.classname[i] == "souppothandle"): souppothandle_score.append(get_msg.score[i]) if (get_msg.classname[i] == "panhandle"): panhandle_score.append(get_msg.score[i]) if (get_msg.classname[i] == "seasoningbowl"): beef_score.append(get_msg.score[i]) if (get_msg.classname[i] == "nethandle"): nethandle_score.append(get_msg.score[i]) if (get_msg.classname[i] == "seasoningbottle"): seasoningbottle_score.append(get_msg.score[i]) plate_score = sorted(plate_score, reverse=True) pan_score = sorted(pan_score, reverse=True) vegetablebowl_score = sorted(vegetablebowl_score, reverse=True) broccoli_score = sorted(broccoli_score, reverse=True) souppothandle_score = sorted(souppothandle_score, reverse=True) panhandle_score = sorted(panhandle_score, reverse=True) beef_score = sorted(beef_score, reverse=True) nethandle_score = sorted(nethandle_score, reverse=True) seasoningbottle_score = sorted(seasoningbottle_score, reverse=True) seasoningbowl_score = sorted(seasoningbowl_score, reverse=True) det_bar_pos = detectionbar.model_detection(cv_image) bro_list = broccolidetection.broccoli_detection(cv_image) global temp_list_broc if len(bro_list) > 0: brocili_list.broc_uv_list = [] brocili_list.broc_uv_list = bro_list temp_list_broc = bro_list if len(bro_list) <= 0: brocili_list.broc_uv_list = temp_list_broc #print("wo cao :",temp_list_broc) #print("xi lan hua: ",brocili_list.broc_uv_list) ''' print("plate score : ",plate_score) print("pan score : ",pan_score) print("vegetablebowl score : ",vegetablebowl_score) print("broccoli score : ",broccoli_score) print("souppothandle score : ",souppothandle_score) print("panhandle score : ",panhandle_score) print("beef score : ",beef_score) print("nethandle score : ",nethandle_score) print("seasoningbottle score : ",seasoningbottle_score) print("seasoningbowl score : ",seasoningbowl_score) ''' single_obj.element_data_temp = [] single_obj.classname = [] single_obj.score = [] for i in range(len(get_msg.id)): element_data = element_info() element_data.id_info = [] mask_area = get_msg.roi[i].height * get_msg.roi[i].width if mask_area > 0 and get_msg.score[i] > 0.19: mask_image = bridge.imgmsg_to_cv2(get_msg.masks[i], "passthrough") if get_msg.classname[i] == "plate": center_x = int(get_msg.roi[i].x_offset + get_msg.roi[i].height / 2) # rect area center center_y = int(get_msg.roi[i].y_offset + get_msg.roi[i].width / 2) # rect area center if center_x > plate1[0] and center_x < plate1[ 1] and center_y > plate1[2] and center_y < plate1[3]: temp_list = [] temp_list = prcess_contours( cv_image, get_msg.classname[i], mask_image, get_msg.id[i], get_msg.roi[i].x_offset, get_msg.roi[i].y_offset, get_msg.roi[i].height, get_msg.roi[i].width, get_msg.score[i]) if temp_list != []: #element_data.id_info = [] element_data.id_info = temp_list single_obj.element_data_temp.append(element_data) #print("aaa:",single_obj.element_data_temp) single_obj.score.append(get_msg.score[i]) single_obj.classname.append("plate1") if get_msg.classname[i] == "plate": center_x = int(get_msg.roi[i].x_offset + get_msg.roi[i].height / 2) # rect area center center_y = int(get_msg.roi[i].y_offset + get_msg.roi[i].width / 2) # rect area center if center_x > plate2[0] and center_x < plate2[ 1] and center_y > plate2[2] and center_y < plate2[3]: temp_list1 = [] temp_list1 = prcess_contours( cv_image, get_msg.classname[i], mask_image, get_msg.id[i], get_msg.roi[i].x_offset, get_msg.roi[i].y_offset, get_msg.roi[i].height, get_msg.roi[i].width, get_msg.score[i]) if temp_list1 != []: #element_data.id_info = [] element_data.id_info = temp_list1 single_obj.element_data_temp.append(element_data) #single_obj.classname.append(get_msg.classname[i]) single_obj.score.append(get_msg.score[i]) single_obj.classname.append("plate2") if get_msg.classname[i] == "plate": center_x = int(get_msg.roi[i].x_offset + get_msg.roi[i].height / 2) # rect area center center_y = int(get_msg.roi[i].y_offset + get_msg.roi[i].width / 2) # rect area center if center_x > plate3[0] and center_x < plate3[ 1] and center_y > plate3[2] and center_y < plate3[3]: temp_list2 = [] temp_list2 = prcess_contours( cv_image, get_msg.classname[i], mask_image, get_msg.id[i], get_msg.roi[i].x_offset, get_msg.roi[i].y_offset, get_msg.roi[i].height, get_msg.roi[i].width, get_msg.score[i]) if temp_list2 != []: element_data.id_info = temp_list2 #print("wo-2-cia",temp_list) single_obj.element_data_temp.append(element_data) #single_obj.classname.append(get_msg.classname[i]) single_obj.score.append(get_msg.score[i]) single_obj.classname.append("plate3") if get_msg.classname[i] == "beef" and get_msg.score[ i] == beef_score[0]: temp_list3 = [] temp_list3 = prcess_contours( cv_image, get_msg.classname[i], mask_image, get_msg.id[i], get_msg.roi[i].x_offset, get_msg.roi[i].y_offset, get_msg.roi[i].height, get_msg.roi[i].width, get_msg.score[i]) if temp_list3 != []: element_data.id_info = temp_list3 single_obj.element_data_temp.append(element_data) single_obj.classname.append(get_msg.classname[i]) single_obj.score.append(get_msg.score[i]) if get_msg.classname[i] == "pan" and get_msg.score[i] == pan_score[ 0]: temp_list4 = [] temp_list4 = prcess_contours( cv_image, get_msg.classname[i], mask_image, get_msg.id[i], get_msg.roi[i].x_offset, get_msg.roi[i].y_offset, get_msg.roi[i].height, get_msg.roi[i].width, get_msg.score[i]) if temp_list4 != []: element_data.id_info = temp_list4 single_obj.element_data_temp.append(element_data) single_obj.classname.append(get_msg.classname[i]) single_obj.score.append(get_msg.score[i]) ''' if get_msg.classname[i] == "broccoli" and get_msg.score[i] == broccoli_score[0]: temp_list5 =[] temp_list = prcess_contours(cv_image, get_msg.classname[i], mask_image, get_msg.id[i], get_msg.roi[i].x_offset, get_msg.roi[i].y_offset, get_msg.roi[i].height, get_msg.roi[i].width, get_msg.score[i]) if temp_list5!=[]: element_data.id_info = temp_list5 single_obj.element_data_temp.append(element_data) single_obj.classname.append(get_msg.classname[i]) single_obj.score.append(get_msg.score[i]) if get_msg.classname[i] == "broccoli" and get_msg.score[i] == broccoli_score[1]: temp_list6 = [] temp_list6 = prcess_contours(cv_image, get_msg.classname[i], mask_image, get_msg.id[i], get_msg.roi[i].x_offset, get_msg.roi[i].y_offset, get_msg.roi[i].height, get_msg.roi[i].width, get_msg.score[i]) if temp_list6!=[]: element_data.id_info = temp_list6 single_obj.element_data_temp.append(element_data) single_obj.classname.append(get_msg.classname[i]) single_obj.score.append(get_msg.score[i]) if get_msg.classname[i] == "broccoli" and get_msg.score[i] == broccoli_score[2]: temp_list7 = [] temp_list7 = prcess_contours(cv_image, get_msg.classname[i], mask_image, get_msg.id[i], get_msg.roi[i].x_offset, get_msg.roi[i].y_offset, get_msg.roi[i].height, get_msg.roi[i].width, get_msg.score[i]) if temp_list7!=[]: element_data.id_info = temp_list7 single_obj.element_data_temp.append(element_data) single_obj.classname.append(get_msg.classname[i]) single_obj.score.append(get_msg.score[i]) if get_msg.classname[i] == "souppothandle" and get_msg.score[i] == souppothandle_score[0]: temp_list8 = [] temp_list8 = prcess_contours(cv_image, get_msg.classname[i], mask_image, get_msg.id[i], get_msg.roi[i].x_offset, get_msg.roi[i].y_offset, get_msg.roi[i].height, get_msg.roi[i].width, get_msg.score[i]) if temp_list8!=[]: element_data.id_info = temp_list8 single_obj.element_data_temp.append(element_data) single_obj.classname.append(get_msg.classname[i]) single_obj.score.append(get_msg.score[i]) ''' ''' if get_msg.classname[i] == "nethandle" and get_msg.score[i] == nethandle_score[0]: temp_list9 = [] temp_list9 = prcess_contours(cv_image, get_msg.classname[i], mask_image, get_msg.id[i], get_msg.roi[i].x_offset, get_msg.roi[i].y_offset, get_msg.roi[i].height, get_msg.roi[i].width, get_msg.score[i]) if temp_list9!=[]: element_data.id_info = temp_list9 single_obj.element_data_temp.append(element_data) single_obj.classname.append(get_msg.classname[i]) single_obj.score.append(get_msg.score[i]) if get_msg.classname[i] == "vegetablebowl" and get_msg.score[i] == vegetablebowl_score[0]: temp_list10 = [] temp_list10 = prcess_contours(cv_image, get_msg.classname[i], mask_image, get_msg.id[i], get_msg.roi[i].x_offset, get_msg.roi[i].y_offset, get_msg.roi[i].height, get_msg.roi[i].width, get_msg.score[i]) if temp_list10!=[]: element_data.id_info = temp_list10 single_obj.element_data_temp.append(element_data) single_obj.classname.append(get_msg.classname[i]) single_obj.score.append(get_msg.score[i]) ''' if get_msg.classname[i] == "seasoningbowl" and get_msg.score[ i] == seasoningbowl_score[0]: temp_list11 = [] temp_list11 = prcess_contours( cv_image, get_msg.classname[i], mask_image, get_msg.id[i], get_msg.roi[i].x_offset, get_msg.roi[i].y_offset, get_msg.roi[i].height, get_msg.roi[i].width, get_msg.score[i]) if temp_list11 != []: element_data.id_info = temp_list11 single_obj.element_data_temp.append(element_data) #single_obj.classname.append(get_msg.classname[i]) single_obj.score.append(get_msg.score[i]) if get_msg.classname[i] == "seasoningbottle": center_x = int(get_msg.roi[i].x_offset + get_msg.roi[i].height / 2) # rect area center center_y = int(get_msg.roi[i].y_offset + get_msg.roi[i].width / 2) # rect area center if center_x > bottle1[0] and center_x < bottle1[ 1] and center_y > bottle1[2] and center_y < bottle1[3]: temp_list12 = [] temp_list12 = prcess_contours( cv_image, get_msg.classname[i], mask_image, get_msg.id[i], get_msg.roi[i].x_offset, get_msg.roi[i].y_offset, get_msg.roi[i].height, get_msg.roi[i].width, get_msg.score[i]) if temp_list12 != []: single_obj.classname.append("seasoningbottle1") element_data.id_info = temp_list12 single_obj.element_data_temp.append(element_data) #single_obj.classname.append(get_msg.classname[i]) single_obj.score.append(get_msg.score[i]) if get_msg.classname[i] == "seasoningbottle": center_x = int(get_msg.roi[i].x_offset + get_msg.roi[i].height / 2) # rect area center center_y = int(get_msg.roi[i].y_offset + get_msg.roi[i].width / 2) # rect area center if center_x > bottle2[0] and center_x < bottle2[ 1] and center_y > bottle2[2] and center_y < bottle2[3]: temp_list13 = [] temp_list13 = prcess_contours( cv_image, get_msg.classname[i], mask_image, get_msg.id[i], get_msg.roi[i].x_offset, get_msg.roi[i].y_offset, get_msg.roi[i].height, get_msg.roi[i].width, get_msg.score[i]) if temp_list13 != []: single_obj.classname.append("seasoningbottle2") element_data.id_info = temp_list13 single_obj.element_data_temp.append(element_data) #single_obj.classname.append(get_msg.classname[i]) single_obj.score.append(get_msg.score[i]) ''' if get_msg.classname[i] == "panhandle" and get_msg.score[i] == panhandle_score[0]: temp_list14 = [] temp_list14 = prcess_contours(cv_image, get_msg.classname[i], mask_image, get_msg.id[i], get_msg.roi[i].x_offset, get_msg.roi[i].y_offset, get_msg.roi[i].height, get_msg.roi[i].width, get_msg.score[i]) if temp_list14!=[]: element_data.id_info = temp_list14 single_obj.element_data_temp.append(element_data) single_obj.classname.append(get_msg.classname[i]) single_obj.score.append(get_msg.score[i]) ''' #print("88888",single_obj) if len(single_obj.score) > 0: #print("1111:",single_obj) temp_single_obj = single_obj #print("****",temp_single_obj) list_obj.single_obj_info.append(single_obj) else: list_obj.single_obj_info.append(temp_single_obj) #print("1111:",list_obj.single_obj_info) list_obj.header.stamp = data.header.stamp list_obj.header.frame_id = data.header.frame_id print("---" * 20) print("--ww--", list_obj.single_obj_info) list_obj.rgb_img_to_pos = bridge.cv2_to_imgmsg(rgb_img_to_pos, encoding="bgr8") list_obj.depth_img_to_pos = bridge.cv2_to_imgmsg(depth_img_to_pos, "passthrough") temp_det_bar_pos = Float32MultiArray(data=det_bar_pos) #print("1111:",list_obj) list_obj_pub = rospy.Publisher("process_uv/listobjs", listobj, queue_size=1) temp_det_bar_pos_pub = rospy.Publisher("process_uv/bar_pos", Float32MultiArray, queue_size=1) list_broc_pub = rospy.Publisher("process_uv/listbroc", broclist, queue_size=1) list_broc_pub.publish(brocili_list) list_obj_pub.publish(list_obj) temp_det_bar_pos_pub.publish(temp_det_bar_pos) cv2.rectangle(cv_image, (plate2[0], plate2[2]), (plate2[0] + (plate2[1] - plate2[0]), plate2[2] + (plate2[3] - plate2[2])), (0, 0, 255), 2) cv2.rectangle(cv_image, (plate1[0], plate1[2]), (plate1[0] + (plate1[1] - plate1[0]), plate1[2] + (plate1[3] - plate1[2])), (0, 0, 255), 2) cv2.rectangle(cv_image, (plate3[0], plate3[2]), (plate3[0] + (plate3[1] - plate3[0]), plate3[2] + (plate3[3] - plate3[2])), (0, 0, 255), 2) #cv2.rectangle(cv_image,(bottle1[0], bottle1[2]),(bottle1[0]+(bottle1[1]-bottle1[0])/2, bottle1[2]+(bottle1[3]-bottle1[2])/2),(0,0,255),2) #cv2.rectangle(cv_image,(bottle2[0], bottle2[2]),(bottle2[0]+(bottle2[1]-bottle2[0])/2, bottle2[2]+(bottle2[3]-bottle2[2])/2),(0,0,255),2) cv2.imshow("img2", cv_image) cv2.waitKey(1)
class MakeBoundingBox(): def __init__(self): rospy.loginfo("pointcloud object detection is running...") # frame size self.frame_x = 640 self.frame_y = 480 self.bridge = CvBridge() # cv_image and pcl variables self.cv_image = np.zeros([self.frame_x, self.frame_y]) self.pcl = None # transform config # self.tf_pub = tf.TransformBroadcaster() # load torch weights_path = os.path.abspath(os.path.dirname(__file__)) + '/weights' model_lst = [ x for x in sorted(os.listdir(weights_path)) if x.endswith('.pkl') ] if len(model_lst) == 0: print('No previous model found, please train first!') exit() else: print('Using previous model %s' % model_lst[-1]) my_vgg = vgg.vgg19_bn(pretrained=True) self.model = Model.Model(features=my_vgg.features, bins=2).cuda() checkpoint = torch.load(weights_path + '/%s' % model_lst[-1]) self.model.load_state_dict(checkpoint['model_state_dict']) self.model.eval() # load yolo yolo_path = os.path.abspath(os.path.dirname(__file__)) + '/weights' self.yolo = cv_Yolo(yolo_path) self.averages = ClassAverages.ClassAverages() # TODO: clean up how this is done. flag? self.angle_bins = generate_bins(2) calib_path = os.path.abspath( os.path.dirname(__file__)) + "/" + "camera_cal/" self.calib_file = calib_path + "calib_cam_to_cam.txt" # subscribers self.img_sub = rospy.Subscriber("/kitti/camera_color_right/image_raw", Image, self.rgb_callback) #self.pcl_sub = rospy.Subscriber("/camera/depth_registered/points", PointCloud2, self.pcl_callback) # publishers self.img_detected_pub = rospy.Publisher( "ROS_3D_BBox/img_detected_frame", Image, queue_size=100) self.location_pub = rospy.Publisher("ROS_3D_BBox/location_array", LocationArray, queue_size=100) self.rate = rospy.Rate(1) def plot_regressed_3d_bbox(self, img, cam_to_img, box_2d, dimensions, alpha, theta_ray, img_2d=None): # the math! returns X, the corners used for constraint location, X = calc_location(dimensions, cam_to_img, box_2d, alpha, theta_ray) orient = alpha + theta_ray if img_2d is not None: plot_2d_box(img_2d, box_2d) ret_img = plot_3d_box(img, cam_to_img, orient, dimensions, location) # 3d boxes return location, ret_img def rgb_callback(self, img_data): try: truth_img = self.bridge.imgmsg_to_cv2(img_data, 'bgr8') except CvBridgeError as e: print(e) img = np.copy(truth_img) yolo_img = np.copy(truth_img) start_time = time.time() detections = self.yolo.detect(yolo_img) locations = [] for detection in detections: if not self.averages.recognized_class(detection.detected_class): continue # this is throwing when the 2d bbox is invalid # TODO: better check try: detectedObject = DetectedObject(img, detection.detected_class, detection.box_2d, self.calib_file) except: continue theta_ray = detectedObject.theta_ray input_img = detectedObject.img proj_matrix = detectedObject.proj_matrix box_2d = detection.box_2d detected_class = detection.detected_class input_tensor = torch.zeros([1, 3, 224, 224]).cuda() input_tensor[0, :, :, :] = input_img [orient, conf, dim] = self.model(input_tensor) orient = orient.cpu().data.numpy()[0, :, :] conf = conf.cpu().data.numpy()[0, :] dim = dim.cpu().data.numpy()[0, :] dim += self.averages.get_item(detected_class) argmax = np.argmax(conf) orient = orient[argmax, :] cos = orient[0] sin = orient[1] alpha = np.arctan2(sin, cos) alpha += self.angle_bins[argmax] alpha -= np.pi location, ret_img = self.plot_regressed_3d_bbox( img, proj_matrix, box_2d, dim, alpha, theta_ray, truth_img) loc_msg = Location() loc_msg.point.x, loc_msg.point.y, loc_msg.point.z = location[ 0], location[1], location[2] loc_msg.size.x, loc_msg.size.y, loc_msg.size.z = dim[0], dim[ 1], dim[2] loc_msg.alpha = alpha loc_msg.theta_ray = theta_ray loc_msg.object_type = detection.detected_class locations.append(loc_msg) try: img_msg = self.bridge.cv2_to_imgmsg(ret_img, 'bgr8') except CvBridgeError as e: print(e) loc_msg_header = Header() loc_msg_header.frame_id = 'locations array' loc_msg_header.stamp = img_msg.header.stamp loc_arr_msg = LocationArray(loc_msg_header, locations) self.img_detected_pub.publish(img_msg) self.location_pub.publish(loc_arr_msg) print("\n") print('Got %s poses in %.3f seconds FPS: %.2f' % (len(detections), time.time() - start_time, 1 / (time.time() - start_time))) print('-------------') with open('data.csv', 'a') as f: f.write( str(len(detections)) + ',' + str(time.time() - start_time) + ',' + str(1 / (time.time() - start_time)) + '\n')
class image_converter: def __init__(self): self.image_pub = rospy.Publisher("image_topic_2", Image) self.bridge = CvBridge() #self.image_sub = rospy.Subscriber("camera/rgb/image_raw",Image,self.callback) #self.image_sub = rospy.Subscriber("usb_cam/image_raw", Image, self.callback) self.fgbg = cv2.createBackgroundSubtractorMOG2() self.blur = np.ones((5, 5), np.float32) / 25 cv2.namedWindow('camera') # create trackbars for color change #cv2.createTrackbar('R', 'image', 0, 255, nothing) #cv2.createTrackbar('G', 'image', 0, 255, nothing) #cv2.createTrackbar('B', 'image', 0, 255, nothing) # Creating track bar default_tapis_low = [0, 3, 0] default_tapis_high = [152, 247, 255] cv2.createTrackbar('H_low', 'camera', default_tapis_low[0], 179, nothing) cv2.createTrackbar('S_low', 'camera', default_tapis_low[1], 255, nothing) cv2.createTrackbar('V_low', 'camera', default_tapis_low[2], 255, nothing) cv2.createTrackbar('H_high', 'camera', default_tapis_high[0], 179, nothing) cv2.createTrackbar('S_high', 'camera', default_tapis_high[1], 255, nothing) cv2.createTrackbar('V_high', 'camera', default_tapis_high[2], 255, nothing) # create switch for ON/OFF functionality self.switch = '0 : OFF \n1 : ON' cv2.createTrackbar(self.switch, 'camera', 0, 1, nothing) self.first = True #switch = '0 : OFF \n1 : ON' #cv2.createTrackbar(switch, 'image', 0, 1, nothing) def add_blobs(self, crop_frame): frame = cv2.GaussianBlur(crop_frame, (3, 3), 0) # Convert BGR to HSV hsv = cv2.cvtColor(frame, cv2.COLOR_BGR2HSV) # define range of green color in HSV lower_green = np.array([70, 50, 50]) upper_green = np.array([85, 255, 255]) # Threshold the HSV image to get only blue colors mask = cv2.inRange(hsv, lower_green, upper_green) mask = cv2.erode(mask, None, iterations=1) mask = cv2.dilate(mask, None, iterations=1) # Bitwise-AND mask and original image res = cv2.bitwise_and(frame, frame, mask=mask) detector = cv2.SimpleBlobDetector_create() # Detect blobs. reversemask = 255 - mask keypoints = detector.detect(reversemask) if keypoints: print "found blobs" if len(keypoints) > 4: keypoints.sort(key=(lambda s: s.size)) keypoints = keypoints[0:3] # Draw detected blobs as red circles. # cv2.DRAW_MATCHES_FLAGS_DRAW_RICH_KEYPOINTS ensures the size of the circle corresponds to the size of blob im_with_keypoints = cv2.drawKeypoints( frame, keypoints, np.array([]), (0, 0, 255), cv2.DRAW_MATCHES_FLAGS_DRAW_RICH_KEYPOINTS) else: print "no blobs" im_with_keypoints = crop_frame return im_with_keypoints # , max_blob_dist, blob_center, keypoint_in_orders def callback(self, data): try: cv_image = self.bridge.imgmsg_to_cv2(data, "bgr8") except CvBridgeError as e: print(e) mask = np.zeros(cv_image.shape[:2], np.uint8) bgdModel = np.zeros((1, 65), np.float64) fgdModel = np.zeros((1, 65), np.float64) x1 = 10 x2 = 400 y1 = 10 y2 = 400 rect = (x1, x2, y1, y2) # BACKGROUND SUBSTRACTION cv_image = cv2.filter2D(cv_image, -1, self.blur) #cv_image = self.fgbg.apply(cv_image) #cv_image = cv2.medianBlur(cv_image, 5) # COLOR DETECTION hsv = cv2.cvtColor(cv_image, cv2.COLOR_BGR2HSV) cv_image = cv2.filter2D(cv_image, -1, self.blur) lower_green = np.array([70, 50, 50]) upper_green = np.array([85, 255, 255]) mask = cv2.inRange(hsv, lower_green, upper_green) mask = cv2.medianBlur(mask, 5) cv_image = cv2.bitwise_and(cv_image, cv_image, mask=mask) imgray = cv2.cvtColor(cv_image, cv2.COLOR_BGR2GRAY) im2, contours, hierarchy = cv2.findContours(imgray, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE) if len(contours) != 0: # draw in blue the contours that were founded cv2.drawContours(cv_image, contours, -1, 255, 3) # find the biggest area c = max(contours, key=cv2.contourArea) x, y, w, h = cv2.boundingRect(c) # draw the book contour (in green) cv2.rectangle(cv_image, (x, y), (x + w, y + h), (0, 255, 0), 2) #cv2.drawContours(cv_image, contours, -1, (0, 255, 0), 3) #cv2.grabCut(cv_image, mask, rect, bgdModel, fgdModel, 5, cv2.GC_INIT_WITH_RECT) #mask2 = np.where((mask == 2) | (mask == 0), 0, 1).astype('uint8') #cv_image = cv_image * mask2[:, :, np.newaxis] #cv_image = self.add_blobs(cv_image) #cv2.rectangle(cv_image, (x1,y1), (x2,y2), (255,0,0), 2) cv2.imshow('image', imgray) cv2.waitKey(1) try: self.image_pub.publish(self.bridge.cv2_to_imgmsg(cv_image, "bgr8")) except CvBridgeError as e: print(e) def callback_no_topic(self): data = rospy.wait_for_message("/camera/rgb/image_raw", Image) #data = rospy.wait_for_message("usb_cam/image_raw", Image) try: cv_image_full = self.bridge.imgmsg_to_cv2(data, "bgr8") except CvBridgeError as e: print(e) if self.first: self.r = cv2.selectROI(cv_image_full) self.first = False if cv2.getTrackbarPos(self.switch, 'camera') == 1: self.r = cv2.selectROI(cv_image_full) cv_image = cv_image_full[int(self.r[1]):int(self.r[1] + self.r[3]), int(self.r[0]):int(self.r[0] + self.r[2])] # BACKGROUND SUBSTRACTION output = cv_image cv_image = cv2.filter2D(cv_image, -1, self.blur) # CONVERT TO HSV hsv = cv2.cvtColor(cv_image, cv2.COLOR_BGR2HSV) # BLUR TO GET RID OF NOISE cv_image = cv2.filter2D(cv_image, -1, self.blur) # get info from track bar and appy to result h_low = cv2.getTrackbarPos('H_low', 'camera') s_low = cv2.getTrackbarPos('S_low', 'camera') v_low = cv2.getTrackbarPos('V_low', 'camera') h_high = cv2.getTrackbarPos('H_high', 'camera') s_high = cv2.getTrackbarPos('S_high', 'camera') v_high = cv2.getTrackbarPos('V_high', 'camera') lower_green = np.array([h_low, s_low, v_low]) upper_green = np.array([h_high, s_high, v_high]) mask = cv2.inRange(hsv, lower_green, upper_green) mask2 = mask #mask = cv2.medianBlur(mask, 5) #mask = cv2.medianBlur(mask, 5) mask3 = mask mask3 = cv2.bitwise_not(mask3) invert = mask3 #cv_image3 = cv_image #cv_image2 = cv2.bitwise_and(cv_image, cv_image, mask=mask) #cv_image = cv_image2 #imgray = cv2.cvtColor(cv_image, cv2.COLOR_BGR2GRAY) # TEST EXTRAIRE OBJET im2, contours, hierarchy = cv2.findContours(mask3, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE) #im2, contours, hierarchy = cv2.findContours(imgray, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE) if len(contours) != 0: # draw in blue the contours that were founded #cv2.drawContours(output, contours, -1, 255, 3) # find the biggest area c = max(contours, key=cv2.contourArea) cv2.drawContours(output, c, -1, 255, 3) x, y, w, h = cv2.boundingRect(c) # draw the book contour (in green) cv2.rectangle(output, (x, y), (x + w, y + h), (0, 255, 0), 2) #cv2.drawContours(cv_image, contours, -1, (0, 255, 0), 3) #cv2.grabCut(cv_image, mask, rect, bgdModel, fgdModel, 5, cv2.GC_INIT_WITH_RECT) #mask2 = np.where((mask == 2) | (mask == 0), 0, 1).astype('uint8') #cv_image = cv_image * mask2[:, :, np.newaxis] #cv_image = self.add_blobs(cv_image) #cv2.rectangle(cv_image, (x1,y1), (x2,y2), (255,0,0), 2) cv2.imshow('inRange', mask2) cv2.imshow('invert', invert) cv2.imshow('camera', output) cv2.waitKey(1) try: self.image_pub.publish(self.bridge.cv2_to_imgmsg(cv_image, "bgr8")) except CvBridgeError as e: print(e)
class MyNode(DTROS): def __init__(self, node_name): # initialize the DTROS parent class super(MyNode, self).__init__(node_name=node_name) # construct publisher and subsriber self.pub = rospy.Publisher('/duckiesam/chatter', String, queue_size=10) self.sub_image = rospy.Subscriber( "/duckiesam/camera_node/image/compressed", CompressedImage, self.find_marker, buff_size=921600, queue_size=1) self.pub_image = rospy.Publisher('/duckiesam/modified_image', Image, queue_size=1) self.sub_info = rospy.Subscriber("/duckiesam/camera_node/camera_info", CameraInfo, self.get_camera_info, queue_size=1) self.pub_move = rospy.Publisher("/duckiesam/joy_mapper_node/car_cmd", Twist2DStamped, queue_size=1) self.leader_detected = rospy.Publisher("/duckiesam/detection", BoolStamped, queue_size=1) #values for detecting marker self.starting = 0 self.ending = 0 self.camerainfo = PinholeCameraModel() self.bridge = CvBridge() self.gotimage = False self.imagelast = None self.processedImg = None self.detected = False #values for calculating pose of robot self.solP = False self.rotationvector = None self.translationvector = None self.axis = np.float32([[0.0125, 0, 0], [0, 0.0125, 0], [0, 0, -0.0375]]).reshape(-1, 3) self.distance = None self.angle_f = None self.angle_l = None #values for driving the robot self.maxdistance = 0.2 self.speedN = 0 self.e_vB = 0 self.rotationN = 0 self.mindistance = 0.1 self.d_e = 0 #distance error self.d_e_1 = 5 self.d_e_2 = 10 self.y2 = 0 self.controltime = rospy.Time.now() self.Kp = 1 self.Ki = 0.1 self.Kd = 0 self.I = 0 self.Rp = 1 self.Ri = 1 self.Rd = 1 def initialvalues(self): self.default_v = 0.22 self.maxdistance = 0.3 self.speedN = 0 self.e_vB = 0 self.rotationN = 0 self.mindistance = 0.2 self.d_e = 0 #distance error self.d_e_1 = 5 self.d_e_2 = 10 self.y2 = 0 self.controltime = rospy.Time.now() self.Kp = 1 self.Ki = 0.1 self.Kd = 0 self.I = 0 self.Rp = 1 self.Ri = 1 self.Rd = 1 #get camera info for pinhole camera model def get_camera_info(self, camera_msg): self.camerainfo.fromCameraInfo(camera_msg) #step 1 : find the back circle grids using cv2.findCirclesGrid ##### set (x,y) for points and flag for detection def find_marker(self, image_msg): try: self.imagelast = self.bridge.compressed_imgmsg_to_cv2( image_msg, "bgr8") except CvBridgeError as e: print(e) if self.gotimage == False: self.gotimage = True #time checking self.starting = rospy.Time.now() #from_last_image = (self.starting - self.ending).to_sec() gray = cv2.cvtColor(self.imagelast, cv2.COLOR_BGR2GRAY) detection, corners = cv2.findCirclesGrid(gray, (7, 3)) self.processedImg = self.imagelast.copy() cmd = Twist2DStamped() cmd.header.stamp = self.starting if detection: cv2.drawChessboardCorners(self.processedImg, (7, 3), corners, detection) self.detected = True #self.controltime = rospy.Time.now() twoone = [] for i in range(0, 21): point = [corners[i][0][0], corners[i][0][1]] twoone.append(point) twoone = np.array(twoone) self.originalmatrix() self.gradient(twoone) self.detected = self.solP img_out = self.bridge.cv2_to_imgmsg(self.processedImg, "bgr8") self.pub_image.publish(img_out) self.find_distance() ###self.move(self.y2, self.angle_l, self.distance) self.ending = rospy.Time.now() else: self.detected = False img_out = self.bridge.cv2_to_imgmsg(gray, "bgr8") self.pub_image.publish(img_out) self.ending = rospy.Time.now() cmd.v = 0 cmd.omega = 0 ####self.pub_move.publish(cmd) #step 2 : makes matrix for 3d original shape def originalmatrix(self): #coners and points self.originalmtx = np.zeros([21, 3]) for i in range(0, 7): for j in range(0, 3): self.originalmtx[i + j * 7, 0] = 0.0125 * i - 0.0125 * 3 self.originalmtx[i + j * 7, 1] = 0.0125 * j - 0.0125 #step 3 : use intrinsic matrix and solvePnP, return rvec and tvec, print axis def gradient(self, imgpts): #using solvePnP to find rotation vector and translation vector and also find 3D point to the image plane self.solP, self.rotationvector, self.translationvector = cv2.solvePnP( self.originalmtx, imgpts, self.camerainfo.intrinsicMatrix(), self.camerainfo.distortionCoeffs()) if self.solP: pointsin3D, jacoB = cv2.projectPoints( self.originalmtx, self.rotationvector, self.translationvector, self.camerainfo.intrinsicMatrix(), self.camerainfo.distortionCoeffs()) pointaxis, _ = cv2.projectPoints( self.axis, self.rotationvector, self.translationvector, self.camerainfo.intrinsicMatrix(), self.camerainfo.distortionCoeffs()) self.processedImg = cv2.line(self.processedImg, tuple(imgpts[10].ravel()), tuple(pointaxis[0].ravel()), (255, 0, 0), 2) self.processedImg = cv2.line(self.processedImg, tuple(imgpts[10].ravel()), tuple(pointaxis[1].ravel()), (0, 255, 0), 2) self.processedImg = cv2.line(self.processedImg, tuple(imgpts[10].ravel()), tuple(pointaxis[2].ravel()), (0, 0, 255), 3) #textdistance = "x = %s, y = %s, z = %s" % (self.distance, self.angle_f, self.angle_l, self.y2) #rospy.loginfo("%s" % textdistance) #step 4 : find distance between robot and following robot print out distance and time def find_distance(self): #use tvec to calculate distance tvx = self.translationvector[0] tvy = self.translationvector[1] tvz = self.translationvector[2] self.distance = math.sqrt(tvx * tvx + tvz * tvz) self.angle_f = np.arctan2(tvx[0], tvz[0]) R, _ = cv2.Rodrigues(self.rotationvector) R_inverse = np.transpose(R) self.angle_l = np.arctan2( -R_inverse[2, 0], math.sqrt(R_inverse[2, 1]**2 + R_inverse[2, 2]**2)) T = np.array([-np.sin(self.angle_l), np.cos(self.angle_l)]) tvecW = -np.dot(R_inverse, self.translationvector) x_y = np.array([tvz[0], tvx[0]]) self.y2 = -np.dot(T, x_y) - 0.01 * np.sin(self.angle_l) textdistance = "Distance = %s, Angle of Follower = %s, Angle of Leader = %s, y = %s" % ( self.distance, self.angle_f, self.angle_l, self.y2) rospy.loginfo("%s" % textdistance) #self.pub.publish(textdistance) #step 5 : use joy mapper to control the robot PID controller def move(self, y_to, angle_to, d): #y_to is needed y value to be parallel to leader's center line #angle_to is angle needed to rotate #d is distance between required position and now position cmd = Twist2DStamped() time = rospy.Time.now() cmd.header.stamp = time dt = (time - self.controltime).to_sec() if dt > 3: if d < self.maxdistance: cmd.v = 0 cmd.omega = 0 else: self.d_e = d - self.maxdistance error_d = (self.d_e - self.d_e_1) / dt errorB = (self.d_e_1 - self.d_e_2) / dt e_v = error_d - errorB P = self.Kp * (e_v) self.I = self.I + self.Ki * (e_v + self.e_vB) / 2 * dt D = self.Kd * (e_v + self.e_vB) / dt self.speedN = P + self.I + D self.rotationN = self.Rp * (y_to) + self.Ri * ( angle_to) + self.Rd * (np.sin(angle_to)) cmd.v = self.speedN cmd.omega = self.rotationN self.e_vB = e_v self.d_e_2 = self.d_e_1 self.d_e_1 = self.d_e if self.d_e < 0.05 or self.speedN < 0: cmd.v = 0 cmd.omega = 0 textdistance = "Velocity = %s, Rotation = %s" % (cmd.v, cmd.omega) rospy.loginfo("%s" % textdistance) self.pub_move.publish(cmd) self.controltime = time
class camera_node(object): def __init__(self): ''' Camera node constructor ''' rospy.init_node( "camera_node") # Register this as ROS node named 'camera_node' self.cap = None # Will store opencv capture device self.bridge = CvBridge( ) # Bridge used to convert CV images into ROS Image messages self._load_ros_parameters() self._initialize_camera() # initialize camera atexit.register( self._exit_handler) # Register function to be called upon exit signal.signal(signal.SIGINT, self._signal_handler) # Register ctrl-c signal handler self.cap_pub = rospy.Publisher( CAM_TOPIC, Image, queue_size=10) # Create publisher for camera images def _load_ros_parameters(self): ''' Calling this function tries to update globals for this module using ROS params ''' global CAM_TOPIC, CAM_DEVICE_INDEX, CV_BRIDGE_IMAGE_ENCODING, SHOW_CAM try: CAM_DEVICE_INDEX = rospy.get_param("/CAM_DEVICE_INDEX") except: pass try: SHOW_CAM = rospy.get_param("/DISPLAY_CAM_WINDOW") except: pass try: CAM_TOPIC = rospy.get_param("/CAM_TOPIC_NAME") except: pass try: CV_BRIDGE_IMAGE_ENCODING = rospy.get_param( "/CV_BRIDGE_IMAGE_ENCODING") except: pass def _initialize_camera(self): ''' This function is called to initialize the camera sensor. This function in a helper function for the camera node class. ''' self.cap = cv2.VideoCapture(CAM_DEVICE_INDEX) def transmit_video(self): ''' This function runs until ROS is shutdown or process is killed. Continuously captures frames and publishes them over ROS topic. ''' rate = rospy.Rate(50) # create a 10Hz rate while not rospy.is_shutdown(): ret, frame = self.cap.read() # Grab current frame from webcam # Only show cv2 image if show cam parameter is true if (SHOW_CAM): cv2.imshow("frame", frame) if cv2.waitKey(1) & 0xFF == ord('q'): break # publish frame as ROS Image message using cv_bridge try: self.cap_pub.publish( self.bridge.cv2_to_imgmsg(frame, CV_BRIDGE_IMAGE_ENCODING)) except CvBridgeError, e: # TODO: log error print(e) rate.sleep()
class ObjectDetect: def __init__(self): print('openCV version:', cv2.__version__) self.image_pub = rospy.Publisher("/object_detect", Image, queue_size=2) self.bridge = CvBridge() self.image_sub = rospy.Subscriber("/cf1/camera/image_raw", Image, self.callback) ##### DNN stuff ##### # arguments from the argument parser of the example self.prototxt = '/home/robot/dd2419_ws/src/crazyflie_9/milestone2/include/dnn_models/MobileNetSSD_deploy.prototxt.txt' self.model = '/home/robot/dd2419_ws/src/crazyflie_9/milestone2/include/dnn_models/MobileNetSSD_deploy.caffemodel' self.confidence = 0.2 # Minimum probability # initialize the list of class labels MobileNet SSD was trained to # detect, then generate a set of bounding box colors for each class self.CLASSES = [ "background", "aeroplane", "bicycle", "bird", "boat", "bottle", "bus", "car", "cat", "chair", "cow", "diningtable", "dog", "horse", "motorbike", "person", "pottedplant", "sheep", "sofa", "train", "tvmonitor" ] self.COLORS = np.random.uniform(0, 255, size=(len(self.CLASSES), 3)) # load our serialized model from disk print("[INFO] loading model...") self.net = cv2.dnn.readNetFromCaffe(self.prototxt, self.model) rospy.sleep(5) def callback(self, data): # Convert the image from OpenCV to ROS format try: cv_image = self.bridge.imgmsg_to_cv2(data, "bgr8") except CvBridgeError as e: print(e) ## # Do the dnn stuff to the image ## result = self.dnn_object_detect(cv_image) # Publish the image try: self.image_pub.publish(self.bridge.cv2_to_imgmsg(result, "bgr8")) except CvBridgeError as e: print(e) def dnn_object_detect(self, image): (h, w) = image.shape[:2] blob = cv2.dnn.blobFromImage(cv2.resize(image, (300, 300)), 0.007843, (300, 300), 127.5) # pass the blob through the network and obtain the detections and # predictions print("[INFO] computing object detections...") self.net.setInput(blob) detections = self.net.forward() # loop over the detections for i in np.arange(0, detections.shape[2]): # extract the confidence (i.e., probability) associated with the # prediction confidence = detections[0, 0, i, 2] # filter out weak detections by ensuring the `confidence` is # greater than the minimum confidence if confidence > self.confidence: # extract the index of the class label from the `detections`, # then compute the (x, y)-coordinates of the bounding box for # the object idx = int(detections[0, 0, i, 1]) box = detections[0, 0, i, 3:7] * np.array([w, h, w, h]) (startX, startY, endX, endY) = box.astype("int") # display the prediction label = "{}: {:.2f}%".format(self.CLASSES[idx], confidence * 100) print("[INFO] {}".format(label)) cv2.rectangle(image, (startX, startY), (endX, endY), self.COLORS[idx], 2) y = startY - 15 if startY - 15 > 15 else startY + 15 cv2.putText(image, label, (startX, y), cv2.FONT_HERSHEY_SIMPLEX, 0.5, self.COLORS[idx], 2) return image
def image_publisher(self, image): bridge = CvBridge() msg = bridge.cv2_to_imgmsg(image) self.get_logger().info('Publishing something !') self.publisher.publish(msg)
def detect_video(self, yolo, video_path): from PIL import Image, ImageFont, ImageDraw #Start ROS node # pub, pub_flag, pub_track, pub_frame1, pub_frame2 = start_node() pub, pub_flag, pub_frame1, pub_frame2 = start_node() vid = RealsenseCapture() vid.start() bridge = CvBridge() accum_time = 0 curr_fps = 0 fps = "FPS: ??" prev_time = timer() worldy = 0.0 flag = 0 while True: # pub_track.publish(0) # flag = 0 if self.garbage_in_can == 1: # print("ゴミを捨てました") flag = 0 pub_flag.publish(flag) ret, frames, _ = vid.read() frame = frames[0] depth_frame = frames[1] image = Image.fromarray(frame) image, bottle, person, right, left, bottom, top, right2, left2, bottom2, top2 = yolo.detect_image( image, pub) result = np.asarray(image) curr_time = timer() exec_time = curr_time - prev_time prev_time = curr_time accum_time = accum_time + exec_time curr_fps = curr_fps + 1 if accum_time > 1: accum_time = accum_time - 1 fps = "FPS: " + str(curr_fps) curr_fps = 0 cv2.putText(result, text=fps, org=(3, 15), fontFace=cv2.FONT_HERSHEY_SIMPLEX, fontScale=0.50, color=(255, 0, 0), thickness=2) # cv2.imshow("result", result) yolo_frame = bridge.cv2_to_imgmsg(result, "bgr8") pub_frame1.publish(yolo_frame) if cv2.waitKey(1) & 0xFF == ord('q'): break if (bottle == False) or (person == False): continue # ------------------------------Tracking----------------------------------- # tracker_types = ['BOOSTING', 'MIL','KCF', 'TLD', 'MEDIANFLOW', 'GOTURN', 'MOSSE', 'CSRT'] # tracker_type = tracker_types[7] tracker = cv2.TrackerCSRT_create() tracker2 = cv2.TrackerCSRT_create() # setup initial location of window left, right, top, bottom = left, right, top, bottom r, h, ci, w = top, bottom - top, left, right - left # simply hardcoded the values r, h, c, w frame_b, frame_g, frame_r = frame[:, :, 0], frame[:, :, 1], frame[:, :, 2] hist_b = cv2.calcHist([frame_b[top:bottom, left:right]], [0], None, [256], [0, 256]) hist_g = cv2.calcHist([frame_g[top:bottom, left:right]], [0], None, [256], [0, 256]) hist_r = cv2.calcHist([frame_r[top:bottom, left:right]], [0], None, [256], [0, 256]) cv2.normalize(hist_b, hist_b, 0, 255, cv2.NORM_MINMAX) cv2.normalize(hist_g, hist_g, 0, 255, cv2.NORM_MINMAX) cv2.normalize(hist_r, hist_r, 0, 255, cv2.NORM_MINMAX) track_window = (ci, r, w, h) r2, h2, ci2, w2 = top2, bottom2 - top2, left2, right2 - left2 # simply hardcoded the values r, h, c, w hist_bp = cv2.calcHist([frame_b[top2:bottom2, left2:right2]], [0], None, [256], [0, 256]) hist_gp = cv2.calcHist([frame_g[top2:bottom2, left2:right2]], [0], None, [256], [0, 256]) hist_rp = cv2.calcHist([frame_r[top2:bottom2, left2:right2]], [0], None, [256], [0, 256]) cv2.normalize(hist_bp, hist_bp, 0, 255, cv2.NORM_MINMAX) cv2.normalize(hist_gp, hist_gp, 0, 255, cv2.NORM_MINMAX) cv2.normalize(hist_rp, hist_rp, 0, 255, cv2.NORM_MINMAX) track_window2 = (ci2, r2, w2, h2) cv2.imwrite('bottledetect.jpg', frame[r:r + h, ci:ci + w]) cv2.imwrite('persondetect.jpg', frame[r2:r2 + h2, ci2:ci2 + w2]) # set up the ROI for tracking roi = frame[r:r + h, ci:ci + w] hsv_roi = cv2.cvtColor(roi, cv2.COLOR_BGR2HSV) mask = cv2.inRange(hsv_roi, np.array((0., 60., 32.)), np.array((180., 255., 255.))) roi_hist = cv2.calcHist([hsv_roi], [0], mask, [180], [0, 180]) cv2.normalize(roi_hist, roi_hist, 0, 255, cv2.NORM_MINMAX) # Setup the termination criteria, either 10 iteration or move by atleast 1 pt term_crit = (cv2.TERM_CRITERIA_EPS | cv2.TERM_CRITERIA_COUNT, 10, 1) ok = tracker.init(frame, track_window) ok2 = tracker2.init(frame, track_window2) track_thing = 0 #bottle pts = Point() pts2 = Point() untrack = 0 self.emergency_stop = 0 # flag = 0 # pub_flag.publish(flag) while (1): ret, frames, depth = vid.read() frame = frames[0] depth_frame = frames[1] if ret == True: hsv = cv2.cvtColor(frame, cv2.COLOR_BGR2HSV) dst = cv2.calcBackProject([hsv], [0], roi_hist, [0, 180], 1) # apply meanshift to get the new location ok, track_window = tracker.update(frame) x, y, w, h = track_window ok, track_window2 = tracker2.update(frame) x2, y2, w2, h2 = track_window2 # Draw it on image img2 = cv2.rectangle(frame, (x, y), (x + w, y + h), 255, 2) if not track_thing: img2 = cv2.rectangle(img2, (x2, y2), (x2 + w2, y2 + h2), 255, 2) else: img2 = cv2.rectangle(img2, (x2, y2), (x2 + w2, y2 + h2), (0, 0, 255), 2) # cv2.imshow('Tracking',img2) tracking_frame = bridge.cv2_to_imgmsg(img2, "bgr8") pub_frame2.publish(tracking_frame) # https://www.intelrealsense.com/wp-content/uploads/2020/06/Intel-RealSense-D400-Series-Datasheet-June-2020.pdf total, cnt = 0, 0 for i in range(3): for j in range(3): dep = depth.get_distance( np.maximum(0, np.minimum(i + x + w // 2, 639)), np.maximum(0, np.minimum(j + y + h // 2, 479))) if (dep) != 0: total += dep cnt += 1 if cnt != 0: worldz = total / cnt # 人にぶつからないように距離を確保するため if (worldz < 0.6) or (worldz > 3.0): worldz = 0 else: worldz = 0 total2, cnt2 = 0, 0 for i in range(3): for j in range(3): dep2 = depth.get_distance( np.maximum(0, np.minimum(i + x2 + w2 // 2, 639)), np.maximum(0, np.minimum(j + y2 + h2 // 2, 479))) if dep2 != 0: total2 += dep2 cnt2 += 1 if cnt2 != 0: worldz2 = total2 / cnt2 if (worldz2 < 0.6) or (worldz2 > 3.0): worldz2 = 0 else: worldz2 = 0 # print('worldz', worldz) # print('worldz2', worldz2) # if (worldz == 0) or (worldz2 == 0): if worldz2 == 0: # break worldx, worldy = 0, 0 # worldx = 0 pts.x, pts.y, pts.z = 0.0, 0.0, 0.0 worldx2, worldy2 = 0, 0 pts2.x, pts2.y, pts2.z = 0.0, 0.0, 0.0 else: # focus length = 1.93mm, distance between depth cameras = about 5cm, a pixel size = 3um if (track_thing == 0) and (not worldz == 0): #bottle Tracking u_ud = (0.05 * 1.88 * 10**(-3)) / (3 * 10**(-6) * worldz) # print('u_ud', u_ud) # print('x, y =', (x+w//2)-(img2.shape[1]//2), (img2.shape[0]//2)-(y+h//2)) # 深度カメラとカラーカメラの物理的な距離を考慮した項(-0.3*u_ud) # これらの座標は物体を見たときの左の深度カメラを基準とする worldx = 0.05 * (x + w // 2 - (img2.shape[1] // 2) - 0.3 * u_ud) / u_ud worldy = 0.05 * ((img2.shape[0] // 2) - (y + h)) / u_ud print('x,y,z = ', worldx, worldy, worldz - 0.6) pts.y, pts.z, pts.x = -1.0 * float(worldx), float( worldy), float(worldz) - 0.6 # pts.y, pts.z, pts.x = float(0.0), float(worldy), float(1.0) else: #human Tracking # if worldz==0: # worldx, worldy = 0, 0 # pts.x, pts.y, pts.z = 0.0, 0.0, 0.0 u_ud = (0.05 * 1.88 * 10**(-3)) / (3 * 10**(-6) * worldz2) worldx2 = 0.05 * (x2 + w2 // 2 - (img2.shape[1] // 2) - 0.3 * u_ud) / u_ud worldy2 = 0.05 * ((img2.shape[0] // 2) - (y2 + h2)) / u_ud print('x2,y2,z2 = ', worldx2, worldy2, worldz2 - 0.6) pts2.y, pts2.z, pts2.x = -1.0 * float( worldx2), float(worldy2), float(worldz2) - 0.6 # pts.y, pts.z, pts.x = float(0.0), float(worldy), float(1.0) if worldz == 0: worldx, worldy = 0, 0 pts.x, pts.y, pts.z = 0.0, 0.0, 0.0 print("track_thing = ", track_thing) frame_b, frame_g, frame_r = frame[:, :, 0], frame[:, :, 1], frame[:, :, 2] hist_b2 = cv2.calcHist([frame_b[y:y + h, x:x + w]], [0], None, [256], [0, 256]) hist_g2 = cv2.calcHist([frame_g[y:y + h, x:x + w]], [0], None, [256], [0, 256]) hist_r2 = cv2.calcHist([frame_r[y:y + h, x:x + w]], [0], None, [256], [0, 256]) cv2.normalize(hist_b2, hist_b2, 0, 255, cv2.NORM_MINMAX) cv2.normalize(hist_g2, hist_g2, 0, 255, cv2.NORM_MINMAX) cv2.normalize(hist_r2, hist_r2, 0, 255, cv2.NORM_MINMAX) hist_bp2 = cv2.calcHist([frame_b[y2:y2 + h2, x2:x2 + w2]], [0], None, [256], [0, 256]) hist_gp2 = cv2.calcHist([frame_g[y2:y2 + h2, x2:x2 + w2]], [0], None, [256], [0, 256]) hist_rp2 = cv2.calcHist([frame_r[y2:y2 + h2, x2:x2 + w2]], [0], None, [256], [0, 256]) cv2.normalize(hist_bp2, hist_bp2, 0, 255, cv2.NORM_MINMAX) cv2.normalize(hist_gp2, hist_gp2, 0, 255, cv2.NORM_MINMAX) cv2.normalize(hist_rp2, hist_rp2, 0, 255, cv2.NORM_MINMAX) comp_b = cv2.compareHist(hist_b, hist_b2, cv2.HISTCMP_CORREL) comp_g = cv2.compareHist(hist_g, hist_g2, cv2.HISTCMP_CORREL) comp_r = cv2.compareHist(hist_r, hist_r2, cv2.HISTCMP_CORREL) comp_bp = cv2.compareHist(hist_bp, hist_bp2, cv2.HISTCMP_CORREL) comp_gp = cv2.compareHist(hist_gp, hist_gp2, cv2.HISTCMP_CORREL) comp_rp = cv2.compareHist(hist_rp, hist_rp2, cv2.HISTCMP_CORREL) # print('compareHist(b)', comp_b) # print('compareHist(g)', comp_g) # print('compareHist(r)', comp_r) # print('compareHist(bp)', comp_bp) # print('compareHist(gp)', comp_gp) # print('compareHist(rp)', comp_rp) # print("garbage_in_can", garbage_in_can) # 追跡を止める条件は,bottle追跡中にヒストグラムが大きく変化するか枠が無くなるまたはpersonを見失う,もしくはperson追跡中にヒストグラムが大きく変化するか枠が無くなるまたはゴミがゴミ箱に入れられた, if ((track_thing == 0 and ((comp_b <= 0.1) or (comp_g <= 0.1) or (comp_r <= 0.1) or track_window == (0, 0, 0, 0))) or (track_window2 == (0, 0, 0, 0)) or (track_thing == 1 and ((comp_bp <= 0.) or (comp_gp <= 0.) or (comp_rp <= 0.)))): untrack += 1 print("untrack = ", untrack) if untrack >= 30: print("追跡が外れた!\n") break elif (track_thing == 0 and (x + w > 640 or x < 0) and (y + h > 480 or y < 0)) or (track_thing == 1 and (x2 + w2 > 640 or x2 < 0) and (y2 + h2 > 480 or y2 < 0)): untrack += 1 print("untrack = ", untrack) if untrack >= 50: print("枠が画面外で固まった") break # elif (track_thing==1 and self.garbage_in_can==1): elif self.garbage_in_can == 1: print("ゴミを捨てたため追跡を終えます") flag = 0 break else: untrack = 0 # print('garbage_in_can', self.garbage_in_can) # ポイ捨ての基準はbottleを追跡していて,地面から10cmのところまで落ちたか,bottleを見失ったかつ見失う前のフレームでの高さがカメラの10cmより下 # print('track_window = ', track_window) if (((worldy <= -0.01) or ((track_window == (0, 0, 0, 0) or untrack >= 1) and (worldy < 0.5))) and (not track_thing)): print("ポイ捨てした!\n") track_thing = 1 #human # if track_thing==0: # tracking_point = pts # if not (pts.x==0.0 and pts.y==0.0 and pts.z==0.0): # pub.publish(tracking_point) # flag = 0 #bottle # else: # tracking_point = pts2 # if not (pts2.x==0.0 and pts2.y==0.0 and pts2.z==0.0): # pub.publish(tracking_point) # flag = 1 #person if track_thing == 1: tracking_point = pts2 if not (pts2.x == 0.0 and pts2.y == 0.0 and pts2.z == 0.0): pub.publish(tracking_point) flag = 1 # else: # flag = 0 pub_flag.publish(flag) k = cv2.waitKey(1) & 0xff # print("emergency_stop", self.emergency_stop) # if (k == 27) or self.emergency_stop==1: # dev if self.emergency_stop == 1: # ops print('emergency_stop == 1') break else: break # pub_track.publish(1) # pub_flag.publish(flag) yolo.close_session()
class CPM(object): """ This class takes in image data and finds / annotates objects within the image """ def __init__(self): rospy.init_node('rail_pose_estimator_node') self.person_keypoints = [] self.keypoint_arrays = [] self.image_datastream = None self.input_image = None self.bridge = CvBridge() self.detector = pose_estimation.PoseMachine() self.debug = rospy.get_param('~debug', default=False) self.image_sub_topic_name = rospy.get_param( '~image_sub_topic_name', default='/camera/rgb/image_raw') self.use_compressed_image = rospy.get_param('~use_compressed_image', default=False) self.part_str = [ 'nose', 'neck', 'right_shoulder', 'right_elbow', 'right_wrist', 'left_shoulder', 'left_elbow', 'left_wrist', 'Rhip', 'Rkne', 'Rank', 'Lhip', 'Lkne', 'Lank', 'right_eye', 'left_eye', 'right_ear', 'left_ear' ] def _convert_msg_to_image(self, image_msg): """ Convert an incoming image message (compressed or otherwise) into a cv2 image """ if not self.use_compressed_image: try: image_cv = self.bridge.imgmsg_to_cv2(image_msg, "bgr8") except CvBridgeError as e: print e return None else: image_np = np.fromstring(image_msg.data, np.uint8) image_cv = cv2.imdecode(image_np, cv2.CV_LOAD_IMAGE_COLOR) return image_cv def _parse_image(self, image_msg): header = image_msg.header image_cv = self._convert_msg_to_image(image_msg) if image_cv is None: return # self.person_keypoints = self.detector.estimate_keypoints(image_cv) # candidate, subset = self.detector.estimate_keypoints(image_cv) people = self.detector.estimate_keypoints(image_cv) #### DEBUG #### if self.debug: # out_image = self.detector.visualize_keypoints(image_cv, candidate, subset) out_image = self.detector.visualize_keypoints(image_cv, people) try: image_msg = self.bridge.cv2_to_imgmsg(out_image, "bgr8") except CvBridgeError as e: print e image_msg.header = header self.image_pub.publish(image_msg) #### END DEBUG #### # Instantiate poses object obj_arr = Poses() obj_arr.header = header for person in people: msg = Keypoints() nose = person.get('nose', [-1, -1]) msg.nose_y = nose[0] msg.nose_x = nose[1] neck = person.get('neck', [-1, -1]) msg.neck_y = neck[0] msg.neck_x = neck[1] right_shoulder = person.get('right_shoulder', [-1, -1]) msg.right_shoulder_y = right_shoulder[0] msg.right_shoulder_x = right_shoulder[1] left_shoulder = person.get('left_shoulder', [-1, -1]) msg.left_shoulder_y = left_shoulder[0] msg.left_shoulder_x = left_shoulder[1] right_elbow = person.get('right_elbow', [-1, -1]) msg.right_elbow_y = right_elbow[0] msg.right_elbow_x = right_elbow[1] left_elbow = person.get('left_elbow', [-1, -1]) msg.left_elbow_y = left_elbow[0] msg.left_elbow_x = left_elbow[1] right_wrist = person.get('right_wrist', [-1, -1]) msg.right_wrist_y = right_wrist[0] msg.right_wrist_x = right_wrist[1] left_wrist = person.get('left_wrist', [-1, -1]) msg.left_wrist_y = left_wrist[0] msg.left_wrist_x = left_wrist[1] Lhip = person.get('Lhip', [-1, -1]) msg.left_hip_y = Lhip[0] msg.left_hip_x = Lhip[1] Rhip = person.get('Rhip', [-1, -1]) msg.right_hip_y = Rhip[0] msg.right_hip_x = Rhip[1] left_eye = person.get('left_eye', [-1, -1]) msg.left_eye_y = left_eye[0] msg.left_eye_x = left_eye[1] right_eye = person.get('right_eye', [-1, -1]) msg.right_eye_y = right_eye[0] msg.right_eye_x = right_eye[1] right_ear = person.get('right_ear', [-1, -1]) msg.right_ear_y = right_ear[0] msg.right_ear_x = right_ear[1] left_ear = person.get('left_ear', [-1, -1]) msg.left_ear_y = left_ear[0] msg.left_ear_x = left_ear[1] Rkne = person.get('Rkne', [-1, -1]) msg.right_knee_y = Rkne[0] msg.right_knee_x = Rkne[1] Lkne = person.get('Lkne', [-1, -1]) msg.left_knee_y = Lkne[0] msg.left_knee_x = Lkne[1] Rank = person.get('Rank', [-1, -1]) msg.right_ankle_y = Rank[0] msg.right_ankle_x = Rank[1] Lank = person.get('Lank', [-1, -1]) msg.left_ankle_y = Lank[0] msg.left_ankle_x = Lank[1] obj_arr.people.append(msg) self.object_pub.publish(obj_arr) def read_points(self, points): # print points pass def pcl_callback(self, pcl_msg): all_points = np.array([3]) print all_points for point in pc2.read_points(pcl_msg, skip_nans=True): pt_x = point[0] pt_y = point[1] pt_z = point[2] points = np.array([pt_x, pt_y, pt_z]) all_points = np.append(all_points, points) self.read_points(all_points) def run(self, pub_image_topic='~debug/poses_image', pub_object_topic='~poses'): if not self.use_compressed_image: rospy.Subscriber(self.image_sub_topic_name, Image, self._parse_image) rospy.Subscriber("/pcl_table", pc2.PointCloud2, self.pcl_callback, queue_size=1) else: rospy.Subscriber(self.image_sub_topic_name + '/compressed', CompressedImage, self._parse_image) if self.debug: self.image_pub = rospy.Publisher(pub_image_topic, Image, queue_size=2) # image publisher self.object_pub = rospy.Publisher(pub_object_topic, Poses, queue_size=2) # objects publisher rospy.spin()
class ImageListener: def __init__(self, object_list, cfg_list, checkpoint_list, codebook_list, modality, cad_model_dir, category='ycb', refine_single=True, refine_multiple=False, use_depth=True, use_self_supervised_ckpts=True): print(' *** Initializing PoseRBPF ROS Node ... ') # variables self.cv_bridge = CvBridge() self.count = 0 self.objects = [] self.frame_names = [] self.frame_lost = [] self.renders = dict() self.n_renders = 0 self.num_lost = 50 self.queue_size = 10 self.scene = 1 self.use_depth = use_depth self.posecnn_classes = ('__background__', '002_master_chef_can', '003_cracker_box', '004_sugar_box', '005_tomato_soup_can', \ '006_mustard_bottle', '007_tuna_fish_can', '008_pudding_box', '009_gelatin_box', '010_potted_meat_can', \ '011_banana', '019_pitcher_base', '021_bleach_cleanser', '024_bowl', '025_mug', '035_power_drill', \ '036_wood_block', '037_scissors', '040_large_marker', '052_extra_large_clamp', '061_foam_brick') # initialize poserbpf with cfg_file self.object_list = object_list self.cfg_list = cfg_list self.ckpt_list = checkpoint_list self.codebook_list = codebook_list self.pose_rbpf = PoseRBPF(self.object_list, self.cfg_list, self.ckpt_list, self.codebook_list, category, modality, cad_model_dir, refine=refine_single) self.ssv_ckpts = use_self_supervised_ckpts # initial sdf refinement for multiple objects at once self.refine_multiple = refine_multiple if refine_multiple: print('loading SDFs') sdf_files = [] for cls in self.object_list: sdf_file = '{}/ycb_models/{}/textured_simple_low_res.pth'.format( cad_model_dir, cls) sdf_files.append(sdf_file) reg_trans = 1000.0 reg_rot = 10.0 self.sdf_optimizer = sdf_multiple_optimizer( self.object_list, sdf_files, reg_trans, reg_rot) # target list self.target_list = range(len(self.pose_rbpf.obj_list)) self.target_object = cfg.TEST.OBJECTS[0] self.pose_rbpf.add_object_instance(self.target_object) self.target_cfg = self.pose_rbpf.set_target_obj(0) self.class_info = torch.ones((1, 1, 128, 128), dtype=torch.float32) self.init_failure_steps = 0 self.input_rgb = None self.input_depth = None self.input_seg = None self.input_rois = None self.input_stamp = None self.input_frame_id = None self.input_joint_states = None self.input_robot_joint_states = None self.main_thread_free = True self.kf_time_stamp = None # roi information self.prefix = '00_' self.target_frame = 'measured/base_link' # initialize a node rospy.init_node('poserbpf_image_listener') self.br = tf.TransformBroadcaster() self.listener = tf.TransformListener() self.pose_pub = rospy.Publisher('poserbpf_image', ROS_Image, queue_size=1) # target detection self.flag_detected = False # forward kinematics (base to camera link transformation) self.Tbr_now = np.eye(4, dtype=np.float32) self.Tbr_prev = np.eye(4, dtype=np.float32) self.Trc = np.load('./ros/extrinsics_D415.npy') self.Tbc_now = np.eye(4, dtype=np.float32) # dry run poserbpf to initialize the numba modules self.run_poserbpf_initial() print(' *** PoseRBPF Ready ... ') # subscriber for camera information msg = rospy.wait_for_message('/camera/color/camera_info', CameraInfo) K = np.array(msg.K).reshape(3, 3) self.intrinsic_matrix = K self.pose_rbpf.set_intrinsics(K, 640, 480) print('Intrinsics matrix : ') print(self.intrinsic_matrix) # subscriber for rgbd images and joint states rgb_sub = message_filters.Subscriber('/camera/color/image_raw', ROS_Image, queue_size=1) depth_sub = message_filters.Subscriber( '/camera/aligned_depth_to_color/image_raw', ROS_Image, queue_size=1) # subscriber for posecnn label label_sub = message_filters.Subscriber('/posecnn_label', ROS_Image, queue_size=1) queue_size = 10 slop_seconds = 0.2 ts = message_filters.ApproximateTimeSynchronizer( [rgb_sub, depth_sub, label_sub], queue_size, slop_seconds) ts.registerCallback(self.callback) def visualize_poses(self, rgb): image_rgb = rgb.astype(np.float32) / 255.0 Tco_list = [] object_initialized = [] for i in range(len(self.pose_rbpf.rbpf_list)): if self.pose_rbpf.rbpf_ok_list[i]: Tco = np.eye(4, dtype=np.float32) Tco[:3, :3] = self.pose_rbpf.rbpf_list[i].rot_bar Tco[:3, 3] = self.pose_rbpf.rbpf_list[i].trans_bar Tco_list.append(Tco.copy()) object_initialized.append( self.pose_rbpf.obj_list.index( self.pose_rbpf.instance_list[i])) image_est_render, _, _ = self.pose_rbpf.renderer.render_pose_multiple( self.intrinsic_matrix, Tco_list, object_initialized) image_est_disp = image_est_render[0].permute(1, 2, 0).cpu().numpy() image_disp = 0.4 * image_rgb + 0.6 * image_est_disp image_disp = np.clip(image_disp, 0, 1.0) return image_disp def run_poserbpf_initial(self): image_rgb = torch.zeros((640, 480, 3), dtype=torch.float32) image_depth = torch.zeros((640, 480, 1), dtype=torch.float32) roi = np.array([0, 0, 0, 0, 0, 0], dtype=np.float32) self.pose_rbpf.set_target_obj(0) self.pose_rbpf.pose_estimation_single(0, roi, image_rgb, image_depth, dry_run=True) # reset lists self.pose_rbpf.rbpf_list = [] self.pose_rbpf.rbpf_ok_list = [] self.pose_rbpf.instance_list = [] self.pose_rbpf.mope_Tbo_list = [] self.pose_rbpf.mope_pc_b_list = [] def query_posecnn_detection(self, classes): # detection information of the target object rois_est = np.zeros((0, 7), dtype=np.float32) # TODO look for multiple object instances max_objects = 5 for i in range(len(classes)): for object_id in range(max_objects): # check posecnn frame cls = classes[i] suffix_frame = '_%02d_roi' % (object_id) source_frame = 'posecnn/' + cls + suffix_frame try: # print('look for posecnn detection ' + source_frame) trans, rot = self.listener.lookupTransform( self.target_frame, source_frame, rospy.Time(0)) n = trans[0] secs = trans[1] now = rospy.Time.now() if abs(now.secs - secs) > 1.0: print 'posecnn pose for %s time out %f %f' % ( source_frame, now.secs, secs) continue roi = np.zeros((1, 7), dtype=np.float32) roi[0, 0] = 0 roi[0, 1] = i roi[0, 2] = rot[0] * n roi[0, 3] = rot[1] * n roi[0, 4] = rot[2] * n roi[0, 5] = rot[3] * n roi[0, 6] = trans[2] rois_est = np.concatenate((rois_est, roi), axis=0) print('find posecnn detection ' + source_frame) except: continue if rois_est.shape[0] > 0: # non-maximum suppression within class index = nms(rois_est, 0.2) rois_est = rois_est[index, :] return rois_est def callback(self, rgb, depth, label): # decode image if depth is not None: if depth.encoding == '32FC1': depth_cv = self.cv_bridge.imgmsg_to_cv2(depth) elif depth.encoding == '16UC1': depth_cv = self.cv_bridge.imgmsg_to_cv2(depth) else: rospy.logerr_throttle( 1, 'Unsupported depth type. Expected 16UC1 or 32FC1, got {}'. format(depth.encoding)) return else: depth_cv = None with lock: self.input_depth = depth_cv # rgb image used for posecnn detection self.input_rgb = self.cv_bridge.imgmsg_to_cv2(rgb, 'rgb8') self.input_seg = self.cv_bridge.imgmsg_to_cv2(label, 'mono8') # other information self.input_stamp = rgb.header.stamp self.input_frame_id = rgb.header.frame_id def process_data(self): # callback data with lock: input_stamp = self.input_stamp input_rgb = self.input_rgb.copy() input_depth = self.input_depth.copy() input_seg = self.input_seg.copy() # subscribe the transformation try: source_frame = 'measured/camera_color_optical_frame' target_frame = 'measured/base_link' trans, rot = self.listener.lookupTransform(target_frame, source_frame, input_stamp) Tbc = ros_qt_to_rt(rot, trans) self.Tbc_now = Tbc.copy() self.Tbr_now = Tbc.dot(np.linalg.inv(self.Trc)) if np.linalg.norm(self.Tbr_prev[:3, 3]) == 0: self.pose_rbpf.T_c1c0 = np.eye(4, dtype=np.float32) else: Tbc0 = np.matmul(self.Tbr_prev, self.Trc) Tbc1 = np.matmul(self.Tbr_now, self.Trc) self.pose_rbpf.T_c1c0 = np.matmul(np.linalg.inv(Tbc1), Tbc0) self.Tbr_prev = self.Tbr_now.copy() except: # print('missing forward kinematics info') return # call object detection ''' if len(self.pose_rbpf.obj_list) == 1: rois = np.array([[0, 0, 447, 167, 447, 167]], dtype=np.float32) else: rois = np.array([[0, 0, 332, 150, 332, 150], [0, 1, 480, 193, 480, 193], [0, 2, 407, 370, 407, 370]], dtype=np.float32) ''' rois = self.query_posecnn_detection(self.pose_rbpf.obj_list) rois = rois[:, :6] # call pose estimation function self.pose_estimation(input_rgb, input_depth, input_seg, rois) for idx_tf in range(len(self.pose_rbpf.rbpf_list)): if not self.pose_rbpf.rbpf_ok_list[idx_tf]: continue Tco = np.eye(4, dtype=np.float32) Tco[:3, :3] = self.pose_rbpf.rbpf_list[idx_tf].rot_bar Tco[:3, 3] = self.pose_rbpf.rbpf_list[idx_tf].trans_bar Tbo = self.Tbc_now.dot(Tco) # publish tf t_bo = Tbo[:3, 3] q_bo = mat2quat(Tbo[:3, :3]) instance_count = 0 for i in range(idx_tf): if self.pose_rbpf.instance_list[ i] == self.pose_rbpf.instance_list[idx_tf]: instance_count += 1 self.br.sendTransform( t_bo, [q_bo[1], q_bo[2], q_bo[3], q_bo[0]], self.input_stamp, 'poserbpf/{}_{}'.format(self.pose_rbpf.instance_list[idx_tf], instance_count), 'measured/base_link') # visualization image_disp = self.visualize_poses(input_rgb) * 255.0 image_disp = image_disp.astype(np.uint8) image_disp = np.clip(image_disp, 0, 255) pose_msg = self.cv_bridge.cv2_to_imgmsg(image_disp) pose_msg.header.stamp = self.input_stamp pose_msg.header.frame_id = self.input_frame_id pose_msg.encoding = 'rgb8' self.pose_pub.publish(pose_msg) def pose_estimation(self, rgb, depth, label, rois): image_rgb = torch.from_numpy(rgb).float() / 255.0 image_depth = torch.from_numpy(depth.astype( np.float32)).float() / 1000.0 im_depth = image_depth.cuda() image_depth = image_depth.unsqueeze(2) im_label = torch.from_numpy(label).cuda() if self.ssv_ckpts: image_input = image_rgb[:, :, [2, 1, 0]] else: image_input = image_rgb if not self.use_depth: image_depth = None # propagate particles for all the initialized objects for i in range(len(self.pose_rbpf.instance_list)): if self.pose_rbpf.rbpf_ok_list[i]: self.pose_rbpf.propagate_with_forward_kinematics(i) # collect rois from rbpfs rois_rbpf = np.zeros((0, 6), dtype=np.float32) index_rbpf = [] for i in range(len(self.pose_rbpf.instance_list)): if self.pose_rbpf.rbpf_ok_list[i]: roi = self.pose_rbpf.rbpf_list[i].roi rois_rbpf = np.concatenate((rois_rbpf, roi), axis=0) index_rbpf.append(i) self.pose_rbpf.rbpf_list[i].roi_assign = None # data association based on bounding box overlap num_rois = rois.shape[0] num_rbpfs = rois_rbpf.shape[0] assigned_rois = np.zeros((num_rois, ), dtype=np.int32) if num_rbpfs > 0 and num_rois > 0: # overlaps: (rois x gt_boxes) (batch_id, x1, y1, x2, y2) overlaps = bbox_overlaps( np.ascontiguousarray(rois_rbpf[:, (1, 2, 3, 4, 5)], dtype=np.float), np.ascontiguousarray(rois[:, (1, 2, 3, 4, 5)], dtype=np.float)) # assign rois to rbpfs assignment = overlaps.argmax(axis=1) max_overlaps = overlaps.max(axis=1) unassigned = [] for i in range(num_rbpfs): if max_overlaps[i] > 0.2: self.pose_rbpf.rbpf_list[index_rbpf[i]].roi_assign = rois[ assignment[i]] assigned_rois[assignment[i]] = 1 else: unassigned.append(i) # check if there are un-assigned rois index = np.where(assigned_rois == 0)[0] # if there is un-assigned rbpfs if len(unassigned) > 0 and len(index) > 0: for i in range(len(unassigned)): for j in range(len(index)): if assigned_rois[ index[j]] == 0 and self.pose_rbpf.rbpf_list[ index_rbpf[unassigned[i]]].roi[ 0, 1] == rois[index[j], 1]: self.pose_rbpf.rbpf_list[index_rbpf[ unassigned[i]]].roi_assign = rois[index[j]] assigned_rois[index[j]] = 1 elif num_rbpfs == 0 and num_rois == 0: return # filter tracked objects for i in range(len(self.pose_rbpf.instance_list)): if self.pose_rbpf.rbpf_ok_list[i]: roi = self.pose_rbpf.rbpf_list[i].roi_assign Tco, max_sim = self.pose_rbpf.pose_estimation_single( i, roi, image_input, image_depth, visualize=False) # initialize new object for i in range(num_rois): if assigned_rois[i]: continue roi = rois[i] obj_idx = int(roi[1]) target_obj = self.pose_rbpf.obj_list[obj_idx] add_new_instance = True for j in range(len(self.pose_rbpf.instance_list)): if self.pose_rbpf.instance_list[ j] == target_obj and self.pose_rbpf.rbpf_ok_list[ j] == False: add_new_instance = False Tco, max_sim = self.pose_rbpf.pose_estimation_single( j, roi, image_input, image_depth, visualize=False) if add_new_instance: self.pose_rbpf.add_object_instance(target_obj) Tco, max_sim = self.pose_rbpf.pose_estimation_single( len(self.pose_rbpf.instance_list) - 1, roi, image_input, image_depth, visualize=False) # SDF refinement for multiple objects if self.refine_multiple and self.use_depth: # backproject depth fx = self.intrinsic_matrix[0, 0] fy = self.intrinsic_matrix[1, 1] px = self.intrinsic_matrix[0, 2] py = self.intrinsic_matrix[1, 2] im_pcloud = posecnn_cuda.backproject_forward( fx, fy, px, py, im_depth)[0] index_sdf = [] for i in range(len(self.pose_rbpf.instance_list)): if self.pose_rbpf.rbpf_ok_list[i]: index_sdf.append(i) if len(index_sdf) > 0: self.pose_rbpf.pose_refine_multiple(self.sdf_optimizer, self.posecnn_classes, index_sdf, im_depth, im_pcloud, im_label, steps=50)
# Se define el dispositivo aa utilizar cap = cv.VideoCapture(1) #La funcion obtiene la imagen proveniente de la camara def get_Image(): ret, img = cap.read() array = cv.cvtColor(img, cv.COLOR_BGR2GRAY) return img #Configuracion de nodo y publicador rospy.init_node('camara') pub = rospy.Publisher('rgb/image', Image, queue_size=100) bridge = CvBridge() rate = rospy.Rate(10) while not rospy.is_shutdown(): try: frame = get_Image() cv.imshow('frame', frame) cv.waitKey(1) frame = np.uint8(frame) #e convierte la imagen a un formato soportado por ROS image_msg = bridge.cv2_to_imgmsg(frame, encoding="bgr8") pub.publish(image_msg) rate.sleep() except TypeError: continue
class OpenPosePreviewNode(Node): openpose_wrapper: OpenPoseWrapper def __init__(self): super().__init__('openpose_node') # ros params is_debug_mode_descriptor = ParameterDescriptor( type=ParameterType.PARAMETER_BOOL, description='If true, run debug mode.') self.declare_parameter('is_debug_mode', False, is_debug_mode_descriptor) self.is_debug_mode: bool = self.get_parameter( "is_debug_mode").get_parameter_value().bool_value openpose_descriptor = ParameterDescriptor( type=ParameterType.PARAMETER_STRING, description='The path of openpose project root.') self.declare_parameter('openpose_root', '/openpose', openpose_descriptor) openpose_root: str = self.get_parameter( "openpose_root").get_parameter_value().string_value is_image_compressed_descriptor = ParameterDescriptor( type=ParameterType.PARAMETER_BOOL, description='Is input image compressed?') self.declare_parameter('is_image_compressed', False, is_image_compressed_descriptor) is_image_compressed: bool = self.get_parameter( "is_image_compressed").get_parameter_value().bool_value image_node_descriptor = ParameterDescriptor( type=ParameterType.PARAMETER_STRING, description='The node name of input image.') self.declare_parameter('image_node', '/image', image_node_descriptor) image_node: str = self.get_parameter( "image_node").get_parameter_value().string_value self.openpose_wrapper = OpenPoseWrapper(openpose_root) self.bridge = CvBridge() # show info self.get_logger().info('IsDebugMode : ' + str(self.is_debug_mode)) self.get_logger().info('OpenposeRoot : ' + openpose_root) self.get_logger().info('ImageNode : ' + image_node) self.get_logger().info('IsImageCompressed : ' + str(is_image_compressed)) if self.is_debug_mode: self._publisher = self.create_publisher(Image, '/openpose/preview', 10) self._publisher_compressed = self.create_publisher( CompressedImage, '/openpose/preview/compressed', 10) self._pose_publisher = self.create_publisher( PoseKeyPointsList, '/openpose/pose_key_points', 10) if is_image_compressed: self.subscription = self.create_subscription( CompressedImage, image_node, self.get_img_compressed_callback, 10) else: self.subscription = self.create_subscription( Image, image_node, self.get_img_callback, 10) def publish_from_img(self, img: np.ndarray, timestamp: Time, frame_id: str = ""): result = self.openpose_wrapper.body_from_image(img) if self.is_debug_mode: result_image: Image = self.bridge.cv2_to_imgmsg( result.cvOutputData, "rgb8") result_image_compressed: CompressedImage = self.bridge.cv2_to_compressed_imgmsg( result.cvOutputData) result_image.header.stamp = timestamp result_image.header.frame_id = frame_id result_image_compressed.header.stamp = timestamp result_image_compressed.header.frame_id = frame_id self._publisher.publish(result_image) self._publisher_compressed.publish(result_image_compressed) # Convert to KeyPointsList pose_key_points_list_obj = PoseKeyPointsList() if isinstance(result.poseKeypoints, np.ndarray): pose_key_points_list = [] for result_pose_key_points in result.poseKeypoints: pose_key_points = [] for result_pose_key_point in result_pose_key_points: x, y, score = result_pose_key_point pose_key_points.append( PoseKeyPoint(x=x.item(), y=y.item(), score=score.item())) pose_key_points_list.append( PoseKeyPoints(pose_key_points=pose_key_points)) pose_key_points_list_obj = PoseKeyPointsList( pose_key_points_list=pose_key_points_list) pose_key_points_list_obj.header.stamp = timestamp pose_key_points_list_obj.header.frame_id = frame_id self._pose_publisher.publish(pose_key_points_list_obj) def get_img_callback(self, image_raw: Image) -> None: try: print('[' + str(datetime.datetime.now()) + '] Image received', end='\r') image: np.ndarray = self.bridge.imgmsg_to_cv2(image_raw) self.publish_from_img(image, image_raw.header.stamp, image_raw.header.frame_id) except Exception as err: self.get_logger().error(err) def get_img_compressed_callback(self, image_raw: CompressedImage) -> None: try: print('[' + str(datetime.datetime.now()) + '] Compressed image received', end='\r') image: np.ndarray = self.bridge.compressed_imgmsg_to_cv2(image_raw) self.publish_from_img(image, image_raw.header.stamp, image_raw.header.frame_id) except Exception as err: self.get_logger().error(err)
class SSDPrediction(object): def __init__(self): self.net = build_ssd('test', 300, 2) self.bridge = CvBridge() self.image1 = None self.image2 = None self.image_to_plot = None self.rospack = rospkg.RosPack() pkg_path = self.rospack.get_path('visual_system') self.model_name = rospy.get_param("~model_name", "ssd300_BARCODE_21500.pth") rospy.loginfo("model: %s" % (self.model_name)) # Only considered as target when confidence greater than self.confidence_thres = rospy.get_param("~confidence_thres", 0.5) rospy.loginfo("confidence_threshold: %f" % (self.confidence_thres)) self.model_path = pkg_path + "/model/" + self.model_name self.net.load_weights(self.model_path) if torch.cuda.is_available(): self.net.cuda() self.sub_img1 = rospy.Subscriber("/camera1/color/image_raw", Image, self.img1_cb) self.sub_img2 = rospy.Subscriber("/camera2/color/image_raw", Image, self.img2_cb) self.pub_img_with_bb = rospy.Publisher("~prediction_res", Image, queue_size=10) self.detection_service = rospy.Service("~barcode_detection", barcode_detect, self.service_cb) rospy.loginfo("Finish loading weights...") # Subscriber callback for camera 1, save image in self.image1 def img1_cb(self, msg): try: self.image1 = self.bridge.imgmsg_to_cv2(msg, "rgb8") # In RGB except CvBridgeError as e: print(e) # Subscriber callback for camera 2, save image in self.image2 def img2_cb(self, msg): try: self.image2 = self.bridge.imgmsg_to_cv2(msg, "rgb8") # In RGB except CvBridgeError as e: print(e) # Service callback def service_cb(self, req): res = barcode_detectResponse() res.result = False if (req.camera_id): # camera1 if (self.image1 is not None): self.img_to_net = self.img_preprocessing(self.image1) self.image_to_plot = self.image1 else: rospy.logerr("No image receive from camera 1 yet, aborting...") return res else: # camera 2 if (self.image2 is not None): self.img_to_net = self.img_preprocessing(self.image2) self.image_to_plot = self.image2 else: rospy.logerr("No image receive from camera 2 yet, aborting...") return res var = Variable(self.img_to_net.unsqueeze(0)) # Wrap tensor in Variable if torch.cuda.is_available(): var = var.cuda() prediction_res = self.net(var) # Forward pass detection = prediction_res.data scale = torch.Tensor(self.image_to_plot.shape[1::-1]).repeat(2) for i in range(detection.size(1)): j = 0 while detection[0, i, j, 0].cpu().numpy() >= self.confidence_thres: res.result = True score = detection[0, i, j, 0] display_txt = '%d' % (score * 100.) pt = (detection[0, i, j, 1:] * scale).cpu().numpy() pt = [int(p) for p in pt] # Draw rectangle cv2.rectangle(self.image_to_plot, (pt[0], pt[1]), (pt[2], pt[3]), \ (0, 0, 255), 3) # Put text font = cv2.FONT_HERSHEY_SIMPLEX cv2.putText(self.image_to_plot, display_txt, (pt[0], pt[1]), font, \ 3, (255, 0, 0), cv2.LINE_AA) j += 1 if (res.result == True): rospy.loginfo("Detect barcode") else: rospy.loginfo("No barcode detected") # Publish drawed image try: self.image_to_plot = cv2.cvtColor(self.image_to_plot, cv2.COLOR_BGR2RGB) self.pub_img_with_bb.publish( self.bridge.cv2_to_imgmsg(self.image_to_plot)) except CvBridgeError as e: print(e) return res # Preprocessing for SSD def img_preprocessing(self, img): res = cv2.resize(img, (300, 300)).astype(np.float32) res -= (104., 117., 123.) res = res.astype(np.float32) res = res[:, :, ::-1].copy() res = torch.from_numpy(res).permute(2, 0, 1) return res