def ball_candidates_msg(self, xs, ys, ds, cs): msg = BallsInImage() msg.header.stamp = rospy.Time.now() i = 0 cans = [] for x in xs: cans.append(self.ball_img_msg(xs[i], ys[i], ds[i], cs[i])) i += 1 msg.candidates = cans return msg
def work(self): img = self.last_imgs.pop(self.latest_image_id, None) if img is None: print(str(self.latest_image_id) + " was late") return ra = self.bridge.imgmsg_to_cv2(img, "bgr8") quality = [] for i in self.candidates: x_b = max(i[1] - i[2] - 3, 0) x_e = min(i[1] + i[2] + 3, ra.shape[0]) y_b = max(i[0] - i[2] - 3, 0) y_e = min(i[0] + i[2] + 3, ra.shape[1]) corp = ra[x_b:x_e, y_b:y_e] corp = cv2.resize(corp, (30, 30), interpolation=cv2.INTER_CUBIC) corp.reshape((1,) + corp.shape) p = self.model.predict(np.array([corp]), verbose=0) msg = BallInImage() msg.center.x = i[0] msg.center.y = i[1] msg.diameter = i[2] * 2 msg.confidence = p[0][0] msg.header.frame_id = img.header.frame_id msg.header.stamp = img.header.stamp quality.append(msg) print(p[0][0]) if len(quality) > 0: self.pub_ball.publish(max(quality, key=lambda x: x.confidence)) rb = BallsInImage() rb.candidates = quality rb.header.frame_id = img.header.frame_id rb.header.stamp = img.header.stamp self.pub_rated_ball.publish(rb) else: b = BallInImage() b.header.frame_id = img.header.frame_id b.header.stamp = img.header.stamp self.pub_ball.publish(b) bs = BallsInImage() bs.header.frame_id = img.header.frame_id bs.header.stamp = img.header.stamp self.pub_rated_ball.publish(bs)
def run(): pub_ball = rospy.Publisher("ball_in_image", BallsInImage, queue_size=1) pub_hmg = rospy.Publisher("head_motor_goals", JointTrajectory, queue_size=1) hmg = JointTrajectory() goal = JointTrajectoryPoint() goal.positions = [0, 0] goal.velocities = [0, 0] hmg.points = [goal] counter = 320 direction = 1 rospy.loginfo("Create Test") rospy.init_node("bitbots_testHeadBehaviour") pub_hmg.publish(hmg) rate = rospy.Rate(4) rospy.logdebug("Laeuft...") while not rospy.is_shutdown(): # Ball in Image ball = BallInImage() ball.center.x = counter if (counter > 340 or counter < 300): direction *= -1 counter += direction else: counter += direction ball.center.y = 200 ball.diameter = 10 ball.confidence = 1 balls = BallsInImage() balls.candidates.append(ball) pub_ball.publish(balls) rospy.loginfo("Published ball: %s" % counter) rate.sleep()
def work(self, img): """ @param img: @type img: Image @return: """ ra = self.bridge.imgmsg_to_cv2(img, "bgr8") # bimg = cv2.bilateralFilter(ra, 14, 100, 100) bimg = cv2.GaussianBlur(ra, (9, 9), 0) mask = cv2.inRange(bimg, self.green_min, self.green_max) maskb = cv2.GaussianBlur(mask, (9, 9), 0) # mask = cv2.erode(mask, None, iterations = 2) # mask = 255 - mask # Horizion horizonbase = np.zeros((ra.shape[0] / 30, ra.shape[1] / 30)) for x in range(ra.shape[0] / 30): for y in range(ra.shape[1] / 30): horizonbase[x, y] = self.check_img(mask[x * 30:(x + 1) * 30, y * 30:(y + 1) * 30]) horizon = [] for col in range(horizonbase.shape[1]): hl = list(horizonbase[:, col]) if 1 in hl: horizon.append(hl.index(1) * 30) else: horizon.append(len(hl) * 30) #b, g, r = cv2.split(bimg) circles = cv2.HoughCircles(maskb, cv2.HOUGH_GRADIENT, 1, 100, param1=50, param2=43, minRadius=15, maxRadius=200) # Ball msg = BallsInImage() msg.header.frame_id = img.header.frame_id msg.header.stamp = img.header.stamp if circles is not None: circles = np.uint16(np.around(circles)) for i in circles[0, :]: if not self.under_horizon(horizon, (i[1], i[0])): cv2.circle(bimg, (i[0], i[1]), i[2], (0, 0, 255)) continue # corp = ra[i[1] - i[2]:i[1] + i[2], i[0] - i[2]:i[0] + i[2]] can = BallInImage() cv2.circle(bimg, (i[0], i[1]), i[2], (255, 0, 0)) can.center.x = i[0] can.center.y = i[1] can.diameter = (i[2] * 2) + 3 can.header.frame_id = img.header.frame_id can.header.stamp = img.header.stamp msg.candidates.append(can) # Linepoints li = LineInformationInImage() li.header.frame_id = img.header.frame_id li.header.stamp = img.header.stamp for x in range(1000): p = randint(0, bimg.shape[0] - 1), randint(0, bimg.shape[1] - 31) if self.under_horizon(horizon, p): if mask[p[0], p[1]] == 0 and sum(bimg[p[0], p[1]]) > 400: is_ball = False if circles is not None: for b in circles[0, :]: if math.sqrt((b[0] - p[1])**2 + (b[1] - p[0])**2) < b[2] + 15: is_ball = True if is_ball: continue ls = LineSegmentInImage() ls.start.x = p[1] ls.start.y = p[0] ls.end = ls.start li.segments.append(ls) cv2.circle(bimg, (p[1], p[0]), 1, (0, 0, 255)) # Plotting for x in range(len(horizon) - 1): cv2.line(bimg, (30 * x + 15, horizon[x]), (30 * (x + 1) + 15, horizon[x + 1]), color=(0, 255, 0)) cv2.imshow("Image", bimg) cv2.imshow("Mask", mask) cv2.waitKey(1) self.pub_lines.publish(li) self.pub_balls.publish(msg)
def handle_image(self, image_msg): # converting the ROS image message to CV2-image image = self.bridge.imgmsg_to_cv2(image_msg, 'bgr8') # setup detectors self.horizon_detector.set_image(image) self.obstacle_detector.set_image(image) self.line_detector.set_image(image) if (self.config['vision_ball_classifier'] == 'cascade'): self.ball_finder.set_image(image) self.ball_detector.set_image( image, self.horizon_detector.balls_under_horizon( self.ball_finder.get_ball_candidates(), self._ball_candidate_y_offset)) elif (self.config['vision_ball_classifier'] == 'fcnn'): self.ball_detector.set_image(image) top_ball_candidate = self.ball_detector.get_top_candidate() # create ball msg if top_ball_candidate and top_ball_candidate.rating > self._ball_candidate_threshold: balls_msg = BallsInImage() balls_msg.header.frame_id = image_msg.header.frame_id balls_msg.header.stamp = image_msg.header.stamp ball_msg = BallInImage() ball_msg.center.x = top_ball_candidate.get_center_x() ball_msg.center.y = top_ball_candidate.get_center_y() ball_msg.diameter = top_ball_candidate.get_diameter() ball_msg.confidence = 1 balls_msg.candidates.append(ball_msg) rospy.loginfo('found a ball! \o/') self.pub_balls.publish(balls_msg) # create obstacle msg obstacles_msg = ObstaclesInImage() obstacles_msg.header.frame_id = image_msg.header.frame_id obstacles_msg.header.stamp = image_msg.header.stamp for red_obs in self.obstacle_detector.get_red_obstacles(): obstacle_msg = ObstacleInImage() obstacle_msg.color = ObstacleInImage.ROBOT_MAGENTA obstacle_msg.top_left.x = red_obs.get_upper_left_x() obstacle_msg.top_left.y = red_obs.get_upper_left_y() obstacle_msg.height = red_obs.get_height() obstacle_msg.width = red_obs.get_width() obstacle_msg.confidence = 1.0 obstacle_msg.playerNumber = 42 obstacles_msg.obstacles.append(obstacle_msg) for blue_obs in self.obstacle_detector.get_blue_obstacles(): obstacle_msg = ObstacleInImage() obstacle_msg.color = ObstacleInImage.ROBOT_CYAN obstacle_msg.top_left.x = blue_obs.get_upper_left_x() obstacle_msg.top_left.y = blue_obs.get_upper_left_y() obstacle_msg.height = blue_obs.get_height() obstacle_msg.width = blue_obs.get_width() obstacle_msg.confidence = 1.0 obstacle_msg.playerNumber = 42 obstacles_msg.obstacles.append(obstacle_msg) for other_obs in self.obstacle_detector.get_other_obstacles(): obstacle_msg = ObstacleInImage() obstacle_msg.color = ObstacleInImage.UNDEFINED obstacle_msg.top_left.x = other_obs.get_upper_left_x() obstacle_msg.top_left.y = other_obs.get_upper_left_y() obstacle_msg.height = other_obs.get_height() obstacle_msg.width = other_obs.get_width() obstacle_msg.confidence = 1.0 obstacles_msg.obstacles.append(obstacle_msg) self.pub_obstacle.publish(obstacles_msg) # create line msg line_msg = LineInformationInImage() # Todo: add lines line_msg.header.frame_id = image_msg.header.frame_id line_msg.header.stamp = image_msg.header.stamp for lp in self.line_detector.get_linepoints(): ls = LineSegmentInImage() ls.start.x = lp[0] ls.start.y = lp[1] ls.end = ls.start line_msg.segments.append(ls) self.pub_lines.publish(line_msg) # do debug stuff if self.debug: self.debug_image_dings.set_image(image) self.debug_image_dings.draw_obstacle_candidates( self.obstacle_detector.get_candidates(), (0, 0, 0), thickness=3) self.debug_image_dings.draw_obstacle_candidates( self.obstacle_detector.get_red_obstacles(), (0, 0, 255), thickness=3) self.debug_image_dings.draw_obstacle_candidates( self.obstacle_detector.get_blue_obstacles(), (255, 0, 0), thickness=3) self.debug_image_dings.draw_obstacle_candidates( self.obstacle_detector.get_white_obstacles(), (255, 255, 255), thickness=3) self.debug_image_dings.draw_horizon( self.horizon_detector.get_horizon_points(), (0, 0, 255)) self.debug_image_dings.draw_ball_candidates( self.ball_detector.get_candidates(), (0, 0, 255)) self.debug_image_dings.draw_ball_candidates( self.horizon_detector.balls_under_horizon( self.ball_detector.get_candidates(), self._ball_candidate_y_offset), (0, 255, 255)) # draw top candidate in self.debug_image_dings.draw_ball_candidates([top_ball_candidate], (0, 255, 0)) # draw linepoints in black self.debug_image_dings.draw_points( self.line_detector.get_linepoints(), (0, 0, 255)) # debug_image_dings.draw_line_segments(line_detector.get_linesegments(), (180, 105, 255)) if self.debug_image: self.debug_image_dings.imshow() if self.debug_image_msg: self.pub_debug_image.publish( self.bridge.cv2_to_imgmsg( self.debug_image_dings.get_image(), 'bgr8'))
signal.signal(signal.SIGINT, _signal_term_handler) rospy.init_node("ball_tester") pub = rospy.Publisher("ball_in_image", BallsInImage, queue_size=10) while True: x_str = raw_input("x:") try: x = int(x_str) except ValueError: print("try again") continue y_str = raw_input("y:") try: y = int(y_str) except ValueError: print("try again") continue bi = BallsInImage() bi.header.stamp = rospy.get_rostime() - rospy.Duration(0.2) ball = BallInImage() ball.confidence = 1 ball.center.x = x ball.center.y = y ball.diameter = 0.13 bi.candidates.append(ball) pub.publish(bi)
def handle_image(self, image_msg): # converting the ROS image message to CV2-image image = self.bridge.imgmsg_to_cv2(image_msg, 'bgr8') if self._first_callback: mean = cv2.mean(image) if sum(mean) < self._blind_threshold: self._speak("Hey! Remove my camera cap!", self.speak_publisher) # setup detectors self.field_boundary_detector.set_image(image) self.obstacle_detector.set_image(image) self.line_detector.set_image(image) self.runtime_evaluator.set_image() self.ball_detector.set_image(image) if self.config['vision_parallelize']: self.field_boundary_detector.compute_all() # computes stuff which is needed later in the processing fcnn_thread = threading.Thread(target=self.ball_detector.compute_top_candidate) conventional_thread = threading.Thread(target=self._conventional_precalculation()) conventional_thread.start() fcnn_thread.start() conventional_thread.join() fcnn_thread.join() else: self.ball_detector.compute_top_candidate() self._conventional_precalculation() # TODO: handle all ball candidates #""" ball_candidates = self.ball_detector.get_candidates() if ball_candidates: balls_under_field_boundary = self.field_boundary_detector.balls_under_convex_field_boundary(ball_candidates) if balls_under_field_boundary: sorted_rated_candidates = sorted(balls_under_field_boundary, key=lambda x: x.rating) top_ball_candidate = list([max(sorted_rated_candidates[0:1], key=lambda x: x.rating)])[0] else: top_ball_candidate = None else: top_ball_candidate = None """ # check whether ball candidates are under the field_boundary # TODO: handle multiple ball candidates top_ball_candidate = self.ball_detector.get_top_candidate() if top_ball_candidate: ball = [] ball.append(top_ball_candidate) ball_under_field_boundary = self.field_boundary_detector.balls_under_field_boundary(ball) if ball_under_field_boundary: top_ball_candidate = ball_under_field_boundary[0] else: top_ball_candidate = None #""" # check whether ball candidates are over rating threshold if top_ball_candidate and top_ball_candidate.rating > self._ball_candidate_threshold: # create ball msg # TODO: publish empty msg if no top candidate as described in msg description balls_msg = BallsInImage() balls_msg.header.frame_id = image_msg.header.frame_id balls_msg.header.stamp = image_msg.header.stamp ball_msg = BallInImage() ball_msg.center.x = top_ball_candidate.get_center_x() ball_msg.center.y = top_ball_candidate.get_center_y() ball_msg.diameter = top_ball_candidate.get_diameter() ball_msg.confidence = 1 balls_msg.candidates.append(ball_msg) self.debug_printer.info('found a ball! \o/', 'ball') self.pub_balls.publish(balls_msg) # create goalpost msg goal_parts_msg = GoalPartsInImage() goal_parts_msg.header.frame_id = image_msg.header.frame_id goal_parts_msg.header.stamp = image_msg.header.stamp # create obstacle msg obstacles_msg = ObstaclesInImage() obstacles_msg.header.frame_id = image_msg.header.frame_id obstacles_msg.header.stamp = image_msg.header.stamp for red_obs in self.obstacle_detector.get_red_obstacles(): obstacle_msg = ObstacleInImage() obstacle_msg.color = ObstacleInImage.ROBOT_MAGENTA obstacle_msg.top_left.x = red_obs.get_upper_left_x() obstacle_msg.top_left.y = red_obs.get_upper_left_y() obstacle_msg.height = int(red_obs.get_height()) obstacle_msg.width = int(red_obs.get_width()) obstacle_msg.confidence = 1.0 obstacle_msg.playerNumber = 42 obstacles_msg.obstacles.append(obstacle_msg) for blue_obs in self.obstacle_detector.get_blue_obstacles(): obstacle_msg = ObstacleInImage() obstacle_msg.color = ObstacleInImage.ROBOT_CYAN obstacle_msg.top_left.x = blue_obs.get_upper_left_x() obstacle_msg.top_left.y = blue_obs.get_upper_left_y() obstacle_msg.height = int(blue_obs.get_height()) obstacle_msg.width = int(blue_obs.get_width()) obstacle_msg.confidence = 1.0 obstacle_msg.playerNumber = 42 obstacles_msg.obstacles.append(obstacle_msg) if self.config['vision_ball_classifier'] == 'yolo': candidates = self.goalpost_detector.get_candidates() for goalpost in candidates: post_msg = PostInImage() post_msg.width = goalpost.get_width() post_msg.confidence = goalpost.get_rating() post_msg.foot_point.x = goalpost.get_center_x() post_msg.foot_point.y = goalpost.get_lower_right_y() post_msg.top_point = post_msg.foot_point goal_parts_msg.posts.append(post_msg) else: for white_obs in self.obstacle_detector.get_white_obstacles(): post_msg = PostInImage() post_msg.width = white_obs.get_width() post_msg.confidence = 1.0 post_msg.foot_point.x = white_obs.get_center_x() post_msg.foot_point.y = white_obs.get_lower_right_y() post_msg.top_point = post_msg.foot_point goal_parts_msg.posts.append(post_msg) for other_obs in self.obstacle_detector.get_other_obstacles(): obstacle_msg = ObstacleInImage() obstacle_msg.color = ObstacleInImage.UNDEFINED obstacle_msg.top_left.x = other_obs.get_upper_left_x() obstacle_msg.top_left.y = other_obs.get_upper_left_y() obstacle_msg.height = int(other_obs.get_height()) obstacle_msg.width = int(other_obs.get_width()) obstacle_msg.confidence = 1.0 obstacles_msg.obstacles.append(obstacle_msg) self.pub_obstacle.publish(obstacles_msg) goal_msg = GoalInImage() goal_msg.header = goal_parts_msg.header left_post = PostInImage() left_post.foot_point.x = 9999999999 left_post.confidence = 1.0 right_post = PostInImage() right_post.foot_point.x = -9999999999 right_post.confidence = 1.0 for post in goal_parts_msg.posts: if post.foot_point.x < left_post.foot_point.x: left_post = post left_post.confidence = post.confidence if post.foot_point.x > right_post.foot_point.x: right_post = post right_post.confidence = post.confidence goal_msg.left_post = left_post goal_msg.right_post = right_post goal_msg.confidence = 1.0 if goal_parts_msg.posts: self.pub_goal.publish(goal_msg) # create line msg line_msg = LineInformationInImage() # Todo: add lines line_msg.header.frame_id = image_msg.header.frame_id line_msg.header.stamp = image_msg.header.stamp for lp in self.line_detector.get_linepoints(): ls = LineSegmentInImage() ls.start.x = lp[0] ls.start.y = lp[1] ls.end = ls.start line_msg.segments.append(ls) self.pub_lines.publish(line_msg) # create non_line msg # non_line_msg = LineInformationInImage() # non_line_msg.header.frame_id = image_msg.header.frame_id # non_line_msg.header.stamp = image_msg.header.stamp # i = 0 # for nlp in self.line_detector.get_nonlinepoints(): # nls = LineSegmentInImage() # nls.start.x = nlp[0] # nls.start.y = nlp[1] # nls.end = nls.start # if i % 2 == 0: # non_line_msg.segments.append(nls) # i += 1 # self.pub_non_lines.publish(non_line_msg) if self.ball_fcnn_publish_output and self.config['vision_ball_classifier'] == 'fcnn': self.pub_ball_fcnn.publish(self.ball_detector.get_cropped_msg()) if self.publish_fcnn_debug_image and self.config['vision_ball_classifier'] == 'fcnn': self.pub_debug_fcnn_image.publish(self.ball_detector.get_debug_image()) # do debug stuff if self.publish_debug_image: self.debug_image_dings.set_image(image) self.debug_image_dings.draw_obstacle_candidates( self.obstacle_detector.get_candidates(), (0, 0, 0), thickness=3 ) self.debug_image_dings.draw_obstacle_candidates( self.obstacle_detector.get_red_obstacles(), (0, 0, 255), thickness=3 ) self.debug_image_dings.draw_obstacle_candidates( self.obstacle_detector.get_blue_obstacles(), (255, 0, 0), thickness=3 ) if self.config['vision_ball_classifier'] == "yolo": post_candidates = self.goalpost_detector.get_candidates() else: post_candidates = self.obstacle_detector.get_white_obstacles() self.debug_image_dings.draw_obstacle_candidates( post_candidates, (255, 255, 255), thickness=3 ) self.debug_image_dings.draw_field_boundary( self.field_boundary_detector.get_field_boundary_points(), (0, 0, 255)) self.debug_image_dings.draw_field_boundary( self.field_boundary_detector.get_convex_field_boundary_points(), (0, 255, 255)) self.debug_image_dings.draw_ball_candidates( self.ball_detector.get_candidates(), (0, 0, 255)) self.debug_image_dings.draw_ball_candidates( self.field_boundary_detector.balls_under_field_boundary( self.ball_detector.get_candidates(), self._ball_candidate_y_offset), (0, 255, 255)) # draw top candidate in self.debug_image_dings.draw_ball_candidates([top_ball_candidate], (0, 255, 0)) # draw linepoints in red self.debug_image_dings.draw_points( self.line_detector.get_linepoints(), (0, 0, 255)) # draw nonlinepoints in black # self.debug_image_dings.draw_points( # self.line_detector.get_nonlinepoints(), # (0, 0, 0)) # publish debug image self.pub_debug_image.publish(self.bridge.cv2_to_imgmsg(self.debug_image_dings.get_image(), 'bgr8')) self._first_callback = False
def create_balls_msg(ball_msgs, seq, stamp): msg = BallsInImage() msg.header.seq = seq msg.header.stamp = stamp msg.candidates = ball_msgs return msg