def __init__(self):
# type () -> None
"""
Vision is the main ROS-node for handling all tasks related to image processing.
Initiating 'bitbots_vision' node.
:return: None
"""
rospack = rospkg.RosPack()
self.package_path = rospack.get_path('bitbots_vision')
rospy.init_node('bitbots_vision')
rospy.loginfo('Initializing vision...')
self.bridge = CvBridge()
self.config = {}
# the head_joint_states is used by the dynamic field_boundary detector
self.head_joint_state = None
self.debug_image_dings = debug.DebugImage(
) # Todo: better variable name
if self.debug_image_dings:
self.runtime_evaluator = evaluator.RuntimeEvaluator(None)
# Register publisher of 'vision_config'-messages
# For changes of topic name: also change topic name in dynamic_color_space.py
self.pub_config = rospy.Publisher('vision_config',
Config,
queue_size=1,
latch=True)
# Speak publisher
self.speak_publisher = rospy.Publisher('/speak', Speak, queue_size=10)
self._first_callback = True
# Register VisionConfig server (dynamic reconfigure) and set callback
#rospy.logerr('start srv')
srv = Server(VisionConfig, self._dynamic_reconfigure_callback)
#rospy.logerr('end err')
rospy.spin()
def vision_config_callback(self, msg):
# type: (Config) -> None
"""
This method is called by the 'vision_config'-message subscriber.
Load and update vision config.
Handle config changes.
This callback is delayed (about 30 seconds) after changes through dynamic reconfigure
:param Config msg: new 'vision_config'-message subscriber
:return: None
"""
# Load dict from string in yaml-format in msg.data
vision_config = yaml.load(msg.data)
self.debug_printer = debug.DebugPrinter(
debug_classes=debug.DebugPrinter.
generate_debug_class_list_from_string(
vision_config['vision_debug_printer_classes']))
self.runtime_evaluator = evaluator.RuntimeEvaluator(None)
# Print status of dynamic color space after toggeling 'dynamic_color_space_active' parameter
if 'dynamic_color_space_active' not in self.vision_config or \
vision_config['dynamic_color_space_active'] != self.vision_config['dynamic_color_space_active']:
if vision_config['dynamic_color_space_active']:
rospy.loginfo('Dynamic color space turned ON.')
else:
rospy.logwarn('Dynamic color space turned OFF.')
# Set publisher of ColorSpace-messages
if 'ROS_dynamic_color_space_msg_topic' not in self.vision_config or \
self.vision_config['ROS_dynamic_color_space_msg_topic'] != vision_config['ROS_dynamic_color_space_msg_topic']:
if hasattr(self, 'pub_color_space'):
self.pub_color_space.unregister()
self.pub_color_space = rospy.Publisher(
vision_config['ROS_dynamic_color_space_msg_topic'],
ColorSpace,
queue_size=1)
# Set Color- and FieldBoundaryDetector
self.color_detector = color.DynamicPixelListColorDetector(
self.debug_printer, self.package_path, vision_config)
self.field_boundary_detector = field_boundary.FieldBoundaryDetector(
self.color_detector,
vision_config,
self.debug_printer,
used_by_dyn_color_detector=True)
# Reset queue
if hasattr(self, 'color_value_queue'):
self.color_value_queue.clear()
# Set params
self.queue_max_size = vision_config[
'dynamic_color_space_queue_max_size']
self.color_value_queue = deque(maxlen=self.queue_max_size)
self.pointfinder = Pointfinder(
self.debug_printer, vision_config['dynamic_color_space_threshold'],
vision_config['dynamic_color_space_kernel_radius'])
self.heuristic = Heuristic(self.debug_printer)
# Subscribe to Image-message
if 'ROS_img_msg_topic' not in self.vision_config or \
self.vision_config['ROS_img_msg_topic'] != vision_config['ROS_img_msg_topic']:
if hasattr(self, 'sub_image_msg'):
self.sub_image_msg.unregister()
self.sub_image_msg = rospy.Subscriber(
vision_config['ROS_img_msg_topic'],
Image,
self.image_callback,
queue_size=vision_config['ROS_img_queue_size'],
tcp_nodelay=True,
buff_size=60000000)
# https://github.com/ros/ros_comm/issues/536
self.vision_config = vision_config
def _dynamic_reconfigure_callback(self, config, level):
self.debug_printer = debug.DebugPrinter(
debug_classes=debug.DebugPrinter.generate_debug_class_list_from_string(
config['vision_debug_printer_classes']))
self.runtime_evaluator = evaluator.RuntimeEvaluator(self.debug_printer)
self._blind_threshold = config['vision_blind_threshold']
self._ball_candidate_threshold = config['vision_ball_candidate_rating_threshold']
self._ball_candidate_y_offset = config['vision_ball_candidate_field_boundary_y_offset']
self.publish_debug_image = config['vision_publish_debug_image']
if self.publish_debug_image:
rospy.logwarn('Debug images are enabled')
else:
rospy.loginfo('Debug images are disabled')
self.ball_fcnn_publish_output = config['ball_fcnn_publish_output']
if self.ball_fcnn_publish_output:
rospy.logwarn('ball FCNN output publishing is enabled')
else:
rospy.logwarn('ball FCNN output publishing is disabled')
self.publish_fcnn_debug_image = config['ball_fcnn_publish_debug_img']
if config['vision_ball_classifier'] == 'dummy':
self.ball_detector = dummy_ballfinder.DummyClassifier(None, None, self.debug_printer)
# Print status of color config
if 'vision_use_sim_color' not in self.config or \
config['vision_use_sim_color'] != self.config['vision_use_sim_color']:
if config['vision_use_sim_color']:
rospy.logwarn('Loaded color space for SIMULATOR.')
else:
rospy.loginfo('Loaded color space for REAL WORLD.')
self.white_color_detector = color.HsvSpaceColorDetector(
self.debug_printer,
[config['white_color_detector_lower_values_h'], config['white_color_detector_lower_values_s'],
config['white_color_detector_lower_values_v']],
[config['white_color_detector_upper_values_h'], config['white_color_detector_upper_values_s'],
config['white_color_detector_upper_values_v']])
self.red_color_detector = color.HsvSpaceColorDetector(
self.debug_printer,
[config['red_color_detector_lower_values_h'], config['red_color_detector_lower_values_s'],
config['red_color_detector_lower_values_v']],
[config['red_color_detector_upper_values_h'], config['red_color_detector_upper_values_s'],
config['red_color_detector_upper_values_v']])
self.blue_color_detector = color.HsvSpaceColorDetector(
self.debug_printer,
[config['blue_color_detector_lower_values_h'], config['blue_color_detector_lower_values_s'],
config['blue_color_detector_lower_values_v']],
[config['blue_color_detector_upper_values_h'], config['blue_color_detector_upper_values_s'],
config['blue_color_detector_upper_values_v']])
if config['dynamic_color_space_active']:
self.field_color_detector = color.DynamicPixelListColorDetector(
self.debug_printer,
self.package_path,
config,
primary_detector=True)
else:
self.field_color_detector = color.PixelListColorDetector(
self.debug_printer,
self.package_path,
config)
self.field_boundary_detector = field_boundary.FieldBoundaryDetector(
self.field_color_detector,
config,
self.debug_printer,
self.runtime_evaluator)
self.line_detector = lines.LineDetector(
self.white_color_detector,
self.field_color_detector,
self.field_boundary_detector,
config,
self.debug_printer)
self.obstacle_detector = obstacle.ObstacleDetector(
self.red_color_detector,
self.blue_color_detector,
self.white_color_detector,
self.field_boundary_detector,
self.runtime_evaluator,
config,
self.debug_printer
)
# set up ball config for fcnn
# these config params have domain-specific names which could be problematic for fcnn handlers handling e.g. goal candidates
# this enables 2 fcnns with different configs.
self.ball_fcnn_config = {
'debug': config['ball_fcnn_publish_debug_img'],
'threshold': config['ball_fcnn_threshold'],
'expand_stepsize': config['ball_fcnn_expand_stepsize'],
'pointcloud_stepsize': config['ball_fcnn_pointcloud_stepsize'],
'shuffle_candidate_list': config['ball_fcnn_shuffle_candidate_list'],
'min_candidate_diameter': config['ball_fcnn_min_ball_diameter'],
'max_candidate_diameter': config['ball_fcnn_max_ball_diameter'],
'candidate_refinement_iteration_count': config['ball_fcnn_candidate_refinement_iteration_count'],
'publish_field_boundary_offset': config['ball_fcnn_publish_field_boundary_offset'],
}
# load fcnn
if config['vision_ball_classifier'] == 'fcnn':
if 'neural_network_model_path' not in self.config or \
self.config['neural_network_model_path'] != config['neural_network_model_path'] or \
self.config['vision_ball_classifier'] != config['vision_ball_classifier']:
ball_fcnn_path = os.path.join(self.package_path, 'models', config['neural_network_model_path'])
if not os.path.exists(ball_fcnn_path):
rospy.logerr('AAAAHHHH! The specified fcnn model file doesn\'t exist!')
self.ball_fcnn = live_fcnn_03.FCNN03(ball_fcnn_path, self.debug_printer)
rospy.loginfo(config['vision_ball_classifier'] + " vision is running now")
self.ball_detector = fcnn_handler.FcnnHandler(
self.ball_fcnn,
self.field_boundary_detector,
self.ball_fcnn_config,
self.debug_printer)
if config['vision_ball_classifier'] == 'yolo':
if 'neural_network_model_path' not in self.config or \
self.config['neural_network_model_path'] != config['neural_network_model_path'] or \
self.config['vision_ball_classifier'] != config['vision_ball_classifier']:
yolo_model_path = os.path.join(self.package_path, 'models', config['neural_network_model_path'])
if not os.path.exists(yolo_model_path):
rospy.logerr('AAAAHHHH! The specified yolo model file doesn\'t exist!')
# TODO replace following strings with path to config/weights
yolo = yolo_handler.YoloHandler(config, yolo_model_path)
self.ball_detector = yolo_handler.YoloBallDetector(yolo)
self.goalpost_detector = yolo_handler.YoloGoalpostDetector(yolo)
rospy.loginfo(config['vision_ball_classifier'] + " vision is running now")
# publishers
# TODO: topic: ball_in_... BUT MSG TYPE: balls_in_img... CHANGE TOPIC TYPE!
if 'ROS_ball_msg_topic' not in self.config or \
self.config['ROS_ball_msg_topic'] != config['ROS_ball_msg_topic']:
if hasattr(self, 'pub_balls'):
self.pub_balls.unregister()
self.pub_balls = rospy.Publisher(
config['ROS_ball_msg_topic'],
BallsInImage,
queue_size=1)
if 'ROS_line_msg_topic' not in self.config or \
self.config['ROS_line_msg_topic'] != config['ROS_line_msg_topic']:
if hasattr(self, 'pub_lines'):
self.pub_lines.unregister()
self.pub_lines = rospy.Publisher(
config['ROS_line_msg_topic'],
LineInformationInImage,
queue_size=5)
# publisher for nonlinepoints
# if 'ROS_non_line_msg_topic' not in self.config or \
# self.config['ROS_non_line_msg_topic'] != config['ROS_non_line_msg_topic']:
# if hasattr(self, 'pub_non_lines'):
# self.pub_non_lines.unregister()
# self.pub_non_lines = rospy.Publisher(
# config['ROS_non_line_msg_topic'],
# LineInformationInImage,
# queue_size=5)
if 'ROS_obstacle_msg_topic' not in self.config or \
self.config['ROS_obstacle_msg_topic'] != config['ROS_obstacle_msg_topic']:
if hasattr(self, 'pub_obstacle'):
self.pub_obstacle.unregister()
self.pub_obstacle = rospy.Publisher(
config['ROS_obstacle_msg_topic'],
ObstaclesInImage,
queue_size=3)
if 'ROS_goal_msg_topic' not in self.config or \
self.config['ROS_goal_msg_topic'] != config['ROS_goal_msg_topic']:
if hasattr(self, 'pub_goal'):
self.pub_goal.unregister()
self.pub_goal = rospy.Publisher(
config['ROS_goal_msg_topic'],
GoalInImage,
queue_size=3)
if 'ROS_fcnn_img_msg_topic' not in self.config or \
self.config['ROS_fcnn_img_msg_topic'] != config['ROS_fcnn_img_msg_topic']:
if hasattr(self, 'pub_ball_fcnn'):
self.pub_ball_fcnn.unregister()
self.pub_ball_fcnn = rospy.Publisher(
config['ROS_fcnn_img_msg_topic'],
ImageWithRegionOfInterest,
queue_size=1)
if 'ROS_debug_image_msg_topic' not in self.config or \
self.config['ROS_debug_image_msg_topic'] != config['ROS_debug_image_msg_topic']:
if hasattr(self, 'pub_debug_image'):
self.pub_debug_image.unregister()
self.pub_debug_image = rospy.Publisher(
config['ROS_debug_image_msg_topic'],
Image,
queue_size=1)
if 'ROS_debug_fcnn_image_msg_topic' not in self.config or \
self.config['ROS_debug_fcnn_image_msg_topic'] != config['ROS_debug_fcnn_image_msg_topic']:
if hasattr(self, 'pub_debug_fcnn_image'):
self.pub_debug_fcnn_image.unregister()
self.pub_debug_fcnn_image = rospy.Publisher(
config['ROS_debug_fcnn_image_msg_topic'],
Image,
queue_size=1)
# subscribers
if 'ROS_img_msg_topic' not in self.config or \
self.config['ROS_img_msg_topic'] != config['ROS_img_msg_topic']:
if hasattr(self, 'image_sub'):
self.image_sub.unregister()
self.image_sub = rospy.Subscriber(
config['ROS_img_msg_topic'],
Image,
self._image_callback,
queue_size=config['ROS_img_queue_size'],
tcp_nodelay=True,
buff_size=60000000)
# https://github.com/ros/ros_comm/issues/536
# subscriber for the vertical position of the head, used by the dynamic field-boundary-detector
if 'ROS_head_joint_msg_topic' not in self.config or \
self.config['ROS_head_joint_msg_topic'] != config['ROS_head_joint_msg_topic']:
if hasattr(self, 'head_sub'):
self.head_sub.unregister()
self.head_sub = rospy.Subscriber(
config['ROS_head_joint_msg_topic'],
JointState,
self._head_joint_state_callback,
queue_size=config['ROS_head_joint_state_queue_size'],
tcp_nodelay=True)
# Publish Config-message
# Clean config dict to avoid not dumpable types
config_cleaned = {}
for key, value in config.items():
if type(value) != DynamicReconfigureConfig:
config_cleaned[key] = value
msg = Config()
msg.data = yaml.dump(config_cleaned)
self.pub_config.publish(msg)
self.config = config
return config