def create_ball_msg(ball_label):
msg = BallInImage()
if float(ball_label["x"]) != 0:
msg.center.x = float(ball_label["x"])
msg.center.y = float(ball_label["y"])
msg.diameter = float(ball_label["dia"])
msg.confidence = 1.0
else:
msg.confidence = 0.0
return msg
def ball_img_msg(self, x, y, d, c):
msg = BallInImage()
msg.center.x = x
msg.center.y = y
msg.diameter = d
msg.confidence = c
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 run():
node = Node("testHeadBehaviour")
pub_ball = node.create_publisher(BallInImage, "ball_in_image", 1)
pub_hmg = node.create_publisher(JointTrajectory, "head_motor_goals", 1)
hmg = JointTrajectory()
goal = JointTrajectoryPoint()
goal.positions = [0, 0]
goal.velocities = [0, 0]
hmg.points = [goal]
counter = 320
direction = 1
node.get_logger().info("Create Test")
rclpy.init(args=None)
pub_hmg.publish(hmg)
rate = node.create_rate(4)
node.get_logger().debug("Laeuft...")
while rclpy.ok():
# 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 = BallInImageArray()
balls.candidates.append(ball)
pub_ball.publish(balls)
node.get_logger().info("Published ball: %s" % counter)
rate.sleep()
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