def image_callback(self, img): try: cv_image = CvBridge().imgmsg_to_cv2(img, "bgr8") pil_image = Image.fromarray( cv2.cvtColor(cv_image, cv2.COLOR_BGR2RGB)) width, height = pil_image.size resize_ratio = 1.0 * self.input_size / max(width, height) target_size = (int(resize_ratio * width), int(resize_ratio * height)) resized_image = pil_image.convert('RGB').resize( target_size, Image.ANTIALIAS) input_image = resized_image.to(self.device) with torch.no_grad(): output = self.model(input_image) seg_image = np.squeeze(output.data.max(1)[1].cpu().numpy(), axis=0) seg_image = decode_segmap(self.num_classes, seg_image) cv_seg_image = np.asarray(seg_image) pub_seg_image = CvBridge().cv2_to_imgmsg(cv_seg_image, "rgb8") pub_seg_image.header = img.header if not self.use_subscribed_images_stamp: pub_seg_image.header.stamp = rospy.get_rostime() self.segmented_image_pub.publish(pub_seg_image) except CvBridgeError as e: print(e)
def callback(data): print(rospy.get_name(), "I heard %s" % str(data.data)) img_raveled = data.data[0:-2] img_size = data.data[-2:].astype(int) img = np.float32(np.reshape(img_raveled, (img_size[0], img_size[1]))) #img = np.float32((np.reshape(data.data, (DEPTH_IMG_WIDTH, DEPTH_IMG_HEIGHT)))) h = std_msgs.msg.Header() h.stamp = rospy.Time.now() image_message = CvBridge().cv2_to_imgmsg(img, encoding="passthrough") image_message.header = h pub.publish(image_message) camera_info_msg = CameraInfo() camera_info_msg.header = h fx, fy = DEPTH_IMG_WIDTH / 2, DEPTH_IMG_HEIGHT / 2 cx, cy = DEPTH_IMG_WIDTH / 2, DEPTH_IMG_HEIGHT / 2 camera_info_msg.width = DEPTH_IMG_WIDTH camera_info_msg.height = DEPTH_IMG_HEIGHT camera_info_msg.distortion_model = "plumb_bob" camera_info_msg.K = np.float32([fx, 0, cx, 0, fy, cy, 0, 0, 1]) camera_info_msg.D = np.float32([0, 0, 0, 0, 0]) camera_info_msg.P = [fx, 0, cx, 0, 0, fy, cy, 0, 0, 0, 1, 0] camera_info_pub.publish(camera_info_msg)
def load_and_publish_images(self, annotation_filename): try: f = open(annotation_filename) annotation = json.loads(f.read()) ids = annotation['class_id'] bboxes = annotation['bbox'] image = cv2.imread( self._data_dir + '/Annotations/' + annotation['img_path'], cv2.IMREAD_UNCHANGED) for id, bbox in zip(ids, bboxes): self.draw_bbox(image, id, bbox) imgmsg = CvBridge().cv2_to_imgmsg(image, encoding='passthrough') depth = cv2.imread( self._data_dir + '/Annotations/' + annotation['depth_path'], cv2.IMREAD_UNCHANGED) dptmsg = CvBridge().cv2_to_imgmsg(depth, encoding='passthrough') except Exception as e: rospy.logerr('%s(%s)', str(e), annotation_filename) return for bbox in bboxes: self._dfilter.roi = bbox now = rospy.Time.now() self._cinfo.header.stamp = now imgmsg.header = self._cinfo.header dptmsg.header = self._cinfo.header self._cinfo_pub.publish(self._cinfo) self._image_pub.publish(imgmsg) self._depth_pub.publish(dptmsg) key = raw_input('Hit [p|q|return] >> ') if key == 'p': self._dfilter.detect_plane_send_goal() plane = self._dfilter.detect_plane_wait_for_result() print(plane) elif key == 'q': sys.exit()
def load_and_localize(self, annotation_filename): self._spawner.delete_all() try: f = open(annotation_filename) annotation = json.loads(f.read()) ids = annotation['class_id'] bboxes = annotation['bbox'] image = cv2.imread( self._data_dir + '/Annotations/' + annotation['img_path'], cv2.IMREAD_UNCHANGED) for id, bbox in zip(ids, bboxes): self.draw_bbox(image, id, bbox) imsg = CvBridge().cv2_to_imgmsg(image, encoding='passthrough') depth = cv2.imread( self._data_dir + '/Annotations/' + annotation['depth_path'], cv2.IMREAD_UNCHANGED) dmsg = CvBridge().cv2_to_imgmsg(depth, encoding='passthrough') except Exception as e: rospy.logerr('(Feeder) %s(%s)', str(e), annotation_filename) return for id, bbox in zip(ids, bboxes): self._dfilter.roi = bbox now = rospy.Time.now() self._cinfo.header.stamp = now imsg.header = self._cinfo.header dmsg.header = self._cinfo.header self._cinfo_pub.publish(self._cinfo) self._image_pub.publish(imsg) self._depth_pub.publish(dmsg) rospy.loginfo('*** (Feeder) --------------') rospy.loginfo('*** (Feeder) localize id=%d', id + 1) self.localize(ImageFeeder._Models[id])
def image_callback(self, img: Image): t_start = time.time() if not self.camera.ready( ) or not self.trajectory_complete or not self.goal_post_need: return self.camera.reset_position(publish_basecamera=False, timestamp=img.header.stamp) image = CvBridge().imgmsg_to_cv2(img, desired_encoding="rgb8") hsv = cv2.cvtColor(src=image, code=cv2.COLOR_BGR2HSV) h = self.camera.calculateHorizonCoverArea() cv2.rectangle(image, [0, 0], [640, int(h * 7 / 10.0)], [0, 0, 0], cv2.FILLED) # Field line detection mask2 = cv2.inRange(hsv, (0, 0, 255 - 65), (255, 65, 255)) out = cv2.bitwise_and(image, image, mask=mask2) kernel = np.ones((7, 7), np.uint8) out = cv2.morphologyEx(out, cv2.MORPH_CLOSE, kernel) cdst = cv2.cvtColor(out, cv2.COLOR_BGR2GRAY) retval, dst = cv2.threshold(cdst, 127, 255, cv2.THRESH_BINARY) edges = cv2.Canny(dst, 50, 150) lines = cv2.HoughLinesP( edges, rho=DetectorFieldline.HOUGH_RHO, theta=DetectorFieldline.HOUGH_THETA, threshold=DetectorFieldline.HOUGH_THRESHOLD, minLineLength=DetectorFieldline.HOUGH_MIN_LINE_LENGTH, maxLineGap=DetectorFieldline.HOUGH_MAX_LINE_GAP) ccdst = cv2.cvtColor(cdst, cv2.COLOR_GRAY2RGB) if lines is None: return computed_lines = [] vert_x_max, vert_x_min, vert_y_max, vert_y_min = 0, 1000, 0, 1000 for l in lines: x1, y1, x2, y2 = l[0] # computing magnitude and angle of the line mag = np.sqrt((x2 - x1)**2. + (y2 - y1)**2.) if x2 == x1: angle = 0 else: angle = np.rad2deg(np.arctan((y2 - y1) / (x2 - x1))) computed_lines += [(mag, angle)] pt1 = (x1, y1) pt2 = (x2, y2) if abs(angle) < 10: # Horizontal cv2.line(ccdst, pt1, pt2, (255, 0, 0), thickness=3, lineType=cv2.LINE_AA) elif abs(abs(angle) - 90) < 10: # Vertical cv2.line(ccdst, pt1, pt2, (0, 255, 0), thickness=3, lineType=cv2.LINE_AA) vert_x_max = max(vert_x_max, x1, x2) vert_x_min = min(vert_x_min, x1, x2) vert_y_max = max(vert_y_max, y1, y2) vert_y_min = min(vert_y_min, y1, y2) else: cv2.line(ccdst, pt1, pt2, (0, 0, 255), thickness=3, lineType=cv2.LINE_AA) computed_lines = np.array(computed_lines) # an image has a goalpost if two perpendicular lines with 0 degrees and 90 degrees intersect vertical_line = len( computed_lines[(abs(abs(computed_lines[:, 1]) - 90) < 10)]) > 0 if vertical_line: w = vert_y_max - vert_y_min l = vert_x_max - vert_x_min area = w * l if area < 50000: cv2.rectangle(ccdst, [vert_x_min, vert_y_min], [vert_x_max, vert_y_max], [0, 255, 255], thickness=2) x_avg = (vert_x_max + vert_x_min) / 2 [floor_center_x, floor_center_y, _] = self.camera.findFloorCoordinate([x_avg, vert_y_max]) [floor_close_x, floor_close_y, _] = self.camera.findFloorCoordinate([x_avg, vert_y_max]) camera_pose = self.camera.pose distance = ( (floor_center_x - camera_pose.get_position()[0])**2 + (floor_center_y - camera_pose.get_position()[1])**2)**0.5 theta = math.atan2(distance, camera_pose.get_position()[2]) ratio = math.tan(theta)**2 ratio2 = 1 / (1 + ratio) if 1 < ratio2 < 0: print('here') # TODO floor_x = floor_close_x * (1 - ratio2) + floor_center_x * ratio2 floor_y = floor_close_y * (1 - ratio2) + floor_center_y * ratio2 if floor_x > 0.0: br = tf2_ros.TransformBroadcaster() robot_pose = TransformStamped() robot_pose.header.frame_id = self.robot_name + "/base_camera" robot_pose.child_frame_id = self.robot_name + "/goal_post" robot_pose.header.stamp = img.header.stamp robot_pose.header.seq = img.header.seq robot_pose.transform.translation.x = floor_x robot_pose.transform.translation.y = floor_y robot_pose.transform.translation.z = 0 robot_pose.transform.rotation.x = 0 robot_pose.transform.rotation.y = 0 robot_pose.transform.rotation.z = 0 robot_pose.transform.rotation.w = 1 br.sendTransform(robot_pose) if self.image_publisher.get_num_connections() > 0: img_out = CvBridge().cv2_to_imgmsg(ccdst) img_out.header = img.header self.image_publisher.publish(img_out) t_end = time.time() rospy.loginfo_throttle( 20, "GoalPost detection rate: " + str(t_end - t_start))
def image_callback(self, img: Image): rospy.loginfo_once("Recieved Message") t_start = time.time() if not self.camera.ready() or not self.trajectory_complete: return pts = [] self.camera.reset_position(publish_basecamera=True, timestamp=img.header.stamp) image = CvBridge().imgmsg_to_cv2(img, desired_encoding="rgb8") hsv = cv2.cvtColor(src=image, code=cv2.COLOR_BGR2HSV) h = self.camera.calculateHorizonCoverArea() cv2.rectangle(image, [0, 0], [640, h], [0, 0, 0], cv2.FILLED) # Field line detection mask2 = cv2.inRange(hsv, (0, 0, 255 - 65), (255, 65, 255)) out = cv2.bitwise_and(image, image, mask=mask2) kernel = np.ones((7, 7), np.uint8) out = cv2.morphologyEx(out, cv2.MORPH_CLOSE, kernel) cdst = cv2.cvtColor(out, cv2.COLOR_BGR2GRAY) retval, dst = cv2.threshold(cdst, 127, 255, cv2.THRESH_BINARY) edges = cv2.Canny(dst, 50, 150) lines = cv2.HoughLinesP(edges, rho=DetectorFieldline.HOUGH_RHO, theta=DetectorFieldline.HOUGH_THETA, threshold=DetectorFieldline.HOUGH_THRESHOLD, minLineLength=DetectorFieldline.HOUGH_MIN_LINE_LENGTH, maxLineGap=DetectorFieldline.HOUGH_MAX_LINE_GAP) ccdst = cv2.cvtColor(cdst, cv2.COLOR_GRAY2RGB) if lines is None: return for l in lines: x1, y1, x2, y2 = l[0] # computing magnitude and angle of the line if x2 == x1: angle = 0 else: angle = np.rad2deg(np.arctan((y2 - y1) / (x2 - x1))) pt1 = (x1, y1) pt2 = (x2, y2) if abs(angle) < 10: # Horizontal cv2.line(ccdst, pt1, pt2, (255, 0, 0), thickness=3, lineType=cv2.LINE_AA) elif abs(abs(angle) - 90) < 10: # Vertical cv2.line(ccdst, pt1, pt2, (0, 255, 0), thickness=3, lineType=cv2.LINE_AA) else: cv2.line(ccdst, pt1, pt2, (0, 0, 255), thickness=3, lineType=cv2.LINE_AA) if (pt2[0] - pt1[0]) == 0: continue slope = (pt2[1] - pt1[1]) / (pt2[0] - pt1[0]) b = y1 - slope * x1 for i in range(x1, x2, 15): y = slope * i + b pt = [i, y] pts.append(pt) if self.image_publisher.get_num_connections() > 0: img_out = CvBridge().cv2_to_imgmsg(ccdst) img_out.header = img.header self.image_publisher.publish(img_out) if self.point_cloud_publisher.get_num_connections() > 0: points3d = [] for p in pts: points3d.append(self.camera.findFloorCoordinate(p)) # Publish fieldlines in laserscan format header = Header() header.stamp = img.header.stamp header.frame_id = self.robot_name + "/base_camera" point_cloud_msg = pcl2.create_cloud_xyz32(header, points3d) if self.point_cloud_publisher.get_num_connections() > 0: self.point_cloud_publisher.publish(point_cloud_msg) t_end = time.time() rospy.loginfo_throttle(20, "Fieldline detection rate: " + str(t_end - t_start))