diff --git a/bitbots_misc/bitbots_technical_challenge_vision/bitbots_technical_challenge_vision/__init__.py b/bitbots_misc/bitbots_technical_challenge_vision/bitbots_technical_challenge_vision/__init__.py
new file mode 100644
index 000000000..e69de29bb
diff --git a/bitbots_misc/bitbots_technical_challenge_vision/bitbots_technical_challenge_vision/bitbots_technical_challenge_vision.py b/bitbots_misc/bitbots_technical_challenge_vision/bitbots_technical_challenge_vision/bitbots_technical_challenge_vision.py
new file mode 100755
index 000000000..7c7957aad
--- /dev/null
+++ b/bitbots_misc/bitbots_technical_challenge_vision/bitbots_technical_challenge_vision/bitbots_technical_challenge_vision.py
@@ -0,0 +1,183 @@
+from typing import Optional, Tuple
+
+import cv2
+import numpy as np
+import rclpy
+import rclpy.logging
+from ament_index_python.packages import get_package_share_directory
+from cv_bridge import CvBridge
+from rclpy.node import Node
+from sensor_msgs.msg import Image
+from soccer_vision_2d_msgs.msg import Robot, RobotArray
+
+from bitbots_technical_challenge_vision.bitbots_technical_challenge_vision_params import (
+ bitbots_technical_challenge_vision,
+)
+
+
+class TechnicalChallengeVision(Node):
+ def __init__(self):
+ super().__init__("bitbots_technical_challenge_vision")
+
+ self._package_path = get_package_share_directory("bitbots_technical_challenge_vision")
+ self._cv_bridge = CvBridge()
+ self._annotations_pub = self.create_publisher(RobotArray, "/robots_in_image", 10)
+ self._debug_img_pub = self.create_publisher(Image, "/bitbots_technical_challenge_vision_debug_img", 10)
+ self._debug_clrmp_pub_blue = self.create_publisher(
+ Image, "/bitbots_technical_challenge_vision_debug_clrmp_blue", 10
+ )
+ self._debug_clrmp_pub_red = self.create_publisher(
+ Image, "/bitbots_technical_challenge_vision_debug_clrmp_red", 10
+ )
+ self._img_sub = self.create_subscription(Image, "/camera/image_proc", self.image_callback, 10)
+ self._param_listener = bitbots_technical_challenge_vision.ParamListener(self)
+ self._params = self._param_listener.get_params()
+
+ def create_robot_msg(self, x: int, y: int, w: int, h: int, t: int) -> Robot:
+ """
+ Creates a Robot message from a robots bounding box data and its color.
+
+ :param x: bb top left x
+ :param y: bb top left y
+ :param w: bb width
+ :param h: bb height
+ :param t: robot team
+ :return: robot message for that robot
+ """
+ robot = Robot()
+
+ robot.bb.center.position.x = float(x + (w / 2))
+ robot.bb.center.position.y = float(y + (h / 2))
+ robot.bb.size_x = float(w)
+ robot.bb.size_y = float(h)
+ robot.attributes.team = t
+
+ return robot
+
+ def process_image(
+ self, img: np.ndarray, debug_img: np.ndarray, arg
+ ) -> Tuple[list[Robot], list[Robot], np.ndarray, np.ndarray]:
+ """
+ gets annotations from the camera image
+
+ :param img: ndarray containing the camera image
+ :param debug_img: copy of img debug annotations should be drawn here
+ :param arg: __RosParameters object containing the dynamic parameters
+ :return: [[blue_robots], [red_robots], clrmp_blue, clrmp_red]
+ """
+ # convert img to hsv to isolate colors better
+ img = cv2.cvtColor(img, cv2.COLOR_BGR2HSV)
+
+ # get color maps
+ blue_map = cv2.inRange(
+ img,
+ (arg.blue_lower_h, arg.blue_lower_s, arg.blue_lower_v),
+ (arg.blue_upper_h, arg.blue_upper_s, arg.blue_upper_v),
+ )
+
+ red_map = cv2.inRange(
+ img,
+ (arg.red_lower_h, arg.red_lower_s, arg.red_lower_v),
+ (arg.red_upper_h, arg.red_upper_s, arg.red_upper_v),
+ )
+
+ # get contours in color maps
+ blue_contours, _ = cv2.findContours(blue_map, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_NONE)
+ red_contours, _ = cv2.findContours(red_map, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_NONE)
+
+ # get lists of bounding boxes
+ blue_robots = []
+ red_robots = []
+
+ def annotate(x, y, w, h, c) -> Optional[np.ndarray]:
+ if not arg.debug_mode:
+ return None
+ return cv2.rectangle(
+ debug_img,
+ (x, y),
+ (x + w, y + h),
+ c,
+ 2,
+ )
+
+ for cnt in blue_contours:
+ x, y, w, h = cv2.boundingRect(cnt)
+ if min(w, h) >= arg.min_size and max(h, w) <= arg.max_size:
+ # draw bb on debug img
+ annotate(x, y, w, h, (255, 0, 0))
+
+ # TODO I think 1 is for the blue team?
+ blue_robots.append(self.create_robot_msg(x, y, w, h, 1))
+
+ for cnt in red_contours:
+ x, y, w, h = cv2.boundingRect(cnt)
+ if min(w, h) >= arg.min_size and max(h, w) <= arg.max_size:
+ # draw bb on debug img
+ annotate(x, y, w, h, (0, 0, 255))
+
+ red_robots.append(self.create_robot_msg(x, y, w, h, 2))
+
+ return blue_robots, red_robots, blue_map, red_map, debug_img
+
+ def image_callback(self, msg: Image):
+ # get dynamic parameters
+ if self._param_listener.is_old(self._params):
+ self._param_listener.refresh_dynamic_parameters()
+ self._params = self._param_listener.get_params()
+
+ arg = self._params
+
+ # set variables
+ img = self._cv_bridge.imgmsg_to_cv2(img_msg=msg, desired_encoding="bgr8")
+ header = msg.header
+
+ if arg.debug_mode:
+ debug_img = np.copy(img)
+ else:
+ debug_img = None
+
+ # get annotations
+ blue_robots, red_robots, blue_map, red_map, debug_img = self.process_image(img, debug_img, arg)
+ robots = []
+ robots.extend(blue_robots)
+ robots.extend(red_robots)
+
+ # make output message
+ robot_array_message = RobotArray()
+ robot_array_message.header = header
+ robot_array_message.robots = robots
+
+ # publish output message
+ self._annotations_pub.publish(robot_array_message)
+
+ if arg.debug_mode:
+ # make debug image message
+ debug_img_msg = self._cv_bridge.cv2_to_imgmsg(cvim=debug_img, encoding="bgr8", header=header)
+
+ # publish debug image
+ self._debug_img_pub.publish(debug_img_msg)
+
+ # make color map messages
+ clrmp_blue_img = cv2.cvtColor(blue_map, cv2.COLOR_GRAY2BGR)
+ clrmp_blue_msg = self._cv_bridge.cv2_to_imgmsg(cvim=clrmp_blue_img, encoding="bgr8", header=header)
+
+ clrmp_red_img = cv2.cvtColor(red_map, cv2.COLOR_GRAY2BGR)
+ clrmp_red_msg = self._cv_bridge.cv2_to_imgmsg(clrmp_red_img, encoding="bgr8", header=header)
+
+ # publish color map messages
+ self._debug_clrmp_pub_blue.publish(clrmp_blue_msg)
+ self._debug_clrmp_pub_red.publish(clrmp_red_msg)
+
+
+def main(args=None):
+ rclpy.init(args=args)
+ node = TechnicalChallengeVision()
+ try:
+ rclpy.spin(node)
+ except KeyboardInterrupt:
+ pass
+ node.destroy_node()
+
+
+if __name__ == "__main__":
+ main()
diff --git a/bitbots_misc/bitbots_technical_challenge_vision/bitbots_technical_challenge_vision/bitbots_technical_challenge_vision_params.py b/bitbots_misc/bitbots_technical_challenge_vision/bitbots_technical_challenge_vision/bitbots_technical_challenge_vision_params.py
new file mode 100644
index 000000000..6eaa8cbd1
--- /dev/null
+++ b/bitbots_misc/bitbots_technical_challenge_vision/bitbots_technical_challenge_vision/bitbots_technical_challenge_vision_params.py
@@ -0,0 +1,475 @@
+# flake8: noqa
+
+# auto-generated DO NOT EDIT
+
+from rcl_interfaces.msg import ParameterDescriptor
+from rcl_interfaces.msg import SetParametersResult
+from rcl_interfaces.msg import FloatingPointRange, IntegerRange
+from rclpy.clock import Clock
+from rclpy.exceptions import InvalidParameterValueException
+from rclpy.time import Time
+import copy
+import rclpy
+from generate_parameter_library_py.python_validators import ParameterValidators
+
+
+class bitbots_technical_challenge_vision:
+ class Params:
+ # for detecting if the parameter struct has been updated
+ stamp_ = Time()
+
+ blue_lower_h = 0
+ blue_upper_h = 179
+ blue_lower_s = 0
+ blue_upper_s = 255
+ blue_lower_v = 0
+ blue_upper_v = 255
+ red_lower_h = 0
+ red_upper_h = 179
+ red_lower_s = 0
+ red_upper_s = 255
+ red_lower_v = 0
+ red_upper_v = 255
+ min_size = 0
+ max_size = 1
+ debug_mode = True
+
+ class ParamListener:
+ def __init__(self, node, prefix=""):
+ self.prefix_ = prefix
+ self.params_ = bitbots_technical_challenge_vision.Params()
+ self.node_ = node
+ self.logger_ = rclpy.logging.get_logger("bitbots_technical_challenge_vision." + prefix)
+
+ self.declare_params()
+
+ self.node_.add_on_set_parameters_callback(self.update)
+ self.clock_ = Clock()
+
+ def get_params(self):
+ tmp = self.params_.stamp_
+ self.params_.stamp_ = None
+ paramCopy = copy.deepcopy(self.params_)
+ paramCopy.stamp_ = tmp
+ self.params_.stamp_ = tmp
+ return paramCopy
+
+ def is_old(self, other_param):
+ return self.params_.stamp_ != other_param.stamp_
+
+ def refresh_dynamic_parameters(self):
+ updated_params = self.get_params()
+ # TODO remove any destroyed dynamic parameters
+
+ # declare any new dynamic parameters
+
+ def update(self, parameters):
+ updated_params = self.get_params()
+
+ for param in parameters:
+ if param.name == self.prefix_ + "blue_lower_h":
+ validation_result = ParameterValidators.bounds(param, 0, 179)
+ if validation_result:
+ return SetParametersResult(successful=False, reason=validation_result)
+ updated_params.blue_lower_h = param.value
+ self.logger_.debug(param.name + ": " + param.type_.name + " = " + str(param.value))
+
+ if param.name == self.prefix_ + "blue_upper_h":
+ validation_result = ParameterValidators.bounds(param, 0, 179)
+ if validation_result:
+ return SetParametersResult(successful=False, reason=validation_result)
+ updated_params.blue_upper_h = param.value
+ self.logger_.debug(param.name + ": " + param.type_.name + " = " + str(param.value))
+
+ if param.name == self.prefix_ + "blue_lower_s":
+ validation_result = ParameterValidators.bounds(param, 0, 255)
+ if validation_result:
+ return SetParametersResult(successful=False, reason=validation_result)
+ updated_params.blue_lower_s = param.value
+ self.logger_.debug(param.name + ": " + param.type_.name + " = " + str(param.value))
+
+ if param.name == self.prefix_ + "blue_upper_s":
+ validation_result = ParameterValidators.bounds(param, 0, 255)
+ if validation_result:
+ return SetParametersResult(successful=False, reason=validation_result)
+ updated_params.blue_upper_s = param.value
+ self.logger_.debug(param.name + ": " + param.type_.name + " = " + str(param.value))
+
+ if param.name == self.prefix_ + "blue_lower_v":
+ validation_result = ParameterValidators.bounds(param, 0, 255)
+ if validation_result:
+ return SetParametersResult(successful=False, reason=validation_result)
+ updated_params.blue_lower_v = param.value
+ self.logger_.debug(param.name + ": " + param.type_.name + " = " + str(param.value))
+
+ if param.name == self.prefix_ + "blue_upper_v":
+ validation_result = ParameterValidators.bounds(param, 0, 255)
+ if validation_result:
+ return SetParametersResult(successful=False, reason=validation_result)
+ updated_params.blue_upper_v = param.value
+ self.logger_.debug(param.name + ": " + param.type_.name + " = " + str(param.value))
+
+ if param.name == self.prefix_ + "red_lower_h":
+ validation_result = ParameterValidators.bounds(param, 0, 179)
+ if validation_result:
+ return SetParametersResult(successful=False, reason=validation_result)
+ updated_params.red_lower_h = param.value
+ self.logger_.debug(param.name + ": " + param.type_.name + " = " + str(param.value))
+
+ if param.name == self.prefix_ + "red_upper_h":
+ validation_result = ParameterValidators.bounds(param, 0, 179)
+ if validation_result:
+ return SetParametersResult(successful=False, reason=validation_result)
+ updated_params.red_upper_h = param.value
+ self.logger_.debug(param.name + ": " + param.type_.name + " = " + str(param.value))
+
+ if param.name == self.prefix_ + "red_lower_s":
+ validation_result = ParameterValidators.bounds(param, 0, 255)
+ if validation_result:
+ return SetParametersResult(successful=False, reason=validation_result)
+ updated_params.red_lower_s = param.value
+ self.logger_.debug(param.name + ": " + param.type_.name + " = " + str(param.value))
+
+ if param.name == self.prefix_ + "red_upper_s":
+ validation_result = ParameterValidators.bounds(param, 0, 255)
+ if validation_result:
+ return SetParametersResult(successful=False, reason=validation_result)
+ updated_params.red_upper_s = param.value
+ self.logger_.debug(param.name + ": " + param.type_.name + " = " + str(param.value))
+
+ if param.name == self.prefix_ + "red_lower_v":
+ validation_result = ParameterValidators.bounds(param, 0, 255)
+ if validation_result:
+ return SetParametersResult(successful=False, reason=validation_result)
+ updated_params.red_lower_v = param.value
+ self.logger_.debug(param.name + ": " + param.type_.name + " = " + str(param.value))
+
+ if param.name == self.prefix_ + "red_upper_v":
+ validation_result = ParameterValidators.bounds(param, 0, 255)
+ if validation_result:
+ return SetParametersResult(successful=False, reason=validation_result)
+ updated_params.red_upper_v = param.value
+ self.logger_.debug(param.name + ": " + param.type_.name + " = " + str(param.value))
+
+ if param.name == self.prefix_ + "min_size":
+ validation_result = ParameterValidators.gt_eq(param, 0)
+ if validation_result:
+ return SetParametersResult(successful=False, reason=validation_result)
+ updated_params.min_size = param.value
+ self.logger_.debug(param.name + ": " + param.type_.name + " = " + str(param.value))
+
+ if param.name == self.prefix_ + "max_size":
+ validation_result = ParameterValidators.gt_eq(param, 0)
+ if validation_result:
+ return SetParametersResult(successful=False, reason=validation_result)
+ updated_params.max_size = param.value
+ self.logger_.debug(param.name + ": " + param.type_.name + " = " + str(param.value))
+
+ if param.name == self.prefix_ + "debug_mode":
+ updated_params.debug_mode = param.value
+ self.logger_.debug(param.name + ": " + param.type_.name + " = " + str(param.value))
+
+ updated_params.stamp_ = self.clock_.now()
+ self.update_internal_params(updated_params)
+ return SetParametersResult(successful=True)
+
+ def update_internal_params(self, updated_params):
+ self.params_ = updated_params
+
+ def declare_params(self):
+ updated_params = self.get_params()
+ # declare all parameters and give default values to non-required ones
+ if not self.node_.has_parameter(self.prefix_ + "blue_lower_h"):
+ descriptor = ParameterDescriptor(
+ description="hue value of the lower boundary for blue obstacles", read_only=False
+ )
+ descriptor.integer_range.append(IntegerRange())
+ descriptor.integer_range[-1].from_value = 0
+ descriptor.integer_range[-1].to_value = 179
+ parameter = updated_params.blue_lower_h
+ self.node_.declare_parameter(self.prefix_ + "blue_lower_h", parameter, descriptor)
+
+ if not self.node_.has_parameter(self.prefix_ + "blue_upper_h"):
+ descriptor = ParameterDescriptor(
+ description="hue value of the upper boundary for blue obstacles", read_only=False
+ )
+ descriptor.integer_range.append(IntegerRange())
+ descriptor.integer_range[-1].from_value = 0
+ descriptor.integer_range[-1].to_value = 179
+ parameter = updated_params.blue_upper_h
+ self.node_.declare_parameter(self.prefix_ + "blue_upper_h", parameter, descriptor)
+
+ if not self.node_.has_parameter(self.prefix_ + "blue_lower_s"):
+ descriptor = ParameterDescriptor(
+ description="separation value of the lower boundary for blue obstacles", read_only=False
+ )
+ descriptor.integer_range.append(IntegerRange())
+ descriptor.integer_range[-1].from_value = 0
+ descriptor.integer_range[-1].to_value = 255
+ parameter = updated_params.blue_lower_s
+ self.node_.declare_parameter(self.prefix_ + "blue_lower_s", parameter, descriptor)
+
+ if not self.node_.has_parameter(self.prefix_ + "blue_upper_s"):
+ descriptor = ParameterDescriptor(
+ description="separation value of the upper boundary for blue obstacles", read_only=False
+ )
+ descriptor.integer_range.append(IntegerRange())
+ descriptor.integer_range[-1].from_value = 0
+ descriptor.integer_range[-1].to_value = 255
+ parameter = updated_params.blue_upper_s
+ self.node_.declare_parameter(self.prefix_ + "blue_upper_s", parameter, descriptor)
+
+ if not self.node_.has_parameter(self.prefix_ + "blue_lower_v"):
+ descriptor = ParameterDescriptor(
+ description="value value of the lower boundary for blue obstacles", read_only=False
+ )
+ descriptor.integer_range.append(IntegerRange())
+ descriptor.integer_range[-1].from_value = 0
+ descriptor.integer_range[-1].to_value = 255
+ parameter = updated_params.blue_lower_v
+ self.node_.declare_parameter(self.prefix_ + "blue_lower_v", parameter, descriptor)
+
+ if not self.node_.has_parameter(self.prefix_ + "blue_upper_v"):
+ descriptor = ParameterDescriptor(
+ description="value value of the upper boundary for blue obstacles", read_only=False
+ )
+ descriptor.integer_range.append(IntegerRange())
+ descriptor.integer_range[-1].from_value = 0
+ descriptor.integer_range[-1].to_value = 255
+ parameter = updated_params.blue_upper_v
+ self.node_.declare_parameter(self.prefix_ + "blue_upper_v", parameter, descriptor)
+
+ if not self.node_.has_parameter(self.prefix_ + "red_lower_h"):
+ descriptor = ParameterDescriptor(
+ description="hue value of the lower boundary for red obstacles", read_only=False
+ )
+ descriptor.integer_range.append(IntegerRange())
+ descriptor.integer_range[-1].from_value = 0
+ descriptor.integer_range[-1].to_value = 179
+ parameter = updated_params.red_lower_h
+ self.node_.declare_parameter(self.prefix_ + "red_lower_h", parameter, descriptor)
+
+ if not self.node_.has_parameter(self.prefix_ + "red_upper_h"):
+ descriptor = ParameterDescriptor(
+ description="hue value of the upper boundary for red obstacles", read_only=False
+ )
+ descriptor.integer_range.append(IntegerRange())
+ descriptor.integer_range[-1].from_value = 0
+ descriptor.integer_range[-1].to_value = 179
+ parameter = updated_params.red_upper_h
+ self.node_.declare_parameter(self.prefix_ + "red_upper_h", parameter, descriptor)
+
+ if not self.node_.has_parameter(self.prefix_ + "red_lower_s"):
+ descriptor = ParameterDescriptor(
+ description="separation value of the lower boundary for red obstacles", read_only=False
+ )
+ descriptor.integer_range.append(IntegerRange())
+ descriptor.integer_range[-1].from_value = 0
+ descriptor.integer_range[-1].to_value = 255
+ parameter = updated_params.red_lower_s
+ self.node_.declare_parameter(self.prefix_ + "red_lower_s", parameter, descriptor)
+
+ if not self.node_.has_parameter(self.prefix_ + "red_upper_s"):
+ descriptor = ParameterDescriptor(
+ description="separation value of the upper boundary for red obstacles", read_only=False
+ )
+ descriptor.integer_range.append(IntegerRange())
+ descriptor.integer_range[-1].from_value = 0
+ descriptor.integer_range[-1].to_value = 255
+ parameter = updated_params.red_upper_s
+ self.node_.declare_parameter(self.prefix_ + "red_upper_s", parameter, descriptor)
+
+ if not self.node_.has_parameter(self.prefix_ + "red_lower_v"):
+ descriptor = ParameterDescriptor(
+ description="value value of the lower boundary for red obstacles", read_only=False
+ )
+ descriptor.integer_range.append(IntegerRange())
+ descriptor.integer_range[-1].from_value = 0
+ descriptor.integer_range[-1].to_value = 255
+ parameter = updated_params.red_lower_v
+ self.node_.declare_parameter(self.prefix_ + "red_lower_v", parameter, descriptor)
+
+ if not self.node_.has_parameter(self.prefix_ + "red_upper_v"):
+ descriptor = ParameterDescriptor(
+ description="value value of the upper boundary for red obstacles", read_only=False
+ )
+ descriptor.integer_range.append(IntegerRange())
+ descriptor.integer_range[-1].from_value = 0
+ descriptor.integer_range[-1].to_value = 255
+ parameter = updated_params.red_upper_v
+ self.node_.declare_parameter(self.prefix_ + "red_upper_v", parameter, descriptor)
+
+ if not self.node_.has_parameter(self.prefix_ + "min_size"):
+ descriptor = ParameterDescriptor(
+ description="minimum size of an obstacle to be considered", read_only=False
+ )
+ descriptor.integer_range.append(IntegerRange())
+ descriptor.integer_range[-1].from_value = 0
+ descriptor.integer_range[-1].to_value = 2**31 - 1
+ parameter = updated_params.min_size
+ self.node_.declare_parameter(self.prefix_ + "min_size", parameter, descriptor)
+
+ if not self.node_.has_parameter(self.prefix_ + "max_size"):
+ descriptor = ParameterDescriptor(
+ description="maximum size of an obstacle to be considered", read_only=False
+ )
+ descriptor.integer_range.append(IntegerRange())
+ descriptor.integer_range[-1].from_value = 0
+ descriptor.integer_range[-1].to_value = 2**31 - 1
+ parameter = updated_params.max_size
+ self.node_.declare_parameter(self.prefix_ + "max_size", parameter, descriptor)
+
+ if not self.node_.has_parameter(self.prefix_ + "debug_mode"):
+ descriptor = ParameterDescriptor(
+ description="set true if debug images should be drawn and published", read_only=False
+ )
+ parameter = updated_params.debug_mode
+ self.node_.declare_parameter(self.prefix_ + "debug_mode", parameter, descriptor)
+
+ # TODO: need validation
+ # get parameters and fill struct fields
+ param = self.node_.get_parameter(self.prefix_ + "blue_lower_h")
+ self.logger_.debug(param.name + ": " + param.type_.name + " = " + str(param.value))
+ validation_result = ParameterValidators.bounds(param, 0, 179)
+ if validation_result:
+ raise InvalidParameterValueException(
+ "blue_lower_h",
+ param.value,
+ "Invalid value set during initialization for parameter blue_lower_h: " + validation_result,
+ )
+ updated_params.blue_lower_h = param.value
+ param = self.node_.get_parameter(self.prefix_ + "blue_upper_h")
+ self.logger_.debug(param.name + ": " + param.type_.name + " = " + str(param.value))
+ validation_result = ParameterValidators.bounds(param, 0, 179)
+ if validation_result:
+ raise InvalidParameterValueException(
+ "blue_upper_h",
+ param.value,
+ "Invalid value set during initialization for parameter blue_upper_h: " + validation_result,
+ )
+ updated_params.blue_upper_h = param.value
+ param = self.node_.get_parameter(self.prefix_ + "blue_lower_s")
+ self.logger_.debug(param.name + ": " + param.type_.name + " = " + str(param.value))
+ validation_result = ParameterValidators.bounds(param, 0, 255)
+ if validation_result:
+ raise InvalidParameterValueException(
+ "blue_lower_s",
+ param.value,
+ "Invalid value set during initialization for parameter blue_lower_s: " + validation_result,
+ )
+ updated_params.blue_lower_s = param.value
+ param = self.node_.get_parameter(self.prefix_ + "blue_upper_s")
+ self.logger_.debug(param.name + ": " + param.type_.name + " = " + str(param.value))
+ validation_result = ParameterValidators.bounds(param, 0, 255)
+ if validation_result:
+ raise InvalidParameterValueException(
+ "blue_upper_s",
+ param.value,
+ "Invalid value set during initialization for parameter blue_upper_s: " + validation_result,
+ )
+ updated_params.blue_upper_s = param.value
+ param = self.node_.get_parameter(self.prefix_ + "blue_lower_v")
+ self.logger_.debug(param.name + ": " + param.type_.name + " = " + str(param.value))
+ validation_result = ParameterValidators.bounds(param, 0, 255)
+ if validation_result:
+ raise InvalidParameterValueException(
+ "blue_lower_v",
+ param.value,
+ "Invalid value set during initialization for parameter blue_lower_v: " + validation_result,
+ )
+ updated_params.blue_lower_v = param.value
+ param = self.node_.get_parameter(self.prefix_ + "blue_upper_v")
+ self.logger_.debug(param.name + ": " + param.type_.name + " = " + str(param.value))
+ validation_result = ParameterValidators.bounds(param, 0, 255)
+ if validation_result:
+ raise InvalidParameterValueException(
+ "blue_upper_v",
+ param.value,
+ "Invalid value set during initialization for parameter blue_upper_v: " + validation_result,
+ )
+ updated_params.blue_upper_v = param.value
+ param = self.node_.get_parameter(self.prefix_ + "red_lower_h")
+ self.logger_.debug(param.name + ": " + param.type_.name + " = " + str(param.value))
+ validation_result = ParameterValidators.bounds(param, 0, 179)
+ if validation_result:
+ raise InvalidParameterValueException(
+ "red_lower_h",
+ param.value,
+ "Invalid value set during initialization for parameter red_lower_h: " + validation_result,
+ )
+ updated_params.red_lower_h = param.value
+ param = self.node_.get_parameter(self.prefix_ + "red_upper_h")
+ self.logger_.debug(param.name + ": " + param.type_.name + " = " + str(param.value))
+ validation_result = ParameterValidators.bounds(param, 0, 179)
+ if validation_result:
+ raise InvalidParameterValueException(
+ "red_upper_h",
+ param.value,
+ "Invalid value set during initialization for parameter red_upper_h: " + validation_result,
+ )
+ updated_params.red_upper_h = param.value
+ param = self.node_.get_parameter(self.prefix_ + "red_lower_s")
+ self.logger_.debug(param.name + ": " + param.type_.name + " = " + str(param.value))
+ validation_result = ParameterValidators.bounds(param, 0, 255)
+ if validation_result:
+ raise InvalidParameterValueException(
+ "red_lower_s",
+ param.value,
+ "Invalid value set during initialization for parameter red_lower_s: " + validation_result,
+ )
+ updated_params.red_lower_s = param.value
+ param = self.node_.get_parameter(self.prefix_ + "red_upper_s")
+ self.logger_.debug(param.name + ": " + param.type_.name + " = " + str(param.value))
+ validation_result = ParameterValidators.bounds(param, 0, 255)
+ if validation_result:
+ raise InvalidParameterValueException(
+ "red_upper_s",
+ param.value,
+ "Invalid value set during initialization for parameter red_upper_s: " + validation_result,
+ )
+ updated_params.red_upper_s = param.value
+ param = self.node_.get_parameter(self.prefix_ + "red_lower_v")
+ self.logger_.debug(param.name + ": " + param.type_.name + " = " + str(param.value))
+ validation_result = ParameterValidators.bounds(param, 0, 255)
+ if validation_result:
+ raise InvalidParameterValueException(
+ "red_lower_v",
+ param.value,
+ "Invalid value set during initialization for parameter red_lower_v: " + validation_result,
+ )
+ updated_params.red_lower_v = param.value
+ param = self.node_.get_parameter(self.prefix_ + "red_upper_v")
+ self.logger_.debug(param.name + ": " + param.type_.name + " = " + str(param.value))
+ validation_result = ParameterValidators.bounds(param, 0, 255)
+ if validation_result:
+ raise InvalidParameterValueException(
+ "red_upper_v",
+ param.value,
+ "Invalid value set during initialization for parameter red_upper_v: " + validation_result,
+ )
+ updated_params.red_upper_v = param.value
+ param = self.node_.get_parameter(self.prefix_ + "min_size")
+ self.logger_.debug(param.name + ": " + param.type_.name + " = " + str(param.value))
+ validation_result = ParameterValidators.gt_eq(param, 0)
+ if validation_result:
+ raise InvalidParameterValueException(
+ "min_size",
+ param.value,
+ "Invalid value set during initialization for parameter min_size: " + validation_result,
+ )
+ updated_params.min_size = param.value
+ param = self.node_.get_parameter(self.prefix_ + "max_size")
+ self.logger_.debug(param.name + ": " + param.type_.name + " = " + str(param.value))
+ validation_result = ParameterValidators.gt_eq(param, 0)
+ if validation_result:
+ raise InvalidParameterValueException(
+ "max_size",
+ param.value,
+ "Invalid value set during initialization for parameter max_size: " + validation_result,
+ )
+ updated_params.max_size = param.value
+ param = self.node_.get_parameter(self.prefix_ + "debug_mode")
+ self.logger_.debug(param.name + ": " + param.type_.name + " = " + str(param.value))
+ updated_params.debug_mode = param.value
+
+ self.update_internal_params(updated_params)
diff --git a/bitbots_misc/bitbots_technical_challenge_vision/config/range.yaml b/bitbots_misc/bitbots_technical_challenge_vision/config/range.yaml
new file mode 100644
index 000000000..6716d55af
--- /dev/null
+++ b/bitbots_misc/bitbots_technical_challenge_vision/config/range.yaml
@@ -0,0 +1,118 @@
+bitbots_technical_challenge_vision:
+ blue_lower_h: {
+ type: int,
+ default_value: 92,
+ description: "hue value of the lower boundary for blue obstacles",
+ validation: {
+ bounds<>: [0, 179]
+ }
+ }
+ blue_upper_h: {
+ type: int,
+ default_value: 110,
+ description: "hue value of the upper boundary for blue obstacles",
+ validation: {
+ bounds<>: [0, 179]
+ }
+ }
+ blue_lower_s: {
+ type: int,
+ default_value: 90,
+ description: "separation value of the lower boundary for blue obstacles",
+ validation: {
+ bounds<>: [0, 255]
+ }
+ }
+ blue_upper_s: {
+ type: int,
+ default_value: 236,
+ description: "separation value of the upper boundary for blue obstacles",
+ validation: {
+ bounds<>: [0, 255]
+ }
+ }
+ blue_lower_v: {
+ type: int,
+ default_value: 0,
+ description: "value value of the lower boundary for blue obstacles",
+ validation: {
+ bounds<>: [0, 255]
+ }
+ }
+ blue_upper_v: {
+ type: int,
+ default_value: 255,
+ description: "value value of the upper boundary for blue obstacles",
+ validation: {
+ bounds<>: [0, 255]
+ }
+ }
+ red_lower_h: {
+ type: int,
+ default_value: 138,
+ description: "hue value of the lower boundary for red obstacles",
+ validation: {
+ bounds<>: [0, 179]
+ }
+ }
+ red_upper_h: {
+ type: int,
+ default_value: 179,
+ description: "hue value of the upper boundary for red obstacles",
+ validation: {
+ bounds<>: [0, 179]
+ }
+ }
+ red_lower_s: {
+ type: int,
+ default_value: 78,
+ description: "separation value of the lower boundary for red obstacles",
+ validation: {
+ bounds<>: [0, 255]
+ }
+ }
+ red_upper_s: {
+ type: int,
+ default_value: 255,
+ description: "separation value of the upper boundary for red obstacles",
+ validation: {
+ bounds<>: [0, 255]
+ }
+ }
+ red_lower_v: {
+ type: int,
+ default_value: 0,
+ description: "value value of the lower boundary for red obstacles",
+ validation: {
+ bounds<>: [0, 255]
+ }
+ }
+ red_upper_v: {
+ type: int,
+ default_value: 255,
+ description: "value value of the upper boundary for red obstacles",
+ validation: {
+ bounds<>: [0, 255]
+ }
+ }
+ min_size: {
+ type: int,
+ default_value: 20,
+ description: "minimum size of an obstacle to be considered",
+ validation: {
+ gt_eq<>: 0
+ }
+ }
+ max_size: {
+ type: int,
+ default_value: 400,
+ description: "maximum size of an obstacle to be considered",
+ validation: {
+ gt_eq<>: 0
+ }
+ }
+ debug_mode: {
+ type: bool,
+ default_value: false,
+ description: "set true if debug images should be drawn and published",
+ }
diff --git a/bitbots_misc/bitbots_technical_challenge_vision/launch/vision.launch b/bitbots_misc/bitbots_technical_challenge_vision/launch/vision.launch
new file mode 100644
index 000000000..1d36dc25f
--- /dev/null
+++ b/bitbots_misc/bitbots_technical_challenge_vision/launch/vision.launch
@@ -0,0 +1,8 @@
+
+
+
+
+
+
+
+
diff --git a/bitbots_misc/bitbots_technical_challenge_vision/package.xml b/bitbots_misc/bitbots_technical_challenge_vision/package.xml
new file mode 100644
index 000000000..9c0a53d6e
--- /dev/null
+++ b/bitbots_misc/bitbots_technical_challenge_vision/package.xml
@@ -0,0 +1,27 @@
+
+
+
+ bitbots_technical_challenge_vision
+ 0.0.0
+ TODO: Package description
+ par
+ TODO: License declaration
+
+ rclpy
+ python3-opencv
+ python3-numpy
+ cv_bridge
+ sensor_msgs
+ soccer_vision_2d_msgs
+
+ generate_parameter_library
+
+ ament_copyright
+ ament_flake8
+ ament_pep257
+ python3-pytest
+
+
+ ament_python
+
+
diff --git a/bitbots_misc/bitbots_technical_challenge_vision/resource/bitbots_technical_challenge_vision b/bitbots_misc/bitbots_technical_challenge_vision/resource/bitbots_technical_challenge_vision
new file mode 100644
index 000000000..e69de29bb
diff --git a/bitbots_misc/bitbots_technical_challenge_vision/setup.cfg b/bitbots_misc/bitbots_technical_challenge_vision/setup.cfg
new file mode 100644
index 000000000..34ccaf1e9
--- /dev/null
+++ b/bitbots_misc/bitbots_technical_challenge_vision/setup.cfg
@@ -0,0 +1,4 @@
+[develop]
+script_dir=$base/lib/bitbots_technical_challenge_vision
+[install]
+install_scripts=$base/lib/bitbots_technical_challenge_vision
diff --git a/bitbots_misc/bitbots_technical_challenge_vision/setup.py b/bitbots_misc/bitbots_technical_challenge_vision/setup.py
new file mode 100644
index 000000000..4603d372c
--- /dev/null
+++ b/bitbots_misc/bitbots_technical_challenge_vision/setup.py
@@ -0,0 +1,34 @@
+import glob
+
+from generate_parameter_library_py.setup_helper import generate_parameter_module
+from setuptools import find_packages, setup
+
+package_name = "bitbots_technical_challenge_vision"
+
+setup(
+ name=package_name,
+ version="0.0.0",
+ packages=find_packages(exclude=["test"]),
+ data_files=[
+ ("share/ament_index/resource_index/packages", ["resource/" + package_name]),
+ ("share/" + package_name, ["package.xml"]),
+ ("share/" + package_name + "/config", glob.glob("config/*.yaml")),
+ ("share/" + package_name + "/launch", glob.glob("launch/*.launch")),
+ ],
+ install_requires=["setuptools"],
+ zip_safe=True,
+ maintainer="par",
+ maintainer_email="paer-wiegmann@gmx.de",
+ description="This Package provides a simple vision to detect the obstacles for the obstacle avoidance challenge.",
+ license="MIT",
+ entry_points={
+ "console_scripts": [
+ "bitbots_technical_challenge_vision = bitbots_technical_challenge_vision.bitbots_technical_challenge_vision:main",
+ ],
+ },
+)
+
+generate_parameter_module(
+ "bitbots_technical_challenge_vision_params",
+ "config/range.yaml",
+)
diff --git a/sync_includes_wolfgang_nuc.yaml b/sync_includes_wolfgang_nuc.yaml
index 7768d2b72..7dce97899 100644
--- a/sync_includes_wolfgang_nuc.yaml
+++ b/sync_includes_wolfgang_nuc.yaml
@@ -14,6 +14,7 @@ include:
- bitbots_ipm
- bitbots_parameter_blackboard
- bitbots_robot_description
+ - bitbots_technical_challenge_vision
- bitbots_teleop
- bitbots_tf_buffer
- bitbots_tts