From 9d45a98910a72ea2968a8df73781aebe3b358f40 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?P=C3=A4r?= Date: Sat, 18 May 2024 08:05:53 +0200 Subject: [PATCH 01/31] add empty package --- .../technical_challenge_vision/package.xml | 18 +++++++++++++ .../resource/technical_challenge_vision | 0 .../technical_challenge_vision/setup.cfg | 4 +++ .../technical_challenge_vision/setup.py | 23 +++++++++++++++++ .../technical_challenge_vision/__init__.py | 0 .../test/test_copyright.py | 25 +++++++++++++++++++ .../test/test_flake8.py | 23 +++++++++++++++++ .../test/test_pep257.py | 23 +++++++++++++++++ 8 files changed, 116 insertions(+) create mode 100644 bitbots_misc/technical_challenge_vision/package.xml create mode 100644 bitbots_misc/technical_challenge_vision/resource/technical_challenge_vision create mode 100644 bitbots_misc/technical_challenge_vision/setup.cfg create mode 100644 bitbots_misc/technical_challenge_vision/setup.py create mode 100644 bitbots_misc/technical_challenge_vision/technical_challenge_vision/__init__.py create mode 100644 bitbots_misc/technical_challenge_vision/test/test_copyright.py create mode 100644 bitbots_misc/technical_challenge_vision/test/test_flake8.py create mode 100644 bitbots_misc/technical_challenge_vision/test/test_pep257.py diff --git a/bitbots_misc/technical_challenge_vision/package.xml b/bitbots_misc/technical_challenge_vision/package.xml new file mode 100644 index 000000000..093659ce8 --- /dev/null +++ b/bitbots_misc/technical_challenge_vision/package.xml @@ -0,0 +1,18 @@ + + + + technical_challenge_vision + 0.0.0 + TODO: Package description + par + TODO: License declaration + + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + + + ament_python + + diff --git a/bitbots_misc/technical_challenge_vision/resource/technical_challenge_vision b/bitbots_misc/technical_challenge_vision/resource/technical_challenge_vision new file mode 100644 index 000000000..e69de29bb diff --git a/bitbots_misc/technical_challenge_vision/setup.cfg b/bitbots_misc/technical_challenge_vision/setup.cfg new file mode 100644 index 000000000..7e2825e6e --- /dev/null +++ b/bitbots_misc/technical_challenge_vision/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/technical_challenge_vision +[install] +install_scripts=$base/lib/technical_challenge_vision diff --git a/bitbots_misc/technical_challenge_vision/setup.py b/bitbots_misc/technical_challenge_vision/setup.py new file mode 100644 index 000000000..0f67d9809 --- /dev/null +++ b/bitbots_misc/technical_challenge_vision/setup.py @@ -0,0 +1,23 @@ +from setuptools import find_packages, setup + +package_name = "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"]), + ], + install_requires=["setuptools"], + zip_safe=True, + maintainer="par", + maintainer_email="paer-wiegmann@gmx.de", + description="TODO: Package description", + license="TODO: License declaration", + tests_require=["pytest"], + entry_points={ + "console_scripts": [], + }, +) diff --git a/bitbots_misc/technical_challenge_vision/technical_challenge_vision/__init__.py b/bitbots_misc/technical_challenge_vision/technical_challenge_vision/__init__.py new file mode 100644 index 000000000..e69de29bb diff --git a/bitbots_misc/technical_challenge_vision/test/test_copyright.py b/bitbots_misc/technical_challenge_vision/test/test_copyright.py new file mode 100644 index 000000000..60c2d1e68 --- /dev/null +++ b/bitbots_misc/technical_challenge_vision/test/test_copyright.py @@ -0,0 +1,25 @@ +# Copyright 2015 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import pytest +from ament_copyright.main import main + + +# Remove the `skip` decorator once the source file(s) have a copyright header +@pytest.mark.skip(reason="No copyright header has been placed in the generated source file.") +@pytest.mark.copyright +@pytest.mark.linter +def test_copyright(): + rc = main(argv=[".", "test"]) + assert rc == 0, "Found errors" diff --git a/bitbots_misc/technical_challenge_vision/test/test_flake8.py b/bitbots_misc/technical_challenge_vision/test/test_flake8.py new file mode 100644 index 000000000..22fffcb86 --- /dev/null +++ b/bitbots_misc/technical_challenge_vision/test/test_flake8.py @@ -0,0 +1,23 @@ +# Copyright 2017 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import pytest +from ament_flake8.main import main_with_errors + + +@pytest.mark.flake8 +@pytest.mark.linter +def test_flake8(): + rc, errors = main_with_errors(argv=[]) + assert rc == 0, "Found %d code style errors / warnings:\n" % len(errors) + "\n".join(errors) diff --git a/bitbots_misc/technical_challenge_vision/test/test_pep257.py b/bitbots_misc/technical_challenge_vision/test/test_pep257.py new file mode 100644 index 000000000..4eddb46ed --- /dev/null +++ b/bitbots_misc/technical_challenge_vision/test/test_pep257.py @@ -0,0 +1,23 @@ +# Copyright 2015 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import pytest +from ament_pep257.main import main + + +@pytest.mark.linter +@pytest.mark.pep257 +def test_pep257(): + rc = main(argv=[".", "test"]) + assert rc == 0, "Found code style errors / warnings" From 2ff67059c9be3fba661df6529c5a582dd07494e5 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?P=C3=A4r?= Date: Sat, 18 May 2024 11:00:12 +0200 Subject: [PATCH 02/31] add messages for vision --- bitbots_msgs/CMakeLists.txt | 3 +++ bitbots_msgs/msg/BoundingBox.msg | 6 ++++++ bitbots_msgs/msg/CircleInImage.msg | 3 +++ bitbots_msgs/msg/CircleInImageArray.msg | 2 ++ 4 files changed, 14 insertions(+) create mode 100644 bitbots_msgs/msg/BoundingBox.msg create mode 100644 bitbots_msgs/msg/CircleInImage.msg create mode 100644 bitbots_msgs/msg/CircleInImageArray.msg diff --git a/bitbots_msgs/CMakeLists.txt b/bitbots_msgs/CMakeLists.txt index 2488d45d1..043a82221 100644 --- a/bitbots_msgs/CMakeLists.txt +++ b/bitbots_msgs/CMakeLists.txt @@ -23,7 +23,10 @@ rosidl_generate_interfaces( "action/PlayAnimation.action" "msg/Animation.msg" "msg/Audio.msg" + "msg/BoundingBox.msg" "msg/Buttons.msg" + "msg/CircleInImage.msg" + "msg/CircleInImageArray.msg" "msg/Cpu.msg" "msg/Filesystem.msg" "msg/FootPressure.msg" diff --git a/bitbots_msgs/msg/BoundingBox.msg b/bitbots_msgs/msg/BoundingBox.msg new file mode 100644 index 000000000..3ccf1662a --- /dev/null +++ b/bitbots_msgs/msg/BoundingBox.msg @@ -0,0 +1,6 @@ +std_msgs/Header header + +uint8 top_left_x +uint8 top_left_y +uint8 height +uint8 width diff --git a/bitbots_msgs/msg/CircleInImage.msg b/bitbots_msgs/msg/CircleInImage.msg new file mode 100644 index 000000000..670c8efdb --- /dev/null +++ b/bitbots_msgs/msg/CircleInImage.msg @@ -0,0 +1,3 @@ +uint8 center_x +uint8 center_y +uint8 diameter diff --git a/bitbots_msgs/msg/CircleInImageArray.msg b/bitbots_msgs/msg/CircleInImageArray.msg new file mode 100644 index 000000000..5f3e9790b --- /dev/null +++ b/bitbots_msgs/msg/CircleInImageArray.msg @@ -0,0 +1,2 @@ +std_msgs/Header header +bitbots_msgs/CircleInImage[] circles From 182ed0e84f18883efc759b870b8c38f1c56494de Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?P=C3=A4r?= Date: Sat, 18 May 2024 11:08:41 +0200 Subject: [PATCH 03/31] add array message for bounding boxes --- bitbots_msgs/CMakeLists.txt | 1 + bitbots_msgs/msg/BoundingBox.msg | 2 -- bitbots_msgs/msg/BoundingBoxArray.msg | 3 +++ 3 files changed, 4 insertions(+), 2 deletions(-) create mode 100644 bitbots_msgs/msg/BoundingBoxArray.msg diff --git a/bitbots_msgs/CMakeLists.txt b/bitbots_msgs/CMakeLists.txt index 043a82221..11a6d54fe 100644 --- a/bitbots_msgs/CMakeLists.txt +++ b/bitbots_msgs/CMakeLists.txt @@ -24,6 +24,7 @@ rosidl_generate_interfaces( "msg/Animation.msg" "msg/Audio.msg" "msg/BoundingBox.msg" + "msg/BoundingBoxArray.msg" "msg/Buttons.msg" "msg/CircleInImage.msg" "msg/CircleInImageArray.msg" diff --git a/bitbots_msgs/msg/BoundingBox.msg b/bitbots_msgs/msg/BoundingBox.msg index 3ccf1662a..a06a0c6c5 100644 --- a/bitbots_msgs/msg/BoundingBox.msg +++ b/bitbots_msgs/msg/BoundingBox.msg @@ -1,5 +1,3 @@ -std_msgs/Header header - uint8 top_left_x uint8 top_left_y uint8 height diff --git a/bitbots_msgs/msg/BoundingBoxArray.msg b/bitbots_msgs/msg/BoundingBoxArray.msg new file mode 100644 index 000000000..cd72c1980 --- /dev/null +++ b/bitbots_msgs/msg/BoundingBoxArray.msg @@ -0,0 +1,3 @@ +std_msgs/Header header + +bitbots_msgs/BoundingBox[] boxes From d8533fef8823a8e70f9a26bf1ffe0db7899c1f26 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?P=C3=A4r?= Date: Mon, 20 May 2024 20:40:30 +0200 Subject: [PATCH 04/31] add seperate lists for different obstacles --- bitbots_msgs/msg/BoundingBoxArray.msg | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/bitbots_msgs/msg/BoundingBoxArray.msg b/bitbots_msgs/msg/BoundingBoxArray.msg index cd72c1980..3fcd8bc00 100644 --- a/bitbots_msgs/msg/BoundingBoxArray.msg +++ b/bitbots_msgs/msg/BoundingBoxArray.msg @@ -1,3 +1,4 @@ std_msgs/Header header -bitbots_msgs/BoundingBox[] boxes +bitbots_msgs/BoundingBox[] blue_boxes +bitbots_msgs/BoundingBox[] red_boxes From f7a3bfb5a8c32711ecfb39b300542b66ebe218c5 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?P=C3=A4r?= Date: Mon, 20 May 2024 20:41:18 +0200 Subject: [PATCH 05/31] add dependencies for ros messages and dynamic reconfigure --- bitbots_misc/technical_challenge_vision/package.xml | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/bitbots_misc/technical_challenge_vision/package.xml b/bitbots_misc/technical_challenge_vision/package.xml index 093659ce8..c6aac8c0d 100644 --- a/bitbots_misc/technical_challenge_vision/package.xml +++ b/bitbots_misc/technical_challenge_vision/package.xml @@ -7,6 +7,11 @@ par TODO: License declaration + rclpy + bitbots_msgs + + generate_parameter_library + ament_copyright ament_flake8 ament_pep257 From 12bddf022d61c9570a54dfd13ebf9802a5a0d784 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?P=C3=A4r?= Date: Mon, 20 May 2024 20:44:35 +0200 Subject: [PATCH 06/31] remove unused tests --- .../test/test_copyright.py | 25 ------------------- .../test/test_flake8.py | 23 ----------------- .../test/test_pep257.py | 23 ----------------- 3 files changed, 71 deletions(-) delete mode 100644 bitbots_misc/technical_challenge_vision/test/test_copyright.py delete mode 100644 bitbots_misc/technical_challenge_vision/test/test_flake8.py delete mode 100644 bitbots_misc/technical_challenge_vision/test/test_pep257.py diff --git a/bitbots_misc/technical_challenge_vision/test/test_copyright.py b/bitbots_misc/technical_challenge_vision/test/test_copyright.py deleted file mode 100644 index 60c2d1e68..000000000 --- a/bitbots_misc/technical_challenge_vision/test/test_copyright.py +++ /dev/null @@ -1,25 +0,0 @@ -# Copyright 2015 Open Source Robotics Foundation, Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -import pytest -from ament_copyright.main import main - - -# Remove the `skip` decorator once the source file(s) have a copyright header -@pytest.mark.skip(reason="No copyright header has been placed in the generated source file.") -@pytest.mark.copyright -@pytest.mark.linter -def test_copyright(): - rc = main(argv=[".", "test"]) - assert rc == 0, "Found errors" diff --git a/bitbots_misc/technical_challenge_vision/test/test_flake8.py b/bitbots_misc/technical_challenge_vision/test/test_flake8.py deleted file mode 100644 index 22fffcb86..000000000 --- a/bitbots_misc/technical_challenge_vision/test/test_flake8.py +++ /dev/null @@ -1,23 +0,0 @@ -# Copyright 2017 Open Source Robotics Foundation, Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -import pytest -from ament_flake8.main import main_with_errors - - -@pytest.mark.flake8 -@pytest.mark.linter -def test_flake8(): - rc, errors = main_with_errors(argv=[]) - assert rc == 0, "Found %d code style errors / warnings:\n" % len(errors) + "\n".join(errors) diff --git a/bitbots_misc/technical_challenge_vision/test/test_pep257.py b/bitbots_misc/technical_challenge_vision/test/test_pep257.py deleted file mode 100644 index 4eddb46ed..000000000 --- a/bitbots_misc/technical_challenge_vision/test/test_pep257.py +++ /dev/null @@ -1,23 +0,0 @@ -# Copyright 2015 Open Source Robotics Foundation, Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -import pytest -from ament_pep257.main import main - - -@pytest.mark.linter -@pytest.mark.pep257 -def test_pep257(): - rc = main(argv=[".", "test"]) - assert rc == 0, "Found code style errors / warnings" From 5c2fa8e4ad95b6c9169c3a05fe6637a94eb31500 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?P=C3=A4r?= Date: Mon, 20 May 2024 20:53:51 +0200 Subject: [PATCH 07/31] add a bunch of stuff to setup --- .../technical_challenge_vision/setup.py | 19 +++++++++++++++---- 1 file changed, 15 insertions(+), 4 deletions(-) diff --git a/bitbots_misc/technical_challenge_vision/setup.py b/bitbots_misc/technical_challenge_vision/setup.py index 0f67d9809..8b1a0d312 100644 --- a/bitbots_misc/technical_challenge_vision/setup.py +++ b/bitbots_misc/technical_challenge_vision/setup.py @@ -1,3 +1,6 @@ +import glob + +from generate_parameter_library_py.setup_helper import generate_parameter_module from setuptools import find_packages, setup package_name = "technical_challenge_vision" @@ -9,15 +12,23 @@ 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="TODO: Package description", - license="TODO: License declaration", - tests_require=["pytest"], + description="This Package provides a simple vision to detect the obstacles for the obstacle avoidance challenge.", + license="MIT", entry_points={ - "console_scripts": [], + "console_scripts": [ + "technical_challenge_vision = technical_challenge_vision.technical_challenge_vision:main", + ], }, ) + +generate_parameter_module( + "technical_challenge_vision_params", + "config/range.yaml", +) From 03a9ef67efd1d9be39e83182ff4b6ae20cd84d5c Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?P=C3=A4r?= Date: Mon, 20 May 2024 20:57:44 +0200 Subject: [PATCH 08/31] add vision node --- .../technical_challenge_vision.py | 143 ++++++++++++++++++ 1 file changed, 143 insertions(+) create mode 100755 bitbots_misc/technical_challenge_vision/technical_challenge_vision/technical_challenge_vision.py diff --git a/bitbots_misc/technical_challenge_vision/technical_challenge_vision/technical_challenge_vision.py b/bitbots_misc/technical_challenge_vision/technical_challenge_vision/technical_challenge_vision.py new file mode 100755 index 000000000..9a94e1ca3 --- /dev/null +++ b/bitbots_misc/technical_challenge_vision/technical_challenge_vision/technical_challenge_vision.py @@ -0,0 +1,143 @@ +from typing import 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 bitbots_msgs.msg import BoundingBox, BoundingBoxArray +from technical_challenge_vision.technical_challenge_vision_params import technical_challenge_vision + + +class TechnicalChallengeVision(Node): + def __init__(self): + super().__init__("technical_challenge_vision") + + self._package_path = get_package_share_directory("technical_challenge_vision") + self._cv_bridge = CvBridge() + self._annotations_pub = self.create_publisher(BoundingBoxArray, "/technical_challenge_vision", 10) + self._debug_img_pub = self.create_publisher(Image, "/technical_challenge_vision_debug_img", 10) + self._debug_clrmp_pub_blue = self.create_publisher(Image, "/technical_challenge_vision_debug_clrmp_blue", 10) + self._debug_clrmp_pub_red = self.create_publisher(Image, "/technical_challenge_vision_debug_clrmp_red", 10) + self._img_sub = self.create_subscription(Image, "/camera/image_proc", self.image_callback, 10) + self._param_listener = technical_challenge_vision.ParamListener(self) + self._params = self._param_listener.get_params() + + def process_image(self, img: np.ndarray) -> Tuple[list[BoundingBox], list[BoundingBox], np.ndarray, np.ndarray]: + """ + gets annotations from the camera image + + :param img: ndarray containing the camera image + """ + # convert img to hsv to isolate colors better + img = cv2.cvtColor(img, cv2.COLOR_BGR2HSV) + + # get dynamic parameters + arg = self._params.ros__parameters + + # 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_boxes = [] + red_boxes = [] + + for cnt in blue_contours: + x, y, h, w = cv2.boundingRect(cnt) + if min(h, w) >= arg.min_size and max(h, w) <= arg.max_size: + blue_boxes.append(BoundingBox(top_left_x=x, top_left_y=y, height=h, width=w)) + + for cnt in red_contours: + x, y, h, w = cv2.boundingRect(cnt) + if min(h, w) >= arg.min_size and max(h, w) <= arg.max_size: + red_boxes.append(BoundingBox(top_left_x=x, top_left_y=y, height=h, width=w)) + + return blue_boxes, red_boxes, blue_map, red_map + + def image_callback(self, msg: Image): + # get image from message + img = self._cv_bridge.imgmsg_to_cv2(img_msg=msg, desired_encoding="bgr8") + header = msg.header + + # get annotations + blue_boxes, red_boxes, blue_map, red_map = self.process_image(img) + + # make output message + bb_message = BoundingBoxArray() + bb_message.header = header + bb_message.blue_boxes = blue_boxes + bb_message.red_boxes = red_boxes + + # publish output message + self._annotations_pub.publish(bb_message) + + # draw on debug image + debug_img = np.copy(img) + + for bb in blue_boxes: + debug_img = cv2.rectangle( + debug_img, + (bb.top_left_x, bb.top_left_y), + (bb.top_left_x + bb.width, bb.top_left_y + bb.height), + (255, 0, 0), + 2, + ) + + for bb in red_boxes: + debug_img = cv2.rectangle( + debug_img, + (bb.top_left_x, bb.top_left_y), + (bb.top_left_x + bb.width, bb.top_left_y + bb.height), + (0, 0, 255), + 2, + ) + + # make debug image message + # debug_img = cv2.cvtColor(debug_img, cv2.COLOR_BGR2RGB) + 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() From 43e243c114775305178d2ad0c8f949e9d7b5fa12 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?P=C3=A4r?= Date: Mon, 20 May 2024 21:00:15 +0200 Subject: [PATCH 09/31] add dynamic reconfigure param file to gitignore --- .gitignore | 1 + 1 file changed, 1 insertion(+) diff --git a/.gitignore b/.gitignore index be8ecd54c..07777c991 100644 --- a/.gitignore +++ b/.gitignore @@ -225,3 +225,4 @@ doku/* # Protobuf generated file /bitbots_team_communication/bitbots_team_communication/robocup_extension_pb2.py +bitbots_misc/technical_challenge_vision/technical_challenge_vision/technical_challenge_vision_params.py From 9d5674000ada3781acc1ad7261aeb8c969fcc45d Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?P=C3=A4r?= Date: Mon, 20 May 2024 21:00:36 +0200 Subject: [PATCH 10/31] add config --- .../config/range.yaml | 114 ++++++++++++++++++ 1 file changed, 114 insertions(+) create mode 100644 bitbots_misc/technical_challenge_vision/config/range.yaml diff --git a/bitbots_misc/technical_challenge_vision/config/range.yaml b/bitbots_misc/technical_challenge_vision/config/range.yaml new file mode 100644 index 000000000..d8574819b --- /dev/null +++ b/bitbots_misc/technical_challenge_vision/config/range.yaml @@ -0,0 +1,114 @@ +technical_challenge_vision: + ros__parameters: + blue_lower_h: { + type: int, + default_value: 0, + description: "hue value of the lower boundary for blue obstacles", + validation: { + bounds<>: [0, 179] + } + } + blue_upper_h: { + type: int, + default_value: 179, + description: "hue value of the upper boundary for blue obstacles", + validation: { + bounds<>: [0, 179] + } + } + blue_lower_s: { + type: int, + default_value: 0, + description: "separation value of the lower boundary for blue obstacles", + validation: { + bounds<>: [0, 255] + } + } + blue_upper_s: { + type: int, + default_value: 255, + 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: 0, + 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: 0, + 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: 0, + description: "minimum size of an obstacle to be considered", + validation: { + gt_eq<>: 0 + } + } + max_size: { + type: int, + default_value: 1, + description: "maximum size of an obstacle to be considered", + validation: { + gt_eq<>: 0 + } + } From 6f8f72f021572d1a6e53098cf1b132e26a0a4ab2 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?P=C3=A4r?= Date: Mon, 20 May 2024 21:00:51 +0200 Subject: [PATCH 11/31] add launch file --- .../launch/vision.launch | 30 +++++++++++++++++++ 1 file changed, 30 insertions(+) create mode 100644 bitbots_misc/technical_challenge_vision/launch/vision.launch diff --git a/bitbots_misc/technical_challenge_vision/launch/vision.launch b/bitbots_misc/technical_challenge_vision/launch/vision.launch new file mode 100644 index 000000000..7309d9e6d --- /dev/null +++ b/bitbots_misc/technical_challenge_vision/launch/vision.launch @@ -0,0 +1,30 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + From be0bf06e1a2a7b50e11e378ad16e6dc9e87312c8 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?P=C3=A4r?= Date: Mon, 20 May 2024 23:20:53 +0200 Subject: [PATCH 12/31] use Robot message instead of custom message type --- .../technical_challenge_vision.py | 96 ++++++++++++------- 1 file changed, 60 insertions(+), 36 deletions(-) diff --git a/bitbots_misc/technical_challenge_vision/technical_challenge_vision/technical_challenge_vision.py b/bitbots_misc/technical_challenge_vision/technical_challenge_vision/technical_challenge_vision.py index 9a94e1ca3..0b276de7d 100755 --- a/bitbots_misc/technical_challenge_vision/technical_challenge_vision/technical_challenge_vision.py +++ b/bitbots_misc/technical_challenge_vision/technical_challenge_vision/technical_challenge_vision.py @@ -8,8 +8,8 @@ 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_msgs.msg import BoundingBox, BoundingBoxArray from technical_challenge_vision.technical_challenge_vision_params import technical_challenge_vision @@ -19,7 +19,7 @@ def __init__(self): self._package_path = get_package_share_directory("technical_challenge_vision") self._cv_bridge = CvBridge() - self._annotations_pub = self.create_publisher(BoundingBoxArray, "/technical_challenge_vision", 10) + self._annotations_pub = self.create_publisher(RobotArray, "/technical_challenge_vision", 10) self._debug_img_pub = self.create_publisher(Image, "/technical_challenge_vision_debug_img", 10) self._debug_clrmp_pub_blue = self.create_publisher(Image, "/technical_challenge_vision_debug_clrmp_blue", 10) self._debug_clrmp_pub_red = self.create_publisher(Image, "/technical_challenge_vision_debug_clrmp_red", 10) @@ -27,7 +27,30 @@ def __init__(self): self._param_listener = technical_challenge_vision.ParamListener(self) self._params = self._param_listener.get_params() - def process_image(self, img: np.ndarray) -> Tuple[list[BoundingBox], list[BoundingBox], np.ndarray, np.ndarray]: + def create_robot_msg(self, x: int, y: int, h: int, w: 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 h: bb height + :param w: bb width + :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 + ) -> Tuple[list[Robot], list[Robot], np.ndarray, np.ndarray]: """ gets annotations from the camera image @@ -57,58 +80,59 @@ def process_image(self, img: np.ndarray) -> Tuple[list[BoundingBox], list[Boundi red_contours, _ = cv2.findContours(red_map, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_NONE) # get lists of bounding boxes - blue_boxes = [] - red_boxes = [] + blue_robots = [] + red_robots = [] for cnt in blue_contours: x, y, h, w = cv2.boundingRect(cnt) if min(h, w) >= arg.min_size and max(h, w) <= arg.max_size: - blue_boxes.append(BoundingBox(top_left_x=x, top_left_y=y, height=h, width=w)) + # draw bb on debug img + debug_img = cv2.rectangle( + debug_img, + (x, y), + (x + w, y + h), + (255, 0, 0), + 2, + ) + + # TODO I think 1 is for the blue team? + blue_robots.append(self.create_robot_msg(x, y, h, w, 1)) for cnt in red_contours: x, y, h, w = cv2.boundingRect(cnt) if min(h, w) >= arg.min_size and max(h, w) <= arg.max_size: - red_boxes.append(BoundingBox(top_left_x=x, top_left_y=y, height=h, width=w)) + # draw bb on debug img + debug_img = cv2.rectangle( + debug_img, + (x, y), + (x + w, y + h), + (0, 0, 255), + 2, + ) - return blue_boxes, red_boxes, blue_map, red_map + red_robots.append(self.create_robot_msg(x, y, h, w, 2)) + + return blue_robots, red_robots, blue_map, red_map, debug_img def image_callback(self, msg: Image): - # get image from message + # set variables img = self._cv_bridge.imgmsg_to_cv2(img_msg=msg, desired_encoding="bgr8") header = msg.header + debug_img = np.copy(img) # get annotations - blue_boxes, red_boxes, blue_map, red_map = self.process_image(img) + blue_robots, red_robots, blue_map, red_map, debug_img = self.process_image(img, debug_img) + robots = [] + robots.extend(blue_robots) + robots.extend(red_robots) # make output message - bb_message = BoundingBoxArray() - bb_message.header = header - bb_message.blue_boxes = blue_boxes - bb_message.red_boxes = red_boxes + robot_array_message = RobotArray() + robot_array_message.header = header + robot_array_message.robots = robots # publish output message - self._annotations_pub.publish(bb_message) - - # draw on debug image - debug_img = np.copy(img) - - for bb in blue_boxes: - debug_img = cv2.rectangle( - debug_img, - (bb.top_left_x, bb.top_left_y), - (bb.top_left_x + bb.width, bb.top_left_y + bb.height), - (255, 0, 0), - 2, - ) - - for bb in red_boxes: - debug_img = cv2.rectangle( - debug_img, - (bb.top_left_x, bb.top_left_y), - (bb.top_left_x + bb.width, bb.top_left_y + bb.height), - (0, 0, 255), - 2, - ) + self._annotations_pub.publish(robot_array_message) # make debug image message # debug_img = cv2.cvtColor(debug_img, cv2.COLOR_BGR2RGB) From 70176bd71a24cb545c5087a4a7f4dee85622c6f1 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?P=C3=A4r?= Date: Mon, 20 May 2024 23:31:43 +0200 Subject: [PATCH 13/31] remove custom messages that are no longer needed --- bitbots_msgs/msg/BoundingBox.msg | 4 ---- bitbots_msgs/msg/BoundingBoxArray.msg | 4 ---- 2 files changed, 8 deletions(-) delete mode 100644 bitbots_msgs/msg/BoundingBox.msg delete mode 100644 bitbots_msgs/msg/BoundingBoxArray.msg diff --git a/bitbots_msgs/msg/BoundingBox.msg b/bitbots_msgs/msg/BoundingBox.msg deleted file mode 100644 index a06a0c6c5..000000000 --- a/bitbots_msgs/msg/BoundingBox.msg +++ /dev/null @@ -1,4 +0,0 @@ -uint8 top_left_x -uint8 top_left_y -uint8 height -uint8 width diff --git a/bitbots_msgs/msg/BoundingBoxArray.msg b/bitbots_msgs/msg/BoundingBoxArray.msg deleted file mode 100644 index 3fcd8bc00..000000000 --- a/bitbots_msgs/msg/BoundingBoxArray.msg +++ /dev/null @@ -1,4 +0,0 @@ -std_msgs/Header header - -bitbots_msgs/BoundingBox[] blue_boxes -bitbots_msgs/BoundingBox[] red_boxes From c1f3efe7992f30d18bb021927003320f2c1ec251 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?P=C3=A4r?= Date: Mon, 20 May 2024 23:42:01 +0200 Subject: [PATCH 14/31] remove rest of unused message types --- bitbots_msgs/CMakeLists.txt | 4 ---- bitbots_msgs/msg/CircleInImage.msg | 3 --- bitbots_msgs/msg/CircleInImageArray.msg | 2 -- 3 files changed, 9 deletions(-) delete mode 100644 bitbots_msgs/msg/CircleInImage.msg delete mode 100644 bitbots_msgs/msg/CircleInImageArray.msg diff --git a/bitbots_msgs/CMakeLists.txt b/bitbots_msgs/CMakeLists.txt index 11a6d54fe..2488d45d1 100644 --- a/bitbots_msgs/CMakeLists.txt +++ b/bitbots_msgs/CMakeLists.txt @@ -23,11 +23,7 @@ rosidl_generate_interfaces( "action/PlayAnimation.action" "msg/Animation.msg" "msg/Audio.msg" - "msg/BoundingBox.msg" - "msg/BoundingBoxArray.msg" "msg/Buttons.msg" - "msg/CircleInImage.msg" - "msg/CircleInImageArray.msg" "msg/Cpu.msg" "msg/Filesystem.msg" "msg/FootPressure.msg" diff --git a/bitbots_msgs/msg/CircleInImage.msg b/bitbots_msgs/msg/CircleInImage.msg deleted file mode 100644 index 670c8efdb..000000000 --- a/bitbots_msgs/msg/CircleInImage.msg +++ /dev/null @@ -1,3 +0,0 @@ -uint8 center_x -uint8 center_y -uint8 diameter diff --git a/bitbots_msgs/msg/CircleInImageArray.msg b/bitbots_msgs/msg/CircleInImageArray.msg deleted file mode 100644 index 5f3e9790b..000000000 --- a/bitbots_msgs/msg/CircleInImageArray.msg +++ /dev/null @@ -1,2 +0,0 @@ -std_msgs/Header header -bitbots_msgs/CircleInImage[] circles From 966dfac8083f0331b0684fe441aa7f48b0d6676f Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?P=C3=A4r?= Date: Mon, 20 May 2024 23:42:34 +0200 Subject: [PATCH 15/31] rename package to bitbots_ --- .../__init__.py | 0 .../bitbots_technical_challenge_vision.py} | 22 +- ...tbots_technical_challenge_vision_params.py | 477 ++++++++++++++++++ .../config/range.yaml | 2 +- .../launch/vision.launch | 8 +- .../package.xml | 2 +- .../bitbots_technical_challenge_vision} | 0 .../setup.cfg | 4 + .../setup.py | 6 +- .../technical_challenge_vision/setup.cfg | 4 - 10 files changed, 504 insertions(+), 21 deletions(-) rename bitbots_misc/{technical_challenge_vision/technical_challenge_vision => bitbots_technical_challenge_vision/bitbots_technical_challenge_vision}/__init__.py (100%) rename bitbots_misc/{technical_challenge_vision/technical_challenge_vision/technical_challenge_vision.py => bitbots_technical_challenge_vision/bitbots_technical_challenge_vision/bitbots_technical_challenge_vision.py} (86%) create mode 100644 bitbots_misc/bitbots_technical_challenge_vision/bitbots_technical_challenge_vision/bitbots_technical_challenge_vision_params.py rename bitbots_misc/{technical_challenge_vision => bitbots_technical_challenge_vision}/config/range.yaml (98%) rename bitbots_misc/{technical_challenge_vision => bitbots_technical_challenge_vision}/launch/vision.launch (61%) rename bitbots_misc/{technical_challenge_vision => bitbots_technical_challenge_vision}/package.xml (93%) rename bitbots_misc/{technical_challenge_vision/resource/technical_challenge_vision => bitbots_technical_challenge_vision/resource/bitbots_technical_challenge_vision} (100%) create mode 100644 bitbots_misc/bitbots_technical_challenge_vision/setup.cfg rename bitbots_misc/{technical_challenge_vision => bitbots_technical_challenge_vision}/setup.py (80%) delete mode 100644 bitbots_misc/technical_challenge_vision/setup.cfg diff --git a/bitbots_misc/technical_challenge_vision/technical_challenge_vision/__init__.py b/bitbots_misc/bitbots_technical_challenge_vision/bitbots_technical_challenge_vision/__init__.py similarity index 100% rename from bitbots_misc/technical_challenge_vision/technical_challenge_vision/__init__.py rename to bitbots_misc/bitbots_technical_challenge_vision/bitbots_technical_challenge_vision/__init__.py diff --git a/bitbots_misc/technical_challenge_vision/technical_challenge_vision/technical_challenge_vision.py b/bitbots_misc/bitbots_technical_challenge_vision/bitbots_technical_challenge_vision/bitbots_technical_challenge_vision.py similarity index 86% rename from bitbots_misc/technical_challenge_vision/technical_challenge_vision/technical_challenge_vision.py rename to bitbots_misc/bitbots_technical_challenge_vision/bitbots_technical_challenge_vision/bitbots_technical_challenge_vision.py index 0b276de7d..c9b6e3b53 100755 --- a/bitbots_misc/technical_challenge_vision/technical_challenge_vision/technical_challenge_vision.py +++ b/bitbots_misc/bitbots_technical_challenge_vision/bitbots_technical_challenge_vision/bitbots_technical_challenge_vision.py @@ -10,21 +10,27 @@ from sensor_msgs.msg import Image from soccer_vision_2d_msgs.msg import Robot, RobotArray -from technical_challenge_vision.technical_challenge_vision_params import technical_challenge_vision +from bitbots_technical_challenge_vision.bitbots_technical_challenge_vision_params import ( + bitbots_technical_challenge_vision, +) class TechnicalChallengeVision(Node): def __init__(self): - super().__init__("technical_challenge_vision") + super().__init__("bitbots_technical_challenge_vision") - self._package_path = get_package_share_directory("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, "/technical_challenge_vision", 10) - self._debug_img_pub = self.create_publisher(Image, "/technical_challenge_vision_debug_img", 10) - self._debug_clrmp_pub_blue = self.create_publisher(Image, "/technical_challenge_vision_debug_clrmp_blue", 10) - self._debug_clrmp_pub_red = self.create_publisher(Image, "/technical_challenge_vision_debug_clrmp_red", 10) + self._annotations_pub = self.create_publisher(RobotArray, "/bitbots_technical_challenge_vision", 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 = technical_challenge_vision.ParamListener(self) + 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, h: int, w: int, t: int) -> Robot: 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..dce257aae --- /dev/null +++ b/bitbots_misc/bitbots_technical_challenge_vision/bitbots_technical_challenge_vision/bitbots_technical_challenge_vision_params.py @@ -0,0 +1,477 @@ +# 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() + + class __RosParameters: + 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 + + ros__parameters = __RosParameters() + + 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_ + "ros__parameters.blue_lower_h": + validation_result = ParameterValidators.bounds(param, 0, 179) + if validation_result: + return SetParametersResult(successful=False, reason=validation_result) + updated_params.ros__parameters.blue_lower_h = param.value + self.logger_.debug(param.name + ": " + param.type_.name + " = " + str(param.value)) + + if param.name == self.prefix_ + "ros__parameters.blue_upper_h": + validation_result = ParameterValidators.bounds(param, 0, 179) + if validation_result: + return SetParametersResult(successful=False, reason=validation_result) + updated_params.ros__parameters.blue_upper_h = param.value + self.logger_.debug(param.name + ": " + param.type_.name + " = " + str(param.value)) + + if param.name == self.prefix_ + "ros__parameters.blue_lower_s": + validation_result = ParameterValidators.bounds(param, 0, 255) + if validation_result: + return SetParametersResult(successful=False, reason=validation_result) + updated_params.ros__parameters.blue_lower_s = param.value + self.logger_.debug(param.name + ": " + param.type_.name + " = " + str(param.value)) + + if param.name == self.prefix_ + "ros__parameters.blue_upper_s": + validation_result = ParameterValidators.bounds(param, 0, 255) + if validation_result: + return SetParametersResult(successful=False, reason=validation_result) + updated_params.ros__parameters.blue_upper_s = param.value + self.logger_.debug(param.name + ": " + param.type_.name + " = " + str(param.value)) + + if param.name == self.prefix_ + "ros__parameters.blue_lower_v": + validation_result = ParameterValidators.bounds(param, 0, 255) + if validation_result: + return SetParametersResult(successful=False, reason=validation_result) + updated_params.ros__parameters.blue_lower_v = param.value + self.logger_.debug(param.name + ": " + param.type_.name + " = " + str(param.value)) + + if param.name == self.prefix_ + "ros__parameters.blue_upper_v": + validation_result = ParameterValidators.bounds(param, 0, 255) + if validation_result: + return SetParametersResult(successful=False, reason=validation_result) + updated_params.ros__parameters.blue_upper_v = param.value + self.logger_.debug(param.name + ": " + param.type_.name + " = " + str(param.value)) + + if param.name == self.prefix_ + "ros__parameters.red_lower_h": + validation_result = ParameterValidators.bounds(param, 0, 179) + if validation_result: + return SetParametersResult(successful=False, reason=validation_result) + updated_params.ros__parameters.red_lower_h = param.value + self.logger_.debug(param.name + ": " + param.type_.name + " = " + str(param.value)) + + if param.name == self.prefix_ + "ros__parameters.red_upper_h": + validation_result = ParameterValidators.bounds(param, 0, 179) + if validation_result: + return SetParametersResult(successful=False, reason=validation_result) + updated_params.ros__parameters.red_upper_h = param.value + self.logger_.debug(param.name + ": " + param.type_.name + " = " + str(param.value)) + + if param.name == self.prefix_ + "ros__parameters.red_lower_s": + validation_result = ParameterValidators.bounds(param, 0, 255) + if validation_result: + return SetParametersResult(successful=False, reason=validation_result) + updated_params.ros__parameters.red_lower_s = param.value + self.logger_.debug(param.name + ": " + param.type_.name + " = " + str(param.value)) + + if param.name == self.prefix_ + "ros__parameters.red_upper_s": + validation_result = ParameterValidators.bounds(param, 0, 255) + if validation_result: + return SetParametersResult(successful=False, reason=validation_result) + updated_params.ros__parameters.red_upper_s = param.value + self.logger_.debug(param.name + ": " + param.type_.name + " = " + str(param.value)) + + if param.name == self.prefix_ + "ros__parameters.red_lower_v": + validation_result = ParameterValidators.bounds(param, 0, 255) + if validation_result: + return SetParametersResult(successful=False, reason=validation_result) + updated_params.ros__parameters.red_lower_v = param.value + self.logger_.debug(param.name + ": " + param.type_.name + " = " + str(param.value)) + + if param.name == self.prefix_ + "ros__parameters.red_upper_v": + validation_result = ParameterValidators.bounds(param, 0, 255) + if validation_result: + return SetParametersResult(successful=False, reason=validation_result) + updated_params.ros__parameters.red_upper_v = param.value + self.logger_.debug(param.name + ": " + param.type_.name + " = " + str(param.value)) + + if param.name == self.prefix_ + "ros__parameters.min_size": + validation_result = ParameterValidators.gt_eq(param, 0) + if validation_result: + return SetParametersResult(successful=False, reason=validation_result) + updated_params.ros__parameters.min_size = param.value + self.logger_.debug(param.name + ": " + param.type_.name + " = " + str(param.value)) + + if param.name == self.prefix_ + "ros__parameters.max_size": + validation_result = ParameterValidators.gt_eq(param, 0) + if validation_result: + return SetParametersResult(successful=False, reason=validation_result) + updated_params.ros__parameters.max_size = 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_ + "ros__parameters.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.ros__parameters.blue_lower_h + self.node_.declare_parameter(self.prefix_ + "ros__parameters.blue_lower_h", parameter, descriptor) + + if not self.node_.has_parameter(self.prefix_ + "ros__parameters.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.ros__parameters.blue_upper_h + self.node_.declare_parameter(self.prefix_ + "ros__parameters.blue_upper_h", parameter, descriptor) + + if not self.node_.has_parameter(self.prefix_ + "ros__parameters.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.ros__parameters.blue_lower_s + self.node_.declare_parameter(self.prefix_ + "ros__parameters.blue_lower_s", parameter, descriptor) + + if not self.node_.has_parameter(self.prefix_ + "ros__parameters.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.ros__parameters.blue_upper_s + self.node_.declare_parameter(self.prefix_ + "ros__parameters.blue_upper_s", parameter, descriptor) + + if not self.node_.has_parameter(self.prefix_ + "ros__parameters.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.ros__parameters.blue_lower_v + self.node_.declare_parameter(self.prefix_ + "ros__parameters.blue_lower_v", parameter, descriptor) + + if not self.node_.has_parameter(self.prefix_ + "ros__parameters.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.ros__parameters.blue_upper_v + self.node_.declare_parameter(self.prefix_ + "ros__parameters.blue_upper_v", parameter, descriptor) + + if not self.node_.has_parameter(self.prefix_ + "ros__parameters.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.ros__parameters.red_lower_h + self.node_.declare_parameter(self.prefix_ + "ros__parameters.red_lower_h", parameter, descriptor) + + if not self.node_.has_parameter(self.prefix_ + "ros__parameters.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.ros__parameters.red_upper_h + self.node_.declare_parameter(self.prefix_ + "ros__parameters.red_upper_h", parameter, descriptor) + + if not self.node_.has_parameter(self.prefix_ + "ros__parameters.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.ros__parameters.red_lower_s + self.node_.declare_parameter(self.prefix_ + "ros__parameters.red_lower_s", parameter, descriptor) + + if not self.node_.has_parameter(self.prefix_ + "ros__parameters.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.ros__parameters.red_upper_s + self.node_.declare_parameter(self.prefix_ + "ros__parameters.red_upper_s", parameter, descriptor) + + if not self.node_.has_parameter(self.prefix_ + "ros__parameters.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.ros__parameters.red_lower_v + self.node_.declare_parameter(self.prefix_ + "ros__parameters.red_lower_v", parameter, descriptor) + + if not self.node_.has_parameter(self.prefix_ + "ros__parameters.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.ros__parameters.red_upper_v + self.node_.declare_parameter(self.prefix_ + "ros__parameters.red_upper_v", parameter, descriptor) + + if not self.node_.has_parameter(self.prefix_ + "ros__parameters.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.ros__parameters.min_size + self.node_.declare_parameter(self.prefix_ + "ros__parameters.min_size", parameter, descriptor) + + if not self.node_.has_parameter(self.prefix_ + "ros__parameters.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.ros__parameters.max_size + self.node_.declare_parameter(self.prefix_ + "ros__parameters.max_size", parameter, descriptor) + + # TODO: need validation + # get parameters and fill struct fields + param = self.node_.get_parameter(self.prefix_ + "ros__parameters.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( + "ros__parameters.blue_lower_h", + param.value, + "Invalid value set during initialization for parameter ros__parameters.blue_lower_h: " + + validation_result, + ) + updated_params.ros__parameters.blue_lower_h = param.value + param = self.node_.get_parameter(self.prefix_ + "ros__parameters.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( + "ros__parameters.blue_upper_h", + param.value, + "Invalid value set during initialization for parameter ros__parameters.blue_upper_h: " + + validation_result, + ) + updated_params.ros__parameters.blue_upper_h = param.value + param = self.node_.get_parameter(self.prefix_ + "ros__parameters.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( + "ros__parameters.blue_lower_s", + param.value, + "Invalid value set during initialization for parameter ros__parameters.blue_lower_s: " + + validation_result, + ) + updated_params.ros__parameters.blue_lower_s = param.value + param = self.node_.get_parameter(self.prefix_ + "ros__parameters.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( + "ros__parameters.blue_upper_s", + param.value, + "Invalid value set during initialization for parameter ros__parameters.blue_upper_s: " + + validation_result, + ) + updated_params.ros__parameters.blue_upper_s = param.value + param = self.node_.get_parameter(self.prefix_ + "ros__parameters.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( + "ros__parameters.blue_lower_v", + param.value, + "Invalid value set during initialization for parameter ros__parameters.blue_lower_v: " + + validation_result, + ) + updated_params.ros__parameters.blue_lower_v = param.value + param = self.node_.get_parameter(self.prefix_ + "ros__parameters.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( + "ros__parameters.blue_upper_v", + param.value, + "Invalid value set during initialization for parameter ros__parameters.blue_upper_v: " + + validation_result, + ) + updated_params.ros__parameters.blue_upper_v = param.value + param = self.node_.get_parameter(self.prefix_ + "ros__parameters.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( + "ros__parameters.red_lower_h", + param.value, + "Invalid value set during initialization for parameter ros__parameters.red_lower_h: " + + validation_result, + ) + updated_params.ros__parameters.red_lower_h = param.value + param = self.node_.get_parameter(self.prefix_ + "ros__parameters.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( + "ros__parameters.red_upper_h", + param.value, + "Invalid value set during initialization for parameter ros__parameters.red_upper_h: " + + validation_result, + ) + updated_params.ros__parameters.red_upper_h = param.value + param = self.node_.get_parameter(self.prefix_ + "ros__parameters.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( + "ros__parameters.red_lower_s", + param.value, + "Invalid value set during initialization for parameter ros__parameters.red_lower_s: " + + validation_result, + ) + updated_params.ros__parameters.red_lower_s = param.value + param = self.node_.get_parameter(self.prefix_ + "ros__parameters.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( + "ros__parameters.red_upper_s", + param.value, + "Invalid value set during initialization for parameter ros__parameters.red_upper_s: " + + validation_result, + ) + updated_params.ros__parameters.red_upper_s = param.value + param = self.node_.get_parameter(self.prefix_ + "ros__parameters.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( + "ros__parameters.red_lower_v", + param.value, + "Invalid value set during initialization for parameter ros__parameters.red_lower_v: " + + validation_result, + ) + updated_params.ros__parameters.red_lower_v = param.value + param = self.node_.get_parameter(self.prefix_ + "ros__parameters.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( + "ros__parameters.red_upper_v", + param.value, + "Invalid value set during initialization for parameter ros__parameters.red_upper_v: " + + validation_result, + ) + updated_params.ros__parameters.red_upper_v = param.value + param = self.node_.get_parameter(self.prefix_ + "ros__parameters.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( + "ros__parameters.min_size", + param.value, + "Invalid value set during initialization for parameter ros__parameters.min_size: " + + validation_result, + ) + updated_params.ros__parameters.min_size = param.value + param = self.node_.get_parameter(self.prefix_ + "ros__parameters.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( + "ros__parameters.max_size", + param.value, + "Invalid value set during initialization for parameter ros__parameters.max_size: " + + validation_result, + ) + updated_params.ros__parameters.max_size = param.value + + self.update_internal_params(updated_params) diff --git a/bitbots_misc/technical_challenge_vision/config/range.yaml b/bitbots_misc/bitbots_technical_challenge_vision/config/range.yaml similarity index 98% rename from bitbots_misc/technical_challenge_vision/config/range.yaml rename to bitbots_misc/bitbots_technical_challenge_vision/config/range.yaml index d8574819b..55a433393 100644 --- a/bitbots_misc/technical_challenge_vision/config/range.yaml +++ b/bitbots_misc/bitbots_technical_challenge_vision/config/range.yaml @@ -1,4 +1,4 @@ -technical_challenge_vision: +bitbots_technical_challenge_vision: ros__parameters: blue_lower_h: { type: int, diff --git a/bitbots_misc/technical_challenge_vision/launch/vision.launch b/bitbots_misc/bitbots_technical_challenge_vision/launch/vision.launch similarity index 61% rename from bitbots_misc/technical_challenge_vision/launch/vision.launch rename to bitbots_misc/bitbots_technical_challenge_vision/launch/vision.launch index 7309d9e6d..390a6c8be 100644 --- a/bitbots_misc/technical_challenge_vision/launch/vision.launch +++ b/bitbots_misc/bitbots_technical_challenge_vision/launch/vision.launch @@ -8,8 +8,8 @@ - - + + @@ -17,8 +17,8 @@ - - + + diff --git a/bitbots_misc/technical_challenge_vision/package.xml b/bitbots_misc/bitbots_technical_challenge_vision/package.xml similarity index 93% rename from bitbots_misc/technical_challenge_vision/package.xml rename to bitbots_misc/bitbots_technical_challenge_vision/package.xml index c6aac8c0d..d1abd0852 100644 --- a/bitbots_misc/technical_challenge_vision/package.xml +++ b/bitbots_misc/bitbots_technical_challenge_vision/package.xml @@ -1,7 +1,7 @@ - technical_challenge_vision + bitbots_technical_challenge_vision 0.0.0 TODO: Package description par diff --git a/bitbots_misc/technical_challenge_vision/resource/technical_challenge_vision b/bitbots_misc/bitbots_technical_challenge_vision/resource/bitbots_technical_challenge_vision similarity index 100% rename from bitbots_misc/technical_challenge_vision/resource/technical_challenge_vision rename to bitbots_misc/bitbots_technical_challenge_vision/resource/bitbots_technical_challenge_vision 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/technical_challenge_vision/setup.py b/bitbots_misc/bitbots_technical_challenge_vision/setup.py similarity index 80% rename from bitbots_misc/technical_challenge_vision/setup.py rename to bitbots_misc/bitbots_technical_challenge_vision/setup.py index 8b1a0d312..4603d372c 100644 --- a/bitbots_misc/technical_challenge_vision/setup.py +++ b/bitbots_misc/bitbots_technical_challenge_vision/setup.py @@ -3,7 +3,7 @@ from generate_parameter_library_py.setup_helper import generate_parameter_module from setuptools import find_packages, setup -package_name = "technical_challenge_vision" +package_name = "bitbots_technical_challenge_vision" setup( name=package_name, @@ -23,12 +23,12 @@ license="MIT", entry_points={ "console_scripts": [ - "technical_challenge_vision = technical_challenge_vision.technical_challenge_vision:main", + "bitbots_technical_challenge_vision = bitbots_technical_challenge_vision.bitbots_technical_challenge_vision:main", ], }, ) generate_parameter_module( - "technical_challenge_vision_params", + "bitbots_technical_challenge_vision_params", "config/range.yaml", ) diff --git a/bitbots_misc/technical_challenge_vision/setup.cfg b/bitbots_misc/technical_challenge_vision/setup.cfg deleted file mode 100644 index 7e2825e6e..000000000 --- a/bitbots_misc/technical_challenge_vision/setup.cfg +++ /dev/null @@ -1,4 +0,0 @@ -[develop] -script_dir=$base/lib/technical_challenge_vision -[install] -install_scripts=$base/lib/technical_challenge_vision From 61edfe8f78c493bf4cbe0a9fa3d069e91433b7f6 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?P=C3=A4r?= Date: Mon, 20 May 2024 23:56:28 +0200 Subject: [PATCH 16/31] remove build dependent files from git --- .gitignore | 3 +- ...tbots_technical_challenge_vision_params.py | 477 ------------------ .../bitbots_technical_challenge_vision | 0 3 files changed, 2 insertions(+), 478 deletions(-) delete mode 100644 bitbots_misc/bitbots_technical_challenge_vision/bitbots_technical_challenge_vision/bitbots_technical_challenge_vision_params.py delete mode 100644 bitbots_misc/bitbots_technical_challenge_vision/resource/bitbots_technical_challenge_vision diff --git a/.gitignore b/.gitignore index 07777c991..4034634ce 100644 --- a/.gitignore +++ b/.gitignore @@ -225,4 +225,5 @@ doku/* # Protobuf generated file /bitbots_team_communication/bitbots_team_communication/robocup_extension_pb2.py -bitbots_misc/technical_challenge_vision/technical_challenge_vision/technical_challenge_vision_params.py +bitbots_misc/bitbots_technical_challenge_vision/bitbots_technical_challenge_vision/bitbots_technical_challenge_vision_params.py +bitbots_misc/bitbots_technical_challenge_vision/resource/bitbots_technical_challenge_vision 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 deleted file mode 100644 index dce257aae..000000000 --- a/bitbots_misc/bitbots_technical_challenge_vision/bitbots_technical_challenge_vision/bitbots_technical_challenge_vision_params.py +++ /dev/null @@ -1,477 +0,0 @@ -# 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() - - class __RosParameters: - 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 - - ros__parameters = __RosParameters() - - 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_ + "ros__parameters.blue_lower_h": - validation_result = ParameterValidators.bounds(param, 0, 179) - if validation_result: - return SetParametersResult(successful=False, reason=validation_result) - updated_params.ros__parameters.blue_lower_h = param.value - self.logger_.debug(param.name + ": " + param.type_.name + " = " + str(param.value)) - - if param.name == self.prefix_ + "ros__parameters.blue_upper_h": - validation_result = ParameterValidators.bounds(param, 0, 179) - if validation_result: - return SetParametersResult(successful=False, reason=validation_result) - updated_params.ros__parameters.blue_upper_h = param.value - self.logger_.debug(param.name + ": " + param.type_.name + " = " + str(param.value)) - - if param.name == self.prefix_ + "ros__parameters.blue_lower_s": - validation_result = ParameterValidators.bounds(param, 0, 255) - if validation_result: - return SetParametersResult(successful=False, reason=validation_result) - updated_params.ros__parameters.blue_lower_s = param.value - self.logger_.debug(param.name + ": " + param.type_.name + " = " + str(param.value)) - - if param.name == self.prefix_ + "ros__parameters.blue_upper_s": - validation_result = ParameterValidators.bounds(param, 0, 255) - if validation_result: - return SetParametersResult(successful=False, reason=validation_result) - updated_params.ros__parameters.blue_upper_s = param.value - self.logger_.debug(param.name + ": " + param.type_.name + " = " + str(param.value)) - - if param.name == self.prefix_ + "ros__parameters.blue_lower_v": - validation_result = ParameterValidators.bounds(param, 0, 255) - if validation_result: - return SetParametersResult(successful=False, reason=validation_result) - updated_params.ros__parameters.blue_lower_v = param.value - self.logger_.debug(param.name + ": " + param.type_.name + " = " + str(param.value)) - - if param.name == self.prefix_ + "ros__parameters.blue_upper_v": - validation_result = ParameterValidators.bounds(param, 0, 255) - if validation_result: - return SetParametersResult(successful=False, reason=validation_result) - updated_params.ros__parameters.blue_upper_v = param.value - self.logger_.debug(param.name + ": " + param.type_.name + " = " + str(param.value)) - - if param.name == self.prefix_ + "ros__parameters.red_lower_h": - validation_result = ParameterValidators.bounds(param, 0, 179) - if validation_result: - return SetParametersResult(successful=False, reason=validation_result) - updated_params.ros__parameters.red_lower_h = param.value - self.logger_.debug(param.name + ": " + param.type_.name + " = " + str(param.value)) - - if param.name == self.prefix_ + "ros__parameters.red_upper_h": - validation_result = ParameterValidators.bounds(param, 0, 179) - if validation_result: - return SetParametersResult(successful=False, reason=validation_result) - updated_params.ros__parameters.red_upper_h = param.value - self.logger_.debug(param.name + ": " + param.type_.name + " = " + str(param.value)) - - if param.name == self.prefix_ + "ros__parameters.red_lower_s": - validation_result = ParameterValidators.bounds(param, 0, 255) - if validation_result: - return SetParametersResult(successful=False, reason=validation_result) - updated_params.ros__parameters.red_lower_s = param.value - self.logger_.debug(param.name + ": " + param.type_.name + " = " + str(param.value)) - - if param.name == self.prefix_ + "ros__parameters.red_upper_s": - validation_result = ParameterValidators.bounds(param, 0, 255) - if validation_result: - return SetParametersResult(successful=False, reason=validation_result) - updated_params.ros__parameters.red_upper_s = param.value - self.logger_.debug(param.name + ": " + param.type_.name + " = " + str(param.value)) - - if param.name == self.prefix_ + "ros__parameters.red_lower_v": - validation_result = ParameterValidators.bounds(param, 0, 255) - if validation_result: - return SetParametersResult(successful=False, reason=validation_result) - updated_params.ros__parameters.red_lower_v = param.value - self.logger_.debug(param.name + ": " + param.type_.name + " = " + str(param.value)) - - if param.name == self.prefix_ + "ros__parameters.red_upper_v": - validation_result = ParameterValidators.bounds(param, 0, 255) - if validation_result: - return SetParametersResult(successful=False, reason=validation_result) - updated_params.ros__parameters.red_upper_v = param.value - self.logger_.debug(param.name + ": " + param.type_.name + " = " + str(param.value)) - - if param.name == self.prefix_ + "ros__parameters.min_size": - validation_result = ParameterValidators.gt_eq(param, 0) - if validation_result: - return SetParametersResult(successful=False, reason=validation_result) - updated_params.ros__parameters.min_size = param.value - self.logger_.debug(param.name + ": " + param.type_.name + " = " + str(param.value)) - - if param.name == self.prefix_ + "ros__parameters.max_size": - validation_result = ParameterValidators.gt_eq(param, 0) - if validation_result: - return SetParametersResult(successful=False, reason=validation_result) - updated_params.ros__parameters.max_size = 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_ + "ros__parameters.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.ros__parameters.blue_lower_h - self.node_.declare_parameter(self.prefix_ + "ros__parameters.blue_lower_h", parameter, descriptor) - - if not self.node_.has_parameter(self.prefix_ + "ros__parameters.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.ros__parameters.blue_upper_h - self.node_.declare_parameter(self.prefix_ + "ros__parameters.blue_upper_h", parameter, descriptor) - - if not self.node_.has_parameter(self.prefix_ + "ros__parameters.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.ros__parameters.blue_lower_s - self.node_.declare_parameter(self.prefix_ + "ros__parameters.blue_lower_s", parameter, descriptor) - - if not self.node_.has_parameter(self.prefix_ + "ros__parameters.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.ros__parameters.blue_upper_s - self.node_.declare_parameter(self.prefix_ + "ros__parameters.blue_upper_s", parameter, descriptor) - - if not self.node_.has_parameter(self.prefix_ + "ros__parameters.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.ros__parameters.blue_lower_v - self.node_.declare_parameter(self.prefix_ + "ros__parameters.blue_lower_v", parameter, descriptor) - - if not self.node_.has_parameter(self.prefix_ + "ros__parameters.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.ros__parameters.blue_upper_v - self.node_.declare_parameter(self.prefix_ + "ros__parameters.blue_upper_v", parameter, descriptor) - - if not self.node_.has_parameter(self.prefix_ + "ros__parameters.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.ros__parameters.red_lower_h - self.node_.declare_parameter(self.prefix_ + "ros__parameters.red_lower_h", parameter, descriptor) - - if not self.node_.has_parameter(self.prefix_ + "ros__parameters.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.ros__parameters.red_upper_h - self.node_.declare_parameter(self.prefix_ + "ros__parameters.red_upper_h", parameter, descriptor) - - if not self.node_.has_parameter(self.prefix_ + "ros__parameters.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.ros__parameters.red_lower_s - self.node_.declare_parameter(self.prefix_ + "ros__parameters.red_lower_s", parameter, descriptor) - - if not self.node_.has_parameter(self.prefix_ + "ros__parameters.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.ros__parameters.red_upper_s - self.node_.declare_parameter(self.prefix_ + "ros__parameters.red_upper_s", parameter, descriptor) - - if not self.node_.has_parameter(self.prefix_ + "ros__parameters.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.ros__parameters.red_lower_v - self.node_.declare_parameter(self.prefix_ + "ros__parameters.red_lower_v", parameter, descriptor) - - if not self.node_.has_parameter(self.prefix_ + "ros__parameters.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.ros__parameters.red_upper_v - self.node_.declare_parameter(self.prefix_ + "ros__parameters.red_upper_v", parameter, descriptor) - - if not self.node_.has_parameter(self.prefix_ + "ros__parameters.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.ros__parameters.min_size - self.node_.declare_parameter(self.prefix_ + "ros__parameters.min_size", parameter, descriptor) - - if not self.node_.has_parameter(self.prefix_ + "ros__parameters.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.ros__parameters.max_size - self.node_.declare_parameter(self.prefix_ + "ros__parameters.max_size", parameter, descriptor) - - # TODO: need validation - # get parameters and fill struct fields - param = self.node_.get_parameter(self.prefix_ + "ros__parameters.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( - "ros__parameters.blue_lower_h", - param.value, - "Invalid value set during initialization for parameter ros__parameters.blue_lower_h: " - + validation_result, - ) - updated_params.ros__parameters.blue_lower_h = param.value - param = self.node_.get_parameter(self.prefix_ + "ros__parameters.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( - "ros__parameters.blue_upper_h", - param.value, - "Invalid value set during initialization for parameter ros__parameters.blue_upper_h: " - + validation_result, - ) - updated_params.ros__parameters.blue_upper_h = param.value - param = self.node_.get_parameter(self.prefix_ + "ros__parameters.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( - "ros__parameters.blue_lower_s", - param.value, - "Invalid value set during initialization for parameter ros__parameters.blue_lower_s: " - + validation_result, - ) - updated_params.ros__parameters.blue_lower_s = param.value - param = self.node_.get_parameter(self.prefix_ + "ros__parameters.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( - "ros__parameters.blue_upper_s", - param.value, - "Invalid value set during initialization for parameter ros__parameters.blue_upper_s: " - + validation_result, - ) - updated_params.ros__parameters.blue_upper_s = param.value - param = self.node_.get_parameter(self.prefix_ + "ros__parameters.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( - "ros__parameters.blue_lower_v", - param.value, - "Invalid value set during initialization for parameter ros__parameters.blue_lower_v: " - + validation_result, - ) - updated_params.ros__parameters.blue_lower_v = param.value - param = self.node_.get_parameter(self.prefix_ + "ros__parameters.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( - "ros__parameters.blue_upper_v", - param.value, - "Invalid value set during initialization for parameter ros__parameters.blue_upper_v: " - + validation_result, - ) - updated_params.ros__parameters.blue_upper_v = param.value - param = self.node_.get_parameter(self.prefix_ + "ros__parameters.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( - "ros__parameters.red_lower_h", - param.value, - "Invalid value set during initialization for parameter ros__parameters.red_lower_h: " - + validation_result, - ) - updated_params.ros__parameters.red_lower_h = param.value - param = self.node_.get_parameter(self.prefix_ + "ros__parameters.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( - "ros__parameters.red_upper_h", - param.value, - "Invalid value set during initialization for parameter ros__parameters.red_upper_h: " - + validation_result, - ) - updated_params.ros__parameters.red_upper_h = param.value - param = self.node_.get_parameter(self.prefix_ + "ros__parameters.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( - "ros__parameters.red_lower_s", - param.value, - "Invalid value set during initialization for parameter ros__parameters.red_lower_s: " - + validation_result, - ) - updated_params.ros__parameters.red_lower_s = param.value - param = self.node_.get_parameter(self.prefix_ + "ros__parameters.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( - "ros__parameters.red_upper_s", - param.value, - "Invalid value set during initialization for parameter ros__parameters.red_upper_s: " - + validation_result, - ) - updated_params.ros__parameters.red_upper_s = param.value - param = self.node_.get_parameter(self.prefix_ + "ros__parameters.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( - "ros__parameters.red_lower_v", - param.value, - "Invalid value set during initialization for parameter ros__parameters.red_lower_v: " - + validation_result, - ) - updated_params.ros__parameters.red_lower_v = param.value - param = self.node_.get_parameter(self.prefix_ + "ros__parameters.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( - "ros__parameters.red_upper_v", - param.value, - "Invalid value set during initialization for parameter ros__parameters.red_upper_v: " - + validation_result, - ) - updated_params.ros__parameters.red_upper_v = param.value - param = self.node_.get_parameter(self.prefix_ + "ros__parameters.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( - "ros__parameters.min_size", - param.value, - "Invalid value set during initialization for parameter ros__parameters.min_size: " - + validation_result, - ) - updated_params.ros__parameters.min_size = param.value - param = self.node_.get_parameter(self.prefix_ + "ros__parameters.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( - "ros__parameters.max_size", - param.value, - "Invalid value set during initialization for parameter ros__parameters.max_size: " - + validation_result, - ) - updated_params.ros__parameters.max_size = param.value - - self.update_internal_params(updated_params) 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 deleted file mode 100644 index e69de29bb..000000000 From fab7775d4d762c9dc9b20690e2614236362be01e Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?P=C3=A4r?= Date: Tue, 21 May 2024 00:21:25 +0200 Subject: [PATCH 17/31] make debug mode dynamic --- .../bitbots_technical_challenge_vision/config/range.yaml | 5 +++++ .../launch/vision.launch | 7 ------- 2 files changed, 5 insertions(+), 7 deletions(-) diff --git a/bitbots_misc/bitbots_technical_challenge_vision/config/range.yaml b/bitbots_misc/bitbots_technical_challenge_vision/config/range.yaml index 55a433393..5e868bc22 100644 --- a/bitbots_misc/bitbots_technical_challenge_vision/config/range.yaml +++ b/bitbots_misc/bitbots_technical_challenge_vision/config/range.yaml @@ -112,3 +112,8 @@ bitbots_technical_challenge_vision: gt_eq<>: 0 } } + debug_mode: { + type: bool, + default_value: true, + 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 index 390a6c8be..3960f8248 100644 --- a/bitbots_misc/bitbots_technical_challenge_vision/launch/vision.launch +++ b/bitbots_misc/bitbots_technical_challenge_vision/launch/vision.launch @@ -2,7 +2,6 @@ - @@ -10,18 +9,12 @@ - - - - - - From 20d536e5723c26eedb6fac3786d72eaf3f942ef3 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?P=C3=A4r?= Date: Tue, 21 May 2024 00:28:07 +0200 Subject: [PATCH 18/31] update dependencies --- bitbots_misc/bitbots_technical_challenge_vision/package.xml | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/bitbots_misc/bitbots_technical_challenge_vision/package.xml b/bitbots_misc/bitbots_technical_challenge_vision/package.xml index d1abd0852..37543d280 100644 --- a/bitbots_misc/bitbots_technical_challenge_vision/package.xml +++ b/bitbots_misc/bitbots_technical_challenge_vision/package.xml @@ -8,7 +8,11 @@ TODO: License declaration rclpy - bitbots_msgs + cv2 + numpy + cv_bridge + sensor_msgs + soccer_vision_2d_msgs generate_parameter_library From dc8e9672966515d088d015a07ec8ae96de0c149f Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?P=C3=A4r?= Date: Tue, 21 May 2024 00:46:42 +0200 Subject: [PATCH 19/31] make debug mode actually do something --- .../bitbots_technical_challenge_vision.py | 74 +++++++++++-------- 1 file changed, 42 insertions(+), 32 deletions(-) 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 index c9b6e3b53..ac030c050 100755 --- 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 @@ -55,19 +55,19 @@ def create_robot_msg(self, x: int, y: int, h: int, w: int, t: int) -> Robot: return robot def process_image( - self, img: np.ndarray, debug_img: np.ndarray + 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 dynamic parameters - arg = self._params.ros__parameters - # get color maps blue_map = cv2.inRange( img, @@ -89,17 +89,26 @@ def process_image( blue_robots = [] red_robots = [] - for cnt in blue_contours: - x, y, h, w = cv2.boundingRect(cnt) - if min(h, w) >= arg.min_size and max(h, w) <= arg.max_size: - # draw bb on debug img - debug_img = cv2.rectangle( + if arg.debug_mode: + + def annotate(x, y, h, w, c): + return cv2.rectangle( debug_img, (x, y), (x + w, y + h), - (255, 0, 0), + c, 2, ) + else: + + def annotate(x, y, h, w, c): + return None + + for cnt in blue_contours: + x, y, h, w = cv2.boundingRect(cnt) + if min(h, w) >= arg.min_size and max(h, w) <= arg.max_size: + # draw bb on debug img + annotate(x, y, h, w, (255, 0, 0)) # TODO I think 1 is for the blue team? blue_robots.append(self.create_robot_msg(x, y, h, w, 1)) @@ -108,26 +117,27 @@ def process_image( x, y, h, w = cv2.boundingRect(cnt) if min(h, w) >= arg.min_size and max(h, w) <= arg.max_size: # draw bb on debug img - debug_img = cv2.rectangle( - debug_img, - (x, y), - (x + w, y + h), - (0, 0, 255), - 2, - ) + annotate(x, y, h, w, (0, 0, 255)) red_robots.append(self.create_robot_msg(x, y, h, w, 2)) return blue_robots, red_robots, blue_map, red_map, debug_img def image_callback(self, msg: Image): + # get dynamic parameters + arg = self._params.ros__parameters + # set variables img = self._cv_bridge.imgmsg_to_cv2(img_msg=msg, desired_encoding="bgr8") header = msg.header - debug_img = np.copy(img) + + 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) + 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) @@ -140,23 +150,23 @@ def image_callback(self, msg: Image): # publish output message self._annotations_pub.publish(robot_array_message) - # make debug image message - # debug_img = cv2.cvtColor(debug_img, cv2.COLOR_BGR2RGB) - debug_img_msg = self._cv_bridge.cv2_to_imgmsg(cvim=debug_img, encoding="bgr8", header=header) + 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) + # 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) + # 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) + 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) + # 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): From 04893f392c03002dd4c52155a7d47ddf2f4a1d72 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?P=C3=A4r?= Date: Tue, 21 May 2024 01:17:46 +0200 Subject: [PATCH 20/31] fix dependency --- bitbots_misc/bitbots_technical_challenge_vision/package.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/bitbots_misc/bitbots_technical_challenge_vision/package.xml b/bitbots_misc/bitbots_technical_challenge_vision/package.xml index 37543d280..b428ec3d3 100644 --- a/bitbots_misc/bitbots_technical_challenge_vision/package.xml +++ b/bitbots_misc/bitbots_technical_challenge_vision/package.xml @@ -9,7 +9,7 @@ rclpy cv2 - numpy + python-numpy cv_bridge sensor_msgs soccer_vision_2d_msgs From 6b6ecaacf1bc4ebe952def80fc1f04aae5825139 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?P=C3=A4r?= Date: Tue, 21 May 2024 11:55:04 +0200 Subject: [PATCH 21/31] remove sim param in launch file --- .../bitbots_technical_challenge_vision/launch/vision.launch | 6 ------ 1 file changed, 6 deletions(-) diff --git a/bitbots_misc/bitbots_technical_challenge_vision/launch/vision.launch b/bitbots_misc/bitbots_technical_challenge_vision/launch/vision.launch index 3960f8248..768915deb 100644 --- a/bitbots_misc/bitbots_technical_challenge_vision/launch/vision.launch +++ b/bitbots_misc/bitbots_technical_challenge_vision/launch/vision.launch @@ -1,8 +1,5 @@ - - - @@ -15,9 +12,6 @@ - - - From a827d69dc54326e5ccd017b287a906268099a634 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?P=C3=A4r?= Date: Tue, 21 May 2024 11:58:08 +0200 Subject: [PATCH 22/31] remove ros__parameters --- .../config/range.yaml | 207 +++++++++--------- 1 file changed, 103 insertions(+), 104 deletions(-) diff --git a/bitbots_misc/bitbots_technical_challenge_vision/config/range.yaml b/bitbots_misc/bitbots_technical_challenge_vision/config/range.yaml index 5e868bc22..e731c2c21 100644 --- a/bitbots_misc/bitbots_technical_challenge_vision/config/range.yaml +++ b/bitbots_misc/bitbots_technical_challenge_vision/config/range.yaml @@ -1,119 +1,118 @@ bitbots_technical_challenge_vision: - ros__parameters: - blue_lower_h: { - type: int, - default_value: 0, - description: "hue value of the lower boundary for blue obstacles", - validation: { - bounds<>: [0, 179] - } + blue_lower_h: { + type: int, + default_value: 0, + description: "hue value of the lower boundary for blue obstacles", + validation: { + bounds<>: [0, 179] } - blue_upper_h: { - type: int, - default_value: 179, - description: "hue value of the upper boundary for blue obstacles", - validation: { - bounds<>: [0, 179] - } + } + blue_upper_h: { + type: int, + default_value: 179, + description: "hue value of the upper boundary for blue obstacles", + validation: { + bounds<>: [0, 179] } - blue_lower_s: { - type: int, - default_value: 0, - description: "separation value of the lower boundary for blue obstacles", - validation: { - bounds<>: [0, 255] - } + } + blue_lower_s: { + type: int, + default_value: 0, + description: "separation value of the lower boundary for blue obstacles", + validation: { + bounds<>: [0, 255] } - blue_upper_s: { - type: int, - default_value: 255, - description: "separation value of the upper boundary for blue obstacles", - validation: { - bounds<>: [0, 255] - } + } + blue_upper_s: { + type: int, + default_value: 255, + 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_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] - } + } + 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: 0, - description: "hue value of the lower boundary for red obstacles", - validation: { - bounds<>: [0, 179] - } + } + red_lower_h: { + type: int, + default_value: 0, + 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_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: 0, - description: "separation value of the lower boundary for red obstacles", - validation: { - bounds<>: [0, 255] - } + } + red_lower_s: { + type: int, + default_value: 0, + 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_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_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] - } + } + 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: 0, - description: "minimum size of an obstacle to be considered", - validation: { - gt_eq<>: 0 - } + } + min_size: { + type: int, + default_value: 0, + description: "minimum size of an obstacle to be considered", + validation: { + gt_eq<>: 0 } - max_size: { - type: int, - default_value: 1, - description: "maximum size of an obstacle to be considered", - validation: { - gt_eq<>: 0 - } - } - debug_mode: { - type: bool, - default_value: true, - description: "set true if debug images should be drawn and published", + } + max_size: { + type: int, + default_value: 1, + description: "maximum size of an obstacle to be considered", + validation: { + gt_eq<>: 0 } + } + debug_mode: { + type: bool, + default_value: true, + description: "set true if debug images should be drawn and published", + } From a1f5d4c750cc30c8d085c78dc5cf05b796258625 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?P=C3=A4r?= Date: Tue, 21 May 2024 12:06:03 +0200 Subject: [PATCH 23/31] fix dependency names --- bitbots_misc/bitbots_technical_challenge_vision/package.xml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/bitbots_misc/bitbots_technical_challenge_vision/package.xml b/bitbots_misc/bitbots_technical_challenge_vision/package.xml index b428ec3d3..9c0a53d6e 100644 --- a/bitbots_misc/bitbots_technical_challenge_vision/package.xml +++ b/bitbots_misc/bitbots_technical_challenge_vision/package.xml @@ -8,8 +8,8 @@ TODO: License declaration rclpy - cv2 - python-numpy + python3-opencv + python3-numpy cv_bridge sensor_msgs soccer_vision_2d_msgs From 214ad6e64290e8c12f7f227d8d559a78979444bf Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?P=C3=A4r?= Date: Tue, 21 May 2024 12:44:52 +0200 Subject: [PATCH 24/31] get build dependent files back because they are actually really important apparently and should also be in the git repository --- .gitignore | 3 - ...tbots_technical_challenge_vision_params.py | 475 ++++++++++++++++++ .../bitbots_technical_challenge_vision | 0 3 files changed, 475 insertions(+), 3 deletions(-) create mode 100644 bitbots_misc/bitbots_technical_challenge_vision/bitbots_technical_challenge_vision/bitbots_technical_challenge_vision_params.py create mode 100644 bitbots_misc/bitbots_technical_challenge_vision/resource/bitbots_technical_challenge_vision diff --git a/.gitignore b/.gitignore index 4034634ce..b9493ca95 100644 --- a/.gitignore +++ b/.gitignore @@ -224,6 +224,3 @@ doku/* /bitbots_team_communication/bitbots_team_communication/RobocupProtocol # Protobuf generated file /bitbots_team_communication/bitbots_team_communication/robocup_extension_pb2.py - -bitbots_misc/bitbots_technical_challenge_vision/bitbots_technical_challenge_vision/bitbots_technical_challenge_vision_params.py -bitbots_misc/bitbots_technical_challenge_vision/resource/bitbots_technical_challenge_vision 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/resource/bitbots_technical_challenge_vision b/bitbots_misc/bitbots_technical_challenge_vision/resource/bitbots_technical_challenge_vision new file mode 100644 index 000000000..e69de29bb From 14ba794ec20fcabfa4e835e8aba7b627a7fc2fec Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?P=C3=A4r?= Date: Thu, 23 May 2024 16:28:59 +0200 Subject: [PATCH 25/31] remove rest of sim param --- .../launch/vision.launch | 14 +++----------- 1 file changed, 3 insertions(+), 11 deletions(-) diff --git a/bitbots_misc/bitbots_technical_challenge_vision/launch/vision.launch b/bitbots_misc/bitbots_technical_challenge_vision/launch/vision.launch index 768915deb..ca515be71 100644 --- a/bitbots_misc/bitbots_technical_challenge_vision/launch/vision.launch +++ b/bitbots_misc/bitbots_technical_challenge_vision/launch/vision.launch @@ -3,15 +3,7 @@ - - - - - - - - - - - + + + From cf95af09fc33656415d50ba1339b1b35d9b510e0 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?P=C3=A4r?= Date: Fri, 24 May 2024 15:10:36 +0200 Subject: [PATCH 26/31] stop including range.yaml as params file --- .../bitbots_technical_challenge_vision/launch/vision.launch | 1 - 1 file changed, 1 deletion(-) diff --git a/bitbots_misc/bitbots_technical_challenge_vision/launch/vision.launch b/bitbots_misc/bitbots_technical_challenge_vision/launch/vision.launch index ca515be71..a39c19572 100644 --- a/bitbots_misc/bitbots_technical_challenge_vision/launch/vision.launch +++ b/bitbots_misc/bitbots_technical_challenge_vision/launch/vision.launch @@ -4,6 +4,5 @@ - From a37c0fb74281fa2e1ed090aa8df127f71b485d25 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?P=C3=A4r?= Date: Fri, 24 May 2024 15:11:39 +0200 Subject: [PATCH 27/31] update params in 1s intervals --- .../bitbots_technical_challenge_vision.py | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) 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 index ac030c050..595391552 100755 --- 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 @@ -32,6 +32,7 @@ def __init__(self): 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() + self._timer = self.create_timer(1, self.timer_callback) def create_robot_msg(self, x: int, y: int, h: int, w: int, t: int) -> Robot: """ @@ -125,7 +126,7 @@ def annotate(x, y, h, w, c): def image_callback(self, msg: Image): # get dynamic parameters - arg = self._params.ros__parameters + arg = self._params # set variables img = self._cv_bridge.imgmsg_to_cv2(img_msg=msg, desired_encoding="bgr8") @@ -168,6 +169,11 @@ def image_callback(self, msg: Image): self._debug_clrmp_pub_blue.publish(clrmp_blue_msg) self._debug_clrmp_pub_red.publish(clrmp_red_msg) + def timer_callback(self): + if self._param_listener.is_old(self._params): + self._param_listener.refresh_dynamic_parameters() + self._params = self._param_listener.get_params() + def main(args=None): rclpy.init(args=args) From f3c34df98d1bd8cd2bcf41fb52a2ebc65ff1d15f Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?P=C3=A4r?= Date: Wed, 29 May 2024 10:23:39 +0200 Subject: [PATCH 28/31] update parameters in image_callback --- .../bitbots_technical_challenge_vision.py | 10 ++++------ 1 file changed, 4 insertions(+), 6 deletions(-) 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 index 595391552..989b7e41a 100755 --- 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 @@ -32,7 +32,6 @@ def __init__(self): 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() - self._timer = self.create_timer(1, self.timer_callback) def create_robot_msg(self, x: int, y: int, h: int, w: int, t: int) -> Robot: """ @@ -126,6 +125,10 @@ def annotate(x, y, h, w, c): 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 @@ -169,11 +172,6 @@ def image_callback(self, msg: Image): self._debug_clrmp_pub_blue.publish(clrmp_blue_msg) self._debug_clrmp_pub_red.publish(clrmp_red_msg) - def timer_callback(self): - if self._param_listener.is_old(self._params): - self._param_listener.refresh_dynamic_parameters() - self._params = self._param_listener.get_params() - def main(args=None): rclpy.init(args=args) From a39f7926a5d166d28e1667a30d2d9c94f3911daa Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?P=C3=A4r?= Date: Wed, 29 May 2024 12:23:10 +0200 Subject: [PATCH 29/31] use same topic as vision for robot annotations --- .../bitbots_technical_challenge_vision.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 index 989b7e41a..a5999dd03 100755 --- 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 @@ -21,7 +21,7 @@ def __init__(self): self._package_path = get_package_share_directory("bitbots_technical_challenge_vision") self._cv_bridge = CvBridge() - self._annotations_pub = self.create_publisher(RobotArray, "/bitbots_technical_challenge_vision", 10) + 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 From 09f4f22e5987071e146a69846535a6f6deb9a51d Mon Sep 17 00:00:00 2001 From: Florian Vahl Date: Sun, 7 Jul 2024 16:27:09 +0000 Subject: [PATCH 30/31] Fix and tune tcv --- .../bitbots_technical_challenge_vision.py | 44 +++++++++---------- .../config/range.yaml | 18 ++++---- .../launch/vision.launch | 2 +- sync_includes_wolfgang_nuc.yaml | 1 + 4 files changed, 31 insertions(+), 34 deletions(-) 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 index a5999dd03..7c7957aad 100755 --- 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 @@ -1,4 +1,4 @@ -from typing import Tuple +from typing import Optional, Tuple import cv2 import numpy as np @@ -33,14 +33,14 @@ def __init__(self): 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, h: int, w: int, t: int) -> Robot: + 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 h: bb height :param w: bb width + :param h: bb height :param t: robot team :return: robot message for that robot """ @@ -89,37 +89,33 @@ def process_image( blue_robots = [] red_robots = [] - if arg.debug_mode: - - def annotate(x, y, h, w, c): - return cv2.rectangle( - debug_img, - (x, y), - (x + w, y + h), - c, - 2, - ) - else: - - def annotate(x, y, h, w, c): + 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, h, w = cv2.boundingRect(cnt) - if min(h, w) >= arg.min_size and max(h, w) <= arg.max_size: + 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, h, w, (255, 0, 0)) + 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, h, w, 1)) + blue_robots.append(self.create_robot_msg(x, y, w, h, 1)) for cnt in red_contours: - x, y, h, w = cv2.boundingRect(cnt) - if min(h, w) >= arg.min_size and max(h, w) <= arg.max_size: + 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, h, w, (0, 0, 255)) + annotate(x, y, w, h, (0, 0, 255)) - red_robots.append(self.create_robot_msg(x, y, h, w, 2)) + red_robots.append(self.create_robot_msg(x, y, w, h, 2)) return blue_robots, red_robots, blue_map, red_map, debug_img diff --git a/bitbots_misc/bitbots_technical_challenge_vision/config/range.yaml b/bitbots_misc/bitbots_technical_challenge_vision/config/range.yaml index e731c2c21..1b2a7ba58 100644 --- a/bitbots_misc/bitbots_technical_challenge_vision/config/range.yaml +++ b/bitbots_misc/bitbots_technical_challenge_vision/config/range.yaml @@ -1,7 +1,7 @@ bitbots_technical_challenge_vision: blue_lower_h: { type: int, - default_value: 0, + default_value: 92, description: "hue value of the lower boundary for blue obstacles", validation: { bounds<>: [0, 179] @@ -9,7 +9,7 @@ bitbots_technical_challenge_vision: } blue_upper_h: { type: int, - default_value: 179, + default_value: 110, description: "hue value of the upper boundary for blue obstacles", validation: { bounds<>: [0, 179] @@ -17,7 +17,7 @@ bitbots_technical_challenge_vision: } blue_lower_s: { type: int, - default_value: 0, + default_value: 90, description: "separation value of the lower boundary for blue obstacles", validation: { bounds<>: [0, 255] @@ -25,7 +25,7 @@ bitbots_technical_challenge_vision: } blue_upper_s: { type: int, - default_value: 255, + default_value: 236, description: "separation value of the upper boundary for blue obstacles", validation: { bounds<>: [0, 255] @@ -49,7 +49,7 @@ bitbots_technical_challenge_vision: } red_lower_h: { type: int, - default_value: 0, + default_value: 138, description: "hue value of the lower boundary for red obstacles", validation: { bounds<>: [0, 179] @@ -65,7 +65,7 @@ bitbots_technical_challenge_vision: } red_lower_s: { type: int, - default_value: 0, + default_value: 78, description: "separation value of the lower boundary for red obstacles", validation: { bounds<>: [0, 255] @@ -97,7 +97,7 @@ bitbots_technical_challenge_vision: } min_size: { type: int, - default_value: 0, + default_value: 20, description: "minimum size of an obstacle to be considered", validation: { gt_eq<>: 0 @@ -105,7 +105,7 @@ bitbots_technical_challenge_vision: } max_size: { type: int, - default_value: 1, + default_value: 200, description: "maximum size of an obstacle to be considered", validation: { gt_eq<>: 0 @@ -113,6 +113,6 @@ bitbots_technical_challenge_vision: } debug_mode: { type: bool, - default_value: true, + 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 index a39c19572..1d36dc25f 100644 --- a/bitbots_misc/bitbots_technical_challenge_vision/launch/vision.launch +++ b/bitbots_misc/bitbots_technical_challenge_vision/launch/vision.launch @@ -3,6 +3,6 @@ - + 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 From 34ef2add27633d8a73ddecbd380f6a804ce4cef9 Mon Sep 17 00:00:00 2001 From: Florian Vahl Date: Sun, 7 Jul 2024 16:28:57 +0000 Subject: [PATCH 31/31] Increase obstacle size --- .../bitbots_technical_challenge_vision/config/range.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/bitbots_misc/bitbots_technical_challenge_vision/config/range.yaml b/bitbots_misc/bitbots_technical_challenge_vision/config/range.yaml index 1b2a7ba58..6716d55af 100644 --- a/bitbots_misc/bitbots_technical_challenge_vision/config/range.yaml +++ b/bitbots_misc/bitbots_technical_challenge_vision/config/range.yaml @@ -105,7 +105,7 @@ bitbots_technical_challenge_vision: } max_size: { type: int, - default_value: 200, + default_value: 400, description: "maximum size of an obstacle to be considered", validation: { gt_eq<>: 0