From c30ddcf7e0849e43e3dc3327c498ae37f4fbcdf0 Mon Sep 17 00:00:00 2001 From: Yurii Schetchikov Date: Mon, 19 Aug 2024 16:45:01 +0300 Subject: [PATCH 01/23] add automatic waterstrider teleoperation --- package.xml | 18 ++++ requirements.txt | 3 + resource/ws_nodes_py | 0 setup.cfg | 4 + setup.py | 29 +++++++ test/test_copyright.py | 25 ++++++ test/test_flake8.py | 25 ++++++ test/test_pep257.py | 23 +++++ ws_nodes_py/__init__.py | 0 ws_nodes_py/nmea_parser.py | 151 +++++++++++++++++++++++++++++++++ ws_nodes_py/point_logger.py | 124 +++++++++++++++++++++++++++ ws_nodes_py/regulator.py | 162 ++++++++++++++++++++++++++++++++++++ ws_nodes_py/video.py | 33 ++++++++ 13 files changed, 597 insertions(+) create mode 100644 package.xml create mode 100644 requirements.txt create mode 100644 resource/ws_nodes_py create mode 100644 setup.cfg create mode 100644 setup.py create mode 100644 test/test_copyright.py create mode 100644 test/test_flake8.py create mode 100644 test/test_pep257.py create mode 100644 ws_nodes_py/__init__.py create mode 100644 ws_nodes_py/nmea_parser.py create mode 100644 ws_nodes_py/point_logger.py create mode 100644 ws_nodes_py/regulator.py create mode 100644 ws_nodes_py/video.py diff --git a/package.xml b/package.xml new file mode 100644 index 0000000..37762f7 --- /dev/null +++ b/package.xml @@ -0,0 +1,18 @@ + + + + ws_nodes_py + 0.0.0 + TODO: Package description + mnc + TODO: License declaration + + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + + + ament_python + + diff --git a/requirements.txt b/requirements.txt new file mode 100644 index 0000000..3e992ab --- /dev/null +++ b/requirements.txt @@ -0,0 +1,3 @@ +geographic-msgs==1.0.6 +pyserial==3.5 +pynmea2==1.19.0 \ No newline at end of file diff --git a/resource/ws_nodes_py b/resource/ws_nodes_py new file mode 100644 index 0000000..e69de29 diff --git a/setup.cfg b/setup.cfg new file mode 100644 index 0000000..b5128d1 --- /dev/null +++ b/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/ws_nodes_py +[install] +install_scripts=$base/lib/ws_nodes_py diff --git a/setup.py b/setup.py new file mode 100644 index 0000000..29c4a42 --- /dev/null +++ b/setup.py @@ -0,0 +1,29 @@ +from setuptools import find_packages, setup + +package_name = 'ws_nodes_py' + +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='mnc', + maintainer_email='m.chemodanov@noniusgroup.ru', + description='TODO: Package description', + license='TODO: License declaration', + tests_require=['pytest'], + entry_points={ + 'console_scripts': [ + 'regulator = ws_nodes_py.regulator:main', + 'point_logger = ws_nodes_py.point_logger:main', + 'nmea_reader = ws_nodes_py.nmea_parser:main', + 'video_recorder = ws_nodes_py.video:main', + ], + }, +) diff --git a/test/test_copyright.py b/test/test_copyright.py new file mode 100644 index 0000000..97a3919 --- /dev/null +++ b/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. + +from ament_copyright.main import main +import pytest + + +# 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/test/test_flake8.py b/test/test_flake8.py new file mode 100644 index 0000000..27ee107 --- /dev/null +++ b/test/test_flake8.py @@ -0,0 +1,25 @@ +# 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. + +from ament_flake8.main import main_with_errors +import pytest + + +@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/test/test_pep257.py b/test/test_pep257.py new file mode 100644 index 0000000..b234a38 --- /dev/null +++ b/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. + +from ament_pep257.main import main +import pytest + + +@pytest.mark.linter +@pytest.mark.pep257 +def test_pep257(): + rc = main(argv=['.', 'test']) + assert rc == 0, 'Found code style errors / warnings' diff --git a/ws_nodes_py/__init__.py b/ws_nodes_py/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/ws_nodes_py/nmea_parser.py b/ws_nodes_py/nmea_parser.py new file mode 100644 index 0000000..eda1d0c --- /dev/null +++ b/ws_nodes_py/nmea_parser.py @@ -0,0 +1,151 @@ +#!/usr/bin/python + +import rclpy +from rclpy.node import Node +from std_msgs.msg import Float64, Int64, String +from geometry_msgs.msg import Twist +# sudo apt install ros-iron-geographic-msgs +from geographic_msgs.msg import GeoPoint # GeoPoint.latitude, GeoPoint.longitude, GeoPoint.altitude +# pip3 install pynmea2 +import pynmea2 +import serial +import io + +KNOTS_TO_M_PER_S = 0.514444 + +class NmeaReader(Node): + def __init__(self): + super().__init__('nmea_reader') + + # все поддерживаемые типы есть в pynmea2>types>talker.py (в VS можно перейти с зажатым ctrl и лкм по одному из типов) + all_types = { + "dbt": [pynmea2.types.talker.DBT, [ + ["dbt", Float64, self.dbt_publisher], # depth + ]], + "gga": [pynmea2.types.talker.GGA, [ + ["gga", GeoPoint, self.gga_publisher], # lat lon alt + ["gga_quality", Int64, self.gga_quality_publisher], # quality + ]], + "rmc": [pynmea2.types.talker.RMC, [ + ["rmc", Twist, self.rmc_publisher], # linear.x = SOG, angular.x = COG + ]], + # TODO добавить другие типы сообщений, которые будут нужны (ещё надо сделать обработчик) + } + + self.listen_types = {} + self.listen_raw_types = {} + + self.declare_parameter('port', rclpy.Parameter.Type.STRING) + self.declare_parameter('baudrate', rclpy.Parameter.Type.INTEGER) + self.declare_parameter('hz', rclpy.Parameter.Type.INTEGER) + self.declare_parameter('messages', rclpy.Parameter.Type.STRING_ARRAY) + self.declare_parameter('raw_messages', rclpy.Parameter.Type.STRING_ARRAY) + + # подключаемся к порту + port = self.get_parameter('port').value + baudrate = self.get_parameter('baudrate').value + ser = serial.Serial(port, baudrate, timeout=1) + self.serial_io = io.TextIOWrapper(io.BufferedRWPair(ser, ser)) + + messages = list(dict.fromkeys(map(lambda x: x.lower(), self.get_parameter('messages').value))) # remove duplicates and make lowercase + for m in messages: + if m in all_types.keys(): + msg_type = all_types[m] + pynmea2_type = msg_type[0] + if pynmea2_type not in self.listen_types.keys(): + self.listen_types[pynmea2_type] = [] + processors = self.listen_types[pynmea2_type] + for t in msg_type[1]: + processors.append([self.create_publisher(t[1], t[0], 1), t[2]]) + else: + raise Exception("message type not found or such message is already being processed") + + raw_messages = list(dict.fromkeys(map(lambda x: x.lower(), self.get_parameter('raw_messages').value))) # remove duplicates and make lowercase + for m in raw_messages: + if m in all_types: + self.listen_raw_types[all_types[m][0]] = self.create_publisher(String, f"{m}_raw", 1) # gga_raw, dbt_raw, etc. + else: + raise Exception("message type not found or such message is already being processed") + + self.updater = self.create_timer(1 / self.get_parameter('hz').value, self.update) + + + + # TODO другие обработчики сообщений сюда + + @staticmethod + def dbt_publisher(self, msg: pynmea2.types.talker.DBT, publisher): + # dbt - глубина в метрах + depth = Float64() + depth.data = float(msg.depth_meters) + publisher.publish(depth) + + @staticmethod + def gga_quality_publisher(self, msg: pynmea2.types.talker.GGA, publisher): + # gga - ~~координаты~~(перечёркнуто), качество сигнала + gps_qual = Int64() + gps_qual.data = msg.gps_qual + publisher.publish(gps_qual) + + @staticmethod + def gga_publisher(self, msg: pynmea2.types.talker.GGA, publisher): + # gga - координаты, ~~качество сигнала~~(перечёркнуто). Для координат надо использовать сообщение GeoPoint + point = GeoPoint() + if msg.latitude is None: + point.latitude = 0.0 + else: + point.latitude = msg.latitude + if msg.longitude is None: + point.longitude = 0.0 + else: + point.longitude = msg.longitude + if msg.altitude is None: + point.altitude = 0.0 + else: + point.altitude = msg.altitude + publisher.publish(point) + + @staticmethod + def rmc_publisher(self, msg: pynmea2.types.talker.RMC, publisher): + # rmc - COG, SOG (скорость, курс) + twist = Twist() + if msg.spd_over_grnd is None: + twist.linear.x = 0.0 + else: + twist.linear.x = msg.spd_over_grnd * KNOTS_TO_M_PER_S # в float'ах + if msg.true_course is None: + twist.angular.z = 0.0 + else: + twist.angular.z = msg.true_course # в float'ах + publisher.publish(twist) + + def update(self): + try: + line = self.serial_io.readline() + msg = pynmea2.parse(line) + if type(msg) in self.listen_types: + # вызываем обработчик сообщений данного типа + # да, возможно это выглядит страшно) + # listen_types = {type(message): [ [publisher, sender], [..] ]} + for publisher, sender in self.listen_types[type(msg)]: + sender(self, msg, publisher) + if type(msg) in self.listen_raw_types: + s = String() + s.data = line + self.listen_raw_types[type(msg)].publish(s) + except serial.SerialException as e: + raise Exception(f'Device error: {e}') + except pynmea2.ParseError as e: + self.get_logger().warn(f'Parse error: {e}') + + +def main(): + rclpy.init() + nmea_reader = NmeaReader() + rclpy.spin(nmea_reader) + nmea_reader.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() \ No newline at end of file diff --git a/ws_nodes_py/point_logger.py b/ws_nodes_py/point_logger.py new file mode 100644 index 0000000..adfde32 --- /dev/null +++ b/ws_nodes_py/point_logger.py @@ -0,0 +1,124 @@ +#!/usr/bin/python + +import rclpy +from rclpy.node import Node +from std_msgs.msg import Float64 +from geographic_msgs.msg import GeoPoint # GeoPoint.latitude, GeoPoint.longitude, GeoPoint.altitude +from time import time +from datetime import datetime +import getpass +import os +import csv + + +class PointLogger(Node): + def save(self, data_1, data_2): + with open(self.FILENAME, 'a', newline='') as f: + writer = csv.writer(f) + now = datetime.now() # current date and time + writer.writerow([now.strftime("%m/%d/%Y, %H:%M:%S"), *data_1, data_2]) + + def update(self): + """ + We are looking for the last pair of messages that fits the specified maximum interval between records. + In any case, we clean the buffer of read messages + """ + if len(self.data) == 2: + time_pos = self.data["position"][-1] + time_dth = self.data["depth"][-1] + + if abs(time_pos - time_dth) <= self.MAX_DELTA_TIME: + lat, lon = self.data["position"][:2] + dth = self.data["depth"][0] + if not(any(x is None for x in [lat ,lon, dth])): + self.save((lat, lon), dth) + self.clear_data() + else: + return + + def __init__(self): + super().__init__('point_logger') + + self.data = {} + + # geting directory for csv file + self.declare_parameter('directory', rclpy.Parameter.Type.STRING) + # geting delays + self.declare_parameter('max_delta_time', rclpy.Parameter.Type.DOUBLE) + self.declare_parameter('delay_between_writes', rclpy.Parameter.Type.DOUBLE) + + # connect to gps topic + self.pos = self.create_subscription( + GeoPoint, + "gga", + self.set_position, 1 + ) + # connect to depth topic + self.create_subscription( + Float64, + "dbt", + self.set_depth, 1 + ) + + # make absolute path to dir and make dir if need + path = self.get_declare_parameter("directory", lambda x: x, "directory not specified") + # если указан не абсолютный путь, то задаём папку в директории пользователя + if not path.startswith("/home"): + # с проверкой / в начале не абсолютного пути, если его там нет то добавляем + path = f"/home/{getpass.getuser()}{'' if path.startswith('/') else '/'}{path}" + # добавляем / в конец, если его там нет то добавляем + path = f"{path}{'' if path.endswith('/') else '/'}" + # создаём папку, если она не существует + if not os.path.isdir(path): + os.makedirs(path) + self.FILENAME = f"{path}{datetime.today().strftime('%Y-%m-%d %H:%M:%S')}.csv" + + self.MAX_DELTA_TIME = self.get_declare_parameter("max_delta_time", lambda x: x > 0, "max_delta_time not more than zero") + + self.get_logger().info(f"point logger ready to write logs to file {self.FILENAME}") + + def set_position(self, msg): + self.data["position"]=(msg.latitude, msg.longitude, time()) + self.update() + + def set_depth(self, msg): + self.data["depth"]=(msg.data, time()) + self.update() + + def get_declare_parameter(self, name, checker, error_text): + value = self.get_parameter(name).value + if not checker(value): + self.get_logger().error(error_text) + raise Exception(error_text) + return value + + def clear_data(self): + self.data = {} # self.data = [ DataLog, DataLog, ... ] + + +class DataLog: + def __init__(self, name, data, time): + self.__name = name + self.__data = data + self.__time = time + + def get_name(self): + return self.__name + + def get_data(self): + return self.__data + + def get_time(self): + return self.__time + + +def main(): + rclpy.init() + point_logger = PointLogger() + rclpy.spin(point_logger) + point_logger.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() \ No newline at end of file diff --git a/ws_nodes_py/regulator.py b/ws_nodes_py/regulator.py new file mode 100644 index 0000000..865b5e8 --- /dev/null +++ b/ws_nodes_py/regulator.py @@ -0,0 +1,162 @@ +#!/usr/bin/env python3 + +import rclpy +from rclpy.node import Node +from std_msgs.msg import Bool +from geometry_msgs.msg import Twist +from geographic_msgs.msg import GeoPoint +from nonius_msgs.msg import Track + +from geographiclib.geodesic import Geodesic + +import math + + +def translate(value, leftMin, leftMax, rightMin, rightMax): + # Figure out how 'wide' each range is + leftSpan = leftMax - leftMin + rightSpan = rightMax - rightMin + + # Convert the left range into a 0-1 range (float) + valueScaled = float(value - leftMin) / float(leftSpan) + + # Convert the 0-1 range into a value in the right range. + return rightMin + (valueScaled * rightSpan) + +def scale_angle(value): + return translate(value, -180.0, 180.0, -1.0, 1.0) + +class Controller(Node): + def add_param(self, name, defvalue): + self.config[name] = self.declare_parameter(name, defvalue).value + + def parameter_callback(self, data): + for parameter in data: + if parameter.name.split('.')[1] == self.name: + self.config[parameter.name.split('.')[2]] = parameter.value + + def __init__(self): + super().__init__('regulator') + self.declare_parameter('hz', rclpy.Parameter.Type.INTEGER) + self.declare_parameter('distance_threshold', rclpy.Parameter.Type.DOUBLE) + #self.declare_parameter('normal_speed', rclpy.Parameter.Type.DOUBLE) + self.declare_parameter('min_speed_for_cog', rclpy.Parameter.Type.DOUBLE) + self.declare_parameter('k_p_angular', rclpy.Parameter.Type.DOUBLE) + self.declare_parameter('max_thrust', rclpy.Parameter.Type.DOUBLE) + #self.declare_parameter('k_p_linear', rclpy.Parameter.Type.DOUBLE) + #self.declare_parameter('linear_regulator_threshold', rclpy.Parameter.Type.DOUBLE) + + self.publisher = self.create_publisher(Twist, 'twist_command', 10) + self.update_timer = self.create_timer(1 / self.get_parameter('hz').value, self.update) + + self.create_subscription(GeoPoint, 'gga', self.set_position, 1) + self.create_subscription(Twist, 'rmca', self.set_state, 1) + self.create_subscription(Track, 'target_position', self.set_target, 1) + self.create_subscription(Bool, 'stop_moving', self.stop, 1) + + self.stopped = True + self.state = None + self.target = None + self.position = None + self.origin = None + + self.path = [] + self.current_target = 0 + + self.get_logger().info('Auto teleoperation started') + + def set_position(self, position): + self.position = position + + def set_state(self, state): + self.state = state + + def stop(self, stop): + self.get_logger().info('Stop regulation') + self.stopped = True + self.publisher.publish(Twist()) + + def can_move(self): + return not self.stopped and self.state is not None and len(self.path) > 0 and self.position is not None + + def error_track(self, position): + vec1 = (self.target.lantitude - self.origin.latitude, self.target.longitude - self.origin.longitude) + vec2 = (position.lantitude - self.origin.latitude, position.longitude - self.origin.longitude) + return (vec1[0]*vec2[1] - vec1[1]*vec2[0]) + + def set_target(self, track): + if (track.mode == "point" or track.mode == "path") and len(track.points) > 0: + self.path = track.points + self.current_target = 0 + self.origin = self.position + self.stopped = False + self.get_logger().info(f'Set new target of {len(self.path)} points') + + @staticmethod + def get_distance_and_heading(position, target): + geod = Geodesic.WGS84 + res = geod.Inverse(position.latitude, position.longitude, target.latitude, target.longitude) + + result = Twist() + result.linear.x = res['s12'] + result.angular.z = res['azi2'] + return result + + @staticmethod + def angle_error(target_angle, source_angle): + result = target_angle - source_angle + if result > 180: + result = result - 360 + return result + + def move_to_point(self, position, target, state, max_thrust, min_speed, distance_threshold, k_p_angular): + error = Controller.get_distance_and_heading(position, target) + heading = state.angular.z + if heading > 180: + heading = heading - 360 + self.get_logger().info(f'Distance {error.linear.x}, heading {heading}, bearing {error.angular.z}') + if error.linear.x < distance_threshold: + return None + + result = Twist() + if state.linear.x > min_speed: + angle_error = Controller.angle_error(error.angular.z, heading) + value = scale_angle(angle_error) + value = k_p_angular * value + self.get_logger().info(f'Angle_error {angle_error}, scaled angle {value}') + result.angular.z = math.copysign(min(abs(max_thrust), abs(value)), value) + result.linear.x = (max_thrust - abs(result.angular.z)) + else: + self.get_logger().info('Speed too low, move straight') + result.linear.x = max_thrust + return result + + def update(self): + if not self.can_move(): + return + result = self.move_to_point(self.position, self.path[self.current_target], self.state, + self.get_parameter('max_thrust').value, + self.get_parameter('min_speed_for_cog').value, + self.get_parameter('distance_threshold').value, + self.get_parameter('k_p_angular').value) + + if result is None: + if self.current_target < len(self.path)-1: + self.current_target = self.current_target + 1 + self.get_logger().info(f'Point {self.current_target}/{len(self.path)} was achieved') + else: + self.path = [] + self.get_logger().info(f'Path finished') + self.stopped = True + else: + self.publisher.publish(result) + +def main(): + rclpy.init() + controller = Controller() + rclpy.spin(controller) + controller.destroy_node() + rclpy.shutdown() + +if __name__ == '__main__': + main() \ No newline at end of file diff --git a/ws_nodes_py/video.py b/ws_nodes_py/video.py new file mode 100644 index 0000000..e4e41ba --- /dev/null +++ b/ws_nodes_py/video.py @@ -0,0 +1,33 @@ +#!/usr/bin/python3 +import rclpy +import subprocess +from rclpy.node import Node +from std_msgs.msg import Bool +from datetime import datetime + +class ImagePublisher(Node): + def __init__(self): + super().__init__("image_recorder") + self.stop_video = self.create_subscription(Bool, "rec", self.control_video, 1) + self.enable = False + + def control_video(self, msg): + self.enable = not self.enable + if self.enable: + subprocess.run(["sudo", "systemctl", "start", "vid.service"]) + # subprocess.run(["ffmpeg", "-f", "v4l2", "-i", "/dev/video0", f"{datetime.now().strftime('%m-%d-%Y_%H:%M:%S')}.mp4", "&"]) + else: + subprocess.run(["sudo", "systemctl", "stop", "vid.service"]) + + + +def main(args=None): + rclpy.init(args=args) + + ip = ImagePublisher() + rclpy.spin(ip) + ip.destroy_node() + rclpy.shutdown() + +if __name__ == '__main__': + main() -- GitLab From 2d9f86d2ca23f04e9dc8f5e12b04989f616bbb12 Mon Sep 17 00:00:00 2001 From: "toropov.nik" Date: Fri, 20 Sep 2024 20:07:37 +0300 Subject: [PATCH 02/23] rewrite for default.GetParameterNode from Node --- setup.py | 16 +- ws_nodes_py/compass.py | 100 ++++++++ ws_nodes_py/default/GetParameterNode.py | 75 ++++++ .../GetParameterNode.cpython-38.pyc | Bin 0 -> 4083 bytes ws_nodes_py/legacy/regulator copy.py | 163 +++++++++++++ ws_nodes_py/legacy/to_berlin.py | 50 ++++ ws_nodes_py/regulator.py | 223 ++++++++---------- .../__pycache__/for_world.cpython-38.pyc | Bin 0 -> 896 bytes ws_nodes_py/settings/for_world.py | 61 +++++ ws_nodes_py/twist_controller.py | 45 ++++ ws_nodes_py/video.py | 33 --- ws_nodes_py/videoD.py | 164 +++++++++++++ ws_nodes_py/videoF.py | 198 ++++++++++++++++ ws_nodes_py/videoS.py | 186 +++++++++++++++ ws_nodes_py/world.py | 94 ++++++++ 15 files changed, 1242 insertions(+), 166 deletions(-) create mode 100644 ws_nodes_py/compass.py create mode 100644 ws_nodes_py/default/GetParameterNode.py create mode 100644 ws_nodes_py/default/__pycache__/GetParameterNode.cpython-38.pyc create mode 100644 ws_nodes_py/legacy/regulator copy.py create mode 100644 ws_nodes_py/legacy/to_berlin.py create mode 100644 ws_nodes_py/settings/__pycache__/for_world.cpython-38.pyc create mode 100644 ws_nodes_py/settings/for_world.py create mode 100644 ws_nodes_py/twist_controller.py delete mode 100644 ws_nodes_py/video.py create mode 100644 ws_nodes_py/videoD.py create mode 100644 ws_nodes_py/videoF.py create mode 100644 ws_nodes_py/videoS.py create mode 100644 ws_nodes_py/world.py diff --git a/setup.py b/setup.py index 29c4a42..825cf75 100644 --- a/setup.py +++ b/setup.py @@ -20,10 +20,20 @@ setup( tests_require=['pytest'], entry_points={ 'console_scripts': [ - 'regulator = ws_nodes_py.regulator:main', + # open water 'point_logger = ws_nodes_py.point_logger:main', 'nmea_reader = ws_nodes_py.nmea_parser:main', - 'video_recorder = ws_nodes_py.video:main', + # sensors + 'compass = ws_nodes_py.compass:main', + # propultion and regulators + 'twist_controller = ws_nodes_py.twist_controller:main', + 'regulator = ws_nodes_py.regulator:main', + 'to_berlin = ws_nodes_py.legacy.to_berlin:main', + # mission + 'camcvF = ws_nodes_py.videoF:main', + 'camcvS = ws_nodes_py.videoS:main', + 'camcvD = ws_nodes_py.videoD:main', + 'world = ws_nodes_py.world:main', ], }, -) +) \ No newline at end of file diff --git a/ws_nodes_py/compass.py b/ws_nodes_py/compass.py new file mode 100644 index 0000000..380ec3c --- /dev/null +++ b/ws_nodes_py/compass.py @@ -0,0 +1,100 @@ +#!/usr/bin/python + +import rclpy +from ws_nodes_py.default.GetParameterNode import GetParameterNode +from time import time +from std_msgs.msg import Int16, String +from geometry_msgs.msg import Twist + + +class Compass(GetParameterNode): + # for andgular speed + last_heading = 0 + last_heading_time = 0 + d_heads = [[0, 0] for _ in range(20)] + + # for reset + __compass = 0 + + def __init__(self): + super().__init__('compass') + # TODO сделать виртуальный курс + # TODO сделать переход на виртуальный курс по требованию + + # for zero heading + self.compass_delta = self.get_declare_parameter('delta_compass', rclpy.Parameter.Type.INTEGER, + checker=lambda x: 360 > x > 0, error_text="delta_compass not in (0, 360)") + + self.surbscribtion(Int16, "compass_topic", self.compass_callback, checker=lambda x: x) + self.surbscribtion(String, "reset_topic", self.reset_callback, checker=lambda x: x) + + self.pub_heading = self.publisher_from_declare(Int16, "heading_topic", checker=lambda x: x) + + def compass_callback(self, msg): + # Проверка на правильность преобразования + assert -10%360 == 350 + + if not 360 >= msg.data >= 0: + return + + # for heading offset + self.__compass = msg.data + + try: + self.last_compass.pop(0) + self.last_compass.append((msg.data - self.compass_delta) % 360) + except AttributeError: # if first compass data + self.get_logger().info("start median") + self.last_compass = [msg.data, msg.data, msg.data] + + # median + a = self.last_compass[0] + b = self.last_compass[1] + c = self.last_compass[2] + self.heading = int(max(a, c) if (max(a, b) == max(b, c)) else max(b, min(a, c))) + + self.pub(self.pub_heading, Int16, self.heading) + + def twist_heading_callback(self): + # Расчёт виртуального курса (trash) + old_motor_ang_speed = { + 0: 0, + # 0.15: -10, + 0.3: -20, + } + + def reset_callback(self, msg): + if msg.data == "reset_heading": + self.get_logger().info(f"reset heading {self.__compass}") + self.compass_delta = self.__compass + + def angular_speed(self): + # Разность угла [-179;180] + d_head = self.heading - self.last_heading + if d_head <= -180: + d_head = 360 + d_head + if d_head >= 180: + d_head = d_head - 360 + + # Мнгновенная угловая скорость + d_time = time() - self.last_heading_time + self.last_heading_time = time() + self.last_heading = self.heading + ang_speed = round((d_head) / (d_time)) + + # Средняя угловая скорость за некоторый период времени + self.d_heads.append([ang_speed, d_time]) + self.d_heads.pop(0) + return round(sum(spd * t for spd, t in self.d_heads) / sum(t for spd, t in self.d_heads),1) + + +def main(): + rclpy.init() + compass = Compass() + rclpy.spin(compass) + compass.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() \ No newline at end of file diff --git a/ws_nodes_py/default/GetParameterNode.py b/ws_nodes_py/default/GetParameterNode.py new file mode 100644 index 0000000..5c57992 --- /dev/null +++ b/ws_nodes_py/default/GetParameterNode.py @@ -0,0 +1,75 @@ +#!/usr/bin/python + +import rclpy +from rclpy.node import Node +from rclpy.exceptions import ParameterAlreadyDeclaredException +from geometry_msgs.msg import Twist, Point + + +class DeclareParameterException(Exception): + pass + + +class GetParameterNode(Node): + def get_declare_parameter(self, name, param_type, checker=None, error_text=None): + try: + self.declare_parameter(name, param_type) + except ParameterAlreadyDeclaredException: + pass + + if checker is None: + checker = lambda x: True + + if error_text is None: + error_text = f"wrong declare parameter {name}" + + value = self.get_parameter(name).value + if not checker(value): + self.get_logger().error(error_text) + raise DeclareParameterException(error_text) + return value + + def surbscribtion(self, topic_type, name, func, checker=None, error_text=None, count=1): + topic_name = self.get_declare_parameter(name, rclpy.Parameter.Type.STRING, checker=checker, error_text=error_text) + self.create_subscription(topic_type, topic_name, func, count) + + def surbscribtion_for_parameter(self, topic_type, name, param_name, func=None, checker=None, error_text=None, count=1, default=None): + """ + Устанавливает значение переменной param_name равной значению данных + По умолчанию если это Twist или Point, то записывает всё, если нет, то только из параметра data + """ + + setattr(self, param_name, default) + + if func is None: + if topic_type in [Twist, Point]: + func = lambda msg: msg + else: + func = lambda msg: msg.data + + topic_name = self.get_declare_parameter(name, rclpy.Parameter.Type.STRING, checker=checker, error_text=error_text) + self.create_subscription(topic_type, topic_name, self.__get_parameter_callback(param_name, func), count) + + def publisher_from_declare(self, topic_type, name, checker=None, error_text=None, count=1): + topic_name = self.get_declare_parameter(name, rclpy.Parameter.Type.STRING, checker=checker, error_text=error_text) + return self.create_publisher(topic_type, topic_name, count) + + def pub(self, topic, data_type, data: dict): + msg = data_type() + if type(data) != dict: + msg.data = data + else: + for key, value in data.items(): + setattr(msg, key, value) + topic.publish(msg) + + def __parameter_callback(self, msg, param_name, func): + setattr(self, param_name, func(msg)) + + def __get_parameter_callback(self, param_name, func): + def callback(msg): + self.__parameter_callback(msg, param_name, func) + return callback + + def create_update(self, update): + self.create_timer(1 / self.get_declare_parameter('hz', rclpy.Parameter.Type.INTEGER, lambda x: x > 0, "hz is less or equal zero"), update) diff --git a/ws_nodes_py/default/__pycache__/GetParameterNode.cpython-38.pyc b/ws_nodes_py/default/__pycache__/GetParameterNode.cpython-38.pyc new file mode 100644 index 0000000000000000000000000000000000000000..b2592483ba09497a047930c6362fc71be65f4b89 GIT binary patch literal 4083 zcma)9-H+SG5#L=dDT>xPI{WNg;?!l*I4M*2E-=tEMN-3U<6e^jf|0u-Es=oGaj84$ zsE;hE*eCFS#CL@aBq`A6HmHR@-J22Ah?CUrzaYI5ed|-6`_`9cW+_V6N7Amaquu51 z?9A-UZ)bnAv}6$|U+ny|_TLkP{0lpe77m>&P@^p%f(RNC7ynsEo6Kbt+ghkKxyx0X zhkDa+jb_0uP_jXUE{r20jKDn9+#>V~!i2sVFz8!?ZW5>X6>ugMho-#J76JS}eXT01 z%^(is&q5hg#r|u7A68``uHNv2PF!!d9G2=^*XvQ7>et%!R_r5-M+=3{6{t}ONI+aF zh|AP=EPZ0cZoJEwZN<@2)AL%ex#y*p=QZ1+8)CcUdGB_sVfN*iNHm%v|Ea9+wwu9v zBZ#ARYhAXZOWyTpeY;x^#rpNgYk^9U*V&(Hi=bBRhVlAF5RZjG-Pbz%(g4myl$-%E zsp`+Er^B}^ILQSvR&PZxp&e0I6K9m&smsM#WpV22;&D+F7JM~=f;jPKDoSGMh`XjZ zDIPl_Zc&uQGW=WOlsFClCzP>P)AFp06(2Xx3rcH3{RPzMJs`buLhg`+Cae$6@fK;V za1zr!OFl~p{(eVV?6)M*1lcp?OM(h^L=LostNyQqme4@chGnT6V%gqhKkKnP{DOixmSnbd@8u@W_*Z_?g#8?%nP zkkwas1bzW|jS9L7x{Pv{i_l0&gCs1b4c5>aJmHBh=*Rq^07tEZ6BXd-jlM#bO>)dC z$4s7uRq}Bljv-g!FV&?VcJ|ZawfsC%zJ+!$Hn-k_5nlUz>B)ukuU2RewcCv z*IIGAQ}>nKB%XW9Yu%PV*(25ccDEJh^U&6_R$Pj@QZe2}V6uUUayUX)Y47};ou(%H z80UFGL>Wg?4LUIx2h<1Pqy*xFLcCl`XiQ^PV|#{tK?OTG;4M>d2zzLOF*PtIZ|Du9 zQAqT}5IPvM2*xzQnD}1=j=ay3f-)+?uhCtDW3uc~8T@tl!SLqb?%)B`PX}KP?gPC$ zxHr6M4?e@e;jO_v_`g56XAk}X^t z$9I?lKqb3Oe}|w!VYpJd*Dsv4NzHH>->1AvlbI z%>$?mx2yi1W2qRFPvcq@d?OT7QT7auj$>VZ4|x|x_ojRnKTeCEo|_7-X_Mjn2!V|l zB|x$gV42AvJ}=J7y7nzhd^BcW5Y(`*fDBDL17vukA&?m(ATtuZufZuGu#SLhkqzDQ z2&_8Y?XVv02GS|aSZT&63vF?ByxgnFb~C@SegvCO!0GupOny5mR$PUj=5Ji$dM526q+I`&> zZ$XIs9A>LLkkHEv#G%h*? zeLl_=*c@w_)=GaAB@5py^U4tcT`5=hl?CeVEX0us9_D-$ z@7(MYi{puB+rEq~+}mCKCUm-rvM8sgW!7{4Z)to63-f|)yyI1%pkD$SKq?9p^4?gb zJPloFg2R(Y@AGqzH)Ga}9^@Dm`2?&;*=}zR-;u9MZ82d;fa4yA6|5pL;mTB88n=|A zp|T3$e~k@w>AX0n(+GRI9SEjiV%AAkyW6wtksSt6WVfXq;ETxa1+wiJZfUeQu0x8n z=#EGWuUEFNZd`rSVOj2FWcy^|j)hE@rlfcS#LZn|KAW1F3i`wQt&*9|(JQ0YBV#3r z$R*Ke5|1Sl^8&Wkk*M4661HAK@?#{Jq{^QnQ6f%=vM9bT(b5rG6jJ0<7PpoyezIt4 zrY`ZTb3);KYN;%B4WEM&kIyY<13~_zjnpe3UET@cIT_1+53+`64O0j7LUr|~#qmp3 cJp$AQi{ygGO!jiUtU@1!##^sM%UYR#4VFD3cmMzZ literal 0 HcmV?d00001 diff --git a/ws_nodes_py/legacy/regulator copy.py b/ws_nodes_py/legacy/regulator copy.py new file mode 100644 index 0000000..149987d --- /dev/null +++ b/ws_nodes_py/legacy/regulator copy.py @@ -0,0 +1,163 @@ +#!/usr/bin/env python3 + +import rclpy +from rclpy.node import Node +from std_msgs.msg import Bool +from geometry_msgs.msg import Twist +from geographic_msgs.msg import GeoPoint +from nonius_msgs.msg import Track + +from geographiclib.geodesic import Geodesic + +import math + + +def translate(value, leftMin, leftMax, rightMin, rightMax): + # Figure out how 'wide' each range is + leftSpan = leftMax - leftMin + rightSpan = rightMax - rightMin + + # Convert the left range into a 0-1 range (float) + valueScaled = float(value - leftMin) / float(leftSpan) + + # Convert the 0-1 range into a value in the right range. + return rightMin + (valueScaled * rightSpan) + +def scale_angle(value): + return translate(value, -180.0, 180.0, -1.0, 1.0) + +class Controller(Node): + def add_param(self, name, defvalue): + self.config[name] = self.declare_parameter(name, defvalue).value + + def parameter_callback(self, data): + for parameter in data: + if parameter.name.split('.')[1] == self.name: + self.config[parameter.name.split('.')[2]] = parameter.value + + def __init__(self): + super().__init__('regulator') + self.declare_parameter('hz', rclpy.Parameter.Type.INTEGER) + self.declare_parameter('distance_threshold', rclpy.Parameter.Type.DOUBLE) + #self.declare_parameter('normal_speed', rclpy.Parameter.Type.DOUBLE) + self.declare_parameter('min_speed_for_cog', rclpy.Parameter.Type.DOUBLE) + self.declare_parameter('k_p_angular', rclpy.Parameter.Type.DOUBLE) + self.declare_parameter('max_thrust', rclpy.Parameter.Type.DOUBLE) + #self.declare_parameter('k_p_linear', rclpy.Parameter.Type.DOUBLE) + #self.declare_parameter('linear_regulator_threshold', rclpy.Parameter.Type.DOUBLE) + + self.publisher = self.create_publisher(Twist, 'twist_command', 10) + self.update_timer = self.create_timer(1 / self.get_parameter('hz').value, self.update) + + self.create_subscription(GeoPoint, 'gga', self.set_position, 1) + self.create_subscription(Twist, 'rmca', self.set_state, 1) + self.create_subscription(Track, 'target_position', self.set_target, 1) + self.create_subscription(Bool, 'stop_moving', self.stop, 1) + + self.stopped = True + self.state = None + self.target = None + self.position = None + self.origin = None + + self.path = [] + self.current_target = 0 + + self.get_logger().info('Auto teleoperation started') + + def set_position(self, position): + self.position = position + + def set_state(self, state): + self.state = state + + def stop(self, stop): + self.get_logger().info('Stop regulation') + self.stopped = True + self.publisher.publish(Twist()) + + def can_move(self): + return not self.stopped and self.state is not None and len(self.path) > 0 and self.position is not None + + def error_track(self, position): + vec1 = (self.target.lantitude - self.origin.latitude, self.target.longitude - self.origin.longitude) + vec2 = (position.lantitude - self.origin.latitude, position.longitude - self.origin.longitude) + return (vec1[0]*vec2[1] - vec1[1]*vec2[0]) + + def set_target(self, track): + if (track.mode == "point" or track.mode == "path") and len(track.points) > 0: + self.path = track.points + self.current_target = 0 + self.origin = self.position + self.stopped = False + self.get_logger().info(f'Set new target of {len(self.path)} points') + + @staticmethod + def get_distance_and_heading(position, target): + geod = Geodesic.WGS84 + res = geod.Inverse(position.latitude, position.longitude, target.latitude, target.longitude) + + result = Twist() + result.linear.x = res['s12'] + result.angular.z = res['azi2'] + return result + + @staticmethod + def angle_error(target_angle, source_angle): + result = target_angle - source_angle + if result > 180: + result = result - 360 + return result + + def move_to_point(self, position, target, state, max_thrust, min_speed, distance_threshold, k_p_angular): + error = Controller.get_distance_and_heading(position, target) + heading = state.angular.z + if heading > 180: + heading = heading - 360 + self.get_logger().info(f'Distance {error.linear.x}, heading {heading}, bearing {error.angular.z}') + if error.linear.x < distance_threshold: + return None + + result = Twist() + if state.linear.x > min_speed: + angle_error = Controller.angle_error(error.angular.z, heading) + value = scale_angle(angle_error) + value = k_p_angular * value + self.get_logger().info(f'Angle_error {angle_error}, scaled angle {value}') + result.angular.z = math.copysign(min(abs(max_thrust), abs(value)), value) + result.linear.x = (max_thrust - abs(result.angular.z)) + else: + self.get_logger().info('Speed too low, move straight') + result.linear.x = max_thrust + return result + + def update(self): + if not self.can_move(): + return + result = self.move_to_point(self.position, self.path[self.current_target], self.state, + self.get_parameter('max_thrust').value, + self.get_parameter('min_speed_for_cog').value, + self.get_parameter('distance_threshold').value, + self.get_parameter('k_p_angular').value) + + if result is None: + if self.current_target < len(self.path)-1: + self.current_target = self.current_target + 1 + self.get_logger().info(f'Point {self.current_target}/{len(self.path)} was achieved') + else: + self.path = [] + self.get_logger().info(f'Path finished') + self.stopped = True + else: + self.publisher.publish(result) + + +def main(): + rclpy.init() + controller = Controller() + rclpy.spin(controller) + controller.destroy_node() + rclpy.shutdown() + +if __name__ == '__main__': + main() \ No newline at end of file diff --git a/ws_nodes_py/legacy/to_berlin.py b/ws_nodes_py/legacy/to_berlin.py new file mode 100644 index 0000000..b662d2b --- /dev/null +++ b/ws_nodes_py/legacy/to_berlin.py @@ -0,0 +1,50 @@ +#!/usr/bin/python + +import rclpy +from rclpy.node import Node +from time import time +from std_msgs.msg import Float64 +from geometry_msgs.msg import Twist + + +class ToBerlin(Node): + def __init__(self): + super().__init__('to_berlin') + self.last_time_joy = 0 + self.delta_time = 2.0 + + self.publisher = self.create_publisher(Twist, 'twist_robot', 10) + self.sub = self.create_subscription(Float64, "camera_front/compressed/z", self.update_front, 10) + self.sub = self.create_subscription(Float64, "camera_side/compressed/z", self.update_side, 10) + self.get_logger().info(f"Twist robot Ready") + + def update_front(self, num): + self.last_time_joy = time() + msg = Twist() + + msg.linear.x = 0.1 + msg.angular.z = 0.3 * num.data + + self.publisher.publish(msg) + + def update_side(self, num): + if self.last_time_joy + self.delta_time < time(): + msg = Twist() + + msg.linear.x = 0.075 + msg.angular.z = -num.data + + self.publisher.publish(msg) + + + +def main(): + rclpy.init() + to_berlin = ToBerlin() + rclpy.spin(to_berlin) + to_berlin.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() \ No newline at end of file diff --git a/ws_nodes_py/regulator.py b/ws_nodes_py/regulator.py index 865b5e8..d6f91c8 100644 --- a/ws_nodes_py/regulator.py +++ b/ws_nodes_py/regulator.py @@ -1,155 +1,118 @@ #!/usr/bin/env python3 import rclpy -from rclpy.node import Node -from std_msgs.msg import Bool -from geometry_msgs.msg import Twist -from geographic_msgs.msg import GeoPoint -from nonius_msgs.msg import Track - -from geographiclib.geodesic import Geodesic - import math - +from ws_nodes_py.default.GetParameterNode import GetParameterNode +from std_msgs.msg import Bool, Int16, String +from geometry_msgs.msg import Twist, Point +from math import acos, degrees +# from nonius_msgs.msg import Track -def translate(value, leftMin, leftMax, rightMin, rightMax): - # Figure out how 'wide' each range is - leftSpan = leftMax - leftMin - rightSpan = rightMax - rightMin +import math - # Convert the left range into a 0-1 range (float) - valueScaled = float(value - leftMin) / float(leftSpan) - # Convert the 0-1 range into a value in the right range. - return rightMin + (valueScaled * rightSpan) +class Controller(GetParameterNode): + def __init__(self): + super().__init__('regulator') + self.distance_threshold = self.get_declare_parameter('distance_threshold', rclpy.Parameter.Type.DOUBLE, checker=lambda x: True) + self.k_p_angular = self.get_declare_parameter('k_p_angular', rclpy.Parameter.Type.DOUBLE, checker=lambda x: True) + self.max_thrust = self.get_declare_parameter('max_thrust', rclpy.Parameter.Type.DOUBLE, checker=lambda x: True) + # self.normal_speed = self.get_declare_parameter('normal_speed', rclpy.Parameter.Type.DOUBLE, checker=lambda x: True) + # self.k_p_linear = self.get_declare_parameter('k_p_linear', rclpy.Parameter.Type.DOUBLE, checker=lambda x: True) + # self.linear_regulator_threshold = self.get_declare_parameter('linear_regulator_threshold', rclpy.Parameter.Type.DOUBLE, checker=lambda x: True) -def scale_angle(value): - return translate(value, -180.0, 180.0, -1.0, 1.0) + self.surbscribtion_for_parameter(Int16, "heading_topic", 'heading', checker=lambda x: x, func=lambda msg: msg.data, default=0) + self.surbscribtion_for_parameter(Point, "position_topic", 'position', checker=lambda x: x, func=lambda msg: msg, default=Point()) + self.surbscribtion_for_parameter(Point, "goal_topic", 'goal', checker=lambda x: x, func=lambda msg: msg, default=Point()) + self.surbscribtion_for_parameter(String, "move_type_topic", 'type_move', checker=lambda x: x, func=lambda msg: msg.data, default="STOP") -class Controller(Node): - def add_param(self, name, defvalue): - self.config[name] = self.declare_parameter(name, defvalue).value + self.pub_twist = self.publisher_from_declare(Twist, "twist_robot_topic", checker=lambda x: x) - def parameter_callback(self, data): - for parameter in data: - if parameter.name.split('.')[1] == self.name: - self.config[parameter.name.split('.')[2]] = parameter.value - - def __init__(self): - super().__init__('regulator') - self.declare_parameter('hz', rclpy.Parameter.Type.INTEGER) - self.declare_parameter('distance_threshold', rclpy.Parameter.Type.DOUBLE) - #self.declare_parameter('normal_speed', rclpy.Parameter.Type.DOUBLE) - self.declare_parameter('min_speed_for_cog', rclpy.Parameter.Type.DOUBLE) - self.declare_parameter('k_p_angular', rclpy.Parameter.Type.DOUBLE) - self.declare_parameter('max_thrust', rclpy.Parameter.Type.DOUBLE) - #self.declare_parameter('k_p_linear', rclpy.Parameter.Type.DOUBLE) - #self.declare_parameter('linear_regulator_threshold', rclpy.Parameter.Type.DOUBLE) - - self.publisher = self.create_publisher(Twist, 'twist_command', 10) - self.update_timer = self.create_timer(1 / self.get_parameter('hz').value, self.update) - - self.create_subscription(GeoPoint, 'gga', self.set_position, 1) - self.create_subscription(Twist, 'rmca', self.set_state, 1) - self.create_subscription(Track, 'target_position', self.set_target, 1) - self.create_subscription(Bool, 'stop_moving', self.stop, 1) - - self.stopped = True - self.state = None - self.target = None - self.position = None - self.origin = None - - self.path = [] - self.current_target = 0 - + self.create_update(self.update) self.get_logger().info('Auto teleoperation started') - - def set_position(self, position): - self.position = position - - def set_state(self, state): - self.state = state - - def stop(self, stop): - self.get_logger().info('Stop regulation') - self.stopped = True - self.publisher.publish(Twist()) - - def can_move(self): - return not self.stopped and self.state is not None and len(self.path) > 0 and self.position is not None - - def error_track(self, position): - vec1 = (self.target.lantitude - self.origin.latitude, self.target.longitude - self.origin.longitude) - vec2 = (position.lantitude - self.origin.latitude, position.longitude - self.origin.longitude) - return (vec1[0]*vec2[1] - vec1[1]*vec2[0]) - - def set_target(self, track): - if (track.mode == "point" or track.mode == "path") and len(track.points) > 0: - self.path = track.points - self.current_target = 0 - self.origin = self.position - self.stopped = False - self.get_logger().info(f'Set new target of {len(self.path)} points') - - @staticmethod - def get_distance_and_heading(position, target): - geod = Geodesic.WGS84 - res = geod.Inverse(position.latitude, position.longitude, target.latitude, target.longitude) - result = Twist() - result.linear.x = res['s12'] - result.angular.z = res['azi2'] - return result - - @staticmethod - def angle_error(target_angle, source_angle): - result = target_angle - source_angle + def angle_point_error(self, point, goal): + x1, y1 = point.x, point.y + x2, y2 = goal.x, goal.y + dx, dy = x2 - x1, y2 - y1 + ln = (dx**2 + dy**2)**0.5 + if ln: + dxn = dx/ln + dyn = dy/ln + ang = degrees(acos(dxn)) + if dyn < 0: + ang = 360 - ang + return ang + return 0 + + def angle_error(self,target_angle, source_angle): + result = (source_angle - target_angle) % 360 if result > 180: - result = result - 360 + result -= 360 return result - def move_to_point(self, position, target, state, max_thrust, min_speed, distance_threshold, k_p_angular): - error = Controller.get_distance_and_heading(position, target) - heading = state.angular.z - if heading > 180: - heading = heading - 360 - self.get_logger().info(f'Distance {error.linear.x}, heading {heading}, bearing {error.angular.z}') - if error.linear.x < distance_threshold: - return None + def move_to_point(self): + position, goal, heading, max_thrust, distance_threshold, k_p_angular = self.position, self.goal, self.heading, self.max_thrust, self.distance_threshold, self.k_p_angular + error_dist = ((goal.x - position.x)**2 + (goal.y - position.y)**2)**0.5 + err_angle = self.angle_error(self.angle_point_error(position, goal), heading) result = Twist() - if state.linear.x > min_speed: - angle_error = Controller.angle_error(error.angular.z, heading) - value = scale_angle(angle_error) - value = k_p_angular * value - self.get_logger().info(f'Angle_error {angle_error}, scaled angle {value}') + if self.type_move == "MOVE TO": + value = k_p_angular * err_angle result.angular.z = math.copysign(min(abs(max_thrust), abs(value)), value) - result.linear.x = (max_thrust - abs(result.angular.z)) - else: - self.get_logger().info('Speed too low, move straight') - result.linear.x = max_thrust + if abs(err_angle) < 10: + result.linear.x = max_thrust + else: result.linear.x = 0.0 + elif self.type_move == "MOVE CIRCLE": + point = [goal.x, goal.y] + slf = [position.x, position.y] + course = heading # -45 + target_dist = goal.z + + # settings + is_clockwise = True + + zero = [0, 0] + + eval_dist = lambda p1, p2: ((p1[0] - p2[0])**2 + (p1[1] - p2[1])**2)**0.5 + relate = [slf[0] - point[0], slf[1] - point[1]] + ln = eval_dist(relate, zero) + + if ln > 0: + norm = [relate[0] / ln, relate[1] / ln] + else: + norm = [1, 0] + + ang = degrees(acos(norm[0])) + if norm[1] < 0: + ang = 360 - ang + ang = ang + (+90 if is_clockwise else -90) + + dist = eval_dist(point, slf) + delta_dist = dist - target_dist + + angle_by_angle = course - ang + angle_by_dist = delta_dist/target_dist * 90 + + result_angle = (angle_by_angle - angle_by_dist) *0.02 + + self.get_logger().info(f"{dist=} {relate=} {ln=} {norm=}") + self.get_logger().info(f"{round(ang)} {round(delta_dist, 2)}") + self.get_logger().info(f"{round(angle_by_angle)} {round(angle_by_dist)}") + result.angular.z = math.copysign(min(abs(max_thrust*2), abs(result_angle)), result_angle) + result.linear.x = 0.075 + + elif self.type_move == "STOP": + pass return result def update(self): - if not self.can_move(): - return - result = self.move_to_point(self.position, self.path[self.current_target], self.state, - self.get_parameter('max_thrust').value, - self.get_parameter('min_speed_for_cog').value, - self.get_parameter('distance_threshold').value, - self.get_parameter('k_p_angular').value) + result = self.move_to_point() - if result is None: - if self.current_target < len(self.path)-1: - self.current_target = self.current_target + 1 - self.get_logger().info(f'Point {self.current_target}/{len(self.path)} was achieved') - else: - self.path = [] - self.get_logger().info(f'Path finished') - self.stopped = True - else: - self.publisher.publish(result) + if result is not None: + # if abs(result.angular.z) < 0.6 and abs(result.linear.x) < 0.15: + self.pub_twist.publish(result) + def main(): rclpy.init() diff --git a/ws_nodes_py/settings/__pycache__/for_world.cpython-38.pyc b/ws_nodes_py/settings/__pycache__/for_world.cpython-38.pyc new file mode 100644 index 0000000000000000000000000000000000000000..e9aa731229ecf8acd81ed1298659f594b07f3b8a GIT binary patch literal 896 zcmZ9I&x_MQ6vwA&n;&hnb$4~ui$^gR1A0}nNAciALK?bY?(B)6lHWSw+1HibS(Ew!(RQSb=(|bOI(=V1ok446Twmx?FAZ&HgxVs;96VXj+=m-k4)UaNNSnf>@>x8hUPn5j~*sGtH z?mvF{I+!c(1BO@BJ_f9(?Q_5m^6QOv&%OqpBWyKS4M$8W3KtQRU4;V$+d`xYZxG3+ z>2&HDqJlZ>s`Ubhg_r(^<%M5g)BD~yPg->d`Jb4WKMC5 zGGSSqmTj)$dBMd@FSl1hoUyss&ggJGY9*|qF06Hr3Z5rj8RfBv`?L`9IT1VRfeL?b z`g^=j{Rgy=Jo5#YSE8ZxJA)(ze<-7j1I5(Q+F8<~NHQk<9!{%!UCu|PeXY-7kkT6l m)|IyqX61zcF&nl~s~eU7{Fm7_4CB~z*7V<|t5r*@$NvD7EDCo3 literal 0 HcmV?d00001 diff --git a/ws_nodes_py/settings/for_world.py b/ws_nodes_py/settings/for_world.py new file mode 100644 index 0000000..ae8d06f --- /dev/null +++ b/ws_nodes_py/settings/for_world.py @@ -0,0 +1,61 @@ +world_markers = { + "start_point": [0, 0], + "yellow_gate": [6, 0], + "blue_gate": [11, 0], + "blue_bou": [10, 0], + "black_circle": [10, 10], + "red_point": [10, 10], #???? + "trash": [0, 10], +} + +start_position = [0, 0] + +zero_speed_signal = 1500 + +# Посчитанно Кириллом и Андреем в опытовом бассейне +old_motor_speed = { +# signal: speed (in m/s) + 1500: 0, + 1540: 0, + 1537: 0.10, + 1574: 0.17, + 1600: 0.40, + 1700: 0.73, + 1800: 0.95, + 1900: 1.04, + 2000: 1.04, +} + +# Посчитанно по компасу +old_motor_ang_speed = { + 0: 0, + # 0.15: -10, + 0.3: -20, +} + +# Посчитанно Кириллом и Андреем в опытовом бассейне +new_motor_speed = { +# signal: speed (in m/s) + 1500: 0, + 1540: 0, + 1600: 0.50, + 1700: 0.97, + 1800: 1.15, + 1900: 1.29, + 2000: 1.29, +} + +# Если бы мы знали, но мы не знаем +new_motor_ang_speed = { + 0: 0, + # 0.15: -10, + 0.3: 0, +} + +world_markers_topics_parametrs = { + "gate_position": "yellow_gate", + "blue_position": "blue_bou", + "blue_gate_position": "blue_gate", + # "black_position": "black_circle", + # "red_position": "red_point", +} \ No newline at end of file diff --git a/ws_nodes_py/twist_controller.py b/ws_nodes_py/twist_controller.py new file mode 100644 index 0000000..b0721b8 --- /dev/null +++ b/ws_nodes_py/twist_controller.py @@ -0,0 +1,45 @@ +#!/usr/bin/python + +import rclpy +from ws_nodes_py.default.GetParameterNode import GetParameterNode +from time import time +from geometry_msgs.msg import Twist + + +class TwistController(GetParameterNode): + def __init__(self): + super().__init__('twist_controller') + + # params + self.last_time_joy = 0 + self.delta_time = self.get_declare_parameter('delta_time', rclpy.Parameter.Type.DOUBLE, + checker=lambda x: x > 0, error_text="delta_time not more than zero") + + # subscribtions + self.surbscribtion(Twist, "twist_joy_topic", self.joy_callback, checker=lambda x: x) + self.surbscribtion(Twist, "twist_robot_topic", self.robot_callback, checker=lambda x: x) + + # publisher + self.pub_twist = self.publisher_from_declare(Twist, "twist_topic", checker=lambda x: x) + + self.get_logger().info('Twist controller started') + + def joy_callback(self, msg): + self.last_time_joy = time() + self.pub_twist.publish(msg) + + def robot_callback(self, msg): + if self.last_time_joy + self.delta_time < time(): + self.pub_twist.publish(msg) + + +def main(): + rclpy.init() + twist_controller = TwistController() + rclpy.spin(twist_controller) + twist_controller.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() \ No newline at end of file diff --git a/ws_nodes_py/video.py b/ws_nodes_py/video.py deleted file mode 100644 index e4e41ba..0000000 --- a/ws_nodes_py/video.py +++ /dev/null @@ -1,33 +0,0 @@ -#!/usr/bin/python3 -import rclpy -import subprocess -from rclpy.node import Node -from std_msgs.msg import Bool -from datetime import datetime - -class ImagePublisher(Node): - def __init__(self): - super().__init__("image_recorder") - self.stop_video = self.create_subscription(Bool, "rec", self.control_video, 1) - self.enable = False - - def control_video(self, msg): - self.enable = not self.enable - if self.enable: - subprocess.run(["sudo", "systemctl", "start", "vid.service"]) - # subprocess.run(["ffmpeg", "-f", "v4l2", "-i", "/dev/video0", f"{datetime.now().strftime('%m-%d-%Y_%H:%M:%S')}.mp4", "&"]) - else: - subprocess.run(["sudo", "systemctl", "stop", "vid.service"]) - - - -def main(args=None): - rclpy.init(args=args) - - ip = ImagePublisher() - rclpy.spin(ip) - ip.destroy_node() - rclpy.shutdown() - -if __name__ == '__main__': - main() diff --git a/ws_nodes_py/videoD.py b/ws_nodes_py/videoD.py new file mode 100644 index 0000000..14ed931 --- /dev/null +++ b/ws_nodes_py/videoD.py @@ -0,0 +1,164 @@ +#! /usr/bin/env python3 + +import rclpy +from ws_nodes_py.default.GetParameterNode import GetParameterNode +from sensor_msgs.msg import CompressedImage +from std_msgs.msg import Float64 +import cv2 +from cv_bridge import CvBridge +import numpy as np +from math import radians, cos, sin + + +class RealsenseToBinary(GetParameterNode): + def __init__(self): + super().__init__('RealOpenCV') + + # Параметры для выделения маски BGR + self.hsv_min = (90, 0, 0) + self.hsv_max = (180, 255, 255) + self.min_countur_size = 300 + self.get_logger().info(f"HSV \n{self.hsv_min} \n {self.hsv_max}") + + # Маска части изображения, которе обрабатываем + x1,y1 = 0, 170 + x2, y2 = 510, 280 + self.roi_corners = np.array([[(x1, y1), (x2, y1), (x2, y2), (x1, y2),]], dtype=np.int32) + + # Параметры для рассчёта расстояние и угла + # см. ~/Desktop/python/calc.py + self.k1 = self.get_declare_parameter('k1', rclpy.Parameter.Type.DOUBLE, checker=lambda x: True) + self.k2 = self.get_declare_parameter('k2', rclpy.Parameter.Type.DOUBLE, checker=lambda x: True) + self.fov = self.get_declare_parameter('fov', rclpy.Parameter.Type.INTEGER, checker=lambda x: True) + # Пареметр апроксимации контура + self.k = self.get_declare_parameter('k', rclpy.Parameter.Type.DOUBLE, checker=lambda x: True) + + topic_name = self.get_declare_parameter('topic_camera', rclpy.Parameter.Type.STRING, checker=lambda x: x, error_text="topic name must be string") + # Топик с отладочным изображением + self.pub = self.create_publisher(CompressedImage, f"cv/{topic_name}" , 10) + self.surbscribtion(CompressedImage, "topic_camera", self.img_callback, checker=lambda x: x) + + self.br = CvBridge() + self.get_logger().info(f"CV Start {topic_name}") + + def img_callback(self, data): + # Получение кадра + current_frame = self.br.compressed_imgmsg_to_cv2(data) + success, frame = True, current_frame + frame = cv2.rotate(frame, cv2.ROTATE_90_COUNTERCLOCKWISE) + if success: + channel_count = frame.shape[2] + + # mask = np.zeros(frame.shape, dtype=np.uint8) + # ignore_mask_color = (255,)*channel_count + # cv2.fillPoly(mask, self.roi_corners, ignore_mask_color) + # masked_image = cv2.bitwise_and(frame, mask) + masked_image = frame + + + height, width = frame.shape[0:2] + pix_per_angle = width / self.fov + # Нулевые параметры управления + med_x = width / 2 + controlX = 0.0 + dist_to_gate = 6 + course_to_gate = 0 + + # Создание HSV маски + hsv = cv2.cvtColor(masked_image, cv2.COLOR_RGB2HSV) + # blue = cv2.inRange(hsv, (0, 76, 165), (28, 255, 255)) # -> выделяем синий + yellow = cv2.inRange(hsv, self.hsv_min, self.hsv_max) # -> выделяем желтый + + # both = blue + yellow + both = yellow + binary = cv2.bitwise_and(both, both, mask=both) + binary = cv2.morphologyEx(binary, cv2.MORPH_CLOSE, np.ones((5,5),np.uint8)) + + + # Выделение самых больших контуров удовлетворяющих условию размера + contours, _ = cv2.findContours(binary, cv2.RETR_EXTERNAL, + cv2.CHAIN_APPROX_NONE) + contours = list(filter(lambda x: cv2.moments(x)["m00"] > self.min_countur_size, sorted(contours, key=cv2.contourArea, reverse=True)[:2])) + approxs = list(sorted(contours, key=lambda x: len(cv2.approxPolyDP( + x, self.k * cv2.arcLength(x, True), True)), reverse=True))[:1] + + if approxs: + # Для выделение левого и правого шара + aligns = [] + # Для выделение ближайшего и дальнего шара + distances = [] + + for contour in approxs: + contour = cv2.approxPolyDP(contour, self.k * cv2.arcLength(contour, True), True) + moments = cv2.moments(contour) + # # Получение хз чего + # area = cv2.contourArea(contour) + # Координаты центра контура + if moments["m00"]: + cx = int(moments["m10"] / moments["m00"]) + cy = int(moments["m01"] / moments["m00"]) + else: + continue + + # Вычисление дистанции + x, y, w, h = cv2.boundingRect(contour) + dist = (w*(self.k1)+self.k2)*0.01 + + + # Запись центра контура по горизонту и расчётной дистанции до него + aligns.append(cx) + distances.append(dist) + + if True: # Вывод информации о контуре + # Координаты центра + # cv2.putText(frame, f'cx: {cx}, cy: {cy}', (cx + 10, cy - 10), cv2.FONT_HERSHEY_SIMPLEX, 0.75, (255, 255, 255), 2, cv2.LINE_AA) + # Выделение контура + cv2.drawContours(frame, [contour], -1, (0, 255, 0), 1) + # Выделение прямоугольника + # cv2.rectangle(frame, (x, y), (x+w, y+h), (0, 255, 0), 2) + # Оси через центр контура + # cv2.line(frame, (cx, 0), (cx, height), (0, 255, 0), 1) + # cv2.line(frame, (0, cy), (width, cy), (0, 255, 0), 1) + # Вывести ширину контура + cv2.putText(frame, f'W{round(w,2)}', (x, y+h+25), cv2.FONT_HERSHEY_SIMPLEX, 0.75, (0, 0, 255), 2) + # Вывести расчётную дистанцию до контура + cv2.putText(frame, f'D{round(dist,2)}', (x, y), cv2.FONT_HERSHEY_SIMPLEX, 0.75, (0, 0, 255), 2) + + # Расчёт курса и дистанции до ворот + if aligns: + med_x = sum(aligns) / len(aligns) + controlX = round(-2 * (med_x - width / 2) / width, 1) + controlX = max(-0.5, min(0.5,controlX)) + course_to_gate = round(controlX * self.fov / 2) + if distances: + dist_to_gate = round(sum(distances) / len(distances), 1) + + # Создание отладочного изображения + miniBin = cv2.resize(binary, (int(binary.shape[1] / 4), int(binary.shape[0] / 4)), + interpolation=cv2.INTER_AREA) + miniBin = cv2.cvtColor(miniBin, cv2.COLOR_GRAY2RGB) + frame[-2 - miniBin.shape[0]:-2, 2:2 + miniBin.shape[1]] = miniBin + + cv2.putText(frame, 'iSee: {};'.format(len(contours)), (width - 120, height - 5), + cv2.FONT_HERSHEY_SIMPLEX, 0.75, (255, 255, 255), 1, cv2.LINE_AA) + cv2.putText(frame, f'x: {round(dist_to_gate * cos(radians(course_to_gate)), 1)} y: {round(dist_to_gate * sin(radians(course_to_gate)), 1)}', (0, 25), + cv2.FONT_HERSHEY_SIMPLEX, 0.75, (255, 255, 255), 1, cv2.LINE_AA) + # cv2.putText(frame, f'{course_to_gate = }', (0, 50), + # cv2.FONT_HERSHEY_SIMPLEX, 0.75, (255, 255, 255), 1, cv2.LINE_AA) + cv2.putText(frame, f'{controlX = }', (0, 75), + cv2.FONT_HERSHEY_SIMPLEX, 0.75, (255, 255, 255), 1, cv2.LINE_AA) + + # Отладочное изображение + self.pub.publish(self.br.cv2_to_compressed_imgmsg(frame)) + + +def main(args=None): + rclpy.init(args=args) + RealOpenCV = RealsenseToBinary() + rclpy.spin(RealOpenCV) + RealOpenCV.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() diff --git a/ws_nodes_py/videoF.py b/ws_nodes_py/videoF.py new file mode 100644 index 0000000..e72e59c --- /dev/null +++ b/ws_nodes_py/videoF.py @@ -0,0 +1,198 @@ +#! /usr/bin/env python3 + +import rclpy +from ws_nodes_py.default.GetParameterNode import GetParameterNode +from sensor_msgs.msg import CompressedImage +from std_msgs.msg import Float64, Int16 +from geometry_msgs.msg import Point +import cv2 +from cv_bridge import CvBridge +import numpy as np +from math import radians, cos, sin + + +class RealsenseToBinary(GetParameterNode): + def __init__(self): + super().__init__('RealOpenCV') + + # Параметры для выделения маски + # yellow = cv2.inRange(hsv, (60, 22, 164), (83, 255, 255)) # -> выделяем желтый + self.hsv_min = (60, 22, 164) + self.hsv_max = (83, 255, 255) + self.min_countur_size = 600 + self.get_logger().info(f"HSV \n{self.hsv_min} \n {self.hsv_max}") + + # Маска части изображения, которе обрабатываем + x1,y1 = 0, 170 + x2, y2 = 510, 280 + self.roi_corners = np.array([[(x1, y1), (x2, y1), (x2, y2), (x1, y2),]], dtype=np.int32) + + # Параметры для рассчёта расстояние и угла + self.surbscribtion_for_parameter(Int16, "heading_topic", 'heading', checker=lambda x: x, func=lambda msg: msg.data, default=0) + # см. ~/Desktop/python/calc.py + self.k1 = self.get_declare_parameter('k1', rclpy.Parameter.Type.DOUBLE, checker=lambda x: True) + self.k2 = self.get_declare_parameter('k2', rclpy.Parameter.Type.DOUBLE, checker=lambda x: True) + self.fov = self.get_declare_parameter('fov', rclpy.Parameter.Type.INTEGER, checker=lambda x: True) + # Пареметр апроксимации контура + self.k = self.get_declare_parameter('k', rclpy.Parameter.Type.DOUBLE, checker=lambda x: True) + + topic_name = self.get_declare_parameter('topic_camera', rclpy.Parameter.Type.STRING, checker=lambda x: x, error_text="topic name must be string") + # Топик с отладочным изображением + self.pub = self.create_publisher(CompressedImage, f"cv/{topic_name}" , 10) + self.surbscribtion(CompressedImage, "topic_camera", self.img_callback, checker=lambda x: x) + + self.pub_yellow_pos = self.publisher_from_declare(Point, "gate_position_topic", checker=lambda x: x) + self.pub_blue_pos = self.publisher_from_declare(Point, "blue_gate_position_topic", checker=lambda x: x) + + self.br = CvBridge() + self.get_logger().info(f"CV Start {topic_name}") + + def img_callback(self, data): + # Получение кадра + current_frame = self.br.compressed_imgmsg_to_cv2(data) + success, frame = True, current_frame + if success: + channel_count = frame.shape[2] + + # mask = np.zeros(frame.shape, dtype=np.uint8) + # ignore_mask_color = (255,)*channel_count + # cv2.fillPoly(mask, self.roi_corners, ignore_mask_color) + # masked_image = cv2.bitwise_and(frame, mask) + masked_image = frame + height, width = frame.shape[0:2] + + # Создание HSV маски + hsv = cv2.cvtColor(masked_image, cv2.COLOR_RGB2HSV) + # blue = cv2.inRange(hsv, (0, 76, 165), (28, 255, 255)) # -> выделяем синий + yellow = cv2.inRange(hsv, (43, 57, 203), (86, 255, 242)) # -> выделяем желтый + blue = cv2.inRange(hsv, (0, 180, 190), (24, 255, 255)) # -> выделяем синий + + both = blue + yellow + # both = yellow + yellow_binary = cv2.bitwise_and(yellow, yellow, mask=yellow) + blue_binary = cv2.bitwise_and(blue, blue, mask=blue) + binary = cv2.bitwise_and(both, both, mask=both) + + # Выделение самых больших контуров удовлетворяющих условию размера + res = self.get_gate(yellow_binary, height, width, frame) + if res is not None: + course_to_gate, dist_to_gate = res + x = round(dist_to_gate * cos(radians(self.heading - course_to_gate)), 1) + y = round(dist_to_gate * sin(radians(self.heading - course_to_gate)), 1) + # Управляющий сигнал + if 6 >= dist_to_gate >= 1.5: + msg = Point() + msg.x = x + msg.y = y + self.pub_yellow_pos.publish(msg) + + res = self.get_gate(blue_binary, height, width, frame) + if res is not None: + course_to_gate, dist_to_gate = res + x = round(dist_to_gate * cos(radians(self.heading - course_to_gate)), 1) + y = round(dist_to_gate * sin(radians(self.heading - course_to_gate)), 1) + # Управляющий сигнал + if 6 >= dist_to_gate >= 1.5: + msg = Point() + msg.x = x + msg.y = y + self.pub_blue_pos.publish(msg) + + # Создание отладочного изображения + miniBin = cv2.resize(binary, (int(binary.shape[1] / 4), int(binary.shape[0] / 4)), + interpolation=cv2.INTER_AREA) + miniBin = cv2.cvtColor(miniBin, cv2.COLOR_GRAY2RGB) + frame[-2 - miniBin.shape[0]:-2, 2:2 + miniBin.shape[1]] = miniBin + + # cv2.putText(frame, 'iSee: {};'.format(len(contours)), (width - 120, height - 5), + # cv2.FONT_HERSHEY_SIMPLEX, 0.75, (255, 255, 255), 1, cv2.LINE_AA) + # cv2.putText(frame, f'x: {x} y: {y}', (0, 25), + # cv2.FONT_HERSHEY_SIMPLEX, 0.75, (255, 255, 255), 1, cv2.LINE_AA) + # cv2.putText(frame, f'course: {self.heading - course_to_gate} dist {dist_to_gate}', (0, 50), + # cv2.FONT_HERSHEY_SIMPLEX, 0.75, (255, 255, 255), 1, cv2.LINE_AA) + # cv2.putText(frame, f'{controlX = }', (0, 75), + # cv2.FONT_HERSHEY_SIMPLEX, 0.75, (255, 255, 255), 1, cv2.LINE_AA) + + # Отладочное изображение + self.pub.publish(self.br.cv2_to_compressed_imgmsg(frame)) + + def get_gate(self, binary, height, width, frame): + pix_per_angle = width / self.fov + # Нулевые параметры управления + med_x = width / 2 + controlX = 0.0 + dist_to_gate = 0 + course_to_gate = 0 + + + contours, _ = cv2.findContours(binary, cv2.RETR_EXTERNAL, + cv2.CHAIN_APPROX_NONE) + contours = list(filter(lambda x: cv2.moments(x)["m00"] > self.min_countur_size, sorted(contours, key=cv2.contourArea, reverse=True)[:3])) + approxs = list(sorted(contours, key=lambda x: len(cv2.approxPolyDP( + x, 0.01 * cv2.arcLength(x, True), True)), reverse=True))[:2] + + if len(approxs) == 2: + # Для выделение левого и правого шара + aligns = [] + # Для выделение ближайшего и дальнего шара + distances = [] + for contour in approxs: + # Точки контура не приближаются к границе ближе чем на 10 пикселей + # self.get_logger().info(f"{contour}") + # 1/0 + if any((not 10 < xy[0][0] < width - 10 or not 10 < xy[0][1] < height - 10) for xy in contour): + # self.get_logger().info("collide border") + continue + moments = cv2.moments(contour) + # # Получение хз чего + # area = cv2.contourArea(contour) + # Координаты центра контура + cx = int(moments["m10"] / moments["m00"]) + cy = int(moments["m01"] / moments["m00"]) + + # Вычисление дистанции + x, y, w, h = cv2.boundingRect(contour) + dist = (w*(self.k1)+self.k2)*0.01 + + + # Запись центра контура по горизонту и расчётной дистанции до него + aligns.append(cx) + distances.append(dist) + + if True: # Вывод информации о контуре + # Координаты центра + # cv2.putText(frame, f'cx: {cx}, cy: {cy}', (cx + 10, cy - 10), cv2.FONT_HERSHEY_SIMPLEX, 0.75, (255, 255, 255), 2, cv2.LINE_AA) + # Выделение контура + cv2.drawContours(frame, contour, -1, (0, 255, 0), 1) + # Выделение прямоугольника + # cv2.rectangle(frame, (x, y), (x+w, y+h), (0, 255, 0), 2) + # Оси через центр контура + # cv2.line(frame, (cx, 0), (cx, height), (0, 255, 0), 1) + # cv2.line(frame, (0, cy), (width, cy), (0, 255, 0), 1) + # Вывести ширину контура + cv2.putText(frame, f'W{round(w,2)}', (x, y+h+25), cv2.FONT_HERSHEY_SIMPLEX, 0.75, (0, 0, 255), 2) + # Вывести расчётную дистанцию до контура + cv2.putText(frame, f'D{round(dist,2)}', (x, y), cv2.FONT_HERSHEY_SIMPLEX, 0.75, (0, 0, 255), 2) + + # Расчёт курса и дистанции до ворот + if len(aligns) == 2: + med_x = sum(aligns) / len(aligns) + controlX = round(-2 * (med_x - width / 2) / width, 1) + controlX = max(-0.5, min(0.5,controlX)) + course_to_gate = round(controlX * self.fov / 2) + dist_to_gate = round(sum(distances) / len(distances), 1) + return course_to_gate, dist_to_gate + else: + return None + + +def main(args=None): + rclpy.init(args=args) + RealOpenCV = RealsenseToBinary() + rclpy.spin(RealOpenCV) + RealOpenCV.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() diff --git a/ws_nodes_py/videoS.py b/ws_nodes_py/videoS.py new file mode 100644 index 0000000..4bc2c97 --- /dev/null +++ b/ws_nodes_py/videoS.py @@ -0,0 +1,186 @@ +#! /usr/bin/env python3 + +import rclpy +from ws_nodes_py.default.GetParameterNode import GetParameterNode +from sensor_msgs.msg import CompressedImage +from std_msgs.msg import Float64, Int16 +from geometry_msgs.msg import Point +import cv2 +from cv_bridge import CvBridge +import numpy as np +from math import radians, cos, sin + + +class RealsenseToBinary(GetParameterNode): + def __init__(self): + super().__init__('RealOpenCV') + + # Параметры для выделения маски синий BGR + # black = cv2.inRange(hsv, (0, 20, 0), (30, 218, 109)) # -> выделяем черный + # self.hsv_min = (0, 149, 0) + # self.hsv_max = (47, 255, 255) + # 5, 66, 69) + # [hsv-1] High: (54, 110, 138) + self.hsv_min = (5, 66, 69) + self.hsv_max = (54, 110, 138) + self.min_countur_size = 300 + self.get_logger().info(f"HSV \n{self.hsv_min} \n {self.hsv_max}") + + # Маска части изображения, которе обрабатываем + x1,y1 = 0, 170 + x2, y2 = 510, 280 + self.roi_corners = np.array([[(x1, y1), (x2, y1), (x2, y2), (x1, y2),]], dtype=np.int32) + + self.camera_angle = 90 + # Параметры для рассчёта расстояние и угла + self.surbscribtion_for_parameter(Int16, "heading_topic", 'heading', checker=lambda x: x, func=lambda msg: msg.data, default=0) + # см. ~/Desktop/python/calc.py + self.k1 = self.get_declare_parameter('k1', rclpy.Parameter.Type.DOUBLE, checker=lambda x: True) + self.k2 = self.get_declare_parameter('k2', rclpy.Parameter.Type.DOUBLE, checker=lambda x: True) + self.fov = self.get_declare_parameter('fov', rclpy.Parameter.Type.INTEGER, checker=lambda x: True) + # Пареметр апроксимации контура + self.k = self.get_declare_parameter('k', rclpy.Parameter.Type.DOUBLE, checker=lambda x: True) + + topic_name = self.get_declare_parameter('topic_camera', rclpy.Parameter.Type.STRING, checker=lambda x: x, error_text="topic name must be string") + # Топик с отладочным изображением + self.pub = self.create_publisher(CompressedImage, f"cv/{topic_name}" , 10) + self.surbscribtion(CompressedImage, "topic_camera", self.img_callback, checker=lambda x: x) + + self.pub_blue_pos = self.publisher_from_declare(Point, "blue_position_topic", checker=lambda x: x) + self.pub_black_pos = self.publisher_from_declare(Point, "black_position_topic", checker=lambda x: x) + + # Топик с отладочным изображением + self.pub = self.create_publisher(CompressedImage, f"cv/{topic_name}" , 10) + + self.br = CvBridge() + self.get_logger().info(f"CV Start {topic_name}") + + def img_callback(self, data): + # Получение кадра + current_frame = self.br.compressed_imgmsg_to_cv2(data) + success, frame = True, current_frame + if success: + channel_count = frame.shape[2] + + # mask = np.zeros(frame.shape, dtype=np.uint8) + # ignore_mask_color = (255,)*channel_count + # cv2.fillPoly(mask, self.roi_corners, ignore_mask_color) + # masked_image = cv2.bitwise_and(frame, mask) + masked_image = frame + + height, width = frame.shape[0:2] + pix_per_angle = width / self.fov + # Нулевые параметры управления + med_x = width / 2 + controlX = 0.0 + dist_to_gate = 0 + course_to_gate = 0 + + # Создание HSV маски + hsv = cv2.cvtColor(masked_image, cv2.COLOR_RGB2HSV) + # yellow = cv2.inRange(hsv, self.hsv_min, self.hsv_max) # -> выделяем желтый + # blue = cv2.inRange(hsv, (0, 149, 0), (47, 255, 255)) # -> выделяем синий + # blue = cv2.inRange(hsv, (0, 0, 190), (24, 255, 255)) # -> выделяем синий + black = cv2.inRange(hsv, (0, 0, 0), (255, 255, 90)) # -> выделяем черный + + # both = blue + yellow + both = black + binary = cv2.bitwise_and(both, both, mask=both) + binary = cv2.morphologyEx(binary, cv2.MORPH_OPEN, np.ones((5,5),np.uint8)) + + # Выделение самых больших контуров удовлетворяющих условию размера + contours, _ = cv2.findContours(binary, cv2.RETR_EXTERNAL, + cv2.CHAIN_APPROX_NONE) + contours = list(filter(lambda x: cv2.moments(x)["m00"] > self.min_countur_size, sorted(contours, key=cv2.contourArea, reverse=True)[:2])) + approxs = list(sorted(contours, key=lambda x: len(cv2.approxPolyDP( + x, self.k * cv2.arcLength(x, True), True)), reverse=True))[:1] + + if approxs: + # Для выделение левого и правого шара + aligns = [] + # Для выделение ближайшего и дальнего шара + distances = [] + + for contour in approxs: + if any((not 10 < xy[0][0] < width - 10 or not 10 < xy[0][1] < height - 10) for xy in contour): + continue + moments = cv2.moments(contour) + contour = cv2.approxPolyDP(contour, self.k * cv2.arcLength(contour, True), True) + # # Получение хз чего + # area = cv2.contourArea(contour) + # Координаты центра контура + cx = int(moments["m10"] / moments["m00"]) + cy = int(moments["m01"] / moments["m00"]) + + # Вычисление дистанции + x, y, w, h = cv2.boundingRect(contour) + dist = (w*(self.k1)+self.k2)*0.01 + + + # Запись центра контура по горизонту и расчётной дистанции до него + aligns.append(cx) + distances.append(dist) + + if True: # Вывод информации о контуре + # Координаты центра + # cv2.putText(frame, f'cx: {cx}, cy: {cy}', (cx + 10, cy - 10), cv2.FONT_HERSHEY_SIMPLEX, 0.75, (255, 255, 255), 2, cv2.LINE_AA) + # Выделение контура + cv2.drawContours(frame, [contour], -1, (0, 255, 0), 1) + # Выделение прямоугольника + # cv2.rectangle(frame, (x, y), (x+w, y+h), (0, 255, 0), 2) + # Оси через центр контура + # cv2.line(frame, (cx, 0), (cx, height), (0, 255, 0), 1) + # cv2.line(frame, (0, cy), (width, cy), (0, 255, 0), 1) + # Вывести ширину контура + cv2.putText(frame, f'W{round(w,2)}', (x, y+h+25), cv2.FONT_HERSHEY_SIMPLEX, 0.75, (0, 0, 255), 2) + # Вывести расчётную дистанцию до контура + cv2.putText(frame, f'D{round(dist,2)}', (x, y), cv2.FONT_HERSHEY_SIMPLEX, 0.75, (0, 0, 255), 2) + + # Расчёт курса и дистанции до ворот + if aligns: + med_x = sum(aligns) / len(aligns) + controlX = round(-2 * (med_x - width / 2) / width, 1) + controlX = max(-0.5, min(0.5,controlX)) + course_to_gate = round(controlX * self.fov / 2) + if distances: + dist_to_gate = round(sum(distances) / len(distances), 1) + + x = round(dist_to_gate * cos(radians(self.heading + self.camera_angle - course_to_gate)), 1) + y = round(dist_to_gate * sin(radians(self.heading + self.camera_angle - course_to_gate)), 1) + # self.get_logger().info("collide border") + # Управляющий сигнал + if 3 >= dist_to_gate >= 0.5: + msg = Point() + msg.x = x + msg.y = y + self.pub_blue_pos.publish(msg) + + # Создание отладочного изображения + miniBin = cv2.resize(binary, (int(binary.shape[1] / 4), int(binary.shape[0] / 4)), + interpolation=cv2.INTER_AREA) + miniBin = cv2.cvtColor(miniBin, cv2.COLOR_GRAY2RGB) + frame[-2 - miniBin.shape[0]:-2, 2:2 + miniBin.shape[1]] = miniBin + + cv2.putText(frame, 'iSee: {};'.format(len(contours)), (width - 120, height - 5), + cv2.FONT_HERSHEY_SIMPLEX, 0.75, (255, 255, 255), 1, cv2.LINE_AA) + cv2.putText(frame, f'x: {x} y: {y}', (0, 25), + cv2.FONT_HERSHEY_SIMPLEX, 0.75, (255, 255, 255), 1, cv2.LINE_AA) + cv2.putText(frame, f'course to bou{self.heading + self.camera_angle - course_to_gate}', (0, 50), + cv2.FONT_HERSHEY_SIMPLEX, 0.75, (255, 255, 255), 1, cv2.LINE_AA) + cv2.putText(frame, f'{controlX = }', (0, 75), + cv2.FONT_HERSHEY_SIMPLEX, 0.75, (255, 255, 255), 1, cv2.LINE_AA) + + # Отладочное изображение + self.pub.publish(self.br.cv2_to_compressed_imgmsg(frame)) + + +def main(args=None): + rclpy.init(args=args) + RealOpenCV = RealsenseToBinary() + rclpy.spin(RealOpenCV) + RealOpenCV.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() diff --git a/ws_nodes_py/world.py b/ws_nodes_py/world.py new file mode 100644 index 0000000..b29d720 --- /dev/null +++ b/ws_nodes_py/world.py @@ -0,0 +1,94 @@ +#!/usr/bin/python + +import rclpy +from ws_nodes_py.default.GetParameterNode import GetParameterNode +from ws_nodes_py.settings.for_world import * +from time import time +from std_msgs.msg import Float64, String, Int16 +from geometry_msgs.msg import Twist, Point # Point.x, Point.y, Point.z +from math import ceil, floor, radians, cos, sin + +class World(GetParameterNode): + def update(self): + try: + delta_time = time() - self.last_time + except AttributeError: + self.get_logger().info("start updating") + delta_time = 0 + self.last_time = time() + + # Вычисление скорости катамарана + med_signal = round(1_500 + abs(self.throtle) * 500) + low, high = 1500, 2000 + for key in self.motor_speed.keys(): + if low <= key <= med_signal: + low = key + if high >= key >= med_signal: + high = key + break + # Линейная интерполяция + speed = round(self.motor_speed[low] + ((med_signal - low) / (high - low) * (self.motor_speed[high] - self.motor_speed[low]) if high != low else 0), 2) + + # Расчёт смещения + dx = speed * cos(radians(self.heading)) * delta_time + dy = speed * sin(radians(self.heading)) * delta_time + # Обновление позиции + self.position[0] += dx + self.position[1] += dy + msg = Point() + msg.x = float(self.position[0]) + msg.y = float(self.position[1]) + self.pub_position.publish(msg) + + def __init__(self): + super().__init__('world') + + self.position = start_position + self.world_markers = world_markers + + self.surbscribtion_for_parameter(Twist, "twist_topic", 'throtle', checker=lambda x: x, func=lambda msg: msg.linear.x, default=0) + self.surbscribtion_for_parameter(Int16, "heading_topic", 'heading', checker=lambda x: x, func=lambda msg: msg.data, default=0) + self.surbscribtion(String, "reset_topic", self.reset_callback, checker=lambda x: x) + for key, value in world_markers_topics_parametrs.items(): + self.surbscribtion(Point, f"{key}_topic", self.get_marker_callback(value), checker=lambda x: x) + + # Имеется в виду новая насадка на мотор или нет (новая - от Кирилла (обтекаемая) или старая - от трионикса (белая)) + if self.get_declare_parameter('is_new_motor', rclpy.Parameter.Type.BOOL, lambda x: True, "is_new_motor can be 0 or 1"): + self.motor_speed = new_motor_speed + self.motor_ang_speed = new_motor_ang_speed + # TODO + self.get_logger().error("CALCULATE new_motor_ang_speed") + 1/0 + else: + self.motor_speed = old_motor_speed + self.motor_ang_speed = old_motor_ang_speed + + self.pub_position = self.publisher_from_declare(Point, "position_topic", checker=lambda x: x) + + self.create_update(self.update) + + def reset_callback(self, msg): + if msg.data == "reset_map": + self.get_logger().info(f"reset map") + self.position = start_position + + def get_marker_callback(self, name): + def callback(msg): + self.update_position(msg, name) + return callback + + def update_position(self, msg, name): + # TODO Проверять чему мы сейчас можем доверять + obj_pos = self.world_markers[name] + self.position = [obj_pos[0] - msg.x, obj_pos[1] - msg.y] + + +def main(): + rclpy.init() + world = World() + rclpy.spin(world) + world.destroy_node() + rclpy.shutdown() + +if __name__ == '__main__': + main() -- GitLab From 5e3ffb0d6bbc3efb73dda3a09ccb1fd4ca0adc64 Mon Sep 17 00:00:00 2001 From: "toropov.nik" Date: Sat, 21 Sep 2024 08:36:04 +0300 Subject: [PATCH 03/23] fix suRbscription --- ws_nodes_py/compass.py | 4 ++-- ws_nodes_py/default/GetParameterNode.py | 4 ++-- ws_nodes_py/regulator.py | 8 ++++---- ws_nodes_py/twist_controller.py | 4 ++-- ws_nodes_py/videoD.py | 2 +- ws_nodes_py/videoF.py | 4 ++-- ws_nodes_py/videoS.py | 4 ++-- ws_nodes_py/world.py | 8 ++++---- 8 files changed, 19 insertions(+), 19 deletions(-) diff --git a/ws_nodes_py/compass.py b/ws_nodes_py/compass.py index 380ec3c..76c2d73 100644 --- a/ws_nodes_py/compass.py +++ b/ws_nodes_py/compass.py @@ -25,8 +25,8 @@ class Compass(GetParameterNode): self.compass_delta = self.get_declare_parameter('delta_compass', rclpy.Parameter.Type.INTEGER, checker=lambda x: 360 > x > 0, error_text="delta_compass not in (0, 360)") - self.surbscribtion(Int16, "compass_topic", self.compass_callback, checker=lambda x: x) - self.surbscribtion(String, "reset_topic", self.reset_callback, checker=lambda x: x) + self.subscribtion(Int16, "compass_topic", self.compass_callback, checker=lambda x: x) + self.subscribtion(String, "reset_topic", self.reset_callback, checker=lambda x: x) self.pub_heading = self.publisher_from_declare(Int16, "heading_topic", checker=lambda x: x) diff --git a/ws_nodes_py/default/GetParameterNode.py b/ws_nodes_py/default/GetParameterNode.py index 5c57992..58d02aa 100644 --- a/ws_nodes_py/default/GetParameterNode.py +++ b/ws_nodes_py/default/GetParameterNode.py @@ -29,11 +29,11 @@ class GetParameterNode(Node): raise DeclareParameterException(error_text) return value - def surbscribtion(self, topic_type, name, func, checker=None, error_text=None, count=1): + def subscribtion(self, topic_type, name, func, checker=None, error_text=None, count=1): topic_name = self.get_declare_parameter(name, rclpy.Parameter.Type.STRING, checker=checker, error_text=error_text) self.create_subscription(topic_type, topic_name, func, count) - def surbscribtion_for_parameter(self, topic_type, name, param_name, func=None, checker=None, error_text=None, count=1, default=None): + def subscribtion_for_parameter(self, topic_type, name, param_name, func=None, checker=None, error_text=None, count=1, default=None): """ Устанавливает значение переменной param_name равной значению данных По умолчанию если это Twist или Point, то записывает всё, если нет, то только из параметра data diff --git a/ws_nodes_py/regulator.py b/ws_nodes_py/regulator.py index d6f91c8..cb9fcf8 100644 --- a/ws_nodes_py/regulator.py +++ b/ws_nodes_py/regulator.py @@ -21,10 +21,10 @@ class Controller(GetParameterNode): # self.k_p_linear = self.get_declare_parameter('k_p_linear', rclpy.Parameter.Type.DOUBLE, checker=lambda x: True) # self.linear_regulator_threshold = self.get_declare_parameter('linear_regulator_threshold', rclpy.Parameter.Type.DOUBLE, checker=lambda x: True) - self.surbscribtion_for_parameter(Int16, "heading_topic", 'heading', checker=lambda x: x, func=lambda msg: msg.data, default=0) - self.surbscribtion_for_parameter(Point, "position_topic", 'position', checker=lambda x: x, func=lambda msg: msg, default=Point()) - self.surbscribtion_for_parameter(Point, "goal_topic", 'goal', checker=lambda x: x, func=lambda msg: msg, default=Point()) - self.surbscribtion_for_parameter(String, "move_type_topic", 'type_move', checker=lambda x: x, func=lambda msg: msg.data, default="STOP") + self.subscribtion_for_parameter(Int16, "heading_topic", 'heading', checker=lambda x: x, func=lambda msg: msg.data, default=0) + self.subscribtion_for_parameter(Point, "position_topic", 'position', checker=lambda x: x, func=lambda msg: msg, default=Point()) + self.subscribtion_for_parameter(Point, "goal_topic", 'goal', checker=lambda x: x, func=lambda msg: msg, default=Point()) + self.subscribtion_for_parameter(String, "move_type_topic", 'type_move', checker=lambda x: x, func=lambda msg: msg.data, default="STOP") self.pub_twist = self.publisher_from_declare(Twist, "twist_robot_topic", checker=lambda x: x) diff --git a/ws_nodes_py/twist_controller.py b/ws_nodes_py/twist_controller.py index b0721b8..f181539 100644 --- a/ws_nodes_py/twist_controller.py +++ b/ws_nodes_py/twist_controller.py @@ -16,8 +16,8 @@ class TwistController(GetParameterNode): checker=lambda x: x > 0, error_text="delta_time not more than zero") # subscribtions - self.surbscribtion(Twist, "twist_joy_topic", self.joy_callback, checker=lambda x: x) - self.surbscribtion(Twist, "twist_robot_topic", self.robot_callback, checker=lambda x: x) + self.subscribtion(Twist, "twist_joy_topic", self.joy_callback, checker=lambda x: x) + self.subscribtion(Twist, "twist_robot_topic", self.robot_callback, checker=lambda x: x) # publisher self.pub_twist = self.publisher_from_declare(Twist, "twist_topic", checker=lambda x: x) diff --git a/ws_nodes_py/videoD.py b/ws_nodes_py/videoD.py index 14ed931..aac0b28 100644 --- a/ws_nodes_py/videoD.py +++ b/ws_nodes_py/videoD.py @@ -36,7 +36,7 @@ class RealsenseToBinary(GetParameterNode): topic_name = self.get_declare_parameter('topic_camera', rclpy.Parameter.Type.STRING, checker=lambda x: x, error_text="topic name must be string") # Топик с отладочным изображением self.pub = self.create_publisher(CompressedImage, f"cv/{topic_name}" , 10) - self.surbscribtion(CompressedImage, "topic_camera", self.img_callback, checker=lambda x: x) + self.subscribtion(CompressedImage, "topic_camera", self.img_callback, checker=lambda x: x) self.br = CvBridge() self.get_logger().info(f"CV Start {topic_name}") diff --git a/ws_nodes_py/videoF.py b/ws_nodes_py/videoF.py index e72e59c..febcdbb 100644 --- a/ws_nodes_py/videoF.py +++ b/ws_nodes_py/videoF.py @@ -28,7 +28,7 @@ class RealsenseToBinary(GetParameterNode): self.roi_corners = np.array([[(x1, y1), (x2, y1), (x2, y2), (x1, y2),]], dtype=np.int32) # Параметры для рассчёта расстояние и угла - self.surbscribtion_for_parameter(Int16, "heading_topic", 'heading', checker=lambda x: x, func=lambda msg: msg.data, default=0) + self.subscribtion_for_parameter(Int16, "heading_topic", 'heading', checker=lambda x: x, func=lambda msg: msg.data, default=0) # см. ~/Desktop/python/calc.py self.k1 = self.get_declare_parameter('k1', rclpy.Parameter.Type.DOUBLE, checker=lambda x: True) self.k2 = self.get_declare_parameter('k2', rclpy.Parameter.Type.DOUBLE, checker=lambda x: True) @@ -39,7 +39,7 @@ class RealsenseToBinary(GetParameterNode): topic_name = self.get_declare_parameter('topic_camera', rclpy.Parameter.Type.STRING, checker=lambda x: x, error_text="topic name must be string") # Топик с отладочным изображением self.pub = self.create_publisher(CompressedImage, f"cv/{topic_name}" , 10) - self.surbscribtion(CompressedImage, "topic_camera", self.img_callback, checker=lambda x: x) + self.subscribtion(CompressedImage, "topic_camera", self.img_callback, checker=lambda x: x) self.pub_yellow_pos = self.publisher_from_declare(Point, "gate_position_topic", checker=lambda x: x) self.pub_blue_pos = self.publisher_from_declare(Point, "blue_gate_position_topic", checker=lambda x: x) diff --git a/ws_nodes_py/videoS.py b/ws_nodes_py/videoS.py index 4bc2c97..2e18453 100644 --- a/ws_nodes_py/videoS.py +++ b/ws_nodes_py/videoS.py @@ -33,7 +33,7 @@ class RealsenseToBinary(GetParameterNode): self.camera_angle = 90 # Параметры для рассчёта расстояние и угла - self.surbscribtion_for_parameter(Int16, "heading_topic", 'heading', checker=lambda x: x, func=lambda msg: msg.data, default=0) + self.subscribtion_for_parameter(Int16, "heading_topic", 'heading', checker=lambda x: x, func=lambda msg: msg.data, default=0) # см. ~/Desktop/python/calc.py self.k1 = self.get_declare_parameter('k1', rclpy.Parameter.Type.DOUBLE, checker=lambda x: True) self.k2 = self.get_declare_parameter('k2', rclpy.Parameter.Type.DOUBLE, checker=lambda x: True) @@ -44,7 +44,7 @@ class RealsenseToBinary(GetParameterNode): topic_name = self.get_declare_parameter('topic_camera', rclpy.Parameter.Type.STRING, checker=lambda x: x, error_text="topic name must be string") # Топик с отладочным изображением self.pub = self.create_publisher(CompressedImage, f"cv/{topic_name}" , 10) - self.surbscribtion(CompressedImage, "topic_camera", self.img_callback, checker=lambda x: x) + self.subscribtion(CompressedImage, "topic_camera", self.img_callback, checker=lambda x: x) self.pub_blue_pos = self.publisher_from_declare(Point, "blue_position_topic", checker=lambda x: x) self.pub_black_pos = self.publisher_from_declare(Point, "black_position_topic", checker=lambda x: x) diff --git a/ws_nodes_py/world.py b/ws_nodes_py/world.py index b29d720..900a335 100644 --- a/ws_nodes_py/world.py +++ b/ws_nodes_py/world.py @@ -46,11 +46,11 @@ class World(GetParameterNode): self.position = start_position self.world_markers = world_markers - self.surbscribtion_for_parameter(Twist, "twist_topic", 'throtle', checker=lambda x: x, func=lambda msg: msg.linear.x, default=0) - self.surbscribtion_for_parameter(Int16, "heading_topic", 'heading', checker=lambda x: x, func=lambda msg: msg.data, default=0) - self.surbscribtion(String, "reset_topic", self.reset_callback, checker=lambda x: x) + self.subscribtion_for_parameter(Twist, "twist_topic", 'throtle', checker=lambda x: x, func=lambda msg: msg.linear.x, default=0) + self.subscribtion_for_parameter(Int16, "heading_topic", 'heading', checker=lambda x: x, func=lambda msg: msg.data, default=0) + self.subscribtion(String, "reset_topic", self.reset_callback, checker=lambda x: x) for key, value in world_markers_topics_parametrs.items(): - self.surbscribtion(Point, f"{key}_topic", self.get_marker_callback(value), checker=lambda x: x) + self.subscribtion(Point, f"{key}_topic", self.get_marker_callback(value), checker=lambda x: x) # Имеется в виду новая насадка на мотор или нет (новая - от Кирилла (обтекаемая) или старая - от трионикса (белая)) if self.get_declare_parameter('is_new_motor', rclpy.Parameter.Type.BOOL, lambda x: True, "is_new_motor can be 0 or 1"): -- GitLab From a9a314482077a973b564741d2632fb01a6441c14 Mon Sep 17 00:00:00 2001 From: "toropov.nik" Date: Mon, 23 Sep 2024 10:01:12 +0300 Subject: [PATCH 04/23] make virtual compass --- ws_nodes_py/compass.py | 80 +++++++++++++++--- .../GetParameterNode.cpython-38.pyc | Bin 4083 -> 4078 bytes 2 files changed, 68 insertions(+), 12 deletions(-) diff --git a/ws_nodes_py/compass.py b/ws_nodes_py/compass.py index 76c2d73..29dcc69 100644 --- a/ws_nodes_py/compass.py +++ b/ws_nodes_py/compass.py @@ -3,12 +3,14 @@ import rclpy from ws_nodes_py.default.GetParameterNode import GetParameterNode from time import time -from std_msgs.msg import Int16, String +import math +from std_msgs.msg import Int16, String, Bool from geometry_msgs.msg import Twist +from ws_nodes_py.settings.for_world import old_motor_ang_speed, new_motor_ang_speed class Compass(GetParameterNode): - # for andgular speed + # for compass angular speed_caltulation last_heading = 0 last_heading_time = 0 d_heads = [[0, 0] for _ in range(20)] @@ -21,15 +23,73 @@ class Compass(GetParameterNode): # TODO сделать виртуальный курс # TODO сделать переход на виртуальный курс по требованию + # Имеется в виду новая насадка на мотор или нет (новая - от Кирилла (обтекаемая) или старая - от трионикса (белая)) + if self.get_declare_parameter('is_new_motor', rclpy.Parameter.Type.BOOL, lambda x: True, "is_new_motor can be 0 or 1"): + self.motor_ang_speed = new_motor_ang_speed + # TODO + self.get_logger().error("CALCULATE new_motor_ang_speed") + 1/0 + else: + self.motor_ang_speed = old_motor_ang_speed + + # (in degrees) + self.ang_accel = 0 + self.angular_speed = 1.5 + self.test_heading = 0 + # for zero heading self.compass_delta = self.get_declare_parameter('delta_compass', rclpy.Parameter.Type.INTEGER, checker=lambda x: 360 > x > 0, error_text="delta_compass not in (0, 360)") self.subscribtion(Int16, "compass_topic", self.compass_callback, checker=lambda x: x) + self.subscribtion_for_parameter(Twist, "twist_topic", 'ang_throtle', checker=lambda x: x, func=lambda msg: msg.angular.z, default=0) + # self.subscribtion(Twist, "twist_topic", self.twist_heading_callback, checker=lambda x: x) self.subscribtion(String, "reset_topic", self.reset_callback, checker=lambda x: x) + self.subscribtion_for_parameter(Bool, "compass_type_topic", 'is_use_real_compass', checker=lambda x: x, func=lambda msg: msg.data, default=True) self.pub_heading = self.publisher_from_declare(Int16, "heading_topic", checker=lambda x: x) + # self.pub_test_heading = self.create_publisher(Int16, 'test_heading', 1) + self.create_update(self.twist_heading_update) + + def twist_heading_update(self): + default_target_angl_speed = 1.5 + zero_throtle_multiplier = 0.6 + full_throtle_angle_speed = 70 + angl_accel_koef = 0.8 + + try: + dt = time() - self.last_time + except Exception: + dt = 0 + self.last_time = time() + + if self.is_use_real_compass: + return + + # linear interpolation of target speed + if abs(self.ang_throtle) > 0.04: # моторы начинают крутиться + target_angle_speed = -self.ang_throtle * full_throtle_angle_speed + else: + target_angle_speed = default_target_angl_speed + # clamp max speed + target_angle_accel = target_angle_speed - self.angular_speed + + # clamp max acceleration + if target_angle_accel > full_throtle_angle_speed: + target_angle_accel = full_throtle_angle_speed + elif target_angle_accel < -full_throtle_angle_speed: + target_angle_accel = -full_throtle_angle_speed + + ang_accel = target_angle_accel + if abs(ang_accel) > 1: + self.angular_speed += ang_accel * dt * (zero_throtle_multiplier if target_angle_speed == default_target_angl_speed else 1) + self.heading += self.angular_speed * dt + self.heading = self.heading % 360 + + self.pub(self.pub_heading, Int16, int(self.heading)) + self.get_logger().info("USE VIRTUAL COMPASS") + def compass_callback(self, msg): # Проверка на правильность преобразования assert -10%360 == 350 @@ -47,6 +107,9 @@ class Compass(GetParameterNode): self.get_logger().info("start median") self.last_compass = [msg.data, msg.data, msg.data] + if not self.is_use_real_compass: + return + # median a = self.last_compass[0] b = self.last_compass[1] @@ -54,21 +117,14 @@ class Compass(GetParameterNode): self.heading = int(max(a, c) if (max(a, b) == max(b, c)) else max(b, min(a, c))) self.pub(self.pub_heading, Int16, self.heading) - - def twist_heading_callback(self): - # Расчёт виртуального курса (trash) - old_motor_ang_speed = { - 0: 0, - # 0.15: -10, - 0.3: -20, - } + # self.compass_angular_speed() def reset_callback(self, msg): if msg.data == "reset_heading": self.get_logger().info(f"reset heading {self.__compass}") self.compass_delta = self.__compass - def angular_speed(self): + def compass_angular_speed(self): # Разность угла [-179;180] d_head = self.heading - self.last_heading if d_head <= -180: @@ -85,7 +141,7 @@ class Compass(GetParameterNode): # Средняя угловая скорость за некоторый период времени self.d_heads.append([ang_speed, d_time]) self.d_heads.pop(0) - return round(sum(spd * t for spd, t in self.d_heads) / sum(t for spd, t in self.d_heads),1) + self.angular_speed = round(sum(spd * t for spd, t in self.d_heads) / sum(t for spd, t in self.d_heads),1) def main(): diff --git a/ws_nodes_py/default/__pycache__/GetParameterNode.cpython-38.pyc b/ws_nodes_py/default/__pycache__/GetParameterNode.cpython-38.pyc index b2592483ba09497a047930c6362fc71be65f4b89..994cfb3497bb7a70515272ee5fa52b30c1615475 100644 GIT binary patch delta 205 zcmew?|4yDal$V!_0SF2s@1*_R$ZN`~&r@8QRGeItnN*UQpQiwnDrNyGV3c62l66ll z2}mqT%uOvxE%M7xN!2SZ-CWEn%p_uqMP~9j4vEPw%tD)wva>QWOBI&_1q?*AajDei zOkupWc>-q)Gq)Acz9NvbiYzyO=MiI)G6b=WL4-MoumBM{Ai{3)etz-EcKllb-`P6C delta 216 zcmaDS|5=_ll$V!_0SI=b-%0zwk=K;fkhi$BD5*HPC^M-fGe1uOC|Aq^Qo$&}SS9D4 zS`v_0l$e`Zl3L`KpOUHvRJ^&6RhUWC4x7|uK2C|v8`#AeS)_pqLA=S2I3z@MaHy?e zjN07I8N)1U3AC>W Date: Tue, 24 Sep 2024 15:43:08 +0300 Subject: [PATCH 05/23] from real smtupool --- setup.py | 3 +- ws_nodes_py/compass.py | 19 +- .../__pycache__/for_world.cpython-38.pyc | Bin 896 -> 891 bytes ws_nodes_py/settings/for_world.py | 4 +- ws_nodes_py/state_machine.py | 309 ++++++++++++++++++ ws_nodes_py/videoF.py | 44 ++- ws_nodes_py/videoS.py | 215 ++++++------ ws_nodes_py/world.py | 21 +- 8 files changed, 480 insertions(+), 135 deletions(-) create mode 100644 ws_nodes_py/state_machine.py diff --git a/setup.py b/setup.py index 825cf75..fd9415f 100644 --- a/setup.py +++ b/setup.py @@ -34,6 +34,7 @@ setup( 'camcvS = ws_nodes_py.videoS:main', 'camcvD = ws_nodes_py.videoD:main', 'world = ws_nodes_py.world:main', + 'state_machine = ws_nodes_py.state_machine:main', ], }, -) \ No newline at end of file +) diff --git a/ws_nodes_py/compass.py b/ws_nodes_py/compass.py index 29dcc69..4473358 100644 --- a/ws_nodes_py/compass.py +++ b/ws_nodes_py/compass.py @@ -36,25 +36,27 @@ class Compass(GetParameterNode): self.ang_accel = 0 self.angular_speed = 1.5 self.test_heading = 0 + self.heading = 0 # for zero heading self.compass_delta = self.get_declare_parameter('delta_compass', rclpy.Parameter.Type.INTEGER, checker=lambda x: 360 > x > 0, error_text="delta_compass not in (0, 360)") + # self.compass_delta = 0 self.subscribtion(Int16, "compass_topic", self.compass_callback, checker=lambda x: x) self.subscribtion_for_parameter(Twist, "twist_topic", 'ang_throtle', checker=lambda x: x, func=lambda msg: msg.angular.z, default=0) # self.subscribtion(Twist, "twist_topic", self.twist_heading_callback, checker=lambda x: x) self.subscribtion(String, "reset_topic", self.reset_callback, checker=lambda x: x) - self.subscribtion_for_parameter(Bool, "compass_type_topic", 'is_use_real_compass', checker=lambda x: x, func=lambda msg: msg.data, default=True) + self.subscribtion_for_parameter(Bool, "compass_type_topic", 'is_use_real_compass', checker=lambda x: x, func=lambda msg: msg.data, default=False) self.pub_heading = self.publisher_from_declare(Int16, "heading_topic", checker=lambda x: x) # self.pub_test_heading = self.create_publisher(Int16, 'test_heading', 1) self.create_update(self.twist_heading_update) def twist_heading_update(self): - default_target_angl_speed = 1.5 + default_target_angl_speed = 0 zero_throtle_multiplier = 0.6 - full_throtle_angle_speed = 70 + full_throtle_angle_speed = 55 angl_accel_koef = 0.8 try: @@ -88,7 +90,7 @@ class Compass(GetParameterNode): self.heading = self.heading % 360 self.pub(self.pub_heading, Int16, int(self.heading)) - self.get_logger().info("USE VIRTUAL COMPASS") + # self.get_logger().info("USE VIRTUAL COMPASS") def compass_callback(self, msg): # Проверка на правильность преобразования @@ -105,7 +107,7 @@ class Compass(GetParameterNode): self.last_compass.append((msg.data - self.compass_delta) % 360) except AttributeError: # if first compass data self.get_logger().info("start median") - self.last_compass = [msg.data, msg.data, msg.data] + self.last_compass = [msg.data - self.compass_delta, msg.data - self.compass_delta, msg.data - self.compass_delta] if not self.is_use_real_compass: return @@ -120,9 +122,12 @@ class Compass(GetParameterNode): # self.compass_angular_speed() def reset_callback(self, msg): - if msg.data == "reset_heading": - self.get_logger().info(f"reset heading {self.__compass}") + if msg.data == "reset_heading" and self.is_use_real_compass: + self.get_logger().info(f"reset real heading {self.__compass}") self.compass_delta = self.__compass + elif msg.data == "reset_heading" and not self.is_use_real_compass: + self.get_logger().info(f"reset virtual heading {self.__compass}") + self.heading = 0 def compass_angular_speed(self): # Разность угла [-179;180] diff --git a/ws_nodes_py/settings/__pycache__/for_world.cpython-38.pyc b/ws_nodes_py/settings/__pycache__/for_world.cpython-38.pyc index e9aa731229ecf8acd81ed1298659f594b07f3b8a..203a404cb43b4b1401ddbc3d0f087dd53b068a1d 100644 GIT binary patch delta 207 zcmX|*I}XAy5JdfpO&rWWKt~HTMGk_}xS|*-)La0r5V`dUxCRp91T>r=2cV*1t(~)suFZ(a7W3@>r}0oOz=kpKVy delta 212 zcmey(*1*mc%FD~e00f`ICvutBGp8`5Go>)6FvCd(Fq>r#dlW+o5T-LlF{ZFaF@e}A zY$@z194VYBTq)c@$TNpC3aCGYH-&EwQxpqW1%HY_ieQQm5DEiLU 0, "no hz is described"), self.update) + + def set_goal(self, x, y, z=0.0): + self.goal.x = float(x) + self.goal.y = float(y) + self.goal.z = float(z) + + def update(self): + # self.get_logger().info(f"{self.move_type} [{self.goal.x} {self.goal.y}] {self.is_real_compass}") + self.pub(self.pub_move_type, String, self.move_type) + self.pub_goal.publish(self.goal) + self.pub(self.pub_compass_type, Bool, self.is_real_compass) + + def next_step(self, move_type: str, goal: list, is_real_compass: bool): + self.move_type = move_type + # self.goal = goal + self.set_goal(*goal) + self.is_real_compass = is_real_compass + # self.get_logger().info(f"{self.move_type} {self.is_real_compass}") + self.update() + + def handle_start(self, d): + if d.data: + self.enabled_ = d.data + + def pos_callback(self, pos): + self.position = pos + + def heading_callback(self, msg): + self.heading = msg.data + + def get_declare_parameter(self, name, checker, error_text): + value = self.get_parameter(name).value + if not checker(value): + self.get_logger().error(error_text) + raise Exception(error_text) + return value + + def shutdown(self): + return False + + def pub(self, topic, data_type, data: dict): + msg = data_type() + if type(data) != dict: + msg.data = data + else: + for key, value in data.items(): + setattr(msg, key, value) + topic.publish(msg) + + +class Start(smach.State): + def __init__(self, c, pid_enabled=False): + smach.State.__init__(self, outcomes=['start']) + self.cntr = c + self.pid_enabled = pid_enabled + + def execute(self, userdata): + self.cntr.next_step("STOP", [0, 0], False) + while not self.cntr.shutdown(): + rclpy.spin_once(self.cntr) + if self.cntr.enabled_: + self.cntr.get_logger().info('finish') + return 'start' + +class Wait(smach.State): + def __init__(self, c:Controller, time:int): + smach.State.__init__(self, outcomes=['timeout']) + self.cntr = c + self.time = time + + def execute(self, userdata): + self.cntr.next_step("STOP", [0, 0], False) + self.cntr.state_change_time = self.cntr.get_clock().now() + while not self.cntr.shutdown(): + rclpy.spin_once(self.cntr) + if (self.cntr.get_clock().now() - self.cntr.state_change_time) > Duration(seconds=self.time): + self.cntr.get_logger().info('finish') + return 'timeout' + +class MoveToGoal(smach.State): + def __init__(self, c:Controller, time:int, goal:list, is_real_compass): + smach.State.__init__(self, outcomes=['timeout']) + self.cntr = c + + self.time = time + self.goal = goal + self.is_real_compass = is_real_compass + self.distance_threshold = 0.5 + + + def execute(self, userdata): + self.cntr.next_step("MOVE TO", self.goal, self.is_real_compass) + self.cntr.state_change_time = self.cntr.get_clock().now() + while not self.cntr.shutdown(): + rclpy.spin_once(self.cntr) + if (self.cntr.get_clock().now() - self.cntr.state_change_time) > Duration(seconds=self.time): + self.cntr.get_logger().info('finish') + return 'timeout' + else: + error_dist = ((self.goal[0] - self.cntr.position.x)**2 + (self.goal[1] - self.cntr.position.y)**2)**0.5 + # self.cntr.get_logger().info(f"{error_dist=}") + if error_dist < self.distance_threshold: + return 'timeout' + +class MoveToCircle(smach.State): + def __init__(self, c:Controller, time:int, goal:list, dist, angle:int, is_real_compass): + smach.State.__init__(self, outcomes=['timeout']) + self.cntr = c + + self.time = time + self.trg_angle = angle + self.goal = list(goal) + self.goal.append(dist) + self.is_real_compass = is_real_compass + self.distance_threshold = 0.5 + + def angle_error(self, target_angle, source_angle): + result = (source_angle - target_angle) % 360 + if result > 180: + result -= 360 + return result + + def execute(self, userdata): + self.cntr.state_change_time = self.cntr.get_clock().now() + self.cntr.next_step("MOVE CIRCLE", self.goal, is_real_compass) + while not self.cntr.shutdown(): + rclpy.spin_once(self.cntr) + typs = String() + typs.data = 'MOVE CIRCLE' + self.cntr.pub_move_type.publish(typs) + goal = Point() + goal.x = float(self.goal[0]) + goal.y = float(self.goal[1]) + goal.z = float(self.goal[2]) + self.cntr.pub_goal.publish(goal) + if (self.cntr.get_clock().now() - self.cntr.state_change_time) > Duration(seconds=self.time): + self.cntr.get_logger().info('finish') + return 'timeout' + else: + error_angle = abs(self.angle_error(self.trg_angle, self.cntr.heading)) + # self.cntr.get_logger().info(f'{error_angle}') + if error_angle < 5: + return 'timeout' + +class Finish(smach.State): + def __init__(self, c): + smach.State.__init__(self, outcomes=['fin']) + self.cntr = c + + def execute(self, userdata): + self.cntr.next_step("STOP", [0, 0], False) + while not self.cntr.shutdown(): + self.cntr.get_logger().info('end') + return 'fin' + + +def main(): + rclpy.init() + c = Controller() + c.get_logger().info("---- Waiting! ----") + sm = smach.StateMachine(outcomes=['fin']) + sis = smach_ros.IntrospectionServer('tree', sm, '/SM_ROOT') + sis.start() + square_mission = { + # point_name: [x, y] in meters + "yellow": { + "type": "moveTo", + "pos": [1, 0], + "is_real_compass": False, + }, + "left_blue": { + "type": "moveTo", + "pos": [1, 1], + "is_real_compass": False, + }, + "point_3": { + "type": "moveTo", + "pos": [0, 1], + "is_real_compass": False, + }, + "point_4": { + "type": "moveTo", + "pos": [0, 0], + "is_real_compass": False, + }, + } + circle_mission = { + "circle_1": { + "type": "moveAround", + "pos": [10, 0], + "radius": 1, + "out_heading": 120, # надо проверить + "is_real_compass": False, + }, + } + + vlad24_mission = { + "gate-1-yellow": { + "type": "moveTo", + "pos": [6.5, 0], + "is_real_compass": False, + }, + "point-left-blue": { + "type": "moveTo", + "pos": [10, -2], + "is_real_compass": False, + }, + # "point-2-blue": { + # "type": "moveTo", + # "pos": [12, 0], + # "is_real_compass": False, + # }, + "point-2-blue": { + "type": "moveTo", + "pos": [10, 2], + "is_real_compass": False, + }, + # "gate-2-blue": { + # "type": "moveAround", + # "pos": [10, 0], + # "radius": 1, + # "out_heading": 120, # надо проверить + # }, + "gate-3-yellow": { + "type": "moveTo", + "pos": [6, 0], + "is_real_compass": False, + }, + "move-to-start": { + "type": "moveTo", + "pos": [0, 0], + "is_real_compass": False, + }, + } + mission = vlad24_mission + with sm: + c.get_logger().info("---!---") + smach.StateMachine.add('STARTING', Start(c), + transitions={'start': 'GOAL-1'}) + + for i, key in enumerate(mission.keys(), 1): + if mission[key]["type"] == "moveTo": + smach.StateMachine.add(f'GOAL-{i}', MoveToGoal(c, 1000, mission[key]["pos"], mission[key]["is_real_compass"]), + transitions={'timeout': f'GOAL-{ "FINISH" if len(mission.keys()) == i else i+1}'}) + elif mission[key]["type"] == "moveAround": + smach.StateMachine.add(f'GOAL-{i}', MoveToCircle(c, 1000, mission[key]["pos"], mission[key]["radius"], mission[key]["out_heading"], mission[key]["is_real_compass"]), + transitions={'timeout': f'GOAL-{ "FINISH" if len(mission.keys()) == i else i+1}'}) + + #for i, key in enumerate(mission.keys(), 1): + # smach.StateMachine.add(f'GOAL-{i}', MoveToGoal(c, 1000, mission[key]), + # transitions={'timeout': f'GOAL-{ "FINISH" if len(mission.keys()) == i else i+1}'}) + + smach.StateMachine.add('GOAL-FINISH', Finish(c), + transitions={'fin': 'STARTING'}) + + outcome = sm.execute() + +if __name__ == '__main__': + main() \ No newline at end of file diff --git a/ws_nodes_py/videoF.py b/ws_nodes_py/videoF.py index febcdbb..534ef06 100644 --- a/ws_nodes_py/videoF.py +++ b/ws_nodes_py/videoF.py @@ -24,7 +24,7 @@ class RealsenseToBinary(GetParameterNode): # Маска части изображения, которе обрабатываем x1,y1 = 0, 170 - x2, y2 = 510, 280 + x2, y2 = 640, 280 self.roi_corners = np.array([[(x1, y1), (x2, y1), (x2, y2), (x1, y2),]], dtype=np.int32) # Параметры для рассчёта расстояние и угла @@ -64,14 +64,19 @@ class RealsenseToBinary(GetParameterNode): # Создание HSV маски hsv = cv2.cvtColor(masked_image, cv2.COLOR_RGB2HSV) # blue = cv2.inRange(hsv, (0, 76, 165), (28, 255, 255)) # -> выделяем синий - yellow = cv2.inRange(hsv, (43, 57, 203), (86, 255, 242)) # -> выделяем желтый - blue = cv2.inRange(hsv, (0, 180, 190), (24, 255, 255)) # -> выделяем синий - - both = blue + yellow - # both = yellow + # Low: (91, 126, 100) + # [hsv-1] High: (96, 255, 255) + yellow = cv2.inRange(hsv, (91, 126, 100), (96, 255, 255)) # -> выделяем желтый + # [hsv-1] Low: (0, 37, 121) + # [hsv-1] High: (19, 255, 255) + blue = cv2.inRange(hsv, (0, 37, 121), (19, 255, 255)) # -> выделяем синий + + # both = blue + yellow + both = yellow yellow_binary = cv2.bitwise_and(yellow, yellow, mask=yellow) - blue_binary = cv2.bitwise_and(blue, blue, mask=blue) + blue_binary = self.get_blue_BALLS(frame) binary = cv2.bitwise_and(both, both, mask=both) + binary = blue_binary # Выделение самых больших контуров удовлетворяющих условию размера res = self.get_gate(yellow_binary, height, width, frame) @@ -116,6 +121,31 @@ class RealsenseToBinary(GetParameterNode): # Отладочное изображение self.pub.publish(self.br.cv2_to_compressed_imgmsg(frame)) + def set_border(self, frame): + channel_count = frame.shape[2] + mask = np.zeros(frame.shape, dtype=np.uint8) + ignore_mask_color = (255,)*channel_count + cv2.fillPoly(mask, self.roi_corners, ignore_mask_color) + masked_image = cv2.bitwise_and(frame, mask) + return masked_image + + def get_blue_BALLS(self, frame): + frame = self.set_border(frame) + lab= cv2.cvtColor(frame, cv2.COLOR_BGR2LAB) + l_channel, a, b = cv2.split(lab) + + clahe = cv2.createCLAHE(clipLimit=2.0, tileGridSize=(8,8)) + cl = clahe.apply(l_channel) + + limg = cv2.merge((cl,a,b)) + + enhanced_img = cv2.cvtColor(limg, cv2.COLOR_LAB2BGR) + + frame = cv2.cvtColor(enhanced_img, cv2.COLOR_RGB2YUV) + + binary = cv2.inRange(frame, (0, 0, 153), (360, 220, 255)) + return binary + def get_gate(self, binary, height, width, frame): pix_per_angle = width / self.fov # Нулевые параметры управления diff --git a/ws_nodes_py/videoS.py b/ws_nodes_py/videoS.py index 2e18453..fdbf773 100644 --- a/ws_nodes_py/videoS.py +++ b/ws_nodes_py/videoS.py @@ -43,7 +43,7 @@ class RealsenseToBinary(GetParameterNode): topic_name = self.get_declare_parameter('topic_camera', rclpy.Parameter.Type.STRING, checker=lambda x: x, error_text="topic name must be string") # Топик с отладочным изображением - self.pub = self.create_publisher(CompressedImage, f"cv/{topic_name}" , 10) + self.pub = self.create_publisher(CompressedImage, f"cv/{topic_name}/compressed" , 10) self.subscribtion(CompressedImage, "topic_camera", self.img_callback, checker=lambda x: x) self.pub_blue_pos = self.publisher_from_declare(Point, "blue_position_topic", checker=lambda x: x) @@ -55,123 +55,114 @@ class RealsenseToBinary(GetParameterNode): self.br = CvBridge() self.get_logger().info(f"CV Start {topic_name}") + + def set_border(self, frame): + channel_count = frame.shape[2] + mask = np.zeros(frame.shape, dtype=np.uint8) + ignore_mask_color = (255,)*channel_count + cv2.fillPoly(mask, self.roi_corners, ignore_mask_color) + masked_image = cv2.bitwise_and(frame, mask) + return masked_image + + def get_blue_BALLS(self, frame): + frame = self.set_border(frame) + lab= cv2.cvtColor(frame, cv2.COLOR_BGR2LAB) + l_channel, a, b = cv2.split(lab) + + clahe = cv2.createCLAHE(clipLimit=2.0, tileGridSize=(8,8)) + cl = clahe.apply(l_channel) + + limg = cv2.merge((cl,a,b)) + + enhanced_img = cv2.cvtColor(limg, cv2.COLOR_LAB2BGR) + + frame = cv2.cvtColor(enhanced_img, cv2.COLOR_RGB2YUV) + + binary = cv2.inRange(frame, (0, 0, 163), (360, 220, 255)) + return binary + + def img_callback(self, data): - # Получение кадра - current_frame = self.br.compressed_imgmsg_to_cv2(data) - success, frame = True, current_frame - if success: - channel_count = frame.shape[2] - - # mask = np.zeros(frame.shape, dtype=np.uint8) - # ignore_mask_color = (255,)*channel_count - # cv2.fillPoly(mask, self.roi_corners, ignore_mask_color) - # masked_image = cv2.bitwise_and(frame, mask) - masked_image = frame - - height, width = frame.shape[0:2] - pix_per_angle = width / self.fov - # Нулевые параметры управления - med_x = width / 2 - controlX = 0.0 - dist_to_gate = 0 - course_to_gate = 0 - - # Создание HSV маски - hsv = cv2.cvtColor(masked_image, cv2.COLOR_RGB2HSV) - # yellow = cv2.inRange(hsv, self.hsv_min, self.hsv_max) # -> выделяем желтый - # blue = cv2.inRange(hsv, (0, 149, 0), (47, 255, 255)) # -> выделяем синий - # blue = cv2.inRange(hsv, (0, 0, 190), (24, 255, 255)) # -> выделяем синий - black = cv2.inRange(hsv, (0, 0, 0), (255, 255, 90)) # -> выделяем черный - - # both = blue + yellow - both = black - binary = cv2.bitwise_and(both, both, mask=both) - binary = cv2.morphologyEx(binary, cv2.MORPH_OPEN, np.ones((5,5),np.uint8)) - - # Выделение самых больших контуров удовлетворяющих условию размера - contours, _ = cv2.findContours(binary, cv2.RETR_EXTERNAL, + frame = self.br.compressed_imgmsg_to_cv2(data) + binary = self.get_blue_BALLS(frame) + contours, _ = cv2.findContours(binary, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_NONE) - contours = list(filter(lambda x: cv2.moments(x)["m00"] > self.min_countur_size, sorted(contours, key=cv2.contourArea, reverse=True)[:2])) - approxs = list(sorted(contours, key=lambda x: len(cv2.approxPolyDP( - x, self.k * cv2.arcLength(x, True), True)), reverse=True))[:1] - - if approxs: - # Для выделение левого и правого шара - aligns = [] - # Для выделение ближайшего и дальнего шара - distances = [] - - for contour in approxs: - if any((not 10 < xy[0][0] < width - 10 or not 10 < xy[0][1] < height - 10) for xy in contour): - continue - moments = cv2.moments(contour) - contour = cv2.approxPolyDP(contour, self.k * cv2.arcLength(contour, True), True) - # # Получение хз чего - # area = cv2.contourArea(contour) - # Координаты центра контура - cx = int(moments["m10"] / moments["m00"]) - cy = int(moments["m01"] / moments["m00"]) - - # Вычисление дистанции - x, y, w, h = cv2.boundingRect(contour) - dist = (w*(self.k1)+self.k2)*0.01 - - - # Запись центра контура по горизонту и расчётной дистанции до него - aligns.append(cx) - distances.append(dist) - - if True: # Вывод информации о контуре - # Координаты центра - # cv2.putText(frame, f'cx: {cx}, cy: {cy}', (cx + 10, cy - 10), cv2.FONT_HERSHEY_SIMPLEX, 0.75, (255, 255, 255), 2, cv2.LINE_AA) - # Выделение контура - cv2.drawContours(frame, [contour], -1, (0, 255, 0), 1) - # Выделение прямоугольника - # cv2.rectangle(frame, (x, y), (x+w, y+h), (0, 255, 0), 2) - # Оси через центр контура - # cv2.line(frame, (cx, 0), (cx, height), (0, 255, 0), 1) - # cv2.line(frame, (0, cy), (width, cy), (0, 255, 0), 1) - # Вывести ширину контура - cv2.putText(frame, f'W{round(w,2)}', (x, y+h+25), cv2.FONT_HERSHEY_SIMPLEX, 0.75, (0, 0, 255), 2) - # Вывести расчётную дистанцию до контура - cv2.putText(frame, f'D{round(dist,2)}', (x, y), cv2.FONT_HERSHEY_SIMPLEX, 0.75, (0, 0, 255), 2) - - # Расчёт курса и дистанции до ворот - if aligns: - med_x = sum(aligns) / len(aligns) - controlX = round(-2 * (med_x - width / 2) / width, 1) - controlX = max(-0.5, min(0.5,controlX)) - course_to_gate = round(controlX * self.fov / 2) - if distances: - dist_to_gate = round(sum(distances) / len(distances), 1) + contours = list(filter(lambda x: cv2.moments(x)["m00"] > 600, sorted(contours, key=cv2.contourArea, reverse=True)[:1])) + + height, width = frame.shape[0:2] + pix_per_angle = width / self.fov + # Нулевые параметры управления + med_x = width / 2 + dist_to_gate = 0 + course_to_gate = 0 + aligns = [] + distances = [] + for contour in contours: + moments = cv2.moments(contour) + cx = int(moments["m10"] / moments["m00"]) + cy = int(moments["m01"] / moments["m00"]) + + x, y, w, h = cv2.boundingRect(contour) + dist = (w*(self.k1)+self.k2)*0.01 + + if any((not 70 < xy[0][0] < 640 - 70 or not 10 < xy[0][1] < 480 - 10) for xy in contour) or w-h >= h*1.5//1: + continue + h = w + aligns.append(cx) + distances.append(dist) + + if True: # Вывод информации о контуре + # Координаты центра + # cv2.putText(frame, f'cx: {cx}, cy: {cy}', (cx + 10, cy - 10), cv2.FONT_HERSHEY_SIMPLEX, 0.75, (255, 255, 255), 2, cv2.LINE_AA) + # Выделение контура + cv2.drawContours(frame, [contour], -1, (0, 255, 0), 1) + cv2.rectangle(frame, (x, y), (x+w, y+h), (0, 255, 0), 2) + # Выделение прямоугольника + # cv2.rectangle(frame, (x, y), (x+w, y+h), (0, 255, 0), 2) + # Оси через центр контура + # cv2.line(frame, (cx, 0), (cx, height), (0, 255, 0), 1) + # cv2.line(frame, (0, cy), (width, cy), (0, 255, 0), 1) + # Вывести ширину контура + cv2.putText(frame, f'W{round(w,2)}', (x, y+h+25), cv2.FONT_HERSHEY_SIMPLEX, 0.75, (0, 0, 255), 2) + # Вывести расчётную дистанцию до контура + cv2.putText(frame, f'D{round(dist,2)}', (x, y), cv2.FONT_HERSHEY_SIMPLEX, 0.75, (0, 0, 255), 2) - x = round(dist_to_gate * cos(radians(self.heading + self.camera_angle - course_to_gate)), 1) - y = round(dist_to_gate * sin(radians(self.heading + self.camera_angle - course_to_gate)), 1) - # self.get_logger().info("collide border") - # Управляющий сигнал - if 3 >= dist_to_gate >= 0.5: - msg = Point() - msg.x = x - msg.y = y - self.pub_blue_pos.publish(msg) - - # Создание отладочного изображения - miniBin = cv2.resize(binary, (int(binary.shape[1] / 4), int(binary.shape[0] / 4)), - interpolation=cv2.INTER_AREA) - miniBin = cv2.cvtColor(miniBin, cv2.COLOR_GRAY2RGB) - frame[-2 - miniBin.shape[0]:-2, 2:2 + miniBin.shape[1]] = miniBin + # Расчёт курса и дистанции до ворот + if aligns: + med_x = sum(aligns) / len(aligns) + controlX = round(-2 * (med_x - width / 2) / width, 1) + controlX = max(-0.5, min(0.5,controlX)) + course_to_gate = round(controlX * self.fov / 2) + if distances: + dist_to_gate = round(sum(distances) / len(distances), 1) - cv2.putText(frame, 'iSee: {};'.format(len(contours)), (width - 120, height - 5), - cv2.FONT_HERSHEY_SIMPLEX, 0.75, (255, 255, 255), 1, cv2.LINE_AA) - cv2.putText(frame, f'x: {x} y: {y}', (0, 25), - cv2.FONT_HERSHEY_SIMPLEX, 0.75, (255, 255, 255), 1, cv2.LINE_AA) - cv2.putText(frame, f'course to bou{self.heading + self.camera_angle - course_to_gate}', (0, 50), - cv2.FONT_HERSHEY_SIMPLEX, 0.75, (255, 255, 255), 1, cv2.LINE_AA) - cv2.putText(frame, f'{controlX = }', (0, 75), + x = round(dist_to_gate * cos(radians(self.heading + self.camera_angle - course_to_gate)), 1) + y = round(dist_to_gate * sin(radians(self.heading + self.camera_angle - course_to_gate)), 1) + # self.get_logger().info("collide border") + # Управляющий сигнал + if 3 >= dist_to_gate >= 0.5: + msg = Point() + msg.x = x + msg.y = y + self.pub_blue_pos.publish(msg) + + # Создание отладочного изображения + miniBin = cv2.resize(binary, (int(binary.shape[1] / 4), int(binary.shape[0] / 4)), + interpolation=cv2.INTER_AREA) + miniBin = cv2.cvtColor(miniBin, cv2.COLOR_GRAY2RGB) + frame[-2 - miniBin.shape[0]:-2, 2:2 + miniBin.shape[1]] = miniBin + + cv2.putText(frame, 'iSee: {};'.format(len(contours)), (width - 120, height - 5), + cv2.FONT_HERSHEY_SIMPLEX, 0.75, (255, 255, 255), 1, cv2.LINE_AA) + cv2.putText(frame, f'x: {x} y: {y}', (0, 25), + cv2.FONT_HERSHEY_SIMPLEX, 0.75, (255, 255, 255), 1, cv2.LINE_AA) + cv2.putText(frame, f'course to bou{self.heading + self.camera_angle - course_to_gate}', (0, 50), cv2.FONT_HERSHEY_SIMPLEX, 0.75, (255, 255, 255), 1, cv2.LINE_AA) + # cv2.putText(frame, f'{controlX = }', (0, 75), + # cv2.FONT_HERSHEY_SIMPLEX, 0.75, (255, 255, 255), 1, cv2.LINE_AA) - # Отладочное изображение - self.pub.publish(self.br.cv2_to_compressed_imgmsg(frame)) + # Отладочное изображение + self.pub.publish(self.br.cv2_to_compressed_imgmsg(frame)) def main(args=None): diff --git a/ws_nodes_py/world.py b/ws_nodes_py/world.py index 900a335..ebffb94 100644 --- a/ws_nodes_py/world.py +++ b/ws_nodes_py/world.py @@ -30,24 +30,26 @@ class World(GetParameterNode): speed = round(self.motor_speed[low] + ((med_signal - low) / (high - low) * (self.motor_speed[high] - self.motor_speed[low]) if high != low else 0), 2) # Расчёт смещения - dx = speed * cos(radians(self.heading)) * delta_time - dy = speed * sin(radians(self.heading)) * delta_time + dx = speed * cos(radians(self.heading)) * delta_time + dy = speed * sin(radians(self.heading)) * delta_time # Обновление позиции self.position[0] += dx self.position[1] += dy msg = Point() msg.x = float(self.position[0]) msg.y = float(self.position[1]) + # self.get_logger().info(f"{self.position} {speed} {self.throtle}") self.pub_position.publish(msg) def __init__(self): super().__init__('world') - self.position = start_position + self.position = start_position[:] self.world_markers = world_markers self.subscribtion_for_parameter(Twist, "twist_topic", 'throtle', checker=lambda x: x, func=lambda msg: msg.linear.x, default=0) self.subscribtion_for_parameter(Int16, "heading_topic", 'heading', checker=lambda x: x, func=lambda msg: msg.data, default=0) + self.subscribtion_for_parameter(Point, "goal_topic", 'goal_x', checker=lambda x: x, func=lambda msg: msg.x, default=0) self.subscribtion(String, "reset_topic", self.reset_callback, checker=lambda x: x) for key, value in world_markers_topics_parametrs.items(): self.subscribtion(Point, f"{key}_topic", self.get_marker_callback(value), checker=lambda x: x) @@ -70,7 +72,7 @@ class World(GetParameterNode): def reset_callback(self, msg): if msg.data == "reset_map": self.get_logger().info(f"reset map") - self.position = start_position + self.position = start_position[:] def get_marker_callback(self, name): def callback(msg): @@ -79,8 +81,15 @@ class World(GetParameterNode): def update_position(self, msg, name): # TODO Проверять чему мы сейчас можем доверять - obj_pos = self.world_markers[name] - self.position = [obj_pos[0] - msg.x, obj_pos[1] - msg.y] + # self.get_logger().info(f"{name=} {self.goal_x=}") + if (7 > self.goal_x > 5) and name == "yellow_gate": + # self.get_logger().info(f"SADADSAD {name=} {self.goal_x=}") + obj_pos = self.world_markers[name] + self.position = [obj_pos[0] - msg.x, obj_pos[1] - msg.y] + if (11 > self.goal_x > 9) and name == "blue_gate": + # self.get_logger().info(f"SADADSAD {name=} {self.goal_x=}") + obj_pos = self.world_markers[name] + self.position = [obj_pos[0] - msg.x, obj_pos[1] - msg.y] def main(): -- GitLab From a0737fc9494ffcaec752df222f7a64266735d1a6 Mon Sep 17 00:00:00 2001 From: "toropov.nik" Date: Sat, 28 Sep 2024 23:27:49 +0300 Subject: [PATCH 06/23] make first etap by sckislenie --- .../__pycache__/__init__.cpython-310.pyc | Bin 0 -> 159 bytes .../__pycache__/compass.cpython-310.pyc | Bin 0 -> 5226 bytes .../__pycache__/regulator.cpython-310.pyc | Bin 0 -> 4899 bytes .../__pycache__/state_machine.cpython-310.pyc | Bin 0 -> 10910 bytes .../twist_controller.cpython-310.pyc | Bin 0 -> 2274 bytes .../__pycache__/videoD.cpython-310.pyc | Bin 0 -> 5007 bytes .../__pycache__/videoF.cpython-310.pyc | Bin 0 -> 7049 bytes .../__pycache__/videoS.cpython-310.pyc | Bin 0 -> 6441 bytes ws_nodes_py/__pycache__/world.cpython-310.pyc | Bin 0 -> 4675 bytes ws_nodes_py/compass.py | 29 ++-- .../GetParameterNode.cpython-310.pyc | Bin 0 -> 4085 bytes ws_nodes_py/regulator.py | 10 +- .../__pycache__/for_world.cpython-310.pyc | Bin 0 -> 906 bytes ws_nodes_py/settings/for_world.py | 3 +- ws_nodes_py/state_machine.py | 125 +++++++++++------ ws_nodes_py/twist_controller.py | 10 +- ws_nodes_py/videoD.py | 2 +- ws_nodes_py/videoF.py | 130 ++++++++---------- ws_nodes_py/videoS.py | 2 +- ws_nodes_py/world.py | 12 +- 20 files changed, 186 insertions(+), 137 deletions(-) create mode 100644 ws_nodes_py/__pycache__/__init__.cpython-310.pyc create mode 100644 ws_nodes_py/__pycache__/compass.cpython-310.pyc create mode 100644 ws_nodes_py/__pycache__/regulator.cpython-310.pyc create mode 100644 ws_nodes_py/__pycache__/state_machine.cpython-310.pyc create mode 100644 ws_nodes_py/__pycache__/twist_controller.cpython-310.pyc create mode 100644 ws_nodes_py/__pycache__/videoD.cpython-310.pyc create mode 100644 ws_nodes_py/__pycache__/videoF.cpython-310.pyc create mode 100644 ws_nodes_py/__pycache__/videoS.cpython-310.pyc create mode 100644 ws_nodes_py/__pycache__/world.cpython-310.pyc create mode 100644 ws_nodes_py/default/__pycache__/GetParameterNode.cpython-310.pyc create mode 100644 ws_nodes_py/settings/__pycache__/for_world.cpython-310.pyc diff --git a/ws_nodes_py/__pycache__/__init__.cpython-310.pyc b/ws_nodes_py/__pycache__/__init__.cpython-310.pyc new file mode 100644 index 0000000000000000000000000000000000000000..6f3e9bb01dacb26cdc3b2bf0a4fb3c3601a3e9b5 GIT binary patch literal 159 zcmd1j<>g`kf&=b%(m?cM5P=LBfgA@QE@lA|DGb33nv8xc8Hzx{2;!HMenx(7s(w*c zc2#0ok$zErv5|gxaeR5Peo|>>P6~*fm!Fba9A8k0!j6y6%*!l^kJl@xyv1RYo1ape PlWGStyO;?`urL4s?-VA` literal 0 HcmV?d00001 diff --git a/ws_nodes_py/__pycache__/compass.cpython-310.pyc b/ws_nodes_py/__pycache__/compass.cpython-310.pyc new file mode 100644 index 0000000000000000000000000000000000000000..ddb0215cb347bd5fafdcd787f7a70ed7286b4cd5 GIT binary patch literal 5226 zcma)A%WoUU8K0Sba7mH0UY2FaHi*P1!ZwxdBuH8`b>cdXo5EF!+G$!M>4M^{C{oKM zGrNo|mP8+No0AVc7cFEU7x&OpZ~af^R`jHEE=3Pff8Xpb^@!Al#LmupzQ^x-%=Rml z62tEw&3|v!Rv7yiHIDymG~UJ&{{e+$l83B8ZxQmIXb4VqE3|r6!!mUq+6^0hcIfon zhTAJN3O%pknK36U_DYSCsk`A!uiPm2Dvb(f_n9n6?+KG$Aoi?ARTgFG32R&kX64Mb zPGmgawYY_xL+(k2hTO7CvK}TsqQy0$HTIHi{XSB{?I@;C4n}D)OH|g>-WlPrQht&Bp9wcLG`@Yl}aMPXu=0X5n)NcE#FF z&3!e4siyePof}*IUT{Now@1xS)eY5;U%9ap`#bTCjbSI0RF5Fh*dOeksJBv*)&{$} z^kxWIQpfFCqJxN9WTW|v>b0Zc10wMR1H_DnhlCl=qm{5dw$CMh#yMEo7g8iv*X}x9 zS6b2rIGks;dLb!{neSn|_>AqBq)W3&psN5#WF<2+DlzVtO<6Hz)sz>;tSe+O+jUl! z%&rjrQycT=Fgl+s$O_(zvTDXIJhh3I?!{zD&dNDCZ+aI@>B+^Xt{I(AF3F3SwJeuT zt-35P$z^%j^sbn4b-HQ=XI+t3vlSL7SOr&Cu+LS)6NCD$t?NQ2Ze8v`A2~R`B1w|w zLy{z6f()fD)*l%?Kj)f1K2p1?VuJSG44b_T*}T1q))y(3Mla-Q^ZicLNqisr>$jR= zd>fT?`e@}i6}46~s%-?do3*~G-5OoJd*{L34<6ikbgwog^vJXhh(rTNU6DbUH2sXT zukb>MFuW|IBKa6Nx7fDbFG{g(KV$-$27lK2!4|eOJErU{&;e6?8%EVd^nDADf#xzB zT|IGhE$Sz=PE>p8=F7ELU%y$0nG{-EL2Elux)dk{*N_B{6J5zj^^^Xf)A|a*x$wUr z;g3PwbH4t+Nm?UPmr%e$;dPmwNysau`|BC(4H5G0#P zQ@uz_&%h&XX@usocXeeeXiCJV{{yYBOYUnqx%F$>iH8FK*~`sYUu;7YGH5}qfj`K@ zrgl^-gflNr1F5zjL8^$D@4x>*6|tkfa5_$wXsfW_ZsP#QSc)PP=^{b5*=hx$u4D+x zB5OGb0Pu`kU6@whTYq%#{=J_b*_g3VU5y_h^ z{7snalq*#rNjS8wnnEyNrAS|TX)xSC@Y)KLzp47YTr7G9{ZlF{Enq1L{NVr!4fJAa zq`Ah@UdlM)Al!tN6n!7EjblFxv*c>kw^5K6iob!P%vT(jS4D}t!nIsZ|ElG2?{L|^ z#+Su4z9eSFX!dRvV{_G=03T_Dgl$4UGBEf^gncnV#GF{tn%EjQzgqm;{eO-=e@k6LQ_(=ZtSGkVB9sJ|gfvkX?>%k>1B0z!Z1o(xzFo<`*uVoz$lKz5o@eNKH74Cy&P zlLqWPHB?>&MQ!NmA}Nr zm#%H|(Q>ZRjA}o3kSEl6;-1mPt|UZKsJaZnaVqx(#BM>gtZV;?{aIv6qomp)u})Xq zq80J0(-ly{(F#fPU7DXib!S~yQs14q+|hz*)+oZ)vcB4ADee5UqmtpVexty8!Kf@1 zvI&lKXHy?gL5eYwu8Sklo|foanDl!*F*%i4z62RkS>oQ8qorJeCj-n0XH71HOu{(p z&rt!-xohTQG7YY(WEPj$gxW9(wS>2To8^fxZrnm$q;;QUW{d<5mXF0MZkQA5bN1b_ zH5Tn$`p$kKHH5@YJahA;QysdI?s4=Ka2Gw+vx>X-9yI6|jUg1*I}Z*I4~-!l{2f#q zVV+=&JSjafJy%ft7TbMX{76jr1ni8(BKsNa`V&DL9_6XeEmg*3ZN&qrd2&kJy7*>0 zh=Ruhb^9;0FG)}QW`&Jz=AzEQdLp?vc2UWfpHC~@Dh#cKj8<_LooWNqPEn@<^c|yX ziJ90^FQsuJ^#K_y(?~OC+(Mo(n26)3{xFhAkWH2mtDn<3AEJOrhCLH@>UJ7k+;on{ zI}KJ~I*kaY2-6qO6w6cW8WX!^ffU31%am*ohOIJxjpNFeB^qb0uU9E~jPP~E_jSp~ zcM&;6O!9Tv_dkKDr6X#SIA{|Gjsk^&>QILRqlQ@-7BdD0K8y>Ua%jUUlftMG(o8Hw~PtlAPp6L(1+hCGTj9*5P;ETdDppcAb zLM@m|>Yny()6e!DdQUN0cNN`f$TfEoa{#itK_}8>d|Ec$4!R u<_*27Pg@_Q6sATh!e@jO61Pe4Fg(FQlge(t)39`I^IkVeb?$cD?WdrfRy$5~UW6SPhDn<^9)Ii!kz4^1GSanaeZ;o8 z;KF*~HEm%F2QoS$CtSR9A}>6=T~QE4yz?J1hc&%BtWna%8*x9CaTJBJ&5vzDxQ9l2 zB~?%Ij@J)Z&z>1jr*fl#AQ(siYyjU>(HG-*sz)pKY`DLSs(0c?@H9_ZHd;kdD_qdaMOgd{ z*Sb%J8YbzPR{E-3iL*3Z%R6C9;6?>$Jm|J{enOtPgEGW(w;`O0*Uvw69g z0&f8~c^)VZ)e)_HI!DJVXXYTL=44zZ$&?LSF5;z~?O`9%i*UO&j8a`0#7Q^p z#{G#}-jqI57P$&GMv5=j>KsiY%Y9`|FVKparyi*?&16*Q}+h|##agkr{1qEFP;AXu<8YxV&*+Z*jZLJGroZt+PSlN>$aXxDNO`2 z-ip&q?yoJZ*`JI%T77$%#%pO9h4CPiEpoUuL^PR(qOquR$q>ETn??=2*a_1>gl)uy zFc?hJoX*L1gy>V4B|hzJ!phqH(XC&+bMsy4lfFv?mPxPGWVn^IWp~T?_jW9gSV;X{lF z7gRyMsq(6Do{fvjGw)K0T&#)*{D6;rRTB0dlb3~~{1pb;M`dCoU*$HQP#X9b8lJyj zH!T6rv zC$JBIba5(1=y~)-@Dt({gi^-xMKDY#0#cvJf1woY^ePt9!jM2 zjB=6J5s**oR?37kT74J%5;EUZkI~XIT9}C^E-u}3s&tL~H=zmT5j2iwp`otYbdh8H&*I_|%Wj(T#{m2@7Y4LzOiS3fF zSaLJxffUil(_j=^UE;rqTQ8VYcQVcvq8R|~3o zV2x|4Ci0N)p;|C2YUdd+nhgvy>wO2cBk``M3u#T&X_mPWi@-4ttC{?RD5*uBI>73WYL_+#R8)8%gmUE93%grd@@WT+eO z|NO?ycQ;KuXwyBKK7`ZVqM9{%1wg^iU~zl}(D_h2LXT~F0J9$Wom0aS#2drWZ2pAm z8zVt>+bnGj5Jv{Dow`h>OuR+g(cZ7H(aXoKPnNUeEQ&l^wn`E+yVgx$-QPcb`n2<5 z{nPjUG5X#0QI6{Kt0QkshA7y>t0QME>R%nXYyDXEu71YXI^Q$TAFs=ovAVG$Um|uC z_oS(AC|?~*UlcIqTLgYa;3fe|MX~{a7`T;ar`Jk%wAYRY`$@Oc*LegAs-ra@X*cTj z!Oo*8U`BmS=RfPZmNa0-h`+b@{{rzm5%(O@MFni%1WyRW!)~iDKRRtMiKgHy^aa zUGCVPb zWDgR&YB`^e=8uV)6*7$i6=8k+s}Kab7zDjo43Sv_?FYfHhpi|ZkyO28PHEiJ$U9<`OE_JW6FXuj0AY%aTUgXtRf|^~^vNAyWKD@Cg5h zq!uy^QOI4BDCd}dc9^60k)d393G1jP>!xM{lx2f%U;FsN#UB&<=37^L$<8np@x4CE zwqkqsm&GY7(v|D>?}1-JqI&QScm@1Ken@-3a5NqSjjGvH{*rnx9{ZkNpL}$!pZ@Pg zk}IGdNJW6Jxn!L_=}7UcN;L)Qd?&hS6+_w1_eb-jF%J5T=$f_5%P literal 0 HcmV?d00001 diff --git a/ws_nodes_py/__pycache__/state_machine.cpython-310.pyc b/ws_nodes_py/__pycache__/state_machine.cpython-310.pyc new file mode 100644 index 0000000000000000000000000000000000000000..5e8afc5dcb66f4ae2df89f66a6ae20bd356416d8 GIT binary patch literal 10910 zcmbtaTZ|l6TCRInU#F+%ZjYU@bM0kq$Bq>ukxiU9wqr=fS&w%YnuVg>r)H+xeev63=QA3f&((gozHm{~{)G~wkA=h~gy8eKrU^~xO|7L@^p;UE zT4u#;Srw~gSL~KkaayTLs^wN(o$49QbR~^{vzcjSE7>?N!+BOS*UDG&oM$%+t*OdX zt5_+vrYqB}Ql-?Ism#RnXDhSZ)?8%{|4wtB>Ty}BxzJjyEVh;^OPqGm4&K+bYnn)l z%tK9N{QRz2IfisrD?6p&Mhb50;26>vXq+`bJyX)v(&W zpv<7&R%WdesFbXVdbN#+@HeFI2g<4VD_q8oq4eOmY;?`@cE0% zzS8Q3+sb;i(`hREdOJM-ymHn;S#NKYb(yEW4Q1cDUk^fMztyR?Lu40_U88Z0KIu3S zJVeCTD!R}rhR`eKyV|-g3}HTWE0#DRY~eiAE4D}p7xQ++Nvv^7WPKNFn-+OLweE_7 zn0jbeGNLG^Q70=(Vg~;?F)QZqpBM9D0sjTDD3lo);!+pvNGw}QN*B3wiA%F!O)8yDN{?~r99T4J z>p0TOaa(C@0@YriUK%G*!{RwEhbOtU3^s?#m$>|~k@6*hb{i|=IK3e>Y}E3qnhnBg z=zFbdZL{9?z0hy^oo;V#jdEAw+!fT3p)ZuRcI(Dlx0EJn8`V^+a}RH9cR@PcdQD|E zI@M+zFLXOWotQsP=Qn*El=eoPEY&)#ZZ!x-UpF>;2Y7`#T0{7RkgSjB$NK8WAWDBA zA43+C|Gsu^v(xg=$;O>t^`1N@JHdr>_XF>KaPIb2y(uW(#`z1p?)F%GZ0={f+sb{h zS#8}G)t7#X`ZPc+t@q@Rna_G&yvx^d>rKS6nO%qcMLrZmwK$L&)mzIq?gm^<=`%j z^`@rV)qC{~205^hyyMHyJb-7D0nA_k;G(e}KXdcvljaxDe9V+%T_~H`{SmQV`4m!H zU0*79u=6Ut;fG!g+kZ!y?aqBQ9dS`@v)bOkNvOAc90XKpb~ZNfqE&CNcVr2TDR*!t zlttU8(#Zi*Q}w`;K2A@(;_@kaAr+5LO$)!)tV-YOR%Ny2hiKQ9wI(<={Z^7=-2&r5 zh}^n)ef643U0=O*<=T~-s#rtAIO5%{+s%5g=}TFpw&!S!2b;~Knl~JmOGz3m%N|vv zDu;CxP2b~9z(KbH zzqyY8w1-6}#PFhH!sp_1Xk#8Bpu>^V3;4N)YdAXpjFLY5oitDCC-tJfVyx)%dT-{C zL;XE8!cUFRX7Es)kl->x@C-<(_4QpHP^x}SGMF;BqZg?pCj@Hkp$EvO;EW3c8S*id87gdB&X*vi?Q(!>Kr&6HtO@RUq=Me z0imIX2B^7jlGJEedyt(CCzigHlL}qXbX3l7WcZCN0b7XenjYD%sLpECBd;pA zOpVrP#)F6A$;MqmqK{4Ucn$i-H=*qHO`&b+a;i`A+UR4`kXq=IPGPo9LA6n`XYG?> zNb=HHmBejjJL>yQ#G_g9gx-pZu2L?q#`xGK=bCQ&4??iH-+dKt1av4JBLn_*h=+B@ z$Qixak!dH@Yc#3BL-AyiG`POr&}5n@h7Vm3`fS3BhO)4{Rpt8a>g}d4yt2VesB|$g zI{6x!`XU1JoCBV-KJ6`xjArzd9z7rue9@{82cgSnkvr(w1SU-OP3!R`>YFB((Z~87 zg^;8B&Edt-C5VoqA#WmI!cZD^-wHM+yQdTL^9bC1PVXHbop!9n4B%=EDTv`r1U_udTSL6a6Ecqg{k>{)%a_!ivVqRO-96P+An3kYV6oMPH5p!+kIyj5O#OEuLJZfTKyqYI?5uo z4d&vL3%sc+}p^LtXQXe58 z?xZ8->P6sNLGKktmL1>;OYb_W0B%;xGo*RY=6asWc#zCu3&PP;InTShRc%Hkl2l|# z!ca0M5fZ^h4WWGih(l%FhLtJlIPd*iePhDlB2c zw6uf+R&tfSMyAKr8of1mC{EmV8Q~KeP6(;ifK(Iu1`OW!$mWZ%8+jg!;|d7A4xu#8iJAw&Q= zI1g{g!T|8g_Ntsi>yfGEtix=9M5Z+;yE32xW(VH-=aE$Dpj&Tyop#NSxQXyao&)(f zmJ#k&;7eloi4{;O|AAlI3jHlq4hkS8tO0X3Af@!cV2(LuKcygQahWx+_Mm(~M!1@wt_rz+ap%F8y<#+)TPD@R)8DW+(;Y|Bq7;Xpk zD8vj`B8H>%0fwVtU!sbyf=n`;FnwECz9Vcn*D?a^2Ig~xE7CwXxH&}@{~3`JdHiQZ z0n9k1GH=3%aI15TY%XHSXi?5n>^UOD*)_tN!9#Hh1cw2sZR2pbp>M#C`v#gnVtS_EL+&(7F#3AK?JIp%Hu!MH3SKo zLXw0r-1GsR1dS?FZ+Evs4}vjJSsa@|V&mJtN6&X&E~k$?LXj1bOkI_zqbgv^3pb@7 zY<8NW>`b1r@#UK2Av$#5L;2sd0>Uew%_goyxZ}U58vM$2RUY1e=oR2)8(Ho5vj%N;~dB`?8o|pK& z0*>d@m;^9{y~KTx?{g6j_ZdnVB9-KpqTB^dxHH-g#<@3xFdO$Xw{Ls^_xndiU-#zu zO|*xc%%By-vmaR~g>MtEfnMTLikFho#Xhx@<#v{!_G*n|cQpAsiJZdR<*@`ra;v@6 zn>W6EW#!fl`8HN{h#oAu9vPHJX7~3H@ci@3Jqxb2jh?yEO81PFR{8h{$V_bfVVFb8 zktC6KH1Ir2rHNs#vdqj8pR4I8(IuCBl*I;x{B?Tg8$^DM$h$;-6C_q2WDoIyUQR4L z$&>_aA0PxPAd3WSFa`(EcF|ad^Wr4Z83RD~gI;NbL1VZZQ_Ztzgm{!e;WrTB9b+vC zJEu2H!`RV@0~(glnLjLHENRF!VXByYeNh827>kqx1-fqw6Wrtw1@~Cso>V_2F9SAG z$F&`|@9=**v_q%w?!rB?lj*00j(vXxppseCkj}sqB?AG_LRJHBs{AdAy$AAW6PeJj zS$2+ZKm-Bg50G@(QO-sRnepJfGPV&OT);nqc!032j3(}8!~-JRAlR?TwSFJ1{5b-v zuk-LPl=NA^fQujLEo2H=dT)7TjSp=4n2tbd2k|Mb_9rVK;Cif}(C4)s6J7=br?!B9 z{RK|Liw%ABw^8rWUxM)HK91Z$DhK_FEHCa*<$}(Z1k^<-2Y9#DJis{wWq*zE39Zmz z{Jr@Dqi396g-l+jPKUY|2}Su1kp_`FMCwEcCnYOov|y6@8Cl7pdbW!K+QJ|M;%X;# zH}B5o3X<|`2xAW!KbH`Kmq3n?n6LpLH62LJ9A8(F)Z}YHNKL*Dgw)J~lL{&q&GK>` z-cWx+UgENjgwTlCqy-v06dza-0b#WJhkUWW+b>@^U{4FJ8q=N zmy~4@YDFTHbwp-yEd5<{beN*3(xKdlwmI4b0y5=PyIsF6hBJr|#XU^ik?9Il`D5xJ zk(MLdb(#ujNB<5XD1nf%LNbqFhWw?Fdvl}9Ih1@RY7d@KUY8#tSAIlV4j~!&!$;C1?%)(V*ptj+WB!w*9EA*p##VM4SVUgM)1cU?E^rqvq8K+Be+KSVVnrLGd zZLkGKsnO9jOh{}vdK&Y>W{KV48VeZzqF`Su86F|s{?UybZuijI@;)5Nz+OwZK+UgF z4Ss7}TBcWfS(cWUuiw1<#+6AReVZ0KQNJJ2I+jmFkUAj>zeqKS$wruELM1O!=}jVE zAaa8U@s0dhB8hm6{MK}u4^!fSzqf-3I{t*Ju=6c)T16s}{4N2HL}epQGCH-)K|?ko z82Tp!w(Ls!V#2iraBh)*h1d?5#|;34N$Cxd#3bOw{0A$!>6dVp|IIcg!n7&Kn zB0O=m)4H3>Ig)Y@ARkGHO9;U-$PoF20qx&p)0F%kFty5#a;~z$K6WbTeL8)Y2W=dM zSd2cfQcYO>k%ej<20XC~HbTwe+!CPpKJ`N#l9AP$ibga91scxap*S%c4~@2%Sr^y( zhd4EG^^M~K{~V5Q;=te`kGU*N0=_>(Q!uYtCt!|c7Cnnxc1;{QScws0%4G=K`F2#<&EtqX(D+9o^lTh5HbcpmnXL*79gG? zf7uY)4SdG{?-UH|9hhR|fY^X$PPX8O7N_m8G;GF+^l1IehZfKbk`P}VP(D7}KwHHR z?Yh209<=F+*6|s~SZgIw7)wvqhwL3GpZ(A#T-vew`ul&O4`((<)(Xw6krMOdK-ksA zf>^BUV(C3NTEleT-h&fC6m|``V!sYotOKnP8sm~Cjv-k1?B?&|_mB6T544Yr5LS>_ zj_aI&UK-UtnZzCg0^0)LFu)?>)`8a!+WA{?s%=6)Ei6EZ{>T`N;qLn*{pIc$-@-RE z6FuUKkwbbso%Hy45*r^y>fk8Qx_#6*;$EK!3%pK-m>-PsD{wptEoj$L9OAnsD zkCmn0FOY3GwO15R_I2^pzFD0Pr}s)^F~W&Gvk%Ri56;u8z1cH(R#wRWfv+r1t*lU* z&krMja$4+Gi1ckzzpqADS(fyWkY6H_s1306oo#r3al4&v`s<;08#fTR*JGRKxk2P2 zzKdeL%R8?!)ch+FjshW{q_&;`c|@Ka9N(k|<phl^!cV`nZzsi^jIgW7o-+L^F(25tix_EK1v zQwYjkyY_Q>Fd(R?EjnWw@vyX9{cB%53^=mN*PjaO9Fx+rvjNYUVn9g%x!D8r{FI z`SKp_hVboLz*A<`9-bF=d>TOu-Wyyg;3I=pJqYN&z7TJ3lD^lhiVGK$^3-?+rxWkt%&~!hYw~G2j{R!Ruv}C-lKZP9Q?E-psKRx;V!W%)c~OPCaqx0SO6}_hua@aaCZgc{}rV{62o~H*fM zT-<& zUXV^nguHy>$j72|PT(K((2i#1CDc9 zspu7s?<%iAK!mTn@F|f<;bDaxT*3QVReo1EQyl%Y{2!Fk8>Kfr; z@gf0LJ((AixUrQIc@vTD{@5*eJ1gn6@Puddz@!tR}N05 z(JyjW=IV>xQ^iizZg-rd9OpwQnPQ{Ko%wQLtqeGK(a^`yurU=fZ#t$FAB*@zNK+9~=8|dgOdpxG zBA7uwVLBfr@f(;RXo$%xB>x#quV}l#pbUx|G~@~{t03;dc!e2wq)BBBCT6|xBJ*yp z|HF?&0OUFfi}c&PY~hX#G`><8|M&3!ui1C6%}%#1_>CL>G?b2{7QDG(d^H{clHqkd zOf8LoYe*^vP?~#vp&1u(8cD%M*XP&xGESk^<;96M-qBU}IX0?EOvECYi z(L@~!$$B!+uH!Rh&@+pnsbJh`-tR+lUNY?E*lv_adqDj%V~~$hj1`5k^~Ne-&s3Wr zsx)v{X-LD@fxGQ)(=B(ynZtO~Miv}4Zl?M%^5pu#IX0977^(#Vn+hCo3T?oVYiD%e z4m?gb3CiehoF z3o5@4I{de71|bb-9e&dXmKC?sxefW-v)Kbf5HAjTpJ?ZtoDlf{R<7KHY@kWb^YWMv z+F(|?$%{j7vIz#7VUZKkq(~9tlaEn+h5~bwpQAwZmJ@1#?jslq3$`&&$oqTx-f~KJ zc>`Ag@O%hY5M!poSeElKaFdxTV?U3hwD=%t3e2}I*%Q8Ii4A1sPUiwZEl zO68Ym9Du^_j5W)Jw~ue#0WB_i(KywuYi&WzdSAemqUD6a;ZQ9Q_GH_5tqZw( iHz*4yA_+H0QJH)KbOmB_$2@Q@AZYtn)VNRIsY;6@*bYx*C0X@%+o6L%RN@-70!s)J-u#J zjF>mPMBS{IG0(hYC5irom#U{L>65lhr3-DQm#yb2xq5e{yWUgjVcNVVk|K3a6Dfxu z=#{+4inPewW0hW~Pjqc@IqUQuXwbn{;%7W}j1_5%<4on}op9NfcHIdbx#$ZAZ9Ox7 zy(yg_aKxRuU8APt`<`!yKY9afYG(VEbOoAqm6Wz{?M9&Vst*#ljVdWV|Itrgf-7Jg zt-^#>;a_R%OfbRkr7F76g>g@-7+-0IR!OXA;|bMUc5E+j8iBLw-*Owa+^e#4I!*BO zkm{g}o=`i`4x#XfVUIl0EG~3VC|58`^Jdnlb`W6Yjcsp zuiiGoj5ntz#enJiKECQ_=HHQAQNZ+SfGSJf{GGTTzx7oefBf3NuKFHmtx$|*f z_gP=hxxU`GFMrb4CwkBIU5NYo&c#iw1p!F8u~}iY%|gs7h6Pw6Gj)6S+(oc?b&htxB^2<@<2?` z@F(!ckEkIB;$B7He&Y7Z$ED1^Udoilc`SJR2Xu~e_d9gEMG!tn z)3bu-UvzFLR3Gxi)rZ#z}g9}t+iZF z5Z^!?2Uc_M34YrZjz2rm+*7G{JiERo?03g#HV$HF`&XZdxe3d18*XS>6YqF_6#@A! zn9e{t5!DT+y5&f0W$L+Gk-fC+zSFhK!WkFuXMkP@K`s$f1X23Bzx}<{mCn?$t?#Yu zg)?Q@&~Lg`3t{0%`?-2!_eJpJL$=s)+w*o!86n>F{!pyB)PUwvy%mI|HK!DW(rwfb zN<$D+8AnQATA{NW?(5a9t8b%s!W8GK0(NqarN_$Tw z+(!8NlnWjR7kqts-std{RA&Z~S?I(7)Ptu@aLHeftTmP*IF$_A~qpeo(9(DfTC z>(*;`vi#N@Y4RK%TA7~DuSh;qpb$g@}z^^Hb@uwyYQirkPU{~BHd*FB_l-bXadJQTO}Jkd7XV4r`D z(PK3FGowAy9&KN2k3|@`I4SbYA{x zxzE3j`eB(X;mc9~0SjLtO1=)SploJ3o-b+zElQ2RmIu7u6}~El!f`Pib;0)ZekJO{ zo=n&cQ39Lfq5-mXNIC~%ouDV9vsmS8@btx~7txm;(!>ajIeFJl-{V#OK#U@4O4xZa z0^PidNGnApSfLn|FdEWDO%%W60^hg=`UbTx0aM;Nwb#YNA7W2^2RIMd%UDs1lHyYF zv9(HqRy1*xI?a#u3n!!80XtyW6?P4P^g`Tz{pt2^4DsTL<)76B%RJx!9xpvB z0gFu?z7DD#z7ZKm9him!AqZ%^ytsNyr#waswbQeHO!y8})~MUT);K?=yd-Bqo=N$X z4JgM4l;cCn2ig&!;1Iwpi9Fpw`}KwOa@GWj;~PuAq0 zky2V&LEE2vHbtFJoBsnKaHVfKdjQDJHgdi5%(Bx^951^BCe%W92t)?P`A0N&G z0HGK`02k|?(~$3DfXdpc)WrUJB|c2^wdM(K<8PmXtK8uBu<##5e=eSP;v zY2PUA-8co8as_hxT~)skO5eL%`sr!M@du>fP5BmnRIz$?m9gNldQh`M-$EF4%xFqK zM442><)zi>)j8|!No!{5BUIJ$%)-*l2cO(onFAQuuz?1s#;R0xJDl-7U#i^9(!x^N zD$n1V0`65ww^6nmDE%>}d5L7Z*4%K%4IImE2$ik-vblkgwY|Ar)%D?0dHJ@5IacQ6 zI0+km!wFQP1=GBxvg>X`%wS`F3os#9o?9(jb9b?*#pwmrJ9B&b&Z0HFyj)(oYb`D< z&M5;WK2+wq>%p$d415_nLdB+mhG`%HxqwOHFp(b;H&1iwm`r8YfMEoHR@teB=dgu| z0CQ;JLfSiLYe_a+;VK{)Kne|0h4+^hSFPJ~<(1oWpIR$-K3rayyNi2Zt1ih4h$uiM zoq&^x`(%ug2(y%}>GIsP>h9QZzC8Wu6by+S__A(?D!FiHan72amPuMG#j8vad6QIP z2{ELBYqzIjXFl!W@yqdXR)(-cTji=PDRJJdb-LuLWYDTsakW*6&U>!9VK*9%*THh| z6Y`BC5G%O%Ds#heYa7_@jw`|qm20}Y7D53Yt$B`0)FHTwm^cYR*b082CDkaVx`7UH z^3UFNU`S=|IiBb5C}YisLUS#?+;&vN&SsWMc8+$SOxts7xLc=5t=+(?ap~wpMaO}> zx~q6~Pxg~t*q&lLifv%q6b(rj-NoIxlL6vf5QDQHL-s13;Flm-o?+bpXBlRgeR>9P z%wY8G*88}j=h-mJ7zRr{dX489N5U|f@yOH-ZtxTz#i*1~U;~WvKD_|R47h?ZnS70# z@z^};gYE(Ls%~IxH-D9tcsDfMh-uy+zGxV~-5>k^*%eq6NSqL-WvPs1)qTgh6E=; zLTwY}MX>vTE+IeA+6)CpCjbgigS>r&J1ko8Zv*{b6PF&UVYaLVoR!*j{{TRjFX36$*c+fqTy($!EwmOE`)%A9q}NR-~vlx{U`A3g|8bpiLfO zStfr^17R}i$NuRWG=8Bo{grdK;|bxc+bu7gc=|6ONgG!^^al*CT*#Th1W4xwkqtue zxQQ})CzGbD+tynAw@~K}k!ZB)&ApT4hC{v4Imhvdjt?_gJ6@^$9dYrt9usmB?&%#@ gze~j>p!-dVXR*O-yv-CVu)MDQBGZ#fA%GwK7q=tb8~^|S literal 0 HcmV?d00001 diff --git a/ws_nodes_py/__pycache__/videoF.cpython-310.pyc b/ws_nodes_py/__pycache__/videoF.cpython-310.pyc new file mode 100644 index 0000000000000000000000000000000000000000..228516d99da57924f6b95cbbfca304181c1690e4 GIT binary patch literal 7049 zcmcIp&2Jk?cJJyhHYt*#C|V!3$D{e!$#5}id$hqM0gRcEWsU4Wkqt@fc$ir|qxHHqjx6AeTiBael9w zlw@nXhfG4DtLv+)UcGuBzxUKBl?obue`o(@UoISN zR~nV}SYxbRZB&`IsBwqqA89=A>jy?-oR@fkk33?Hnty>8w{@}P*A6tS!B*8%FCT+K zZMn{5WzkRWdBSV^i7%Er+(+N&e5c(NejNM!PTOnwbzSB@2|Hf$!S71zPL#a=0VY}Z zIzg16;>_>d6amLHL*|6X122lD(d?jzgQ!Uxo&6buMtlPmUu!U~HS{Oi2IGwDkDP|V z4Q@Wt8YZ{6jkm>f+`-%Cd0xOf_e3+bhO?^GN95Rw?}f1+#r|67W)OK|ugT8rWuxxk zp)KPtF_LHp+95VNV&H|IYOc-=l-Lj@o*sIjS&@21a;O{lSpgRx0| zNsV3RuRSySIQijJGR?1~7S65n*N=#bi}>Oadg*x166 z_}gcCXOcG#FyDRigv;N`_E5alaGmcgOS|c{n>(M>jbjpy;|VH$h5{X{RDX^_jG+h~ z=$f`~-Cn&vQ`k3V3Nv+G(9IwJ6^gnZ{1C&z6negbpz>s|>&w!n57~@bZqn%nO_>`s zUl15tf*#uJKS`TYRKH@&U(q^jS4^Wz760||warf3zb3Y}_q`o)O?2YfYY$`hVSH`9 z7lfSZ5tJ!*yL&IxcLMHrKDpZ6lg^FMYp-+f!`Epu&a2oP3|Qg-bOoaogPD6#;AA=1rZSw5w@lnNxIzJ^qbqh zkmg1&0>ik!;q}5q>f7(Xa%aUfvNB)k%WK&_7io(V#+~l?2K8S-QD=hcSaxysD z?02Y8OTB8*H)#efDvgcK&Np7}t&`>0_BUQk9YN7nR3hj?*nQ!>dfiFdo$l|qpZRx& zMD9nEo&~lrdUX=8N8wdNoEVTT@4LhwC=}r)Bbfg}2rR53X?MVwx^SiThM05gt zB_dzM@?zXukDDS`S4`W$1$BoWSy&)UUZ54Z-wZtg?hePKC7K~DN&dutOLL7(oqg-> z_iipNh_~rbBpxz%XL)U5abZQgOI^S&nzUEfR_-h>%1TrC>e71aVGwUZK*(K6quX1T z#j}LTV$g0uXv1*bYi`R4OvpZPH$=bmDS|Ru*RjSZo|sNlWVUHDn_Xd! z;js7E72~pgnc+{rtWW4uY(k%44&M70Uu|ETWc`M}8Y)EPJf8S3F+$e#kflHlKmhRi zy21dd@fS=irsOxxEu8~RJl3Z*-~+p#ON>J^wGv~?I@0?NT6XGifSMztpC7bro?km66Zi}09EI+EIHE;WzLekm<+t;fXUw1l0?{m~?!jwXd<1T+@Y z@|1SK4pZqep=(jOE~|ajl0O&8#?Aq z@DjK?uI>fp)+Fv_JRMI>UQWm9_NKKdAhXA~MW90J!FrS0V<5a6n%ZXs>yEs%54Hc- z`<(7_x+6DgC%c00Q#dKf-Zbt$!>#b@98Y}|9+tq$F$pdZ5?SHn-=pGQ))C{S(2^?Z zMmGD`G~)-^jPVy|04B7(c>7qbN_yPFgWqpHIwmj#_)voN@i@R7ZTt%Hf>B9E;EVR}K}P>}ml7Drq{O|m_)NblUhHi_HR zvJuLVT{b|xERZE!apzVR=43e|VR2>d(^=3VNZWCaomoZs%g3qL#2;dUAgClu&7Kf` zl)(1ENXcB>Yc>%uNh@m?iH|-9BM?4Gd(#hEn~Ag@20Yo627HLLGwUc#vWl`au$Aj7 zhyf4C&kY&|$d_e!7y+h$rHUOz9>mG8r4BCwB5sC=%fTfDh=*Q=T?-hT#S^cgDCz}R zOw+2evQcJbR?#c00_*9J-DFcb*;AVp(SN@0UBhJd^RoUfdsokDAF!#S`S<%*&fC|s zPU8g|O9DpTP1)F6sHAKQ(do3-*AupdNNP*N+u-^$*jng^UegeDnTHnQEp5w&J{U|x zJBOIdfG%691+6fb#R}FEB-%ih6fD#o;ZfHID5SOT1FTD{2VwYqu})piMi7SgI^mx9 z1DdvhqHbq*Eoi*d-1MTz4>K<>8q_xyw4x52G2JuzA<#mp+lxR~7GlWndPi_y?BKJw zh@!x%U~&blHQD~<^9(!vA>g zx6p4oRXrdTeiJOZnvrBU~VM@9omL173%ly*Z?F9gm)q+YU2 z#!6}K&AFweRl1{?fJA|`ikK^cufemY`cvq~ zC9Q8g1s=iM=6XNJjlP2?yF4WjxIpL$a6$pN!b0{(Mr!YGt(=%E5OIaG}%ZU81Uxx-NiAhY_;c9V@7Mbff7m~;lpY0|wa)lc^iFBp zTBPep8x^%~{bUvtWiacVr+{l`XWhr#&tZ+pSqT-}65bMjf?`>b3*PBhuOk@RL)f0s zSdWU6tC9H|EkE*iyW+#2VIU@Y^&d90{qKCeO(DQ==8A^jLL%P3E{GZ85XCXD-1DJ{ zCby~Vv?5EgKR%}22V(ed9}TA|!fCE#w}g8-Qh5k!2x6D0&@ zWNBq#ZN*)906exlwic>15g&Z@E)E}0GybtAr~Sx>D$^TcbC`P z+Y2kJw--KjSMU7My`_Z*;)sTfxYq{15bhhmR`tnDS;&W-xu6Uzbc0;=i96Fv}q`X;3{Ri9!6b7d|Q;#k^V2xt+@JOMm%IWZD(*?HA5;gohy@t$N4 zCEG-H50f+5F*1UbGAprB%1qbsBRnzn7Z3}Z03RkR7$$R`e_Jmy9f|-n_}ny2;Jqnb zX9c~cSB)Agm<6WmRr7id^Nb3+2wzrZh^LvUe_cF1Gz#@RrA;V=cU@U<-FAofLaG;C z_w$|?W+Q^)c(Fi*^893R1W_wXgQ&lWEW$AIS1Lf!O_oL^NL0q7V3mU`fb*7f%W*1W zf=15iHJPA!nofHr+7`?tXV2hH5*-0AgW)s?Pa(twf;xhhqQ|1Q3b%CnZ;9!NRt#`> z11nH&CrH#*NMm%76DlI%i(B4PsbA^Do4tg09!AjOf28>ZQqQ{(!}onjEG7ksz&vpe(*h z8DL@svO5Vs?IJ6ZWt+xYKHNhh_D%=MT4vTZ-`sK6)xUePG>#SZ+TFbjTjLZky9x@b zIE~Jt_@j;?{(+k63Qr4?PNc#!;rNj3iSh*wL=vF~9^!(-Y6wlV9~5pnP60ytKhsr@ A!T5yAlhvYH zYBo95Em`6;vm3~hSp+bU`^JETV1djY@&_ys>>;NBIp&U0J3Sn|g9HOOGA>MIwHFhgy* z%;eCb7e8@@+wfvfEH}A_y21Hoqb0m3^7x~MTldPkOn(wI-S~qa$<(7TzViW?Qcs$G z7$alP@7xzY2b&?&!sWgjM$)J?K_Wk_(LkqvMz0Y42pLbSFs@bfZ?tvB8P{Lf6@wex ze4$lLp5hkL6i;&-sr8L!YL)b=R?f)b70(SKFO0mk=6yeO#a@k_8pA@~#z#X%ze7u` z?P~`Z;E+KFdZIZxH$X8ED87T#p>|;LRHAL_zlP3{8o$OByB1v!DZzmW%UzfakC;1?_<-^u zk-q@(Z(y(6sfVjiOWC$j%9hHyAiFsFEl64S|A2yD1o;-`%j3P4cT_=(%x`!w+pzA$ z&6Z!2>0a>-n<6>%t>XJnI_L?q-?HUzNf_G|=TN1{|9pIFquKCoiOsFHyCZIiW;A>2 zdE`8gZdJGafKxuiIg6au-W&NHpL@;Ko2@-*-woVGmAfBZp}}+zQ)}NiWAis1#}ED3 zacRi;vxu5BBDuHn$jj6 z^V)`2+wz1o*SA9`jC&`nzR!SoW^sTQV&})?MqoQ}4uQBC+&rM`9STdRME3v!ZjoaA$_7Pta> z?zhR5s0DB^nd2K@nrm=_(t7at)B8&cLY=%a2}nO$URzjPSP{xLe@Io<>e|Yq~=NqqYc;%U9{3bS>{epOy>MX9kv_=FsT$twQFz zrQRlrG;!!vQMRNRdBM8Wckf7j58v)A(pj199vd=mxZ{lDVBt~ZINjr+Tp8`&AijtW zMUGjv#VmG#*@n$-FiXFn&*(D@f22jd$R_j&HeuMvwWr_hd28es`#)L-DSsYc^Z+Db zn+Uh2w2mILO@s)W8d8JnFA+!}0MW-ZgcIgL3W0>SX(i^N!8DZ92qug}9oKOR7ofR( zM1zPikTN@ZO^FSo+i+oZKSoKW+8!<$nc9Zg{1}nf&aAZ8{UCVK4EDrNh-0AY$It!9 zgZ1#TC4Q!Qty9n^^R*2(485Rhp3-c%(Uu(c>tPdyLj^PhO(AoX_qgLzK=_u^me~l_ zR&5IIsqvy52FS7zX0sudV~cdp0@d= zxrYmI5Uo}K2Z7t6j=k%40P`$5^@}Tu_h&!*^r=ky;R?KkhtX)m#TOvb2;8d7V7I%w zE}1J?RiAtblzl z=vg+T7t|jXP&#V?Z+3sf3g~L1D@V!vtr>jkzRx)1(}r;%p3 z)yd);sB4`ZzI0 z#nVJdr|~48NHSdw7d5OOn&zo}y)zk~se94r8fWsOtccIjwaS*MFD8 zJm$E}Qzslx!S|)kEhr~_^a(_(sresbNO6;jzX0+736ow{ZW+tbZ{9{cwD-1`&fKkg zp|{%-9|hipzr7Yrg#T!y+x zPe|L?{jk(FOM4%ddZ1tY3ViK6qHgSNDQ=dk&F%hs0L6mkTVe@`ES{ho1UGenV89Uv zL6m5txoeBLbwA|u%`hfmGQYC0w&E;2TU%IJo?DW`^AG19Ejx2ho~%56<}5#6UZ6`f zilv1h3=5G~)D*GDag%o?%n8pGKPTZTwoYgk2G%Tdm|{o|5i4FTJ{5}_Zn!5cT>W0i zWsVE?`SDyb0<2w3kZx_qYd~)Z@=1&w`{eQRn)7gBW%c30XU^)QpFLSxc!oE*?FOO% zFNB2ynHWGOB13agp-qu;cZtyLt0Hr89y!VO#7`;3AS&pW7TyOHkRhC~;>@iq%!v)+ zD0G98#g)0wW)X@&3r*2*W0_uhw7lTV%?Yw*Xnki^>G>F9_LN_q?nWz@z@;Vl?2Ud}Vt{ohgEjK`n8_5iD zQKS>$QKP#(LT!j%+eJLIr&ce1LsMYSCELJj32m*Kj%WtY#ARwqLyJBhBHucD;vpKz zn?ySxIhMuWWcKT8`Urv}TQ^anzm57a{73XLc1_n&L!7!kW?p8u)7N#=7-G{1sYZZU zCYb)q%lc(DV-S9^5MEz5OlG|<=+{|EFEI1^_i5>IW{ zG$N?}j4LG{Fi*_4L1Jwaw*+S|>1x$+@iN>SaGEOMLFj)72Z@>#r5u+_S*O=*(-rjy z2bQN=`9-|9?mDa zNzZVk3SjA&pHz`^>|*=jx`(SV7JDa+1Ud31zPFtatA%0IewSgyupflbhyH@cLdGd&=Y(0n76SyNS*s$FpL;FScoNdFxU;i&l C+#Qnu literal 0 HcmV?d00001 diff --git a/ws_nodes_py/__pycache__/world.cpython-310.pyc b/ws_nodes_py/__pycache__/world.cpython-310.pyc new file mode 100644 index 0000000000000000000000000000000000000000..ccfcf75f5e0c34ec176629c3b7480cdb659900bc GIT binary patch literal 4675 zcmb_gNpBp-6|U-Krf20&k<>O;j3ASRv@J(Y0!52uM=&DO3P{7q#6hDu)f_h2(>+;Z~SIRrU4`Cj$Rkd#CrfIUQ2SG`yD>eZ{aGOg9h z48K1;{P*UKNyh$3lhZ#3lRIeHU(g9Ac+6V#wPN12S{A3V9oy^lbz-OMv>XF-W4Bvq z6}rV%vFo)w11rR(Zn;(NR$3J^FUHkwtySyRTlMZlYoa^Zn(R)srZ~ILgeOW*nJ9(U zq1~DmWl?#`S~KCSsP0(uqj2VsfdXGMty%jDys_0L*OT`{^)Qe@H&mfqO+^S8{~*BBCG*CC*)rB=BqokwVj}rCvji#mD_1!2_+I^TtiE6sqX@^m)-OV^nr7p@q zL_v~iyPcxTqNGhnp8naxNp=&1khQpAElY4=fpPm8+vLI)&Qq`D2v-yUb45{j_!dM- zl<_S-V-9P1Yphw(?q{itMVp_ZPp{;m-9yX%0L;Fp*dZIRZN9(`_-8Dcc9^pEmN+EC z0Y9>j*^wjoF^8nA1O8i<@LiVg@wsw^tqRy(JmkWeWf#~XKd^-RxpiP4IDmUcrDOI1 z`+eiU9oPfcVK&?0^7g<&e=otiC)U6{um{c%=gNA)J;3ud;!F47IWfdlMt)dHKvK4D#8qHt&(7#@YS!{lI4s4&b6s;nwlT|MN7 ze3lK+4vK)*UV?g9RTq}aK)v`LZg7)Msf(h4csWE(9I}Iwn#|WOLJotHKQ){ehx2J* zXO3nEB_mCSNAgzp>zN9qYV>;|P*5dF&-K+mT*jh*Vc4`J?1@ExF9K>2itx=jNfxC` zagZsWlv3C4DyZT{Uxgn^30*69LgmM4rxQx;M9F5VU77Y1q3vKJ(?zuUi#kaVYdcPNwX+p`1d))fQv*&FO;|1=R<^XobDf0rKx99n^HI30E{aO1}V^qsIu`{+src1U( z-)tSvL{@UdbK6lCJRAF?MH=ec3bx^seYmP1EHY`~!cP@U4}<=)K(+^Lk<40D47~ah z))O^R7ZV0P`4YAyro^l^J+(c(JtHm|+~s3O{#ni9j&lm9_!qEh0e)mr zTrrw|RTMA6yCem*WY%2F*KA)>o@j_ShV*SfFJtEw@Nso)G1peLyGw7XD~epLuhL%B zmWvSjnb#mQp|*zhH5tnQ!{>a}Xw!2puL5ModAE$I_HM>OcS8iX-la`sZ4TRSVURCA_t=cI0nEd>t}%HX69kAt93^2O&G}&C$AG@ThWsgD#6d%W2|w0}0^|nVxzH@vZrPkNA46$WQTqix!{9zoBIKXSyQ841vHH z$+u?F97rrSx%@dky6_zl{smo!2Q!Y`%9-u zH`+nc*a#bMHB#Ald;jviyASSt`rz*RhmG;+evowXz$mAQwfZQ&eMgtR52EJM%?1Kr zzX!|qMqWneS;07@%FDz-%{acHCT4mm>u+Rj1bw5Qn+W5*QPL{O^+@Cl%miym#;U8Q z*Xw$&8SrKCM$q2TSI!X6?}%ucQdDB^8cR+faP$y(x}phZWn@Xy-1%mIHow4GB|38ry27YsWI@l^GrpM@Gaj-F z%dq;Ajgx85&~cRae!-6rWWHd>rbHY?QLZQQmpZWza!t=I?IbAL@~cqRDD$vB($n7% zS(5G~-~Un0|0}flB*KsOnYSJ(M2F~ZQD;urq>)hr$}~t;#yQowW9=`Vdq$fCGy2hu zX?_naBUgyh27wjtjl^i150EsXM6v}(^2oO>6g{?}M3Qf#0P0|^fBNlBx?nsO-unYk zlOI!O@|^~a%zHbh-kWZui|NKTi8H^|Sa>o7@>8_o>zDa8di}ZlW-q)#vqT<@p1}8Y z*@sbzeoW)4??37XagH(OWsF9WxFpHV9HVPP>uQH_JhCvu$p$SkLY}~=f|gMp?=g>8 zuX&d}chZ%FX`_vQl>LpxN{X$F@xW{>)M_+ChDl}wqjHXsECS-vmy9S+*9wxZPW4ly z%mJupdQqaQC^VEz_e{y5y=<$mM7o_4;Pn9yu!z6F{{tN$+kfg@&*(X)cGU7ZHkl8Xtf?Z0H^CGdDDfq_>2y_yK zB{Si`L4~iJLpfZ5o-EI_YxLyP502 cEqaV>0i9=6&E;p|884xr@*aA{GI##_KY&&h{Qv*} literal 0 HcmV?d00001 diff --git a/ws_nodes_py/compass.py b/ws_nodes_py/compass.py index 4473358..03f7aad 100644 --- a/ws_nodes_py/compass.py +++ b/ws_nodes_py/compass.py @@ -34,7 +34,7 @@ class Compass(GetParameterNode): # (in degrees) self.ang_accel = 0 - self.angular_speed = 1.5 + self.angular_speed = 0 self.test_heading = 0 self.heading = 0 @@ -54,10 +54,15 @@ class Compass(GetParameterNode): self.create_update(self.twist_heading_update) def twist_heading_update(self): + # real settings + # default_target_angl_speed = 0 + # zero_throtle_multiplier = 1 + # full_throtle_angle_speed = 55 + # sim settings default_target_angl_speed = 0 - zero_throtle_multiplier = 0.6 - full_throtle_angle_speed = 55 - angl_accel_koef = 0.8 + zero_throtle_multiplier = 25 + full_throtle_multiplier = 25 + full_throtle_angle_speed = 14 try: dt = time() - self.last_time @@ -65,7 +70,7 @@ class Compass(GetParameterNode): dt = 0 self.last_time = time() - if self.is_use_real_compass: + if self.is_use_real_compass and False: return # linear interpolation of target speed @@ -73,7 +78,7 @@ class Compass(GetParameterNode): target_angle_speed = -self.ang_throtle * full_throtle_angle_speed else: target_angle_speed = default_target_angl_speed - + # clamp max speed target_angle_accel = target_angle_speed - self.angular_speed @@ -85,12 +90,16 @@ class Compass(GetParameterNode): ang_accel = target_angle_accel if abs(ang_accel) > 1: - self.angular_speed += ang_accel * dt * (zero_throtle_multiplier if target_angle_speed == default_target_angl_speed else 1) - self.heading += self.angular_speed * dt + self.angular_speed += ang_accel * dt * (zero_throtle_multiplier if target_angle_speed == default_target_angl_speed else full_throtle_multiplier) + else: + self.angular_speed = 0 + # with accel + # self.heading += self.angular_speed * dt + # no accel + self.heading += target_angle_speed * dt self.heading = self.heading % 360 self.pub(self.pub_heading, Int16, int(self.heading)) - # self.get_logger().info("USE VIRTUAL COMPASS") def compass_callback(self, msg): # Проверка на правильность преобразования @@ -109,7 +118,7 @@ class Compass(GetParameterNode): self.get_logger().info("start median") self.last_compass = [msg.data - self.compass_delta, msg.data - self.compass_delta, msg.data - self.compass_delta] - if not self.is_use_real_compass: + if not self.is_use_real_compass or True: return # median diff --git a/ws_nodes_py/default/__pycache__/GetParameterNode.cpython-310.pyc b/ws_nodes_py/default/__pycache__/GetParameterNode.cpython-310.pyc new file mode 100644 index 0000000000000000000000000000000000000000..23ca236b27894714d2a9edad37c9bda57fd7c77c GIT binary patch literal 4085 zcma)9-ESMm5#Qb0J08!JEZMSSJ0HC?b<#^*DQ!_8MH1I-;|fUu!mtDsAzB=$+oMG4 zNXmOh%OXxdW2vx#GzI!x6g812y%|A`xJ~T-3*_sfZ+*&h-wG7%%pQ-FNILBjH{9LZ zot>GT`ORE=Zq6alZhiRo+P_Z`@(&ywZyW~K;EC6P2qI`iD)`SL+GG_*v2R31lUKOz z^T=#k6{}gO6ew9G!W7ma5msm)7?mQ73&Mu69Wod@g02y-_!V#_WsfReZi^89U%pY5 z)n=H4^2d=3t77lHFo>!$6xZ(r;Z9O-w>+lI_1$`$DDy_U-bw;walBC&T!SYr0SQTk z3Q}SEIHoQR*^N#au`PRsvVFe=hx@*Ae81Tioe29S-@nzVM%kMqBGG7?{Ojh*R=XLl z$i{ZJdRwl@cKp4S-PqraS2jBJNMOGOYQ_G}-if{lYt>GatgME~ND$P2d1p^rz`cml zqxEgVsh*iq(tx+ua9IV&NXaF^gmFkKhB&9KPb*xU*LJ5B6XuG-fwz`86&7ktl*HU2 zuh?Qo<~Iq+a9?a_v@Z>C0S z9?WLxBwnYQL*}EE%Di2TI-zoKHfnEfhEkcKlx?X!!m|{Q!>Fb>I;L{;K0n#p z2~{E33WIG}?&#%y65dJVSvX-*=W{c)nTM6}3qTlMpe4#F`;Tqd)MgGXF`Ld)8|J#N zu1-4dOlDuF}}ojp~&k>7~o>u3pUZT;r^#Gqr>XrYwRfZ7?PN7l9-1v9zE~iXd!tmf?~dci8%$4n7*(?LX{4f#I~YAE7Yr;1iJUsjE|f3sT^mP6St6{Q{18 z2y#6JEj}Jg{uvB^e`U-CxPnH*Ew~R3`)&WPz!+G5j*30h!h>RiyV!F@HK~rI{2y?W z0`eTua*j4eGv1u=8F?Bmco`nK0Hpiw*Sd*cYs)b(Ory=Su@3_c<73C?Jv>Bcd zBe0b$o0X^oXm)|UK)Y{Din{!49swFNZ-{6()+mOeO`sU2G(<6Lh+D7g$Wx?m}I6U&X2bHHQ8?F8S4jd_z;@rY4^1$-DmHT2+{%G ze%?@dbOf#Z&!CgC13(|ZE8_t0Y1@!!@Ze_ z&j2PrgcZ6(v>?gZbb-x7YU?h|GRg_%(GVl$mtde_nt{UffTCJsRj3x(< z_UsAgUGirH$WLIk4oAo&m!~9~wIeWKOWsga9ODzCsQ1V{dXLo%U3b|~cY#ZK#x$6r zepoQXXMo&i55T+c)BB`%Qag2dMx;+ixdxjfBhy;zk9ju>-mLV>@c>;(t9yoa-=3lF z=1deB<6+K6^Ulpaw)l@m$6m#rKKsJ6u<1I=f(BfmoOQkbTbcmTGjG_(6t4pXF&EGP zzN3Ib;hX4sr(p_1aQH4Vd;EDQo3Us{5Av+C#3D?wt?m@QBVS$HV!e<8$88QfSXpAV zm05Of)YFcJ%4&rFIdrs*ZPI;nN~lbWo73~;`rPNDL%*rew}t-`g(!E0`M7E&J5?7 z 3: + value = k_p_angular * err_angle + result.angular.z = math.copysign(abs(max_thrust), value) + else: result.linear.x = max_thrust - else: result.linear.x = 0.0 + # else: + # result.linear.x = 0.0 elif self.type_move == "MOVE CIRCLE": point = [goal.x, goal.y] slf = [position.x, position.y] diff --git a/ws_nodes_py/settings/__pycache__/for_world.cpython-310.pyc b/ws_nodes_py/settings/__pycache__/for_world.cpython-310.pyc new file mode 100644 index 0000000000000000000000000000000000000000..b7e36b1ab11a8fd16c2fe17a885fd00df7c030d0 GIT binary patch literal 906 zcmZ9J&x_MQ6vrpiHc8W_KX(0f(Ss)gVnDBI_79Ln@D>(A+L_MArkOI6TDpIPUR8Q? zFa8OFC;tM$gBUyr9;6_;hzqN*)Hk#JvGqOj;mvp6yf^d0UDrWi&mW%jj>-sqRg3dh zgT)Sf@+p9TGfWIkyg>{)I1p);rdGJf4-HZdm5xY-nBW$%NtHO@wW$-9K(7;bio>$b zX^IX9~JdZ8h86qHxb^ z9p88xER;_H)56}jfc3Qf2-xB9p7Zh5QQ$d3jc!(k+a-p=b%mRN*^a`Yf^7j(h4+Bu z`+Pq4urS~tyQA$xz+eIPC16k&ivL_T`?YNLJMb*sOw=J!xC#x0rb0`htpIO90@@$} zt&vwMTs0VYuuSrVXN9ZBvRZDf2DVDs_3XkObCHs05Q{z)vS=&^z9`Z@pn^woNMV#R zVOg9OO|J5J&c&HtY_EklV+*mF(eY;1T3BaY+vpzUe3*1)G>k<&pt+E*k+`ZJsPOlJ zzt0EM7mxb~@e|<-E^qr|8I7gi86_!EamERS!~fV$8RH5z{&({jw=gzOarOL;d4gT7T1s_J{{fAd4|V_m literal 0 HcmV?d00001 diff --git a/ws_nodes_py/settings/for_world.py b/ws_nodes_py/settings/for_world.py index 5b5a391..da88cb2 100644 --- a/ws_nodes_py/settings/for_world.py +++ b/ws_nodes_py/settings/for_world.py @@ -18,7 +18,8 @@ old_motor_speed = { 1500: 0, 1540: 0, 1537: 0.10, - 1574: 0.15, + 1570: 0.11, + 1580: 0.11, 1600: 0.40, 1700: 0.73, 1800: 0.95, diff --git a/ws_nodes_py/state_machine.py b/ws_nodes_py/state_machine.py index c91ab32..016ae49 100644 --- a/ws_nodes_py/state_machine.py +++ b/ws_nodes_py/state_machine.py @@ -1,5 +1,5 @@ #!/usr/bin/env python -from math import pi, copysign, atan2, sin, cos, radians, degrees +from math import pi, copysign, atan2, sin, cos, radians, degrees, acos import rclpy from rclpy.duration import Duration @@ -29,6 +29,7 @@ class Controller(Node): self.goal = Point() self.move_type = "STOP" self.is_real_compass = True + self.navigation_marker = "" self.heading = 0 self.declare_parameter('move_type_topic', rclpy.Parameter.Type.STRING) @@ -42,6 +43,7 @@ class Controller(Node): self.pub_move_type = self.create_publisher(String, self.get_declare_parameter('move_type_topic', lambda x: x, "no move type is described"), 10) self.pub_goal = self.create_publisher(Point, self.get_declare_parameter('goal_topic', lambda x: x, "no goal_topic is described"), 10) self.pub_compass_type = self.create_publisher(Bool, self.get_declare_parameter('compass_type_topic', lambda x: x, "no compass_type_topic is described"), 10) + self.pub_navigation_marker = self.create_publisher(String, 'navigation_marker', 10) self.create_subscription(Point, self.get_declare_parameter('position_topic', lambda x: x, "no pos is described"), self.pos_callback, 10) self.create_subscription(Int16, self.get_declare_parameter('heading_topic', lambda x: x, "no heading is described"), self.heading_callback, 10) @@ -55,17 +57,25 @@ class Controller(Node): self.goal.z = float(z) def update(self): - # self.get_logger().info(f"{self.move_type} [{self.goal.x} {self.goal.y}] {self.is_real_compass}") + # self.get_logger().info(f"{self.move_type} [{self.goal.x} {self.goal.y}] {self.is_real_compass} {self.navigation_marker}") self.pub(self.pub_move_type, String, self.move_type) self.pub_goal.publish(self.goal) self.pub(self.pub_compass_type, Bool, self.is_real_compass) + self.pub(self.pub_navigation_marker, String, self.navigation_marker) - def next_step(self, move_type: str, goal: list, is_real_compass: bool): + def next_step(self, move_type: str, goal: list, is_real_compass: bool=None, navigation_marker: str=None): + # self.get_logger().info("WTF") self.move_type = move_type - # self.goal = goal self.set_goal(*goal) - self.is_real_compass = is_real_compass - # self.get_logger().info(f"{self.move_type} {self.is_real_compass}") + + if is_real_compass is not None: + self.is_real_compass = is_real_compass + + if navigation_marker is not None: + self.navigation_marker = navigation_marker + else: + self.navigation_marker = "" + self.update() def handle_start(self, d): @@ -105,7 +115,7 @@ class Start(smach.State): self.pid_enabled = pid_enabled def execute(self, userdata): - self.cntr.next_step("STOP", [0, 0], False) + self.cntr.next_step("STOP", [0, 0]) while not self.cntr.shutdown(): rclpy.spin_once(self.cntr) if self.cntr.enabled_: @@ -119,7 +129,7 @@ class Wait(smach.State): self.time = time def execute(self, userdata): - self.cntr.next_step("STOP", [0, 0], False) + self.cntr.next_step("STOP", [0, 0]) self.cntr.state_change_time = self.cntr.get_clock().now() while not self.cntr.shutdown(): rclpy.spin_once(self.cntr) @@ -127,33 +137,64 @@ class Wait(smach.State): self.cntr.get_logger().info('finish') return 'timeout' + class MoveToGoal(smach.State): - def __init__(self, c:Controller, time:int, goal:list, is_real_compass): - smach.State.__init__(self, outcomes=['timeout']) + def __init__(self, c:Controller, time:int, goal:list, is_real_compass: bool, navigation_marker: str): + smach.State.__init__(self, outcomes=['timeout'], + input_keys=['sm_counter'], + output_keys=['sm_goal', 'sm_pos', 'sm_error_dist', 'sm_error_angle'],) self.cntr = c self.time = time self.goal = goal self.is_real_compass = is_real_compass - self.distance_threshold = 0.5 + self.navigation_marker = navigation_marker + self.distance_threshold = 0.1 def execute(self, userdata): - self.cntr.next_step("MOVE TO", self.goal, self.is_real_compass) + self.cntr.next_step("MOVE TO", self.goal, self.is_real_compass, self.navigation_marker) self.cntr.state_change_time = self.cntr.get_clock().now() + while not self.cntr.shutdown(): + self.cntr.update() rclpy.spin_once(self.cntr) if (self.cntr.get_clock().now() - self.cntr.state_change_time) > Duration(seconds=self.time): self.cntr.get_logger().info('finish') return 'timeout' else: error_dist = ((self.goal[0] - self.cntr.position.x)**2 + (self.goal[1] - self.cntr.position.y)**2)**0.5 - # self.cntr.get_logger().info(f"{error_dist=}") + userdata.sm_error_angle = f'{self.angle_error(self.angle_point_error(), self.cntr.heading)} deg' + userdata.sm_goal = f'{self.goal[0]} m\t{self.goal[1]} m' + userdata.sm_pos = f'{self.cntr.position.x} m\t{self.cntr.position.y} m' + userdata.sm_error_dist = f'{error_dist} m' + if error_dist < self.distance_threshold: return 'timeout' + + def angle_point_error(self): + x1, y1 = self.cntr.position.x, self.cntr.position.y + x2, y2 = self.goal[0], self.goal[1] + dx, dy = x2 - x1, y2 - y1 + ln = (dx**2 + dy**2)**0.5 + if ln: + dxn = dx/ln + dyn = dy/ln + ang = degrees(acos(dxn)) + if dyn < 0: + ang = 360 - ang + return ang + return 0 + + def angle_error(self, target_angle, source_angle): + result = (source_angle - target_angle) % 360 + if result > 180: + result -= 360 + return result + class MoveToCircle(smach.State): - def __init__(self, c:Controller, time:int, goal:list, dist, angle:int, is_real_compass): + def __init__(self, c:Controller, time:int, goal:list, dist, angle:int, is_real_compass: bool, navigation_marker: str): smach.State.__init__(self, outcomes=['timeout']) self.cntr = c @@ -162,6 +203,7 @@ class MoveToCircle(smach.State): self.goal = list(goal) self.goal.append(dist) self.is_real_compass = is_real_compass + self.navigation_marker = navigation_marker self.distance_threshold = 0.5 def angle_error(self, target_angle, source_angle): @@ -172,7 +214,7 @@ class MoveToCircle(smach.State): def execute(self, userdata): self.cntr.state_change_time = self.cntr.get_clock().now() - self.cntr.next_step("MOVE CIRCLE", self.goal, is_real_compass) + self.cntr.next_step("MOVE CIRCLE", self.goal, self.is_real_compass, self.navigation_marker) while not self.cntr.shutdown(): rclpy.spin_once(self.cntr) typs = String() @@ -198,7 +240,7 @@ class Finish(smach.State): self.cntr = c def execute(self, userdata): - self.cntr.next_step("STOP", [0, 0], False) + self.cntr.next_step("STOP", [0, 0]) while not self.cntr.shutdown(): self.cntr.get_logger().info('end') return 'fin' @@ -209,29 +251,33 @@ def main(): c = Controller() c.get_logger().info("---- Waiting! ----") sm = smach.StateMachine(outcomes=['fin']) - sis = smach_ros.IntrospectionServer('tree', sm, '/SM_ROOT') - sis.start() + global sis square_mission = { # point_name: [x, y] in meters "yellow": { "type": "moveTo", - "pos": [1, 0], + "pos": [2, 0], "is_real_compass": False, + # "navigation_marker": "yellow_gate", + "navigation_marker": "", }, "left_blue": { "type": "moveTo", - "pos": [1, 1], + "pos": [2, 2], "is_real_compass": False, + "navigation_marker": "", }, "point_3": { "type": "moveTo", - "pos": [0, 1], + "pos": [0, 2], "is_real_compass": False, + "navigation_marker": "", }, "point_4": { "type": "moveTo", "pos": [0, 0], "is_real_compass": False, + "navigation_marker": "", }, } circle_mission = { @@ -247,53 +293,52 @@ def main(): vlad24_mission = { "gate-1-yellow": { "type": "moveTo", - "pos": [6.5, 0], + "pos": [3.5, 0], # "pos": [6.5, 0], "is_real_compass": False, + # "navigation_marker": "yellow_gate", + "navigation_marker": "", }, "point-left-blue": { "type": "moveTo", - "pos": [10, -2], + "pos": [8.5, -1.5], "is_real_compass": False, + "navigation_marker": "", }, - # "point-2-blue": { - # "type": "moveTo", - # "pos": [12, 0], - # "is_real_compass": False, - # }, "point-2-blue": { "type": "moveTo", - "pos": [10, 2], + "pos": [8.5, 1], "is_real_compass": False, + # "navigation_marker": "blue_gate", + "navigation_marker": "", }, - # "gate-2-blue": { - # "type": "moveAround", - # "pos": [10, 0], - # "radius": 1, - # "out_heading": 120, # надо проверить - # }, "gate-3-yellow": { "type": "moveTo", - "pos": [6, 0], + "pos": [3.5, 0], "is_real_compass": False, + # "navigation_marker": "yellow_gate", + "navigation_marker": "", }, "move-to-start": { "type": "moveTo", "pos": [0, 0], "is_real_compass": False, + "navigation_marker": "", }, } mission = vlad24_mission + # mission = square_mission with sm: c.get_logger().info("---!---") smach.StateMachine.add('STARTING', Start(c), transitions={'start': 'GOAL-1'}) + for i, key in enumerate(mission.keys(), 1): if mission[key]["type"] == "moveTo": - smach.StateMachine.add(f'GOAL-{i}', MoveToGoal(c, 1000, mission[key]["pos"], mission[key]["is_real_compass"]), + smach.StateMachine.add(f'GOAL-{i}', MoveToGoal(c, 1000, mission[key]["pos"], mission[key]["is_real_compass"], mission[key]["navigation_marker"]), transitions={'timeout': f'GOAL-{ "FINISH" if len(mission.keys()) == i else i+1}'}) elif mission[key]["type"] == "moveAround": - smach.StateMachine.add(f'GOAL-{i}', MoveToCircle(c, 1000, mission[key]["pos"], mission[key]["radius"], mission[key]["out_heading"], mission[key]["is_real_compass"]), + smach.StateMachine.add(f'GOAL-{i}', MoveToCircle(c, 1000, mission[key]["pos"], mission[key]["radius"], mission[key]["out_heading"], mission[key]["is_real_compass"], mission[key]["navigation_marker"]), transitions={'timeout': f'GOAL-{ "FINISH" if len(mission.keys()) == i else i+1}'}) #for i, key in enumerate(mission.keys(), 1): @@ -303,7 +348,9 @@ def main(): smach.StateMachine.add('GOAL-FINISH', Finish(c), transitions={'fin': 'STARTING'}) + sis = smach_ros.IntrospectionServer('tree', sm, '/SM_ROOT') + sis.start() outcome = sm.execute() if __name__ == '__main__': - main() \ No newline at end of file + main() diff --git a/ws_nodes_py/twist_controller.py b/ws_nodes_py/twist_controller.py index f181539..ea028c9 100644 --- a/ws_nodes_py/twist_controller.py +++ b/ws_nodes_py/twist_controller.py @@ -15,6 +15,8 @@ class TwistController(GetParameterNode): self.delta_time = self.get_declare_parameter('delta_time', rclpy.Parameter.Type.DOUBLE, checker=lambda x: x > 0, error_text="delta_time not more than zero") + self.is_double_angular_speed_for_simulation = True + # subscribtions self.subscribtion(Twist, "twist_joy_topic", self.joy_callback, checker=lambda x: x) self.subscribtion(Twist, "twist_robot_topic", self.robot_callback, checker=lambda x: x) @@ -24,12 +26,16 @@ class TwistController(GetParameterNode): self.get_logger().info('Twist controller started') - def joy_callback(self, msg): + def joy_callback(self, msg: Twist): self.last_time_joy = time() + if self.is_double_angular_speed_for_simulation: + msg.angular.z = msg.angular.z * 2 self.pub_twist.publish(msg) - def robot_callback(self, msg): + def robot_callback(self, msg: Twist): if self.last_time_joy + self.delta_time < time(): + if self.is_double_angular_speed_for_simulation: + msg.angular.z = msg.angular.z * 2 self.pub_twist.publish(msg) diff --git a/ws_nodes_py/videoD.py b/ws_nodes_py/videoD.py index aac0b28..d0478c3 100644 --- a/ws_nodes_py/videoD.py +++ b/ws_nodes_py/videoD.py @@ -12,7 +12,7 @@ from math import radians, cos, sin class RealsenseToBinary(GetParameterNode): def __init__(self): - super().__init__('RealOpenCV') + super().__init__('camcvD') # Параметры для выделения маски BGR self.hsv_min = (90, 0, 0) diff --git a/ws_nodes_py/videoF.py b/ws_nodes_py/videoF.py index 534ef06..7de23c1 100644 --- a/ws_nodes_py/videoF.py +++ b/ws_nodes_py/videoF.py @@ -13,7 +13,7 @@ from math import radians, cos, sin class RealsenseToBinary(GetParameterNode): def __init__(self): - super().__init__('RealOpenCV') + super().__init__('camcvF') # Параметры для выделения маски # yellow = cv2.inRange(hsv, (60, 22, 164), (83, 255, 255)) # -> выделяем желтый @@ -52,8 +52,6 @@ class RealsenseToBinary(GetParameterNode): current_frame = self.br.compressed_imgmsg_to_cv2(data) success, frame = True, current_frame if success: - channel_count = frame.shape[2] - # mask = np.zeros(frame.shape, dtype=np.uint8) # ignore_mask_color = (255,)*channel_count # cv2.fillPoly(mask, self.roi_corners, ignore_mask_color) @@ -63,22 +61,13 @@ class RealsenseToBinary(GetParameterNode): # Создание HSV маски hsv = cv2.cvtColor(masked_image, cv2.COLOR_RGB2HSV) - # blue = cv2.inRange(hsv, (0, 76, 165), (28, 255, 255)) # -> выделяем синий - # Low: (91, 126, 100) - # [hsv-1] High: (96, 255, 255) - yellow = cv2.inRange(hsv, (91, 126, 100), (96, 255, 255)) # -> выделяем желтый - # [hsv-1] Low: (0, 37, 121) - # [hsv-1] High: (19, 255, 255) - blue = cv2.inRange(hsv, (0, 37, 121), (19, 255, 255)) # -> выделяем синий - - # both = blue + yellow - both = yellow + yellow = cv2.inRange(hsv, (42, 55, 80), (115, 255, 255)) # -> выделяем желтый в симуляции + blue = cv2.inRange(hsv, (0, 230, 127), (19, 255, 255)) # -> выделяем синий в симуляции + yellow_binary = cv2.bitwise_and(yellow, yellow, mask=yellow) - blue_binary = self.get_blue_BALLS(frame) - binary = cv2.bitwise_and(both, both, mask=both) - binary = blue_binary + blue_binary = cv2.bitwise_and(blue, blue, mask=blue) + # blue_binary = self.get_blue_BALLS(frame) - # Выделение самых больших контуров удовлетворяющих условию размера res = self.get_gate(yellow_binary, height, width, frame) if res is not None: course_to_gate, dist_to_gate = res @@ -104,10 +93,15 @@ class RealsenseToBinary(GetParameterNode): self.pub_blue_pos.publish(msg) # Создание отладочного изображения - miniBin = cv2.resize(binary, (int(binary.shape[1] / 4), int(binary.shape[0] / 4)), + miniBin = cv2.resize(blue_binary, (int(blue_binary.shape[1] / 4), int(blue_binary.shape[0] / 4)), + interpolation=cv2.INTER_AREA) + miniBin = cv2.cvtColor(miniBin, cv2.COLOR_GRAY2RGB) + frame[-2 - miniBin.shape[0]:-2, 2:2 + miniBin.shape[1]] = miniBin + + miniBin = cv2.resize(yellow_binary, (int(yellow_binary.shape[1] / 4), int(yellow_binary.shape[0] / 4)), interpolation=cv2.INTER_AREA) miniBin = cv2.cvtColor(miniBin, cv2.COLOR_GRAY2RGB) - frame[-2 - miniBin.shape[0]:-2, 2:2 + miniBin.shape[1]] = miniBin + frame[-2 - miniBin.shape[0]:-2, 2 + miniBin.shape[1]:2 + miniBin.shape[1] * 2] = miniBin # cv2.putText(frame, 'iSee: {};'.format(len(contours)), (width - 120, height - 5), # cv2.FONT_HERSHEY_SIMPLEX, 0.75, (255, 255, 255), 1, cv2.LINE_AA) @@ -161,59 +155,53 @@ class RealsenseToBinary(GetParameterNode): approxs = list(sorted(contours, key=lambda x: len(cv2.approxPolyDP( x, 0.01 * cv2.arcLength(x, True), True)), reverse=True))[:2] - if len(approxs) == 2: - # Для выделение левого и правого шара - aligns = [] - # Для выделение ближайшего и дальнего шара - distances = [] - for contour in approxs: - # Точки контура не приближаются к границе ближе чем на 10 пикселей - # self.get_logger().info(f"{contour}") - # 1/0 - if any((not 10 < xy[0][0] < width - 10 or not 10 < xy[0][1] < height - 10) for xy in contour): - # self.get_logger().info("collide border") - continue - moments = cv2.moments(contour) - # # Получение хз чего - # area = cv2.contourArea(contour) - # Координаты центра контура - cx = int(moments["m10"] / moments["m00"]) - cy = int(moments["m01"] / moments["m00"]) - - # Вычисление дистанции - x, y, w, h = cv2.boundingRect(contour) - dist = (w*(self.k1)+self.k2)*0.01 - - - # Запись центра контура по горизонту и расчётной дистанции до него - aligns.append(cx) - distances.append(dist) - - if True: # Вывод информации о контуре - # Координаты центра - # cv2.putText(frame, f'cx: {cx}, cy: {cy}', (cx + 10, cy - 10), cv2.FONT_HERSHEY_SIMPLEX, 0.75, (255, 255, 255), 2, cv2.LINE_AA) - # Выделение контура - cv2.drawContours(frame, contour, -1, (0, 255, 0), 1) - # Выделение прямоугольника - # cv2.rectangle(frame, (x, y), (x+w, y+h), (0, 255, 0), 2) - # Оси через центр контура - # cv2.line(frame, (cx, 0), (cx, height), (0, 255, 0), 1) - # cv2.line(frame, (0, cy), (width, cy), (0, 255, 0), 1) - # Вывести ширину контура - cv2.putText(frame, f'W{round(w,2)}', (x, y+h+25), cv2.FONT_HERSHEY_SIMPLEX, 0.75, (0, 0, 255), 2) - # Вывести расчётную дистанцию до контура - cv2.putText(frame, f'D{round(dist,2)}', (x, y), cv2.FONT_HERSHEY_SIMPLEX, 0.75, (0, 0, 255), 2) + # Для выделение левого и правого шара + aligns = [] + # Для выделение ближайшего и дальнего шара + distances = [] + for contour in approxs: + # Точки контура не приближаются к границе ближе чем на 10 пикселей + if any((not 10 < xy[0][0] < width - 10 or not 10 < xy[0][1] < height - 10) for xy in contour): + continue + + moments = cv2.moments(contour) + # Координаты центра контура + cx = int(moments["m10"] / moments["m00"]) - # Расчёт курса и дистанции до ворот - if len(aligns) == 2: - med_x = sum(aligns) / len(aligns) - controlX = round(-2 * (med_x - width / 2) / width, 1) - controlX = max(-0.5, min(0.5,controlX)) - course_to_gate = round(controlX * self.fov / 2) - dist_to_gate = round(sum(distances) / len(distances), 1) - return course_to_gate, dist_to_gate - else: - return None + # Вычисление дистанции + x, y, w, h = cv2.boundingRect(contour) + dist = (w*(self.k1)+self.k2)*0.01 + + + # Запись центра контура по горизонту и расчётной дистанции до него + aligns.append(cx) + distances.append(dist) + + if True: # Вывод информации о контуре + # Координаты центра + # cv2.putText(frame, f'cx: {cx}, cy: {cy}', (cx + 10, cy - 10), cv2.FONT_HERSHEY_SIMPLEX, 0.75, (255, 255, 255), 2, cv2.LINE_AA) + # Выделение контура + cv2.drawContours(frame, contour, -1, (0, 255, 0), 1) + # Выделение прямоугольника + # cv2.rectangle(frame, (x, y), (x+w, y+h), (0, 255, 0), 2) + # Оси через центр контура + # cv2.line(frame, (cx, 0), (cx, height), (0, 255, 0), 1) + # cv2.line(frame, (0, cy), (width, cy), (0, 255, 0), 1) + # Вывести ширину контура + cv2.putText(frame, f'W{round(w,2)}', (x, y+h+25), cv2.FONT_HERSHEY_SIMPLEX, 0.75, (0, 0, 255), 2) + # Вывести расчётную дистанцию до контура + cv2.putText(frame, f'D{round(dist,2)}', (x, y), cv2.FONT_HERSHEY_SIMPLEX, 0.75, (0, 0, 255), 2) + + # Расчёт курса и дистанции до ворот + if len(aligns) == 2: + med_x = sum(aligns) / len(aligns) + controlX = round(-2 * (med_x - width / 2) / width, 1) + controlX = max(-0.5, min(0.5,controlX)) + course_to_gate = round(controlX * self.fov / 2) + dist_to_gate = round(sum(distances) / len(distances), 1) + return course_to_gate, dist_to_gate + else: + return None def main(args=None): diff --git a/ws_nodes_py/videoS.py b/ws_nodes_py/videoS.py index fdbf773..4afe955 100644 --- a/ws_nodes_py/videoS.py +++ b/ws_nodes_py/videoS.py @@ -13,7 +13,7 @@ from math import radians, cos, sin class RealsenseToBinary(GetParameterNode): def __init__(self): - super().__init__('RealOpenCV') + super().__init__('camcvS') # Параметры для выделения маски синий BGR # black = cv2.inRange(hsv, (0, 20, 0), (30, 218, 109)) # -> выделяем черный diff --git a/ws_nodes_py/world.py b/ws_nodes_py/world.py index ebffb94..e5342f2 100644 --- a/ws_nodes_py/world.py +++ b/ws_nodes_py/world.py @@ -28,6 +28,8 @@ class World(GetParameterNode): break # Линейная интерполяция speed = round(self.motor_speed[low] + ((med_signal - low) / (high - low) * (self.motor_speed[high] - self.motor_speed[low]) if high != low else 0), 2) + if self.throtle < 0: + speed = -speed # Расчёт смещения dx = speed * cos(radians(self.heading)) * delta_time @@ -50,6 +52,7 @@ class World(GetParameterNode): self.subscribtion_for_parameter(Twist, "twist_topic", 'throtle', checker=lambda x: x, func=lambda msg: msg.linear.x, default=0) self.subscribtion_for_parameter(Int16, "heading_topic", 'heading', checker=lambda x: x, func=lambda msg: msg.data, default=0) self.subscribtion_for_parameter(Point, "goal_topic", 'goal_x', checker=lambda x: x, func=lambda msg: msg.x, default=0) + self.subscribtion_for_parameter(String, "navigation_marker_topic", 'navigation_marker', checker=lambda x: x, func=lambda msg: msg.data, default="") self.subscribtion(String, "reset_topic", self.reset_callback, checker=lambda x: x) for key, value in world_markers_topics_parametrs.items(): self.subscribtion(Point, f"{key}_topic", self.get_marker_callback(value), checker=lambda x: x) @@ -80,14 +83,7 @@ class World(GetParameterNode): return callback def update_position(self, msg, name): - # TODO Проверять чему мы сейчас можем доверять - # self.get_logger().info(f"{name=} {self.goal_x=}") - if (7 > self.goal_x > 5) and name == "yellow_gate": - # self.get_logger().info(f"SADADSAD {name=} {self.goal_x=}") - obj_pos = self.world_markers[name] - self.position = [obj_pos[0] - msg.x, obj_pos[1] - msg.y] - if (11 > self.goal_x > 9) and name == "blue_gate": - # self.get_logger().info(f"SADADSAD {name=} {self.goal_x=}") + if name == self.navigation_marker: obj_pos = self.world_markers[name] self.position = [obj_pos[0] - msg.x, obj_pos[1] - msg.y] -- GitLab From b75090c51f74d9e39172f5af91e3f77c8902b005 Mon Sep 17 00:00:00 2001 From: "toropov.nik" Date: Sun, 29 Sep 2024 23:00:52 +0300 Subject: [PATCH 07/23] end of 29 sept make gate crossing and droping of probootborink --- .../__pycache__/state_machine.cpython-310.pyc | Bin 10910 -> 11197 bytes .../__pycache__/videoD.cpython-310.pyc | Bin 5007 -> 5424 bytes .../__pycache__/videoF.cpython-310.pyc | Bin 7049 -> 7087 bytes .../__pycache__/videoS.cpython-310.pyc | Bin 6441 -> 6457 bytes .../__pycache__/for_world.cpython-310.pyc | Bin 906 -> 959 bytes ws_nodes_py/settings/for_world.py | 6 +- ws_nodes_py/state_machine.py | 51 ++++-- ws_nodes_py/videoD.py | 165 +++++++++--------- ws_nodes_py/videoF.py | 11 +- ws_nodes_py/videoS.py | 7 +- 10 files changed, 138 insertions(+), 102 deletions(-) diff --git a/ws_nodes_py/__pycache__/state_machine.cpython-310.pyc b/ws_nodes_py/__pycache__/state_machine.cpython-310.pyc index 5e8afc5dcb66f4ae2df89f66a6ae20bd356416d8..5da828fcddacef1d9e74834efca9d33940e72f80 100644 GIT binary patch delta 1707 zcmZ`(U1%It6rMYoy)(Nz``2W4v&kmiP0Y{6ZrhLtk;b;7hBTH!iKSSgA?9q-ZMWIv z?!;i6b&(BLrD=*61vM?ZVATgns8K=1iqIFq2SuT{FM8NP&TiM3rgu2s zocrB#zH?^o+_`e%vyHmdMZw!c? zzQ6sr|FT%QB4D?GB6i>CT0!?4F@0-et^G+tmboXoN(IZ_;p@?lu+KrGEp!!$8)#V# zL6fi@U8n4-Xl7$AHbBTa|FL6!U=3G)5nonk9(2DkYRhW?LnuPQkibg|hWR(#+`g}|yKMREPvnj#QeZXlBBv}n;)lE7DxAC{h28k2Ks zPQ$NM3ro;}rks`8ueJP6OMO$iESqjif_rK)U16!_b=SR~y4M?)3Ka5j=rr-u7ljJM zQc?0J=$mPUz4elN>R!E}5x|ZLD%{Zq%J9yaP4DF&x{zX?Pe{=y~s<<9UGkrxr?6nB&D>Y06%s_Eh30aF28%t8oX`l4{ z#}=CsIHJ*$6Dcz_GaZXY#hk6wbvrjWYW@!42*Twkw5DVfo?qK60 z_ymH%1`x~M>c8f6?PtSByBG3{E|!GEWQt9ulVvCR|_L zPrxpjm`u)?Nyg=#=bLSzyhx`*M~)pC9cG^h=}-AAL;FL=)f|w ze5&_o{ySXK2qj*;WCta(xMbxo*;(_6^0%z*?9-p+n2fo04#p`J*}ZM9|F^oM;-HYf zTd6HP{a)Fp$kjF}p_QQGCkOch@pf{PkHudlU-94L!~W`Xiif=^U<&`eA%9sLJePs_ Wbn>9s^Xwiz*Vm&-d5yY=Qu!CFs*mCT delta 1335 zcmZ`(O>7%g5Pq}XeO~{qoy0$NVmo%y#<3%~qz5DrEwn@_hf1iFil`;3G9*eWYIU;# z(XQ)5!jIJafJPi3Zm~t-f*KWBs@el;DZQaaLL5LnB5nu)7gPzPGS6|Ggp^s$x8FB! z-puH&pT9nF<+xP}h5Ul8IPq{YvHvT}iTRa2$Ugf#Eyj8WC;ceVyP%JmXrgaHjQP-y z0e<^2h#`Il&^jgxVfrz)&zg|DanmFv<1Yr$aK*TZf^#I5;^>?DZulCn{-)B@F`2@Ipl++!1bf?j%tGvk*k{kvmHwwIRULIS(3sND ztG+^0%UxP-TlZJ>vW5ZmjZnoc4D;Sd)tH8|hpTloqg&NFv1{9I9Xru#>xyds-wkp{ zfoRnz>#kllTv)jd!WA`7UDR}p)d7TK%*Yup#vRiY^MDE5GYyzr^6-k!HERC4KxO!rnF3KC1Er0ZU z^a-|X_QdIFXZq|^E}K=_UgO`DU1E43-zdHL`pYG|d82?MBX*pzpxfgr=eUAl1p`9R z`oL^@iUnKnV2k87$-~k20Me#<)OSZXZ)Z)`WN9| BEHVH9 diff --git a/ws_nodes_py/__pycache__/videoD.cpython-310.pyc b/ws_nodes_py/__pycache__/videoD.cpython-310.pyc index 5be606262a09f997eba4cc0f057ed733fa9ad320..158747401253b3a2b068a42d82be7bdc04541b8d 100644 GIT binary patch literal 5424 zcmbtYTW=f372X?{%R6Labu z`lzZrP?*IsPZgGNwH>{Fh80+r<({hbF?XEhH#ENBj_oLrr!L7-E$(BbO0BAj@&h+s zaJbWQW0%)L=Ax}M6SmsijUtylYB^16N<9kVTepc>2)!T%W6f;d;~qn=Dbn0wo)bhO z&Dt4=T|$Fs@QpnNrt2R#sJ}GVQ5V*O|_Yr%K&m zDQ4oC`dBfPy1ArO(_(1Rb^OQ;B6m5w=LHV$Hq;ZcP4GDsnj-o=dSYcq*@fUe6>88D z#nzY(iU~kbc2j%GuE|n~vaUVH5#XsmS3WW1`1B98p4!uxg{R&#;`F+esEM|!CHjCG z;MD}QX*5;zXISPL&5~tV&>YKw<{`7d@^Nv!v|e5x!XXuLWTj`cj^TJD9!38!E1y$# z)EyOkZd74I=M?mwk#7zU-W+2iC*B;FeWQcE33f*Ijj{1(Mi0B$os7@2i6n)+Xl!zi z<~oNr&d0B@v+Nu@FGs&7=>>N28H^IW7w`_P<|3;MW`g0OUWqTUOYHKXjbvSpuk6qY zuO4ae3eCk(@9%~9uiD!xwET%FwX5JjU#|(X;j|i?_pADT9f!v0G$ld9Kfv!FP{R<2 zcMW~psfSCCE7@(mlC4xVIdK0k=&Wkq7wGmTK*(HK+-l@q7CcnYMXzIq3$bJ&tSm}5^ zBR;?_itKjxC4SRmZg~HCyDO}Be5bX-oI96jHVwp3wy&Ks=j*oZ1zv31*WdBO25kKf znEs2{SGuw0Ha1+oPwzKT@Qd#Xb>o%$;^P>?Cy2=1oIT=X^*mKy1G!4f1rU*s!?xG3 zaV#!(UMW}VD}m}>sI{+`{H;UD+mljqC4l5gs}seQ6}J+_+zXmGN+7HvvM%Q#w_|rJ z-qstNH}(mEMPbba&;(6ezAw^)>MPgAuh5ioM_;?=>fy>>DJddAflXtdQXY=LU{37d zskfr$v4QX-MUv2nj>kN-x`M4NUCh~HH2JJ#%_CC!n{BK{=KP0S>n7<)!9m*n_Dl4F>um&en@vw-)? zczYFD6^tsq5?arlYAp}smmsq=kY5I0K9WClTK-q8X*i)M<@|<1u@4*hig#YyQF=p3 zDH*aaBxTTx7+=}FlnjwC2Y(qno%GZ{*Cw6@&$>J=CW0cgvKHmce_**bq8 zWS@L6AMN)RfCesahrUDiYX6IagM)38c~Sg!Wjj*=s_@8t`^fbZybs*qr|(vE87CZ* zZG8)G8I|tw3sZOja=hq1K;^jk2Q>LrP7g8BI0}PpTw1_9K-F$+P67MY93ToD)0D6p zoAFHOhg=k9=I7=Y?ZpT8rVyKmv==No0g&z%gmIcrRR3 z9nRe#2H*jzL^|p;8pt(7s$b8ORbZnYWgfy@bG_yoR{q3eZ~;1ejL07AL6|Et_6HI} zAqJTT;z(&5A`S23AvNH%V%{bqeWi>9j$@vO=%5pdh{WZLSuLu@!I(Ct_PNVC)lH4iG_OwR#y__&oW{(PxZfaE9{$Q;EEJim zFx-JyqGpUt1V!J|fsLu285=PYpu^-SHi45sMoN*M=|~V$grYmzj@nCMBzomolM<=^ zm>r~7EKQ?&X_?7by-bqke@@aYgSISYwUX?wmGulbpeob%G-M!{U-spaEN~n+%CeIR zZ^ne8xk+Ff)d@FydD)hKsjU!bboY}&PDwPDItNU|38_j@Nw%r75-YRx zUJ9sWF{PvOzr&YgR*#X zObod|5J&ti)Hp@dFVw!JAKMqj)Eg)=6dYByZ=be|(|7xH%)eaKgudZ+k!!n~h>Tob zfzVT1pt~!#K)taNtGmcTv3wmIvRvf9bf)T_zJm-$W&_of(0w=PZz%7y+dSM_2>tH; zg?ad>0S4Vs9%WXkYM2tE-~(Dr;386ca=C@u+qj5<_R>!bb8QG?cQ-6YlB;%89x z)?nrPK=cD>Qi49^b&{(N4OX)11#Bh^V!F(tusFNCXwQBCh^tM{iJ_T?(~oNQ^uog8 z{0DYzzBVfi#AUeqtDX<9AE9Xe(td6rz8W>G zyrmlI_SJ7)KbOmB_$2@Q@AZYtn)VNRIsY;6@*bYx*C0X@%+o6L%RN@-70!s)J-u#J zjF>mPMBS{IG0(hYC5irom#U{L>65lhr3-DQm#yb2xq5e{yWUgjVcNVVk|K3a6Dfxu z=#{+4inPewW0hW~Pjqc@IqUQuXwbn{;%7W}j1_5%<4on}op9NfcHIdbx#$ZAZ9Ox7 zy(yg_aKxRuU8APt`<`!yKY9afYG(VEbOoAqm6Wz{?M9&Vst*#ljVdWV|Itrgf-7Jg zt-^#>;a_R%OfbRkr7F76g>g@-7+-0IR!OXA;|bMUc5E+j8iBLw-*Owa+^e#4I!*BO zkm{g}o=`i`4x#XfVUIl0EG~3VC|58`^Jdnlb`W6Yjcsp zuiiGoj5ntz#enJiKECQ_=HHQAQNZ+SfGSJf{GGTTzx7oefBf3NuKFHmtx$|*f z_gP=hxxU`GFMrb4CwkBIU5NYo&c#iw1p!F8u~}iY%|gs7h6Pw6Gj)6S+(oc?b&htxB^2<@<2?` z@F(!ckEkIB;$B7He&Y7Z$ED1^Udoilc`SJR2Xu~e_d9gEMG!tn z)3bu-UvzFLR3Gxi)rZ#z}g9}t+iZF z5Z^!?2Uc_M34YrZjz2rm+*7G{JiERo?03g#HV$HF`&XZdxe3d18*XS>6YqF_6#@A! zn9e{t5!DT+y5&f0W$L+Gk-fC+zSFhK!WkFuXMkP@K`s$f1X23Bzx}<{mCn?$t?#Yu zg)?Q@&~Lg`3t{0%`?-2!_eJpJL$=s)+w*o!86n>F{!pyB)PUwvy%mI|HK!DW(rwfb zN<$D+8AnQATA{NW?(5a9t8b%s!W8GK0(NqarN_$Tw z+(!8NlnWjR7kqts-std{RA&Z~S?I(7)Ptu@aLHeftTmP*IF$_A~qpeo(9(DfTC z>(*;`vi#N@Y4RK%TA7~DuSh;qpb$g@}z^^Hb@uwyYQirkPU{~BHd*FB_l-bXadJQTO}Jkd7XV4r`D z(PK3FGowAy9&KN2k3|@`I4SbYA{x zxzE3j`eB(X;mc9~0SjLtO1=)SploJ3o-b+zElQ2RmIu7u6}~El!f`Pib;0)ZekJO{ zo=n&cQ39Lfq5-mXNIC~%ouDV9vsmS8@btx~7txm;(!>ajIeFJl-{V#OK#U@4O4xZa z0^PidNGnApSfLn|FdEWDO%%W60^hg=`UbTx0aM;Nwb#YNA7W2^2RIMd%UDs1lHyYF zv9(HqRy1*xI?a#u3n!!80XtyW6?P4P^g`Tz{pt2^4DsTL<)76B%RJx!9xpvB z0gFu?z7DD#z7ZKm9him!AqZ%^ytsNyr#waswbQeHO!y8})~MUT);K?=yd-Bqo=N$X z4JgM4l;cCn2ig&!;1Iwpi9Fpw`}KwOa@GWj;~PuAq0 zky2V&LEE2vHbtFJoBsnKaHVfKdjQDJHgdi5%(Bx^951^BCe%W92t)?P`A0N&G z0HGK`02k|?(~$3DfXdpc)WrUJB|c2^wdM(K<8PmXtK8uBu<##5e=eSP;v zY2PUA-8co8as_hxT~)skO5eL%`sr!M@du>fP5BmnRIz$?m9gNldQh`M-$EF4%xFqK zM442><)zi>)j8|!No!{5BUIJ$%)-*l2cO(onFAQuuz?1s#;R0xJDl-7U#i^9(!x^N zD$n1V0`65ww^6nmDE%>}d5L7Z*4%K%4IImE2$ik-vblkgwY|Ar)%D?0dHJ@5IacQ6 zI0+km!wFQP1=GBxvg>X`%wS`F3os#9o?9(jb9b?*#pwmrJ9B&b&Z0HFyj)(oYb`D< z&M5;WK2+wq>%p$d415_nLdB+mhG`%HxqwOHFp(b;H&1iwm`r8YfMEoHR@teB=dgu| z0CQ;JLfSiLYe_a+;VK{)Kne|0h4+^hSFPJ~<(1oWpIR$-K3rayyNi2Zt1ih4h$uiM zoq&^x`(%ug2(y%}>GIsP>h9QZzC8Wu6by+S__A(?D!FiHan72amPuMG#j8vad6QIP z2{ELBYqzIjXFl!W@yqdXR)(-cTji=PDRJJdb-LuLWYDTsakW*6&U>!9VK*9%*THh| z6Y`BC5G%O%Ds#heYa7_@jw`|qm20}Y7D53Yt$B`0)FHTwm^cYR*b082CDkaVx`7UH z^3UFNU`S=|IiBb5C}YisLUS#?+;&vN&SsWMc8+$SOxts7xLc=5t=+(?ap~wpMaO}> zx~q6~Pxg~t*q&lLifv%q6b(rj-NoIxlL6vf5QDQHL-s13;Flm-o?+bpXBlRgeR>9P z%wY8G*88}j=h-mJ7zRr{dX489N5U|f@yOH-ZtxTz#i*1~U;~WvKD_|R47h?ZnS70# z@z^};gYE(Ls%~IxH-D9tcsDfMh-uy+zGxV~-5>k^*%eq6NSqL-WvPs1)qTgh6E=; zLTwY}MX>vTE+IeA+6)CpCjbgigS>r&J1ko8Zv*{b6PF&UVYaLVoR!*j{{TRjFX36$*c+fqTy($!EwmOE`)%A9q}NR-~vlx{U`A3g|8bpiLfO zStfr^17R}i$NuRWG=8Bo{grdK;|bxc+bu7gc=|6ONgG!^^al*CT*#Th1W4xwkqtue zxQQ})CzGbD+tynAw@~K}k!ZB)&ApT4hC{v4Imhvdjt?_gJ6@^$9dYrt9usmB?&%#@ gze~j>p!-dVXR*O-yv-CVu)MDQBGZ#fA%GwK7q=tb8~^|S diff --git a/ws_nodes_py/__pycache__/videoF.cpython-310.pyc b/ws_nodes_py/__pycache__/videoF.cpython-310.pyc index 228516d99da57924f6b95cbbfca304181c1690e4..a18a0468978c08f6b3846e46d2d15e629b652faf 100644 GIT binary patch delta 1785 zcma)6O>7%g5Z+nayU&j8`2WAnFG-z{=BFy5O2wfh4ap&;X~mBl$#U`>$FkA31<>>H!IfQ<>RCX%!LFUG2B?zBe;( z-kX{C$>_JE{$-!fqu^8i>hHw|eLMaP+}i0MmLmSdI-Rd>#foL2Q1ExDF| zId9H~oL==Ct+`eskP8^WTo9B=g){EHrf_#r-E!nY+{Zn<Ue}_>o345A!(gYd#tMk1+i%eRE`(RJw zu!E>02w4#A@{+4#pqrRi2>#cw(=wD_x;jt1q(zryfzX7CyzffA>`GQFN92Uo4>5UP z4<`@q5?%J%(cIBGpk4l`oj6>x&(>_yM-JC{)z7XP38DmU0vACFp$Y5aEdBMB)%j|kY%Dk)JgfUwZr#1iHk&QVoE-3O~Ct-1v2(A*k`dj7#IXB@qDlXQ}XBF z*%v2eCRA0us{Ax`1GZ!=+|%9&aBor(C9?%3UsAEhX=MvGf;G4@F5eFKBzS8h=2RR? z#U-xuK*d%6xCWr$5 zT%@|Laah{g1MfzjJu!g(A7S#)Q-CR(znHh0jUX9FUM(A9rukqBa%ssEG zjC>k-82!(lmye=1hx^Ht3QOb=W~{7c)uwuUsHuLio>hfObE^clQ+Yi$JuGe`ffvp2 zkhqJ|o>g#jBt63?>##jq6>R&Kf5kd5tBH6!^vYB5b3ynSI+C;6 zAgYRSc`4aBXl=xTb3|I+=vMcWH!lKHyuiBHIAejJ@W`)|dG8p~O(&MVi9|fr-SZ0( zp4noddTymyd-I(G{Xi{Su04^3R5<>GR9QYxOg=BH>pCjNy72mx_Y=zUiv18Y}DKB+0mdxny^~bqBbS6toRN>xst%&v$Ak o54k2gZ7lQEYVi%qOjrFVB2&F!zYPBjA`oyWo1RI=Jm7rx52{pThX4Qo delta 1718 zcma)6O>7%g5Z<@8ch_rg;W|AgaavQng}J8TLChWEGN%N+@Y{sE{itLZ}UMY(edb6I4*(fK&uWBoIibLRut_5eW6b38Wm#o86M43RPLj-;TdG z-_E@EX6DA|+oN5pnx+c;`C9sW=}vyTD@(3!4;Pj!xhxZb3N+AAtYA6l+N7bDRnAF` zkQFY6?{QkWi*s_L+lrJUmR{DaXgNxRDS;|9ctN1Sn%MD`V^pIm4P78*qZX&(vm$%G zW^8{su8NL)ys=ufN1poRuZ~D`IOls!GI>VKgk9f9eJO6{iQ}hLyUO1_tF_F$W%S7+ z9DwiqFAce5+7<4Bx6J?MkUGdAPgo9S1G&PZ$n+yTRM_hZ;cB3FzlujZ#UFv5{VLp4 z4#LyYa3P6e-15V-8t`IVmU1KqpGrrb+^25H)p2lQa854lH{4@hxOI7SzlwsVxGe(- zKwTL#`;fr29eH~8^yJiJi47u^hg-@#ksuxHB{Dn{JQ{MZ!@aoT%{PJrK1oME9X<|c z|22OM$H_A6g!{=fXxf=#5Iq!x1OgT`lMpf-9J0x#@GY-xEHvt^6?OtX)C%Mn+}3)e z8FV`YyV`q$=*;Q}Jscd_syFKs_2z@sHVL11?U*I>i*euug$d>rRz+$AKI_i)A4PKs zX^FWJ7`Gj5ai!X9)*6+?)s3dj%COs=^|tHn`DSnAal$*Gq@N`-@Rff2zl+cvZHPMr zwxaKlbqL1#4Lj71(6A|R)J!2GbjX$tbFqGSH+JaRZW^Kb^#BoUa!qghY1EFk4N5xV z72$oc9c{W~#a zlj<$oEf4-V8^7{?6e^Vpu2(AV%*iL%IDCPwQ4rotniuJI*fzY zZK?%a3@*M>SzvX#Tw^l+4>UKdwM{mHHjinqc>U;hCPro<1Psc)MYzAwy_;+k>tq`~bJ3m}ks476h8ocnJ`si#Rw;&T)}r3Y z9eisUUr#pU4`dXd+`*s8>6Rjq%{qaxD17rT{!fftMb$v}++xl(G?@HN@DyYI=7U02 p%#2l&ZNwBgI2c73#Tc2GHYbYBXJlNxnNdQKkzJ01gNu>nKLCc&H=6(e delta 192 zcmdmKw9<$-pO=@50SLtQeotGvkvE%{anwQ~1_0KAr5u zAIK;;c?y3fr$dTFHtPh&qQK2;0-qSUZZYQ?8WdFn9Wt3o=oDkl=CeXo%#3A|J;W4Q dL>NUFHy4V{XJlNlSx`cekzJaDgNu>nKL9pfF}?r* diff --git a/ws_nodes_py/settings/__pycache__/for_world.cpython-310.pyc b/ws_nodes_py/settings/__pycache__/for_world.cpython-310.pyc index b7e36b1ab11a8fd16c2fe17a885fd00df7c030d0..6832dcec1325f6bf74a091b1544b1963eeab93b0 100644 GIT binary patch delta 286 zcmeBT-_OpQ&&$ij00eF;f2O^e$SYev3CKwS!gQt-Mli_)CYhlmGf^6v;WPQS2#F!3>(xFG0?H$p#|0Cmx;7$TK;XF=z5Q zMmc#QpjZ(|qKE@Tu!9Ir5CQU;ChO!sj7Flj_>yuGle6Os@{2P|GV}9p@f4+|Ojc!D b%ccNQsWABlQvzeqWJ_j$HYP?UMy}rgKD25gHX# z5fQPEtfXFp@xlh>P8|tSG>9g>5thyPfv^fKw9!G=ORLeFvAq8*zW7w9` выделяем синий - yellow = cv2.inRange(hsv, self.hsv_min, self.hsv_max) # -> выделяем желтый + red_circle = cv2.inRange(hsv, self.hsv_min, self.hsv_max) # -> выделяем желтый # both = blue + yellow - both = yellow - binary = cv2.bitwise_and(both, both, mask=both) + binary = cv2.bitwise_and(red_circle, red_circle, mask=red_circle) binary = cv2.morphologyEx(binary, cv2.MORPH_CLOSE, np.ones((5,5),np.uint8)) - - # Выделение самых больших контуров удовлетворяющих условию размера - contours, _ = cv2.findContours(binary, cv2.RETR_EXTERNAL, - cv2.CHAIN_APPROX_NONE) - contours = list(filter(lambda x: cv2.moments(x)["m00"] > self.min_countur_size, sorted(contours, key=cv2.contourArea, reverse=True)[:2])) - approxs = list(sorted(contours, key=lambda x: len(cv2.approxPolyDP( - x, self.k * cv2.arcLength(x, True), True)), reverse=True))[:1] - - if approxs: - # Для выделение левого и правого шара - aligns = [] - # Для выделение ближайшего и дальнего шара - distances = [] - - for contour in approxs: - contour = cv2.approxPolyDP(contour, self.k * cv2.arcLength(contour, True), True) - moments = cv2.moments(contour) - # # Получение хз чего - # area = cv2.contourArea(contour) - # Координаты центра контура - if moments["m00"]: - cx = int(moments["m10"] / moments["m00"]) - cy = int(moments["m01"] / moments["m00"]) - else: - continue - - # Вычисление дистанции - x, y, w, h = cv2.boundingRect(contour) - dist = (w*(self.k1)+self.k2)*0.01 - - - # Запись центра контура по горизонту и расчётной дистанции до него - aligns.append(cx) - distances.append(dist) - - if True: # Вывод информации о контуре - # Координаты центра - # cv2.putText(frame, f'cx: {cx}, cy: {cy}', (cx + 10, cy - 10), cv2.FONT_HERSHEY_SIMPLEX, 0.75, (255, 255, 255), 2, cv2.LINE_AA) - # Выделение контура - cv2.drawContours(frame, [contour], -1, (0, 255, 0), 1) - # Выделение прямоугольника - # cv2.rectangle(frame, (x, y), (x+w, y+h), (0, 255, 0), 2) - # Оси через центр контура - # cv2.line(frame, (cx, 0), (cx, height), (0, 255, 0), 1) - # cv2.line(frame, (0, cy), (width, cy), (0, 255, 0), 1) - # Вывести ширину контура - cv2.putText(frame, f'W{round(w,2)}', (x, y+h+25), cv2.FONT_HERSHEY_SIMPLEX, 0.75, (0, 0, 255), 2) - # Вывести расчётную дистанцию до контура - cv2.putText(frame, f'D{round(dist,2)}', (x, y), cv2.FONT_HERSHEY_SIMPLEX, 0.75, (0, 0, 255), 2) - - # Расчёт курса и дистанции до ворот - if aligns: - med_x = sum(aligns) / len(aligns) - controlX = round(-2 * (med_x - width / 2) / width, 1) - controlX = max(-0.5, min(0.5,controlX)) - course_to_gate = round(controlX * self.fov / 2) - if distances: - dist_to_gate = round(sum(distances) / len(distances), 1) + pos = self.get_red_circle_pos(binary, height, width, frame) + if pos: + dist = pos[0] + course = pos[1] # Создание отладочного изображения miniBin = cv2.resize(binary, (int(binary.shape[1] / 4), int(binary.shape[0] / 4)), @@ -139,18 +82,76 @@ class RealsenseToBinary(GetParameterNode): miniBin = cv2.cvtColor(miniBin, cv2.COLOR_GRAY2RGB) frame[-2 - miniBin.shape[0]:-2, 2:2 + miniBin.shape[1]] = miniBin - cv2.putText(frame, 'iSee: {};'.format(len(contours)), (width - 120, height - 5), - cv2.FONT_HERSHEY_SIMPLEX, 0.75, (255, 255, 255), 1, cv2.LINE_AA) - cv2.putText(frame, f'x: {round(dist_to_gate * cos(radians(course_to_gate)), 1)} y: {round(dist_to_gate * sin(radians(course_to_gate)), 1)}', (0, 25), - cv2.FONT_HERSHEY_SIMPLEX, 0.75, (255, 255, 255), 1, cv2.LINE_AA) - # cv2.putText(frame, f'{course_to_gate = }', (0, 50), - # cv2.FONT_HERSHEY_SIMPLEX, 0.75, (255, 255, 255), 1, cv2.LINE_AA) - cv2.putText(frame, f'{controlX = }', (0, 75), - cv2.FONT_HERSHEY_SIMPLEX, 0.75, (255, 255, 255), 1, cv2.LINE_AA) + if pos: + cv2.putText(frame, f'dist: {round(dist, 1)} course: {round(course, 1)}', (0, 25), + cv2.FONT_HERSHEY_SIMPLEX, 0.75, (255, 255, 255), 1, cv2.LINE_AA) + x = dist * cos(radians(course + self.heading)) + y = dist * sin(radians(course + self.heading)) + cv2.putText(frame, f'x: {round(x, 1)} y: {round(y, 1)}', (0, 50), + cv2.FONT_HERSHEY_SIMPLEX, 0.75, (255, 255, 255), 1, cv2.LINE_AA) + # dist = (x**2 + y**2)**0.5 + cv2.putText(frame, f'dist: {round(dist, 1)} isDrop: {dist < self.drop_threshold}', (0, 75), + cv2.FONT_HERSHEY_SIMPLEX, 0.75, (255, 255, 255), 1, cv2.LINE_AA) + msg = Point() + msg.x = x + msg.y = y + self.pub_red_pos.publish(msg) # Отладочное изображение self.pub.publish(self.br.cv2_to_compressed_imgmsg(frame)) + def get_red_circle_pos(self, binary, height, width, frame): + # Выделение самых больших контуров удовлетворяющих условию размера + contours, _ = cv2.findContours(binary, cv2.RETR_EXTERNAL, + cv2.CHAIN_APPROX_NONE) + contours = list(filter(lambda x: cv2.moments(x)["m00"] > self.min_countur_size, sorted(contours, key=cv2.contourArea, reverse=True)[:2])) + approxs = list(sorted(contours, key=lambda x: len(cv2.approxPolyDP( + x, self.k * cv2.arcLength(x, True), True)), reverse=True))[:1] + + dist, course = None, None + if approxs: + contour = approxs[0] + contour = cv2.approxPolyDP(contour, self.k * cv2.arcLength(contour, True), True) + moments = cv2.moments(contour) + # Координаты центра контура + if moments["m00"]: + cx = int(moments["m10"] / moments["m00"]) + cy = int(moments["m01"] / moments["m00"]) + else: + return None + + # Вычисление положения + lx = (-cy + height / 2) * self.meter_per_pix + ly = (cx - width / 2) * self.meter_per_pix + dist = (lx**2 + ly**2)**0.5 + if dist != 0: + course = degrees(acos(lx/dist)) + if ly < 0: + course = 360 - course + else: + course = 0 + + if True: # Вывод информации о контуре + # Координаты центра + # cv2.putText(frame, f'cx: {cx}, cy: {cy}', (cx + 10, cy - 10), cv2.FONT_HERSHEY_SIMPLEX, 0.75, (255, 255, 255), 2, cv2.LINE_AA) + # Выделение контура + cv2.drawContours(frame, [contour], -1, (0, 255, 0), 1) + # Выделение прямоугольника + # cv2.rectangle(frame, (x, y), (x+w, y+h), (0, 255, 0), 2) + # Оси через центр контура + # cv2.line(frame, (cx, 0), (cx, height), (0, 255, 0), 1) + # cv2.line(frame, (0, cy), (width, cy), (0, 255, 0), 1) + # Вывести ширину контура + # cv2.putText(frame, f'W{round(w,2)}', (x, y+h+25), cv2.FONT_HERSHEY_SIMPLEX, 0.75, (0, 0, 255), 2) + # Вывести расчётную дистанцию до контура + # cv2.putText(frame, f'D{round(dist,2)}', (x, y), cv2.FONT_HERSHEY_SIMPLEX, 0.75, (0, 0, 255), 2) + + # Расчёт положения относительно круга + if dist is not None: + return dist, course + return None + + def main(args=None): rclpy.init(args=args) diff --git a/ws_nodes_py/videoF.py b/ws_nodes_py/videoF.py index 7de23c1..b3dcdcf 100644 --- a/ws_nodes_py/videoF.py +++ b/ws_nodes_py/videoF.py @@ -3,7 +3,7 @@ import rclpy from ws_nodes_py.default.GetParameterNode import GetParameterNode from sensor_msgs.msg import CompressedImage -from std_msgs.msg import Float64, Int16 +from std_msgs.msg import Int16 from geometry_msgs.msg import Point import cv2 from cv_bridge import CvBridge @@ -166,12 +166,17 @@ class RealsenseToBinary(GetParameterNode): moments = cv2.moments(contour) # Координаты центра контура - cx = int(moments["m10"] / moments["m00"]) + if moments["m00"]: + cx = int(moments["m10"] / moments["m00"]) + else: + continue # Вычисление дистанции x, y, w, h = cv2.boundingRect(contour) dist = (w*(self.k1)+self.k2)*0.01 - + # Проверка границ интерполяции + if not 5 >= dist >= 1.5: + continue # Запись центра контура по горизонту и расчётной дистанции до него aligns.append(cx) diff --git a/ws_nodes_py/videoS.py b/ws_nodes_py/videoS.py index 4afe955..4b9439e 100644 --- a/ws_nodes_py/videoS.py +++ b/ws_nodes_py/videoS.py @@ -99,8 +99,11 @@ class RealsenseToBinary(GetParameterNode): distances = [] for contour in contours: moments = cv2.moments(contour) - cx = int(moments["m10"] / moments["m00"]) - cy = int(moments["m01"] / moments["m00"]) + if moments["m00"]: + cx = int(moments["m10"] / moments["m00"]) + cy = int(moments["m01"] / moments["m00"]) + else: + continue x, y, w, h = cv2.boundingRect(contour) dist = (w*(self.k1)+self.k2)*0.01 -- GitLab From 9d916bf3dac54c375f54768ff274785c2b0e2965 Mon Sep 17 00:00:00 2001 From: "toropov.nik" Date: Mon, 30 Sep 2024 17:17:20 +0300 Subject: [PATCH 08/23] remake CVs --- .../__pycache__/state_machine.cpython-310.pyc | Bin 11197 -> 11241 bytes .../__pycache__/videoD.cpython-310.pyc | Bin 5424 -> 4924 bytes .../__pycache__/videoF.cpython-310.pyc | Bin 7087 -> 5926 bytes .../__pycache__/videoS.cpython-310.pyc | Bin 6457 -> 5960 bytes ws_nodes_py/__pycache__/world.cpython-310.pyc | Bin 4675 -> 4675 bytes ws_nodes_py/default/ObjectsDetection.py | 77 ++++++++ .../ObjectsDetection.cpython-310.pyc | Bin 0 -> 3048 bytes .../__pycache__/for_world.cpython-310.pyc | Bin 959 -> 1003 bytes ws_nodes_py/settings/for_world.py | 4 +- ws_nodes_py/state_machine.py | 13 +- ws_nodes_py/videoD.py | 88 +++------ ws_nodes_py/videoF.py | 150 +++++--------- ws_nodes_py/videoS.py | 183 ++++++++---------- 13 files changed, 252 insertions(+), 263 deletions(-) create mode 100644 ws_nodes_py/default/ObjectsDetection.py create mode 100644 ws_nodes_py/default/__pycache__/ObjectsDetection.cpython-310.pyc diff --git a/ws_nodes_py/__pycache__/state_machine.cpython-310.pyc b/ws_nodes_py/__pycache__/state_machine.cpython-310.pyc index 5da828fcddacef1d9e74834efca9d33940e72f80..448df5a0282291aba0f1fbb7e4ae1e5dbc662f6e 100644 GIT binary patch delta 650 zcmdlR{xX~|pO=@50SHXi{7Q4R*vQwf%y?IXBbS1r#tmpR zniEpw=djnZ*09#H)iCyNWn`#fSiroHVF62u!a_zyh7#5+wk-A(#S)GhhGxbTr4;2% z#uSywZ&eiQRV5ixShAUlzC+ch)v(WIn9EecUBi%~Uc*+y0hHH}WPq|X;VdmUOIwnm znJq;pMXiLVhHU}YLZA&b>_8LWrs(D|*RZGXi!*>s1licj%*ar<4Qdw1X5RX2rU{Hi zR%n*U%;Rb?{ zKwQGVKp@4ihB3uxvagyXqw(Z)wOeApxQbF!bdxiSl5tgFSt#&(ItjE#A+ua-9Bt;v;IQyI@s=GP8l Yy9HEQls4H>RD5!swiPQc7bDAm0CxqV5&!@I delta 604 zcmZ`#yGjE=6rH=-osDa<5u(QR9TW33MzB!u7pyD{VqxKpk|>H{MU)9CQftIP5PXmn zf}Mz1iXe7c3Zfq%*x8DO&aPp>*9;d9XU?2+IWL*ZjK1vkYDAo?lef8*ku$v#f*XDm z+AHa(&r7JH8~vyqC^;y~C}T5Hn{B~Xixo@&+XT}hlnrRuOuMbIm=1^W{m^{*Sen+l zhC07&pHb{Qphbl#MapC`P`LoYNYzy%RT;Z#0K`=CG_PW}>1L!1*n_<}z#xAQ2TOeh zL3u$cyw)`#rtXtN$!ufy{u$!X+heC!tjr9@1NmGCvw0+-xeyXzt<4%iT@T6seY@Ml&Raj zNZ4)nl8OWZ<7=Wf>rRO=ebpu$WPoWt9Wh{tuST9c8(JZYso7j%b}`Gd(HSXD`E7LO qJRREsxaZgLV>f-2)3QW?uP2i5z>gEVaL31z69R7~Cl#uJ{P78%yO>u1 diff --git a/ws_nodes_py/__pycache__/videoD.cpython-310.pyc b/ws_nodes_py/__pycache__/videoD.cpython-310.pyc index 158747401253b3a2b068a42d82be7bdc04541b8d..ba2f8bf362375731d8308f94b42d0fcb6321dd6d 100644 GIT binary patch literal 4924 zcmbtY&2JmW72la%E|(OkFU$I{9Y<->HY@_$af`HQ1J{b|I8I&4kliFD3T#(9OKPR% zE;~DvEn!szkb@KjeC?q@0qIuXivA7#9}3K=Ko5n^JvE1_zc)+SvK0pbN@5=8^Szlj z@8dVjn$5a}&)@&>W%t7c%laoZPJSwATt~_O3L-4Q601YMJYjv_;f(5bV)vbnW9m*) z>AM}*)GJA~?{z#=cavJb-l>~9OU625SXWIN{br{*>KpG&pwCMt`%|5%{&Z)$f2MP$ zKhv4%pY5FO&vs^+b;}YpQGaBKdc-4V&*{vGaWN(uk633uS`f`mE2vlM+3%UORQqdbqq?S9asX_ecl zdix!!+|A-tq2euXy(eRVQCC-GAmSj+byY+?8AZ9a!we*kQ*Emt)lPtRNc%YXu}6*k z2dG3=hY730KexI}Fu@;r9b4GKd1Q4QQ4ubFE21hq{JNi8j@7B&vsyJhwHgIU9;JD7 zKYK4u1Gyct6EZmJV<;ple;*^t+Ozf{;D9l!u=e=@sfrht4`HB=a6l`f0_qmlMpd}V z+o)~SH^xfgqV|~PHB?iLW4s~i=dC@q$55}#SYqrvxcU=jWn;8*QZ#AXqm@%;Y(3xM#DR(gnOT;-1KU{$$0u|ub?&rOd{qu?>ra4`YYqjk1 zR8Du8h7^jr!7x!T74}WH{ar9GuWDLG!XIRLOmVW*&i~E5bpGDd^6XR{W$f*Y?!K{^z(Qp(v8xJ{2=Wmk*=G$A{wZ5 zJuYN6@YOn^`g)cKeYOW)A_|j0M*iS9C+La{6F5)pX_nB={p~@dtGC0Tt|UEMf2K{w)l51x`FYeBfFXPfvir*Xh_v*6TsBPMmhL7S~Q5C0$)F-H@CG!Kmi@P)(luera>YZ{Gr!NtApMq`^Gy za+kTT%Pz2s>;k{QXZb}o&t|#Dud$tTFON0hapD6Xi}Xl4UPj3uf)q%DLneP*uwU_A zd)HA`;V7n%qc-fqo+JNo=1k5(zF~WOw_;{gzGjB2+&yczT2zXvUoBivui*Q&qIw+9 zO-$$QLkiEWI?Mo8B_u@ybID*N->eyUCGXOTX>ueI6~+h| zaPBOZ*J%K0dhtXg;v=Ixz}r)}zF&P@2rg)YW1Qn>;sO_0QqKYtf<4h$BHpgU$A6PHBcK{u*x=>>TbjI zHJ8s0A8jlmlR$kfwU*A zG_L2-#m|W;MUWY5=bh7*aoPz^!^rzcG4^JJI4Ps82!YV}>8T^sAjBMD5fx=AwvT|1 z?c1m!)ZoEFcghazJ%DBdFk2OECsA4+s1XbXGJAM8OSW&^E%R?Jkl~#u?IGzK6xs#j z6WNSVXdA^~F4vlY3z%>LRgL0_z$mo#_7zgy+j0uccpX-Lh{$mSO;WNrJOVMb*3@Vx z-8dD?S*j@N>*ngs`>X!VPY?mxOLz3t@&`+|+y2tsyQ?do`0bVUP3-^_DD8IR1kp*m zc_vjP^jKJOSdvj7H%JCy3b{!nBGMu9Ln5R(*#yxIA%h3Uq7mCD9mzE!za|n8@j(!V zj`zoWT`g6Y%M}_Yl>qmb<3^O`;X{;d%@h14+ocYTHVrKJ7%Oh0RZ(?OFk66ihVTDo7$oJ0B*0?AwTXL@HmaK=WD`b7a*S?_&A(5R&4I>In?T7a|9gaV zE_(IJDM|g$*)2H+xt>Xgaj5eiL_%iI69^2@tm`6AHGN->K zoIv*SmMgyx4ur~LWkdm(4e(_X$lR3dwj;tBJJ=GRGID}yLq{^*5Z zubqAnr@zG{bC3p51MYw|nEX90gaTcu#)+5ZrO^xG(ybT%Hj#92{gqQ+ z{5M6Ce)I(WK>=`!#F}5Chm1qLja-RmT?Xz>^&$i&CAXjTyO>&fRxRA}*UVooC7`XO z!#=<|PX}~VxH;vb(jDmLN~mVAI(diDsX4m*l$z$!pAouwJeo^Ox<;;@)63c84QQLabu z`lzZrP?*IsPZgGNwH>{Fh80+r<({hbF?XEhH#ENBj_oLrr!L7-E$(BbO0BAj@&h+s zaJbWQW0%)L=Ax}M6SmsijUtylYB^16N<9kVTepc>2)!T%W6f;d;~qn=Dbn0wo)bhO z&Dt4=T|$Fs@QpnNrt2R#sJ}GVQ5V*O|_Yr%K&m zDQ4oC`dBfPy1ArO(_(1Rb^OQ;B6m5w=LHV$Hq;ZcP4GDsnj-o=dSYcq*@fUe6>88D z#nzY(iU~kbc2j%GuE|n~vaUVH5#XsmS3WW1`1B98p4!uxg{R&#;`F+esEM|!CHjCG z;MD}QX*5;zXISPL&5~tV&>YKw<{`7d@^Nv!v|e5x!XXuLWTj`cj^TJD9!38!E1y$# z)EyOkZd74I=M?mwk#7zU-W+2iC*B;FeWQcE33f*Ijj{1(Mi0B$os7@2i6n)+Xl!zi z<~oNr&d0B@v+Nu@FGs&7=>>N28H^IW7w`_P<|3;MW`g0OUWqTUOYHKXjbvSpuk6qY zuO4ae3eCk(@9%~9uiD!xwET%FwX5JjU#|(X;j|i?_pADT9f!v0G$ld9Kfv!FP{R<2 zcMW~psfSCCE7@(mlC4xVIdK0k=&Wkq7wGmTK*(HK+-l@q7CcnYMXzIq3$bJ&tSm}5^ zBR;?_itKjxC4SRmZg~HCyDO}Be5bX-oI96jHVwp3wy&Ks=j*oZ1zv31*WdBO25kKf znEs2{SGuw0Ha1+oPwzKT@Qd#Xb>o%$;^P>?Cy2=1oIT=X^*mKy1G!4f1rU*s!?xG3 zaV#!(UMW}VD}m}>sI{+`{H;UD+mljqC4l5gs}seQ6}J+_+zXmGN+7HvvM%Q#w_|rJ z-qstNH}(mEMPbba&;(6ezAw^)>MPgAuh5ioM_;?=>fy>>DJddAflXtdQXY=LU{37d zskfr$v4QX-MUv2nj>kN-x`M4NUCh~HH2JJ#%_CC!n{BK{=KP0S>n7<)!9m*n_Dl4F>um&en@vw-)? zczYFD6^tsq5?arlYAp}smmsq=kY5I0K9WClTK-q8X*i)M<@|<1u@4*hig#YyQF=p3 zDH*aaBxTTx7+=}FlnjwC2Y(qno%GZ{*Cw6@&$>J=CW0cgvKHmce_**bq8 zWS@L6AMN)RfCesahrUDiYX6IagM)38c~Sg!Wjj*=s_@8t`^fbZybs*qr|(vE87CZ* zZG8)G8I|tw3sZOja=hq1K;^jk2Q>LrP7g8BI0}PpTw1_9K-F$+P67MY93ToD)0D6p zoAFHOhg=k9=I7=Y?ZpT8rVyKmv==No0g&z%gmIcrRR3 z9nRe#2H*jzL^|p;8pt(7s$b8ORbZnYWgfy@bG_yoR{q3eZ~;1ejL07AL6|Et_6HI} zAqJTT;z(&5A`S23AvNH%V%{bqeWi>9j$@vO=%5pdh{WZLSuLu@!I(Ct_PNVC)lH4iG_OwR#y__&oW{(PxZfaE9{$Q;EEJim zFx-JyqGpUt1V!J|fsLu285=PYpu^-SHi45sMoN*M=|~V$grYmzj@nCMBzomolM<=^ zm>r~7EKQ?&X_?7by-bqke@@aYgSISYwUX?wmGulbpeob%G-M!{U-spaEN~n+%CeIR zZ^ne8xk+Ff)d@FydD)hKsjU!bboY}&PDwPDItNU|38_j@Nw%r75-YRx zUJ9sWF{PvOzr&YgR*#X zObod|5J&ti)Hp@dFVw!JAKMqj)Eg)=6dYByZ=be|(|7xH%)eaKgudZ+k!!n~h>Tob zfzVT1pt~!#K)taNtGmcTv3wmIvRvf9bf)T_zJm-$W&_of(0w=PZz%7y+dSM_2>tH; zg?ad>0S4Vs9%WXkYM2tE-~(Dr;386ca=C@u+qj5<_R>!bb8QG?cQ-6YlB;%89x z)?nrPK=cD>Qi49^b&{(N4OX)11#Bh^V!F(tusFNCXwQBCh^tM{iJ_T?(~oNQ^uog8 z{0DYzzBVfi#AUeqtDX<9AE9Xe(td6rz8W>G zyrmlI_SJ7Rk6 zC$~N6wfw}F)kyfXc57wbuP5;W*6K+!3b8gkAGO-jk7HllXn73+%iIW)x8A0~?Wh?h z7&!BrS7lRR*Hl^QiKZ9E%B)A|;$~PUn#X@;e-eKK1K+4IVbu6zW0eUe_(P{=3R74Q zjhZDg!p1WrvckdBer#AqEqlkPmNIS>O;;fUN*2n)Nm$S2u#XFa!`$Mw=);VIs7 zhxA_YaIbW1ub}sa`+G$(a%``p_eKxj8#}f)toO$Idn2N(_a?;TQ>)9s_h>REPNnR( z#_zb8+9xi?@kSY|^qdgWeOypE%qNpmVn&=khJUKRdPcl*Y;`)B*#*3N8Whg#8+)&a zvppJgTf)GFzgksx-D}l1uQl)CZdGy3hnT~Ln?&tqU1j^z&&hz~LG2FOKP+@9h99%) z$LJK>lA~DC-Dlssuokua3vzv9$J>+_WE5Yz@E~>{#1~dN%|OsNgo?&)d;4?a&8G09 zYvb9;=+W#>UDxd@r}1$ zN?BhT+3v@@y4<5P25c{)I8E!5M2blo1Mj7m;+3xu^jYe(3|@wGh9C&3GFPL`FO2T= zAv(74g^}qaXx<811zl*TFTI!J9VY4|zyE&bR|ib)Ol!QeA@I(&I&m_);?Ks3gwj`d z?~e1nl#z53e=FHB>zfx2C}4bH4qh8@pvD_s;=AoAZjv+Znc5dd|N0^Nsg)Um zi90KCT{c(r4Xom}+k>DbDFCa922cpU9(WR0HP}-bSr6bAa)%*KSO`i7bWK<@>2RYt7=dy*W9Dq+*z_JbgLK%!Lznrlg5CbI;>SES0*xq_(C zAeYe$SECA7R_q6>D&Nz0(o6W!oa^H3aqPN1z1BYH9I%k8$7JIL=5U*xHP7%f41a!x zPx2`?$tSTs#dc<1?j-AjKn*N9n$iL0(c)jCOA)p^OwOh3L*6yJR$`=9!V<*Pbu%@m zj9s?N%1AqQc{ii?tQYRth{BeIK)sQX=Y@%WJ~Vq#C&}y@-E5jkv#y=m=(8!`bJFY} zDp6$8(aEYFkefXq{ys&+?_XDtBxEFXNyr@LlCl9=qK>Ro%R-K0q}aB~_61gPU4!9waeQI{IiL+No(XSUlKbu0N0wN_Ir;RD3U5~iHU#|e)DdQ1e<(GSOGQgX8 zjtr30}6Nz#~U?@b72Oo>fNWcj)!0`eBCc!i^xMAkC_8UV^_|^Wd|zD_GGvuKWSL z;gD}2YVuo{%5T#e;?E^zLvP7znxLlgiP#i5649EATO1UV0aALxpRje~w9(Bxfxh4gx6{oEv+JPc61K-vmaugb&Du9%C;9GBl1c2x z3{?pVgNII%P4n1wgiU%(^%z;yF5hL{JRq@)YAl1|K=1FuISNJ)-9p3Y7ST#+VZr!l zw>zBV9vhFL)dJ+C52488{O^ZHRFMl58?5W>D0ePP4dq znvbPfk}L%peLJ~i4A@E<8~XlxPMS-dRo)L5XR7ihl%p}w7H!KX5-ZXj3J@`c)P(%0 zu@8=?jVH!B->^^=@hU2Six(@VybBPOZJ~$>lUS02my}c^8K-!1EHYDdAkcJmP<~;F zGBECwCCV~#0CU4fypsN=kAQSQJf2{j!X4FII{GIhM8sqP_1;6hDB$*SS$yBzMPX4f ztDpX~{KxBm*?D+nxne0Z@Wb9=3SPS{qpjOfu)T0w4%MEO`q#|Q>Ih-M3Dlp zZH&QuKgSihNpgf=Czub>{Q@E7qe>S>f-HjEH=0&dBN~5#)hT1c2suajSTNd^LnxS2 zAL^@5aE-^I?gI8c7bebR$=5+b3sw8zs%95=$Io9>d}|x&`;Q60rA|j{lCa8MZup_U z)t2vsn8w5}|9Q>WdGn;-d4Y%q@5(yiGFxw5lxr9rkXMrPG=j^22366C$}?J}k!Db_ zA01H9)>p{CUFn1MAS8AaTafP^L45M(KIkV`DpPV68-1azHbXHVg$boAs<^bcyyPz4 zLlmga-BhFV@6O$*x^uU0FWtK5R&P}om4z%dQTA#xKrB&q9LdBNYN+1BFo*0{nO?Xp zNn_jC+ zwuX`)z&%AWd+MVtTkDX{x!(uD576Qzba~{~7NdXOw3zek4Foq1tFW2%%(5)*uqn=2 zo;%3GIUh62$j31^^E9(AX5Zi?a|EGqgpD!e>CECU%F_pGSaGPPqJqG6Ro-=5k>~_8 zF1YUfju-TH4545?KE;xVpIVYv8me&k_ zPrT7V3dpx1C01aP*60FhA9tX?@to?BfBo2(r}O>E5mFMleY8#Mr-^Ci literal 7087 zcmcIpOKcp;dG6}h^bBW)!{LY`C9RhG@MdSTmb5hD7>KaD6v?GzFqBNvigvI@qdC+f`ruP;IRwZtFmlSN;DZeVad^%p$sx-3SI>}? zxbh*J4AIr~>Z-r~=T|>FrBXq|FZkJiZv5SQn)VxNoc)<-e2gdlYZP4LEYuqG)wtGX5D+D^k!br$9udCaxLLVKh!GUzKdO6bdl<@RV}v|VXb z+GC9|rY&mR;rT}z&-?m;(WvqgFYu8^tTFD_cyU`7-}T22G_1i^)l)AYgF0=w&SYiL zPwsibYx{{WmOI=>-{@SY-4%Wu`}|JZYf+bVCrUo}HdXF*f+#`7ncKM~0*+Bb=7h%s zFN&qn?4XE)s7dRe{TYKsd=nL4YcQ@g^e5T|W(3V;9bhjjOSV!B~x7JTrDdjZL0>H+5!gLXBM-j9uiH)z}sO+B36{lOIkdQ~YXb z;oLfZ{fMZzgfA|mm)=+S^k6NhE!wXoSNR+K%`@x2KInamzkQ~6I(g#&^W8U3xcrT5 z55-#z;(TXW+D)(B-1($#9FuY!Pf+nw6zEu``g0Vbf+BdJYudhbd-eWwVc(c8OxJZm zl0W_#in<>B2*bf7dcK6B@?@{;%hIL~-Hcjp(&+|GnHw};P#9W*9@^~R$$(Q-zhui_ z(mHHcOrlE_|Ml_p%}(3DF1EJ!y&Z8~bmE!o4`cUXe0{wagq-RTY$$fSdoR>?0`7M{ zxz^p2&dtzkuXFFi*J(4*3$@;V|C~Z!bKM{c64$+UGwd{>?H{6&efUb!V^Vxb0mO@o zsKQ81?j;`HMmuhu9z$HlsJMcHh=_>@TT_fATW)Up&23*ubE6l5Vcg&FdSN2ts2${k0cU)GOMGN(EggyDz*~uS@E$}yki@HgQ4c*{$Cx7&$>B~P1vo<^gxiRB+;$KRD&DTFpyf6@y(sAk zHxBlFS!nr*8+KYPvYjB>=tw>4O3M?%+mjZ8=C@|#i0B0PN<_Yh<)yf{9ydj>u9&s~ z0d#}&3Fj)-REhudmu6xaGIe`h; z2kwUGw1?ygi@q*n(B1RHu=9|#K^Cd4gol;}Et}`iDqv17&C?%uo*DfS)h?v?Q|%%m zrjw4OcA87gBeP#h0sVSRJWfm4soWn;^66+&NJccM&!wZ}ksH#N1-2-Y2WX&-9;ulG6K<#b1G)J}E<-zRZW(!D9j zKEthu>Kspf6cLud$}uS}5E6Oe<3FV0Ue*!grO=Wp>P9yEUunhg0OMpXh*rF)<;#)vAbA+XzUxI?I^_7s z-^16z#d1M#UXW#q3smH(Ade$1p^$7(d_wQsz&D9SYT3BKkY6@HyeyC>TybYt7G`BR zBVlo6_R|^AA#Ty^Nsy^1fB87|nz(}r;wsH5HG4w%Q3BrwCna-nui3;sNm^OENPP4; z7=c@pv^V{rwV6okVZf73X&{D3JM)gxB(Eq-17EqWE-~N%#koPl0Qs_v2qVB0uvD?5 z$b&c;w$$N8K*SM_lR1Q>0QaGnVb?qcXYj;pD2jRk9@Dg{tZbB7nN{=(tH66YNm(nS% zuP1B^H>oWRZ-eX4;A>$YdQHQv%RIDj-_o{h*n`0|v~##~8L(w5wO|$IviKg>5>*sH zmULODJHn%`KR_X^eIH<5T0JPkcf>k%IU7M3-s^;W;yW~L14Z4=WG!gC)ZFx<$Pcq9 zFFvKdv7i-o5R6IA=!ZfJWo|D5U0H~szv~^reX)bj;v$LytAfcDu-0VzSI#r+^ph9Z zHG)r1*!5>_L)J~)V_s&i|*Fk03+?xMj$fqRSxrV z6jD-agW8T-1?855Rr0EZax0G(?NCVBAbcS(fzkGI3q;WGfyG;ETW`Sknug^xAsBWGl5gxp!@JF{eg6ag#;vyXWQ~fFI0Zu3YS6GStgdx!a6!OSPa%q8t&4EIm8Av8Mgk28w0|rE5(I^rw7BUfP`~Ybh z&S*xz*wXqXJms`Ful?YF@W&JF38EyYRFjjA&MO2WRzIItYBLJus-zWOoYF)KNTk9` zQ(D%hSlu6sn3`EkG4nICU+s_gYyAsp4JRzsG?e9wh|9442|k)m3=qp>UfH(dClL_I z=ksal5J*L#lQL=(RLjGA99E#-qnvUo<3|>}1v?x=ysstIw8q&1ay&oL(<(5)1?+$a zIDu`A^9kfq*ir6e{v_s4oSpyjJkVcDOW+aP)`193-~_a?K9WsRZ)s8JB#8d11jaCgpHE<{!C4sF{vOhX{thle{xQ*bj&KU{tF#A2-h_mRq#7rm zgh0UcBLoJN#sSg}bz}Lre_H$b;;;7~eY93Lr4jm3c9|UUX4oisWTQui06n_qH{DXQQMglJhmeQjXRqKpDMen4x ztwp+ycGi8&{Tx1?{>w9KgB>y^y)uqX#3y%YR^Nk;*5xf-wNUb)Z&91;t;iC@=Ah^ zd<^XOeCX0C%$^b`*`FMfx(pQZ-#!{nBVeft)kpQo?2f85u(%OK ze6ABE1b$>`Wnpc_U3dWewmiEe$L4O&-dT2M@7-Iu`@mhkySzZoI!>gGcn*{*?YJWn zpCejiE6fVtlZF@Vi60XkAqrVsCs>IL$%@}h&Rm#nsEZE(TI9lectQbiWwM6ciP)rX zYoFX*UUP3RtgPN%_|#p!^T+p=79NNr8ZzQu8%#rbzyKDjU(C#gV%`~1W#FP4?7GM? z0PeOzz}z-s%r0OuNEV&&f#}mWDXppc6eE}`bD28F()L0?kyz#l5K@SWk+{pwtB#2v zwtI>XC3`5@CbEH;oN1Af5v0soDUdR2631WQiC0h*aCbHVR!mkfOy)d)PcJeZCIR&L z+%!$#!%1Cd1>M0dTi2^b4fk!djRLjI8@c!NvQc4|5Cf|W_i$$FUv&>3x`}$8(k*oL zcU@U<-FAofLaG;C_w$|?W+Q^`|6-nsM^umx5=6Bu4YCDIWFLkZ17#FMba`J z1;-p@51hB0dB>@Y2^u+P>Ex8=X!;EliM9pD$=Nf6m_)}#nZcDBgu`$(2J$+B-J-{$ zx4Ne3^v@O36RjAa@{h0r<%EJnZH4ql7rCS&62*}Ao=O=@C*JHOyz?-EUH>!9FOb3B zg*wjNf5}nNeJzOog=iznkp^K97LcW&KH?*dg9|DPd?DNZ?K8RQYlEcjwZ)hI`6I~1 z$x-^>3er)y%*EFzb4;v2Rw&`8UF28fSj$JONW|W0zj74Hiu27KcU}F5D9ijIMB-Mj~L*v8g4V%4-1Qq IQ-GfS55E$WcK`qY diff --git a/ws_nodes_py/__pycache__/videoS.cpython-310.pyc b/ws_nodes_py/__pycache__/videoS.cpython-310.pyc index 5e75d82a46ac627d5990c041e7dfdaa9668cc6d8..d8a3f980d8e5beabbb17ed8977c981aa517661c2 100644 GIT binary patch literal 5960 zcmb_g&2JpXmG7$V>FF8H48KPFk|Hzl#_$-9Bx{WTyQ{S&ku)tp844uVE6+OUG^d)w zA$z*V)lG`z_F#8G8PY|-9+Ch-;DkInhx`XYfczPCN|1xY^Bxl95bf_(&kSi%$|24S zs=7X2y;t?#tM^e+xm+~x`^^vky!oqn!}vXQ&i``g+(wCifl3&H1xACuJYXH(;EdX4 zV0NsArQ24J>(~ujw{tNV|wfUgjsWdA6vC+mD#++ch zGtrpnOg1Jv)kc*W%Z4b3;$uS;eePRFR^yVWh!IhG%obL1yJrm)D9CgoQ1u+vMm5+s=V|>+Y4i5HX~GVJ8Tm1^FOoSiQh%T zHyTVB4gSQ~WP%C)*lC!;6xL&-VTqiu@y&_6aPYOC7?#nC4NsJ@o7z9ojGg9N2pAdg|YPcYV+DeV>Tw)7NQslv1N7vx= zGsq9Kc`=eW+lB4oHh8V)%Fk$JNzYY=bED_x%6e|BKUWds=jKNB+{AF*pGYRfOv-+0h}q9gF?T}ysAA0}jMH~YT<&9o9-=>;%!n)E8|Ns@ z_D8RZ*UpX3oyYw-A7D-{9|4Pd1+(8cF^;c_d7}L}CteBz;`nA=*-fw0+*@nE4PKu% zz;#vL@}UV~%T1zgyQ%X1?pMS_BBQc~@_!bp6wNPK{R>oz?aN6F>FUq7uWv;i|GM1X zIrR4Abs5DsuRn_2NAdNI-F6_T9YURBw|nrF_Fh}~(b~1{fpTsIUS~si@68Y~QV`2H zeEWhLU31-b*iKyc+N~gJdO`di8ri~EZaud3*0-^FIe{jwgeAPh!`JM@t+QjuDa^`g zRJ4hhHenl*I6yb2x#c%^e5tI>-4F~D{-(DZB#Q66^GeE^X>`xO;Pv&4P8HZ*MsbeD ziENF@%lH8A%~#@;uMzY-RayqGLQ0470`d~Y1awFLF-EI`O$%<$9?rWp}_7Ht_Bk-C#e{cM64DcTo7CxF^OFWs( z*IrY(c(;qI;0$h~s>FT*+EL(kym%*8W1V*BHly7z*_CeGKJ-;7ldtQAt-x1v@$N?4 zldS-F!YkjT0iLe%3fUgx4ZY3q}Hl$ z5`z4_`ufuH(yIIx9f8E4?6vjPd-Y{C)|9@N0PXHZ(2lpD1Z3QliM}eG-MAGlJ{3Vn5ha z#q1W4w!*p=To*@=W7o~JU0bHNfMpCNrX!V@!)<2stNaST!tlp;hR?7mKE-ZcE!Z>1(f*5s8aa+T_%@P_6hHqJu5L%D`5#7^0t|pGvu_aY2%0;u_{8C zBi_qth}>U+u;I=v3m$(bC*Kt&>U*J?IjF=wGJ5$mm*(AkYNK{ieq2cN0|!Pt&{d8MU5>*oII2rUb7WpCJ(*}F!0k!K;<9#&H{7AyPCjK>m#w0w4IqX~b z&ympZuQ2P6R_^fiiz2#!qe+fv@FPv4qGfd-?0_r~I;9AmdOYRZoT9YP_^gph4nYBh zQ2Yck(rrt(ZL|?o9p}JxGo>*KvFsBB$-^1@+)SQkDBwwRNiofVN^aH=_GRcmzt0B! zO%Avbk$-0P@`-a?N>Oi@K|i17Q!bn|PkWq&ixtjiT;Q)qfN$FPjBoN=866vV?HA}! z|8J&qM&*p6;ME~jtzm__cK4^maaH-MMOBWz-5XGkMNFTP%|HlICNaxy6nJFHPXF@! z`SV&u{wpDvIddj13<+Wl(xc|yP1w&Z4^~TCgAE-Y%Q~%a$UKnR$y?~kf1xorrkl!! zmXZ}UK}{9Ow5_^#R+sK5r@5CbL_s7~Wntz1%Bs7(dgtSttIK!$`YmS(M{bjn3T1Ek z?bcSJa*x^~*;1wKfY}DrNOnBE8=g%GdsL`QM=C@)Fx$YgCB)mbc4w-#U%*fDSyhgjmt(a_Y`8s7bc{a7iP~v zDJ1Nerz~OHCW>`pLfwnKkpx-76NXd;nZjcy$)`okI>IJ(6k6eCjU#@C{KV=_^d@`N-lepP6PBw6>dLesDpjL5B}UV! zzM1??jP2y&Cn4haPZ!hj@n|}(^P>t{Q`9OLpgxA{JBr+8LO3TmTvB#Co?2ounMkW- znkf@VC(_CE5-3!s4O~yQJ&BxU3fWeE2tN&YjGV#02#5Vw(=vGE?C}3((n>B-FB>D- zBooMa>`YQK2F}DUgPU*-`D9AWCevar&4csQpGfm0S#lkkJB4Hd*eV(u>i#+>Eu_vS z@0?%Dabe@+ zwbqCR#V-j75x!PHy^oPR3RrJ2i=RM6aRjse<$qRxx%|7s$M3J#EM*3Mm>s6%b-Oa! ze-H%+iw{)6lg;~n*h;qaxn8`;blo77OG}Y zU2hk>eY3KI%ARC#r2ebrrh2+Ll-b6<@we0b+x-M#bR!RpFGx4u$eQWk>M zMA@6|0RBPQaU>I8sF7xd;SOS2WqRR(B(ci>pna6!tU}82SMhXoE;hEIDSjvr+eq)x za0iHtl69m)a+%gvKUk@+yB{vCu6?-lvAcHfdk^j}J(SxtWyZT5@XOmr)edXZSs-Qi z9oY{6k_pNs;BvRU?;=^~2mZN^FWh~&E$muG!9)2y0>4j?dQ9_zxhkJ2bF6GHXt(fS zRiHS`3!8}6^8F+0lPccaKMTtgd!*PF@_PVhI%H;=bDPL$_0gC5A-ePq60f2vB9yim z{fqEo&hxk6(>N}K&8+8^WpRhiaL$U{L1@kSgjq$9jlNl=o^>Pt7O$9NY>th=A5J08 zv-rzG?|}x^9Lk$0&Ual^blpxQb^~gcT=&ymFUV#j1^YTEkmPeEZCI82SwYhefONX}qEO$sa# z6tZKB-sCWn({FRIR z>--afFRuDtK<|VLAH86ol-2Rt;m-i14^l$J3@Nb^lRu}0U?9zd2l_#0CL{k3=bo9a z_1|Z%Ex+)jDs^o4QqUJ)S|#yEjmCb6yXGQ%i?7jp5t$j}iHSHH(k6o}sTyzja4CsA zIGfC4JhRY+=AOHupJKC&Fc7%UcJAtKHk%Yl{ZSGn%Qo$?J?5?>+sb0Fm^H>D%Np5%RyHJ!$3x7ZL9$v@ zOU)*yx+P1TW_AO4utBf_f?PKSBm_B37T9ABx$GgQTyw~w5FiP1iI8Is`8llfy&@^e z7CZ2ezUmOs$Qlrz1$Y&nOzq(9gg zDi1Y=%fpS4@(9xwHE#3NOO2;oecvdL@+?pDftReDcgJ{UOBYLSeqVzOwyM5T?ie%F zmP??xJpRzS(FAHw<0=xKXLQC0!;z@tc+CgCEJn;~={G0hkg` zn_dthW6$qA5FQ7cA(NuQy-E;Dqt*loy`V+|o&6cTLii(OT&>KwR@Q%^tuxNJ{?aZR z+~DR*t!(lHw~!`ylG{kFUudRQPOfUDlpJ1hD}Lw(p}W?6-~|=2S7T?!u#mU$(NN*v zpe54wwL=VW#GnH`)*PK1pcn`g-(liNJG6Kr);9HDLFY(~UumyQ#h?5_@34-}ZKOuW zjFOvn%wm0AkBuHRk!LYz-K;aTr+DgsMoIHD=m4Z;qAVZa8SG0AyELfi5Fb9Un1sdGBNo-lwr&=}Pk&Hv8tXv}%<`ZD+M)94D}$#}9}sfS}?7 z$_GUL1jM_Ay>2HStv)NJ+eR^6Ea`&m;`pyXO1k$u6ubh+w=iEG?X}$FGFoJI!-d%f zbth`JyqZk*if`By$)Rr*-+$0SkCFYBEq_bG*si#MDnByS+gzP#?MeHdUujf%<)dpfm=0oU?OW$;e#UXU zz>6Gb=APfI;Xr(ZOt<4ZrykPTzy@LRVhUNDUz1m&3R0sH)=!rqE}>Oi2BArWGzm*n z+N5J%+i+`Ju8`*Xb^wKOcfGRhM^fLq`_7S-(#QdQt1qv0$C;)fP84@K;ti@Vf|Qte z4+-SXzEiGvpSW)lQKH@@RIwFFnz7#8`QGBxfJz+O`rg8RK(klEGRg!USiCDL@0L4B z9ccf)d(F4{a@&(iZZUw|Vq-guidDB5Mgm5UGoo~yc7Eip|)4AQBjvTG}+pg1UhF;_~gYLQe-s10`h|gF3N^R@^TmEccemgb)thgFgL{uzY zlZkM9<*el zB1C0RCUCL-{;V7jO%J_9;EGUYx+dXNg1Ya@sc^d*)`VA8>RZRYwfc9sASWruNIrLK zenmjf{Wh5pH6IQpb#miNa}91#S`VLm`e126sFPPB0m;Y9YYU4DD?-`k52?ypU0Zp) zyeNli!mUIQyIu9Ya07Nj7p*iqz-MdoX!?!z&bXKOj$A-)s?l|Q*Sa=vZPWN~yS4O)x zh%cl=kztl?F^gSdwqdhd%+fFE)A}^SA8A1^uyK8yjT<&{?WuQr-WvJE{*Tr{%Ady< zJ_L!`Cc>>rt)oY56CuK;hScEt0RjmGAbMUyIAIFK9l-TiWN^BV2Mg>;)W0Yi~?c$P=iEWt8j}dw8%u0LR^Zlnye^2~`I0mX- z^uh~WSPw5*;-{+DIt6_)Tid7vf$Mk8Q<{xRxFv_ZdeDU7Pyr2oQ^*YEUG8`k5WeNK zWjchlRhxplYP>Lm0n%)Q*=&eqn917H=T&&x#&F&LW!h8 zC;W_^Ge0p`6o+HR-6ZmE3->;53t28AF>C! zr)_>|?$H7qM62b)LEv_%WAC~hz&wji{o=~vgW1nNeI}D$umW%4Vl>)t@g;~f{7O}( zu-n~Tm#iXLRVHv;He9LKd};bPvohlbV5q76u1l+`V(hnfJx4x99oYwSOG~TD?8!>g zEQfv1>1j5k=hPn-P&{t|Z+3sf3g~L1D@V!vYoi;o95Q7Gh^%1;*!hkT8L$D+%>+K{ z$UuZnmXM5W1tzp86`N>D6QnpWfSeNF=#$#MzRx)3IU zCy}PO)k)(UsB4`JzHDqh)V}=tP7dQ{;~Zu%7!PuLQWI{JLoPL`b;}ntj4-6?(r?ra z2kff_^d6+%UznYd&S)p!8H*{JAIei;Hawy60XT;7XoT>>OD)blgtUDY{t{ec1uY)q zxja@frEMj|!B-10_0JrR#zPb{$64gYDVNePPCnM-T%7@N48f)oa4*K;80tpiQDByl zI6tAqqw&~;wyuLC+2a_Gbv_(T#Ce|F2OfH*_=$^;5GOQR08T;c5pXLc68{+Z$ItS= z%@=3HzekgCW}ih(PsD^=GFUly zpxuEu`=?Bx9x}5^CTOpB8akec)6vD~68c=mSHwC83QCWpJ4hT+99O=BW0X(EX%irF zD}j+d;RTg`768WwgM?c;1!%AU>@o^^4WnE?yb%{j_t4P$u*+O*z{V1j8lR#$_sUkk ztTc3!UpO#2lh9%*2HpGt{Lo}P35|}&lVqWjT0!IE-{^qBNmT=R6WTYxpZEH@pFaE{ zbZYLyOG1|yVIvm5R5;zK%bd=~tt0znw~Fxy5Z;w|lu$0e+S}3de86RA^+&eAubt-x z*Y9ENfE9-`bTUuS+x6vhQ5_Dm0#V@zG7(^TtQ`TGLn-=}35RPe1Bu+Z<2}c%huBp4 z$YcYEDu_byuHc@y6h;}*+IP--nzLcZd9btwXKrk{dw9KYcMxm2#{~4cPW)5KeGP(1 z-MvFIyIcP{|C_~sX}`Sx#?t7HCRiBKw+8Y@IL%*P@8Ed@HwrBepRy*cIgF)hfvy8k zX0+CIgaTa-^MvCnPn>c*1K(FVx1gNl@h1?irsjQ!A;k<8e+J_H117z!Tr`%W&)h+5 zwD-2hPTi}!fxFuhA2rb>r1|PUU(wok|IaGx_wQYb|8@WPHGcgok1jVgImtddCXCW! z`?vdj4y9RQlkDN~@6ff!@z1~QXYL=9g&fn|07|onXfQ_=W7cq%X3R|c>dAc<7H>=>C=@b z&z#AhU2#n~~)hGqk=ltMq&VH+#x2O?4xe zE8tS$i`XN6yQjNQGDlh)u2PJwk1WUfV@%w5L`t{*tD^UP!iqmlN7rH679PpNp&1l!O*N zJW9TG_QWGJk~ay-lrk)hzsc;^H}nw%O15sIM1LFgHp{bdrlTd#Zs6QoQ4fK5?n46MY%fwn0Y$ce&og+1i$rrzy~jyA+}MtI^F;v9@VCUDCQUuT zL7RW})@Bk+@`7K}+-M;{lWNy8O#B^ngaWA@I+#MSG~ONl+L?#Hncj=p%;H?kt%?#BYs*%4zi2&hwCn`$4Km*HWC!=x~o)e$Em94`7Vl11lx_) zUKcoG712ibu&ZOQ&N*#`T88*7<ler@Eo5CQ->&IM=y delta 20 acmX@Ca#)2spO=@50SLMRer)8n5CQ->0|i9@ diff --git a/ws_nodes_py/default/ObjectsDetection.py b/ws_nodes_py/default/ObjectsDetection.py new file mode 100644 index 0000000..dda686a --- /dev/null +++ b/ws_nodes_py/default/ObjectsDetection.py @@ -0,0 +1,77 @@ +import cv2 +import numpy as np + + +class ObjectsDetection: + def set_borders(self, frame, x1, y1, x2, y2): + channel_count = frame.shape[2] + roi_corners = np.array([[(x1,y1), (x2,y1), + (x2,y2), (x1,y2), + ]], + dtype=np.int32) + mask = np.zeros(frame.shape, dtype=np.uint8) + ignore_mask_color = (255,)*channel_count + cv2.fillPoly(mask, roi_corners, ignore_mask_color) + masked_image = cv2.bitwise_and(frame, mask) + return masked_image + + def set_more_contrast(self, img): + lab= cv2.cvtColor(img, cv2.COLOR_BGR2LAB) + l_channel, a, b = cv2.split(lab) + + # Applying CLAHE to L-channel + # feel free to try different values for the limit and grid size: + clahe = cv2.createCLAHE(clipLimit=2.0, tileGridSize=(8,8)) + cl = clahe.apply(l_channel) + + # merge the CLAHE enhanced L-channel with the a and b channel + limg = cv2.merge((cl,a,b)) + + # Converting image from LAB Color model to BGR color spcae + enhanced_img = cv2.cvtColor(limg, cv2.COLOR_LAB2BGR) + return enhanced_img + + def _get_binary(self, frame, mask, color_type, roi_corners: list, is_more_contrast: bool=False): + frame = self.set_borders(frame, *roi_corners) + + if is_more_contrast: + frame = self.set_more_contrast(frame) + + frame = cv2.cvtColor(frame, color_type) + + return cv2.inRange(frame, *mask) + + + def get_binary(self, aim, frame): + result = cv2.inRange(frame, (0,0,0), (0,0,0)) + if aim in self.__settings: + result = self._get_binary(frame, *self.__settings[aim]) + return result + + def set_pool_masks(self): + return self.set_virtual_pool_masks() + + def set_real_pool_masks(self): + roi_corners = (0, 100, 640, 340) + + self.__settings = { + "yellow_gate": [((99, 122, 101), (154, 255, 255)), cv2.COLOR_RGB2HSV_FULL, roi_corners, False], + "blue_bou": [((0, 0, 140), (360, 110, 255)), cv2.COLOR_RGB2YUV, (0, 100, 640, 250), True], + "blue_gate": [((0, 0, 140), (360, 110, 255)), cv2.COLOR_RGB2YUV, (0, 100, 640, 250), True], + "red_poster": [((52, 133, 121), (120, 255, 255)), cv2.COLOR_RGB2YUV, roi_corners, False], + "red_position": [((108, 0, 112), (360, 255, 255)), cv2.COLOR_RGB2HSV_FULL, roi_corners, False], + } + + + def set_virtual_pool_masks(self): + roi_corners = (0, 100, 640, 340) + self.__settings = { + # "yellow_gate": [((0, 0, 0), (255, 255, 255)), cv2.COLOR_RGB2HSV, roi_corners, False], + "yellow_gate": [((80, 70, 70), (100, 255, 255)), cv2.COLOR_RGB2HSV, roi_corners, False], + "blue_bou": [((0, 83, 0), (19, 255, 255)), cv2.COLOR_RGB2HSV, (0, 100, 640, 250), False], + "blue_gate": [((0, 83, 0), (19, 255, 255)), cv2.COLOR_RGB2HSV, (0, 100, 640, 250), False], + "black_position": [((0, 0, 0), (180, 255, 20)), cv2.COLOR_RGB2HSV, roi_corners, False], + "red_poster": [((170, 65, 6), (180, 255, 255)), cv2.COLOR_RGB2HSV, roi_corners, False], + "green_poster": [((43, 136, 5), (101, 255, 255)), cv2.COLOR_RGB2HSV, roi_corners, False], + "red_position": [((90, 0, 0), (180, 255, 255)), cv2.COLOR_RGB2HSV, (0, 0, 640, 320), False], + } diff --git a/ws_nodes_py/default/__pycache__/ObjectsDetection.cpython-310.pyc b/ws_nodes_py/default/__pycache__/ObjectsDetection.cpython-310.pyc new file mode 100644 index 0000000000000000000000000000000000000000..7e01f325116d1b87f9b1355b4e2ac15aecd391f3 GIT binary patch literal 3048 zcmZuzTW=dh6yDj3y}rav)2NiT2vi}V6{4XoH$f?FLlYjNq$F*jQKeUU>*k`tO>R9gxb=g%ZgI9|cy=Ft#&Sql91&Cl?HaClJ^Hf-+l(i3OB$jiKfK9^Vd6zUSM@l&YF ze43v|UEwpBbyiKUG`2*TrJsuoRn$(x0wx-k3KVM3<4a?RoNZw*uNr%WjBS~@nOhrX zZXZeqx|u^Z?IuIN#m!&Ly&^Q5#-Vn!#pPCBm^JoFS>ahRck^Oi+P9d2mYcKO-8c8j zd6~{+xo=`lk12;|y^io${{^*6%1Jkaj!&o4T;`mNG?x{*6%66hu_(8(Gin3D?ZzyL&1}%hod_|c(e2?p>>uM_843b2|e%S6N znX1Tk1XU(Nrpj&w=_7SIY9?(dd}^R8Zc9}qU2s2Y1x;~a$n#k3^VPY{c1z63tw+1T zww%NA>T^4(zmv{2x>3wYPug6hey2C4bFmS0<7{pcpvxWXBo&$8XiJXe5I*Tyh%%dI zRc61it-^mZw##bNV!IcQ1sE9|(g6<(F`D?M(f%4rW;|m#%Zx1(n3!l;xw+3CT6@-> zz30Gg6BD@wlnZ&`nUh=l40s#Q3c%C`6R7R#D3Du4L@DJZjcoI9(i@)HccIJIAUso* z!Z_+IMXe}P)hvp|qKx=jv@1MUR^g%Ca66mVhdnjFvb3`5-&$O)FWtNaN_1ijs)SMm znV4U?`NeH;qtgNIPD{w9P=^EHT*n}}P zc87M3e06vpR&IfP=JJ6h8Acxo&mL^|Dy-^)KM9cU1Kcc1nyGvr9`GJ;Cx}}5FjPT` z6qj{!=jEe~zk_ipF}=ph>>ORp3zM^4KJpG}dBZ|F-BXP-g#e<)zOiP=IvPliXNh{- zk<7Y5?04GjSSO{_V*|J*;HZ)Jkr!<_rSLA>ef`*y#(l?@rN>~LUe*#64vKN4i7g}z zZZ&OgQ|jOjFHGo25d@3&p~NusFFkc~!?!rL=9QNZY>iK!BXs|J%og3nNqDAb^+Pi4 zk`NFu{Dw*rGqhMBrz3FtKM$NXF}$A)oeslGx;XMPi2iekyE;BmtN$a39t7e)Ms*Zl z`^01-X!;%PBX5uUII7YUaopbVn@C8?ZNy!HoZVF=t!ZOfBA0a9X(sS^9O@Ch!iRiR zGlL|zx_GPp#oB#;;oj2HFprV#!M*$PLri;M4v?m#G0DL*1R;{*C^J5QpUzA+t6EjO z6Z-e>?wcp#KQZ_i3Fta&5)e`+l7N8(LqZJo!U*t0h#?8a2tjuIlcBMdr$vy%)a0lx z%TFSW; zly#9m9-=x5pjYxXb^k0XYtMJ}?jwqZ@GWigJbAXnXQY2vuvu zLHLO1qA4O*$yjYlA(BH5N!lUirOZ7_6HrIfMBYco1V7{_u+c(OdalO!K&<#@_pK8- zaXi~Bdu+k0D%bZD{GsxFRrc|A@or3d)%U+eavJm;y)pEzrkS!E$XPnG%8zM<*GPOt zf)+Qj;Pcd^*H%*Up;J!jEpm~&-0JMKTX74nRjumY;T3X{ZmZLi#o?%v!YTLKV5!#$ M8vU1wV$N9q10TfHS^xk5 literal 0 HcmV?d00001 diff --git a/ws_nodes_py/settings/__pycache__/for_world.cpython-310.pyc b/ws_nodes_py/settings/__pycache__/for_world.cpython-310.pyc index 6832dcec1325f6bf74a091b1544b1963eeab93b0..3e50c3c219f9c0f5f073c951f26fa48692f155c0 100644 GIT binary patch delta 321 zcmZ9GJx;?w5QTUBvum$o8;An|LKG2E7$lBBM?r^#M6pdMUP6Nuw9w}a)^^AxC3oN& zw3HSXK&hB<$v)}R)0^3+`P4tUSm(JB^v*xFtFyQ%Ci1PhINd~u9|MFCM1hc~+QbU- zlvoLEITuKfk_<8okdunmSo(z$J8fX2erFeZ7@{JLT5aP#|C;{X5v delta 276 zcmaFOzMq{hpO=@50SMey{!DwrGLcV)aneL>rF!NR=5(eM1~ADohdqh`$Yw}qiegM* zjRJ`?q_Cy110hEWX9`yeHxTm7;fi7giu0!M&0&gS0juFp5l9hC5duPCpc$-SF_0`< zib#rRidc$xibRU!9M&lI6sceaP3e~)hrDD15 выделяем синий - red_circle = cv2.inRange(hsv, self.hsv_min, self.hsv_max) # -> выделяем желтый - - # both = blue + yellow - binary = cv2.bitwise_and(red_circle, red_circle, mask=red_circle) - binary = cv2.morphologyEx(binary, cv2.MORPH_CLOSE, np.ones((5,5),np.uint8)) - - pos = self.get_red_circle_pos(binary, height, width, frame) - if pos: - dist = pos[0] - course = pos[1] - - # Создание отладочного изображения - miniBin = cv2.resize(binary, (int(binary.shape[1] / 4), int(binary.shape[0] / 4)), - interpolation=cv2.INTER_AREA) - miniBin = cv2.cvtColor(miniBin, cv2.COLOR_GRAY2RGB) - frame[-2 - miniBin.shape[0]:-2, 2:2 + miniBin.shape[1]] = miniBin - - if pos: - cv2.putText(frame, f'dist: {round(dist, 1)} course: {round(course, 1)}', (0, 25), - cv2.FONT_HERSHEY_SIMPLEX, 0.75, (255, 255, 255), 1, cv2.LINE_AA) - x = dist * cos(radians(course + self.heading)) - y = dist * sin(radians(course + self.heading)) - cv2.putText(frame, f'x: {round(x, 1)} y: {round(y, 1)}', (0, 50), - cv2.FONT_HERSHEY_SIMPLEX, 0.75, (255, 255, 255), 1, cv2.LINE_AA) - # dist = (x**2 + y**2)**0.5 - cv2.putText(frame, f'dist: {round(dist, 1)} isDrop: {dist < self.drop_threshold}', (0, 75), - cv2.FONT_HERSHEY_SIMPLEX, 0.75, (255, 255, 255), 1, cv2.LINE_AA) - msg = Point() - msg.x = x - msg.y = y - self.pub_red_pos.publish(msg) - - # Отладочное изображение - self.pub.publish(self.br.cv2_to_compressed_imgmsg(frame)) + frame = self.br.compressed_imgmsg_to_cv2(data) + height, width = frame.shape[0:2] + + binary = self.get_binary("red_position", frame) + self.publish_coordinte(self.get_red_circle_pos(binary, height, width, frame), self.pub_red_pos) + + # Создание отладочного изображения + miniBin = cv2.resize(binary, (int(binary.shape[1] / 4), int(binary.shape[0] / 4)), + interpolation=cv2.INTER_AREA) + miniBin = cv2.cvtColor(miniBin, cv2.COLOR_GRAY2RGB) + frame[-2 - miniBin.shape[0]:-2, 2:2 + miniBin.shape[1]] = miniBin + + # Отладочное изображение + self.pub.publish(self.br.cv2_to_compressed_imgmsg(frame)) def get_red_circle_pos(self, binary, height, width, frame): # Выделение самых больших контуров удовлетворяющих условию размера @@ -148,7 +118,7 @@ class RealsenseToBinary(GetParameterNode): # Расчёт положения относительно круга if dist is not None: - return dist, course + return course, dist return None diff --git a/ws_nodes_py/videoF.py b/ws_nodes_py/videoF.py index b3dcdcf..e7cb845 100644 --- a/ws_nodes_py/videoF.py +++ b/ws_nodes_py/videoF.py @@ -2,6 +2,7 @@ import rclpy from ws_nodes_py.default.GetParameterNode import GetParameterNode +from ws_nodes_py.default.ObjectsDetection import ObjectsDetection from sensor_msgs.msg import CompressedImage from std_msgs.msg import Int16 from geometry_msgs.msg import Point @@ -11,21 +12,12 @@ import numpy as np from math import radians, cos, sin -class RealsenseToBinary(GetParameterNode): +class RosOpencvToBinary(GetParameterNode, ObjectsDetection): def __init__(self): super().__init__('camcvF') - # Параметры для выделения маски - # yellow = cv2.inRange(hsv, (60, 22, 164), (83, 255, 255)) # -> выделяем желтый - self.hsv_min = (60, 22, 164) - self.hsv_max = (83, 255, 255) + self.set_pool_masks() self.min_countur_size = 600 - self.get_logger().info(f"HSV \n{self.hsv_min} \n {self.hsv_max}") - - # Маска части изображения, которе обрабатываем - x1,y1 = 0, 170 - x2, y2 = 640, 280 - self.roi_corners = np.array([[(x1, y1), (x2, y1), (x2, y2), (x1, y2),]], dtype=np.int32) # Параметры для рассчёта расстояние и угла self.subscribtion_for_parameter(Int16, "heading_topic", 'heading', checker=lambda x: x, func=lambda msg: msg.data, default=0) @@ -47,98 +39,52 @@ class RealsenseToBinary(GetParameterNode): self.br = CvBridge() self.get_logger().info(f"CV Start {topic_name}") - def img_callback(self, data): - # Получение кадра - current_frame = self.br.compressed_imgmsg_to_cv2(data) - success, frame = True, current_frame - if success: - # mask = np.zeros(frame.shape, dtype=np.uint8) - # ignore_mask_color = (255,)*channel_count - # cv2.fillPoly(mask, self.roi_corners, ignore_mask_color) - # masked_image = cv2.bitwise_and(frame, mask) - masked_image = frame - height, width = frame.shape[0:2] - - # Создание HSV маски - hsv = cv2.cvtColor(masked_image, cv2.COLOR_RGB2HSV) - yellow = cv2.inRange(hsv, (42, 55, 80), (115, 255, 255)) # -> выделяем желтый в симуляции - blue = cv2.inRange(hsv, (0, 230, 127), (19, 255, 255)) # -> выделяем синий в симуляции - - yellow_binary = cv2.bitwise_and(yellow, yellow, mask=yellow) - blue_binary = cv2.bitwise_and(blue, blue, mask=blue) - # blue_binary = self.get_blue_BALLS(frame) - - res = self.get_gate(yellow_binary, height, width, frame) - if res is not None: - course_to_gate, dist_to_gate = res - x = round(dist_to_gate * cos(radians(self.heading - course_to_gate)), 1) - y = round(dist_to_gate * sin(radians(self.heading - course_to_gate)), 1) - # Управляющий сигнал - if 6 >= dist_to_gate >= 1.5: - msg = Point() - msg.x = x - msg.y = y - self.pub_yellow_pos.publish(msg) - - res = self.get_gate(blue_binary, height, width, frame) - if res is not None: - course_to_gate, dist_to_gate = res - x = round(dist_to_gate * cos(radians(self.heading - course_to_gate)), 1) - y = round(dist_to_gate * sin(radians(self.heading - course_to_gate)), 1) - # Управляющий сигнал - if 6 >= dist_to_gate >= 1.5: - msg = Point() - msg.x = x - msg.y = y - self.pub_blue_pos.publish(msg) - - # Создание отладочного изображения - miniBin = cv2.resize(blue_binary, (int(blue_binary.shape[1] / 4), int(blue_binary.shape[0] / 4)), - interpolation=cv2.INTER_AREA) - miniBin = cv2.cvtColor(miniBin, cv2.COLOR_GRAY2RGB) - frame[-2 - miniBin.shape[0]:-2, 2:2 + miniBin.shape[1]] = miniBin - - miniBin = cv2.resize(yellow_binary, (int(yellow_binary.shape[1] / 4), int(yellow_binary.shape[0] / 4)), - interpolation=cv2.INTER_AREA) - miniBin = cv2.cvtColor(miniBin, cv2.COLOR_GRAY2RGB) - frame[-2 - miniBin.shape[0]:-2, 2 + miniBin.shape[1]:2 + miniBin.shape[1] * 2] = miniBin - - # cv2.putText(frame, 'iSee: {};'.format(len(contours)), (width - 120, height - 5), - # cv2.FONT_HERSHEY_SIMPLEX, 0.75, (255, 255, 255), 1, cv2.LINE_AA) - # cv2.putText(frame, f'x: {x} y: {y}', (0, 25), - # cv2.FONT_HERSHEY_SIMPLEX, 0.75, (255, 255, 255), 1, cv2.LINE_AA) - # cv2.putText(frame, f'course: {self.heading - course_to_gate} dist {dist_to_gate}', (0, 50), - # cv2.FONT_HERSHEY_SIMPLEX, 0.75, (255, 255, 255), 1, cv2.LINE_AA) - # cv2.putText(frame, f'{controlX = }', (0, 75), - # cv2.FONT_HERSHEY_SIMPLEX, 0.75, (255, 255, 255), 1, cv2.LINE_AA) - - # Отладочное изображение - self.pub.publish(self.br.cv2_to_compressed_imgmsg(frame)) - - def set_border(self, frame): - channel_count = frame.shape[2] - mask = np.zeros(frame.shape, dtype=np.uint8) - ignore_mask_color = (255,)*channel_count - cv2.fillPoly(mask, self.roi_corners, ignore_mask_color) - masked_image = cv2.bitwise_and(frame, mask) - return masked_image + def publish_coordinte(self, res, publisher): + if res is not None: + course_to_gate, dist_to_gate = res + x = round(dist_to_gate * cos(radians(self.heading - course_to_gate)), 1) + y = round(dist_to_gate * sin(radians(self.heading - course_to_gate)), 1) + # Управляющий сигнал + if 6 >= dist_to_gate >= 1.5: + msg = Point() + msg.x = x + msg.y = y + publisher.publish(msg) - def get_blue_BALLS(self, frame): - frame = self.set_border(frame) - lab= cv2.cvtColor(frame, cv2.COLOR_BGR2LAB) - l_channel, a, b = cv2.split(lab) - clahe = cv2.createCLAHE(clipLimit=2.0, tileGridSize=(8,8)) - cl = clahe.apply(l_channel) - - limg = cv2.merge((cl,a,b)) - - enhanced_img = cv2.cvtColor(limg, cv2.COLOR_LAB2BGR) - - frame = cv2.cvtColor(enhanced_img, cv2.COLOR_RGB2YUV) - - binary = cv2.inRange(frame, (0, 0, 153), (360, 220, 255)) - return binary + def img_callback(self, data): + # Получение кадра + frame = self.br.compressed_imgmsg_to_cv2(data) + + height, width = frame.shape[0:2] + yellow_binary = self.get_binary("yellow_gate", frame) + # blue_binary = self._get_binary(frame, ((0, 83, 0), (19, 255, 255)), cv2.COLOR_RGB2HSV, (0, 100, 640, 250), False) + blue_binary = self.get_binary("blue_gate", frame) + self.publish_coordinte(self.get_gate(yellow_binary, height, width, frame), self.pub_yellow_pos) + self.publish_coordinte(self.get_gate(blue_binary, height, width, frame), self.pub_blue_pos) + + # Создание отладочного изображения + miniBin = cv2.resize(blue_binary, (int(blue_binary.shape[1] / 4), int(blue_binary.shape[0] / 4)), + interpolation=cv2.INTER_AREA) + miniBin = cv2.cvtColor(miniBin, cv2.COLOR_GRAY2RGB) + frame[-2 - miniBin.shape[0]:-2, 2:2 + miniBin.shape[1]] = miniBin + + miniBin = cv2.resize(yellow_binary, (int(yellow_binary.shape[1] / 4), int(yellow_binary.shape[0] / 4)), + interpolation=cv2.INTER_AREA) + miniBin = cv2.cvtColor(miniBin, cv2.COLOR_GRAY2RGB) + frame[-2 - miniBin.shape[0]:-2, 2 + miniBin.shape[1]:2 + miniBin.shape[1] * 2] = miniBin + + # cv2.putText(frame, 'iSee: {};'.format(len(contours)), (width - 120, height - 5), + # cv2.FONT_HERSHEY_SIMPLEX, 0.75, (255, 255, 255), 1, cv2.LINE_AA) + # cv2.putText(frame, f'x: {x} y: {y}', (0, 25), + # cv2.FONT_HERSHEY_SIMPLEX, 0.75, (255, 255, 255), 1, cv2.LINE_AA) + # cv2.putText(frame, f'course: {self.heading - course_to_gate} dist {dist_to_gate}', (0, 50), + # cv2.FONT_HERSHEY_SIMPLEX, 0.75, (255, 255, 255), 1, cv2.LINE_AA) + # cv2.putText(frame, f'{controlX = }', (0, 75), + # cv2.FONT_HERSHEY_SIMPLEX, 0.75, (255, 255, 255), 1, cv2.LINE_AA) + + # Отладочное изображение + self.pub.publish(self.br.cv2_to_compressed_imgmsg(frame)) def get_gate(self, binary, height, width, frame): pix_per_angle = width / self.fov @@ -211,7 +157,7 @@ class RealsenseToBinary(GetParameterNode): def main(args=None): rclpy.init(args=args) - RealOpenCV = RealsenseToBinary() + RealOpenCV = RosOpencvToBinary() rclpy.spin(RealOpenCV) RealOpenCV.destroy_node() rclpy.shutdown() diff --git a/ws_nodes_py/videoS.py b/ws_nodes_py/videoS.py index 4b9439e..663b0d4 100644 --- a/ws_nodes_py/videoS.py +++ b/ws_nodes_py/videoS.py @@ -2,6 +2,7 @@ import rclpy from ws_nodes_py.default.GetParameterNode import GetParameterNode +from ws_nodes_py.default.ObjectsDetection import ObjectsDetection from sensor_msgs.msg import CompressedImage from std_msgs.msg import Float64, Int16 from geometry_msgs.msg import Point @@ -11,25 +12,12 @@ import numpy as np from math import radians, cos, sin -class RealsenseToBinary(GetParameterNode): +class RosOpencvToBinary(GetParameterNode, ObjectsDetection): def __init__(self): super().__init__('camcvS') - # Параметры для выделения маски синий BGR - # black = cv2.inRange(hsv, (0, 20, 0), (30, 218, 109)) # -> выделяем черный - # self.hsv_min = (0, 149, 0) - # self.hsv_max = (47, 255, 255) - # 5, 66, 69) - # [hsv-1] High: (54, 110, 138) - self.hsv_min = (5, 66, 69) - self.hsv_max = (54, 110, 138) + self.set_pool_masks() self.min_countur_size = 300 - self.get_logger().info(f"HSV \n{self.hsv_min} \n {self.hsv_max}") - - # Маска части изображения, которе обрабатываем - x1,y1 = 0, 170 - x2, y2 = 510, 280 - self.roi_corners = np.array([[(x1, y1), (x2, y1), (x2, y2), (x1, y2),]], dtype=np.int32) self.camera_angle = 90 # Параметры для рассчёта расстояние и угла @@ -55,122 +43,121 @@ class RealsenseToBinary(GetParameterNode): self.br = CvBridge() self.get_logger().info(f"CV Start {topic_name}") + def publish_coordinte(self, res, publisher): + if res is not None: + course_to_gate, dist_to_gate = res + x = round(dist_to_gate * cos(radians(self.heading - course_to_gate + self.camera_angle)), 1) + y = round(dist_to_gate * sin(radians(self.heading - course_to_gate + self.camera_angle)), 1) + # Управляющий сигнал + if 6 >= dist_to_gate >= 1.5: + msg = Point() + msg.x = x + msg.y = y + publisher.publish(msg) - def set_border(self, frame): - channel_count = frame.shape[2] - mask = np.zeros(frame.shape, dtype=np.uint8) - ignore_mask_color = (255,)*channel_count - cv2.fillPoly(mask, self.roi_corners, ignore_mask_color) - masked_image = cv2.bitwise_and(frame, mask) - return masked_image - - def get_blue_BALLS(self, frame): - frame = self.set_border(frame) - lab= cv2.cvtColor(frame, cv2.COLOR_BGR2LAB) - l_channel, a, b = cv2.split(lab) - - clahe = cv2.createCLAHE(clipLimit=2.0, tileGridSize=(8,8)) - cl = clahe.apply(l_channel) - limg = cv2.merge((cl,a,b)) - - enhanced_img = cv2.cvtColor(limg, cv2.COLOR_LAB2BGR) + def img_callback(self, data): + frame = self.br.compressed_imgmsg_to_cv2(data) + height, width = frame.shape[0:2] - frame = cv2.cvtColor(enhanced_img, cv2.COLOR_RGB2YUV) + black_binary = self.get_binary("black_position", frame) + blue_binary = self.get_binary("blue_bou", frame) - binary = cv2.inRange(frame, (0, 0, 163), (360, 220, 255)) - return binary + self.publish_coordinte(self.get_gate(black_binary, height, width, frame), self.pub_black_pos) + self.publish_coordinte(self.get_gate(blue_binary, height, width, frame), self.pub_blue_pos) + # Создание отладочного изображения + miniBin = cv2.resize(black_binary, (int(black_binary.shape[1] / 4), int(black_binary.shape[0] / 4)), + interpolation=cv2.INTER_AREA) + miniBin = cv2.cvtColor(miniBin, cv2.COLOR_GRAY2RGB) + frame[-2 - miniBin.shape[0]:-2, 2:2 + miniBin.shape[1]] = miniBin - def img_callback(self, data): - frame = self.br.compressed_imgmsg_to_cv2(data) - binary = self.get_blue_BALLS(frame) - contours, _ = cv2.findContours(binary, cv2.RETR_EXTERNAL, - cv2.CHAIN_APPROX_NONE) - contours = list(filter(lambda x: cv2.moments(x)["m00"] > 600, sorted(contours, key=cv2.contourArea, reverse=True)[:1])) + miniBin = cv2.resize(blue_binary, (int(blue_binary.shape[1] / 4), int(blue_binary.shape[0] / 4)), + interpolation=cv2.INTER_AREA) + miniBin = cv2.cvtColor(miniBin, cv2.COLOR_GRAY2RGB) + frame[-2 - miniBin.shape[0]:-2, 2 + miniBin.shape[1]:2 + miniBin.shape[1] * 2] = miniBin + + # cv2.putText(frame, 'iSee: {};'.format(len(contours)), (width - 120, height - 5), + # cv2.FONT_HERSHEY_SIMPLEX, 0.75, (255, 255, 255), 1, cv2.LINE_AA) + # cv2.putText(frame, f'x: {x} y: {y}', (0, 25), + # cv2.FONT_HERSHEY_SIMPLEX, 0.75, (255, 255, 255), 1, cv2.LINE_AA) + # cv2.putText(frame, f'course: {self.heading - course_to_gate} dist {dist_to_gate}', (0, 50), + # cv2.FONT_HERSHEY_SIMPLEX, 0.75, (255, 255, 255), 1, cv2.LINE_AA) + # cv2.putText(frame, f'{controlX = }', (0, 75), + # cv2.FONT_HERSHEY_SIMPLEX, 0.75, (255, 255, 255), 1, cv2.LINE_AA) - height, width = frame.shape[0:2] + + def get_gate(self, binary, height, width, frame): pix_per_angle = width / self.fov - # Нулевые параметры управления + # Нулевые параметры управления med_x = width / 2 + controlX = 0.0 dist_to_gate = 0 course_to_gate = 0 + + + contours, _ = cv2.findContours(binary, cv2.RETR_EXTERNAL, + cv2.CHAIN_APPROX_NONE) + contours = list(filter(lambda x: cv2.moments(x)["m00"] > self.min_countur_size, sorted(contours, key=cv2.contourArea, reverse=True)[:1])) + approxs = list(sorted(contours, key=lambda x: len(cv2.approxPolyDP( + x, 0.01 * cv2.arcLength(x, True), True)), reverse=True))[:1] + + # Для выделение левого и правого шара aligns = [] + # Для выделение ближайшего и дальнего шара distances = [] - for contour in contours: + for contour in approxs: + # Точки контура не приближаются к границе ближе чем на 10 пикселей + if any((not 10 < xy[0][0] < width - 10 or not 10 < xy[0][1] < height - 10) for xy in contour): + continue + moments = cv2.moments(contour) + # Координаты центра контура if moments["m00"]: cx = int(moments["m10"] / moments["m00"]) - cy = int(moments["m01"] / moments["m00"]) else: continue - + + # Вычисление дистанции x, y, w, h = cv2.boundingRect(contour) dist = (w*(self.k1)+self.k2)*0.01 - - if any((not 70 < xy[0][0] < 640 - 70 or not 10 < xy[0][1] < 480 - 10) for xy in contour) or w-h >= h*1.5//1: + # Проверка границ интерполяции + if not 5 >= dist >= 1.5: continue - h = w + + # Запись центра контура по горизонту и расчётной дистанции до него aligns.append(cx) distances.append(dist) if True: # Вывод информации о контуре - # Координаты центра - # cv2.putText(frame, f'cx: {cx}, cy: {cy}', (cx + 10, cy - 10), cv2.FONT_HERSHEY_SIMPLEX, 0.75, (255, 255, 255), 2, cv2.LINE_AA) - # Выделение контура - cv2.drawContours(frame, [contour], -1, (0, 255, 0), 1) - cv2.rectangle(frame, (x, y), (x+w, y+h), (0, 255, 0), 2) - # Выделение прямоугольника - # cv2.rectangle(frame, (x, y), (x+w, y+h), (0, 255, 0), 2) - # Оси через центр контура - # cv2.line(frame, (cx, 0), (cx, height), (0, 255, 0), 1) - # cv2.line(frame, (0, cy), (width, cy), (0, 255, 0), 1) - # Вывести ширину контура + # Координаты центра + # cv2.putText(frame, f'cx: {cx}, cy: {cy}', (cx + 10, cy - 10), cv2.FONT_HERSHEY_SIMPLEX, 0.75, (255, 255, 255), 2, cv2.LINE_AA) + # Выделение контура + cv2.drawContours(frame, contour, -1, (0, 255, 0), 1) + # Выделение прямоугольника + # cv2.rectangle(frame, (x, y), (x+w, y+h), (0, 255, 0), 2) + # Оси через центр контура + # cv2.line(frame, (cx, 0), (cx, height), (0, 255, 0), 1) + # cv2.line(frame, (0, cy), (width, cy), (0, 255, 0), 1) + # Вывести ширину контура cv2.putText(frame, f'W{round(w,2)}', (x, y+h+25), cv2.FONT_HERSHEY_SIMPLEX, 0.75, (0, 0, 255), 2) - # Вывести расчётную дистанцию до контура + # Вывести расчётную дистанцию до контура cv2.putText(frame, f'D{round(dist,2)}', (x, y), cv2.FONT_HERSHEY_SIMPLEX, 0.75, (0, 0, 255), 2) - - # Расчёт курса и дистанции до ворот - if aligns: - med_x = sum(aligns) / len(aligns) + + # Расчёт курса и дистанции до ворот + if len(aligns) == 1: + med_x = sum(aligns) / len(aligns) controlX = round(-2 * (med_x - width / 2) / width, 1) controlX = max(-0.5, min(0.5,controlX)) course_to_gate = round(controlX * self.fov / 2) - if distances: - dist_to_gate = round(sum(distances) / len(distances), 1) - - x = round(dist_to_gate * cos(radians(self.heading + self.camera_angle - course_to_gate)), 1) - y = round(dist_to_gate * sin(radians(self.heading + self.camera_angle - course_to_gate)), 1) - # self.get_logger().info("collide border") - # Управляющий сигнал - if 3 >= dist_to_gate >= 0.5: - msg = Point() - msg.x = x - msg.y = y - self.pub_blue_pos.publish(msg) - - # Создание отладочного изображения - miniBin = cv2.resize(binary, (int(binary.shape[1] / 4), int(binary.shape[0] / 4)), - interpolation=cv2.INTER_AREA) - miniBin = cv2.cvtColor(miniBin, cv2.COLOR_GRAY2RGB) - frame[-2 - miniBin.shape[0]:-2, 2:2 + miniBin.shape[1]] = miniBin - - cv2.putText(frame, 'iSee: {};'.format(len(contours)), (width - 120, height - 5), - cv2.FONT_HERSHEY_SIMPLEX, 0.75, (255, 255, 255), 1, cv2.LINE_AA) - cv2.putText(frame, f'x: {x} y: {y}', (0, 25), - cv2.FONT_HERSHEY_SIMPLEX, 0.75, (255, 255, 255), 1, cv2.LINE_AA) - cv2.putText(frame, f'course to bou{self.heading + self.camera_angle - course_to_gate}', (0, 50), - cv2.FONT_HERSHEY_SIMPLEX, 0.75, (255, 255, 255), 1, cv2.LINE_AA) - # cv2.putText(frame, f'{controlX = }', (0, 75), - # cv2.FONT_HERSHEY_SIMPLEX, 0.75, (255, 255, 255), 1, cv2.LINE_AA) - - # Отладочное изображение - self.pub.publish(self.br.cv2_to_compressed_imgmsg(frame)) - + dist_to_gate = round(sum(distances) / len(distances), 1) + return course_to_gate, dist_to_gate + else: + return None def main(args=None): rclpy.init(args=args) - RealOpenCV = RealsenseToBinary() + RealOpenCV = RosOpencvToBinary() rclpy.spin(RealOpenCV) RealOpenCV.destroy_node() rclpy.shutdown() -- GitLab From 249bd18724df7c24ae7eeb891392aa4a45a6ad34 Mon Sep 17 00:00:00 2001 From: "toropov.nik" Date: Tue, 1 Oct 2024 09:34:45 +0300 Subject: [PATCH 09/23] ready to test start on waterstrider --- .../__pycache__/compass.cpython-310.pyc | Bin 5226 -> 5756 bytes .../__pycache__/state_machine.cpython-310.pyc | Bin 11241 -> 12946 bytes .../twist_controller.cpython-310.pyc | Bin 2274 -> 2347 bytes ws_nodes_py/compass.py | 49 +++++++----- .../ObjectsDetection.cpython-310.pyc | Bin 3048 -> 3048 bytes .../__pycache__/for_world.cpython-310.pyc | Bin 1003 -> 1120 bytes .../robot_settings.cpython-310.pyc | Bin 0 -> 342 bytes ws_nodes_py/settings/for_world.py | 19 ++++- ws_nodes_py/settings/robot_settings.py | 12 +++ ws_nodes_py/state_machine.py | 73 ++++++++++++++++-- ws_nodes_py/twist_controller.py | 3 +- 11 files changed, 128 insertions(+), 28 deletions(-) create mode 100644 ws_nodes_py/settings/__pycache__/robot_settings.cpython-310.pyc create mode 100644 ws_nodes_py/settings/robot_settings.py diff --git a/ws_nodes_py/__pycache__/compass.cpython-310.pyc b/ws_nodes_py/__pycache__/compass.cpython-310.pyc index ddb0215cb347bd5fafdcd787f7a70ed7286b4cd5..c9a630b4209ff791ea0a2f7ed604ac417143fba4 100644 GIT binary patch delta 2987 zcma(TU2hy$b?$uao!Qy-uDxD=)Uh|UX|io&1ve%sP2!rwag!!?vC~2~rNh{FvRQj~ z*19u`6En-UI1jv7w7oAy6~d7K5eZeQ)MtJJPdp3)fe;U=!~+iq36R1$ck6ZVL+Os@ z%(>@l&pluFes=Od&u5#NjIF@$o8sTMe>e7ic8q-R{&V9Uy`mF^DNL^$4WnX6oYc*V z2{fagYS0R8SQV>bS8T~M>*%<|P;t&(FlOW#*2 zdB2}In<`)Q^ScUYkyZJYGmp@jrHL|0a*KZSn#a9{ANhQ#$$X%tYN5-0ul~tzj}y}Y zN$!Y$k`FP^`(tH8W$8%U&^L^Y0p>6ryvnfb zo(|jsa0jCz>x1tQ%Sm2-4`Kl8;b?^QvjJ9+)IkX;EAE++I~a|!A&`2C4NK0ehiYSN zgpIPNBza82V?DKF@Mq&}LQ?a48n`eXEBg~*sPuqf7*~~YN&HNko5XECBni38yBo$` z_W?-g`MTFwW8TFvLG-Ag&%b_iImujEUb(d3zHxQs=Iis zdVSzCNp$8#9)AYKZ-f4Fn`^BV|nHJW$_L@K@9OLI%8sNxheic3nV4}LD%OF zojMIH%yc*nz#*fCNwTU9s34J`QkCR~{er$cEgM$GLUqbB_(v1Vp zGSVhYZL3UcYfNwJ%xD{dEvY6@O`xWLO54T`jmR!WPaRg;=C_m}tt*js?>Pc3k4@h; z-&6v!t#qYa6(n_j4xquTO|=tVkf;rFVh$T3GGU$CB`m$E^Bah#l~^w*?Np$~q@9XW zK%?+iG^Vi*GPh#AO#`_~E1L>mkM*?Vwk@z_fh7&3+OeX8UTno?K>-^C{%<>0gAR$c zf^t?-6_*AY8h2@I@I5IG0|rz;LGrBt)?mZ}sU8g^Eantp|1NO%VH9noBPYrreV07Y zKJW6(5{J+#)feRNFLQhtR*#@=XO)|eMi9>exWW-l>~4Ew4OG8h_;~TpJ3pV}W56kA z`9=6}UHsi1oqZnB3#g!ZYayS+rL48{lD5@a<5!So$Ym{v$~M0$UP^CfGcaMdYtbFo zTVMC<;uq;toflE^NTt65H2l915h$V0A1tN=Tvc<#A(W_(sX7YOtd<3?tx+gDC_lg* zG6jXFt3}eUIv}$%cB$Fe^1^UB+OCC>d&l=!Ef5!+r`2bO@SIcl zUP9IqvVJsA-vDNa?@0iT_(NtSJF4r%)@;bZ8B$cmzcOb!9{@8Z8_>q#bd zS()-v@OdK85d8(13`l$%!OI9R+-3DJzDL7)8TjwP8)9otks=iOl$h=NjEsp=?v)pg z168U#)OZH)L<_fALZTyP66<``5B$4Z{Nh9LQLgoBPmgaNSm>(A(~oF;867-KIQBfp%HkLYz6~JBxb8Z55W>)?yY3Sky@n#v|1@A3vEM&v zousq$SdQ|v_)Y)P0PebM@)V<+|(OhE%-GsLU}mB7_ni*IE258 z+SGNC@4DpyJ|liOFkMY1w?j98%VX$n-J6C=ifTbMoaW87X5=0$M;E zG}tmFgK4#IfSafdk?X*c%s6`xo>;Ro)wyw9TPvtbIH4-tT?Ro45IQ z&x4)$RxXzz@b_!*aDHO+dVVwg=KAjOu2r@ukwnruFLQYFy4`TfP9s%LHAGn$nO#pe zGUbd}JN0bCExV0eIY-G^B2!XaB~pZJ%_`?*T4t`2a$i`G*+r(N!@e~FT6ESto_iky znDJ;*I2*sBONPJv^9pscAXZtOXXj}_2kbZ<&_A%S+a?Q4rekY?FW3tO znUNe^W~FFy4ol>+bp*{QL!L|u{OE284PW?O|eF^#2eRtS|_(*I(KP8 zS5z-C%q|Zk5y$F5V@?Jq#`FOm>wEnCxIqk>{MQL$)ew-~+OwB;y%4ewu_YiLRipZM z`^erc$QeiSLdE??QD0$2{gzWYh6)}_QbDu2Tn|)2S0Mwd5mh(jVB$jEhyfAAv%u|l z&e4~3d$8yBp>yVCHKuwRy+G3k9oRz6q1q-%Xeus)I8fUV_I$lftW)}4>hRP{7Ui(< z1q4oVt2j&3x+->3p}!S}_1|I#)T|jD z_DtG?(`k*E&#tx z7of}FkVoK*DAHT(Ho0X<27CTJxxsK6;u;Rs#?;m|4x&bMXk7zl=u-`ecn9(SKb}34 zM=j7sd)JrzG)dRrFKEoy*ioeUB>N)NU*l1 zSRQ*?osC5~381gxiSh*QW`UOAhzokG{{bD*&cL}t>y!4brCh)zRoj@ zX1KDXPTbU22A1Dl*Hd#C#)ke1fp-(VLQfL*Wr>(J_cFbfbxnsq$zNG|dT__4kI>2m zAP~TE!}Pbup^LkNTem-NJQ=FA8cRVG`A>$uA1#HUyrzE}obS$~nJSvG6^KJsLxw3A z4l{*~udq=R7DhFJBst%&Ktxd`tk-@2nN&_A#0*0G7X+&l6@duCp|u7mw_GN+i34Iw zK`2C2h`w8VXAn!{ISRir*yXiYWwbpsd9oiE&sayp32y|o=51I-Xh6U{%fJnEsaip) z@O_l~KHR7M`qI$8Bea`LJD{IBXbts%mRjT-2?2*nBrzTo<4#&9#_Pp+ zxMaMR0>?;%HsA~@^-IylKC3EY#!VwOerBiID_=1c98c51$;|rrq?wLQu=Jo2qbxnK z{xD0L##D826uV-B=_7@1{*GG9f2_5V%lZ4-our#@w0Du_GYR`95@cSmomD0#fTq!S z%F-8R^Y7`+{7roaxr~=NqNJQ}a6DYz3;ZmMup6O|zwHQ;EBRj??~v{MHD?{UmjA(d zbFL2tVY||tlqYjQ#pEmjM3bml`C%|~R-w`)Vg4){)Rt96t1+uWOG_yfxiyerpTi?T zW5|8X4^pE#Ye&r;2%QL*BkV!wMrZ+Wu$uv+HrC6(Q?|jq4H_9AfJfE{f47VUaT^w) ztOe~-GnmjOf1M| zrYDjK%R%F2JZ2u9Fqlw#8_rWkjiaof<3?x?^E2LcWGDZlw`=GYXtOp1VFlc}rDRNI zsbk5EX{m$h^cd^MaoiunO)*?O!^N@zgh2$9Yq?{w#8^C&Nf={eu~>03*V@JDNI3JDlcQs%xs#*oG&g0qByGxNIyf)=^P; z4LQvFDw}&IVCa<6XW7lO{S7p`3Hf9b{#y~L6wKdsLIv@E#;ly>t$4(BjB&Le(-tdljkJn;fJci zJC35L4}CT%^Zfb;s9H3*4Gdm^;X;HASKpL<_nup?5AEHItD z6!=Fqop&r9!iv=mAS2!(;jEW@Fqh$sLSW>qHX8mGw(m!H0O3J|M*tSwEX!Z1ZSvqj zJ_%2z5uk>DT>IhX6F{*?0p^DZu-gmnQX(G5#hP`b901WWxHpda9vg?ovwTDE7&)Ye+*$9VH9B(LgDHmg&NpncnNFK0dYqe z^iy4m5Z$d;x;*S&;Wxhy9$4_3wuT2cq3eqqD7FTdU{&V0vtQV^JPIzEb=`6TI0_0JTlB7i1_{kyfaSR`n%ZoG(Xq4S^lTQ zzZ;0;GDl4<51G5*Z>BygP0NscoE*CN?K!&pD%|7tv%Az+{)p#Yd8kqdlCfqsBUv1jpWz=5k&isp}=A7_~5+8<* z%-aB>4JF}ZHBZKfTX5oXq(nlw7O9&MdYImcH{~n&q8UXGI_jF&X>j|$DqD9mXJOcuLg_3jt;czaO#-0)G zb}oFV`5<&h)zv%;lQs;p!x(uui_C_9ioCWG+YR9Oia1NAd0TTU`G&|j&jG_H@DxL& zPFyN`4t3`g==~}@7a{_ z_7`d{o2c>LD>XIOnF3sLQ0}!h|ulwLo$R)te9*V_SiAt zKBW~h#|uEOW)X}CI8+GjNWFlz3I7Yo3us@BXaqCMRABj9`S&aQ9^5f{pIF?pkQy6u zd4(!jc?5O}r8M|+icswd=}B>U@EYVHG-qxty=1otwaT+}XZ!xq5- zITU2;BCk67CDV;p6h=I7Nx=k=6ukNJY;Kt%7Fx^!&OTNRD`gN%r73&C-XVqth3EmN zV5v}Yjh3bd7>ZVuc*c@bh|4no<6lk(-YZ?s1F^C{S4tHbQ+%Q1WD7!&HLCT0a`gP5h#M%n~DL6GF2A&{feu;9U-$YPIoIZv)A z2$>p+OwH7`oaswTTyxSs4KH5PT9%w3k2FFEfr|#0O{Hr^#bi1Yx>a5Ye8sVN#oJA?og{`=+Wo;S&nsyGSxM6b^;3%`oeVD$vr#(}#?` zX?B9=Tk5np_V43ww2b8nB(yD!#c6UfW0k?nTWr{XMrugBMa*N(JC6a(axjC&CerXW z^O^V)qqFcQ#ysA2sI-N!8K6Z}`%L_`(FqYp1e~b)nG>dWCXy@x#pZj+_8z7__C5fg zSl3auxEOYxKfSI&o>%$#bq4tz?`ZAbii1P-FxR>St;Tu21P*PQc69S5+q z8+P>V=v-;3q2<%0BN8oT3(;bh8y%8EjS#D0g7njAM6 zj9DuD4Ul12dRxkvkz&uFAdK=1!xI!FzG#<@@hs|uYNt1PGa2GfM8ozlfyzZC{1?&P zJk;i@*JUCT^$S^B{;T~>uIJ&lTl%iT)Z>8~K)dKye53~wlYY@B_mURz+gkFgU6f}$ zb`AO)=RI{$7ZLUkSb(L+V&idm(T_#f^0RHnG~6Qlkbk9pW0#2}ZjI#-a65&aV3Ad_ r9hAd{A^HaR>2@#t5%qF=Fc9;3d|uxsU#qXoSEW}=IX&Ve>c#&8`!$$f delta 3775 zcma)9TWnOv89sAvd-lHGwY|1k+q*Ux7S=bQhPH`w;}VD11{*`#5H^eFVAj~XX7(70 zInFv5O4F1}GIA{yFKD)o{+wNj-%AgZ>Jj;g2+m0DLy)jU;AAL#$juFcjV zQf_F}x73h0t)ul9q^~B-HX1!C zv$xE|qC_MrkBX6OxU)dU39;||MNW~inB}siQayFkdK$PO(dd$5OOwqsw4@RVBr&T+ z$UU?{$Qy;cwMyPdn_xwpuEP~n8kLfglz~N2>Ss*gvRqXytVwOugUu~5QPwcF7t~%7DJ&!=$hNG zUBEuTBmSc;LP43y7uKraPGkJ4f6PAuGqxQdDYHHN6MuvD3=l8yK;S*HhyOItN?zcv z)~e1M;X37{d6t!r9s=6r$3q**0KXFIDvkopHX{h1p{E?B zU@=FX$`&j~J(kZ;u>&YaPcXbI!%H!|13QZFG6L@F_|xg}sZ60TZca_5(^Y=jiz@i( zSp)z_p#i{oGhy5-LRWkVHjAQH(0m=)>@+Xd zrpS5TxuNyw5(c@HhCrT_O4^cCc*4?4kld5f62VjkuDm3bl(OEW+8|j5aNSh7Oirq7 zr_k_#hL@_if@-7XRj?O}@@QRR;0&HwM0uaOV|v9!uhm~7 zMP3u#1>gBlw6*S$p5@<+CR_KwRu#wMtQTP$LI=VH5i3{t-=q7;PTtqh-d55po)*3> z8Q|%+h40*0GAvKY%kR}T`+dOkQ(5@-NzNPYc5|v$JPK=A6BZXyV0=S!uEF6$yWLR2_hFVYPuXH$c)0`SaGge(f3% zxA}+b7Q%1BZ28BifHnMG6%?|u9okhOKHxWFsmG6_>|v5&M6mM+lL#jfvIx&1h^#?t z8J@tdVlp;jjG?gw3`Ij~BZl9o^#$0Xh?&vmBP%g;xB2xVMz_dDaW#nZM+OckIc|$h zq%*mRDYI+}Ue(bvGc#t6Rt1porcs@~c!E$?DbzvsU8Gk6j=$s!@p~;A`8EGT%f8SJ zlzkgPbhr}cjR$>mA9>&5gRMJ?26$h@AA#J3497XPFvxcm+u5WBLxf!>yh#O$PaSaUsU{L+lJ~`XgXgHLIzjm9czaPq*#h2nwfHP+Kqi~Z@wO%p z4xlDFch#exDM%rdiU#&L&V~`f2qJ>kJV3tCj}i==H8sppGRF!GKLz_9f@sa#7yTr96Snvj?uDr&LfGfi*-a6^Uu-+wo(FbP zeN?RCa{+{G7V8*aZd**;MqRC{ev5*=rZ|&xe5rjCu|+p~A69$}qf&4U;#1lC{MM#s z`FAq^?IvCfQo&Q17o!lHA0$Ow1hzJ*Pr7IVo8Tg$ zZL~d0==#eFB!k=5OV9{ua8ceRy>#J-Jnxz}Y~3~*CAtBI3a{#aS>^Q|w(ELPx*=O$ zTcR5)N}C#`$}G7$+q^p40!gCN6z#Bl<%1xnT(ITGv@@sJE*gwNvArR$m~-kURY&_z z%GU_{Qq|$08L<4-hx4nOn~VQ*@S^Lp$H6wepLbwo`yMMGd?VAuiZO?wWvTGH2!6`B zd!=inFnR_u9Dl!p?Jrmg(O#SErCXPjOpR4j3S(EaNhz`<&AUav+cq>;+X+AM_~A^} z%H}2#@i@{9BU98~N1x80HV@|6W&X=llb%NY82>0WwKCn`*_`;)!%W&ao@L`x<}!Ad zWd*=-12;XBhc^Si-l1E;UkIf>=~@_%0wr!_*MU-K9GI}pRNQ*03`Z@dNoA)$$DZu z2+Pls4&cZO0hbPR%Rd5jkk*7bc&?F7PiNo-oKCj$;oetuw3yxFf9&19p5YWtX7~aV rW>zdj5?8b1P&1-@cxw>;w@Yu0MUI36;ZS%-IBEE#qT%-t^}&AuC{9b5 diff --git a/ws_nodes_py/__pycache__/twist_controller.cpython-310.pyc b/ws_nodes_py/__pycache__/twist_controller.cpython-310.pyc index 990fba375872f997052d26189807d4582b8a0e70..4a0710694d130dbb6918fdc5b72a6585df8f4d13 100644 GIT binary patch delta 929 zcma)3L2J}N6rM>Y$t0U>ySvg#p=)c461%Py4^B?ZHewVcvC4vZ+S{Nej?;1|xk-dAoNXeU*w`{#TyWuTla?EM!h3AU zi?O>!AcHRG9hL^K#<`PZk&N5v;Qc%LR$ZZ&mU7o88zk~J$fHv33l3)>Tw#H(_Xci1 z{5Sbd=_+lAhws~W~4t*949s`iYz zh9?*a?XO^DUck4Rwq7v-v1b|O%vrIYV`&`|zHg7Y`JBd&SD4ce8 zwV$?SL7&PgO|12o(jgp6I9XbqW#s5xL<*A-IN3KFPU9+5Rt9cB^^- delta 914 zcma)4J8u&~5Z<}x+P-sa2LdS!COpLDMI?$y2!SY2Ktfieq#&j^j_i^c&NkY+2qlU{ zku($_MN6Ng1Mw613H*aAktnDrC@90s5+#8GangP}`ex>v$KJ=rmwGq|!!pCK`Tgh9 z^_{Q@ot=qukL->OOfcc3PS5SQz~-26g}2RwC#@~J;|X8zZPxK67p3Qxnvnh$!wgt3 zCn|p<&2|JuZBFL%iAs7hm#RGwa_7@Kc&q2(`eVU2SzOv=&&qj4&>e`faIr?^jRWkE z5AY5}_y=Fb_^KE>_c&V*II&_-nCL#)D?Sn z($FR2JAw*X0m5lKKP&`?MW906w`Znm?&~J2cBMaU){Hn zQ)dX82!)^YSJu)*6|kYk4Dn*^N!ra;KUnHAMO`H@+Q$XXBiGb3-s^i2S`q)P=ToGS zCuKE3FiAkVYKmZ*;4iTjau+rDOKr0$heD`;B1aL%@p77E*|JR2I6jekg+k1@E?V?% z83}V7xH>%IDObPu+r#uck*8>*h12bKbL30>(rpik6Jn%OIK8CXA4e;r<#F(!jC%_} z&61bLG5O;-DydPO@@Z*iE$S 1: self.angular_speed += ang_accel * dt * (zero_throtle_multiplier if target_angle_speed == default_target_angl_speed else full_throtle_multiplier) else: - self.angular_speed = 0 - # with accel - # self.heading += self.angular_speed * dt - # no accel - self.heading += target_angle_speed * dt + self.angular_speed = target_angle_speed + + if is_with_accel: # with accel + self.heading += self.angular_speed * dt + else: # no accel + self.heading += target_angle_speed * dt + self.heading = self.heading % 360 self.pub(self.pub_heading, Int16, int(self.heading)) @@ -118,7 +129,7 @@ class Compass(GetParameterNode): self.get_logger().info("start median") self.last_compass = [msg.data - self.compass_delta, msg.data - self.compass_delta, msg.data - self.compass_delta] - if not self.is_use_real_compass or True: + if (not self.is_use_real_compass and self.COMPASS_MODE == "AUTO") or self.COMPASS_MODE == "FORCE_VIRTUAL": return # median @@ -131,10 +142,10 @@ class Compass(GetParameterNode): # self.compass_angular_speed() def reset_callback(self, msg): - if msg.data == "reset_heading" and self.is_use_real_compass: + if msg.data == "reset_heading" and ((self.is_use_real_compass and self.COMPASS_MODE == "AUTO") or self.COMPASS_MODE == "FORCE_REAL"): self.get_logger().info(f"reset real heading {self.__compass}") self.compass_delta = self.__compass - elif msg.data == "reset_heading" and not self.is_use_real_compass: + elif msg.data == "reset_heading" and ((not self.is_use_real_compass and self.COMPASS_MODE == "AUTO") or self.COMPASS_MODE == "FORCE_VIRTUAL"): self.get_logger().info(f"reset virtual heading {self.__compass}") self.heading = 0 diff --git a/ws_nodes_py/default/__pycache__/ObjectsDetection.cpython-310.pyc b/ws_nodes_py/default/__pycache__/ObjectsDetection.cpython-310.pyc index 7e01f325116d1b87f9b1355b4e2ac15aecd391f3..31f28b3bc9d70d6f33b86d3695ad4ab5588ca378 100644 GIT binary patch delta 18 YcmaDM{z9BHpO=@50SFFk0FBZWVOD}_6SCxsUX1?F%@v7|7h z2&M?lVTxh}vKUf?Q$$ikQ^bH!d=7gQ8(0h^%bp^UBAFtUBAp_WBAX&Nhb@XDMLw88 zQ(@w%LyRvc_cE%Rr5l66VS7!^B7UIlMFJo~5J+UIu>n~^Ag(Zo5CIXQAVQ2`@+L+P zQLIvv`I)L1IVX29neZ29=Empdm*f}47Z;?arcB<$l+UIL(yuz%kU4=dYw{#!17SfX NMpVf5ZSp~8EdcWcJM91f delta 159 zcmaFB@tU14pO=@50SL6#{7O@0oyaG{IBTMIk}ZFVz#OJ1mJ|jc6ig9H5l#^SLeV+w zQLJDwkStq@Sc-UxM2cjJREl(p%pA5T_7vG*22Ht%j}9?rPF~8WKADBdO;iwQdXW%_ t5C#z(Zg36O8F()vFP5#VmFxi|%695#SCIQf+PNt6hf5Mn@#l~7s85~cMO0!gkqr;Xb8VCxI;5O=AAwDI3 zJqaXv1!hL&C5g$D{C*{7OKln?*W!Hr%FM_f`z`Oc(W9=3kGhX^Q#*?Ziws*$;-x4Gk|U4XaXJ^6oEVE0Ree9KpLBe2%>@^QvKr literal 0 HcmV?d00001 diff --git a/ws_nodes_py/settings/for_world.py b/ws_nodes_py/settings/for_world.py index c82742c..33517b8 100644 --- a/ws_nodes_py/settings/for_world.py +++ b/ws_nodes_py/settings/for_world.py @@ -13,8 +13,8 @@ start_position = [0, 0] zero_speed_signal = 1500 -# Посчитанно Кириллом и Андреем в опытовом бассейне -old_motor_speed = { + +sim_motor_speed = { # signal: speed (in m/s) 1500: 0, 1540: 0, @@ -28,6 +28,21 @@ old_motor_speed = { 2000: 1.04, } +# Посчитанно Кириллом и Андреем в опытовом бассейне +old_motor_speed = { +# signal: speed (in m/s) + 1500: 0, + 1540: 0, + 1537: 0.10, + 1574: 0.15, + 1600: 0.40, + 1700: 0.73, + 1800: 0.95, + 1900: 1.04, + 2000: 1.04, +} +# old_motor_speed = sim_motor_speed + # Посчитанно по компасу old_motor_ang_speed = { 0: 0, diff --git a/ws_nodes_py/settings/robot_settings.py b/ws_nodes_py/settings/robot_settings.py new file mode 100644 index 0000000..c39bad8 --- /dev/null +++ b/ws_nodes_py/settings/robot_settings.py @@ -0,0 +1,12 @@ +is_real = True + +NS = 'ws11' + +cameras_ports = { + "2.4.2": "camera_front", # первая камера от провода юсб хаба, воткнутого в красный юсб jetson'a + "2.4.1": "camera_side", # вторая камера от провода юсб хаба, воткнутого в красный юсб jetson'a + "2.4.6": "camera_down", # R5 + "2.4.5": "camera_down", # третья камера от провода юсб хаба, воткнутого в красный юсб jetson'a +} + +camcv = {'F':'camera_front', 'S':'camera_side', 'D':'camera_down'} \ No newline at end of file diff --git a/ws_nodes_py/state_machine.py b/ws_nodes_py/state_machine.py index ddb8209..d542231 100644 --- a/ws_nodes_py/state_machine.py +++ b/ws_nodes_py/state_machine.py @@ -44,6 +44,8 @@ class Controller(Node): self.pub_goal = self.create_publisher(Point, self.get_declare_parameter('goal_topic', lambda x: x, "no goal_topic is described"), 10) self.pub_compass_type = self.create_publisher(Bool, self.get_declare_parameter('compass_type_topic', lambda x: x, "no compass_type_topic is described"), 10) self.pub_navigation_marker = self.create_publisher(String, 'navigation_marker', 10) + self.drop_stakan = self.create_publisher(Int16, "modbus/auto_probe_drop/write", 10) + self.drop_box = self.create_publisher(Int16, "modbus/drop_box/write", 10) self.create_subscription(Point, self.get_declare_parameter('position_topic', lambda x: x, "no pos is described"), self.pos_callback, 10) self.create_subscription(Int16, self.get_declare_parameter('heading_topic', lambda x: x, "no heading is described"), self.heading_callback, 10) @@ -65,6 +67,7 @@ class Controller(Node): def next_step(self, move_type: str, goal: list, is_real_compass: bool=None, navigation_marker: str=None): # self.get_logger().info("WTF") + self.state_change_time = self.get_clock().now() self.move_type = move_type self.set_goal(*goal) @@ -130,7 +133,6 @@ class Wait(smach.State): def execute(self, userdata): self.cntr.next_step("STOP", [0, 0]) - self.cntr.state_change_time = self.cntr.get_clock().now() while not self.cntr.shutdown(): rclpy.spin_once(self.cntr) if (self.cntr.get_clock().now() - self.cntr.state_change_time) > Duration(seconds=self.time): @@ -154,7 +156,6 @@ class MoveToGoal(smach.State): def execute(self, userdata): self.cntr.next_step("MOVE TO", self.goal, self.is_real_compass, self.navigation_marker) - self.cntr.state_change_time = self.cntr.get_clock().now() while not self.cntr.shutdown(): self.cntr.update() @@ -213,7 +214,6 @@ class MoveToCircle(smach.State): return result def execute(self, userdata): - self.cntr.state_change_time = self.cntr.get_clock().now() self.cntr.next_step("MOVE CIRCLE", self.goal, self.is_real_compass, self.navigation_marker) while not self.cntr.shutdown(): rclpy.spin_once(self.cntr) @@ -245,6 +245,46 @@ class Finish(smach.State): self.cntr.get_logger().info('end') return 'fin' +class DropPickStakan(smach.State): + def __init__(self, c, time, state:int): + self.time = time + self.state = state + smach.State.__init__(self, outcomes=['timeout']) + self.cntr = c + + def execute(self, userdata): + self.cntr.next_step("STOP", [0, 0]) + self.cntr.get_logger().info(f'Stakan {"drop" if msg.data == 1 else "pick"}') + while not self.cntr.shutdown(): + rclpy.spin_once(self.cntr) + if (self.cntr.get_clock().now() - self.cntr.state_change_time) > Duration(seconds=self.time): + self.cntr.get_logger().info('finish') + return 'timeout' + else: + msg = Int16() + msg.data = self.state + self.cntr.drop_stakan.publish(msg) + +class DropBox(smach.State): + def __init__(self, c, time, state:int): + self.time = time + self.state = state + smach.State.__init__(self, outcomes=['timeout']) + self.cntr = c + + def execute(self, userdata): + self.cntr.next_step("STOP", [0, 0]) + self.cntr.get_logger().info(f'drop box {"open" if msg.data == 360 else "close"}') + while not self.cntr.shutdown(): + rclpy.spin_once(self.cntr) + if (self.cntr.get_clock().now() - self.cntr.state_change_time) > Duration(seconds=self.time): + self.cntr.get_logger().info('finish') + return 'timeout' + else: + msg = Int16() + msg.data = self.state + self.cntr.drop_box.publish(msg) + def main(): rclpy.init() @@ -302,6 +342,21 @@ def main(): "dist_threshold": 0.1, "navigation_marker": "red_point", }, + 'drop-stakan':{ + 'type':'drop-stakan', + 'time': 15, + 'state': 1, + }, + 'drop-stakan':{ + 'type':'drop-stakan', + 'time': 15, + 'state': 0, + }, + 'drop-box':{ + 'type':'drop-box', + 'time': 15, + 'state': 360, + }, } vlad24_mission = { @@ -357,15 +412,15 @@ def main(): "dist_threshold": 0.1, "navigation_marker": "red_point", }, + } - mission = vlad24_mission + # mission = vlad24_mission # mission = square_mission - # mission = drop_mission + mission = drop_mission with sm: c.get_logger().info("---!---") smach.StateMachine.add('STARTING', Start(c), transitions={'start': 'GOAL-1'}) - for i, key in enumerate(mission.keys(), 1): if mission[key]["type"] == "moveTo": @@ -374,6 +429,12 @@ def main(): elif mission[key]["type"] == "moveAround": smach.StateMachine.add(f'GOAL-{i}', MoveToCircle(c, 1000, mission[key]["pos"], mission[key]["radius"], mission[key]["out_heading"], mission[key]["is_real_compass"], mission[key]["navigation_marker"]), transitions={'timeout': f'GOAL-{ "FINISH" if len(mission.keys()) == i else i+1}'}) + elif mission[key]["type"] == "drop-stakan": + smach.StateMachine.add(f'GOAL-{i}', DropPickStakan(c, mission[key]["time"], mission[key]["state"]), + transitions={'timeout': f'GOAL-{ "FINISH" if len(mission.keys()) == i else i+1}'}) + elif mission[key]["type"] == "drop-box": + smach.StateMachine.add(f'GOAL-{i}', DropBox(c, mission[key]["time"], mission[key]["state"]), + transitions={'timeout': f'GOAL-{ "FINISH" if len(mission.keys()) == i else i+1}'}) #for i, key in enumerate(mission.keys(), 1): # smach.StateMachine.add(f'GOAL-{i}', MoveToGoal(c, 1000, mission[key]), diff --git a/ws_nodes_py/twist_controller.py b/ws_nodes_py/twist_controller.py index ea028c9..53b699a 100644 --- a/ws_nodes_py/twist_controller.py +++ b/ws_nodes_py/twist_controller.py @@ -2,6 +2,7 @@ import rclpy from ws_nodes_py.default.GetParameterNode import GetParameterNode +from ws_nodes_py.settings.robot_settings import is_real from time import time from geometry_msgs.msg import Twist @@ -15,7 +16,7 @@ class TwistController(GetParameterNode): self.delta_time = self.get_declare_parameter('delta_time', rclpy.Parameter.Type.DOUBLE, checker=lambda x: x > 0, error_text="delta_time not more than zero") - self.is_double_angular_speed_for_simulation = True + self.is_double_angular_speed_for_simulation = not is_real # subscribtions self.subscribtion(Twist, "twist_joy_topic", self.joy_callback, checker=lambda x: x) -- GitLab From f6b8bdbe1a671d382961726a93ff1b346be567eb Mon Sep 17 00:00:00 2001 From: "toropov.nik" Date: Fri, 4 Oct 2024 14:40:55 +0300 Subject: [PATCH 10/23] for test remake cv cameras and object data delivery --- .gitignore | 2 + ws_nodes_py/CVCamera.py | 231 +++++++++++++++ .../__pycache__/compass.cpython-310.pyc | Bin 5756 -> 5756 bytes .../__pycache__/regulator.cpython-310.pyc | Bin 4899 -> 5503 bytes .../__pycache__/state_machine.cpython-310.pyc | Bin 12946 -> 13564 bytes .../__pycache__/videoD.cpython-310.pyc | Bin 4924 -> 4981 bytes .../__pycache__/videoF.cpython-310.pyc | Bin 5926 -> 1528 bytes .../__pycache__/videoS.cpython-310.pyc | Bin 5960 -> 1463 bytes ws_nodes_py/compass.py | 19 +- ws_nodes_py/default/ObjectsDetection.py | 273 +++++++++++++++--- .../ObjectsDetection.cpython-310.pyc | Bin 3048 -> 5810 bytes ws_nodes_py/regulator.py | 49 +++- .../__pycache__/for_world.cpython-310.pyc | Bin 1120 -> 1120 bytes .../__pycache__/for_world.cpython-38.pyc | Bin 891 -> 1114 bytes .../robot_settings.cpython-310.pyc | Bin 342 -> 331 bytes ws_nodes_py/settings/for_world.py | 2 +- ws_nodes_py/settings/robot_settings.py | 16 +- ws_nodes_py/state_machine.py | 64 ++-- ws_nodes_py/videoD.py | 126 +------- ws_nodes_py/videoF.py | 157 +--------- ws_nodes_py/videoS.py | 151 +--------- ws_nodes_py/world.py | 76 ++--- 22 files changed, 632 insertions(+), 534 deletions(-) create mode 100644 .gitignore create mode 100644 ws_nodes_py/CVCamera.py diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..e99122e --- /dev/null +++ b/.gitignore @@ -0,0 +1,2 @@ +*__pycache__* +*.pyc \ No newline at end of file diff --git a/ws_nodes_py/CVCamera.py b/ws_nodes_py/CVCamera.py new file mode 100644 index 0000000..4050e47 --- /dev/null +++ b/ws_nodes_py/CVCamera.py @@ -0,0 +1,231 @@ +#! /usr/bin/env python3 + +import rclpy +from ws_nodes_py.default.GetParameterNode import GetParameterNode +from ws_nodes_py.default.ObjectsDetection import ObjectsDetection +from sensor_msgs.msg import CompressedImage +from geometry_msgs.msg import PointStamped +from math import degrees, acos +import cv2 +from cv_bridge import CvBridge + + +class CVCamera(GetParameterNode, ObjectsDetection): + def __init__(self, name, camera_angle, size=(640, 320)): + super().__init__(name) + + self.set_pool_masks() + self.__camera_angle = camera_angle + self.size = (self.width, self.height) = size + self.fov = self.get_declare_parameter('fov', rclpy.Parameter.Type.INTEGER, checker=lambda x: True) + + camera_topic_name = self.get_declare_parameter('topic_camera', rclpy.Parameter.Type.STRING, checker=lambda x: x, error_text="topic name must be string") + # Топик с отладочным изображением + self.__pub_debug = self.create_publisher(CompressedImage, f"cv/{camera_topic_name}" , 10) + self.subscribtion(CompressedImage, "topic_camera", self.__img_callback, checker=lambda x: x) + + self.__pub_pos = self.publisher_from_declare(PointStamped, "object_position", checker=lambda x: x, count=10) + + self.br = CvBridge() + self.get_logger().info(f"CV Start {camera_topic_name} {camera_angle=}") + + def image_processing(self, frame): + pass + + def publish_coordinte(self, func, name, frame, is_debug=True): + binary = self.get_binary(name, frame) + self.__publish_coordinte(func(self.get_counturs(name, binary, True), frame if is_debug else None), name) + + if is_debug: + miniBin = cv2.resize(binary, (int(binary.shape[1] / 5), int(binary.shape[0] / 5)), + interpolation=cv2.INTER_AREA) + miniBin = cv2.cvtColor(miniBin, cv2.COLOR_GRAY2RGB) + frame[-2 - miniBin.shape[0]:-2, 2 + miniBin.shape[1] * self.mask_id:2 + miniBin.shape[1] * (self.mask_id + 1)] = miniBin + self.mask_id += 1 + + def __publish_coordinte(self, res, name): + if res is not None: + for i, ser in enumerate(res): + dist, course = ser + msg = PointStamped() + msg.header.stamp = self.get_clock().now().to_msg() + msg.header.frame_id = f"{name}_{i}" + msg.point.x = round(float(dist), 1) + msg.point.z = round(float((course + self.__camera_angle) % 360), 0) + self.__pub_pos.publish(msg) + + def __img_callback(self, data): + # Получение кадра + frame = self.br.compressed_imgmsg_to_cv2(data) + self.mask_id = 0 + self.image_processing(frame) + # Отладочное изображение + self.__pub_debug.publish(self.br.cv2_to_compressed_imgmsg(frame)) + + def get_course(self, contour): + moments = cv2.moments(contour) + cx = int(moments["m10"] / moments["m00"]) # Горизонтальный центр контура + # cy = int(moments["m01"] / moments["m00"]) # Вертикальный центр контура + course_normalized = round((cx - self.width / 2) / self.width, 1) + course = -round(course_normalized * self.fov) + return course + + def get_poster(self, contours, frame=None): + posters = [] + + for contour in contours: + if len(posters) == 1: + break + + # TODO расчёт дистанции + dist = 1 + + course = self.get_course(contour) + + posters.append((dist, course)) + + if frame is not None: # Вывод информации о контуре + x, y, w, h = cv2.boundingRect(contour) + # Выделение контура + cv2.drawContours(frame, contour, -1, (0, 255, 0), 1) + # Вывести ширину контура + cv2.putText(frame, f'W{round(w,2)}\nS{int(cv2.moments(contour)["m00"])}', (x, y+h+25), cv2.FONT_HERSHEY_SIMPLEX, 0.75, (0, 0, 255), 2) + # Вывести расчётную дистанцию до контура + cv2.putText(frame, f'D{round(dist,2)}', (x, y), cv2.FONT_HERSHEY_SIMPLEX, 0.75, (0, 0, 255), 2) + # Выделение прямоугольника + # cv2.rectangle(frame, (x, y), (x+w, y+h), (0, 255, 0), 2) + # Оси через центр контура + # cv2.line(frame, (cx, 0), (cx, self.height), (0, 255, 0), 1) + # cv2.line(frame, (0, cy), (self.width, cy), (0, 255, 0), 1) + + if len(posters) >= 1: + return posters[:1] + return None + + def get_island(self, contours, frame=None): + islands = [] + + for contour in contours: + if len(islands) == 1: + break + + # TODO расчёт дистанции + dist = 1 + + course = self.get_course(contour) + + islands.append((dist, course)) + + if frame is not None: # Вывод информации о контуре + x, y, w, h = cv2.boundingRect(contour) + # Выделение контура + cv2.drawContours(frame, contour, -1, (0, 255, 0), 1) + # Вывести ширину контура + cv2.putText(frame, f'W{round(w,2)}\nS{int(cv2.moments(contour)["m00"])}', (x, y+h+25), cv2.FONT_HERSHEY_SIMPLEX, 0.75, (0, 0, 255), 2) + # Вывести расчётную дистанцию до контура + cv2.putText(frame, f'D{round(dist,2)}', (x, y), cv2.FONT_HERSHEY_SIMPLEX, 0.75, (0, 0, 255), 2) + # Выделение прямоугольника + # cv2.rectangle(frame, (x, y), (x+w, y+h), (0, 255, 0), 2) + # Оси через центр контура + # cv2.line(frame, (cx, 0), (cx, self.height), (0, 255, 0), 1) + # cv2.line(frame, (0, cy), (self.width, cy), (0, 255, 0), 1) + + if len(islands) >= 1: + return islands[:1] + return None + + def get_bous(self, contours, frame=None): + bous = [] + + for contour in contours: + if len(bous) == 2: + break + + # Вычисление дистанции + x, y, w, h = cv2.boundingRect(contour) + dist = (w * self.k1 + self.k2) * 0.01 + + course = self.get_course(contour) + + bous.append((dist, course)) + + if frame is not None: # Вывод информации о контуре + # Выделение контура + cv2.drawContours(frame, contour, -1, (0, 255, 0), 1) + # Вывести ширину контура + cv2.putText(frame, f'W{round(w,2)}\nS{int(cv2.moments(contour)["m00"])}', (x, y+h+25), cv2.FONT_HERSHEY_SIMPLEX, 0.75, (0, 0, 255), 2) + # Вывести расчётную дистанцию до контура + cv2.putText(frame, f'D{round(dist,2)}', (x, y), cv2.FONT_HERSHEY_SIMPLEX, 0.75, (0, 0, 255), 2) + # Выделение прямоугольника + # cv2.rectangle(frame, (x, y), (x+w, y+h), (0, 255, 0), 2) + # Оси через центр контура + # cv2.line(frame, (cx, 0), (cx, self.height), (0, 255, 0), 1) + # cv2.line(frame, (0, cy), (self.width, cy), (0, 255, 0), 1) + + if len(bous) >= 1: + return bous[:2] + return None + + def get_gates(self, contours, frame=None): + bous = self.get_bous(contours, frame) + + if bous is not None: + # Проверка границ интерполяции + bous = list(filter(lambda bou: 5 >= bou[0] >= 1.5, bous)) + + # Расчёт курса и дистанции до ворот + if len(bous) >= 2: + bous = bous[:2] + med_dist = (bous[0][0] + bous[1][0]) / 2 + med_angle = (bous[0][1] + bous[1][1]) / 2 + return [[med_dist, med_angle]] + return None + + def get_bottom(self, contours, frame=None): + points = [] + + for contour in contours: + if len(points) == 1: + break + + contour = cv2.approxPolyDP(contour, self.k * cv2.arcLength(contour, True), True) + moments = cv2.moments(contour) + # Координаты центра контура + if moments["m00"]: + cx = int(moments["m10"] / moments["m00"]) + cy = int(moments["m01"] / moments["m00"]) + else: + continue + + # Вычисление положения + lx = (-cy + self.height / 2) * self.meter_per_pix + ly = (cx - self.width / 2) * self.meter_per_pix + dist = (lx**2 + ly**2)**0.5 + if dist != 0: + course = degrees(acos(lx/dist)) + if ly < 0: + course = 360 - course + else: + course = 0 + + points.append([dist, course]) + + if True: # Вывод информации о контуре + # Координаты центра + # cv2.putText(frame, f'cx: {cx}, cy: {cy}', (cx + 10, cy - 10), cv2.FONT_HERSHEY_SIMPLEX, 0.75, (255, 255, 255), 2, cv2.LINE_AA) + # Выделение контура + cv2.drawContours(frame, [contour], -1, (0, 255, 0), 1) + # Выделение прямоугольника + # cv2.rectangle(frame, (x, y), (x+w, y+h), (0, 255, 0), 2) + # Оси через центр контура + # cv2.line(frame, (cx, 0), (cx, height), (0, 255, 0), 1) + # cv2.line(frame, (0, cy), (width, cy), (0, 255, 0), 1) + # Вывести ширину контура + # cv2.putText(frame, f'W{round(w,2)}', (x, y+h+25), cv2.FONT_HERSHEY_SIMPLEX, 0.75, (0, 0, 255), 2) + # Вывести расчётную дистанцию до контура + # cv2.putText(frame, f'D{round(dist,2)}', (x, y), cv2.FONT_HERSHEY_SIMPLEX, 0.75, (0, 0, 255), 2) + + if len(points) >= 1: + return points[:1] + return None + \ No newline at end of file diff --git a/ws_nodes_py/__pycache__/compass.cpython-310.pyc b/ws_nodes_py/__pycache__/compass.cpython-310.pyc index c9a630b4209ff791ea0a2f7ed604ac417143fba4..454f1d432117d1b66192e1474615ac5da4b2b3d7 100644 GIT binary patch delta 90 zcmeyP^GAm-pO=@50SIg(?xZQmZshCZX0+Knmpg@#k!3RzZy^(7>E;st>5Pn%HuDIU tFfy*5+$c01NJmN2f{>?~@;$gae}!Nth(9{`}w8e#wd delta 90 zcmeyP^GAm-pO=@50SE#o|4zFmvyrclo6&spT<#P`MyAb7yoF4R#hXj`r!z86*vunX t!pOLCa--06ASo%ligCu|Bf>WsO()M2S;Dwxv$LoXBfA0z2NxsDe*p1z8*Tsq diff --git a/ws_nodes_py/__pycache__/regulator.cpython-310.pyc b/ws_nodes_py/__pycache__/regulator.cpython-310.pyc index 6ae67a7ddb4fdb5f1bc2634f881d2cba7971b0e9..ff0ba2d18db12eda10dfce43da4f4faf52623581 100644 GIT binary patch literal 5503 zcmb_g+ixRR8K0ZSV|(m4H^)h~X}aB3xRiu07qzsz+jhIOOSjoANr760spB(ooQyr@ z%(%NT$0{gWRYGb75*NX$QdUTn_5}&SL;nR{c>w7=AP^4_!CQHu4=um%jO|?7luBhr z=bP_4*YAAiyPXO1`K*Q~xBULbugaSCHzE!{7Kq1DqPGE<#`Hj|CSSd(gEj)AWmZj{ z>de4urK%}~TY=q5SJN%0>a;S|jH*io*;cNaYq?cd(RQG>^3{B+P%X5^s$;EUwW#XT z!FX$;I-zhqz&cNB%wd_E8q4_Ru34RAS(dx0Rj2%E=58DOd4FnGLvMXml}dgeylN{I zT^67A<7+iuYx%LyS3>5aF5Qeg?$?5fDXk~NFp#Os?f9WDOM5lu&32<=$kf{PW)#cR zwXoTaE0(ls^)QlY<~O+SN79U%ZJFjZ)~vN7OlV%a{8XJ*Jp7mgB6mz|1y7GgPsP*hUUo*+CbB9CZEpr#l@pon&ED4b=Gc8iF%744|BB3TztIh9ameKa zgW}ZMb>E9OxgTwY0XuSHbb3@)QQjrfBAvZstgPM9?&wm#!6#9tz$H^Y{{%(u3C zwO9Bu52Lfo*CX$Gw7lMJ28{4F6gu)cJ4f&lGfSNv={y$HTI;NKp5}em0I;;)eFy5a zfvfMnQhf>kf6Fze4>;->8#T;$`VB0uhsWy}1g4oPqcPvk|Nn84xP(N~g_I%$ue9ddK>TsFEuuP_^5 z1d8LgSS_yMYqp}sfgg10e|7a$eZG^R8z^O&}C!YHxv48IW#^q=xIo1ka zfjRGV0F;_!6i&6Dn%|}W&F?+bWs$&X0w+mT&2&F{NNaWN>NPnYQwZ~TxE{s{-=CV< z&(r#(0=>B>x^cJ|`+*;Jd|o4kTZG%=vCk^=G8J`EE1jWhk<$%7_LyIX^YFdSFdEAg zuLp2ZnNfU})*7TN?WeB3@Z^K~EAXLvq-Sw!>o9oIlHbQ>D1g8+nDTZ>gM_KB0*MnxX>2q&`hpi#kJWY)m zGI*q4Wn=NEn$E%6qV}RA|=eSCTwAqHMFPujz}|oQWFlr z8Ki=fT0bK){85n>nR%_B6^{DmVx%D=yQ}Z&eOKg|xvlX>m?hk@2HHi^W+ITLR`w}A z{T&6*UlQ-VzX=~+u{e2msqb*gRe1)WlIO&d_hNn>VLXDLYSS&S%%%xyiV^>gjwp_Z3~C6KzJ&x_1%>nuHm z9`V>7l1ucmc6GoUG|~{c+lI()O?Z=WK1TXSbMNW`{jr7vP5miWE%6_#G%%g4^ilIL zTg_AUY>{2nuzGfDh|?Z2+s2P{{<4w?36W>ktR`H7u)sXC6$Mz<&~H*qh)FTNYxJiC(q-)V&jQIa`b^Dhz^FGc^qg*6 z%vN(x$FuRAn5JIJE=$1C52H2yZNzXQVr*Vx1xR^L%!)ajaS`}Dq&AO)`4k%$Gq(%|J^?4i6yTJYzGb}S4AN@VE51vV zcJ*6G=7E(NPUn{PmU*nl?5M{aVM+VJi7=f`4yQY>PIrEIx~z2UL}t{9G%^~USEand zzedLPEI=>)+|_SfT3oxT63BfMRr{1m?tg%%@ylP(^ZN_Eoa%M)^4Bk3xwKX%10Ozw zlhLoDMC6(T3;>6Kn;9J$IZD)+ss=Erfk9Z47y?JA)=l_%8KV_bi5oDa(Sd{OoIPgc zDzR?Ua^|E zZNN_A&ewMaUOzvglXa3HFktX#`YrQEG~qH9h&BN}qn|-wbCFGCRaW65doaH58dJuC zUN&sBEa=vQ&L6D@+&@u2m3^3w;UL?{>W(poQZy)}6!oluFbUi>?l(>&7deeu%W~g0 zC}fsTQpzLYhBTOLPK6uFci}{|+ge|T{b*gC7vYfgJ%rHV9h~mrDn-Vv=vDOR=K+xZ zC8=D+NKTv%>$ufCj=DEdB8rK`c+s%l?~NZ~GfC_!8A@l{_>JUwGV6J*kadwSc+&N} z?{;fJ(!wcYJEYr+m&Z{j^Mm}Z?gzmz$5UtX42@9PUI7@HN<=E{P=G$;6db#FiBn4* zW%Q6Q8VQKpON^gBxA9*gHV`w3LPQ>TO-t2ldnx)J2+HP^R8o%GjMV~2Q#&v)7e5s6 zr)fw1TEIX;PCRGaOeVZMm61P6I3v;ummc^q2CuvgX`dV9QgX8ylF9ABjbi5E@*FME4VpY~lHaS; ze!Su1e@e`E4tgl(Lr1St%EW7o+=|Rb%~pr9!X{HU3neGDC;kH(b6{Kwqf^BHmP9qq g(-}qN1Ysr+g^LWbsFz_D&@oec)k!;P7~2Q`1&Gbw5dZ)H literal 4899 zcmb_g&2JmW6`$E%E|=tzqD1P$vXiKR)0ROhHz}INjX#pOK^oMR-C7M$qQHXUtSCxL zE<3xlEoOL<(VPPG(3=rJz7+5!K+&RqM9&2Z%&A2Wh0nbdImG?FS(5s2lmg(FQlge(t)39`I^IkVeb?$cD?WdrfRy$5~UW6SPhDn<^9)Ii!kz4^1GSanaeZ;o8 z;KF*~HEm%F2QoS$CtSR9A}>6=T~QE4yz?J1hc&%BtWna%8*x9CaTJBJ&5vzDxQ9l2 zB~?%Ij@J)Z&z>1jr*fl#AQ(siYyjU>(HG-*sz)pKY`DLSs(0c?@H9_ZHd;kdD_qdaMOgd{ z*Sb%J8YbzPR{E-3iL*3Z%R6C9;6?>$Jm|J{enOtPgEGW(w;`O0*Uvw69g z0&f8~c^)VZ)e)_HI!DJVXXYTL=44zZ$&?LSF5;z~?O`9%i*UO&j8a`0#7Q^p z#{G#}-jqI57P$&GMv5=j>KsiY%Y9`|FVKparyi*?&16*Q}+h|##agkr{1qEFP;AXu<8YxV&*+Z*jZLJGroZt+PSlN>$aXxDNO`2 z-ip&q?yoJZ*`JI%T77$%#%pO9h4CPiEpoUuL^PR(qOquR$q>ETn??=2*a_1>gl)uy zFc?hJoX*L1gy>V4B|hzJ!phqH(XC&+bMsy4lfFv?mPxPGWVn^IWp~T?_jW9gSV;X{lF z7gRyMsq(6Do{fvjGw)K0T&#)*{D6;rRTB0dlb3~~{1pb;M`dCoU*$HQP#X9b8lJyj zH!T6rv zC$JBIba5(1=y~)-@Dt({gi^-xMKDY#0#cvJf1woY^ePt9!jM2 zjB=6J5s**oR?37kT74J%5;EUZkI~XIT9}C^E-u}3s&tL~H=zmT5j2iwp`otYbdh8H&*I_|%Wj(T#{m2@7Y4LzOiS3fF zSaLJxffUil(_j=^UE;rqTQ8VYcQVcvq8R|~3o zV2x|4Ci0N)p;|C2YUdd+nhgvy>wO2cBk``M3u#T&X_mPWi@-4ttC{?RD5*uBI>73WYL_+#R8)8%gmUE93%grd@@WT+eO z|NO?ycQ;KuXwyBKK7`ZVqM9{%1wg^iU~zl}(D_h2LXT~F0J9$Wom0aS#2drWZ2pAm z8zVt>+bnGj5Jv{Dow`h>OuR+g(cZ7H(aXoKPnNUeEQ&l^wn`E+yVgx$-QPcb`n2<5 z{nPjUG5X#0QI6{Kt0QkshA7y>t0QME>R%nXYyDXEu71YXI^Q$TAFs=ovAVG$Um|uC z_oS(AC|?~*UlcIqTLgYa;3fe|MX~{a7`T;ar`Jk%wAYRY`$@Oc*LegAs-ra@X*cTj z!Oo*8U`BmS=RfPZmNa0-h`+b@{{rzm5%(O@MFni%1WyRW!)~iDKRRtMiKgHy^aa zUGCVPb zWDgR&YB`^e=8uV)6*7$i6=8k+s}Kab7zDjo43Sv_?FYfHhpi|ZkyO28PHEiJ$U9<`OE_JW6FXuj0AY%aTUgXtRf|^~^vNAyWKD@Cg5h zq!uy^QOI4BDCd}dc9^60k)d393G1jP>!xM{lx2f%U;FsN#UB&<=37^L$<8np@x4CE zwqkqsm&GY7(v|D>?}1-JqI&QScm@1Ken@-3a5NqSjjGvH{*rnx9{ZkNpL}$!pZ@Pg zk}IGdNJW6Jxn!L_=}7UcN;L)Qd?&hS6+_w1_eb-jF%J5T=$f_5%P diff --git a/ws_nodes_py/__pycache__/state_machine.cpython-310.pyc b/ws_nodes_py/__pycache__/state_machine.cpython-310.pyc index 346b1d1a9c1c7bf1ee3d3119bbbd0836b270be13..633b58306e3f657c3d3633fc76a74ad4effab8cb 100644 GIT binary patch delta 4517 zcma)9eQaCR6@T}==jUg~c3kJHNu4;KP29%$XbW9C=oei}p-n$hS`jG5$$bPTj&1IH z>4$M}Xxf!BSUWCsV{}p?U{Xt`F|};%1`^Oso7PRNnx<_}6Pl)JTG%wf*dJ&cWanHv zX=<{KXT6`#J@>rxanE_@-uI00@3=4J^|~dvc4t0*$lm-NU$flx6bucik5H*krle0f zAw5K>OqFALpGrg2LA7I~&q1A3KPL5Qw4V3Ny8^V9*1<4vLQ%nbvvqDaYXrpNwvyAn8 zq~vPNZWLIFuoPi0K-j_15dTU^Ie)-YL$>fEp2vd7vI#A+$L<97K3?X1piT%V>6BS4 zvKd7B_$lu_-hDu^RRHH?b{GG^8zcv~&v%~O#jp56KtEnl=C}=}ALJkS9;{o)cint= z)S{_F$w8hfttb2V>C#8{vFvCs}iS4jb>G*){g0N#z>qy#Q z!jt>(Ic3;5!t$o~z<7v1+7}xY2qyd_(`Vzj#ScOsYJqH9rH6A9f&0o{jq`JFkw4DG%WY@gv+)ofF0r2o2$qXep}$% z-G@<&@Je`aD-3OynT{u;spNpcL`)qh(+MC_A1rGjkMT`q?G*{+k02xwMCjxE_Z5pP z7>rS~sR8Jcn|K_I@mI@skqjs0P3v%_&nZ2&(~6H6sZonrz<}C42!HGlf>0)$o4_eW z6a&lP)C9V(oj~$Yo-4o7kVW!1ih0-q@vt!-YFq3_J!~!%_L!*HabP~hpRZ^O6$OKf zYAbQ5fq7va1?CswE`$}pu#Q4l<-DP?Un@9wvhoBu&KFf}CZl{`Rj6XYnD~=b;SlDd z5E4?|2rCi9>-?n1(wU3zRUIWIXqDyT4%{k!iy@rnWq)!gS zE`V?eG>vDMtaF@!!FTzumy8F#2GsN=6u|a&P6dH@aJAz(82pgG9BjXRfsC*xP(Fd$-&|X^66-e^`pLe9unM5CC$Svg(Xd>U(Yt)C zVav+TnqQGxp;9!Ls#iegHMmWzE|FD_M`N#uoUUm+*kqwib)ocyVe5iPmJzSkKlpo% zM;l&4X~(?v5^vB#ec~hja#J&TTr@bW+YjL`bON1tRCY;tP||#A^)Z+^r!lcKSAbr~ z7c6+3%6AQ(= zQla`3fuoG8SF(H)(wL00SvguNB>j_%_)l6k+;VfNgah-Uz7eo+SS&)mO0aq6v%Cep zEJY9nvk@sQm~(~hoI}QaSgDocKsul~n3X^TNHc*g01iYiEe+QcrCq$yBE|Ww<1#$( z3EUrQ0J`KOklV|+vRf5kY=E164fk%VOLFD3g1XN-XeF(prF8LG1*mE` zUL#clQcKRtGzd?rRE5*EV^D@8cO4wS@fnrY(+1$w0nbGn3mlX|PSYHxv`8ke2W3LJ zf>1Nnii85R3^-2OQgCqTS?8D|tBz?|a+;j#5yy4~>RhF*Sm%~wNE>p}HcQV+6NHB8 zvN)m3pH^VTmDMKQQxYwmkT*-G$${Yg@|bf>&uUq{Iw#TgYKf}D@@c~Kf|5wq`GWKf z*(%9Ov_ptVw6j_&P+fCW_Z+pNTAI-4N*b{|d1G+Mbi!--30;*`vQAoBC85Symmc|1 zL05Y#821X}+l!2QZ!*r`UtW=0$bkvxr_DkroBp3+uv;nYju+YWWoH9jJr~~RAUuWU zJ{2DrI`|n3tr3PU6&Zp?R0ycp&_Z_3(^a|uy_679jWG0wB18W9h9=|%*Xy_cEsTP$ zwbr6+4hu`BZL?Y258sI@{3f8tlCEwjE%RT8pcL_UFWgpOvW$K{OXzJ=O03)}pRAYy zZk9||PDx`f@r`+VPo_KqSAG5dSlo&y2OH|^am;WJ5=ReP8%Z5B_NEw-VB2RL_*({7 zitCr_8TL}xswhg|%inFUU*t@u;D{RSwhP1x?vJe13Ie+#t+~O!?!H`i{4Z~imW#MwZ?$av@J ziFDsQT#<2_2}3o~k#2rHvSi&fZr0Nu&&Szqb9>Mr>!K6B*SwG!|jY+MZk{-I3t|s?9j)747=eN@Y#XvK0^-j>)mTQ z#1|8~lQ)7c|xQRORAL9lW A^Z)<= delta 4110 zcmbVPeQZ0oF{jfZ!QXr5zufm_-(^>W+;swmx->wjG$2zlU^yl2 zBUGlA6WV}6tErW$C&+-6+NgFy8c=BsACez)oRSo2z_UvV`1xCMGr2JLclkNuO-j># zqmufP(t&zfJ);nbN+44+_hV(N#bsI(Mu-{n1QZ5V&XsH5B>cL44e8=v*t>bV<5c?= zm|=AYk0SIV_;XMXl&K|jBRCQZMegT+bNtEoNN!fh2@!QXPD6&ip1)J@X2lK^=s{>g z*a;A@vMu~#VK>>t&BAAVsMI8Mi6XWc*t@yme7Z&mSmLq7Qjtv{vX_7Ae4=$XP^=dq zAhV44SYl+U_*Y^6DIRlOB3pQs+t0g;ORejG_cY(>9;{i;6D_fS+@P_;QJuf*t|hzq zXYONtsLQm54Sg(O+8`Ps!(i-j)NRY?wi>Z`c-XW<5JExYXk2H)yuG;2GO8ctUBzYA z$6&Odzg^Ve3=p;rSIhnVff7Dd+?aX-hO8AqL=R(TS`r2`l}I>Yn95Kr7Gb+^9)rX% z3=ExP=nWf07(zfDrZX5Ej)W44VLcKF2J>RF6-6*%tONj`gcm@Qyb39l9rc>kXVZ!_ zMYF)qR&HS>yt}59-)_)&$nz6>CT+v)5Wn6~Vnu5cyrbl2-Qz%Ij2lF14S;D+#KX~G zEIO<+;admJbOOvmy11*fksRT@rK`&JBR`CA06~Oc@?4F#EQUjP5-NZuIY^#5wp;xL}I^qV$h#l{VG5ExK(3HZkh1hKQQG>uahkvN9?ur&JP zK!Im*c!VD;yIOY?$>RWlLbi;9Y=ZYSdyCR2mGg}q6Oow&=1G3Kyv4u7JB+w#2}g}Q zKU2Un z#Z$S=ot^u@dz=(Z;Z(*|9O{IjY+-M4Z4 z9KyE{P9vNFSPXuWKeWPc#b~|EgDXC*JBtF}1DM|r4GYeeuqJ=kxUURO@=4#rRt{pX z^56T?p67AFFK{XLqge&Y>#6Fs)^jkp#?M!;TE9#Z*f&rq6kklyD>bK6ST3USgcnda%jOA$OuNBGf}!Y0M9)}( z(KgjkJg!G+UcN-MwER*<4MJF<&=}Y(ju-RG-)txr$MOb$+rO=78s}yZM5F$psB#R; zpON><+=kliBqd^=R|PONZv_Zc=T-f3wScFbO&}B@h<>*osmBpK2qK`FdcdK$TOLNL z4Pg}m-_h)7OCyylBhwNZN-(sOT|(#u!21K~B;QcqCMxEyJXXKC=f6#@$g0>9drQSd z*z0%jBs2h#(85BMT@>A=ZRMer2Z0?>mdjrDvSEjF2k8_3!OG)xKgSi;ypRn7O~h1UaQsZ`a3q${+5a7#Ojo=UosFnZ9Exigkvfb> z3EK5rw85QqphG{R504vqKYJa#cfSF1g(!3#SHHoJwN}e-S@?^seSD#{h=0^t%HMA- zau!@rCS_Vkozz8D?rU3-(r77lT(HtIT29@x;(|p`s-V2Us}xiftW&5$S5S3ChWAV# zymjI#h1O6%aD2eC(^|o)6EeuD&vD#KWHS4JObRy2EL%yfON2bML?~M{vEhPk(wbBz z)g(Df&eRYI-eoN_(zHSwX9$rD>jCwEjW!utQko*PnYM%p4SdG}Gxnr9?U<3MdrICU zef{IF$&tI&)N4 zr8K3|ZrWxPW>ig)q-|7Y|Dvm-mZXikDMnz(Z2xHnxVG^ghNNJ>EQ24c#Ps{54}H0 z=mRsBP>E48T{;8YB$+OoktXfpL+L^OdWW|M+qby5O?51GNZ%b};$Flqv3cah@8jRq z4GdctH2Dw;@8JcVwI1~-ykf@=oA!vl&j=1h#&y1~^FcK$k?d?vnRcerU_1sdeYXm* z-?YOQ*=Q&_j|~D|goJ1scf?kDOA{i{l0-)eD7PH2ynQ&#hT%=&FDNG9J)q{XF^<3k zfA4NE3o@nzN8mdw%WeG1eDCdrg@v5WAjxhZEWR;F^93+8?P$4JIj_K&r@CH9p$nbj4s3)2>Iy$8-jcnF@SNB^bVtnU;tiWq6Y_ zyeb%WOt?Ki>h936!v-91N7t?1Nk+J_x=yVlaLp?GrPX7aOqAQ|C-MsJ>KP>EJl5lP zsS9%2WmAc^P&4nU;y>zn0n|U#`#^_*o1zUYg@Af9D)H*h(|Z$#`_I)>@T7%Q6rP#gon1S2+_-7tIF8dFT4B-j4+?E5l+pqfR7pgk1qo@p+1bQv>|JYi zLz`-&NQ4Lh3K9&QdPy#z9#IcS2(FxfBZs{J$4DT!AR!J7@2%56Bv4nHZ)V=S_h$UP zAAh~?&wa(PSj-dnK0k45HvOWwn-&Yxp&!H-VyDyf#Lc60g}z=9BdeT}gtRL0py-Er zL{x?K4iULkJ(E}CQ86lNTQV&nb}b%T1`E5MkhSr3vbIxQ))^($p_(qa!`GwP0=y+ixy0jQLsIeBJ8-c z$~1>@TLIXKixHLRPwdGK{!4FnPwz*47pTfhU%S0MyRPx@lc?OFsVo$ZzT;)R@D#2{?iY5<36yFBLHbBoN0M(YME7ZBdWOGFdbrNhl-< zgQ2aH#AxZZk+5v$u^C&5zDxyOMoG@jXA(%VR^KoYY>2>_wslrWmPo6()}I*bloCNM zWTT-^ZXn$oO~4-UC5cZIjJhw_42?W@mN z8Q1b5R5!AaVIcjvCPeCrFXCouLD|^7D9reQBNtM$;|Kn;e(>*deF~f$gArkH3#?3a zR-l!ewD3DU;NS&jC+v7Z9LWjTWj*Ky?cIO3ey|Hos(Sw`bX0YXLG^C`fhFu)4I|as zULZ3^1*hAU;nnG|y>NC~*3fs(k?wgfn2Vde=GMh4?sq!{q6l-ClR?TiGb$Twa*OK# zy=a}ohX4*8mRQ5`9Kgn=daN`|9d*8RsT!jc@00TgA%ctWxca74-+{>{+Xy~F3!$n0 zC{<5h!sSJP)O5ojhIG!1Q|4ZUv5>Mh%{ES?+M3qs4}ej2dcEJ8UqeJqCOZnIQ=0Su0T1>LroQ^`~*)e+s1RaC~MtSc|_&Q*MXSN%IB)6A>T*HIKK&HX&RDo*co89yEB_RgMm1> zrj(eN@ZycW=mR`9>5J);#>f5-CiCFOlVVJK(gz=0&zYq{O>8=o`E?o6hn8D7-+g;x(jbzIVO!!Jof4xifcz{UYGXRxTC0-KWD-&~1 z`S0hzix&Z+9Jei_<@rwFc&-t6EvE+Y6epjOs2UTAPt|)gCuX#BbXcrvcl1HqSogy4M*z}PXDU;vs;ab1E40kYY>-uGo(?jxqEEJUFv6@{gW+!hgw!IN zlwS_%3fqx(jK86z*-h7RQ?A7$AQZ;Q~Sm0R!ek01@4eG)P0k@i(eE zpF@$vZ9hsJ3vCWn>uqx1hLd*uAok)n&>v5hr{cVIlPA7TjHS*}=$`n^ik}lNW;L+F z5aRqi6wfC|+aWoSEGcA%uy>ftFcjFE6v`ljRb4$~lmuE`50xMhDs^Vb1!5^B$lCFm zK3cCauuEF%zOrxQ#RtRB zi62wN7t~e9H8~tx({Y`vj{D!Hdlmd%g5l%kk!CrXX2l1W)4$X6H$ebyUek61pPz?S ze68H0UHX#_z>DQgv7CK}mc{+-)iyF!l}KvXE{{plW~;@$ttGFqJ-5V%kvC!T+8egJ z9&B{)sx_p4oN@s~@eQJq0jhT5Btp?1XW2p)sd;ek5NV1u|8&u zSZxc&cElR+ZESb95pO7u9?Jl)i+1m43wTvNjf#G9=>UhUg^ki?JSnV_Ew0PKeVd*X?qU2k)*;s7Z?zMMM(I7)4H%-U=@DW8C z`YlOynx^~{)ar(TdieVYEUQI6E!wJxS%x EKVGVWumAu6 diff --git a/ws_nodes_py/__pycache__/videoF.cpython-310.pyc b/ws_nodes_py/__pycache__/videoF.cpython-310.pyc index d429b4a360d11779c434360f3273b86e6c462d44..9e441f977c80a3d06f8ae313a892ff8fa7641d49 100644 GIT binary patch literal 1528 zcmZ`(PjA~c6elH0mR-9`+I1U>9fqzzAz+KwoQt4nwhkBycWAO6bRiUxapcI7`Y1bd z;GL48r+$aznA<)NuRHC$+YVdbqhl{kPzofU-Xq_i-+PZ$uh(HXroaEY|A)`mpY(He z2>f}9)4ae%F~tj((C!s{>?Iyw@chL8kttt^Q>KLWPW(iuNChf9AJ)jIUxp$<+M zp65Gm#_hjQBOAorM%%Bq)3Js$BhJ-<9=Ty2;-*=`6-&IgY@aKxywfQ07d8{|mI;;w zJ1h?D`m4&ktaX`9cdKW4nZhjNmuhGuI>1S)%}=;A?1Y^;h36Q~oQ*u?H~vVBf>Ef1 z3NWPboj+%?RHjHhNN| z<2{vb-6CrqE`l9@a|4_WDf2RKq}+H?R9RY>EhH8fF*U91Kxc;oQKewj%&`#91=YC1vgor|&a<{NN@|*Du8(<+4QmQreD`<4R2mqPtSQnWV)+ z0+a=KKxTWFNy;$1WujcUd_hNC%BV$&0E?*0PcpnrZ<%XCpH{T)AU|#0fS?@S! zjd#L+;|)8*teoTjoS%tvd;*XY(C+Z}=nWDn3{4!uUDRL&b%Zm8HC5-O?P8}jP|aM= z*~lDB8dV*Y2<#&|-+86eg5I3%*VoY%<1{bt;UPosY?p_;!z141@GV_L$Ve__%m;C6 zVf@o8t-Y~)8G*vIem327Zn@j=HS(_5TQwWD{sR6jdTh)ZE|J$ovul2*7eyT|{(BD< C3}^=c literal 5926 zcmcIo&2JmW72la%E|(NZeNnO`JC2&Bg&8!q;}+=$+_Rk6 zC$~N6wfw}F)kyfXc57wbuP5;W*6K+!3b8gkAGO-jk7HllXn73+%iIW)x8A0~?Wh?h z7&!BrS7lRR*Hl^QiKZ9E%B)A|;$~PUn#X@;e-eKK1K+4IVbu6zW0eUe_(P{=3R74Q zjhZDg!p1WrvckdBer#AqEqlkPmNIS>O;;fUN*2n)Nm$S2u#XFa!`$Mw=);VIs7 zhxA_YaIbW1ub}sa`+G$(a%``p_eKxj8#}f)toO$Idn2N(_a?;TQ>)9s_h>REPNnR( z#_zb8+9xi?@kSY|^qdgWeOypE%qNpmVn&=khJUKRdPcl*Y;`)B*#*3N8Whg#8+)&a zvppJgTf)GFzgksx-D}l1uQl)CZdGy3hnT~Ln?&tqU1j^z&&hz~LG2FOKP+@9h99%) z$LJK>lA~DC-Dlssuokua3vzv9$J>+_WE5Yz@E~>{#1~dN%|OsNgo?&)d;4?a&8G09 zYvb9;=+W#>UDxd@r}1$ zN?BhT+3v@@y4<5P25c{)I8E!5M2blo1Mj7m;+3xu^jYe(3|@wGh9C&3GFPL`FO2T= zAv(74g^}qaXx<811zl*TFTI!J9VY4|zyE&bR|ib)Ol!QeA@I(&I&m_);?Ks3gwj`d z?~e1nl#z53e=FHB>zfx2C}4bH4qh8@pvD_s;=AoAZjv+Znc5dd|N0^Nsg)Um zi90KCT{c(r4Xom}+k>DbDFCa922cpU9(WR0HP}-bSr6bAa)%*KSO`i7bWK<@>2RYt7=dy*W9Dq+*z_JbgLK%!Lznrlg5CbI;>SES0*xq_(C zAeYe$SECA7R_q6>D&Nz0(o6W!oa^H3aqPN1z1BYH9I%k8$7JIL=5U*xHP7%f41a!x zPx2`?$tSTs#dc<1?j-AjKn*N9n$iL0(c)jCOA)p^OwOh3L*6yJR$`=9!V<*Pbu%@m zj9s?N%1AqQc{ii?tQYRth{BeIK)sQX=Y@%WJ~Vq#C&}y@-E5jkv#y=m=(8!`bJFY} zDp6$8(aEYFkefXq{ys&+?_XDtBxEFXNyr@LlCl9=qK>Ro%R-K0q}aB~_61gPU4!9waeQI{IiL+No(XSUlKbu0N0wN_Ir;RD3U5~iHU#|e)DdQ1e<(GSOGQgX8 zjtr30}6Nz#~U?@b72Oo>fNWcj)!0`eBCc!i^xMAkC_8UV^_|^Wd|zD_GGvuKWSL z;gD}2YVuo{%5T#e;?E^zLvP7znxLlgiP#i5649EATO1UV0aALxpRje~w9(Bxfxh4gx6{oEv+JPc61K-vmaugb&Du9%C;9GBl1c2x z3{?pVgNII%P4n1wgiU%(^%z;yF5hL{JRq@)YAl1|K=1FuISNJ)-9p3Y7ST#+VZr!l zw>zBV9vhFL)dJ+C52488{O^ZHRFMl58?5W>D0ePP4dq znvbPfk}L%peLJ~i4A@E<8~XlxPMS-dRo)L5XR7ihl%p}w7H!KX5-ZXj3J@`c)P(%0 zu@8=?jVH!B->^^=@hU2Six(@VybBPOZJ~$>lUS02my}c^8K-!1EHYDdAkcJmP<~;F zGBECwCCV~#0CU4fypsN=kAQSQJf2{j!X4FII{GIhM8sqP_1;6hDB$*SS$yBzMPX4f ztDpX~{KxBm*?D+nxne0Z@Wb9=3SPS{qpjOfu)T0w4%MEO`q#|Q>Ih-M3Dlp zZH&QuKgSihNpgf=Czub>{Q@E7qe>S>f-HjEH=0&dBN~5#)hT1c2suajSTNd^LnxS2 zAL^@5aE-^I?gI8c7bebR$=5+b3sw8zs%95=$Io9>d}|x&`;Q60rA|j{lCa8MZup_U z)t2vsn8w5}|9Q>WdGn;-d4Y%q@5(yiGFxw5lxr9rkXMrPG=j^22366C$}?J}k!Db_ zA01H9)>p{CUFn1MAS8AaTafP^L45M(KIkV`DpPV68-1azHbXHVg$boAs<^bcyyPz4 zLlmga-BhFV@6O$*x^uU0FWtK5R&P}om4z%dQTA#xKrB&q9LdBNYN+1BFo*0{nO?Xp zNn_jC+ zwuX`)z&%AWd+MVtTkDX{x!(uD576Qzba~{~7NdXOw3zek4Foq1tFW2%%(5)*uqn=2 zo;%3GIUh62$j31^^E9(AX5Zi?a|EGqgpD!e>CECU%F_pGSaGPPqJqG6Ro-=5k>~_8 zF1YUfju-TH4545?KE;xVpIVYv8me&k_ zPrT7V3dpx1C01aP*60FhA9tX?@to?BfBo2(r}O>E5mFMleY8#Mr-^Ci diff --git a/ws_nodes_py/__pycache__/videoS.cpython-310.pyc b/ws_nodes_py/__pycache__/videoS.cpython-310.pyc index d8a3f980d8e5beabbb17ed8977c981aa517661c2..1c4f7a35cd0e19105c303443ca021e439640af4f 100644 GIT binary patch literal 1463 zcmZ`(&u`l{6ecA~mJK^di+1RS9k!wvCZLG7oQt5STMQTqcc`--bRlGsNo3QaM#|0{ zc&DW3ssBTA%)i9zPCf6o!?yS6I!hCj0LjNk^8NVuJ?dVsL!gM@$EP`>dc~i|{n9;z$-#X&{x|&!%Ep7 zPsMa`i9u|kfJ54Lk)79IHraT0_YY;mg)CHVB(HM=glOPEWC{hzw12tMu>EB%UHtsj zug_k-FfrQPMw`?zE7bu2+2$O~Sj+i33Ln5-ID_z7ys<^U4!@x%Uws~p0s-4Jl=0dC zy8Rx86*)(y0Bcr2#0!SJ@fzUHM&lX5hHh)D@?y$sZOf+A)xJx{YA*R$&(qM|spn%= z+9O_QZA4i$(sdAs(yF`?E;ftnG!f1o z;sC!8mYYMH#^xSaa106UT>&NPvT|K0l*Z_#FBzBEqj@9raRs1$hvz%5Wv=kc7`(ZO zESu$Jbsr8{Bz2d@v_lixrRFhS1julo^OO$L*4p{KYn8dP`PdAm>B3^M>%;Q5%|p;# g^XJh+;GlBX@bBSYVeu`KpbKud!%iFF8H48KPFk|Hzl#_$-9Bx{WTyQ{S&ku)tp844uVE6+OUG^d)w zA$z*V)lG`z_F#8G8PY|-9+Ch-;DkInhx`XYfczPCN|1xY^Bxl95bf_(&kSi%$|24S zs=7X2y;t?#tM^e+xm+~x`^^vky!oqn!}vXQ&i``g+(wCifl3&H1xACuJYXH(;EdX4 zV0NsArQ24J>(~ujw{tNV|wfUgjsWdA6vC+mD#++ch zGtrpnOg1Jv)kc*W%Z4b3;$uS;eePRFR^yVWh!IhG%obL1yJrm)D9CgoQ1u+vMm5+s=V|>+Y4i5HX~GVJ8Tm1^FOoSiQh%T zHyTVB4gSQ~WP%C)*lC!;6xL&-VTqiu@y&_6aPYOC7?#nC4NsJ@o7z9ojGg9N2pAdg|YPcYV+DeV>Tw)7NQslv1N7vx= zGsq9Kc`=eW+lB4oHh8V)%Fk$JNzYY=bED_x%6e|BKUWds=jKNB+{AF*pGYRfOv-+0h}q9gF?T}ysAA0}jMH~YT<&9o9-=>;%!n)E8|Ns@ z_D8RZ*UpX3oyYw-A7D-{9|4Pd1+(8cF^;c_d7}L}CteBz;`nA=*-fw0+*@nE4PKu% zz;#vL@}UV~%T1zgyQ%X1?pMS_BBQc~@_!bp6wNPK{R>oz?aN6F>FUq7uWv;i|GM1X zIrR4Abs5DsuRn_2NAdNI-F6_T9YURBw|nrF_Fh}~(b~1{fpTsIUS~si@68Y~QV`2H zeEWhLU31-b*iKyc+N~gJdO`di8ri~EZaud3*0-^FIe{jwgeAPh!`JM@t+QjuDa^`g zRJ4hhHenl*I6yb2x#c%^e5tI>-4F~D{-(DZB#Q66^GeE^X>`xO;Pv&4P8HZ*MsbeD ziENF@%lH8A%~#@;uMzY-RayqGLQ0470`d~Y1awFLF-EI`O$%<$9?rWp}_7Ht_Bk-C#e{cM64DcTo7CxF^OFWs( z*IrY(c(;qI;0$h~s>FT*+EL(kym%*8W1V*BHly7z*_CeGKJ-;7ldtQAt-x1v@$N?4 zldS-F!YkjT0iLe%3fUgx4ZY3q}Hl$ z5`z4_`ufuH(yIIx9f8E4?6vjPd-Y{C)|9@N0PXHZ(2lpD1Z3QliM}eG-MAGlJ{3Vn5ha z#q1W4w!*p=To*@=W7o~JU0bHNfMpCNrX!V@!)<2stNaST!tlp;hR?7mKE-ZcE!Z>1(f*5s8aa+T_%@P_6hHqJu5L%D`5#7^0t|pGvu_aY2%0;u_{8C zBi_qth}>U+u;I=v3m$(bC*Kt&>U*J?IjF=wGJ5$mm*(AkYNK{ieq2cN0|!Pt&{d8MU5>*oII2rUb7WpCJ(*}F!0k!K;<9#&H{7AyPCjK>m#w0w4IqX~b z&ympZuQ2P6R_^fiiz2#!qe+fv@FPv4qGfd-?0_r~I;9AmdOYRZoT9YP_^gph4nYBh zQ2Yck(rrt(ZL|?o9p}JxGo>*KvFsBB$-^1@+)SQkDBwwRNiofVN^aH=_GRcmzt0B! zO%Avbk$-0P@`-a?N>Oi@K|i17Q!bn|PkWq&ixtjiT;Q)qfN$FPjBoN=866vV?HA}! z|8J&qM&*p6;ME~jtzm__cK4^maaH-MMOBWz-5XGkMNFTP%|HlICNaxy6nJFHPXF@! z`SV&u{wpDvIddj13<+Wl(xc|yP1w&Z4^~TCgAE-Y%Q~%a$UKnR$y?~kf1xorrkl!! zmXZ}UK}{9Ow5_^#R+sK5r@5CbL_s7~Wntz1%Bs7(dgtSttIK!$`YmS(M{bjn3T1Ek z?bcSJa*x^~*;1wKfY}DrNOnBE8=g%GdsL`QM=C@)Fx$YgCB)mbc4w-#U%*fDSyhgjmt(a_Y`8s7bc{a7iP~v zDJ1Nerz~OHCW>`pLfwnKkpx-76NXd;nZjcy$)`okI>IJ(6k6eCjU#@C{KV=_^d@`N-lepP6PBw6>dLesDpjL5B}UV! zzM1??jP2y&Cn4haPZ!hj@n|}(^P>t{Q`9OLpgxA{JBr+8LO3TmTvB#Co?2ounMkW- znkf@VC(_CE5-3!s4O~yQJ&BxU3fWeE2tN&YjGV#02#5Vw(=vGE?C}3((n>B-FB>D- zBooMa>`YQK2F}DUgPU*-`D9AWCevar&4csQpGfm0S#lkkJB4Hd*eV(u>i#+>Eu_vS z@0?%Dabe@+ zwbqCR#V-j75x!PHy^oPR3RrJ2i=RM6aRjse<$qRxx%|7s$M3J#EM*3Mm>s6%b-Oa! ze-H%+iw{)6lg;~n*h;qaxn8`;blo77OG}Y zU2hk>eY3KI%ARC#r2ebrrh2+Ll-b6<@we0b+x-M#bR!RpFGx4u$eQWk>M zMA@6|0RBPQaU>I8sF7xd;SOS2WqRR(B(ci>pna6!tU}82SMhXoE;hEIDSjvr+eq)x za0iHtl69m)a+%gvKUk@+yB{vCu6?-lvAcHfdk^j}J(SxtWyZT5@XOmr)edXZSs-Qi z9oY{6k_pNs;BvRU?;=^~2mZN^FWh~&E$muG!9)2y0>4j?dQ9_zxhkJ2bF6GHXt(fS zRiHS`3!8}6^8F+0lPccaKMTtgd!*PF@_PVhI%H;=bDPL$_0gC5A-ePq60f2vB9yim z{fqEo&hxk6(>N}K&8+8^WpRhiaL$U{L1@kSgjq$9jlNl=o^>Pt7O$9NY>th=A5J08 zv-rzG?|}x^9Lk$0&Ual^blpxQb^~gcT=&ymFUV#j1^YTEkmPeEZCI82SwYhefONX}qEO$sa# z6tZKB-sCWn({FRIR z>--afFRuDtK<|VLAH86ol-2Rt;m-i14^l$J3@Nb^lRu}0U?9zd2l_#0CL{k3=bo9a z_1|Z%Ex+)jDs^o4QqUJ)S|#yEjmCb6yXGQ%i?7jp5t$j}iHSHH(k6o}sTyzja4CsA zIGfC4JhRY+=AOHupJKC&Fc= cv2.moments(contour)["m00"] >= min_size + elif min_size is not None: + couturs_size_checker = lambda contour: cv2.moments(contour)["m00"] >= min_size + elif max_size is not None: + couturs_size_checker = lambda contour: max_size >= cv2.moments(contour)["m00"] + else: + couturs_size_checker = lambda contour: True + contours = list(filter(couturs_size_checker, sorted(contours, key=cv2.contourArea, reverse=is_max_size)[:count_counturs])) + + # Проверка, что все точки внутри рамки с заданным отступом внутрь + contour_in_border_checker = lambda contour: all(border[0] + border_offset <= point[0][0] <= border[2] - border_offset and + border[1] + border_offset <= point[0][1] <= border[3] - border_offset + for point in contour) + contours = list(filter(contour_in_border_checker, contours)) + + # Сортировка по количеству вершин в апроксимированной фигуре + contours = list(sorted(contours, key=lambda x: len(cv2.approxPolyDP(x, aprox_k * cv2.arcLength(x, True), True)), + reverse=is_max_vertices))[:count_aproxs] + + # Убираем контуры с нулевым моментом + contours = list(filter(lambda contour: cv2.moments(contour) != 0, contours)) + + return contours + + def __set_borders(self, frame, x1, y1, x2, y2): + if len(frame.shape) == 3: + channel_count = frame.shape[2] + else: + channel_count = 1 + + roi_corners = np.array([[(x1,y1), (x2,y1), (x2,y2), (x1,y2),]], dtype=np.int32) mask = np.zeros(frame.shape, dtype=np.uint8) ignore_mask_color = (255,)*channel_count cv2.fillPoly(mask, roi_corners, ignore_mask_color) masked_image = cv2.bitwise_and(frame, mask) return masked_image - def set_more_contrast(self, img): - lab= cv2.cvtColor(img, cv2.COLOR_BGR2LAB) + def __set_more_contrast(self, img): + lab = cv2.cvtColor(img, cv2.COLOR_BGR2LAB) l_channel, a, b = cv2.split(lab) # Applying CLAHE to L-channel @@ -30,48 +229,26 @@ class ObjectsDetection: # Converting image from LAB Color model to BGR color spcae enhanced_img = cv2.cvtColor(limg, cv2.COLOR_LAB2BGR) return enhanced_img - - def _get_binary(self, frame, mask, color_type, roi_corners: list, is_more_contrast: bool=False): - frame = self.set_borders(frame, *roi_corners) - - if is_more_contrast: - frame = self.set_more_contrast(frame) - - frame = cv2.cvtColor(frame, color_type) - - return cv2.inRange(frame, *mask) + + def get_binary(self, aim, frame): + if aim in self.__settings and "binary" in self.__settings[aim]: + return self.__get_binary(frame, *self.__settings[aim]["binary"]) + raise Exception(f"no {aim} in binary settings") + return cv2.inRange(frame, (0,0,0), (0,0,0)) - def get_binary(self, aim, frame): - result = cv2.inRange(frame, (0,0,0), (0,0,0)) - if aim in self.__settings: - result = self._get_binary(frame, *self.__settings[aim]) - return result + def get_counturs(self, aim, frame, is_binary): + if not is_binary: + frame = self.get_binary(aim, frame) - def set_pool_masks(self): - return self.set_virtual_pool_masks() - - def set_real_pool_masks(self): - roi_corners = (0, 100, 640, 340) + if aim in self.__settings and "contours" in self.__settings[aim]: + return self.__get_counturs(frame, *self.__settings[aim]["contours"]) - self.__settings = { - "yellow_gate": [((99, 122, 101), (154, 255, 255)), cv2.COLOR_RGB2HSV_FULL, roi_corners, False], - "blue_bou": [((0, 0, 140), (360, 110, 255)), cv2.COLOR_RGB2YUV, (0, 100, 640, 250), True], - "blue_gate": [((0, 0, 140), (360, 110, 255)), cv2.COLOR_RGB2YUV, (0, 100, 640, 250), True], - "red_poster": [((52, 133, 121), (120, 255, 255)), cv2.COLOR_RGB2YUV, roi_corners, False], - "red_position": [((108, 0, 112), (360, 255, 255)), cv2.COLOR_RGB2HSV_FULL, roi_corners, False], - } - + raise Exception(f"no {aim} in contours settings") + return [] - def set_virtual_pool_masks(self): - roi_corners = (0, 100, 640, 340) - self.__settings = { - # "yellow_gate": [((0, 0, 0), (255, 255, 255)), cv2.COLOR_RGB2HSV, roi_corners, False], - "yellow_gate": [((80, 70, 70), (100, 255, 255)), cv2.COLOR_RGB2HSV, roi_corners, False], - "blue_bou": [((0, 83, 0), (19, 255, 255)), cv2.COLOR_RGB2HSV, (0, 100, 640, 250), False], - "blue_gate": [((0, 83, 0), (19, 255, 255)), cv2.COLOR_RGB2HSV, (0, 100, 640, 250), False], - "black_position": [((0, 0, 0), (180, 255, 20)), cv2.COLOR_RGB2HSV, roi_corners, False], - "red_poster": [((170, 65, 6), (180, 255, 255)), cv2.COLOR_RGB2HSV, roi_corners, False], - "green_poster": [((43, 136, 5), (101, 255, 255)), cv2.COLOR_RGB2HSV, roi_corners, False], - "red_position": [((90, 0, 0), (180, 255, 255)), cv2.COLOR_RGB2HSV, (0, 0, 640, 320), False], - } + def set_pool_masks(self): + if is_real: + return self.__set_real_pool_settings() + else: + return self.__set_virtual_pool_settings() diff --git a/ws_nodes_py/default/__pycache__/ObjectsDetection.cpython-310.pyc b/ws_nodes_py/default/__pycache__/ObjectsDetection.cpython-310.pyc index 31f28b3bc9d70d6f33b86d3695ad4ab5588ca378..a15d70aa55f20461b949bcc20aea5a97ddf64e79 100644 GIT binary patch literal 5810 zcmbtYO>i7X74Dv&-Ps?lW!WKioH&j#6dO{KEkhLtmt$FWY$(;*NJ=VEk(wIqjMgK~ z&TM){wj^eYByvhR#1vE!{^C$lPKW~s;KG?BH;#RR;-oVdFjY3+>zS3b`YA}UTl2c7 z`}OPY_w)7Zi3SGB48M;*d^-2~*BSdez3lzzc)5lr9tLrl%L6u(m3&6SyB6qeW5(d@ z7ISsic*tC1RhuzgKE>+hHpZ|)+H;^ z`LUTYTXz{&P-$!#f~MSdiM)?J`_p*OTUD1Rpi7auR@b*lPunX*JP1%> zV#>xTxgRSg{~lhp!yZ#Tn^c(8K;pOm0y3@Se}i5>BC-qu`AaC2Xhjsyuc`aH)J}7N z=g<8-rmQS`K@dH#TTbFhYc^Q)?Ad5hmQ+nOmWAiqohVK`A**@aClk~SsWN@s=))*bC-lMMS>cj6~>( zSX%Tlw-^LlOkBXCpG=P2i`w3Zm|s|N?u!u-#iJt+V*5cnGP~#pF4e=x^$WtEr zPgD-~Py{3VGu2!R@lkXOEkwy<&8?7Ey4B2!{|}x4<=uHSKu&GXT(x|zV|#c8kKj4} zKky7Go-C41JT4-V05ufL4Pjvaf>f-0l;U;!4Ql!tHR+kNQ;1}7;{S+3fxYh5mR2(glW0-IJ3+NQMFMiKKhYDGMaF@kdV>C)k-IU8c=5^>J$Zqg2A9B;n)bx_clDU1=k=%6>KCjZul|1JwId?w z(4{m<&AlJF4*8G@rt7GrrhtQOu#|5C_!GVXI8XIWjq3Ay%3D0uKIfC{hpbyj*}Rb! z&|>Vg6sFh~Z`4hB;c%F;+mY~qOkpCNILY*DHSZ_mQ4k57i{YdbwmfPH0OlV_N38v;ssf5k_fIgFtbo7wtD!>2C&gI&SCE8l;Lt<>Yk2fc-Q}lQx3X@yY>|sI>o^jr zT+`Q#InBi(`_SrE!x!@Yuc8$YleZqrxi78hXf^Xz12CsQWZi0FBxYJo3MoNcF-3at z2+~vIBkeR>)mC}8vc}gm%wnmL1B!njtz><8!^kxT$tCJf0h*8WRU{y3nIv>8#P61A zuHKlmqKRYs;%<{bBbw<1I)IxhSb{^FK%g> zQxw}cqQWHJLbjA65Y048e#|J@2HC8_p9n~`3>;xlm7uVQF?MfIvHaMPfXKLA_`ScXQOGkS*?GutHe%= zH2~34U+|V?QF!+edENo>JBuf1P1qV+zU3X|~Rv2LMrx9_21PI*mR!&pJQF7I>(U+eDYq2It(vkGGuv68iLh{X-L#T8cG@1vNn8Cnw`iGlPx+K73#==thA%KIpBPzHwFBqD=jbd zmOA3<8iXkP(B3zgrd@nG%*)SKZhT=E+~brz(YQ9~P7o*;6K~Us`)#JCCTOo=V(V$2 zx@m(YJj#jBqLMI$?Br~GLry5?yE<>b4j5t?3cuC_g%T&@Cyblv@&H?grI@4&!Kz%sM+UmC5bj@Q-`K zYpibN%bfE=cRbHua%d9ofJ3N6#39~e zc^|?j@ta;OtJz?svRF`7BTu4}cz)@&TA*(&3l1L4A;0yp}!Md0x|N`k=1y!m|Q5o(&l|^j`iiPTQA8&=E+! zH%?Q$50W5$=MnlYU(mz=)hSN9g%C$vw~%N^CA3tZ(^4a+ChA%W+A>-k{f28i(z->| zOd5@{pjs@~ObaJjx0Dn&tkg=2X=zjE3~wbQDi#uza$1H96&fm{1qHZx69isrx|AFM zpXr3Jm&Lm%q#56HI-b@SZ>bH&5WNScdy{Z^i-z*7PW0$G{$lLvn6 z*-q%zE18eb=B2iDNovb@mPS#I%7NxRCk(wH^A&L(JmLm19`{=zZE4CoVxi0Qbf;XY+(uWCcemZ#-&1T@-gCjHp!UeEZy$}pdWsuf>bDYS`(U)I-ju~Qtvl<`dHKWW25mqqBg(D2o zH@HcF*!xl4z@yAU?gHOt^XxtoFSx3f;v}Ruv?3u#3XWcCqZ2Yq#-~_K*32^5Y!(ZlH6%87G+crMF&&-K0^08!|6BVRfs{oYM@W$ETp)9cW83l$b(3}bLv z$`tJQZ9&W4udwp2fZw4zCb&4oU*v<@%9#WD+2w7L8hSsf@2F)OFR30Q%b?o|ycGVb z)Wo5f^GZ#~yp@{HLp^{Or6$$ZrzZWNnh2D|Pe4|lBQ@nX9PcX15I-jtrLC8b&{hw3 z#ZSSC&rhTu@iQW%A6de=km-NFs;ad8-snGqH>NPf*Asq9QwlqHA=x|Pz|l_uuRMt^ z-U+^bL1lasr!Pv4N^un(uplwY`XSpP_@ED#{g+z#+7YaW-aCQ2N&^=#Nw(wckRsXY zixHt;{3rA(-!K4Z)?vp`KYycMm6mOXxCh#{EZey8xr+hSaj|<3_wcMoJf>McBSO$A zenI3qk=KbRV}44tUlG||qN>q4g$_bK8s@I4x3zBSF&wg<+K6u4h(Q6wIKAAtJM@D`qU;0N$K=9MS^15|{{cgA*OI~i-woH=vm%;o#$jN3CaWdr4} zKb~)XGh-NkQ)9eXXk5dWUVz{RXR)y{sM&_OWN?#PPYiB-Z*Ewets9=*ho7+$k{mA% z8rSfpw;_bF!ML$ua>mUk21Z)kMs4#`ydWH26opNjyS((o*(mZdub|K6RX&5d#B2Nn z>N20@Cs9}U9A=$Yv#X755oYPecrGg;}InA2m*;aRUEJl20f?UHiRt)L^6nRJvBNE!5$6D8T@x^i}fY^TcUqWO`s z!kxNuH={Vd-Hv;z(ulH0Q7Zf(;a)}Asfag~vnhiXLOs5u%pShSb<}k=6K(}bB4R&m zcauz2WIKW?6CqP&w}SMcIvF*SwiG@!&=t3(s**0aAGLy}I56Z{toHfZ!dAN_7UcHB z-C##9V0rb0N2&iPU1)Tpn3JBgxk&v^Z$jr{Gw8MTdCC{xueip7$Q_Y9IJX|29|{RSw}i7}`W zN)cq@=JNF~Zh;$}4sdr`LN@9()l^m-G*k&-4?tHeP^_VxFb=jr zjWAYrOm0N#WCvGPpZ{yQ=U4@bx+qj`kmfOFvHRBg!kL8Y>%{7WZf@LNUWI)Lzoo*$3 z&4KT_allqQN1jI<`7Q}MZ}|p^B9JX4G1kbj|%PfB}3msLvxGrA=?Klu{Usa$B<`G zgR9sb+Bx#o@I0*C0{hIB14}ZDJ`|ok*zRRm)j59}Am0bLS(G$W`93`0J>X6dwe(@A zf)pt(>-5gchZ}zf<5FUJjg{FMx|rvtXSsOj9n$hfg><^78fOXtM2&r8-H>%OkRDGH z^>!keb%WUNwA-;xN~y;Na81EcBkw~m+Hy+aUAFuBktI#~PAp50!8pCFWhfjJ<46g0xRacs>iuN>GKpFT(E{&$!yx`UJOOwa0v zWZESmAYk|vl_X|pu}Dsb;P!tSIBj8gKN&d9o^K;PDvh5xv4g zKB~Dvl3QE4QU7B7p1*i^d3lt_$aeqkJ^3M~JunAIQ_`5`;3R`W;ID zItj|U$R7_-9R|=Vd7HX_Bte0(k{d^THpoP47>Ixz2Rt7bh=|cOICw?5N^Z10`6Yy^ zHR2$ANOaK@k!xhEHl+~BkVBGoha(3GC52|f@j{@H!& zSWX;`g{&aCodTHzHE zUy-22jV<^rHR-jLlzixvQ+kVBbP*qy)szp NRf0zUrJ|T~*8eZD)SLhS diff --git a/ws_nodes_py/regulator.py b/ws_nodes_py/regulator.py index 12d65c4..d62ba8b 100644 --- a/ws_nodes_py/regulator.py +++ b/ws_nodes_py/regulator.py @@ -3,11 +3,13 @@ import rclpy import math from ws_nodes_py.default.GetParameterNode import GetParameterNode +from ws_nodes_py.settings.robot_settings import is_real from std_msgs.msg import Bool, Int16, String from geometry_msgs.msg import Twist, Point -from math import acos, degrees +from math import acos, degrees, sin, radians +# pip3 install simple_pid==2.0.1 +from simple_pid import PID # from nonius_msgs.msg import Track - import math @@ -21,6 +23,23 @@ class Controller(GetParameterNode): # self.k_p_linear = self.get_declare_parameter('k_p_linear', rclpy.Parameter.Type.DOUBLE, checker=lambda x: True) # self.linear_regulator_threshold = self.get_declare_parameter('linear_regulator_threshold', rclpy.Parameter.Type.DOUBLE, checker=lambda x: True) + if is_real: + self.IS_WITH_PID = True + ang_p = 0.015 + ang_d = 0 + self.IS_WITH_LIN_PID = True + lin_p = 0.07 + lin_d = 0 + else: + self.IS_WITH_PID = False + ang_p = 0 + ang_d = 0 + self.IS_WITH_LIN_PID = False + lin_p = 1000 + lin_d = 0 + self.pid_ang = PID(ang_p, 0, ang_d, setpoint=0) + self.pid_lin = PID(lin_p, 0, lin_d, setpoint=0) + self.subscribtion_for_parameter(Int16, "heading_topic", 'heading', checker=lambda x: x, func=lambda msg: msg.data, default=0) self.subscribtion_for_parameter(Point, "position_topic", 'position', checker=lambda x: x, func=lambda msg: msg, default=Point()) self.subscribtion_for_parameter(Point, "goal_topic", 'goal', checker=lambda x: x, func=lambda msg: msg, default=Point()) @@ -54,17 +73,29 @@ class Controller(GetParameterNode): def move_to_point(self): position, goal, heading, max_thrust, distance_threshold, k_p_angular = self.position, self.goal, self.heading, self.max_thrust, self.distance_threshold, self.k_p_angular error_dist = ((goal.x - position.x)**2 + (goal.y - position.y)**2)**0.5 - err_angle = self.angle_error(self.angle_point_error(position, goal), heading) + target_heading = self.angle_point_error(position, goal) + err_angle = self.angle_error(target_heading, heading) result = Twist() if self.type_move == "MOVE TO": - if abs(err_angle) > 3: - value = k_p_angular * err_angle - result.angular.z = math.copysign(abs(max_thrust), value) + value = 0 + if self.IS_WITH_PID: + # self.get_logger().info(f"{err_angle}=") + if abs(err_angle) > 3: + # self.pid_ang.setpoint = target_heading + value = -self.pid_ang(err_angle) else: - result.linear.x = max_thrust - # else: - # result.linear.x = 0.0 + if abs(err_angle) > 3: + # value = k_p_angular * err_angle + value = max_thrust * (1 if err_angle > 0 else -1) + result.angular.z = math.copysign(min(abs(max_thrust), abs(value)), value) + + if abs(sin(radians(err_angle))*error_dist) < 0.2: + if self.IS_WITH_LIN_PID: + value = -self.pid_lin(error_dist) + result.linear.x = math.copysign(min(abs(max_thrust), abs(value)), 1) + else: + result.linear.x = max_thrust elif self.type_move == "MOVE CIRCLE": point = [goal.x, goal.y] slf = [position.x, position.y] diff --git a/ws_nodes_py/settings/__pycache__/for_world.cpython-310.pyc b/ws_nodes_py/settings/__pycache__/for_world.cpython-310.pyc index 6ee0372d914911838c6b6ec5982347e610d8d1ba..c0a54f25f24e4367720ee0b096669187a8bffd37 100644 GIT binary patch delta 27 hcmaFB@qmLjpO=@50SMNF+(~1e$oqhiW8=FbW&mq*2vGn4 delta 27 hcmaFB@qmLjpO=@50SGGkf2XlbTz;6yR7Qmm1UvMiLtB?LKOHg>?uxPq(L1*?t9 z2(t7^eNJB*Ij8_?12;z|D(Z+9UdE^JX|OY+60)?WPD}VKK8Mfa3wQ-z#H%9<+4#}} z!@E3&zKt<_nlZK&)aQ;Jg*?EsQANm*%b?GqM5dJ;631*9&`^@p>BIvv+1h&BVS}{V ziNo%8(2aOEh7eA0mRg={g(MsmB@e~sc&}&}^x0FjUi;KrfAV(!+8aMpKKWD&>tB7k zq1Shx?)LALKR*BIdu8sb9t+U;I0uNXC|pJvT>``k?*a6dUhjO*(aF396?h#G zU4>}Aq}+x=Q=z4>42ZrR9UXZl&%r*c@xiCy!20Y{=m(nmF>UhKw8`JTSDL%$f7Lm3 zW^grduIA6x{JHv9@hZTxkgH}5GZc}GSb|(V>0`Hf{06g-+T;n2W2)KYbpc)WzUQRw zkntD?y^wDcF4EfcnX`Kuj|pdi=o5HA5z!=!QC&p6pvNTR6FsdnRbWnN&4e(a)2M_D z7o%pvENXS3oj|gF)D=NL4P5(C4&zz{V?7-ND3QZa8PMv+~@GzSy-k~J*jI&P3E)FSjAXEko( z7FKaPS0=)pGm-7iX(ch&3|=#f9LR%1Q@R|qJ2$OP-*yIFlh`h{?uPx_u?fevJ-VLH zLLqw3PaLnuy@roBHZ+N=ZVGj`uZFtHxvHC}i{>d^wOrD-F2oRoAphx8mjPg*lNJ9Px# delta 133 zcmX@jbdAX=pO=@50SNX@{hb!c#K7OBJh;o{63jgffIsW)RAB;;i{10zea37+9E?n3)(sknN8EI{+!r B7SI3y diff --git a/ws_nodes_py/settings/for_world.py b/ws_nodes_py/settings/for_world.py index 33517b8..1b2ff7f 100644 --- a/ws_nodes_py/settings/for_world.py +++ b/ws_nodes_py/settings/for_world.py @@ -1,7 +1,7 @@ world_markers = { "start_point": [0, 0], "yellow_gate": [6, 0], - "blue_gate": [11, 0], + "blue_gate": [8, 0], "blue_bou": [10, 0], "black_circle": [10, 10], "red_point": [9, 3], #???? diff --git a/ws_nodes_py/settings/robot_settings.py b/ws_nodes_py/settings/robot_settings.py index c39bad8..22cc66e 100644 --- a/ws_nodes_py/settings/robot_settings.py +++ b/ws_nodes_py/settings/robot_settings.py @@ -1,12 +1,18 @@ -is_real = True +is_real = False NS = 'ws11' +# v4l2-ctl --list-devices cameras_ports = { - "2.4.2": "camera_front", # первая камера от провода юсб хаба, воткнутого в красный юсб jetson'a - "2.4.1": "camera_side", # вторая камера от провода юсб хаба, воткнутого в красный юсб jetson'a - "2.4.6": "camera_down", # R5 - "2.4.5": "camera_down", # третья камера от провода юсб хаба, воткнутого в красный юсб jetson'a + # "2.4.2": "camera_front", # первая камера от провода юсб хаба, воткнутого в красный юсб jetson'a + # "2.4.1": "camera_side", # вторая камера от провода юсб хаба, воткнутого в красный юсб jetson'a + # "2.4.6": "camera_down", # R5 + # "2.4.5": "camera_down", # третья камера от провода юсб хаба, воткнутого в красный юсб jetson'a + + "2.3.1": "camera_front", # в хабе + "2.3.2": "camera_side", # в хабе + "2.3.3": "camera_down", # в хабе + } camcv = {'F':'camera_front', 'S':'camera_side', 'D':'camera_down'} \ No newline at end of file diff --git a/ws_nodes_py/state_machine.py b/ws_nodes_py/state_machine.py index d542231..8c328ee 100644 --- a/ws_nodes_py/state_machine.py +++ b/ws_nodes_py/state_machine.py @@ -49,6 +49,8 @@ class Controller(Node): self.create_subscription(Point, self.get_declare_parameter('position_topic', lambda x: x, "no pos is described"), self.pos_callback, 10) self.create_subscription(Int16, self.get_declare_parameter('heading_topic', lambda x: x, "no heading is described"), self.heading_callback, 10) + self.is_grabbed = True + self.create_subscription(Int16, "modbus/probe_drop_sensor", self.probe_sensor_callback, 10) self.create_subscription(Bool, 'start', self.handle_start, 1) # self.create_timer(1 / self.get_declare_parameter('hz', lambda x: x > 0, "no hz is described"), self.update) @@ -109,6 +111,10 @@ class Controller(Node): for key, value in data.items(): setattr(msg, key, value) topic.publish(msg) + + def probe_sensor_callback(self, msg): + self.is_grabbed = bool(msg.data) + class Start(smach.State): @@ -254,10 +260,10 @@ class DropPickStakan(smach.State): def execute(self, userdata): self.cntr.next_step("STOP", [0, 0]) - self.cntr.get_logger().info(f'Stakan {"drop" if msg.data == 1 else "pick"}') + self.cntr.get_logger().info(f'Stakan {"drop" if self.state == 1 else "pick"}') while not self.cntr.shutdown(): rclpy.spin_once(self.cntr) - if (self.cntr.get_clock().now() - self.cntr.state_change_time) > Duration(seconds=self.time): + if (self.cntr.get_clock().now() - self.cntr.state_change_time) > Duration(seconds=self.time) or (self.state == 0 and self.cntr.is_grabbed): self.cntr.get_logger().info('finish') return 'timeout' else: @@ -274,7 +280,7 @@ class DropBox(smach.State): def execute(self, userdata): self.cntr.next_step("STOP", [0, 0]) - self.cntr.get_logger().info(f'drop box {"open" if msg.data == 360 else "close"}') + self.cntr.get_logger().info(f'drop box {"open" if self.state == 360 else "close"}') while not self.cntr.shutdown(): rclpy.spin_once(self.cntr) if (self.cntr.get_clock().now() - self.cntr.state_change_time) > Duration(seconds=self.time): @@ -294,33 +300,33 @@ def main(): global sis square_mission = { # point_name: [x, y] in meters - "yellow": { + "point_1": { "type": "moveTo", - "pos": [2, 0], + "pos": [0, 2], "is_real_compass": False, - "dist_threshold": 0.1, + "dist_threshold": 0.3, # "navigation_marker": "yellow_gate", "navigation_marker": "", }, - "left_blue": { + "point_2": { "type": "moveTo", "pos": [2, 2], "is_real_compass": False, - "dist_threshold": 0.1, + "dist_threshold": 0.3, "navigation_marker": "", }, "point_3": { "type": "moveTo", - "pos": [0, 2], + "pos": [2, 0], "is_real_compass": False, - "dist_threshold": 0.1, + "dist_threshold": 0.3, "navigation_marker": "", }, "point_4": { "type": "moveTo", "pos": [0, 0], "is_real_compass": False, - "dist_threshold": 0.1, + "dist_threshold": 0.3, "navigation_marker": "", }, } @@ -339,24 +345,33 @@ def main(): "type": "moveTo", "pos": [9, 3], "is_real_compass": False, - "dist_threshold": 0.1, + "dist_threshold": 0.14, "navigation_marker": "red_point", }, - 'drop-stakan':{ + 'drop-stakan-1':{ 'type':'drop-stakan', - 'time': 15, + 'time': 5, 'state': 1, }, - 'drop-stakan':{ + 'drop-stakan-2':{ 'type':'drop-stakan', - 'time': 15, + 'time': 40, 'state': 0, }, - 'drop-box':{ - 'type':'drop-box', - 'time': 15, - 'state': 360, + 'sleep-1':{ + 'type':'wait', + 'time': 10, }, + # 'drop-box-1':{ + # 'type':'drop-box', + # 'time': 15, + # 'state': 360, + # }, + # 'drop-box-2':{ + # 'type':'drop-box', + # 'time': 5, + # 'state': 0, + # }, } vlad24_mission = { @@ -370,14 +385,14 @@ def main(): }, "point-left-blue": { "type": "moveTo", - "pos": [10.5, -3], + "pos": [8, -3], "dist_threshold": 0.25, "is_real_compass": False, "navigation_marker": "", }, "point-center-blue": { "type": "moveTo", - "pos": [11, 0], + "pos": [8, 0], "dist_threshold": 0.25, "is_real_compass": False, "navigation_marker": "blue_gate", @@ -385,7 +400,7 @@ def main(): }, "point-2-blue": { "type": "moveTo", - "pos": [11, 1], + "pos": [8, 1], "dist_threshold": 0.25, "is_real_compass": False, "navigation_marker": "", @@ -435,6 +450,9 @@ def main(): elif mission[key]["type"] == "drop-box": smach.StateMachine.add(f'GOAL-{i}', DropBox(c, mission[key]["time"], mission[key]["state"]), transitions={'timeout': f'GOAL-{ "FINISH" if len(mission.keys()) == i else i+1}'}) + elif mission[key]["type"] == "wait": + smach.StateMachine.add(f'GOAL-{i}', Wait(c, mission[key]["time"]), + transitions={'timeout': f'GOAL-{ "FINISH" if len(mission.keys()) == i else i+1}'}) #for i, key in enumerate(mission.keys(), 1): # smach.StateMachine.add(f'GOAL-{i}', MoveToGoal(c, 1000, mission[key]), diff --git a/ws_nodes_py/videoD.py b/ws_nodes_py/videoD.py index df5a7e8..314d0fb 100644 --- a/ws_nodes_py/videoD.py +++ b/ws_nodes_py/videoD.py @@ -1,131 +1,23 @@ #! /usr/bin/env python3 import rclpy -from ws_nodes_py.default.GetParameterNode import GetParameterNode -from ws_nodes_py.default.ObjectsDetection import ObjectsDetection -from sensor_msgs.msg import CompressedImage -from std_msgs.msg import Int16 -from geometry_msgs.msg import Point -import cv2 -from cv_bridge import CvBridge -import numpy as np -from math import radians, degrees, cos, sin, tan, acos +from ws_nodes_py.CVCamera import CVCamera +from math import radians, tan -class RealsenseToBinary(GetParameterNode, ObjectsDetection): +class RosOpencvToBinary(CVCamera): def __init__(self): - super().__init__('camcvD') - - self.set_pool_masks() - # Параметры для выделения маски BGR - self.min_countur_size = 300 - self.camera_angle = 0 + super().__init__('camcvD', -90 + 20) + self.pool_depth = 1.4 # in meters + self.meter_per_pix = (tan(radians(self.fov/2)) * self.pool_depth) / (self.width / 2) - # Маска части изображения, которе обрабатываем - - # Параметры для рассчёта расстояние и угла - # см. ~/Desktop/python/calc.py - self.pool_depth = 4 # in meters - self.drop_threshold = 0.5 # in meters - self.fov = self.get_declare_parameter('fov', rclpy.Parameter.Type.INTEGER, checker=lambda x: True) - self.meter_per_pix = (tan(radians(self.fov/2)) * self.pool_depth) / (640 / 2) - # Пареметр апроксимации контура - self.k = self.get_declare_parameter('k', rclpy.Parameter.Type.DOUBLE, checker=lambda x: True) - - topic_name = self.get_declare_parameter('topic_camera', rclpy.Parameter.Type.STRING, checker=lambda x: x, error_text="topic name must be string") - # Топик с отладочным изображением - self.pub = self.create_publisher(CompressedImage, f"cv/{topic_name}" , 10) - self.subscribtion_for_parameter(Int16, "heading_topic", 'heading', checker=lambda x: x, func=lambda msg: msg.data, default=0) - self.subscribtion(CompressedImage, "topic_camera", self.img_callback, checker=lambda x: x) - - self.pub_red_pos = self.publisher_from_declare(Point, "red_position_topic", checker=lambda x: x) - - self.br = CvBridge() - self.get_logger().info(f"CV Start {topic_name} {self.fov/2} {tan(radians(self.fov/2)), self.pool_depth}") - - def publish_coordinte(self, res, publisher): - if res is not None: - course, dist = res - x = round(dist * cos(radians(self.heading + course + self.camera_angle)), 1) - y = round(dist * sin(radians(self.heading + course + self.camera_angle)), 1) - msg = Point() - msg.x = x - msg.y = y - publisher.publish(msg) - - def img_callback(self, data): - # Получение кадра - frame = self.br.compressed_imgmsg_to_cv2(data) - height, width = frame.shape[0:2] - - binary = self.get_binary("red_position", frame) - self.publish_coordinte(self.get_red_circle_pos(binary, height, width, frame), self.pub_red_pos) - - # Создание отладочного изображения - miniBin = cv2.resize(binary, (int(binary.shape[1] / 4), int(binary.shape[0] / 4)), - interpolation=cv2.INTER_AREA) - miniBin = cv2.cvtColor(miniBin, cv2.COLOR_GRAY2RGB) - frame[-2 - miniBin.shape[0]:-2, 2:2 + miniBin.shape[1]] = miniBin - - # Отладочное изображение - self.pub.publish(self.br.cv2_to_compressed_imgmsg(frame)) - - def get_red_circle_pos(self, binary, height, width, frame): - # Выделение самых больших контуров удовлетворяющих условию размера - contours, _ = cv2.findContours(binary, cv2.RETR_EXTERNAL, - cv2.CHAIN_APPROX_NONE) - contours = list(filter(lambda x: cv2.moments(x)["m00"] > self.min_countur_size, sorted(contours, key=cv2.contourArea, reverse=True)[:2])) - approxs = list(sorted(contours, key=lambda x: len(cv2.approxPolyDP( - x, self.k * cv2.arcLength(x, True), True)), reverse=True))[:1] - - dist, course = None, None - if approxs: - contour = approxs[0] - contour = cv2.approxPolyDP(contour, self.k * cv2.arcLength(contour, True), True) - moments = cv2.moments(contour) - # Координаты центра контура - if moments["m00"]: - cx = int(moments["m10"] / moments["m00"]) - cy = int(moments["m01"] / moments["m00"]) - else: - return None - - # Вычисление положения - lx = (-cy + height / 2) * self.meter_per_pix - ly = (cx - width / 2) * self.meter_per_pix - dist = (lx**2 + ly**2)**0.5 - if dist != 0: - course = degrees(acos(lx/dist)) - if ly < 0: - course = 360 - course - else: - course = 0 - - if True: # Вывод информации о контуре - # Координаты центра - # cv2.putText(frame, f'cx: {cx}, cy: {cy}', (cx + 10, cy - 10), cv2.FONT_HERSHEY_SIMPLEX, 0.75, (255, 255, 255), 2, cv2.LINE_AA) - # Выделение контура - cv2.drawContours(frame, [contour], -1, (0, 255, 0), 1) - # Выделение прямоугольника - # cv2.rectangle(frame, (x, y), (x+w, y+h), (0, 255, 0), 2) - # Оси через центр контура - # cv2.line(frame, (cx, 0), (cx, height), (0, 255, 0), 1) - # cv2.line(frame, (0, cy), (width, cy), (0, 255, 0), 1) - # Вывести ширину контура - # cv2.putText(frame, f'W{round(w,2)}', (x, y+h+25), cv2.FONT_HERSHEY_SIMPLEX, 0.75, (0, 0, 255), 2) - # Вывести расчётную дистанцию до контура - # cv2.putText(frame, f'D{round(dist,2)}', (x, y), cv2.FONT_HERSHEY_SIMPLEX, 0.75, (0, 0, 255), 2) - - # Расчёт положения относительно круга - if dist is not None: - return course, dist - return None - + def image_processing(self, frame): + self.publish_coordinte(self.get_points, "red_position", frame, True) def main(args=None): rclpy.init(args=args) - RealOpenCV = RealsenseToBinary() + RealOpenCV = RosOpencvToBinary() rclpy.spin(RealOpenCV) RealOpenCV.destroy_node() rclpy.shutdown() diff --git a/ws_nodes_py/videoF.py b/ws_nodes_py/videoF.py index e7cb845..3b12d80 100644 --- a/ws_nodes_py/videoF.py +++ b/ws_nodes_py/videoF.py @@ -1,158 +1,21 @@ #! /usr/bin/env python3 import rclpy -from ws_nodes_py.default.GetParameterNode import GetParameterNode -from ws_nodes_py.default.ObjectsDetection import ObjectsDetection -from sensor_msgs.msg import CompressedImage -from std_msgs.msg import Int16 -from geometry_msgs.msg import Point -import cv2 -from cv_bridge import CvBridge -import numpy as np -from math import radians, cos, sin +from ws_nodes_py.CVCamera import CVCamera +import cv2 -class RosOpencvToBinary(GetParameterNode, ObjectsDetection): +class RosOpencvToBinary(CVCamera): def __init__(self): - super().__init__('camcvF') - - self.set_pool_masks() - self.min_countur_size = 600 - - # Параметры для рассчёта расстояние и угла - self.subscribtion_for_parameter(Int16, "heading_topic", 'heading', checker=lambda x: x, func=lambda msg: msg.data, default=0) - # см. ~/Desktop/python/calc.py + super().__init__('camcvF', 0) self.k1 = self.get_declare_parameter('k1', rclpy.Parameter.Type.DOUBLE, checker=lambda x: True) self.k2 = self.get_declare_parameter('k2', rclpy.Parameter.Type.DOUBLE, checker=lambda x: True) - self.fov = self.get_declare_parameter('fov', rclpy.Parameter.Type.INTEGER, checker=lambda x: True) - # Пареметр апроксимации контура - self.k = self.get_declare_parameter('k', rclpy.Parameter.Type.DOUBLE, checker=lambda x: True) - - topic_name = self.get_declare_parameter('topic_camera', rclpy.Parameter.Type.STRING, checker=lambda x: x, error_text="topic name must be string") - # Топик с отладочным изображением - self.pub = self.create_publisher(CompressedImage, f"cv/{topic_name}" , 10) - self.subscribtion(CompressedImage, "topic_camera", self.img_callback, checker=lambda x: x) - - self.pub_yellow_pos = self.publisher_from_declare(Point, "gate_position_topic", checker=lambda x: x) - self.pub_blue_pos = self.publisher_from_declare(Point, "blue_gate_position_topic", checker=lambda x: x) - - self.br = CvBridge() - self.get_logger().info(f"CV Start {topic_name}") - - def publish_coordinte(self, res, publisher): - if res is not None: - course_to_gate, dist_to_gate = res - x = round(dist_to_gate * cos(radians(self.heading - course_to_gate)), 1) - y = round(dist_to_gate * sin(radians(self.heading - course_to_gate)), 1) - # Управляющий сигнал - if 6 >= dist_to_gate >= 1.5: - msg = Point() - msg.x = x - msg.y = y - publisher.publish(msg) - - - def img_callback(self, data): - # Получение кадра - frame = self.br.compressed_imgmsg_to_cv2(data) - - height, width = frame.shape[0:2] - yellow_binary = self.get_binary("yellow_gate", frame) - # blue_binary = self._get_binary(frame, ((0, 83, 0), (19, 255, 255)), cv2.COLOR_RGB2HSV, (0, 100, 640, 250), False) - blue_binary = self.get_binary("blue_gate", frame) - self.publish_coordinte(self.get_gate(yellow_binary, height, width, frame), self.pub_yellow_pos) - self.publish_coordinte(self.get_gate(blue_binary, height, width, frame), self.pub_blue_pos) - - # Создание отладочного изображения - miniBin = cv2.resize(blue_binary, (int(blue_binary.shape[1] / 4), int(blue_binary.shape[0] / 4)), - interpolation=cv2.INTER_AREA) - miniBin = cv2.cvtColor(miniBin, cv2.COLOR_GRAY2RGB) - frame[-2 - miniBin.shape[0]:-2, 2:2 + miniBin.shape[1]] = miniBin - - miniBin = cv2.resize(yellow_binary, (int(yellow_binary.shape[1] / 4), int(yellow_binary.shape[0] / 4)), - interpolation=cv2.INTER_AREA) - miniBin = cv2.cvtColor(miniBin, cv2.COLOR_GRAY2RGB) - frame[-2 - miniBin.shape[0]:-2, 2 + miniBin.shape[1]:2 + miniBin.shape[1] * 2] = miniBin - - # cv2.putText(frame, 'iSee: {};'.format(len(contours)), (width - 120, height - 5), - # cv2.FONT_HERSHEY_SIMPLEX, 0.75, (255, 255, 255), 1, cv2.LINE_AA) - # cv2.putText(frame, f'x: {x} y: {y}', (0, 25), - # cv2.FONT_HERSHEY_SIMPLEX, 0.75, (255, 255, 255), 1, cv2.LINE_AA) - # cv2.putText(frame, f'course: {self.heading - course_to_gate} dist {dist_to_gate}', (0, 50), - # cv2.FONT_HERSHEY_SIMPLEX, 0.75, (255, 255, 255), 1, cv2.LINE_AA) - # cv2.putText(frame, f'{controlX = }', (0, 75), - # cv2.FONT_HERSHEY_SIMPLEX, 0.75, (255, 255, 255), 1, cv2.LINE_AA) - - # Отладочное изображение - self.pub.publish(self.br.cv2_to_compressed_imgmsg(frame)) - - def get_gate(self, binary, height, width, frame): - pix_per_angle = width / self.fov - # Нулевые параметры управления - med_x = width / 2 - controlX = 0.0 - dist_to_gate = 0 - course_to_gate = 0 - - - contours, _ = cv2.findContours(binary, cv2.RETR_EXTERNAL, - cv2.CHAIN_APPROX_NONE) - contours = list(filter(lambda x: cv2.moments(x)["m00"] > self.min_countur_size, sorted(contours, key=cv2.contourArea, reverse=True)[:3])) - approxs = list(sorted(contours, key=lambda x: len(cv2.approxPolyDP( - x, 0.01 * cv2.arcLength(x, True), True)), reverse=True))[:2] - - # Для выделение левого и правого шара - aligns = [] - # Для выделение ближайшего и дальнего шара - distances = [] - for contour in approxs: - # Точки контура не приближаются к границе ближе чем на 10 пикселей - if any((not 10 < xy[0][0] < width - 10 or not 10 < xy[0][1] < height - 10) for xy in contour): - continue - - moments = cv2.moments(contour) - # Координаты центра контура - if moments["m00"]: - cx = int(moments["m10"] / moments["m00"]) - else: - continue - - # Вычисление дистанции - x, y, w, h = cv2.boundingRect(contour) - dist = (w*(self.k1)+self.k2)*0.01 - # Проверка границ интерполяции - if not 5 >= dist >= 1.5: - continue - - # Запись центра контура по горизонту и расчётной дистанции до него - aligns.append(cx) - distances.append(dist) - if True: # Вывод информации о контуре - # Координаты центра - # cv2.putText(frame, f'cx: {cx}, cy: {cy}', (cx + 10, cy - 10), cv2.FONT_HERSHEY_SIMPLEX, 0.75, (255, 255, 255), 2, cv2.LINE_AA) - # Выделение контура - cv2.drawContours(frame, contour, -1, (0, 255, 0), 1) - # Выделение прямоугольника - # cv2.rectangle(frame, (x, y), (x+w, y+h), (0, 255, 0), 2) - # Оси через центр контура - # cv2.line(frame, (cx, 0), (cx, height), (0, 255, 0), 1) - # cv2.line(frame, (0, cy), (width, cy), (0, 255, 0), 1) - # Вывести ширину контура - cv2.putText(frame, f'W{round(w,2)}', (x, y+h+25), cv2.FONT_HERSHEY_SIMPLEX, 0.75, (0, 0, 255), 2) - # Вывести расчётную дистанцию до контура - cv2.putText(frame, f'D{round(dist,2)}', (x, y), cv2.FONT_HERSHEY_SIMPLEX, 0.75, (0, 0, 255), 2) - - # Расчёт курса и дистанции до ворот - if len(aligns) == 2: - med_x = sum(aligns) / len(aligns) - controlX = round(-2 * (med_x - width / 2) / width, 1) - controlX = max(-0.5, min(0.5,controlX)) - course_to_gate = round(controlX * self.fov / 2) - dist_to_gate = round(sum(distances) / len(distances), 1) - return course_to_gate, dist_to_gate - else: - return None + def image_processing(self, frame): + self.publish_coordinte(self.get_poster, "red_poster", frame, True) + self.publish_coordinte(self.get_poster, "green_poster", frame, True) + self.publish_coordinte(self.get_gates, "yellow_gate", frame, True) + self.publish_coordinte(self.get_gates, "blue_gate", frame, True) def main(args=None): @@ -164,4 +27,4 @@ def main(args=None): if __name__ == '__main__': - main() + main() diff --git a/ws_nodes_py/videoS.py b/ws_nodes_py/videoS.py index 663b0d4..d42894a 100644 --- a/ws_nodes_py/videoS.py +++ b/ws_nodes_py/videoS.py @@ -1,159 +1,22 @@ #! /usr/bin/env python3 import rclpy -from ws_nodes_py.default.GetParameterNode import GetParameterNode -from ws_nodes_py.default.ObjectsDetection import ObjectsDetection -from sensor_msgs.msg import CompressedImage -from std_msgs.msg import Float64, Int16 -from geometry_msgs.msg import Point -import cv2 -from cv_bridge import CvBridge -import numpy as np -from math import radians, cos, sin +from ws_nodes_py.CVCamera import CVCamera +import cv2 -class RosOpencvToBinary(GetParameterNode, ObjectsDetection): +class RosOpencvToBinary(CVCamera): def __init__(self): - super().__init__('camcvS') + super().__init__('camcvS', 90) - self.set_pool_masks() - self.min_countur_size = 300 - - self.camera_angle = 90 - # Параметры для рассчёта расстояние и угла - self.subscribtion_for_parameter(Int16, "heading_topic", 'heading', checker=lambda x: x, func=lambda msg: msg.data, default=0) # см. ~/Desktop/python/calc.py self.k1 = self.get_declare_parameter('k1', rclpy.Parameter.Type.DOUBLE, checker=lambda x: True) self.k2 = self.get_declare_parameter('k2', rclpy.Parameter.Type.DOUBLE, checker=lambda x: True) - self.fov = self.get_declare_parameter('fov', rclpy.Parameter.Type.INTEGER, checker=lambda x: True) - # Пареметр апроксимации контура - self.k = self.get_declare_parameter('k', rclpy.Parameter.Type.DOUBLE, checker=lambda x: True) - - topic_name = self.get_declare_parameter('topic_camera', rclpy.Parameter.Type.STRING, checker=lambda x: x, error_text="topic name must be string") - # Топик с отладочным изображением - self.pub = self.create_publisher(CompressedImage, f"cv/{topic_name}/compressed" , 10) - self.subscribtion(CompressedImage, "topic_camera", self.img_callback, checker=lambda x: x) - - self.pub_blue_pos = self.publisher_from_declare(Point, "blue_position_topic", checker=lambda x: x) - self.pub_black_pos = self.publisher_from_declare(Point, "black_position_topic", checker=lambda x: x) - - # Топик с отладочным изображением - self.pub = self.create_publisher(CompressedImage, f"cv/{topic_name}" , 10) - - self.br = CvBridge() - self.get_logger().info(f"CV Start {topic_name}") - - def publish_coordinte(self, res, publisher): - if res is not None: - course_to_gate, dist_to_gate = res - x = round(dist_to_gate * cos(radians(self.heading - course_to_gate + self.camera_angle)), 1) - y = round(dist_to_gate * sin(radians(self.heading - course_to_gate + self.camera_angle)), 1) - # Управляющий сигнал - if 6 >= dist_to_gate >= 1.5: - msg = Point() - msg.x = x - msg.y = y - publisher.publish(msg) - - - def img_callback(self, data): - frame = self.br.compressed_imgmsg_to_cv2(data) - height, width = frame.shape[0:2] - - black_binary = self.get_binary("black_position", frame) - blue_binary = self.get_binary("blue_bou", frame) - - self.publish_coordinte(self.get_gate(black_binary, height, width, frame), self.pub_black_pos) - self.publish_coordinte(self.get_gate(blue_binary, height, width, frame), self.pub_blue_pos) - - # Создание отладочного изображения - miniBin = cv2.resize(black_binary, (int(black_binary.shape[1] / 4), int(black_binary.shape[0] / 4)), - interpolation=cv2.INTER_AREA) - miniBin = cv2.cvtColor(miniBin, cv2.COLOR_GRAY2RGB) - frame[-2 - miniBin.shape[0]:-2, 2:2 + miniBin.shape[1]] = miniBin - - miniBin = cv2.resize(blue_binary, (int(blue_binary.shape[1] / 4), int(blue_binary.shape[0] / 4)), - interpolation=cv2.INTER_AREA) - miniBin = cv2.cvtColor(miniBin, cv2.COLOR_GRAY2RGB) - frame[-2 - miniBin.shape[0]:-2, 2 + miniBin.shape[1]:2 + miniBin.shape[1] * 2] = miniBin - - # cv2.putText(frame, 'iSee: {};'.format(len(contours)), (width - 120, height - 5), - # cv2.FONT_HERSHEY_SIMPLEX, 0.75, (255, 255, 255), 1, cv2.LINE_AA) - # cv2.putText(frame, f'x: {x} y: {y}', (0, 25), - # cv2.FONT_HERSHEY_SIMPLEX, 0.75, (255, 255, 255), 1, cv2.LINE_AA) - # cv2.putText(frame, f'course: {self.heading - course_to_gate} dist {dist_to_gate}', (0, 50), - # cv2.FONT_HERSHEY_SIMPLEX, 0.75, (255, 255, 255), 1, cv2.LINE_AA) - # cv2.putText(frame, f'{controlX = }', (0, 75), - # cv2.FONT_HERSHEY_SIMPLEX, 0.75, (255, 255, 255), 1, cv2.LINE_AA) - - - def get_gate(self, binary, height, width, frame): - pix_per_angle = width / self.fov - # Нулевые параметры управления - med_x = width / 2 - controlX = 0.0 - dist_to_gate = 0 - course_to_gate = 0 - - - contours, _ = cv2.findContours(binary, cv2.RETR_EXTERNAL, - cv2.CHAIN_APPROX_NONE) - contours = list(filter(lambda x: cv2.moments(x)["m00"] > self.min_countur_size, sorted(contours, key=cv2.contourArea, reverse=True)[:1])) - approxs = list(sorted(contours, key=lambda x: len(cv2.approxPolyDP( - x, 0.01 * cv2.arcLength(x, True), True)), reverse=True))[:1] - - # Для выделение левого и правого шара - aligns = [] - # Для выделение ближайшего и дальнего шара - distances = [] - for contour in approxs: - # Точки контура не приближаются к границе ближе чем на 10 пикселей - if any((not 10 < xy[0][0] < width - 10 or not 10 < xy[0][1] < height - 10) for xy in contour): - continue - - moments = cv2.moments(contour) - # Координаты центра контура - if moments["m00"]: - cx = int(moments["m10"] / moments["m00"]) - else: - continue - - # Вычисление дистанции - x, y, w, h = cv2.boundingRect(contour) - dist = (w*(self.k1)+self.k2)*0.01 - # Проверка границ интерполяции - if not 5 >= dist >= 1.5: - continue - # Запись центра контура по горизонту и расчётной дистанции до него - aligns.append(cx) - distances.append(dist) + def image_processing(self, frame): + self.publish_coordinte(self.get_island, "black_position", frame, True) + self.publish_coordinte(self.get_bous, "blue_bou", frame, True) - if True: # Вывод информации о контуре - # Координаты центра - # cv2.putText(frame, f'cx: {cx}, cy: {cy}', (cx + 10, cy - 10), cv2.FONT_HERSHEY_SIMPLEX, 0.75, (255, 255, 255), 2, cv2.LINE_AA) - # Выделение контура - cv2.drawContours(frame, contour, -1, (0, 255, 0), 1) - # Выделение прямоугольника - # cv2.rectangle(frame, (x, y), (x+w, y+h), (0, 255, 0), 2) - # Оси через центр контура - # cv2.line(frame, (cx, 0), (cx, height), (0, 255, 0), 1) - # cv2.line(frame, (0, cy), (width, cy), (0, 255, 0), 1) - # Вывести ширину контура - cv2.putText(frame, f'W{round(w,2)}', (x, y+h+25), cv2.FONT_HERSHEY_SIMPLEX, 0.75, (0, 0, 255), 2) - # Вывести расчётную дистанцию до контура - cv2.putText(frame, f'D{round(dist,2)}', (x, y), cv2.FONT_HERSHEY_SIMPLEX, 0.75, (0, 0, 255), 2) - - # Расчёт курса и дистанции до ворот - if len(aligns) == 1: - med_x = sum(aligns) / len(aligns) - controlX = round(-2 * (med_x - width / 2) / width, 1) - controlX = max(-0.5, min(0.5,controlX)) - course_to_gate = round(controlX * self.fov / 2) - dist_to_gate = round(sum(distances) / len(distances), 1) - return course_to_gate, dist_to_gate - else: - return None def main(args=None): rclpy.init(args=args) diff --git a/ws_nodes_py/world.py b/ws_nodes_py/world.py index e5342f2..3759154 100644 --- a/ws_nodes_py/world.py +++ b/ws_nodes_py/world.py @@ -4,9 +4,9 @@ import rclpy from ws_nodes_py.default.GetParameterNode import GetParameterNode from ws_nodes_py.settings.for_world import * from time import time -from std_msgs.msg import Float64, String, Int16 -from geometry_msgs.msg import Twist, Point # Point.x, Point.y, Point.z -from math import ceil, floor, radians, cos, sin +from std_msgs.msg import String, Int16 +from geometry_msgs.msg import Twist, Point, PointStamped # Point.x, Point.y, Point.z +from math import radians, cos, sin class World(GetParameterNode): def update(self): @@ -17,26 +17,30 @@ class World(GetParameterNode): delta_time = 0 self.last_time = time() - # Вычисление скорости катамарана - med_signal = round(1_500 + abs(self.throtle) * 500) - low, high = 1500, 2000 - for key in self.motor_speed.keys(): - if low <= key <= med_signal: - low = key - if high >= key >= med_signal: - high = key - break - # Линейная интерполяция - speed = round(self.motor_speed[low] + ((med_signal - low) / (high - low) * (self.motor_speed[high] - self.motor_speed[low]) if high != low else 0), 2) - if self.throtle < 0: - speed = -speed + if self.is_update_pos: # Экстраполируем положени катамарана + # Вычисление скорости катамарана + med_signal = round(1_500 + abs(self.throtle) * 500) + low, high = 1500, 2000 + for key in self.motor_speed.keys(): + if low <= key <= med_signal: + low = key + if high >= key >= med_signal: + high = key + break + + # Линейная интерполяция + speed = round(self.motor_speed[low] + ((med_signal - low) / (high - low) * (self.motor_speed[high] - self.motor_speed[low]) if high != low else 0), 2) + + if self.throtle < 0: + speed = -speed + + # Расчёт смещения + dx = speed * cos(radians(self.heading)) * delta_time + dy = speed * sin(radians(self.heading)) * delta_time + # Обновление позиции + self.position[0] += dx + self.position[1] += dy - # Расчёт смещения - dx = speed * cos(radians(self.heading)) * delta_time - dy = speed * sin(radians(self.heading)) * delta_time - # Обновление позиции - self.position[0] += dx - self.position[1] += dy msg = Point() msg.x = float(self.position[0]) msg.y = float(self.position[1]) @@ -48,14 +52,14 @@ class World(GetParameterNode): self.position = start_position[:] self.world_markers = world_markers + self.is_update_pos = True self.subscribtion_for_parameter(Twist, "twist_topic", 'throtle', checker=lambda x: x, func=lambda msg: msg.linear.x, default=0) self.subscribtion_for_parameter(Int16, "heading_topic", 'heading', checker=lambda x: x, func=lambda msg: msg.data, default=0) self.subscribtion_for_parameter(Point, "goal_topic", 'goal_x', checker=lambda x: x, func=lambda msg: msg.x, default=0) self.subscribtion_for_parameter(String, "navigation_marker_topic", 'navigation_marker', checker=lambda x: x, func=lambda msg: msg.data, default="") self.subscribtion(String, "reset_topic", self.reset_callback, checker=lambda x: x) - for key, value in world_markers_topics_parametrs.items(): - self.subscribtion(Point, f"{key}_topic", self.get_marker_callback(value), checker=lambda x: x) + self.subscribtion(PointStamped, "object_position", self.update_position, checker=lambda x: x) # Имеется в виду новая насадка на мотор или нет (новая - от Кирилла (обтекаемая) или старая - от трионикса (белая)) if self.get_declare_parameter('is_new_motor', rclpy.Parameter.Type.BOOL, lambda x: True, "is_new_motor can be 0 or 1"): @@ -74,18 +78,22 @@ class World(GetParameterNode): def reset_callback(self, msg): if msg.data == "reset_map": - self.get_logger().info(f"reset map") self.position = start_position[:] + self.get_logger().info(f"reset map {self.position}") + elif msg.data == "fix_pose": + self.position = [0, 0] + self.get_logger().info(f"reset map {self.position}") + self.is_update_pos = False + elif msg.data == "update_pose": + self.position = [0, 0] + self.is_update_pos = True - def get_marker_callback(self, name): - def callback(msg): - self.update_position(msg, name) - return callback - - def update_position(self, msg, name): - if name == self.navigation_marker: - obj_pos = self.world_markers[name] - self.position = [obj_pos[0] - msg.x, obj_pos[1] - msg.y] + def update_position(self, msg): + if self.navigation_marker in msg.header.frame_id: + obj_pos = self.world_markers[self.navigation_marker] + x = obj_pos[0] - msg.point.x * cos(radians(msg.point.z + self.heading)) + y = obj_pos[1] - msg.point.x * sin(radians(msg.point.z + self.heading)) + self.position = [x, y] def main(): -- GitLab From e9dc970c3f7d2b2b7475d4fd26926628fe9fc147 Mon Sep 17 00:00:00 2001 From: "toropov.nik" Date: Fri, 4 Oct 2024 17:30:59 +0300 Subject: [PATCH 11/23] hot fix update for test --- ws_nodes_py/CVCamera.py | 11 +-- .../__pycache__/compass.cpython-310.pyc | Bin 5756 -> 5895 bytes .../__pycache__/state_machine.cpython-310.pyc | Bin 13564 -> 14247 bytes .../__pycache__/videoD.cpython-310.pyc | Bin 4981 -> 1245 bytes .../__pycache__/videoF.cpython-310.pyc | Bin 1528 -> 1577 bytes ws_nodes_py/__pycache__/world.cpython-310.pyc | Bin 4675 -> 4633 bytes ws_nodes_py/compass.py | 4 +- ws_nodes_py/default/ObjectsDetection.py | 2 +- .../ObjectsDetection.cpython-310.pyc | Bin 5810 -> 6050 bytes .../__pycache__/for_world.cpython-310.pyc | Bin 1120 -> 1120 bytes ws_nodes_py/settings/for_world.py | 2 +- ws_nodes_py/state_machine.py | 66 ++++++++++++++---- ws_nodes_py/videoD.py | 6 +- ws_nodes_py/videoF.py | 5 +- ws_nodes_py/world.py | 9 +-- 15 files changed, 75 insertions(+), 30 deletions(-) diff --git a/ws_nodes_py/CVCamera.py b/ws_nodes_py/CVCamera.py index 4050e47..b3e9b4e 100644 --- a/ws_nodes_py/CVCamera.py +++ b/ws_nodes_py/CVCamera.py @@ -67,7 +67,7 @@ class CVCamera(GetParameterNode, ObjectsDetection): cx = int(moments["m10"] / moments["m00"]) # Горизонтальный центр контура # cy = int(moments["m01"] / moments["m00"]) # Вертикальный центр контура course_normalized = round((cx - self.width / 2) / self.width, 1) - course = -round(course_normalized * self.fov) + course = round(course_normalized * self.fov) return course def get_poster(self, contours, frame=None): @@ -110,10 +110,11 @@ class CVCamera(GetParameterNode, ObjectsDetection): break # TODO расчёт дистанции - dist = 1 - - course = self.get_course(contour) + is_collide_checker = lambda contour: any(point[0][1] >= 400 for point in contour) + dist = 0.05 if is_collide_checker(contour) else 1 + course = (self.get_course(contour) + 10) % 360 # добавляем 10 градусов, чтобы он шёл чуть правее круга + islands.append((dist, course)) if frame is not None: # Вывод информации о контуре @@ -188,7 +189,7 @@ class CVCamera(GetParameterNode, ObjectsDetection): if len(points) == 1: break - contour = cv2.approxPolyDP(contour, self.k * cv2.arcLength(contour, True), True) + contour = cv2.approxPolyDP(contour, 0.01 * cv2.arcLength(contour, True), True) moments = cv2.moments(contour) # Координаты центра контура if moments["m00"]: diff --git a/ws_nodes_py/__pycache__/compass.cpython-310.pyc b/ws_nodes_py/__pycache__/compass.cpython-310.pyc index 454f1d432117d1b66192e1474615ac5da4b2b3d7..03f1c9f7955dc5155726e8f31b4073f02d3ac1fb 100644 GIT binary patch delta 1023 zcmZ9L%TE(g6vpSwtJ7(x4_XS8XN72*peYHDKtx0YBN`ONn5aopa*LtBfVV?Jq=pb` za3KpgaScjLTw+-HSQ%C>bWL~aAK=1`3llu|Vp!Ow-#zDi_sslq&+VV~^>fa=l!NSxrc_Sj~nh-Go=qYH3#k8?XDXt&t^J zJ!_EFMyX9FZB6vDX4dkrLnDOdlCsl64@$iRP?&OCNvH6OmV+c_^dd~)uKpA{Fl}r> zCw?>v_`&dFS&iaFa|pVyY`V~mzf5lS(8=%`Tvg*_`SF=A13mc8cR$c84U+UJY_?tp zEB16Tn|E`Qvw60EU^x)akQs!eW?p-vdYqflGs7L1VN}xU-Lxu{Zupyu!pNvE8t?(h#8M)`ySTCZq!Ut#B(GSt++TRXPVaO=_|3z zu#E??sSFo8ieiVsbMC`DUlavNB`y*dv*qIGxS-*W@m{~vj~-!@ox2@Sr%=}*qs=l2FnKh=r8Xr?Z5y4 delta 926 zcmYk5O=uHA6vub=E1P6@^PM#5SNcIQ+Jqpr)naRrYPCv>RzXmVX(z2s(`}h-u|lwv zDjp;h`)<8hK@qG7(pyh@_9}|Oiyl3A?m^HubEqcdH}n4Q?VEqzu#a=Ud#xqQ@=5p^ zy76r`{myzSlcVrVF0HviHbA{KsSt7$>f@^$M22fpS*CthDXV3z9HAyvA4=4sc3s76 z6tgimP6N0mXi(%rb(KgMH@Qg~rV$zyYE00e@wzUuF*ijMSk+9MMCQ`J$69ETrf9Q} zTLfDVA8WPKZRMDJheNPfC%rK)+gr*Xkp39P-*0WOXlm z)TT*44DJWh9emc&LFVx?-yE!-^?>AW+?lDyv}{0t7} zG6u`?sFy<+7~s1Zj@uj~9Na}l#$%mOQ)(20E&Nq&L^p{OlW_WovG*oaAY>QJ$Xap% zlgvCsfbSf{F1(0LCW<_~gJ>w#l|^w485R5cqN8LRCZn?xOT1#4S7@y0%#|49g0Ph5 zJANmOPk?bh>>i?F6$&$zVzoL`s#FSv|4ttVCD!FRVhk9qK?Aa}o!DEfukzQ;AUd~> ae}b*UkJwjzowr+s5AidePQ&mK_0M1Bo6n0V6?ne)vNldUY*}v)vZh>so^Pp=fUdS2Q}>*lo@<1WX>UYFX@^lG@;kEhF;Vg zM$u@PMYCZQtwyXEYuH7*5iiCYPSMe+o>5N}6L_2TWFu8fg|H;Uta`eUDP|ZJt7jWy z#j(bCalA26oM_~VxyEF1lFRJ+RAah09ez7goZ)t7i?evg>vQxCmpS!)js3;_jRVC4 zoKK)lw54m$Y9c98cQlc5Gq=s+Jo0IgK|bTo)D8l&07zDh0Wuas4goSQCIFcTA$dS@ zViJ(a5ONrhDKQPmbO<>D$c&f;WHy960?3@$2gtqV6>}&Q)#s~yy{vrh?V_v^HgAZ)n;H;T3!&BWl=3Rkr3{hbX_m7 z%78Bw^uT#$U6%c7tBJI=)DmtX7Q`>rTV?;L(?RTFqwQ}5*3+$4J&0Xy`X`?X?5n=4 zHrG%+bF(GuqSPqM>#p<)y3EjEj3Bmrv+DUl?1fgf=>wbvxIzORd{SY?dkqO!E9yck z8bU9cpVn4&VF>e%Q?x{0#DslEFUCY%I9Lc<92QA8j?HmI+D%|n;v(xNR+C~(jNgeB zlVU>TP%kAW#T4FYF)e2B&WKqthj&)&6Z`QV69?S!RYS~+gLkargjf)V0L^_`v$W#m zRjqI&NM2|)ec7tlU3sYjJdPepI|wF%_s_`r+HGx17y3P2*Sgx4aZlSa?;%yPkc%N_ z3xmr1_-%qZ;X5Hr3%Lv0q3c7 zIx0QDrK;sL@SjR&qtba*3Mx=N9pwB1HcjOZh57wqK9Bt2a3l$FfLoiVG1iWthQ(`k z*k3@cSy1&sE??mCM^rhl`%!KMD1h&d4Scs>p#9pT;xM%&G(wA|V9N8$z6+$UT&p(S zlJC~tR=YEOm7w_$nnxY!yCSfzE?;?JInX2_O)%DI-M|+cZA7hhwGt%PTIG6}&bC`# zmFO_cXRf)R+2&f9%~e{BcG>d=KTk>5bN#S_ajmlhO{v$b2yYTc?jm_#U%HPd&~M4Z z0CDu~XHQ&fHQW=jcD+-+Ay3Gbck0AVuXNKpv9eyR3raV^7rat?V<)wg-y5B0bWoyMP3E66&xGAf?+nEka z2~=8X-BRGUy*N)r!=1!%f$l@YeA~^xJ8FIw&4<)8)J115oLGOxCC_boEg8g^Vz-H4 z3XdVT-gaf+^zb!EtbxudK-%kp*=*emCKT>hu9cf>Am(br1udgWy|uQ653Oo*wIwIf zSm5-?InlX} zZQ_JM8p}`uqo}WxvGU_Rju=6bSgl0u|=ZpmUw@``7F<^ui~93 zVLyqVmK5J$M)3{|a2COPAELCL#m_Mu!`8!}k<$l%3+94Ozj6NMjXdh*^clS~xeL$x zCp5`Vg_v>gp)@hd3kV+Vx36{e+d9OKUNVq2!nBFB8Kx}~Ma=S`s>N1GRg+KRQ~3y@ zK;H=TPQj8(J&3$Ysbz{Ho-b*-fkV=RHuem1o{cE2r!kkI@%QTNpwAfU^RPdF1SBXz zP4^AN<~@@nZ_V0*K2x(py@()j-$9&E<@`mGzeo|JKdp3N0)yl^G|Y%9PgCkVq9Bd} zLSZ%IrwLB#rF<7f&rn3GD4(ZjWV*JZEgrLj@|=-X+chz;U8>HK>XBEKTjoZIfmLXk zdJm8$=4UF>6tlo6|Id=_eZ*A&{iE@!%=M8&$;N8}c|qyXAYQ&4TY z8)ImpOR8WTP%LPPR4raOKS_%ypAu$M7go58se5D+ka`kZl7zm?L9)+Ag7|9n7V(op zS}vkcl5mzR4tZ67kg~&blpi4&t&>{)A3~~ND~#fAc3GFv#ZENcTRy1HZ9j*vz5R%? zAfzM+$|j=HZ4eg4`}MTZnHt!E=)(#TM(?3?L?|=JboH7h6BJ{CU_8JgipatUEO60s z;JD55O5GKug2AH?bTKkc`5`p*aRf$x8|ZI6=*$j`W{`dCJs?t1c((@zq03W%_Ift4 z1EYNt;Rx!RCYIEP`W@tad-t2hhl5KX^8ldBm++D#5buW*HyhsCX!n;AB))6C-tt?XI&YR zJ_Ew+M!CN3N>ZVM7$g*=a^m8xip!#)5K|jyH6i+fc;%W~fz25txgM18TfVH|!_g_8 z1+0eP(FUb;hZwqU>7D$*RCkquJGdQb20UL{2%&@6jHOGeyWMz=<6T?#MeAnsW$K;w z7rhROg^3NbxpD~pV2;+e9731ZQ8yC zW)kb#x4}(cp6Ke}zZR{2pEB&gA_W`tQLXsgcMyZh*MMgLjK1vqQhpR&7UF8TW6DlR z5H+#op)jkADuH?3-Pm=}iF^169$_bul%tP>)n@fhc3|1T$M?o-(oRSc)>synjQ#}7 zz*>PbLXCr1i~Kl6!>nGyE2V8B@XAA=4es}@dIG!@A4dIxxwN!Ym?F)O5Uo@Sk|k(D zVjUVpDM*(}FRz#DsziQ_`X!A)G7BXp36F~p?GN(Fm$dQnM<^l|BZ<}}QJf^4lvRqZ zQ^dTM#v5cpm89a<>piuE=XHhZ9i?ar5e4R_4sa@MBp-8P`M5LVSdNKbJZp|8xE|5N z&|@NZ4#8VM6nR|Y>}!GFMOY#RABZJvAh#35u96LVsX|}%9!eAbUqE>C2qGUMzXk~< z^fmZwUL_}ml6fSxSWm%<$^vRa<#JlgUIN+H`W zm?77$$WNfg=qA%ww-MeXF`R?$1s0m+QAdX64h)M3ia3to!ayqOwP7P!4Or^aduE z`)((rRwJwhZ{MZ+F)68ek0K&cg~2~TsXh&HiV^M>3Oe%a_KG<7}r|01kekPwivtf$c-0gB(pwRuKMy7d;-1%D{4<{lN+dW-&_jfR1JtE zI{-i`bn%iz>S`jqzCT4p3U|0|Nq~Xr@yrN&ewN@rj;NENfjk7-v{CtgK%1vV(MI|I zNF4+U#pg7o{|fU}{kkPja7#%b6#Re8r(?S!ls$k@pd)bVN~!h~bYSmoVL!uv}Zj;0w0l7mmC3YF0QR0fb8^XFmMG?8Jv( zn4OvM3y%W~)sG6D>};H|VG8g-D3vT#+@U693(awFAdd)+TVgnK~>CWA*<;}6_hQ%rk^tc7-sd?LCriqAwi1-cEP8}sb=NdpRw_^nQE04qbj zSOMxqPYNkQOh(F|MuInuRVQqoUNa42Qzs;-Swd$Nv4pWt1Jr~?Wp?#B4Vt|%M-b@w za1xk6D4Sv+kOc&acjNK`ct-qywnB zA*8rP(FP*y*62#VfmXhP;LRbLfdeh4Pl5U!{79K0iw)M({JSV5uBXqzUSFa`)Wi?{wB&f{1FO6o75-uCYx;?dn#4zwT*rHc$i?l4Gut#t`r zr*bwJ-gC|4l@lxuFgeX)$sRSx;m+MXJqzB10^Uj=k}Y> zas?d6-rk09Z_%ExB=sFQF$~l;N-SM#$*ojZ9gC+?VzN|)9kx${m)k65!@L>h$HKf7 z=Etes+63BQON_^ogA|?&<>wUSG+a=%84+VSUFI6Is4*wlAx#E|Pe;`_sIK8q8m-OW zgSQ>*))EdFrKB4C73Lx*A{m)j>ZDjkUby_?g%4dE<(#jHGD)5z1brXqlq(6pgralLTXP=be-~i3lur#Z!!e*&De9LwA~F8Au)> zkC}RqQktWpQO1`((h$?xM(~Kg%taWQ1(vDbEeMzo$_Vw{^55x|2@ZlAslYp@&4mIRtMWQJ>2R1H8**1C_iVFu@9T zU|(XheTV|+`-nSH8+&ml<}|QXP1yd*PPKMpGNEf&amfDMXnC2#8WAHdh;tX$1}lu@H{60%&Q>Sf7(C=5$Aw8u?hnxvaAm}}j8JP& z%hA*^4#GtmI$4d@Tb*n;fgT>nPosQS+4Tn~cncx43!w#Al3+V_)CBMx0{f!+JL;Oa z*+VyoaM3LEYT*_U_B9@4pTU{-3)RYXct+>|lFog7cgX)rE8g4YH_6B$wYh}D8^k!@ zs+l;>8FhH!&<1{OQta;Aiu}He^lqCTDi#A`k!$X<{Yk7L>kuRd7zJjB2fsTi(3(W3 z@F&RZjS5@9dX!>~aJ3Aa5@$;Gm$WySEB81bE}RT-afUiTokYlIp-TEGK%o;5pY_gh zdj{Hjj_TGj>aC6fgv02T0O!q;G@!bW2V-1wd}~6<1Im&7kokz`CI=iKow(Y%JS(h_ zu-MSR!fl%CBPL5BH-NW&l;!0fVY_=W(dV#!d~B_l@>NqB9=0 z`w&?A8>r8_zAM9uu++fO$BeLmrLi4Y`jLmkQk%7b^V}OBPl-6=qywDH4B#ZL-vN8a zw#N6w$;2Q|CI}1jxGIwmtQOS6Aco5#aM})Q1BHpVkC9`v@Gntxi6Yhz{+v=Hn?Zq>or2avECN-Fi-x)a7*0gQG)#yqJy-zf7#0oAr{%U8H5W6UJZ!j^C38# zDRg#)%?;%Z-;wlVHQb2AfdRaroA8B^Pk0SZ4RYCuaGO^ z6K+1Z_^1RILeb{L-B?xMg!kExi`+ik0mg3yA8q*=v7~>j;_);p>J&c}?+%?1_=ozC%4@biuj)|k(gC!mnkD;Ao+;2iW zj$Wx=0reEDpkSyswo5%~Pt`jb)_a0iX0*M9Xl4uC@Aut}!%JYpE1Xsf`N;@hw~^5M#HEXSCP!j~&6uqO+NRA(cQMClMel zUeo0}!ZuEI9U?|Q4Vn04SnD)A7^?oeqSRAS>fKT5Jy7Q3;=SS&lzZ+Ks=vdn9}(|s znq5bX&BE#RhS3|{%ikCnM}}**N5%od-DMo#ANBeEC^a;;J;t&65aT$+ zuG?Nb8KgFF7~Z;B!u3^m4Xxn$+q2saUfbV$@ZiDFLh$rQ&L+ic1G93WRHj@GUQ%QaRPYlf4Eqw8@Kpi)Jr<`E~Tt zi39x(BLvRXYf|90$WubltT;>m&@q~yEwsW^0< zq7VCOaTnPj4b|FrpI2Agug|o2)1B z86F#KINGLT{t~W~2a}iS?-X9!t! zqg-vCA`R+2h;UO4N_$o}VzzG3&#=C2B*L_AeaCQiPut%#ju`lszd=3gfsyXz44vAb z66<@0t={&xjM-i}wK`l%wXC=7e>0Bu>Tny$pJpDiea&GI$p<0AZ{l1!pFNGc%xGeMI23Fo|z{~~#7-f_C?mZm01 zB~{l$S#2JtY6}B@XE=^KPb4wC8`8~u*R zYpZ;8Oi_Flfmpnv0&=%2@*oGs0FBVPC^L_h?0!V*HvUPWE*+f#>S^97M33XrBpoKC m_4)8nByEl(HE!jaxomDM_wL-|xm0e-nbtnxd@zY?jQ_!l!gy4FqADo+ThVu^kL3>@h zq=|$`-qS?V%WRm1{m7?88u_$0Q8@s}EFc+?1tc3m4gxYFMgbX(Aaj6>i5wug2yzIJ zaWMhNL!j))nt|DINO%sVO$+%)8uy~j2*%-2}rke}VA zz0NzzXqJ^zYBbmU@=9IBib1h{N|}DSuFO)yS8-Vs^S z49bl<($-=_c=?!$U#vEY!Pzq^cA?e`)|B;pqfu3{%k|*oS!FK;vRq%u>oP-Q8!C3~ zcG(Y9>}sQ24*<>rT%vjPe~BpLe+CIpE9gQi7(y?YpVF@D!Vu;?r(lUgA|~v6dLbs_ z!oj+2F^_YIiMz0&u($ z1=@zTsSABe*R{5`X>4hm<`z<}gBu1RV=7tD7vSo_VEt=atz$bz@ zmpVXQLhoF+bec3%Mq1Tu$x9pL;djv>q+jPm=U{2cO!qOlSJ|(4U6~SP%w*HQz9w$bNK--KOC0xJ|5v-W(De19_wp2BQRd&sFDHMvC4`3O3XIx_HtvX-u0xq3}$l8#@E)Ec+YVy%g&(JYr# za-~tMM(J#`;g^YcqI~9t2STf_MA=-aQEL``zrVS0qqPIaXrLv82gEJgNZ!{M?;}$B z9eD^Kj=p>8#EnMHJ0UAKTg6-QglzbyPTcn0+y05=)pAu(x(@pF-R9ardSLCxn`_E> zx>~F)i{dj+(U=AzOKUyeBZA|uTdtP_*FFApwNWZo{b!H}TeN2Cjd|LUdD@ZrvOkXj zOR~J|iER=DdoE2Lq;Mf75&>iInM%j5v z1EZ<2dhu3yg_$H!NZ#~hYqr)9%d7s0;%d-vo3gR&xk5IYCvMAf;I$^BQUbZljXNRu zEzkN?ch=*WHBfJ0ns2%L)7|d(q5BAv2Zm@(M+@t%*!8`--;gTC)U!>PoPQj-)utzv z(?KMaSn&e41Z=vg%zERt8VixKbfZ{b0b!ME9_R{Hs*RNuG_=a~>kXMhXUgf2n6d~P zRH93dYNYJD(gQI@2PmJQ2JvX-YD{>gYEgP_vnY!-FF?OBS*n5n5?8wr>l*MFGVR(+ zmlrRo_~pfG7cO0RNsX4!F*rtZb-7yhZ+KFUQs0v_$DQ3~P|fYl%b_eymPMf&rEEWr zsY!zDKq@&xL!=mKust`6b>(z0NR3Am@>iFM-kQX8R2qBZf%H zEG~mOF(zYacwD_kX`FzldM+OyIF99e)$908xHv9iU2aIPOrTFQ|o8ANG4i$BM3 z3|r?vBd7QN=FB;LP9NpJd1D?ma{83s8sCM>d=s7VS0eV;`6x}S={$me3{jx9^$i`o zU3U$njVNs*ZANK}_&qZjC^50?q{PURXetjQQu>qzXOq1D0P*hE|-vNPM@{pG)8d|Qs z+7!TcYU7i>lQXj6agFsIS6F8;9FcdGd!|lHwBpW3>F&WTB11a>)ktSW$!OyUT@v!zUEP?a|0Tcr1rI5!q0;b7j8B)%jI4Cd#& z6(1y6cVhAy)r^Hl&gFv>AKr*W-MbTj7QE&~H1vr9Wr5yFpt((`uG>c1XifC(O1D9Y z*4+6h-Q5G4ZCkHsGC?uc1Kk(;dlyv=WnuS=%JJ&O<*Fy#yulMzx)_?DB!qeyfl<;1 zN?H$F`}$_nPt1Eyhy*nsL*FEH`3^ukBb#6>!(+dI@&b)b3rp$)My>*~}*^$=OVSJ^2Mnd>V3OrNfaU_@`p~6@6 zb>Lvz+R%Mci&>d(o2Cb9lIubHyc0-T%J34O2J9yhd^(`C?ht#`Exk40x71w)&<+knVwl%U z8zFS2nv3CZyV1Yn-&hSq<92<8MyK<|sQs+EQ_e>|5D$2a+t?jz$FQK%8;1hNdNjdr` z*k4v}W&5@rymfE9Ce?tXQ-$S|YxEXiDr*_0b2ty$9Z3{4eD0S3^C{5IL&)@>_pVYK zM)r%S3Rbw7pCB!Z@XU2p(uEcxRw2$^m3G~at`@6diF^gs<&zXKgCw>H_dozm3nC+s zH>u=96cLA!MCy_VP7+GW5=9k#k1yUg6q)`1D}c9IRu|fhOWOL&Yl8z4TL3Pu;MLY1GgO&TO!-- zVu@OHK1vh%pGSB=6AB=OE0DrMUxCNrRWi0iSp=BNszcsfly5^#=s!+lHGt0M`J_qs zWFqn)P$Fp;l~sbrK&6^x;YM6LZ$?bJB-c=5_>8F)!5`R&a!`lBL9;yTuC=uT!Mb!s z3=$HPT@Tm+W5oy!dO*On{Cr?;LS9z1P@kx1m6(Kij8YqNH!hf`3Ggz8p@qzLBv3xK zC@0Z-XzSY7VfR6Upgn+my+Z|z6(vfYMWhmbvs`x@^^zCj6fr<~0#S!|JFC7Yi2{dq zK*hZ~UTHP(ej1hiETSCtfXNL!F89B!Ot>3SEx4vG-jB&Dbzh^XP7!19T}t&d7)~+5 zNfgkWSvoq9QR$c@7|nj*BeXb&;6H(=i(I;A3d6ImTLS0>S5b_eQRIY*Gm=@a6w85Z zp$XU$ES5d7O%gHRHy2f$ssWK?004x-E&Rya6!AiMe>bQo#2xNi5}oH!XRBdrAVKQX*v2v0V|$?n5Zh z5g2u;Q5U{!)AT=qD4*U9vE(|H4{3Bsg7JQI(M%Hh3~$d}z&?Y}hgL_RtutDsd9%7$ z6(2?^;!w(j=kf<2*T+%slQaequmj`;auI9?a*-T4hu~jCG>lxr^kTyDY!QQ3)ApR} zY4&tl@N2Wz8D35HHp8pQE;@KMM}T~zD)}OuF4r2D$f_h147X)an^hSBX6^>k^KUe& zq7+BH&PQpQ`FRAN`1>F?T+vtHYyWj!Sg(=;u8%Kv^m5k2$#ofTlk3tU4no|6j#MT6 z8Z6w^I@n|0!kO3Dcv3bJPz_ZEzoG6!6I9u57S?~&1NN_SC*^u`HE$bTawG zo`e(HI;YWEK0#gNsQ)6ElN;QKb|F#@_S`i2X=>eLCtZ5`Wx)LjM9{LyzHNYcc49j zRZ^hLG*TG|F`~?Oa=R>Qj0~`PL&Rpn7Vs`loD2eDQ=1W07-UwK|1xig;IErF)xe~l~Ov_EuC&tKPm2K1`4-U*?&`$ zpY94ItX+k9k<7L}twB{crU?SYy&V%K zaLJ|^xMKl#;_bLR4`n`nKwEdfAm}#{!~(nRY(OhsPqq_6$EiPu+mGb5hI|qxFc~J0 z7i6vQk;*Sn>K74h-vAJ#%OY}c`$L*1H-)52rF<&XyNo+0m9d6!=M;Vs#2th+WmIto zChkzQh6txMywgPJzlY#YBbtJjDyL6?GHT1F8UN7lfs>+Yjau z0q3!ULZ8ysO}IM@&}x&v>134Q!I!UNi&&iq?bJ7&-Pcrl;ZO&-dhrHPRG5LyNeN-a`Eiio^R5$&3! z!TPqdr;7a?3aHR05$2EBwTHxnz7MHspjFQCZIfzQMOP!C>ba+fASCJT$k;4`P&>bEtpg6l9 zAs3-Efo-#L#4<9`1`OAJtl}-i-1iQQnI3quj?B z`e56Na+^5_;PFUwPC!J%r&O5|F&5E9uCWg_rUiR_$)pJALKT;%6gFiqXo<*fl6pm8rWs2CryhN!S zMNIpLDb;hC6R|#m!VZ?Z?1zlvx2eS$M3H|?vJ4~_kIzh9r9ve!q*2C~9@5-JyMIFP zk0OFRj4(6{EDdiNvXOM0!i@x^;(xU!1`w-j0i+(8<1w>i7Dt2k5l(MbzlnmsLSVN! z^SN}|mIU3w&E{Jm%X9eC8 zfLP!N+=QLY?mjpsI>y8#+A>NjlbGD^A`4t$CE!C;M4QS!S9Uq6W*FpDtbZZ)T0}fT%@y;%#t`_BwNAZbe3s+t0-r>Hu|M4pF)SRWe8+1)YHStbYz^sIKVm z{al_2e?#BCa2VYZ;JjIq22>aFV1#RqZjOcWfO2Gx$$8pTM$BZNOQRJJk8Yk7R(Pgy zpvuC#kefr+N|d-Z#ScHkL^B%ZR)$nN>X-%qKg!bP{d66IZ6>zz90FEK1Kj? zZ};IbAK+cUV$h)dDI{|GQD&ECm=tW>V&Qi0JFTgq_PlPa4Kg#Ne*4Di z(acv-@Fv28kY=(vxFEr3r2`+~$2kP~`-nghahk>RjXRPyW=}$pe?UzL!Q~%P^hXr! zg0y(|r^!D?VQVz%_Ytu4cTk^CeOHF{NLc#v4}+!hIUbGgoBD7v(}$B;#>tV*(MQ9{ z1YzMk9-7Rv9iglSbVL=ydkgqt2UUXn*jq=-9!5S-=fG;hpHOP(MD7BH*Oh}<7@DwM zEF9^aQwROz*9Uv*;n<&{AV&vGY(-LT0K156G6?4p{3BqY0pB^pnL;;GIN?a>@Z-H8 zR>4y}+ylV5xel)vxq4UN8WC(&yapegfVHs$5*vcRMLYmWy=FnnOq1`9OYkZbeU7~r zE9>iYXMvZBTpuqLMG~Xnr6OLAPQPX%HG?@&3J>1If!7Q%i@r#H9E`@8!|O zyZAQZrL>EcxHv|R;tgHo#gk=SJoPH>^MXX%CWn<6*)U$vKCgdd7I%Jeb`3Z)ns_?` zxc=vL=|p`jwByA4iJOb#u%4*a3AiG{)H}MVlik!Q=;F3GEf(MmZJjeu#0t@(APX=W+gJXdGBey>T{-U03ms zd-Wdm^`2miam*0MIw$%Hq_c%Tx<9TbP6EsP@7)Gg(%**>g52h~_|dj5p58Kx6T!sh zB)L{`&pfpS=M+o!_b#?37Vyo_(=7oWZXTYWr#x$(tUD{a#*X*U(E0*+%f*$`{Qcdt z;X9O=6oASPQ*;`U%97RHb3n6an!Mtni|}M8ajKIz-ASCm6K?iexF=PThJ8-Lr!M5< z)HSJs+oTCYA?^;SY95Bp#RWEL!}^ zYysLQv`|R`5N=8CbpvUZ#oIKXvTkEvU@3IV^452g=w>0DXe&dKC+cV68m zR{j=_pobfb3*q8EfsSG4U0%Gr^gj7FwA^n{)ZPCUzx`W)P;aHUtFS7r9`Sy^<(yc0 z(S7O4m23HZ;k_(gOyY8KoVw^Ko406;G!)jrdog_a3~O8nz7$mnufAII@b(|?E9lOJ zeRImJdUdv6S$$(E%hz0N0pa$SZo*wWJyzqF>CL>~^h)#;amkan@WKWEap3dT$h#Oi zpJY2j8Ga3SK6s6Qx0toE@6*%8Y=r5Ze3}rcS9+^j6sOK~^YKnKy(}xVsCaZeKcsRf zt5z)6-$pt|5)s~OLD$XdM$Fa?`ZKKW8i^>aTi-LB-P88BjKc=navRjM9vbOh4*ms{ zSl>77-mmqh{h!9szB+r2@m!OG^$-nVw$S)h6)> zK71aDda@Iv-l)cD=QZj=cb0j?#ANfD(j%hY7VlPFil)=?hg;nikeFVr&t zf`pWl&^jYM$|<*IerR)>J2MhGXT%|)dq7&QxxZhi_oc|=;fLaFl1FkJ)0H<7BM11j z+J!-rDLID+wIzLS<&@hO$bvH=%+`)}C*;KA?iQKSDFtkA5EyG{*#g9TPQb0VXS_Jd z;^A)cXFi`x^wRHD?f082r~_%$jjINwkj7^$$rH_(#28Fa7DdXqDD|;)!5}d@WA}=o zaYqT)$7VefT1WJ| zQk|ngl5)g(!G&Vw_!b@}Tok+Qax76e*p&yvCiUoZdiwYdo9$)zF{ZU_h%xCdbcGop zEthmpJ^-+mk2(G%bdQXl*80)EBIjMJ$v`@;;tb_@HB*Zr}C)|M8ZGl;-d< zVTDEsJqgKI5z7f#$_12U54drwR7qC!yfGhbyU6%sN?=RL#@XEhV&uTB)`yjtb3|%Y~-FF{l}3 qu#hy;Svi%D5Zy3dp(MYrY>)XZ{aEsVpV{ literal 4981 zcmbtY&2JmW72nxiE|=6&-~!Zx)XqX>c^b!^M3;=-T|*-cWSz-G0xq*hw) zva>_k5|(HIIY?2!J@wR}fOKkoD|+l7&{O*l%&9;Rg@E3AY7S9z?J-cn| zx}8*dPTSG-N>c5)ZCBTwq}Hpq>$=X8vGy3&RTHn*Xg5ZEW^akSl~?VBRznB=Ic2zVT%xvKK0E21b@R+xe0ajGmCq{FHek;7nUHm$q8n)4{-ZPpt zHMJH6NgkznbU(Wpr-9fB*)uXs)W=XrRQ@B3NMp~~hkygdjKbJA4@hHXVfYXRYI7U3 z!YiOoVQf~pBi+r~W_@$46wYgpX`Uw=avb9xub(&e*d9Z@GHdX$^Wf@F=#}1R-7vw8@w8M*#^y-1JzsO%9Ssd;1EL8up zvvo}D$RVw91B!pOrJOM6h1<8{H=xz{FGoj5-M{~hKL392U*!e-<2_|{vhC;YgYJgP zJ(U00AY`pi%+~!U#uKL4Lotg!O+LSVc_ZsZm&NARZm=ybi!8r-`9bbK$SHLf9JXScFd#f)}{WAa^FpY-Z~Giy+YE+6l`PnjZ+45$@k+lmcD=K-6RWPP1jH< z7sAe~;YJj0MdFawcTw<>*A?4(>AA!-rid9L`ZQ-wYPxxrn)4uwR6*b|lqa)(9Qsgb zB!ZXn)%q%+x+kpl)ttY1%z1Y~b6!lrd9gRh<>Gp@m`f3--KHtdk?eXTL?(P0J(Rmv zxPAGMf}(0{L~!Y}>+AKZI%>XjZ{i|}e3i&op84A}gUA<`QZv|9>@T{ zRxj_C?4MU8G0o|E97B+CHLdb!c4v@=6q7o^AdxSX_;vUG9k4F%Ya-LJ)9+__Ori7t zX5Bnyy<1!QU=cx9$i>guV)GlSk`MZkQ0}pfs_{IM5S%4`FUYrYHQ9?(KgmwpG%v!(WIYJC)Z7V2 zzaz5V@o@y1k*Z>lZb?+BWv&ZVrz0m>w+p4(aoWk6rn2)W>8N_?gybp+MK#}tTJqfY zON-N9`#o@(M9CLHJmxaTbeQ8f>;k*UE|?e0IrAc$XLFWoUSqrGULI+}`<24fzmBX*x3Kq+*Y>EV z=GS!(BL3#sS>(4lk_Q#9LaWvNv-EJL{?;LJ`mR>Yb>aa!{3lSN&l$B-= zlp`}Aj;I`p3D9CCCrqH+4&e(w<}e>vBYrwb7D!d;6K)W5;4LnJD7Gu!qhGokGKHoi z=;X?RRVtg*kJOS4L>|HMV+1Q5GqlH_xV@ShEr#W10)a|K#5(afjS3S7v6IRtRn z2aMRS6i#6kHu`KR8-P(do7DnI%n`7Wx zEvkjdK}r6CF%KB|iOKQRJ>YlN_{8j(@6v9){f6H2IHYT^qc7QQ>8lmzaU|u5h@XTo zv4XBcGD8H~1}RkZvm_voJp2O5y*VMS)1vwC1S0zILl^>5gdc8Sg&k}J2ol=C)-@6n zAJK|wvL6x^#)vD30E7%_frAUDcx6GFs#9!P^KYyz-%xJ2Etj$+6KZ^E_0H;=zp{4Y zr&rfjZi;unP!M*&KDMuF$uDlF>1uNluhFcEHcB$pxVr&cd=PVlK^NiMFAc5SsH!4} z#(15*o9{aJ=vei+bjddnSr;UYRk938Dy4 zjTvL;7A~)CS>iJ}E-_xXC9uZMC(v(*-vQ#*=8a(ua%#g`jdV8&@2YnhpWu_cdQbuA zb#>_cQ&B72j){j5cGR3|ol4Mj0ff+U3Mu*$i~~f^1=^swWn+=q0^YcC1y~}$6j%tr zXex8@eY6BYx+knOw3DEuL0t47P8fhUkMQ4&5m`u=W74H5_eLl$Atjc_1(7_eJ*bZkFuax?ASz zS|GwZQQ8GS(3rIk#?NFw!mO((8lgGHEL=cG4XA1qR#J>YYj0j5)x9am+Qs`IhX^1? z(4ojB!7St=>$tH#>${WX92BSeGNjXP>;>HQnGTmFr^ch^=w@>{E| zWo094Nab|m1VKtUc_w7U)mT_^xFI5-Cz3({@xbH#jBEmfC`4{7`Z8n2GVwJ2qV4^eiMncz3sDRro|sbR#&Sn*?& z{5nY8w3&PKU2~F|P^iP~BNx%dMe3?E*PLZ@h&i*?v`%P~=y#d$ZbZ9$Pwd<>!*BP0|?Npg&aw9PNm z6n&s^)Fx1JdONs;j4rzM$!S6T&)F^ISaQ83l>7<^;u;>koIS?FQksA{49;j#o`o;N zj_G-ho{D~^lP{a}e})~1ZVudV6&wh~#Zrp`kn7_g2M>=zDY6~?JXCJJF_1iakmAAf z8=7AyeXaJRbm@Z^!o9M4L7e`A_C^Qs03&b*%wyuWv=9oUap-{FIn8sW_}89!hh7>z zH7>2Z@PCV-gR3u{`r_X(g7l*%=r0WbVdU5R5gxMs9bs-BQLk+_MAkJb=VXI+GK^0+# ziKH+DGiY-7-Qp@rO^GkaFD^+fa=XQoUX+@e2j$-4OUg-1&Ia)^OEUBGLMAU{mSkj^ zyqj5pn=3uFBtEk^CowN&@-1e2Hjrb47$?iJL@=sME@L^(s5#k*RYg<`C|nGZ<6&fD jx)Ckm5G) delta 187 zcmZ3<^Mji&pO=@50SL-k|D|nU-pHrQ%;+=OgxMsRX(0N&YdGdQ!V|HE+4lYKP{{UeiF-!me diff --git a/ws_nodes_py/__pycache__/world.cpython-310.pyc b/ws_nodes_py/__pycache__/world.cpython-310.pyc index a8caf32d0f6d90acd2979a35c0312cefa4e5a66f..6f441d1f1ddf5266ba9b9fe55dbc87ad45ab93ca 100644 GIT binary patch literal 4633 zcmb_gNpBp-6|U-SdRFcfNiCi@fq=s#w-bAv-cI85oVH_VZsPX5 zw%04QOJ?jPey`jv_bTm5uiCB}S}CdZ>g{^3(Qfo6+7q0;$AmA+kC`Y()}h^=6ctf@ z%-U1Yw5aV^@=i2$$gq;HnWt6%95S)h7T1&SMe2Si!(OB!xtfUxH2zIAj*5Fx%hKMO zl5x7F-JhoF+Vz&L-SxdVSK7Uw#i`OY^IB73uOA6;OEMI3nC9B$4Oy2jumk=+OQ#*CtooeU>?iEV5&Q{AL0ZaKFE7k-Y z>sp${$=;9_==?7a8W-JGPWVYiKW+(E__j-%%bq z;ErZ3uvx4V)&(ZKL+gN6`@-5`^0z~;$}sj+RnCw0}8qa_`do(vJ8=W$k%3mEcaurI| z-0h1{!ILBz?eo97jKzOPw`Iu&2=QM^pjyNtersM*{OEEL<|-gp)QwvTp185AqB~M% zQdhPj6(rf#RwT6(r<<9s$9Z5*iGn_y&z0G3DzqJL_bTQ0VHCgjLcq#KIp z{3b>eJV(3;DP)QzeS>BEk{hh;vF{pLi6Q;QExnM9t9a zXSik25EEk3(5KFDD`HyAh*?9QGwuAz&Q(CkQ``tEw)R@NS5Ur|Cx% z;*y%|PIafdGvc!0TzTTikJK!_V@}PZ#coFnNF$<8a%LXdz#~rx<{hm=gDSr*m7ZfN%bf5B7 z<2p~dyaJNx7tMDt)c%bm>}`nf<_~ERMVi9~&BC@p5T~&Ug5?`Y)(MmRCOXCBbC1o* z@Q@zqb&bi_FrZ?Q#Ay^tb3VlQ7O2mOAt~vT1UX6IGm{OdZgjSz&JKn0=5E^2B@u0g zyNS~EQAmz7aS@Bo6-izL*N9Ra`aK#_Fpaffj{gynzK`bIoxbqErH>nZ+Edg+WUo-Z zak87)mlS)lrO~O!keISCDdg3j=|4L6psyW~VnXOWOTl8+Fu4nt$mNZqj$r3A%<#l}3AkDMsh+n^R;HW@}li>o0ccTgO`umQHWo?1X7^ zBWixPnaSq0!R6bx?%w|8-COH-nkT0RVY*e6Lpe#JHIA#=|4FSsKbEZ3M7i7TBgFmV zbfXJsaH3&(i6p3-6gYCpT+R_~Dc{}5J1GCgTQ^a``^TVF0yr>90c+~C1h9aDPtVtl z!cUOdl8vylBg-^pVhk0EDn3VmLPVV;lmHON+XF^)5|q>O2};(^`hGvs&fE9y-PIFg zMVeYJ7isUAG4({fdZK_HCj>b|Q{Bj*nTaeBg*%L}$;_B=vO>>~fCfT!Q)azmX=Rgk zs&`}*s%Qj(9>I2l1-F` zRRJ0RGO&OK8qZdY4yEzqKkAeE4FvxTogweSfpGy@R6zMBXaFc+1isPhT7P7pTE~FV ztgx}-b)W-hi}1pk*~0zQHuC{`16LqPn^h&Oy7z)r{%BSCconT|RaSMmaF1TtA5=`Y ziEdMuHseR+nx;ezYRxbe%@OHcT^W1NV8XD*g!PsuO{^Q=C!aQ^mb26&cLwGqv;5Ec zcn=JEaw_^2Ahac3!EeE|#oPJ!V79Qrvwr=IEyxZO6vh@Gp+n^iwy@=!)Pu%&*Luk0 zEsTIhx&Z$N*3c4!*Lc_Ix(XQ0^yqp}ri%|o{yiF zvnt<#G)Ac=jOXdmLqYiJgLi?8AbZ2YgmT%p;qHBEA8`INO%5-iY_0Ff$o6YAH3 z;K6Q~6dV(ClA=nI2_>DP>!Zl)L`iZSeMXw!&=i9p>gdt8<;5M==e4W;<;l7vdI!(R zA7MTlk1;B3q5!v_0B$M^w=){SloT2WFvrl@kxOqH@6Dy=i9qVWP2$x$O`5^r$6oV!Z;OR6e1%@RB*AVaoMNMp(^4BL2ITs?Hi|V zE6XEjxBUEn9wem?edY92RCl^E=F61vO<5?eg|%EETjpS(F&WqBr{1xe4vy^ri~)L7ipy$HmbjvAX)3<8olPCEsev_8}fnTa{ H=ga>BDt;0z literal 4675 zcmb_gNpBp-6|U-Krf20&k<>O;j3ASRv@J(Y0!52yM=&DO3Pi)m#6hDu)g1P)r<k|+eQhp6hR_o`mKdi7SOwOX0s z_r>FHH*ZWb_HUY;{yCW3LCgMzPB6h^)}pT!^PbhRIF0StUZ<}UJ3Xi67?>Noy+W(d zE4GR~ujLt7AujdGt#Yr@s+f5(uJ&rJTCd)!_a<5sy~);OZ>lxL**zvaQF_WmDYOpl z*0d;#%2U>w31>xh+mauJGlvWm_(Rj0wXeY&TWfMXc`sBC0vYr|70R_#gn;p%qH|Q# z3!Ans{w_`fb>m%Kc&KEQbhP_XqTasIw6wdv8)Ztn57H=6O;FBwCTvxKYKXIZekF!78k5#2`(%!Za-t2T-d^S>a`rD|u+EXxU$Y+4mGXWCPaa3+#Y@#*%4=DQjYW%(YTD_7X6fZfGIE}U6*fgSP#OSqp~2ljyjxOY@KW*@LW zHxAr^J#Zanvu!SK4=nVn3En-i2JV49aE3Tnehi!|9}@1-b>N@}34e55xkPPXFEXsP zw{4jYtbx5?W(({h-M||-zz$a}uvzRA)&(XCht`4NQCQnd4hDq^!@Qu%s)E(kLw?9- z*#PaJ2x#pksE1W`VYv*{i|^qEH|dnRC>n^DL)63}J1D8ieC;CSFev#`!+CKyp9Xg3 zXm(IC(qwofZ*9MxsX(eme@6rgswC;TzW$fXSo9wZo0fziS&;Rov*S@Ixt~YvoR;{5b7&LaCi7*-W)7(|#hf9c*N}sJ3LP;!sz5sY<1v z?S!Gw&UUz$$r>ojDv>X4g-}@VRHvD@lV*{M(nOYMN;(}V&7Ww#r>i^t4S$5!Ma<$T z+iI4zlZEl7E{iZ$LC$Eo7YaX%I!O>~J5G1CvlVr=w2ia1oAY6bC)yHw+U{kY=S=E)Y z&{=$vS9zU#-1^S5=WP$;1@4&U0CvYI^8%l@=lQfXjnO3iS^HIERLeQBGqy#hOSVPd zY#q-;R&vC1+ff!g8~ef{4fSmW+wjRgTvZSjnY3`>rwXQrL4Q>s+XJ>pW-TfPUVRDc ziJGX32?L*e30o3VVp_}?_^j#Y#+1vdB<68KMO-kLg|A>|s(Ms2);b6Kg&hN5R^EXB zor%SNV0Kg=SjSu}o$Pq@IiM5bqMGbZb*H;C;*!B#K6d0^)hzBfr*MjY0jn0^M;65u zqxn}w@glrSQcz1~&Bc68_mc8NL%cDhZv%Q6JFkF`t7D6~wx-=(dQ)9dO4KfpI57E9MV;NxhoUa*edd}rlfXq1WmNC`d%{b_7h~U<{w27?EVf&5z!F@kU zBIWzbH{-M&#Mvzj^2O&Kn~^qvc{taHOkT$X0iqB`Nf=0TKG^sj2luE;P$ATUPq zy;(E|5{pePe~phW{6K_%L)Rg~x*EZchr7NBOXo72kwH`9*0k2vU$4pL&588>(rMC- zc91kS!p2*TR5sq;zr1?){_3aq@2-E?7@zJ3Nhc4Ca++AHkK)@8bm@B_YA)SuAn^5f zVA87OnK-B!$2Zi(OfO~qjjWBJZ}f8$VSHzlv`TV45;+4i!CI2B>gwtB zx}IwWd|A8^w72z@GsN>dBAVtDWv1Oog}qGAP+l>#h4J1w0*bI52QnOO!F62OjuClF z<6K%h>w7z)cHaNwllyw&MA9bg%OzrR<^(#Hzn;jmlrccg(NZ@wna)HSi(FrZY0S(C zV7x=m4|5Wj@TN?ABW~pzM5)%6VW2`kkLq#-u%<14gpd4*Iq7i__d~e2lJDb^S?2q> z)=}7yzeh)d?8oSkQdk9k#k#`jZvjcen|#SGAc3e`NGO1tw~$8E&qkFtx%$zM#*{>? zqGd#Qz`M|Plq_=yha7>_5(vZy*!HQU!Vz%A)SC91F6EEg3wHMNVFSaaD{q6E5uTi< z(S7PjwUB0-+rBCsJpi7rXu??;S<*CjzT2P8FK||g&Rm18FlrfD&@=XoZ|22}hwQ>K ztiEL9WZE-y9Ob=V@*@P9FWIpv5l2y!>xulOPV9qR({oEZ35vG-DwH+KJgkrO^bbUq zq&vy?f0pzA3T-}#@S}a^tw##cA-Y@CnG-f?WYmB%4U&~{PIc~B`-|tE(I&x+esp7+ zucBq-3Q^i1u;RUu7~cPyo&;;gSGzYw>Rm6@mP584?sB4&!)aVTO|pT4IDefl&o5qdeYY9GCA2T4(NfegM zgaZ>D^i(HAS*7IO$qExXOo<1^Q7019l4Nd*!n{1J)08A`?jXA~GB0(~<)(Kt*MVE~ a7})|k&#Ics&%!faLP6y{@QP*b{P%zUTNWSy diff --git a/ws_nodes_py/compass.py b/ws_nodes_py/compass.py index fbf530c..8e37856 100644 --- a/ws_nodes_py/compass.py +++ b/ws_nodes_py/compass.py @@ -149,10 +149,10 @@ class Compass(GetParameterNode): elif msg.data == "reset_heading" and ((not self.is_use_real_compass and self.COMPASS_MODE == "AUTO") or self.COMPASS_MODE == "FORCE_VIRTUAL"): self.get_logger().info(f"reset virtual heading {self.__compass}") self.heading = 0 - elif msg.data == "fix_pose": + elif msg.data == "fix_pos": self.heading = 0 self.is_enabled = False - elif msg.data == "update_pose": + elif msg.data == "update_pos": self.heading = 0 self.is_enabled = True diff --git a/ws_nodes_py/default/ObjectsDetection.py b/ws_nodes_py/default/ObjectsDetection.py index 3ae5f53..909b5c8 100644 --- a/ws_nodes_py/default/ObjectsDetection.py +++ b/ws_nodes_py/default/ObjectsDetection.py @@ -105,7 +105,7 @@ class ObjectsDetection: ], }, "black_position": { - "binary": [((0, 0, 0), (180, 255, 20)), cv2.COLOR_RGB2HSV, roi_corners, False], + "binary": [((0, 0, 0), (180, 255, 20)), cv2.COLOR_RGB2HSV, roi_full, False], "contours": [ # Выделение контуров по размерам 400, None, 2, True, # min_size, max_size, count_counturs, is_max_size diff --git a/ws_nodes_py/default/__pycache__/ObjectsDetection.cpython-310.pyc b/ws_nodes_py/default/__pycache__/ObjectsDetection.cpython-310.pyc index a15d70aa55f20461b949bcc20aea5a97ddf64e79..7fab8a7cd187cd63e4c6e4d0081d321fdfb5859a 100644 GIT binary patch delta 1728 zcmZuxO>7%g5Z<@DUay_?#*UqkGz9YJpiNS@1+`L0LQ^3D0X5-5LFxez;!jl%IaCx0#Eq3ef(tU2B2F|1;LWqKA&Dh_nw_2b zX6AeI_M?MahjcHIhzsnu{oCJ5!NlwOpYa5HdW`-3tB@o;mo33ktmqoCR4Zmht+*4j z+BQT>3xpR$D`7<)*%2&#i|pfVZ+Md|maK`E?8sJ%@x>XR*x>7E@THNDRl%6dMr2Je zB};f(Fi0oZ)WNERl=_FSx3uG*$k2<%YhX3?vL8nrER+4D<5$#cVa<0-HyToOwp2fv6f z9v){bGLeZUtCB1#qPQdfrD+Oj;eY(P_8pX^sWu#jA77F{igd#r$tEHkkuSn4ayM+r z50QuAYxzKDLkk?2q6{>}0JO-0NRv>G^pIn)9yv8L$-G9C4q(I}!VrT{FP0077n=Rk zEIKuZ+O~^MHU=GLE<>UtkW$hI49pxw$Ti^*v%+4O>FiZznCUNosm#M`iY9%&Iu5RM zAAGB9k`cHVogicIVYF`o^$3$C->i7lHWxiNpanmm9ZgD3&UrP{bnL(^mfQkex%M$p zuMkGH9?NA~G3r#gP$^o4{3mcb=4NM+W)9&D1B1|Kn!HF!&ocNFo{LY8?une=8~GW! z&cUtt=mWeWa_$lqQcg#K0pmx<@{Gt%p4wCGjcpi2E4@RNRb zP{)*}90my^Rcv*W-!X2fX`?zWGyv+PawDZHbTkWUUNxEu5oILT~X@8-l|!Zdwc z1&>-b^}m4+lG|h*ywrd)f;q?Gjnr|rU_Ykv!#t^#nXYI4Z`*!=>5i>IRAEPQz5dm7 zf648yuUS7U6O>%XXOkNsrE|x4ffLxbM6uUUDwHbpB$nM(D&E@G8aU}qC5=IEf}ZJ9 z-a*rVBbjXeZDtxW=b{_vMy@c%u0*|s-Lx9g{wwhBa5ZC*$KhUPX_zaws|OurTK2rM zSt7%g5cYa^?Tx+ujopxv5GS<-n=}ap1*sx9Nds-wmZVMHh9aS5o!weX?cM0D z-I5T53K6}4L^YJZ6r| z=FQvn(Hp~YHx`Qo@b}j1x6e)tU5;-=6dXtH>_?ZZYSq1_CWM36Fh+6$IDe?;T);sir8*)sugBg&ZcDcAi6N69-4_w&mNnd)8`Hy zc_?plIhO%Gihc%^l{{RO0 zN!!zFF0=Fn*QqnZtFys27iTR{l&b3$tFD)A$6!nL&A*gY0jbVMMl#zm%6QeNl})4g zDtr@h3Wtg22*EJ~O<=QRx5L?S9KHreqfZXr6FGBl7C2Fx%eS?-;LGj`i{)D_(+qv2r(OdyeAUF`VrZd>K0$o5KlB z^2Ms12-iFEe#NM{l`=xamDLiVCctBH-PdyRF!d>AFX);4;%<-WB*L z^@DH`^mKoST9i$~o9TntU_YgcdA?|SWoK*UUEQ`0qn+ymRE7UU$IUHw=4>a|YO@?} zQ@5RphsyilpU#md_y$vyx5y}0Uo`9*qwBq+=6+sIxe;hpoo z2XEK>>8rHM#{^dhJ|s9mFhRgmTO;cef^EWf5gC1j=)>Y=5R`x-C~>6(xvor%e<5V2 hQCnPM45qpsAGyzZUm35tW}|8q1tL*4NmL;%{trvCX@vj) diff --git a/ws_nodes_py/settings/__pycache__/for_world.cpython-310.pyc b/ws_nodes_py/settings/__pycache__/for_world.cpython-310.pyc index c0a54f25f24e4367720ee0b096669187a8bffd37..6e84228f41134309ca4833ff7b495097e93c63b2 100644 GIT binary patch delta 73 zcmaFB@qmLjpO=@50SGpH{h!9Vkyn+GpD!sVF*!TFAip@XBr`v6vpu5`Bi}6^h-h+V XQF2b| Duration(seconds=self.time): @@ -330,6 +341,7 @@ def main(): "navigation_marker": "", }, } + circle_mission = { "circle_1": { "type": "moveAround", @@ -340,7 +352,28 @@ def main(): }, } - drop_mission = { + drop_cube_mission = { + "black-circle": { + "type": "moveTo", + "pos": world_markers["black_position"], + "is_real_compass": False, + "is_update_pos": False, + "dist_threshold": 0.10, + "navigation_marker": "black_position", + }, + 'drop-box-1':{ + 'type':'drop-box', + 'time': 15, + 'state': 150, + }, + 'drop-box-2':{ + 'type':'drop-box', + 'time': 5, + 'state': 0, + }, + } + + drop_stakan_mission = { "red-circle": { "type": "moveTo", "pos": [9, 3], @@ -365,7 +398,7 @@ def main(): # 'drop-box-1':{ # 'type':'drop-box', # 'time': 15, - # 'state': 360, + # 'state': 150, # }, # 'drop-box-2':{ # 'type':'drop-box', @@ -377,8 +410,9 @@ def main(): vlad24_mission = { "gate-1-yellow": { "type": "moveTo", - "pos": [6, 0], # "pos": [6.5, 0], + "pos": world_markers["yellow_gate"], # "pos": [6.5, 0], "is_real_compass": False, + "is_update_pos": False, "dist_threshold": 0.25, "navigation_marker": "yellow_gate", # "navigation_marker": "", @@ -388,13 +422,15 @@ def main(): "pos": [8, -3], "dist_threshold": 0.25, "is_real_compass": False, + "is_update_pos": False, "navigation_marker": "", }, "point-center-blue": { "type": "moveTo", - "pos": [8, 0], + "pos": world_markers["blue_gate"], "dist_threshold": 0.25, "is_real_compass": False, + "is_update_pos": False, "navigation_marker": "blue_gate", # "navigation_marker": "", }, @@ -403,13 +439,15 @@ def main(): "pos": [8, 1], "dist_threshold": 0.25, "is_real_compass": False, + "is_update_pos": False, "navigation_marker": "", }, "gate-3-yellow": { "type": "moveTo", - "pos": [6, 0], + "pos": world_markers["yellow_gate"], "dist_threshold": 0.25, "is_real_compass": False, + "is_update_pos": False, "navigation_marker": "yellow_gate", # "navigation_marker": "", }, @@ -418,20 +456,22 @@ def main(): "pos": [4, 2], "dist_threshold": 0.25, "is_real_compass": False, + "is_update_pos": False, "navigation_marker": "", }, "red-circle": { "type": "moveTo", - "pos": [9, 3], + "pos": world_markers["red_point"], "is_real_compass": False, + "is_update_pos": False, "dist_threshold": 0.1, "navigation_marker": "red_point", }, } - # mission = vlad24_mission + mission = vlad24_mission # mission = square_mission - mission = drop_mission + # mission = drop_cube_mission with sm: c.get_logger().info("---!---") smach.StateMachine.add('STARTING', Start(c), @@ -439,7 +479,7 @@ def main(): for i, key in enumerate(mission.keys(), 1): if mission[key]["type"] == "moveTo": - smach.StateMachine.add(f'GOAL-{i}', MoveToGoal(c, 1000, mission[key]["pos"], mission[key]["is_real_compass"], mission[key]["navigation_marker"], mission[key]["dist_threshold"]), + smach.StateMachine.add(f'GOAL-{i}', MoveToGoal(c, 1000, mission[key]["pos"], mission[key]["is_real_compass"], mission[key]["navigation_marker"], mission[key]["dist_threshold"], mission[key]["is_update_pos"]), transitions={'timeout': f'GOAL-{ "FINISH" if len(mission.keys()) == i else i+1}'}) elif mission[key]["type"] == "moveAround": smach.StateMachine.add(f'GOAL-{i}', MoveToCircle(c, 1000, mission[key]["pos"], mission[key]["radius"], mission[key]["out_heading"], mission[key]["is_real_compass"], mission[key]["navigation_marker"]), diff --git a/ws_nodes_py/videoD.py b/ws_nodes_py/videoD.py index 314d0fb..fecc09c 100644 --- a/ws_nodes_py/videoD.py +++ b/ws_nodes_py/videoD.py @@ -2,17 +2,19 @@ import rclpy from ws_nodes_py.CVCamera import CVCamera +from ws_nodes_py.settings.robot_settings import is_real from math import radians, tan class RosOpencvToBinary(CVCamera): def __init__(self): - super().__init__('camcvD', -90 + 20) + super().__init__('camcvD', -8 if is_real else 0) self.pool_depth = 1.4 # in meters self.meter_per_pix = (tan(radians(self.fov/2)) * self.pool_depth) / (self.width / 2) def image_processing(self, frame): - self.publish_coordinte(self.get_points, "red_position", frame, True) + # self.publish_coordinte(self.get_bottom, "red_position", frame, True) + pass def main(args=None): diff --git a/ws_nodes_py/videoF.py b/ws_nodes_py/videoF.py index 3b12d80..8d42a7d 100644 --- a/ws_nodes_py/videoF.py +++ b/ws_nodes_py/videoF.py @@ -12,8 +12,9 @@ class RosOpencvToBinary(CVCamera): self.k2 = self.get_declare_parameter('k2', rclpy.Parameter.Type.DOUBLE, checker=lambda x: True) def image_processing(self, frame): - self.publish_coordinte(self.get_poster, "red_poster", frame, True) - self.publish_coordinte(self.get_poster, "green_poster", frame, True) + self.publish_coordinte(self.get_poster, "red_poster", frame, False) + self.publish_coordinte(self.get_poster, "green_poster", frame, False) + self.publish_coordinte(self.get_island, "black_position", frame, True) self.publish_coordinte(self.get_gates, "yellow_gate", frame, True) self.publish_coordinte(self.get_gates, "blue_gate", frame, True) diff --git a/ws_nodes_py/world.py b/ws_nodes_py/world.py index 3759154..5653013 100644 --- a/ws_nodes_py/world.py +++ b/ws_nodes_py/world.py @@ -80,16 +80,17 @@ class World(GetParameterNode): if msg.data == "reset_map": self.position = start_position[:] self.get_logger().info(f"reset map {self.position}") - elif msg.data == "fix_pose": + elif msg.data == "fix_pos": self.position = [0, 0] - self.get_logger().info(f"reset map {self.position}") + self.get_logger().info(f"reset map {self.position} and fix_pos") self.is_update_pos = False - elif msg.data == "update_pose": + elif msg.data == "update_pos": self.position = [0, 0] + self.get_logger().info(f"reset map {self.position} and update_pos") self.is_update_pos = True def update_position(self, msg): - if self.navigation_marker in msg.header.frame_id: + if self.navigation_marker != "" and self.navigation_marker in msg.header.frame_id: obj_pos = self.world_markers[self.navigation_marker] x = obj_pos[0] - msg.point.x * cos(radians(msg.point.z + self.heading)) y = obj_pos[1] - msg.point.x * sin(radians(msg.point.z + self.heading)) -- GitLab From 5469b277f6d6fece7c4b9d08ba73b6317dfde23e Mon Sep 17 00:00:00 2001 From: "toropov.nik" Date: Sat, 5 Oct 2024 09:30:34 +0300 Subject: [PATCH 12/23] 4oct_after_rebase --- ws_nodes_py/CVCamera.py | 4 +- ws_nodes_py/default/ObjectsDetection.py | 22 +++++++ .../__pycache__/for_world.cpython-38.pyc | Bin 1114 -> 1114 bytes ws_nodes_py/settings/for_world.py | 2 +- ws_nodes_py/settings/robot_settings.py | 4 +- ws_nodes_py/state_machine.py | 56 ++++++++++++------ ws_nodes_py/videoD.py | 3 +- ws_nodes_py/videoF.py | 6 +- ws_nodes_py/videoS.py | 2 +- ws_nodes_py/world.py | 2 +- 10 files changed, 72 insertions(+), 29 deletions(-) diff --git a/ws_nodes_py/CVCamera.py b/ws_nodes_py/CVCamera.py index b3e9b4e..eb01329 100644 --- a/ws_nodes_py/CVCamera.py +++ b/ws_nodes_py/CVCamera.py @@ -110,10 +110,10 @@ class CVCamera(GetParameterNode, ObjectsDetection): break # TODO расчёт дистанции - is_collide_checker = lambda contour: any(point[0][1] >= 400 for point in contour) + is_collide_checker = lambda contour: any(point[0][1] >= 330 and point[0][0] <= 476 for point in contour) dist = 0.05 if is_collide_checker(contour) else 1 - course = (self.get_course(contour) + 10) % 360 # добавляем 10 градусов, чтобы он шёл чуть правее круга + course = (self.get_course(contour)-4) % 360 # добавляем 10 градусов, чтобы он шёл чуть правее круга islands.append((dist, course)) diff --git a/ws_nodes_py/default/ObjectsDetection.py b/ws_nodes_py/default/ObjectsDetection.py index 909b5c8..2b84d3d 100644 --- a/ws_nodes_py/default/ObjectsDetection.py +++ b/ws_nodes_py/default/ObjectsDetection.py @@ -53,6 +53,28 @@ class ObjectsDetection: 0.01, True, 1, # aprox_k is_max_vertices, count_aproxs ], }, + "black_position_front": { + "binary": [((5, 126, 124), (221, 134, 154)), cv2.COLOR_RGB2YUV, (280, 230, 640, 340), True], + "contours": [ + # Выделение контуров по размерам + 1500, None, 1, True, # min_size, max_size, count_counturs, is_max_size + # Проверка, что контур не косается границы (roi_full, 0) - нет проверки + roi_full, 0, # border, border_offset + # Сортировка пол количеству граней + 0.01, True, 1, # aprox_k is_max_vertices, count_aproxs + ], + }, + "black_position_side": { + "binary": [((5, 126, 124), (221, 134, 154)), cv2.COLOR_RGB2YUV, (280, 250, 640, 640), True], + "contours": [ + # Выделение контуров по размерам + 500, None, 1, True, # min_size, max_size, count_counturs, is_max_size + # Проверка, что контур не косается границы (roi_full, 0) - нет проверки + roi_full, 0, # border, border_offset + # Сортировка пол количеству граней + 0.01, True, 1, # aprox_k is_max_vertices, count_aproxs + ], + }, # "yellow_gate": [((99, 122, 101), (154, 255, 255)), cv2.COLOR_RGB2HSV_FULL, roi_corners, False], # "blue_bou": [((0, 0, 140), (360, 110, 255)), cv2.COLOR_RGB2YUV, (0, 100, 640, 210), True], # "blue_gate": [((0, 0, 140), (360, 110, 255)), cv2.COLOR_RGB2YUV, (0, 100, 640, 210), True], diff --git a/ws_nodes_py/settings/__pycache__/for_world.cpython-38.pyc b/ws_nodes_py/settings/__pycache__/for_world.cpython-38.pyc index d5136b890fa037d4e5244cada88271caa1e8d60a..abb19df6977577029c8f8919f299e576fb73737c 100644 GIT binary patch delta 219 zcmcb`af^dDl$V!_0SN4C{-$wkjpEM-3FVjM7sVGBq^71s2_pm&^U|S${CTP6sHzYG2vw7_n3dSXK~{=Sp2U0t E00LY}cmMzZ delta 219 zcmcb`af^dDl$V!_0SMNF+(~2J$g9dI!IPAen4BG-oLQ8dlX{D@C^aR%AU`v&WV1h` zIwMyRNOci6keJ-hWN8+~2T@&+Uz}NznV%QM15pp*74ZPo`Dt?A;w{fF%1MdOO)Sbz zEh@e>`4y9~?=Aj(kWhX}eo=gJL27EsEn$Q}VqQ8_kUuZA990!U0HJDf7PAtY49ICR JlP58s000+qO*#Mo diff --git a/ws_nodes_py/settings/for_world.py b/ws_nodes_py/settings/for_world.py index eefa022..3080e5b 100644 --- a/ws_nodes_py/settings/for_world.py +++ b/ws_nodes_py/settings/for_world.py @@ -4,7 +4,7 @@ world_markers = { "blue_gate": [8, 0], "blue_bou": [10, 0], "black_position": [10, 10], - "red_point": [9, 3], #???? + "red_position": [9, 3], #???? "red_poster": [10, 0], "trash": [0, 10], } diff --git a/ws_nodes_py/settings/robot_settings.py b/ws_nodes_py/settings/robot_settings.py index 22cc66e..1b9affb 100644 --- a/ws_nodes_py/settings/robot_settings.py +++ b/ws_nodes_py/settings/robot_settings.py @@ -1,4 +1,4 @@ -is_real = False +is_real = True NS = 'ws11' @@ -11,7 +11,7 @@ cameras_ports = { "2.3.1": "camera_front", # в хабе "2.3.2": "camera_side", # в хабе - "2.3.3": "camera_down", # в хабе + "2.3.5": "camera_down", # в хабе } diff --git a/ws_nodes_py/state_machine.py b/ws_nodes_py/state_machine.py index f7a9e5f..724424a 100644 --- a/ws_nodes_py/state_machine.py +++ b/ws_nodes_py/state_machine.py @@ -353,33 +353,55 @@ def main(): } drop_cube_mission = { - "black-circle": { + # "black-circle": { + # "type": "moveTo", + # "pos": world_markers["black_position"], + # "is_real_compass": False, + # "is_update_pos": False, + # "dist_threshold": 0.11, + # "navigation_marker": "black_position_front", + # }, + # 'drop-box-1':{ + # 'type':'drop-box', + # 'time': 15, + # 'state': 150, + # }, + # 'drop-box-2':{ + # 'type':'drop-box', + # 'time': 5, + # 'state': 0, + # }, + "red-circle": { "type": "moveTo", - "pos": world_markers["black_position"], + "pos": world_markers["red_position"], "is_real_compass": False, "is_update_pos": False, - "dist_threshold": 0.10, - "navigation_marker": "black_position", - }, - 'drop-box-1':{ - 'type':'drop-box', - 'time': 15, - 'state': 150, + "dist_threshold": 0.2, + "navigation_marker": "red_position", }, - 'drop-box-2':{ - 'type':'drop-box', + 'drop-stakan-1':{ + 'type':'drop-stakan', 'time': 5, + 'state': 1, + }, + 'drop-stakan-2':{ + 'type':'drop-stakan', + 'time': 40, 'state': 0, }, + 'sleep-1':{ + 'type':'wait', + 'time': 10, + }, } drop_stakan_mission = { "red-circle": { "type": "moveTo", - "pos": [9, 3], + "pos": world_markers["red_position"], "is_real_compass": False, - "dist_threshold": 0.14, - "navigation_marker": "red_point", + "dist_threshold": 0.1, + "navigation_marker": "red_position", }, 'drop-stakan-1':{ 'type':'drop-stakan', @@ -461,7 +483,7 @@ def main(): }, "red-circle": { "type": "moveTo", - "pos": world_markers["red_point"], + "pos": world_markers["red_position"], "is_real_compass": False, "is_update_pos": False, "dist_threshold": 0.1, @@ -469,9 +491,9 @@ def main(): }, } - mission = vlad24_mission + # mission = vlad24_mission # mission = square_mission - # mission = drop_cube_mission + mission = drop_cube_mission with sm: c.get_logger().info("---!---") smach.StateMachine.add('STARTING', Start(c), diff --git a/ws_nodes_py/videoD.py b/ws_nodes_py/videoD.py index fecc09c..f4c5bce 100644 --- a/ws_nodes_py/videoD.py +++ b/ws_nodes_py/videoD.py @@ -13,8 +13,7 @@ class RosOpencvToBinary(CVCamera): self.meter_per_pix = (tan(radians(self.fov/2)) * self.pool_depth) / (self.width / 2) def image_processing(self, frame): - # self.publish_coordinte(self.get_bottom, "red_position", frame, True) - pass + self.publish_coordinte(self.get_bottom, "red_position", frame, True) def main(args=None): diff --git a/ws_nodes_py/videoF.py b/ws_nodes_py/videoF.py index 8d42a7d..0bacf28 100644 --- a/ws_nodes_py/videoF.py +++ b/ws_nodes_py/videoF.py @@ -12,9 +12,9 @@ class RosOpencvToBinary(CVCamera): self.k2 = self.get_declare_parameter('k2', rclpy.Parameter.Type.DOUBLE, checker=lambda x: True) def image_processing(self, frame): - self.publish_coordinte(self.get_poster, "red_poster", frame, False) - self.publish_coordinte(self.get_poster, "green_poster", frame, False) - self.publish_coordinte(self.get_island, "black_position", frame, True) + # self.publish_coordinte(self.get_poster, "red_poster", frame, False) + # self.publish_coordinte(self.get_poster, "green_poster", frame, False) + self.publish_coordinte(self.get_island, "black_position_front", frame, True) self.publish_coordinte(self.get_gates, "yellow_gate", frame, True) self.publish_coordinte(self.get_gates, "blue_gate", frame, True) diff --git a/ws_nodes_py/videoS.py b/ws_nodes_py/videoS.py index d42894a..d89ade4 100644 --- a/ws_nodes_py/videoS.py +++ b/ws_nodes_py/videoS.py @@ -14,7 +14,7 @@ class RosOpencvToBinary(CVCamera): self.k2 = self.get_declare_parameter('k2', rclpy.Parameter.Type.DOUBLE, checker=lambda x: True) def image_processing(self, frame): - self.publish_coordinte(self.get_island, "black_position", frame, True) + self.publish_coordinte(self.get_island, "black_position_side", frame, True) self.publish_coordinte(self.get_bous, "blue_bou", frame, True) diff --git a/ws_nodes_py/world.py b/ws_nodes_py/world.py index 5653013..61313f7 100644 --- a/ws_nodes_py/world.py +++ b/ws_nodes_py/world.py @@ -91,7 +91,7 @@ class World(GetParameterNode): def update_position(self, msg): if self.navigation_marker != "" and self.navigation_marker in msg.header.frame_id: - obj_pos = self.world_markers[self.navigation_marker] + obj_pos = self.world_markers[self.navigation_marker.replace("_front", "").replace("_side", "")] x = obj_pos[0] - msg.point.x * cos(radians(msg.point.z + self.heading)) y = obj_pos[1] - msg.point.x * sin(radians(msg.point.z + self.heading)) self.position = [x, y] -- GitLab From b50de71f880136654bb0c496d81caeccfed0b8f9 Mon Sep 17 00:00:00 2001 From: "toropov.nik" Date: Sun, 6 Oct 2024 00:10:18 +0300 Subject: [PATCH 13/23] remake state machine and make executing first step by visual navigation --- setup.py | 8 +- ws_nodes_py/{ => CVs}/CVCamera.py | 2 +- ws_nodes_py/{ => CVs}/videoD.py | 2 +- ws_nodes_py/{ => CVs}/videoF.py | 3 +- ws_nodes_py/{ => CVs}/videoS.py | 4 +- ws_nodes_py/StateMachine/Controller.py | 145 +++++ ws_nodes_py/StateMachine/DefaultStates.py | 59 ++ ws_nodes_py/StateMachine/MissionMachine.py | 73 +++ ws_nodes_py/StateMachine/MotionStates.py | 117 ++++ ws_nodes_py/StateMachine/SpecialStates.py | 81 +++ ws_nodes_py/StateMachine/state_machine.py | 13 + ws_nodes_py/StateMachine/test.py | 5 + .../__pycache__/regulator.cpython-310.pyc | Bin 5503 -> 5561 bytes ws_nodes_py/__pycache__/world.cpython-310.pyc | Bin 4633 -> 4679 bytes ws_nodes_py/default/ObjectsDetection.py | 27 +- .../ObjectsDetection.cpython-310.pyc | Bin 6050 -> 6385 bytes ws_nodes_py/regulator.py | 5 +- .../__pycache__/for_world.cpython-310.pyc | Bin 1120 -> 1120 bytes .../robot_settings.cpython-310.pyc | Bin 331 -> 331 bytes ws_nodes_py/settings/mission.py | 153 +++++ ws_nodes_py/settings/robot_settings.py | 2 +- ws_nodes_py/state_machine.py | 531 ------------------ ws_nodes_py/world.py | 2 +- 23 files changed, 687 insertions(+), 545 deletions(-) rename ws_nodes_py/{ => CVs}/CVCamera.py (99%) rename ws_nodes_py/{ => CVs}/videoD.py (93%) rename ws_nodes_py/{ => CVs}/videoF.py (94%) rename ws_nodes_py/{ => CVs}/videoS.py (86%) create mode 100644 ws_nodes_py/StateMachine/Controller.py create mode 100644 ws_nodes_py/StateMachine/DefaultStates.py create mode 100644 ws_nodes_py/StateMachine/MissionMachine.py create mode 100644 ws_nodes_py/StateMachine/MotionStates.py create mode 100644 ws_nodes_py/StateMachine/SpecialStates.py create mode 100644 ws_nodes_py/StateMachine/state_machine.py create mode 100644 ws_nodes_py/StateMachine/test.py create mode 100644 ws_nodes_py/settings/mission.py delete mode 100644 ws_nodes_py/state_machine.py diff --git a/setup.py b/setup.py index fd9415f..4356ffe 100644 --- a/setup.py +++ b/setup.py @@ -30,11 +30,11 @@ setup( 'regulator = ws_nodes_py.regulator:main', 'to_berlin = ws_nodes_py.legacy.to_berlin:main', # mission - 'camcvF = ws_nodes_py.videoF:main', - 'camcvS = ws_nodes_py.videoS:main', - 'camcvD = ws_nodes_py.videoD:main', + 'camcvF = ws_nodes_py.CVs.videoF:main', + 'camcvS = ws_nodes_py.CVs.videoS:main', + 'camcvD = ws_nodes_py.CVs.videoD:main', 'world = ws_nodes_py.world:main', - 'state_machine = ws_nodes_py.state_machine:main', + 'state_machine = ws_nodes_py.StateMachine.state_machine:main', ], }, ) diff --git a/ws_nodes_py/CVCamera.py b/ws_nodes_py/CVs/CVCamera.py similarity index 99% rename from ws_nodes_py/CVCamera.py rename to ws_nodes_py/CVs/CVCamera.py index eb01329..b8aefeb 100644 --- a/ws_nodes_py/CVCamera.py +++ b/ws_nodes_py/CVs/CVCamera.py @@ -24,7 +24,7 @@ class CVCamera(GetParameterNode, ObjectsDetection): self.__pub_debug = self.create_publisher(CompressedImage, f"cv/{camera_topic_name}" , 10) self.subscribtion(CompressedImage, "topic_camera", self.__img_callback, checker=lambda x: x) - self.__pub_pos = self.publisher_from_declare(PointStamped, "object_position", checker=lambda x: x, count=10) + self.__pub_pos = self.publisher_from_declare(PointStamped, "object_position_topic", checker=lambda x: x, count=10) self.br = CvBridge() self.get_logger().info(f"CV Start {camera_topic_name} {camera_angle=}") diff --git a/ws_nodes_py/videoD.py b/ws_nodes_py/CVs/videoD.py similarity index 93% rename from ws_nodes_py/videoD.py rename to ws_nodes_py/CVs/videoD.py index f4c5bce..5dfa460 100644 --- a/ws_nodes_py/videoD.py +++ b/ws_nodes_py/CVs/videoD.py @@ -1,7 +1,7 @@ #! /usr/bin/env python3 import rclpy -from ws_nodes_py.CVCamera import CVCamera +from ws_nodes_py.CVs.CVCamera import CVCamera from ws_nodes_py.settings.robot_settings import is_real from math import radians, tan diff --git a/ws_nodes_py/videoF.py b/ws_nodes_py/CVs/videoF.py similarity index 94% rename from ws_nodes_py/videoF.py rename to ws_nodes_py/CVs/videoF.py index 0bacf28..f115901 100644 --- a/ws_nodes_py/videoF.py +++ b/ws_nodes_py/CVs/videoF.py @@ -1,8 +1,7 @@ #! /usr/bin/env python3 import rclpy -from ws_nodes_py.CVCamera import CVCamera -import cv2 +from ws_nodes_py.CVs.CVCamera import CVCamera class RosOpencvToBinary(CVCamera): diff --git a/ws_nodes_py/videoS.py b/ws_nodes_py/CVs/videoS.py similarity index 86% rename from ws_nodes_py/videoS.py rename to ws_nodes_py/CVs/videoS.py index d89ade4..5860486 100644 --- a/ws_nodes_py/videoS.py +++ b/ws_nodes_py/CVs/videoS.py @@ -1,8 +1,7 @@ #! /usr/bin/env python3 import rclpy -from ws_nodes_py.CVCamera import CVCamera -import cv2 +from ws_nodes_py.CVs.CVCamera import CVCamera class RosOpencvToBinary(CVCamera): @@ -16,6 +15,7 @@ class RosOpencvToBinary(CVCamera): def image_processing(self, frame): self.publish_coordinte(self.get_island, "black_position_side", frame, True) self.publish_coordinte(self.get_bous, "blue_bou", frame, True) + self.publish_coordinte(self.get_bous, "yellow_bou", frame, True) def main(args=None): diff --git a/ws_nodes_py/StateMachine/Controller.py b/ws_nodes_py/StateMachine/Controller.py new file mode 100644 index 0000000..3655c03 --- /dev/null +++ b/ws_nodes_py/StateMachine/Controller.py @@ -0,0 +1,145 @@ +#!/usr/bin/env python +from math import pi, copysign, atan2, sin, cos, radians, degrees, acos + +import rclpy +import rclpy.duration +from rclpy.node import Node +from std_msgs.msg import Bool, Int16, String + + +from geometry_msgs.msg import Point, PointStamped +from std_msgs.msg import Bool + + +class Controller(Node): + enabled_ = False + + def __init__(self): + super().__init__('state_machine_teleop') + self.get_logger().info('State machine started') + + self.position = Point() + + self.goal = Point() + self.move_type = "STOP" + self.is_real_compass = True + self.is_update_pos = True + self.navigation_marker = "" + self.detected_objects = [] + self.detected_objects_lifetime = 1 # int sec + + self.running = True + + self.heading = 0 + self.declare_parameter('move_type_topic', rclpy.Parameter.Type.STRING) + self.declare_parameter('goal_topic', rclpy.Parameter.Type.STRING) + self.declare_parameter('position_topic', rclpy.Parameter.Type.STRING) + self.declare_parameter('heading_topic', rclpy.Parameter.Type.STRING) + self.declare_parameter('compass_type_topic', rclpy.Parameter.Type.STRING) + self.declare_parameter('reset_topic', rclpy.Parameter.Type.STRING) + self.declare_parameter('hz', rclpy.Parameter.Type.INTEGER) + self.declare_parameter('object_position_topic', rclpy.Parameter.Type.STRING) + + + self.pub_move_type = self.create_publisher(String, self.get_declare_parameter('move_type_topic', lambda x: x, "no move type is described"), 10) + self.pub_goal = self.create_publisher(Point, self.get_declare_parameter('goal_topic', lambda x: x, "no goal_topic is described"), 10) + self.pub_compass_type = self.create_publisher(Bool, self.get_declare_parameter('compass_type_topic', lambda x: x, "no compass_type_topic is described"), 10) + self.pub_reset_topic = self.create_publisher(String, self.get_declare_parameter('reset_topic', lambda x: x, "no reset_topic is described"), 10) + self.pub_navigation_marker = self.create_publisher(String, 'navigation_marker', 10) + self.drop_stakan = self.create_publisher(Int16, "modbus/auto_probe_drop/write", 10) + self.drop_box = self.create_publisher(Int16, "modbus/drop_box/write", 10) + + self.create_subscription(Point, self.get_declare_parameter('position_topic', lambda x: x, "no pos is described"), self.pos_callback, 10) + self.create_subscription(Int16, self.get_declare_parameter('heading_topic', lambda x: x, "no heading is described"), self.heading_callback, 10) + self.create_subscription(PointStamped, self.get_declare_parameter('object_position_topic', lambda x: x, "no object_position is described"), self.obj_callback, 10) + self.is_grabbed = True + self.create_subscription(Int16, "modbus/probe_drop_sensor", self.probe_sensor_callback, 10) + self.create_subscription(Bool, 'start', self.handle_start, 1) + + # self.create_timer(1 / self.get_declare_parameter('hz', lambda x: x > 0, "no hz is described"), self.update) + + def __remove_old_detected_objects(self): + for i in range(len(self.detected_objects) - 1, -1, -1): + if self.detected_objects[i].header.stamp.sec + self.detected_objects_lifetime < self.get_clock().now().seconds_nanoseconds()[0]: + self.detected_objects.pop(i) + + def obj_callback(self, msg: PointStamped): + for i in range(len(self.detected_objects)): + if self.detected_objects[i].header.frame_id == msg.header.frame_id: + self.detected_objects[i] = msg + break + else: + self.detected_objects.append(msg) + + def get_detected_objects(self): + self.__remove_old_detected_objects() + return self.detected_objects + + def set_goal(self, x, y, z=0.0): + self.goal.x = float(x) + self.goal.y = float(y) + self.goal.z = float(z) + + def update(self): + # self.get_logger().info(f"{self.move_type} [{self.goal.x} {self.goal.y}] {self.is_real_compass} {self.navigation_marker}") + self.pub(self.pub_move_type, String, self.move_type) + self.pub_goal.publish(self.goal) + self.pub(self.pub_compass_type, Bool, self.is_real_compass) + self.pub(self.pub_navigation_marker, String, self.navigation_marker) + + def next_step(self, move_type: str=None, goal: list=None, is_real_compass: bool=None, navigation_marker: str=None, is_update_pos: bool=None): + if move_type is not None: + self.move_type = move_type + + if goal is not None: + self.set_goal(*goal) + + if is_real_compass is not None: + self.is_real_compass = is_real_compass + + if navigation_marker is not None: + self.navigation_marker = navigation_marker + + if is_update_pos is not None: + self.is_update_pos = is_update_pos + self.pub(self.pub_reset_topic, String, "update_pos" if self.is_update_pos else "fix_pos") + + self.update() + + def handle_start(self, d): + if d.data: + self.enabled_ = d.data + + def pos_callback(self, pos): + self.position = pos + + def heading_callback(self, msg): + self.heading = msg.data + + def get_declare_parameter(self, name, checker, error_text): + value = self.get_parameter(name).value + if not checker(value): + self.get_logger().error(error_text) + raise Exception(error_text) + return value + + def pub(self, topic, data_type, data: dict): + msg = data_type() + if type(data) != dict: + msg.data = data + else: + for key, value in data.items(): + setattr(msg, key, value) + topic.publish(msg) + + def probe_sensor_callback(self, msg): + self.is_grabbed = bool(msg.data) + + def child_term_cb(self, outcome_map): + self.running = not any(value == "complite" for value in outcome_map.values()) + + def reset_running(self): + self.running = True + + def shutdown(self): + return not self.running diff --git a/ws_nodes_py/StateMachine/DefaultStates.py b/ws_nodes_py/StateMachine/DefaultStates.py new file mode 100644 index 0000000..572745b --- /dev/null +++ b/ws_nodes_py/StateMachine/DefaultStates.py @@ -0,0 +1,59 @@ +from rclpy.duration import Duration +import rclpy.duration +from smach import State + + +class Start(State): + def __init__(self, c): + State.__init__(self, outcomes=['complite']) + self.cntr = c + + def execute(self, userdata): + self.cntr.next_step("STOP", [0, 0]) + self.cntr.update() + while not self.cntr.shutdown(): + rclpy.spin_once(self.cntr) + if self.cntr.enabled_: + self.cntr.get_logger().info('start') + break + return 'complite' + + +class Wait(State): + def __init__(self, c, time:int): + State.__init__(self, outcomes=['complite']) + self.cntr = c + self.time = time + + def execute(self, userdata): + self.state_change_time = self.cntr.get_clock().now() + while not self.cntr.shutdown(): + # rclpy.spin_once(self.cntr) + if (self.cntr.get_clock().now() - self.state_change_time) > Duration(seconds=self.time): + self.cntr.get_logger().info('timeout') + break + return 'complite' + + +class Finish(State): + def __init__(self, c): + State.__init__(self, outcomes=['complite']) + self.cntr = c + + def execute(self, userdata): + self.cntr.next_step("STOP", [0, 0]) + self.cntr.update() + self.cntr.enabled_ = False + self.cntr.get_logger().info('end') + return 'complite' + + +class ResetConcurance(State): + def __init__(self, c): + State.__init__(self, outcomes=['complite']) + self.cntr = c + + def execute(self, userdata): + self.cntr.reset_running() + self.cntr.get_logger().info(f'reset_running {not self.cntr.shutdown()}') + return 'complite' \ No newline at end of file diff --git a/ws_nodes_py/StateMachine/MissionMachine.py b/ws_nodes_py/StateMachine/MissionMachine.py new file mode 100644 index 0000000..5ac4476 --- /dev/null +++ b/ws_nodes_py/StateMachine/MissionMachine.py @@ -0,0 +1,73 @@ +#!/usr/bin/env python + +import rclpy +from ws_nodes_py.StateMachine.Controller import Controller +from ws_nodes_py.StateMachine.DefaultStates import Start, Finish, ResetConcurance +from smach import StateMachine, Concurrence, State +from smach_ros import IntrospectionServer + +class MissionMachine: + def __init__(self, outcomes:list=['exit'], server_name:str="tree", path:str="/SM_ROOT"): + rclpy.init() + self.controller = Controller() + self.sm = StateMachine(outcomes=outcomes) + self.goal_id = 1 + self.server_name = server_name + self.path = path + + def load_mission(self, mission: list): + with self.sm: + self.__add_start() + for step in mission: + if not len(step): + raise Exception("Empty step on mission") + elif type(step[0]) is list: + self.__add_concurrence(step) + elif issubclass(step[0], State): + self.__add_state(step[0], step[1]) + self.__add_finish() + self.__start_interuption_server() + + def __add_state(self, state, kwargs:dict={}, outcomes:str=None): + StateMachine.add(f'GOAL-{self.goal_id}', self.__init_state(state, kwargs), transitions={'complite': f'GOAL-{self.goal_id + 1}' if outcomes is None else outcomes}) + self.goal_id += 1 + + def __add_concurrence(self, states:list): + outcome_map = {} + for i in range(len(states)): + outcome_map[f"state-{i}"] = "complite" + + def child_term_cb(outcome_map): + return any(value == "complite" for value in outcome_map.values()) + + sm_con = Concurrence(outcomes=['processing', 'complite'], default_outcome='processing', outcome_map={'complite': outcome_map}, + child_termination_cb=self.controller.child_term_cb + ) + + with sm_con: + for i in range(len(states)): + Concurrence.add(f"state-{i}", self.__init_state(states[i][0], states[i][1])) + + StateMachine.add(f"GOAL-{self.goal_id}", sm_con, transitions={f"processing": f"GOAL-{self.goal_id}", "complite": f"GOAL-{self.goal_id + 1}"}) + self.goal_id += 1 + + self.__add_reset() + + def __init_state(self, state, kwargs): + return state(self.controller, **kwargs) + + def __start_interuption_server(self): + self.sis = IntrospectionServer(self.server_name, self.sm, self.path) + self.sis.start() + + def __add_start(self): + self.__add_state(Start) + + def __add_finish(self): + self.__add_state(Finish, {}, "exit") + + def __add_reset(self): + self.__add_state(ResetConcurance) + + def execute(self): + self.sm.execute() diff --git a/ws_nodes_py/StateMachine/MotionStates.py b/ws_nodes_py/StateMachine/MotionStates.py new file mode 100644 index 0000000..382f593 --- /dev/null +++ b/ws_nodes_py/StateMachine/MotionStates.py @@ -0,0 +1,117 @@ +import rclpy.duration +from math import pi, copysign, atan2, sin, cos, radians, degrees, acos +from ws_nodes_py.settings.for_world import world_markers +from smach import State + + + +class MoveToGoal(State): + def __init__(self, c, goal:str | list=[0, 0], dist_treshold: float=None, navigation_marker: str=None, is_real_compass: bool=None, is_update_pos: bool=None): + State.__init__(self, outcomes=['complite'], input_keys=['sm_counter'], + output_keys=['sm_goal', 'sm_pos', 'sm_error_dist', 'sm_error_angle'],) + + self.cntr = c + # Получаем координаты объекта из данных о метке или задём координаты вручную + self.goal = world_markers[goal] if type(goal) is str else goal + self.distance_threshold = dist_treshold + self.navigation_marker = navigation_marker + self.is_real_compass = is_real_compass + self.is_update_pos = is_update_pos + self.__is_started = False + + def execute(self, userdata): + if not self.__is_started: + self.cntr.next_step("MOVE TO", self.goal, self.is_real_compass, self.navigation_marker, self.is_update_pos) + self.__is_started = True + + while not self.cntr.shutdown(): + self.cntr.update() + rclpy.spin_once(self.cntr) + + error_dist = ((self.goal[0] - self.cntr.position.x)**2 + (self.goal[1] - self.cntr.position.y)**2)**0.5 + userdata.sm_error_angle = f'{round(self.angle_error(self.angle_point_error(), self.cntr.heading), 0)} deg' + userdata.sm_goal = f'{round(self.goal[0], 1)} m\t{round(self.goal[1], 1)} m' + userdata.sm_pos = f'{round(self.cntr.position.x, 1)} m\t{round(self.cntr.position.y, 1)} m' + userdata.sm_error_dist = f'{round(error_dist, 1)} m' + + if self.distance_threshold is not None and error_dist < self.distance_threshold: + break + return 'complite' + + def angle_point_error(self): + x1, y1 = self.cntr.position.x, self.cntr.position.y + x2, y2 = self.goal[0], self.goal[1] + dx, dy = x2 - x1, y2 - y1 + ln = (dx**2 + dy**2)**0.5 + if ln: + dxn = dx/ln + dyn = dy/ln + ang = degrees(acos(dxn)) + if dyn < 0: + ang = 360 - ang + return ang + return 0 + + def angle_error(self, target_angle, source_angle): + result = (source_angle - target_angle) % 360 + if result > 180: + result -= 360 + return result + + +class MoveByCircle(State): + def __init__(self, c, goal:str | list=[0, 0], dist:int=1, out_angle:int=None, navigation_marker: str=None, is_real_compass: bool=None, is_update_pos: bool=None): + State.__init__(self, outcomes=['complite'], input_keys=['sm_counter'], + output_keys=['sm_goal', 'sm_pos', 'sm_error_dist', 'sm_error_angle'],) + + self.cntr = c + # Получаем координаты объекта из данных о метке или задём координаты вручную + self.goal = world_markers[goal] if type(goal) is str else goal + self.goal.append(dist) + self.out_angle = out_angle + self.navigation_marker = navigation_marker + self.is_real_compass = is_real_compass + self.is_update_pos = is_update_pos + + def execute(self, userdata): + self.cntr.next_step("MOVE CIRCLE", self.goal, self.is_real_compass, self.navigation_marker, self.is_update_pos) + while not self.cntr.shutdown(): + self.cntr.update() + rclpy.spin_once(self.cntr) + + if self.out_angle is not None: + error_angle = self.angle_error(self.out_angle, self.cntr.heading) + else: + error_angle = 0 + + error_dist = ((self.goal[0] - self.cntr.position.x)**2 + (self.goal[1] - self.cntr.position.y)**2)**0.5 + userdata.sm_error_angle = f'{round(error_angle, 0)} deg' + userdata.sm_goal = f'{round(self.goal[0], 1)} m\t{round(self.goal[1], 1)} m' + userdata.sm_pos = f'{round(self.cntr.position.x, 1)} m\t{round(self.cntr.position.y, 1)} m' + userdata.sm_error_dist = f'{round(error_dist, 1)} m' + + if self.out_angle is not None and error_angle < 5: + break + return 'complite' + + def angle_error(self, target_angle, source_angle): + result = (source_angle - target_angle) % 360 + if result > 180: + result -= 360 + return result + + +class MoveByTwist(State): + def __init__(self, c, goal:list=[0, 0]): + State.__init__(self, outcomes=['complite'], input_keys=['sm_counter'], + output_keys=['sm_goal', 'sm_pos', 'sm_error_dist', 'sm_error_angle'],) + + self.cntr = c + self.goal = goal + + def execute(self, userdata): + self.cntr.next_step("MOVE TWIST", self.goal) + while not self.cntr.shutdown(): + self.cntr.update() + rclpy.spin_once(self.cntr) + return 'complite' diff --git a/ws_nodes_py/StateMachine/SpecialStates.py b/ws_nodes_py/StateMachine/SpecialStates.py new file mode 100644 index 0000000..ad4f7dd --- /dev/null +++ b/ws_nodes_py/StateMachine/SpecialStates.py @@ -0,0 +1,81 @@ +from rclpy.duration import Duration +import rclpy.duration +from std_msgs.msg import Int16 +from smach import State + + + +class DropPickStakan(State): + def __init__(self, c, time, state:int, is_wait_pickup:bool=None): + State.__init__(self, outcomes=['complite']) + self.cntr = c + + self.state_change_time = self.cntr.get_clock().now() + self.time = time + self.state = state + self.is_wait_pickup = is_wait_pickup if is_wait_pickup is not None else False + + def execute(self, userdata): + self.cntr.get_logger().info(f'{"Dropping" if self.state == 1 else "Picking"} stakan') + while not self.cntr.shutdown(): + # rclpy.spin_once(self.cntr) + if (self.cntr.get_clock().now() - self.state_change_time) > Duration(seconds=self.time) or (self.state == 0 and self.cntr.is_grabbed): + self.cntr.get_logger().info(f'Stakan is {"dropped" if self.state == 1 else "picked up"}') + if not self.is_wait_pickup: + self.cntr.get_logger().info("Stakan will be automatically picked up along the way") + msg = Int16() + msg.data = 0 + self.cntr.drop_stakan.publish(msg) + break + else: + msg = Int16() + msg.data = self.state + self.cntr.drop_stakan.publish(msg) + return 'complite' + + +class DropBox(State): + def __init__(self, c, time: int=5): + State.__init__(self, outcomes=['complite']) + self.cntr = c + + self.time = time + self.state_change_time = self.cntr.get_clock().now() + + def execute(self, userdata): + self.cntr.get_logger().info(f'Droping box') + while not self.cntr.shutdown(): + # rclpy.spin_once(self.cntr) + if (self.cntr.get_clock().now() - self.state_change_time) > Duration(seconds=self.time): + self.cntr.get_logger().info('Box is dropped') + msg = Int16() + msg.data = 0 + self.cntr.drop_box.publish(msg) + break + else: + msg = Int16() + msg.data = 150 + self.cntr.drop_box.publish(msg) + return 'complite' + + +class FindObject(State): + def __init__(self, c, object_name: str="", min_max_angle: list=[0, 359]): + State.__init__(self, outcomes=['complite']) + self.cntr = c + + self.object_name = object_name + self.min_angle, self.max_angle = min_max_angle + + def execute(self, userdata): + self.cntr.get_logger().info(f'Start seraching {self.object_name} in [{self.min_angle}; {self.max_angle}] deg') + while not self.cntr.shutdown(): + # rclpy.spin_once(self.cntr) + # self.cntr.get_logger().info(" ".join(obj.header.frame_id for obj in self.cntr.detected_objects)) + for obj in self.cntr.get_detected_objects(): + if self.object_name in obj.header.frame_id: + if ((self.max_angle > self.min_angle and self.max_angle >= (obj.point.z % 360) >= self.min_angle or + self.max_angle < self.min_angle and (self.min_angle <= (obj.point.z % 360) or self.max_angle >= (obj.point.z % 360)))): + self.cntr.get_logger().info(f'Detect {self.object_name} at {obj.point.z % 360} deg') + return 'complite' + return 'complite' diff --git a/ws_nodes_py/StateMachine/state_machine.py b/ws_nodes_py/StateMachine/state_machine.py new file mode 100644 index 0000000..52d9598 --- /dev/null +++ b/ws_nodes_py/StateMachine/state_machine.py @@ -0,0 +1,13 @@ +#!/usr/bin/env python + +from ws_nodes_py.settings.mission import mission +from ws_nodes_py.StateMachine.MissionMachine import MissionMachine + + +def main(): + mm = MissionMachine() + mm.load_mission(mission) + mm.execute() + +if __name__ == '__main__': + main() diff --git a/ws_nodes_py/StateMachine/test.py b/ws_nodes_py/StateMachine/test.py new file mode 100644 index 0000000..bc848fa --- /dev/null +++ b/ws_nodes_py/StateMachine/test.py @@ -0,0 +1,5 @@ +for i in range(10): + # break + a = 0 +else: + print("HELLOW") \ No newline at end of file diff --git a/ws_nodes_py/__pycache__/regulator.cpython-310.pyc b/ws_nodes_py/__pycache__/regulator.cpython-310.pyc index ff0ba2d18db12eda10dfce43da4f4faf52623581..1f37fafc87416d0ed4be833f511c39f3763ab600 100644 GIT binary patch delta 750 zcmZuvJ&)5s5Z!ScJMnjHCvjpBhd3lcL_-IN5(J0fiaS9AI-RsA!Z{K!h9ZoW!ZdWC zEdZTB$rV70QXx^&QqtT%prWNaW*sC>2(R??X7|m^&hs9e{@i!Qwrw!_UXNiAcRo2M zEchkPT2H2zQAsp35^GB%rW3Qb95gljiLasHV4(%^Fyr zgUNDpk1E!Al{5+0N5E?#DKdW+WFe{40tn3l3I)x%k}Ml$Ue?T7vUmZghm4xk zhb(uB7;R>Ww#g&xHgcgZVH^qiAtTym_79qdMoH;Bh%Ks;x&BizUK zuEGNh2P4U!;p(S29$EU6AD0U0jWp68U0QT1^4$@NcJuZEZOS|Yl^Y%YhMWt1f zI+HJ;*SV3d(VI!H5loEh>o+g0Y~H%Mu{qI~d_4s^`B9NTO!VD3JQUyDFX6tnb8mEa zJKfi}M}x;ZL|`JGd#hn;%7T`Ja}q8PEWUevh#n9-1*>4u(i8*aQZE``h3B8II#2^& htosXiDJsD`xFYs~5Aa5;g*`|XS*U_;)O1QX_zkzgwDbS~ delta 651 zcmZvZ&ubGw6vyYS)7@l$Ws}`aHd#f)7FvrGys3Cpq!m4Q5TQ^5W1~sg!mxUfw|LlI zyohB`4@F7v;7yQR6!dQpJO^{9=Qd`ey0m*qaS>Y(+z)&B|! zvnrHqnP^{U)ZQK*dNy=KePlcVc}wkk$JfH5A?zX$T=0QWG=(GFJyx{P6THjN)tWFm znbpZu-@N#S9_1^BRU%3mIsHnGD#RHVW>qWByLk^|ozi0zZSs^O5KMj-9<~!kcWn&h z58)G*hPvi2o%RV%54+e)m5y z%<5-@dpmtOg9p;lAy>;;E$2yQ=FZ*hc3)1d<|+ht=w=xf!KG~)HmLR1xdj($5U5A3 gDLhse!k2JSeFbfbumASqv$R z*~~@mAXW`y7GoAu8AFjLkjD%XS-_IQl)|`>2}m=8Xi0`zrW&Rc7D&pO=@50SG#O{7+lDk@qU60Dpc`R%&udd_jJ3W=Up#-sB0Kj+-sHs#xmV zfa+=(vKUervzd$RL980aEXFLRGKL~YAdeX&BFRw8RKt|QBngsZ$zm;GD`5w-IDoR8 z;tb7<&5R-p5)8G>Kvm3eRa`h!vD7fvu%t89FvN4G$TI{pq(HGIi(ioo&^bknAVL60 mX!1-h;Fp~|jX#xx>*$O}kGA6SMIROAKwm9AZ diff --git a/ws_nodes_py/default/ObjectsDetection.py b/ws_nodes_py/default/ObjectsDetection.py index 2b84d3d..4c34d01 100644 --- a/ws_nodes_py/default/ObjectsDetection.py +++ b/ws_nodes_py/default/ObjectsDetection.py @@ -104,6 +104,20 @@ class ObjectsDetection: 0.01, True, 3, # aprox_k is_max_vertices, count_aproxs ], }, + "yellow_bou": { + "binary": [ + ((80, 70, 70), (100, 255, 255)), cv2.COLOR_RGB2HSV, # (min_mask, max_mask), color_space + roi_corners, False # borders, contrast + ], + "contours": [ + # Выделение контуров по размерам + 400, None, 2, True, # min_size, max_size, count_counturs, is_max_size + # Проверка, что контур не косается границы (roi_full, 0) - нет проверки + roi_full, 0, # border, border_offset + # Сортировка пол количеству граней + 0.01, True, 2, # aprox_k is_max_vertices, count_aproxs + ], + }, "blue_gate": { "binary": [((0, 83, 0), (19, 255, 255)), cv2.COLOR_RGB2HSV, (0, 100, 640, 250), False], "contours": [ @@ -126,7 +140,18 @@ class ObjectsDetection: 0.01, True, 3, # aprox_k is_max_vertices, count_aproxs ], }, - "black_position": { + "black_position_front": { + "binary": [((0, 0, 0), (180, 255, 20)), cv2.COLOR_RGB2HSV, roi_full, False], + "contours": [ + # Выделение контуров по размерам + 400, None, 2, True, # min_size, max_size, count_counturs, is_max_size + # Проверка, что контур не косается границы (roi_full, 0) - нет проверки + roi_full, 0, # border, border_offset + # Сортировка пол количеству граней + 0.01, True, 2, # aprox_k is_max_vertices, count_aproxs + ], + }, + "black_position_side": { "binary": [((0, 0, 0), (180, 255, 20)), cv2.COLOR_RGB2HSV, roi_full, False], "contours": [ # Выделение контуров по размерам diff --git a/ws_nodes_py/default/__pycache__/ObjectsDetection.cpython-310.pyc b/ws_nodes_py/default/__pycache__/ObjectsDetection.cpython-310.pyc index 7fab8a7cd187cd63e4c6e4d0081d321fdfb5859a..047d43a5805ca666eb04d703fbc29dbff5557fe6 100644 GIT binary patch delta 2127 zcma)7U2GIp6y7^CJ3IUTTehwA2bz7DQVOg5q*zN4sV0>zwx&P~j@!<3S=sJ1cXlgn zs_-Dh7o+C9X%qfbQsBYF7h_C(@l}1_KzuN2#-tj=1Of^1oV&PNTS(kwzrAzrIo~<= zoNw-4-f=q{vBTkj2R~OXutM(8N0EDhC_Wv;uX7V(>|Dr9nZANzwwN9>0dKR`QFD_p zq{(3VmLyX%{ie@M6#QmzSu$HIo;Rf>W-1uew(MK-7_#Y^@aXNi6@@e8bxne^5(0d~ z_OUSBWs`_GY4dOdw{n~+c$KFSMF5Y9=QyJ7cl(ed=MvFImv*=&b)E@Rpnx`7wNsp= z%dQs}?mGlaWRl%mv_6l(Lu63gR-|=*C4)-fLnz+FFkRt2xOVS1wrIcjH@XYvU9@?Q zV8P>oRmmT0)S&x-$=Tl2R4G4w%9yjAVx?%8jTvs2E4Ac0t5Y;B$jVhV09WOC_5yS& zeek}oRgQ$dhQrEWFo8M5Y>qLAvo3g`#J0)UaQ3L-UK~nr%}|0klmHGTL_TI1{!lu6 zYu0BXB32T^dQ$BF@rkafA@+nC;v0Pe4fO17=N0nM6__+Mn3P+W+wvH6l^-e1vP!<5SjhF7k_Gj1*=rD-!i7`DyKl+ zN6=5OgJ35@EmYu^RVMLNEY7#130bqW6FxZh^4PdB{^Ijbzcew)Nr!wl!9WA{k>68> z;aC-8zQ`+8*x`$H9Yw3Nh@dcqsj9-HhnlKLswCZ49;h+M`HrISLh{AMJ{VKd@U8C> z%R*ioV0+*btz%#>vBrvyF>7P|o+_5}eBsA0Z)@)|q?!-( zr$mvWK5ej+pPe%ELqEe;fpUhbeGp5sHSkPT0@3AULAWuv}T zxDg%==W#<C4$ax2bBm4UrM}1sbcag%u+tL&9=o*q-j~tKfp?sp* zqBAET72Ognkntpe4a3nMmI5=ndpG6Nz1${y(?hgL#ESk$>NA?QtK~|SJGbD6=$~8Z z8s?35=&a^7D+_ZLj)UF|J-|=FPqA${%V4}i6%~y`f4oN}ZSV;=8qd`G-8gYAT$kaL z%}t9t58&hYckBZ&5?fSKGarNVi4hdo_lcp52x>+q>yiIao5o;g^3qmn!+p7IcP}Qp zi{8l!s|2qQn&}3{hlFPV37nnn5(mWc| wj%cT$JDmzsgKFM#b#`unpM?YIXZjx#U(*I>ZL?akhUhwV=prVP5KGAa00X|&-%lJ>6rQ^?yED7Y&JN2il$6C%T+9G&Q-Ub_TMR{gFG1 zm56}{H6}jP=J-%6HMC9q@zR*o80|w}#Kc4&jL`@8KiK-9KA0GMz;o{6Pyr>`FL%EA z&Uel|=PiPhkeWXp~; zDOt(vCkkgMsXGKK0ZA%=5*T7Sj0UEcbtJb0Jcwr=BGnHFR+y7}mK%suycJRS@R)cu z5v_2&AFZ=>^4~3KCrQAP>mTEx{xpI}fyB3Trqoe^d>=ixOYo|kY9d;&RuHbshhbB$ zvJ6y}msvmjqV&s#{yhu?hnuJXadneHGrI>a1x>Z7D!s%U9o`P6?moLw;+F!VIYCEq z3`fN$%?UfIW6Xt}$ZEie2~DGc=6`6K|4#!~LT&lIoLg2^97jNmBa!6_T~I|3y?F`` z^)aFbh>{yq@S8AJcn6!iH3Mohk3omp-Qf$Y#&qu*-Ud9cc8WIE6BJ;=vf94<03H3P z#vo`meaP3}BB1eOe%i2Yp{VgJQ6jm3qa;*$7qJHkx)Ex|%zQ3?&aRZbLbXsX@idY8 z9CzIkl0#nA<@{dcFhvi&ik>+A#OVopV*L2P$;oMMA;)_OGCmw4-F{eU+aEiINAE=h zg()l)f{WT2_9gtG-C{lPX?T$J!JTkwu%80Vf@c@Y+_m%NQkCbtDvu+*(^j|bIk#%h z6iPY1xOVg5$m-lB zg^%2Z6=SAdxV_+8`t{w0zl_JD&*BS9smb$hiTA){?EaYVByz69h1h7nf7QLp`UWjL z{Fq3FWNB!!5qr+0U1~I2bY=?jW>*spjoSpXaKY?mEpXF3a)k0(K|V|TwnDT?#EMH9 zjTz<33#IA;_ijL{`PYMVf9|)#Vzr=CU97k`4%%)j!e<~8@55Q1ji*AQqEql%ygNj@ z!5@PUl)2+_?>nf+*V&tJqopfEi^|8~r+7?~c&N$ 0, "no hz is described"), self.update) - - def set_goal(self, x, y, z=0.0): - self.goal.x = float(x) - self.goal.y = float(y) - self.goal.z = float(z) - - def update(self): - # self.get_logger().info(f"{self.move_type} [{self.goal.x} {self.goal.y}] {self.is_real_compass} {self.navigation_marker}") - self.pub(self.pub_move_type, String, self.move_type) - self.pub_goal.publish(self.goal) - self.pub(self.pub_compass_type, Bool, self.is_real_compass) - self.pub(self.pub_navigation_marker, String, self.navigation_marker) - - def next_step(self, move_type: str, goal: list, is_real_compass: bool=None, navigation_marker: str=None, is_update_pose: bool=None): - # self.get_logger().info("WTF") - self.state_change_time = self.get_clock().now() - self.move_type = move_type - self.set_goal(*goal) - - if is_real_compass is not None: - self.is_real_compass = is_real_compass - - if navigation_marker is not None: - self.navigation_marker = navigation_marker - else: - self.navigation_marker = "" - - if is_update_pose is not None: - self.is_update_pose = is_update_pose - else: - self.is_update_pose = True - self.pub(self.pub_reset_topic, String, "update_pos" if self.is_update_pose else "fix_pos") - - self.update() - - def handle_start(self, d): - if d.data: - self.enabled_ = d.data - - def pos_callback(self, pos): - self.position = pos - - def heading_callback(self, msg): - self.heading = msg.data - - def get_declare_parameter(self, name, checker, error_text): - value = self.get_parameter(name).value - if not checker(value): - self.get_logger().error(error_text) - raise Exception(error_text) - return value - - def shutdown(self): - return False - - def pub(self, topic, data_type, data: dict): - msg = data_type() - if type(data) != dict: - msg.data = data - else: - for key, value in data.items(): - setattr(msg, key, value) - topic.publish(msg) - - def probe_sensor_callback(self, msg): - self.is_grabbed = bool(msg.data) - - - -class Start(smach.State): - def __init__(self, c, pid_enabled=False): - smach.State.__init__(self, outcomes=['start']) - self.cntr = c - self.pid_enabled = pid_enabled - - def execute(self, userdata): - self.cntr.next_step("STOP", [0, 0]) - while not self.cntr.shutdown(): - rclpy.spin_once(self.cntr) - if self.cntr.enabled_: - self.cntr.get_logger().info('finish') - return 'start' - -class Wait(smach.State): - def __init__(self, c:Controller, time:int): - smach.State.__init__(self, outcomes=['timeout']) - self.cntr = c - self.time = time - - def execute(self, userdata): - self.cntr.next_step("STOP", [0, 0]) - while not self.cntr.shutdown(): - rclpy.spin_once(self.cntr) - if (self.cntr.get_clock().now() - self.cntr.state_change_time) > Duration(seconds=self.time): - self.cntr.get_logger().info('finish') - return 'timeout' - - -class MoveToGoal(smach.State): - def __init__(self, c:Controller, time:int, goal:list, is_real_compass: bool, navigation_marker: str, dist_treshold: float, is_update_pos: bool): - smach.State.__init__(self, outcomes=['timeout'], - input_keys=['sm_counter'], - output_keys=['sm_goal', 'sm_pos', 'sm_error_dist', 'sm_error_angle'],) - self.cntr = c - - self.time = time - self.goal = goal - self.is_real_compass = is_real_compass - self.is_update_pos = is_update_pos - self.navigation_marker = navigation_marker - self.distance_threshold = dist_treshold - - - def execute(self, userdata): - self.cntr.next_step("MOVE TO", self.goal, self.is_real_compass, self.navigation_marker, self.is_update_pos) - - while not self.cntr.shutdown(): - self.cntr.update() - rclpy.spin_once(self.cntr) - if (self.cntr.get_clock().now() - self.cntr.state_change_time) > Duration(seconds=self.time): - self.cntr.get_logger().info('finish') - return 'timeout' - else: - error_dist = ((self.goal[0] - self.cntr.position.x)**2 + (self.goal[1] - self.cntr.position.y)**2)**0.5 - userdata.sm_error_angle = f'{self.angle_error(self.angle_point_error(), self.cntr.heading)} deg' - userdata.sm_goal = f'{self.goal[0]} m\t{self.goal[1]} m' - userdata.sm_pos = f'{self.cntr.position.x} m\t{self.cntr.position.y} m' - userdata.sm_error_dist = f'{error_dist} m' - - if error_dist < self.distance_threshold: - return 'timeout' - - def angle_point_error(self): - x1, y1 = self.cntr.position.x, self.cntr.position.y - x2, y2 = self.goal[0], self.goal[1] - dx, dy = x2 - x1, y2 - y1 - ln = (dx**2 + dy**2)**0.5 - if ln: - dxn = dx/ln - dyn = dy/ln - ang = degrees(acos(dxn)) - if dyn < 0: - ang = 360 - ang - return ang - return 0 - - def angle_error(self, target_angle, source_angle): - result = (source_angle - target_angle) % 360 - if result > 180: - result -= 360 - return result - - -class MoveToCircle(smach.State): - def __init__(self, c:Controller, time:int, goal:list, dist, angle:int, is_real_compass: bool, navigation_marker: str): - smach.State.__init__(self, outcomes=['timeout']) - self.cntr = c - - self.time = time - self.trg_angle = angle - self.goal = list(goal) - self.goal.append(dist) - self.is_real_compass = is_real_compass - self.navigation_marker = navigation_marker - self.distance_threshold = 0.5 - - def angle_error(self, target_angle, source_angle): - result = (source_angle - target_angle) % 360 - if result > 180: - result -= 360 - return result - - def execute(self, userdata): - self.cntr.next_step("MOVE CIRCLE", self.goal, self.is_real_compass, self.navigation_marker) - while not self.cntr.shutdown(): - rclpy.spin_once(self.cntr) - typs = String() - typs.data = 'MOVE CIRCLE' - self.cntr.pub_move_type.publish(typs) - goal = Point() - goal.x = float(self.goal[0]) - goal.y = float(self.goal[1]) - goal.z = float(self.goal[2]) - self.cntr.pub_goal.publish(goal) - if (self.cntr.get_clock().now() - self.cntr.state_change_time) > Duration(seconds=self.time): - self.cntr.get_logger().info('finish') - return 'timeout' - else: - error_angle = abs(self.angle_error(self.trg_angle, self.cntr.heading)) - # self.cntr.get_logger().info(f'{error_angle}') - if error_angle < 5: - return 'timeout' - -class Finish(smach.State): - def __init__(self, c): - smach.State.__init__(self, outcomes=['fin']) - self.cntr = c - - def execute(self, userdata): - self.cntr.next_step("STOP", [0, 0]) - while not self.cntr.shutdown(): - self.cntr.get_logger().info('end') - return 'fin' - -class DropPickStakan(smach.State): - def __init__(self, c, time, state:int): - self.time = time - self.state = state - smach.State.__init__(self, outcomes=['timeout']) - self.cntr = c - - def execute(self, userdata): - self.cntr.next_step("STOP", [0, 0]) - self.cntr.get_logger().info(f'Stakan {"drop" if self.state == 1 else "pick"}') - while not self.cntr.shutdown(): - rclpy.spin_once(self.cntr) - if (self.cntr.get_clock().now() - self.cntr.state_change_time) > Duration(seconds=self.time) or (self.state == 0 and self.cntr.is_grabbed): - self.cntr.get_logger().info('finish') - return 'timeout' - else: - msg = Int16() - msg.data = self.state - self.cntr.drop_stakan.publish(msg) - -class DropBox(smach.State): - def __init__(self, c, time, state:int): - self.time = time - self.state = state - smach.State.__init__(self, outcomes=['timeout']) - self.cntr = c - - def execute(self, userdata): - self.cntr.next_step("STOP", [0, 0]) - self.cntr.get_logger().info(f'drop box {"open" if self.state != 0 else "close"}') - while not self.cntr.shutdown(): - rclpy.spin_once(self.cntr) - if (self.cntr.get_clock().now() - self.cntr.state_change_time) > Duration(seconds=self.time): - self.cntr.get_logger().info('finish') - return 'timeout' - else: - msg = Int16() - msg.data = self.state - self.cntr.drop_box.publish(msg) - - -def main(): - rclpy.init() - c = Controller() - c.get_logger().info("---- Waiting! ----") - sm = smach.StateMachine(outcomes=['fin']) - global sis - square_mission = { - # point_name: [x, y] in meters - "point_1": { - "type": "moveTo", - "pos": [0, 2], - "is_real_compass": False, - "dist_threshold": 0.3, - # "navigation_marker": "yellow_gate", - "navigation_marker": "", - }, - "point_2": { - "type": "moveTo", - "pos": [2, 2], - "is_real_compass": False, - "dist_threshold": 0.3, - "navigation_marker": "", - }, - "point_3": { - "type": "moveTo", - "pos": [2, 0], - "is_real_compass": False, - "dist_threshold": 0.3, - "navigation_marker": "", - }, - "point_4": { - "type": "moveTo", - "pos": [0, 0], - "is_real_compass": False, - "dist_threshold": 0.3, - "navigation_marker": "", - }, - } - - circle_mission = { - "circle_1": { - "type": "moveAround", - "pos": [10, 0], - "radius": 1, - "out_heading": 120, # надо проверить - "is_real_compass": False, - }, - } - - drop_cube_mission = { - # "black-circle": { - # "type": "moveTo", - # "pos": world_markers["black_position"], - # "is_real_compass": False, - # "is_update_pos": False, - # "dist_threshold": 0.11, - # "navigation_marker": "black_position_front", - # }, - # 'drop-box-1':{ - # 'type':'drop-box', - # 'time': 15, - # 'state': 150, - # }, - # 'drop-box-2':{ - # 'type':'drop-box', - # 'time': 5, - # 'state': 0, - # }, - "red-circle": { - "type": "moveTo", - "pos": world_markers["red_position"], - "is_real_compass": False, - "is_update_pos": False, - "dist_threshold": 0.2, - "navigation_marker": "red_position", - }, - 'drop-stakan-1':{ - 'type':'drop-stakan', - 'time': 5, - 'state': 1, - }, - 'drop-stakan-2':{ - 'type':'drop-stakan', - 'time': 40, - 'state': 0, - }, - 'sleep-1':{ - 'type':'wait', - 'time': 10, - }, - } - - drop_stakan_mission = { - "red-circle": { - "type": "moveTo", - "pos": world_markers["red_position"], - "is_real_compass": False, - "dist_threshold": 0.1, - "navigation_marker": "red_position", - }, - 'drop-stakan-1':{ - 'type':'drop-stakan', - 'time': 5, - 'state': 1, - }, - 'drop-stakan-2':{ - 'type':'drop-stakan', - 'time': 40, - 'state': 0, - }, - 'sleep-1':{ - 'type':'wait', - 'time': 10, - }, - # 'drop-box-1':{ - # 'type':'drop-box', - # 'time': 15, - # 'state': 150, - # }, - # 'drop-box-2':{ - # 'type':'drop-box', - # 'time': 5, - # 'state': 0, - # }, - } - - vlad24_mission = { - "gate-1-yellow": { - "type": "moveTo", - "pos": world_markers["yellow_gate"], # "pos": [6.5, 0], - "is_real_compass": False, - "is_update_pos": False, - "dist_threshold": 0.25, - "navigation_marker": "yellow_gate", - # "navigation_marker": "", - }, - "point-left-blue": { - "type": "moveTo", - "pos": [8, -3], - "dist_threshold": 0.25, - "is_real_compass": False, - "is_update_pos": False, - "navigation_marker": "", - }, - "point-center-blue": { - "type": "moveTo", - "pos": world_markers["blue_gate"], - "dist_threshold": 0.25, - "is_real_compass": False, - "is_update_pos": False, - "navigation_marker": "blue_gate", - # "navigation_marker": "", - }, - "point-2-blue": { - "type": "moveTo", - "pos": [8, 1], - "dist_threshold": 0.25, - "is_real_compass": False, - "is_update_pos": False, - "navigation_marker": "", - }, - "gate-3-yellow": { - "type": "moveTo", - "pos": world_markers["yellow_gate"], - "dist_threshold": 0.25, - "is_real_compass": False, - "is_update_pos": False, - "navigation_marker": "yellow_gate", - # "navigation_marker": "", - }, - "move-to-start": { - "type": "moveTo", - "pos": [4, 2], - "dist_threshold": 0.25, - "is_real_compass": False, - "is_update_pos": False, - "navigation_marker": "", - }, - "red-circle": { - "type": "moveTo", - "pos": world_markers["red_position"], - "is_real_compass": False, - "is_update_pos": False, - "dist_threshold": 0.1, - "navigation_marker": "red_point", - }, - - } - # mission = vlad24_mission - # mission = square_mission - mission = drop_cube_mission - with sm: - c.get_logger().info("---!---") - smach.StateMachine.add('STARTING', Start(c), - transitions={'start': 'GOAL-1'}) - - for i, key in enumerate(mission.keys(), 1): - if mission[key]["type"] == "moveTo": - smach.StateMachine.add(f'GOAL-{i}', MoveToGoal(c, 1000, mission[key]["pos"], mission[key]["is_real_compass"], mission[key]["navigation_marker"], mission[key]["dist_threshold"], mission[key]["is_update_pos"]), - transitions={'timeout': f'GOAL-{ "FINISH" if len(mission.keys()) == i else i+1}'}) - elif mission[key]["type"] == "moveAround": - smach.StateMachine.add(f'GOAL-{i}', MoveToCircle(c, 1000, mission[key]["pos"], mission[key]["radius"], mission[key]["out_heading"], mission[key]["is_real_compass"], mission[key]["navigation_marker"]), - transitions={'timeout': f'GOAL-{ "FINISH" if len(mission.keys()) == i else i+1}'}) - elif mission[key]["type"] == "drop-stakan": - smach.StateMachine.add(f'GOAL-{i}', DropPickStakan(c, mission[key]["time"], mission[key]["state"]), - transitions={'timeout': f'GOAL-{ "FINISH" if len(mission.keys()) == i else i+1}'}) - elif mission[key]["type"] == "drop-box": - smach.StateMachine.add(f'GOAL-{i}', DropBox(c, mission[key]["time"], mission[key]["state"]), - transitions={'timeout': f'GOAL-{ "FINISH" if len(mission.keys()) == i else i+1}'}) - elif mission[key]["type"] == "wait": - smach.StateMachine.add(f'GOAL-{i}', Wait(c, mission[key]["time"]), - transitions={'timeout': f'GOAL-{ "FINISH" if len(mission.keys()) == i else i+1}'}) - - #for i, key in enumerate(mission.keys(), 1): - # smach.StateMachine.add(f'GOAL-{i}', MoveToGoal(c, 1000, mission[key]), - # transitions={'timeout': f'GOAL-{ "FINISH" if len(mission.keys()) == i else i+1}'}) - - smach.StateMachine.add('GOAL-FINISH', Finish(c), - transitions={'fin': 'STARTING'}) - - sis = smach_ros.IntrospectionServer('tree', sm, '/SM_ROOT') - sis.start() - outcome = sm.execute() - -if __name__ == '__main__': - main() diff --git a/ws_nodes_py/world.py b/ws_nodes_py/world.py index 61313f7..7e449f3 100644 --- a/ws_nodes_py/world.py +++ b/ws_nodes_py/world.py @@ -59,7 +59,7 @@ class World(GetParameterNode): self.subscribtion_for_parameter(Point, "goal_topic", 'goal_x', checker=lambda x: x, func=lambda msg: msg.x, default=0) self.subscribtion_for_parameter(String, "navigation_marker_topic", 'navigation_marker', checker=lambda x: x, func=lambda msg: msg.data, default="") self.subscribtion(String, "reset_topic", self.reset_callback, checker=lambda x: x) - self.subscribtion(PointStamped, "object_position", self.update_position, checker=lambda x: x) + self.subscribtion(PointStamped, "object_position_topic", self.update_position, checker=lambda x: x) # Имеется в виду новая насадка на мотор или нет (новая - от Кирилла (обтекаемая) или старая - от трионикса (белая)) if self.get_declare_parameter('is_new_motor', rclpy.Parameter.Type.BOOL, lambda x: True, "is_new_motor can be 0 or 1"): -- GitLab From 5c202a992d67ab1f52147bc9e324f732a011c8fd Mon Sep 17 00:00:00 2001 From: "toropov.nik" Date: Sun, 6 Oct 2024 00:25:34 +0300 Subject: [PATCH 14/23] remake compass with deque and np.median --- ws_nodes_py/compass.py | 42 ++++++++++++++++++++++++------------------ 1 file changed, 24 insertions(+), 18 deletions(-) diff --git a/ws_nodes_py/compass.py b/ws_nodes_py/compass.py index 8e37856..e82666c 100644 --- a/ws_nodes_py/compass.py +++ b/ws_nodes_py/compass.py @@ -8,6 +8,8 @@ import math from std_msgs.msg import Int16, String, Bool from geometry_msgs.msg import Twist from ws_nodes_py.settings.for_world import old_motor_ang_speed, new_motor_ang_speed +from collections import deque +from numpy import median class Compass(GetParameterNode): @@ -104,43 +106,37 @@ class Compass(GetParameterNode): else: self.angular_speed = target_angle_speed - if is_with_accel: # with accel + if is_with_accel: self.heading += self.angular_speed * dt - else: # no accel + else: self.heading += target_angle_speed * dt self.heading = self.heading % 360 - self.pub(self.pub_heading, Int16, int(self.heading if self.is_enabled else 0)) + self.__pub_heading() def compass_callback(self, msg): # Проверка на правильность преобразования assert -10%360 == 350 - if not 360 >= msg.data >= 0: + new_compass = msg.data + + if not 360 >= new_compass >= 0: return # for heading offset - self.__compass = msg.data + self.__compass = new_compass - try: - self.last_compass.pop(0) - self.last_compass.append((msg.data - self.compass_delta) % 360) - except AttributeError: # if first compass data - self.get_logger().info("start median") - self.last_compass = [msg.data - self.compass_delta, msg.data - self.compass_delta, msg.data - self.compass_delta] + self.__add_new_compass_value(self, new_compass) if (not self.is_use_real_compass and self.COMPASS_MODE == "AUTO") or self.COMPASS_MODE == "FORCE_VIRTUAL": return # median - a = self.last_compass[0] - b = self.last_compass[1] - c = self.last_compass[2] - self.heading = max(a, c) if (max(a, b) == max(b, c)) else max(b, min(a, c)) + self.heading = (median(self.last_compass) - self.compass_delta) % 360 - self.pub(self.pub_heading, Int16, int(self.heading if self.is_enabled else 0)) - # self.compass_angular_speed() + self.__pub_heading() + # self.__compass_angular_speed() def reset_callback(self, msg): if msg.data == "reset_heading" and ((self.is_use_real_compass and self.COMPASS_MODE == "AUTO") or self.COMPASS_MODE == "FORCE_REAL"): @@ -156,7 +152,7 @@ class Compass(GetParameterNode): self.heading = 0 self.is_enabled = True - def compass_angular_speed(self): + def __compass_angular_speed(self): # Разность угла [-179;180] d_head = self.heading - self.last_heading if d_head <= -180: @@ -174,6 +170,16 @@ class Compass(GetParameterNode): self.d_heads.append([ang_speed, d_time]) self.d_heads.pop(0) self.angular_speed = round(sum(spd * t for spd, t in self.d_heads) / sum(t for spd, t in self.d_heads),1) + + def __pub_heading(self): + self.pub(self.pub_heading, Int16, int(self.heading if self.is_enabled else 0)) + + def __add_new_compass_value(self, compass): + try: + self.last_compass.popleft() + self.last_compass.append(compass) + except AttributeError: # if first compass data + self.last_compass = deque([compass, compass, compass]) def main(): -- GitLab From e2a8c45e82a15bca3e03a1bc53b0fb37caf794d7 Mon Sep 17 00:00:00 2001 From: "toropov.nik" Date: Sun, 6 Oct 2024 23:44:41 +0300 Subject: [PATCH 15/23] make first mission and drop cube --- ws_nodes_py/StateMachine/DefaultStates.py | 1 - ws_nodes_py/StateMachine/MotionStates.py | 7 +- .../__pycache__/compass.cpython-310.pyc | Bin 5895 -> 6194 bytes .../__pycache__/regulator.cpython-310.pyc | Bin 5561 -> 5563 bytes ws_nodes_py/regulator.py | 23 ++- ws_nodes_py/settings/mission.py | 194 +++++++++--------- 6 files changed, 113 insertions(+), 112 deletions(-) diff --git a/ws_nodes_py/StateMachine/DefaultStates.py b/ws_nodes_py/StateMachine/DefaultStates.py index 572745b..5e8f732 100644 --- a/ws_nodes_py/StateMachine/DefaultStates.py +++ b/ws_nodes_py/StateMachine/DefaultStates.py @@ -55,5 +55,4 @@ class ResetConcurance(State): def execute(self, userdata): self.cntr.reset_running() - self.cntr.get_logger().info(f'reset_running {not self.cntr.shutdown()}') return 'complite' \ No newline at end of file diff --git a/ws_nodes_py/StateMachine/MotionStates.py b/ws_nodes_py/StateMachine/MotionStates.py index 382f593..2eef8bb 100644 --- a/ws_nodes_py/StateMachine/MotionStates.py +++ b/ws_nodes_py/StateMachine/MotionStates.py @@ -3,6 +3,9 @@ from math import pi, copysign, atan2, sin, cos, radians, degrees, acos from ws_nodes_py.settings.for_world import world_markers from smach import State +# TODO состояние движения занимается движением +# TODO убрать настройки навигации в отдельное состояние +# TODO убрать объекты, относительно которы навигируемся в отдельное состояние class MoveToGoal(State): @@ -60,14 +63,14 @@ class MoveToGoal(State): class MoveByCircle(State): - def __init__(self, c, goal:str | list=[0, 0], dist:int=1, out_angle:int=None, navigation_marker: str=None, is_real_compass: bool=None, is_update_pos: bool=None): + def __init__(self, c, goal:str | list=[0, 0], dist:int=None, out_angle:int=None, navigation_marker: str=None, is_real_compass: bool=None, is_update_pos: bool=None): State.__init__(self, outcomes=['complite'], input_keys=['sm_counter'], output_keys=['sm_goal', 'sm_pos', 'sm_error_dist', 'sm_error_angle'],) self.cntr = c # Получаем координаты объекта из данных о метке или задём координаты вручную self.goal = world_markers[goal] if type(goal) is str else goal - self.goal.append(dist) + self.goal.append(dist if dist is not None else 0) self.out_angle = out_angle self.navigation_marker = navigation_marker self.is_real_compass = is_real_compass diff --git a/ws_nodes_py/__pycache__/compass.cpython-310.pyc b/ws_nodes_py/__pycache__/compass.cpython-310.pyc index 03f1c9f7955dc5155726e8f31b4073f02d3ac1fb..1f3a10577e2ce55a48a1c733bcd23794f367fd8a 100644 GIT binary patch delta 2353 zcma)8&2JM&6rY*>$i`kfact)cN&+D*Tgrzh0#Z;?N&~c%AT_0^sl>AJEUAr1hRfSZd9=KK9tKNE8_0TJqUit^n_tp+JR)ny$zj^cKea?Gt z1yf~`t$1Uh0~mH!|N?&<}#E3-Rf;+(i$)rEuT)(i^peYqjxUeWVY(SlmCW_b5-TfzqqBM%LKA73W*^e*y2BpQG34B*5xp)NXDE;X zh>T;NQ<~?_xj3K;G z@o=mtzhpUiS2N@}Jx#~tMSW<;^YO-UplrYZ3}PwZ<~x1jW!)TUxOu9dd|8c&=L7q; z%X}5HfwHLF2_3Nu&o>EjLjK-$@MWDGjh(D-H@>IiP$o5Sz00t6+jhZog21*L9^y(I z0NFwLPdBIUu1_bXC|e`)$K>F^Fz}xo1Ni6(7T^iSbW&`Qe!A$^8IbC9b{yzDN*61SnoN-oTDpAOiCURU^(t-g_W^o7rYph0W zS|j&ht+6#mNsTNj!7f0Rt6*nOOldGW`%pgAV6U%$#$wkX;jG>d3bp|Ty4RW}Eq$hV zkwS`i5dsJzwjgW;fYkQE5@lM67aBJZjkH(}=eF-&vztfuRmZEk;@MRS6HS;n&yia6FPFt7JZS|v1zU@?g6xdz6rA5# z|0bhRx=pV3ojx;$`q&Hi710Arqy!Zn(Xc}?>OQvM`tJ3LnEVB#0&ER-bC@h2Xm;9O z+m<`B_K|1yL=i+ftOyTyZTkI0V8=uCCgr2-#Fj{RDizn~ky0sFAd+e+ZfZ+~#!ci# zc0J#-(EyBAv(FuWq3Sup4l1t8e~{;L+v_8($TpQKU<+;hkk;YDn+$dWK<8o0(vXD; zwyH1zH6Mmxg{~=$Cb!s3+=rG90axe6d!Q#YfJiC&^-y3j8ol!YQXSE!Y`bkZAAm&s z9`2;e`Bvm@5(RuMH-3$mvl;I@z+Fy5eB>U*X^E*itI;(|$%S|xRT3&UMkyRp5~?+r z;2K>}aNcb4Ni1=#@xoi0mXdg9cKTO4PTd6kHvU=s++<3i&o_v5v6HsthRFH8A0UuO>ZvDvY!7nsL|| zFw#c0R}m?>FF%vTDX?^L5Eh)%VhH@Z`JGdn-IZE$ivA-sjDJTAs>96~a5Lg-)HZAz z&u!Zp5C`Rd`Nv_DER;RZEri9gAH$y5CKBhc>Y-JohD%j$h@5(=+GEc>t@;me<5;Ox)0t-=!3Tt`{k@ra^Y!M<>^>g3 zcQn_?X)g-md*?j6+5Xd?F~Mx~HC)cz6NZ@0%?P_IdI~d1i+0h9rnHMt-1tFz40~Df z5MsE6?LAiLB8biLnp>Zf?&Lsp)A;KRwcDf2UjXZ2nc!HokZ2uxrOFA`-9caj4n~>e z=#e3o>|uDN;oGdSBl44n##!0c;*{%GS~VAASftGLdQj=eUFF1ffkt2oTc?nJCQrU@ z$amTy$5Q8B*J8A7;U@>9KT_HKDp80H>4q-u=RI{q1uu#=8vT1Q%ea1Q&Ur`djU7twYXo{Ear?yA+@q# z`6t4E(-2|u_D4YWthb(eAePrnX(%)0j19I-x2TFvfwU{ahc7umI{1_@BNLS9XNpSF zygDi=3NQ73aYy4sWQr)VCCVhq2(wGtlD?wRsxKKU+QPDica)GAKgHtEA{F9R$bmyQ zxSl8TuCNdmX)WP>!W`YE&bQgFgPp!m{hTU?e0NBMShND_!*HnX$*SukUos9bsGH{* zvAgjIE%9>EIn$^&RY%RYiP)kB3!Uej-fT4Eq}yzIzQogPlRq7XSe zRQ+oW+}1F4_ZE$6)mOcOmq=%;SMV~k+$&e?1Wq$^fFK4phPfE#JEw=6BcvxdNMLC_ zngQCT0j92L7VH8G@^BEe{{{>LGI|CwFapCHdRHEX)X_gCtHo~g0xujw&#etz_gq=^ zD?>OC9V?7|$d%YiU7ZId%NzzibNdjgxwWl)JJUQ%nrj=s6cPZtqF;;ePwin-9)O2B z=4grapebVrI}GFWc*XO*TTPsNL|TDCpxqr1izCnO=4kY&yEm$orr&+x5q(nX@Pju= zw#`Giec?2Zvxyoe^sQ{7w*JBi0>;RdXRn2o+G51%TLprbaiQFp8U z25kiVB{m^RZDLaE;3`|2j>G$oLxXWV`nUZ1MHNSF;#V)bQyFuz^vSF@WUJ;)0&xM4 XpGghLe-nBL3sBI-GG%UNz}WmBtHF+y diff --git a/ws_nodes_py/__pycache__/regulator.cpython-310.pyc b/ws_nodes_py/__pycache__/regulator.cpython-310.pyc index 1f37fafc87416d0ed4be833f511c39f3763ab600..8e9fe4e40e2a05a384792a095d0b8676d19cd5e6 100644 GIT binary patch delta 468 zcmYLF&r2IY6rT5Xb~D|~&Tf)5#!VU}^xzLrXz*BaC{*Y{5ih-zUIqdRDXck^FiQme z1C+soibdOl=aN(L=vgnl_CF{^JXS<~n@iv0G2i#i``-Ncxp29lJ_mu%C{}N=+fRO| zL>3D~-%@R(Giu9^0AK7u(27l{-7PnZw5#1NW+h$J(q0A2aA*Gook^hsQ?0SAYN{sM zhg50R10C8mHNBQQ9n$7a&4hKPRm`%ahuX~&Gi8z*(-A3k!_MmG?5xp$X{L7Jgy|BY zx`tgmVW-YH0P8nt$_M*bwXj#pm)Bz@)n(&v%1@s0H<+&c$A)bdb-ab-8oprp?%af3 z3g6p7_^WXbGu`$$MxE~Q(KtHk61{wG+76@dGIes?Nb}Dip=no2e})4hv+xQerH4M` z$=~^x96XRH5qBiU*n}DqhbHi4an0NEChyOCgd4kPd>Rv&=cr% z0NtQSswBu7@;sn`9L0om&qfrIQ)lRSERQLoIAmB+x=w6dKWF1vtWq%ISfiSn9(iic z(9DRR8s`A)FEyAWe`5$EhQ5*^!jO;H>4X1}=pPXm>i%9{+06t4GPlKBoy=Ei=XM#x z9j;!GO|v3dj^tD4CA^kH=jL%%!ma)w&5WeCx%+OgnPytD+etG$>2!O0=?_TS>wjv} z>aD5&TW-ktN<$vG2a~ws!n1vqEx`f^)jBt!Xe~9rXrP1Og7Xi8d%OW5R~{OmJu<#( y55>2FFBnbkROf8v_#$sY0!=VLj;m*IEL*iRcp<;nKEcW4&|8Hm)0k4rfBgoH@NhH$ diff --git a/ws_nodes_py/regulator.py b/ws_nodes_py/regulator.py index 06d2e83..53d41a1 100644 --- a/ws_nodes_py/regulator.py +++ b/ws_nodes_py/regulator.py @@ -106,7 +106,7 @@ class Controller(GetParameterNode): target_dist = goal.z # settings - is_clockwise = True + is_clockwise = target_dist >= 0 zero = [0, 0] @@ -128,15 +128,24 @@ class Controller(GetParameterNode): delta_dist = dist - target_dist angle_by_angle = course - ang - angle_by_dist = delta_dist/target_dist * 90 + if target_dist != 0: + angle_by_dist = delta_dist/target_dist * 90 + else: + angle_by_dist = 0 result_angle = (angle_by_angle - angle_by_dist) *0.02 - self.get_logger().info(f"{dist=} {relate=} {ln=} {norm=}") - self.get_logger().info(f"{round(ang)} {round(delta_dist, 2)}") - self.get_logger().info(f"{round(angle_by_angle)} {round(angle_by_dist)}") - result.angular.z = math.copysign(min(abs(max_thrust*2), abs(result_angle)), result_angle) - result.linear.x = 0.075 + # self.get_logger().info(f"{dist=} {relate=} {ln=} {norm=}") + # self.get_logger().info(f"{round(ang)} {round(delta_dist, 2)}") + self.get_logger().info(f"{round(angle_by_angle)} {round(result_angle, 2)}") + if is_real: + result.angular.z = math.copysign(min(abs(max_thrust*2), abs(result_angle)), result_angle) + else: + result.angular.z = math.copysign(min(abs(max_thrust*2), abs(result_angle)), result_angle) + if is_real: + result.linear.x = 0.075 + else: + result.linear.x = 0.025 elif self.type_move == "STOP": pass diff --git a/ws_nodes_py/settings/mission.py b/ws_nodes_py/settings/mission.py index 5c69e5a..9a24c9c 100644 --- a/ws_nodes_py/settings/mission.py +++ b/ws_nodes_py/settings/mission.py @@ -1,9 +1,12 @@ +from ws_nodes_py.settings.robot_settings import is_real from ws_nodes_py.settings.for_world import world_markers from ws_nodes_py.StateMachine.MotionStates import * from ws_nodes_py.StateMachine.SpecialStates import * from ws_nodes_py.StateMachine.DefaultStates import * +# TEST MISSIONS + is_real_compass = True square_mission = [ [MoveToGoal, {"goal": [0, 2], "dist_treshold": 0.3, "navigation_marker": "", "is_real_compass": is_real_compass, "is_update_pos": True}], @@ -16,18 +19,8 @@ circle_mission = [ [MoveByCircle, {"goal": [10, 0], "dist": 1, "out_angle": 120, "is_real_compass": is_real_compass, "is_update_pos": True}] ] -drop_cube = [ - [MoveToGoal, {"goal": "black_position", "dist_treshold": 0.11, "navigation_marker": "black_position_front", "is_real_compass": False, "is_update_pos": False}], - [DropBox, {"time": 5}] -] -drop_stakan = [ - [MoveToGoal, {"goal": "red_position", "dist_treshold": 0.15, "navigation_marker": "red_position", "is_real_compass": False, "is_update_pos": False}], - [DropPickStakan, {"time": 5, "state": 1, "is_wait_pickup": True}], - [DropPickStakan, {"time": 15, "state": 0}], -] - -drop_cube_stakan = drop_cube + drop_stakan +# FIRST STEP OF VLAD24 MISSION def trough_gate(color: str): return [ @@ -46,12 +39,14 @@ move_to_blue_gate = [ [MoveByTwist, {"goal": [0, 0.15]}], [FindObject, {"object_name": "blue_bou"}], ], [ # Ждём пока он уйдёт в корму - [MoveByTwist, {"goal": [0.15, 0.0]}], + [MoveByTwist, {"goal": [0.15, -0.05]}], [FindObject, {"object_name": "blue_bou", "min_max_angle": [120, 240]}], - ], [ # Проезжаем чуть вперёд, чтобы выйти +- перпендикулярно воротам - [MoveByTwist, {"goal": [0.15, 0]}], - [Wait, {"time": 7}], - ], [ # Разворачиваемся к воротам + ], + # [ # Проезжаем чуть вперёд, чтобы выйти +- перпендикулярно воротам + # [MoveByTwist, {"goal": [0.15, 0]}], + # [Wait, {"time": 7}], + # ], + [ # Разворачиваемся к воротам [MoveByTwist, {"goal": [0.0, -0.15]}], [FindObject, {"object_name": "blue_gate", "min_max_angle": [0, 360]}], ] @@ -59,95 +54,90 @@ move_to_blue_gate = [ ] move_to_yellow_gate = [ - [ # Пока не видим 2 шара в боковой камере - [MoveByTwist, {"goal": [0.15, 0.00]}], - [FindObject, {"object_name": "yellow_bou_1", "min_max_angle": [0, 360]}], - ], [ # Проежаем чуть вперёд, чтобы не отойти от синих шаров - [MoveByTwist, {"goal": [0.15, -0.05]}], - [Wait, {"time": 15}], - ], [ # Поворочиваемся лицом к воротам - [MoveByTwist, {"goal": [0.0, -0.15]}], - [FindObject, {"object_name": "yellow_gate", "min_max_angle": [0, 360]}], + [ # Поворочиваемся вокруг буя, пока не увидим жёлтые ворота + [MoveByCircle, {"goal": "blue_bou", "navigation_marker": "blue_bou"}], + [FindObject, {"object_name": "yellow_gate", "min_max_angle": [350, 10]}], ] + # [ # Пока не видим 2 шара в боковой камере + # [MoveByTwist, {"goal": [0.15, 0.00]}], + # [FindObject, {"object_name": "yellow_bou_1", "min_max_angle": [0, 360]}], + # ], [ # Проежаем чуть вперёд, чтобы отойти от синих шаров + # [MoveByTwist, {"goal": [0.15, -0.05]}], + # [Wait, {"time": 5}], + # ], [ # Поворочиваемся лицом к воротам + # [MoveByTwist, {"goal": [0.0, -0.15]}], + # [FindObject, {"object_name": "yellow_gate", "min_max_angle": [0, 360]}], + # ] ] -# mission = test_concuration + drop_cube_mission first_step = trough_gate("yellow") + move_to_blue_gate + trough_gate("blue") + move_to_yellow_gate + trough_gate("yellow") mission = first_step -# mission = move_to_yellow_gate + trough_gate("yellow") # first_step -# mission = move_to_yellow_gate - - - - - - - - - - - - - - - -vlad24_mission = { - "gate-1-yellow": {"type": "moveTo", "pos": world_markers["yellow_gate"], "is_real_compass": False, - "is_update_pos": False, - "dist_threshold": 0.25, - "navigation_marker": "yellow_gate", - # "navigation_marker": "", - }, - "point-left-blue": { - "type": "moveTo", - "pos": [8, -3], - "dist_threshold": 0.25, - "is_real_compass": False, - "is_update_pos": False, - "navigation_marker": "", - }, - "point-center-blue": { - "type": "moveTo", - "pos": world_markers["blue_gate"], - "dist_threshold": 0.25, - "is_real_compass": False, - "is_update_pos": False, - "navigation_marker": "blue_gate", - # "navigation_marker": "", - }, - "point-2-blue": { - "type": "moveTo", - "pos": [8, 1], - "dist_threshold": 0.25, - "is_real_compass": False, - "is_update_pos": False, - "navigation_marker": "", - }, - "gate-3-yellow": { - "type": "moveTo", - "pos": world_markers["yellow_gate"], - "dist_threshold": 0.25, - "is_real_compass": False, - "is_update_pos": False, - "navigation_marker": "yellow_gate", - # "navigation_marker": "", - }, - "move-to-start": { - "type": "moveTo", - "pos": [4, 2], - "dist_threshold": 0.25, - "is_real_compass": False, - "is_update_pos": False, - "navigation_marker": "", - }, - "red-circle": { - "type": "moveTo", - "pos": world_markers["red_position"], - "is_real_compass": False, - "is_update_pos": False, - "dist_threshold": 0.1, - "navigation_marker": "red_point", - }, - -} + + +# SECOND AND THIRD STEP OF VLAD24 MISSION + +def get_drop_cube(): + if is_real: + return [[[DropBox, {"time": 5}], [MoveByTwist, {"goal": [0.075, 0]}]]] + else: + return [[[DropBox, {"time": 5}], [MoveByTwist, {"goal": [0, 0]}]]] + +drop_stakan = [ + [MoveToGoal, {"goal": "red_position", "dist_treshold": 0.15, "navigation_marker": "red_position", "is_real_compass": False, "is_update_pos": False}], + [DropPickStakan, {"time": 5, "state": 1, "is_wait_pickup": True}], + [DropPickStakan, {"time": 15, "state": 0}], +] + +move_to_black_island = [ + # TODO время подобранно для симулятора + # WARN в реальности это не прокатит + + # TODO отсюда экстраполируем своё положение (подставляем координаты жёлтых ворот) + # WARN если включаем экстраполяцию положения, то не стоит одновременно использовать linear и angular + # WARN если используем виртуальный компас, то лучше использовать константную угловую скорость 0.15 (отключить пиды) + + # TODO Общий таймер на этап, если сделаем прохождение сбора шаров + # (надо придумать, как это можно сделать) + [ # Отходим от ворот + [MoveByTwist, {"goal": [0.15, 0]}], + [Wait, {"time": 10}], + ], [ # Поворачиваем на "90" градусов + [MoveByTwist, {"goal": [0.0, 0.15]}], + [Wait, {"time": 22}], + # TODO выход по курсу + ], [ # Проходим ещё чуть вперёд + [MoveByTwist, {"goal": [0.15, 0]}], + [Wait, {"time": 20}], + ], [ # Поворачиваемся в сторону острова + [MoveByTwist, {"goal": [0.0, 0.15]}], + [Wait, {"time": 20}], + # TODO выход по курсу + # WARN круг виден только в правой части экрана [0, 20-30]гр + ], [ # Идём вперёд, пока не увидим чёрный круг + [MoveByTwist, {"goal": [0.15, 0]}], + [FindObject, {"object_name": "black_position", "min_max_angle": [0, 360]}], + ], [ # Сближаемся с чёрным кругом + [MoveToGoal, {"goal": "black_position", "dist_treshold": 0.11, "navigation_marker": "black_position", "is_real_compass": False, "is_update_pos": False}], + ], +] + +move_around_black_island = [ + [ # Отходим от острова + [MoveByTwist, {"goal": [-0.15, 0]}], + [Wait, {"time": 3}], + ], [ # Разворачиваемся перпендикулярно острову + [MoveByTwist, {"goal": [0, 0.15]}], + [FindObject, {"object_name": "black_position", "min_max_angle": [90, 110]}], + ], + # [ # Движемся вокруг острова + # [MoveByCircle, {"goal": "black_position", "navigation_marker": "black_position", "is_real_compass": False, "is_update_pos": False}], + # [Wait, {"time": 60}], + # # Если шар под кругом, то через него мы не идём + # # [FindObject, {"object_name": "red_position", "min_max_angle": [180, 360]}], + # ], +] + +second_step = move_to_black_island + get_drop_cube() + move_around_black_island + +mission = first_step + second_step \ No newline at end of file -- GitLab From 324ec46c1aef762bcae78429fce177db7866c996 Mon Sep 17 00:00:00 2001 From: "toropov.nik" Date: Tue, 8 Oct 2024 00:36:07 +0300 Subject: [PATCH 16/23] remake regulator, fix move around, add searching red point --- ws_nodes_py/StateMachine/SpecialStates.py | 49 ++++ .../__pycache__/regulator.cpython-310.pyc | Bin 5563 -> 6706 bytes ws_nodes_py/regulator.py | 215 +++++++++--------- .../robot_settings.cpython-310.pyc | Bin 331 -> 379 bytes ws_nodes_py/settings/mission.py | 29 ++- ws_nodes_py/settings/robot_settings.py | 4 + 6 files changed, 186 insertions(+), 111 deletions(-) diff --git a/ws_nodes_py/StateMachine/SpecialStates.py b/ws_nodes_py/StateMachine/SpecialStates.py index ad4f7dd..7412c99 100644 --- a/ws_nodes_py/StateMachine/SpecialStates.py +++ b/ws_nodes_py/StateMachine/SpecialStates.py @@ -1,4 +1,5 @@ from rclpy.duration import Duration +from ws_nodes_py.settings.for_world import world_markers import rclpy.duration from std_msgs.msg import Int16 from smach import State @@ -79,3 +80,51 @@ class FindObject(State): self.cntr.get_logger().info(f'Detect {self.object_name} at {obj.point.z % 360} deg') return 'complite' return 'complite' + + +class IsAchievePosition(State): + def __init__(self, c, goal:str | list=None, dist_treshold: float=None): + State.__init__(self, outcomes=['complite'], input_keys=['sm_counter'], + output_keys=['sm_goal', 'sm_pos', 'sm_error_dist', 'sm_error_angle'],) + + self.cntr = c + if goal is None: + goal = [0, 0] + if dist_treshold is None: + dist_treshold = 1 + + self.goal = world_markers[goal] if type(goal) is str else goal + self.distance_threshold = dist_treshold + + def execute(self, userdata): + while not self.cntr.shutdown(): + error_dist = ((self.goal[0] - self.cntr.position.x)**2 + (self.goal[1] - self.cntr.position.y)**2)**0.5 + + userdata.sm_goal = f'{round(self.goal[0], 1)} m\t{round(self.goal[1], 1)} m' + userdata.sm_pos = f'{round(self.cntr.position.x, 1)} m\t{round(self.cntr.position.y, 1)} m' + userdata.sm_error_dist = f'{round(error_dist, 1)} m' + + if error_dist < self.distance_threshold: + return 'complite' + return 'complite' + + +class IsAchieveHeading(State): + def __init__(self, c, target_angle: int=None, delta_angle: int=None): + State.__init__(self, outcomes=['complite'], input_keys=['sm_counter'], + output_keys=['sm_goal', 'sm_pos', 'sm_error_dist', 'sm_error_angle'],) + + self.cntr = c + if target_angle is None: + raise Exception("target_angle is not described") + if delta_angle is None: + delta_angle = 10 + + self.target_angle = target_angle + self.delta_angle = delta_angle + + def execute(self, userdata): + while not self.cntr.shutdown(): + if abs(self.cntr.get_heading() - self.target_angle) < self.delta_angle: + return 'complite' + return 'complite' diff --git a/ws_nodes_py/__pycache__/regulator.cpython-310.pyc b/ws_nodes_py/__pycache__/regulator.cpython-310.pyc index 8e9fe4e40e2a05a384792a095d0b8676d19cd5e6..75f1f3814bf2370c8e3946cba2d9ed58a7c6f575 100644 GIT binary patch literal 6706 zcmb_hO^_Q$6`p^Mq>)zpV|l%{7uj(FN?hwWf#3=zPV5A;RI+y1O#m~Uzd&AhldsAFH{-+8K^vqC;BxAQfR>f+hDk)hv1G|x~q#Kz^rje~=Wt$Z^ja((y$XD`>vC3GZP$@LVE8~rc z%7o;ng2~2IWlB{pD$Hi->k3Qz+O}Rfz%nd*U8xlPgUngi_)C6qTfrCVqI}A^dzhoL zP*z3Zq90%OxYzJwpD%>WM_Z~Mx!m`HvL?*=W_{T z1&lfT-C=eFt>bSghEkbWRLZj=b0KWTJPZP#SJfdkNlP1#jwj;a#LBjE13KPRRi&e> zX-tdtHDk?Ovl4xo`HtRE+?3>5173>OqQ|o(&mQp7L%ejH;m@%QddRZumdczhP1YQi z+tN`pnt7I&T&KsSn#0BtZk+4jn~ugDR(Qt%Cyy3<*D(eyV;H>pS0I$Gu_e_mToDxRH0tw2hb+Tzb~9x4IBO`zP7&Uz0ObZFnZ$DmB_skomy_! z14iX0c3R}NHb%;W$;`Djgnc&f8q3UkW}F7pKn!K`;axb*xo*8#k6m}}Y!FtxAbJLc z)k7c?RG0w~nTB_j7OEY^ z4_u|s+N&I#dR?mBnf=k~%}e*J;d;K$I7oWOrq=ur}*<^9Y( zJwjg$;JmBFpeK>3g-1mc){jvYsEC)S_u}1hhw(~{{M8YI3%hdY0=X>&h z&-Wn`A%O4`L>{!NvZpUWLKurnmoAI(m?Esp!{sne=KZ-RJ4fRa1U6@$YsX6P4H0V(x6y!v9e$jnxe(9Sog1#8*S6-Z7pawohBB@rLk*`V$0dj|#sJ$Fj zd3{;#jFph@PJ)vU<|!f+OhvKPUJmNfs?XgO9ya>3=CoNw8cbKNtH@Q6+Qh!wZeh>* zV$$7pscye9Luvdlp~YziatgB4+7dTuLuF=1kkRHGNe_D{PmmMV%|H`zXN z|9=@1AoU_DIO-AkXPB01;E$-KT3Wwss1x{`)kR5tpyjwg z!(0O)@5EFje&dLNDr(vqvIkJ(rc6W^6A29xvyd=}*N=DsK+`Y>i ziQTxw@IjJNH|{4Ghi(!YruTQ^@vz-wWtG1`BaV7DLVJRRC&7#ex*2s^&8UUJQ_%L9 zpobk(4p^nw-2YKdM~IV_pnRdDt|`d1)Q;*Z$e+}8gXfsGr8m>ErtvAJqoy%q3l@T{ zkO6KP$g9kQ3bTsJmdR3E7IH1J)ya<0u?{L*spe6%kR`rCEz{I8g|9W#lr{Y(vf?7L z!gVM-_yT<nj%p}tR!56dvE4~+tJ~_7@>NAsTuX8=+c>j_la90e zDtd8{8`?uipnAGrFJ~5{iAB1KG-VyG%@?V#71tYHAaY(at`FMpy^n>vw~9r(M<=b? zlk)fbbG2XoLjKN`b3)w|$42Z+J9Re~xGw~-Wdq;!IS)B6p<_`T>A`;;p^TD0E($Y>y~6NP>A$tM8pPgKlE;JE!y{UG+> zuaYwpc^I4)_JB0!x^g=t9i79-5$zbb4%OE1kR#DDngePBxAYV>yfs_Pt25dR+6yXh zJU!w{hPqskuDVJ}_J3&<$hAr={{{uv8h8di6QoLy0_*@qH}sBycKDGYOj%6bO2w8$ zltGB^e7OnuPb~WXjIw?KMf?NfkI_1!!M$jY{$B(W|b{FcH&$NTyCqN zHhRHM+17w}tpf_ld?(cz_@T0ypsfd)Kb^Zr3f-e@vOLDWix;mDDS?RD5g3pWvGkND zyHU&c8UG$h5+;Q1Ek_9mBqn4~`xT8-yWtanH!==Ds-AHxxe(;d_4mOC4y5>p{ zdaB(rBH^p~L@7-Gmo((OWR2VVPr zO813Fd^M>#!1ppLz$+n8;S-6vK&l}lD7>RYqt+beoPeyJ#g33({TS1mDl?|w?PDW0 z(F3?Ztz4{apmkfrnX`vaPR8R5HLMUhOGHA9fW>PdI5Wt|Ri}mqkv)qt z>AL&yy9v3d074mt1J|HkbF^u=g3Z|xRP6h3A08;cNj#KL=wLCXu~Z06x(rJO&*QD0 zk#(r3FYs3($6p~r^X%DGIK6$4K;MbNHpgi@saZI3;~2D8FStF)PrUb!(U~;01DxB) zoc6f0AyJtwj98l>GRTpxq5|KMcr&t;x(2nFdPte3%t%GH1V2SJ4Y3i5YD}M0pe88d zvN5SpjOI^cF^t2A(H;0dye+bP$0T0|-vs{$4|j%{ofJjrPO5kLX0?Bd;WMS)uluv^ z-R>W{@9kXc-ro6X_jdP#oi|I}k9M|quI;>0>VDF_UE29^_k-@oJ8z#Xf%!)F};%^-1?m_k+^To87xRZ*@Nc7eaTt??L8v_x)av=&Rq&&bue2a0w#&hkkr8 zGa0xOloAB~iU+?xpOf~*ZQp3R){AMOi!6LNmdXf90i9Do=?718Eka{g@oWe!z%?Pe zN@SxcebQba6cd2&jjV5@mxzv(8RNJ*1w;@o`se^Yj_pJG-X7q6=t99g-7h>b0)O%m z1Jpc=$}nnxA7ssxI}vh#5s4qB0VyieWeX@a3tAG?40!f!N3{AUh7W^BGiP%$2+pVleVH+68NR z4W6pkZlHYzZKmXy39o~juf);g#+Q!N$uUf3PSV=6*Epp|Wo;-J?okDfd;~H#As-P= z$c>XDhy6z5*NAvBfk{f=o4CDpU6G*x$=b;ET;aIx_uF2Oa5$a3_<17oym*O9A&~_T zp~L<%baS8DD%5635KBE|)*y2MvXFpFl4rcC9|Zk#p49AH^f4KUlhY;NEs<$MANxe! znL05uZdl1D~l5uBZ1R&1iV_<}I2G z*@O~IN-G_e|D49asuSOJ%hSoACx$LH=X#e~a~JpgZ-Eo^#6!cPxY3Joj~C4)_Z|Ie za%bQ~F>`UH7R}MU8tud6tUq4!alsn%ja??~gT%UIG-To)ZYRm%aOrwv2JcW+W^iO3 zk%{V!7SMuQ2YLS>mrSb1e?pSGc$Fo&|Ln5~#GWB@$?VHUWF@ptA^UQ)f}#A-K4Pab Is`2T60I#(+Hvj+t literal 5563 zcmb_g&5t9+6|d@U+ikn;@pwGn%Q7qriV2wwONbz#U10f|WSQN~41rjXj>pyG@l4xo zs@v?2(-w(lBcw<~IphK;(kus*y+n$V15*Bi9FjkvPKk0zR*4iv+)xDhy=vPtvoL@} zx>c|1)yJz>uijU6P$=XWp4ERF&1iwKzY%iyv4A{|621o@nBYFEC0}0SzzyGMn>CYD zo9SEaR4t|JmY;5CYMHiOv)kEPR=1`6TsvROx1E}!@wCs|g<7FqtQFf6wTX7AR?_Vm zf3jV!m35u_Sm#+LY>~arMAkF+%-WR5iTrg|t9aAG*)il5ud>H5ny=|nE!+aHY_-Z& z=~*wjT$lB>7kP3u5FXkxtV6tzT`0kxE?- zTAiqBDXZQHLX{C-Q+i&g%&^r_8Ce&tdM5-y^YY454O;Q=V-AtC*o9qC^Xi{gatzvEN-r^HEdr&!Yc zcWHQPJi8K2i@U{X-I~a1CT9Dy;A$?B>Hho;w!a|mAxtow&b_NDE4^mVuSbDYWdX&h zcN(4>ZAmZO3Vdtf72BrbRk?(^y@<$!>DRugNL2Y4G8NGh0F1J0rJu`t{f3 z85x9U&s+`NtKpfAUdtC$??9nLx4V0!K4xaQyQ}QS{Cays)Xx#!4-5c{_3t@Qr)Ae| zby|_@EiN!d)n|w7nspIze)e$$x(@J9q^a|AE5~?_U zOVpz}zGgdY9w>qw3yz5DGfcCpDd&MHYqQq@V}_Q#wsE^vNGR{xKz$YX)_H$f-s3FHHM5v48FV#^sojnrH{F!JK!y z09sA5x=yWMn&0Km=l3oevP9q%f#amADcy?^%352$bXiSC6vA8?Yy?rl_m^h&>$E

-+Pu5A{mf-ee8$MiA zb{wCTwGJt(^i!8!c=F=YvPA14C1G-r00lXfUs-csTv>n4MaWeXBjn=BDmBOg4asy{ zf_zm{@e!&Q!rn&MkgW}E8JmGTU_w#|l@voI*`u22_BQ-hxaCQAQwHr3*D^z$S#-Ku zuBy;L1`&B~uM3+6%foMucIpSsCiZW&I-5Z?rL55NH&qH8x!vR=R0?bnMjru-&$=$8 z6NavvSd2FLvsjZ{YWNKRho8`|l}d99FTpfV8>70#%Xm*3OMHo+YNNnq%nC4D2`~_!Nt6g0n~k zr`RAHXXT@DCeALhK`yrSHyg7g@2>c+q+C^I0jdQ_Oskak9F`aOW=6(Z2vbzo(rV|qd5gh5Xt z*GRB55)K4`+xM1FnA5w`9FVq;_R*jD#4hx}BhKOkCXfN>*@l^fErc$PVxS2P6JFd{1NPem1svi3M&9y)8p`5Q*EUQ~*vqZy*T$KyC@<8UvhT7&jG{)O{28+&Y+HQn9abeEei1rYZ1xi>NTzIZO4SY)CIxi54KoZdn_A1^>VN@!Vx zG#8O-pNQ^==lA$KOiZG-C~zL{uwR*!en%%k<)s5j3r;73Q(M%hwm3Sq&gAEh(X^x} zADoGpI;d+MtLXE2JgXKF$D+m5Er3Z2 zY{{C!6!=iRY{J?PFEM14>vbOg zJav4Zzz+zJ^O2O{s5By`SC=XSr$(0%#ryJM>M-jYq5Kxrtad%xQg$Qg?uM-<{C&IC zk$Ixv5O{>ZqXfpWL_R_2lK{v`f?h}HT%&qIyWG!aSLL+3bm`isibLl-yhf-j`V!wP zU(v*0tNT6thM>9`A}L5W;J-sp<%bhB6$ig38n}$@Ac6Cg)%RooN|>4~0xyrNrYBz@ zDpn_uZTXU}>d86b`B8nyPU84Cc8BqyFvgR0l5jC(@Mxm03P%op1q64hVf&^s2C^toBDC+k7y&orLK*rsjR$WLJMVjs4X9WII?dsnWxMgqkA`x8e;KrS6jQ?NVCPlK= zR_9&N3&|X9=n3=#8Y6#3Yd~<+@4D3)y{O!#*3*Z6eJl^JzRS1&=#!)h>h8lsaqo|C z+YgtMThRzjZWc}$2^SadaG7r3n}jLL 180: - result -= 360 - return result - - def move_to_point(self): - position, goal, heading, max_thrust, distance_threshold, k_p_angular = self.position, self.goal, self.heading, self.max_thrust, self.distance_threshold, self.k_p_angular - error_dist = ((goal.x - position.x)**2 + (goal.y - position.y)**2)**0.5 - target_heading = self.angle_point_error(position, goal) - err_angle = self.angle_error(target_heading, heading) - - result = Twist() + def __update(self): if self.type_move == "MOVE TWIST": - result.linear.x = goal.x - result.angular.z = goal.y + self.__move_by_twist(self.goal.x, self.goal.y) elif self.type_move == "MOVE TO": - value = 0 - if self.IS_WITH_PID: - # self.get_logger().info(f"{err_angle}=") - if abs(err_angle) > 3: - # self.pid_ang.setpoint = target_heading - value = -self.pid_ang(err_angle) - else: - if abs(err_angle) > 3: - # value = k_p_angular * err_angle - value = max_thrust * (1 if err_angle > 0 else -1) - result.angular.z = math.copysign(min(abs(max_thrust), abs(value)), value) - - if abs(sin(radians(err_angle))*error_dist) < 0.2: - if self.IS_WITH_LIN_PID: - value = -self.pid_lin(error_dist) - result.linear.x = math.copysign(min(abs(max_thrust), abs(value)), 1) - else: - result.linear.x = max_thrust + self.__move_to_point(self.goal) elif self.type_move == "MOVE CIRCLE": - point = [goal.x, goal.y] - slf = [position.x, position.y] - course = heading # -45 - target_dist = goal.z - - # settings - is_clockwise = target_dist >= 0 - - zero = [0, 0] - - eval_dist = lambda p1, p2: ((p1[0] - p2[0])**2 + (p1[1] - p2[1])**2)**0.5 - relate = [slf[0] - point[0], slf[1] - point[1]] - ln = eval_dist(relate, zero) - - if ln > 0: - norm = [relate[0] / ln, relate[1] / ln] - else: - norm = [1, 0] - - ang = degrees(acos(norm[0])) - if norm[1] < 0: - ang = 360 - ang - ang = ang + (+90 if is_clockwise else -90) - - dist = eval_dist(point, slf) - delta_dist = dist - target_dist - - angle_by_angle = course - ang - if target_dist != 0: - angle_by_dist = delta_dist/target_dist * 90 - else: - angle_by_dist = 0 + self.__move_around(self.goal) + elif self.type_move == "STOP": + self.__move_by_twist(0, 0) + else: + self.__move_by_twist(0, 0) - result_angle = (angle_by_angle - angle_by_dist) *0.02 + def __move_around(self, point: Point): + target_dist = point.z + point.z = 0 - # self.get_logger().info(f"{dist=} {relate=} {ln=} {norm=}") - # self.get_logger().info(f"{round(ang)} {round(delta_dist, 2)}") - self.get_logger().info(f"{round(angle_by_angle)} {round(result_angle, 2)}") - if is_real: - result.angular.z = math.copysign(min(abs(max_thrust*2), abs(result_angle)), result_angle) + if target_dist > 0: + mode = "clockwise" + elif target_dist == 0: + mode = "optimal" + else: + mode = "anticlockwise" + + heading_to_point = self.__get_heading_to_point(point) + around_heading = (heading_to_point + 180) % 360 + + point_target_delta_degrees = 10 + if mode == "clockwise": + angle = heading_to_point + point_target_delta_degrees + elif mode == "anticlockwise": + angle = heading_to_point - point_target_delta_degrees + else: + if self.__angle_error(heading_to_point, self.heading) % 360 <= 180: + angle = heading_to_point + point_target_delta_degrees else: - result.angular.z = math.copysign(min(abs(max_thrust*2), abs(result_angle)), result_angle) - if is_real: - result.linear.x = 0.075 + angle = heading_to_point - point_target_delta_degrees + + if target_dist == 0: + target_dist = self.__eval_dist(self.position, point) + + point.x += target_dist * cos(radians(angle)) + point.y += target_dist * sin(radians(angle)) + + if is_real: + self.__move_to_point(point, 0.075) + else: + self.__move_to_point(point, 0.025) + + def __move_to_point(self, point: Point, max_thrust:float=None): + target_heading = self.__get_heading_to_point(point) + err_angle = self.__angle_error(target_heading, self.heading) + ang = self.__get_angular_speed(err_angle) + + error_dist = self.__eval_dist(self.position, point) + lin = 0 + if abs(sin(radians(err_angle))*error_dist) < 0.2 or err_angle < 3: + if max_thrust is None: + max_thrust = self.max_thrust + if self.IS_WITH_LIN_PID: + value = -self.pid_lin(error_dist) + lin = copysign(min(abs(max_thrust), abs(value)), 1) else: - result.linear.x = 0.025 - - elif self.type_move == "STOP": - pass + lin = max_thrust + self.__move_by_twist(lin, ang) + + def __move_by_twist(self, lin:int | float, ang:int | float): + msg = Twist() + msg.linear.x = float(lin) + msg.angular.z = float(ang) + self.pub_twist.publish(msg) + + def __get_angular_speed(self, err_angle): + value = 0 + if self.IS_WITH_PID: + # self.get_logger().info(f"{err_angle}=") + if abs(err_angle) > 3: + value = -self.pid_ang(err_angle) + else: + if abs(err_angle) > 3: + # value = k_p_angular * err_angle + value = -self.max_thrust * (1 if err_angle > 0 else -1) + return copysign(min(abs(self.max_thrust), abs(value)), value) + + def __angle_error(self,target_angle, source_angle): + result = (source_angle - target_angle) % 360 + if result > 180: + result -= 360 return result - def update(self): - result = self.move_to_point() - - if result is not None: - # if abs(result.angular.z) < 0.6 and abs(result.linear.x) < 0.15: - self.pub_twist.publish(result) + def __get_heading_to_point(self, goal:list | Point | PointStamped): + """ + Возвращает курс на цель, если положение точки совпадает с целью, то возвращает 0 + """ + goal = self.__point_to_list(goal) + dx, dy = self.position.x - goal[0], self.position.y - goal[1] + ln = (dx**2 + dy**2)**0.5 + if ln: + dxn = dx/ln + dyn = dy/ln + ang = degrees(acos(dxn)) + if dyn < 0: + ang = 360 - ang + return ang + return 0 + + def __eval_dist(self, point_1:list | Point | PointStamped, point_2:list | Point | PointStamped): + point_1 = self.__point_to_list(point_1) + point_2 = self.__point_to_list(point_2) + return ((point_1[0] - point_2[0])**2 + (point_1[1] - point_2[1])**2)**0.5 + + def __point_to_list(self, point:list | Point | PointStamped): + if type(point) is Point: + return [point.x, point.y] + if type(point) is PointStamped: + return [point.point.x, point.point.y] + return point def main(): diff --git a/ws_nodes_py/settings/__pycache__/robot_settings.cpython-310.pyc b/ws_nodes_py/settings/__pycache__/robot_settings.cpython-310.pyc index 6ea5e818ea72458bd37f3a53bc65518d2eda955d..36a9fc0577f3b2e7ca29935bad13c2acda8a4a39 100644 GIT binary patch delta 165 zcmX@j^qa{opO=@50SKb3S<+`QGcY^`agYH!kmCTv#acijg&~R|g)xdTg(-zOg#}8o z&S9JA;9|vA#cHHytY-+Jj3AV06_=rjo`J5ho{63j65AMwZ8&k(d5RtkIWb11L|R6U}0k7WMX7uWcwk&0|5I)5&QrE diff --git a/ws_nodes_py/settings/mission.py b/ws_nodes_py/settings/mission.py index 9a24c9c..7d2561c 100644 --- a/ws_nodes_py/settings/mission.py +++ b/ws_nodes_py/settings/mission.py @@ -130,14 +130,29 @@ move_around_black_island = [ [MoveByTwist, {"goal": [0, 0.15]}], [FindObject, {"object_name": "black_position", "min_max_angle": [90, 110]}], ], - # [ # Движемся вокруг острова - # [MoveByCircle, {"goal": "black_position", "navigation_marker": "black_position", "is_real_compass": False, "is_update_pos": False}], - # [Wait, {"time": 60}], - # # Если шар под кругом, то через него мы не идём - # # [FindObject, {"object_name": "red_position", "min_max_angle": [180, 360]}], - # ], + [ # Движемся вокруг острова + [MoveByCircle, {"goal": "black_position", "navigation_marker": "black_position", "is_real_compass": False, "is_update_pos": False}], + [Wait, {"time": 120}], + # Если шар под кругом, то через него мы не идём + # [FindObject, {"object_name": "red_position", "min_max_angle": [180, 360]}], + # Для теста смотрим только спереди + [FindObject, {"object_name": "red_position", "min_max_angle": [270, 20]}], + ], ] second_step = move_to_black_island + get_drop_cube() + move_around_black_island -mission = first_step + second_step \ No newline at end of file +mission = first_step + second_step + +test = [ + [ + [MoveByCircle, {"goal": "black_position", "navigation_marker": "black_position", "is_real_compass": False, "is_update_pos": False}], + [Wait, {"time": 120}], + # Если шар под кругом, то через него мы не идём + # Для теста смотрим только спереди + [FindObject, {"object_name": "red_position", "min_max_angle": [270, 20]}], + ] +] +# mission = test +# mission = move_around_black_island +# mission = move_to_yellow_gate \ No newline at end of file diff --git a/ws_nodes_py/settings/robot_settings.py b/ws_nodes_py/settings/robot_settings.py index 157a6ec..4fd79e7 100644 --- a/ws_nodes_py/settings/robot_settings.py +++ b/ws_nodes_py/settings/robot_settings.py @@ -13,6 +13,10 @@ cameras_ports = { "2.3.2": "camera_side", # в хабе "2.3.5": "camera_down", # в хабе + "14.0-3.4.2": "camera_front", # в хабе + "14.0-3.4.3": "camera_side", # в хабе + "14.0-3.4.1": "camera_down", # в хабе + } camcv = {'F':'camera_front', 'S':'camera_side', 'D':'camera_down'} \ No newline at end of file -- GitLab From 66aa60795a7941c3f422dea11cc1518ff3c00e13 Mon Sep 17 00:00:00 2001 From: "toropov.nik" Date: Tue, 8 Oct 2024 09:48:24 +0300 Subject: [PATCH 17/23] hotfix regulator, fix drop stacan and cube, make drop stakan step --- ws_nodes_py/StateMachine/SpecialStates.py | 4 +- .../__pycache__/regulator.cpython-310.pyc | Bin 6706 -> 6712 bytes ws_nodes_py/regulator.py | 31 ++++++----- ws_nodes_py/settings/mission.py | 50 ++++++++++++++---- 4 files changed, 60 insertions(+), 25 deletions(-) diff --git a/ws_nodes_py/StateMachine/SpecialStates.py b/ws_nodes_py/StateMachine/SpecialStates.py index 7412c99..e761129 100644 --- a/ws_nodes_py/StateMachine/SpecialStates.py +++ b/ws_nodes_py/StateMachine/SpecialStates.py @@ -11,12 +11,12 @@ class DropPickStakan(State): State.__init__(self, outcomes=['complite']) self.cntr = c - self.state_change_time = self.cntr.get_clock().now() self.time = time self.state = state self.is_wait_pickup = is_wait_pickup if is_wait_pickup is not None else False def execute(self, userdata): + self.state_change_time = self.cntr.get_clock().now() self.cntr.get_logger().info(f'{"Dropping" if self.state == 1 else "Picking"} stakan') while not self.cntr.shutdown(): # rclpy.spin_once(self.cntr) @@ -41,9 +41,9 @@ class DropBox(State): self.cntr = c self.time = time - self.state_change_time = self.cntr.get_clock().now() def execute(self, userdata): + self.state_change_time = self.cntr.get_clock().now() self.cntr.get_logger().info(f'Droping box') while not self.cntr.shutdown(): # rclpy.spin_once(self.cntr) diff --git a/ws_nodes_py/__pycache__/regulator.cpython-310.pyc b/ws_nodes_py/__pycache__/regulator.cpython-310.pyc index 75f1f3814bf2370c8e3946cba2d9ed58a7c6f575..6469f7e449b1a2d2a8cec6c9e7ea41966975097c 100644 GIT binary patch delta 460 zcmZ9Izb^zq6vyZLc5Zie?JnoITSAn6g#@Qj2@R#FR1!9koa7|u!WFX=N1>9KM51z+ zC`qu5l79gIfyTc;CE=Z16ef9Z-sk(?yqS6T#fM_J69k^%b?_p$ZU^qd$e(Z-Gi-}& zyI<@yN+KamF}7vJiz%_n?k`a#Yu5bn-cg;=c0nXgc~D!i+@{L3&X@~-@NnTTBU@3# zHSLotS~R2P)jee5jqcAW9nijxrGn&*u88)lks!=I59`L`6-K&zr(C? zz|Z4>HjzLhMjvwtC5l*!sZCb;k!t{_=Dha{m&W9uab-3NZ_Y@6R@th4f25T0-LX8ENns`=~LXo$sI&kS3Kk`A_pnN z?lp-7%$61wx*-07*optZ+J6v8KxbEz!hxB2-#6bIX6EbK=e7806a^%9zM}?TZheZA zaMcLZ;#=a+2t*b`DmaD3K3T~ll|?BR-obQ(IBUDzaQW4WRaP%h;UCC;D1FWl?g$7K(=2a_;BG`V&mhria9|ubYa@ zkq5p1O4IpFHkLM_MQTb0rL;?bv0I8ZlX~sJYGc*L;N5X!8%vRU9rfL>=q`+l;=X$x z-|uM{7Keu7fJ2D}f#3igmdu7@bh#^h^)3DZ^sA#;4dm7Pxdb;})_0thprk>_?ri?C zCmXr>)(afEH?7SFN{Y?3$MTR7<>Dm@$o?3LGQ}b#!VB^Q@gh|htyAEwJGps@6L;MC fj?z6@`03y4&KYe&x1D^%hw3s}hkx7n0o3n50%>Yv diff --git a/ws_nodes_py/regulator.py b/ws_nodes_py/regulator.py index 6cc3293..54ec18f 100644 --- a/ws_nodes_py/regulator.py +++ b/ws_nodes_py/regulator.py @@ -62,7 +62,6 @@ class Controller(GetParameterNode): def __move_around(self, point: Point): target_dist = point.z - point.z = 0 if target_dist > 0: mode = "clockwise" @@ -71,30 +70,35 @@ class Controller(GetParameterNode): else: mode = "anticlockwise" + # Курс к центру окружности heading_to_point = self.__get_heading_to_point(point) + # Курс от цента окружности к аппарату around_heading = (heading_to_point + 180) % 360 - point_target_delta_degrees = 10 + # Нас сколько градусов на окружнасти цель спереди + point_target_delta_degrees = 20 if mode == "clockwise": - angle = heading_to_point + point_target_delta_degrees + angle = around_heading + point_target_delta_degrees elif mode == "anticlockwise": - angle = heading_to_point - point_target_delta_degrees + angle = around_heading - point_target_delta_degrees else: if self.__angle_error(heading_to_point, self.heading) % 360 <= 180: - angle = heading_to_point + point_target_delta_degrees + angle = around_heading + point_target_delta_degrees else: - angle = heading_to_point - point_target_delta_degrees + angle = around_heading - point_target_delta_degrees if target_dist == 0: target_dist = self.__eval_dist(self.position, point) - point.x += target_dist * cos(radians(angle)) - point.y += target_dist * sin(radians(angle)) + goal = Point() + goal.x = point.x + target_dist * cos(radians(angle)) + goal.y = point.y + target_dist * sin(radians(angle)) + # Настройка линейной скорости if is_real: - self.__move_to_point(point, 0.075) + self.__move_to_point(goal, 0.075) else: - self.__move_to_point(point, 0.025) + self.__move_to_point(goal, 0.025) def __move_to_point(self, point: Point, max_thrust:float=None): target_heading = self.__get_heading_to_point(point) @@ -122,7 +126,6 @@ class Controller(GetParameterNode): def __get_angular_speed(self, err_angle): value = 0 if self.IS_WITH_PID: - # self.get_logger().info(f"{err_angle}=") if abs(err_angle) > 3: value = -self.pid_ang(err_angle) else: @@ -131,8 +134,8 @@ class Controller(GetParameterNode): value = -self.max_thrust * (1 if err_angle > 0 else -1) return copysign(min(abs(self.max_thrust), abs(value)), value) - def __angle_error(self,target_angle, source_angle): - result = (source_angle - target_angle) % 360 + def __angle_error(self, target_angle, source_angle): + result = (target_angle - source_angle) % 360 if result > 180: result -= 360 return result @@ -142,7 +145,7 @@ class Controller(GetParameterNode): Возвращает курс на цель, если положение точки совпадает с целью, то возвращает 0 """ goal = self.__point_to_list(goal) - dx, dy = self.position.x - goal[0], self.position.y - goal[1] + dx, dy = goal[0] - self.position.x, goal[1] - self.position.y ln = (dx**2 + dy**2)**0.5 if ln: dxn = dx/ln diff --git a/ws_nodes_py/settings/mission.py b/ws_nodes_py/settings/mission.py index 7d2561c..84800cc 100644 --- a/ws_nodes_py/settings/mission.py +++ b/ws_nodes_py/settings/mission.py @@ -83,13 +83,8 @@ def get_drop_cube(): else: return [[[DropBox, {"time": 5}], [MoveByTwist, {"goal": [0, 0]}]]] -drop_stakan = [ - [MoveToGoal, {"goal": "red_position", "dist_treshold": 0.15, "navigation_marker": "red_position", "is_real_compass": False, "is_update_pos": False}], - [DropPickStakan, {"time": 5, "state": 1, "is_wait_pickup": True}], - [DropPickStakan, {"time": 15, "state": 0}], -] -move_to_black_island = [ +move_to_black_island_1 = [ # TODO время подобранно для симулятора # WARN в реальности это не прокатит @@ -122,6 +117,28 @@ move_to_black_island = [ ], ] +move_to_black_island_2 = [ + # TODO время подобранно для симулятора + # WARN в реальности это не прокатит + + # TODO отсюда экстраполируем своё положение (подставляем координаты жёлтых ворот) + # WARN если включаем экстраполяцию положения, то не стоит одновременно использовать linear и angular + # WARN если используем виртуальный компас, то лучше использовать константную угловую скорость 0.15 (отключить пиды) + + # TODO Общий таймер на этап, если сделаем прохождение сбора шаров + # (надо придумать, как это можно сделать) + [ # Отходим от ворот + [MoveByTwist, {"goal": [0.02, 0.15]}], + [Wait, {"time": 42}], + # TODO выход по курсу + ], [ # Идём вперёд, пока не увидим чёрный круг + [MoveByTwist, {"goal": [0.15, 0]}], + [FindObject, {"object_name": "black_position", "min_max_angle": [0, 360]}], + ], [ # Сближаемся с чёрным кругом + [MoveToGoal, {"goal": "black_position", "dist_treshold": 0.11, "navigation_marker": "black_position", "is_real_compass": False, "is_update_pos": False}], + ], +] + move_around_black_island = [ [ # Отходим от острова [MoveByTwist, {"goal": [-0.15, 0]}], @@ -140,7 +157,21 @@ move_around_black_island = [ ], ] -second_step = move_to_black_island + get_drop_cube() + move_around_black_island +move_to_red_point = [ + [MoveToGoal, {"goal": "red_position", "dist_treshold": 0.15, "navigation_marker": "red_position", "is_real_compass": False, "is_update_pos": False}], +] + +drop_stakan = [ + [ + [DropPickStakan, {"time": 5, "state": 1, "is_wait_pickup": True}], + [MoveByTwist, {"goal": [0, 0]}] + ], [ + [DropPickStakan, {"time": 15, "state": 0}], + [MoveByTwist, {"goal": [0, 0]}] + ] +] + +second_step = move_to_black_island_2 + get_drop_cube() + move_around_black_island + move_to_red_point + drop_stakan mission = first_step + second_step @@ -153,6 +184,7 @@ test = [ [FindObject, {"object_name": "red_position", "min_max_angle": [270, 20]}], ] ] -# mission = test +mission = second_step # mission = move_around_black_island -# mission = move_to_yellow_gate \ No newline at end of file +# mission = move_to_yellow_gate +# mission = move_to_red_point + drop_stakan \ No newline at end of file -- GitLab From a7d64062ee693443fdb56243d1b0ad382930a465 Mon Sep 17 00:00:00 2001 From: "toropov.nik" Date: Thu, 10 Oct 2024 22:47:04 +0300 Subject: [PATCH 18/23] hotfix camera debug drawing --- ws_nodes_py/CVs/CVCamera.py | 17 +++++++++-------- ws_nodes_py/CVs/videoD.py | 6 +++--- ws_nodes_py/CVs/videoF.py | 12 ++++++------ ws_nodes_py/CVs/videoS.py | 8 ++++---- 4 files changed, 22 insertions(+), 21 deletions(-) diff --git a/ws_nodes_py/CVs/CVCamera.py b/ws_nodes_py/CVs/CVCamera.py index b8aefeb..8cbaff5 100644 --- a/ws_nodes_py/CVs/CVCamera.py +++ b/ws_nodes_py/CVs/CVCamera.py @@ -32,15 +32,15 @@ class CVCamera(GetParameterNode, ObjectsDetection): def image_processing(self, frame): pass - def publish_coordinte(self, func, name, frame, is_debug=True): + def publish_coordinte(self, func, name, frame, debug_frame): binary = self.get_binary(name, frame) - self.__publish_coordinte(func(self.get_counturs(name, binary, True), frame if is_debug else None), name) + self.__publish_coordinte(func(self.get_counturs(name, binary, True), debug_frame), name) - if is_debug: + if debug_frame is not None: miniBin = cv2.resize(binary, (int(binary.shape[1] / 5), int(binary.shape[0] / 5)), interpolation=cv2.INTER_AREA) miniBin = cv2.cvtColor(miniBin, cv2.COLOR_GRAY2RGB) - frame[-2 - miniBin.shape[0]:-2, 2 + miniBin.shape[1] * self.mask_id:2 + miniBin.shape[1] * (self.mask_id + 1)] = miniBin + debug_frame[-2 - miniBin.shape[0]:-2, 2 + miniBin.shape[1] * self.mask_id:2 + miniBin.shape[1] * (self.mask_id + 1)] = miniBin self.mask_id += 1 def __publish_coordinte(self, res, name): @@ -57,10 +57,11 @@ class CVCamera(GetParameterNode, ObjectsDetection): def __img_callback(self, data): # Получение кадра frame = self.br.compressed_imgmsg_to_cv2(data) + debug_frame = frame.copy() self.mask_id = 0 - self.image_processing(frame) + self.image_processing(frame, debug_frame) # Отладочное изображение - self.__pub_debug.publish(self.br.cv2_to_compressed_imgmsg(frame)) + self.__pub_debug.publish(self.br.cv2_to_compressed_imgmsg(debug_frame)) def get_course(self, contour): moments = cv2.moments(contour) @@ -113,7 +114,7 @@ class CVCamera(GetParameterNode, ObjectsDetection): is_collide_checker = lambda contour: any(point[0][1] >= 330 and point[0][0] <= 476 for point in contour) dist = 0.05 if is_collide_checker(contour) else 1 - course = (self.get_course(contour)-4) % 360 # добавляем 10 градусов, чтобы он шёл чуть правее круга + course = (self.get_course(contour)-10) % 360 islands.append((dist, course)) @@ -172,7 +173,7 @@ class CVCamera(GetParameterNode, ObjectsDetection): if bous is not None: # Проверка границ интерполяции - bous = list(filter(lambda bou: 5 >= bou[0] >= 1.5, bous)) + # bous = list(filter(lambda bou: 5 >= bou[0] >= 1.5, bous)) # Расчёт курса и дистанции до ворот if len(bous) >= 2: diff --git a/ws_nodes_py/CVs/videoD.py b/ws_nodes_py/CVs/videoD.py index 5dfa460..a7df50a 100644 --- a/ws_nodes_py/CVs/videoD.py +++ b/ws_nodes_py/CVs/videoD.py @@ -8,12 +8,12 @@ from math import radians, tan class RosOpencvToBinary(CVCamera): def __init__(self): - super().__init__('camcvD', -8 if is_real else 0) + super().__init__('camcvD', -16 if is_real else 0) self.pool_depth = 1.4 # in meters self.meter_per_pix = (tan(radians(self.fov/2)) * self.pool_depth) / (self.width / 2) - def image_processing(self, frame): - self.publish_coordinte(self.get_bottom, "red_position", frame, True) + def image_processing(self, frame, debug_frame): + self.publish_coordinte(self.get_bottom, "red_position", frame, debug_frame) def main(args=None): diff --git a/ws_nodes_py/CVs/videoF.py b/ws_nodes_py/CVs/videoF.py index f115901..e781954 100644 --- a/ws_nodes_py/CVs/videoF.py +++ b/ws_nodes_py/CVs/videoF.py @@ -10,12 +10,12 @@ class RosOpencvToBinary(CVCamera): self.k1 = self.get_declare_parameter('k1', rclpy.Parameter.Type.DOUBLE, checker=lambda x: True) self.k2 = self.get_declare_parameter('k2', rclpy.Parameter.Type.DOUBLE, checker=lambda x: True) - def image_processing(self, frame): - # self.publish_coordinte(self.get_poster, "red_poster", frame, False) - # self.publish_coordinte(self.get_poster, "green_poster", frame, False) - self.publish_coordinte(self.get_island, "black_position_front", frame, True) - self.publish_coordinte(self.get_gates, "yellow_gate", frame, True) - self.publish_coordinte(self.get_gates, "blue_gate", frame, True) + def image_processing(self, frame, debug_frame): + self.publish_coordinte(self.get_poster, "red_poster", frame, debug_frame) + self.publish_coordinte(self.get_poster, "green_poster", frame, debug_frame) + self.publish_coordinte(self.get_island, "black_position_front", frame, debug_frame) + # self.publish_coordinte(self.get_gates, "yellow_gate", frame, debug_frame) + # self.publish_coordinte(self.get_gates, "blue_gate", frame, debug_frame) def main(args=None): diff --git a/ws_nodes_py/CVs/videoS.py b/ws_nodes_py/CVs/videoS.py index 5860486..3c283bb 100644 --- a/ws_nodes_py/CVs/videoS.py +++ b/ws_nodes_py/CVs/videoS.py @@ -12,10 +12,10 @@ class RosOpencvToBinary(CVCamera): self.k1 = self.get_declare_parameter('k1', rclpy.Parameter.Type.DOUBLE, checker=lambda x: True) self.k2 = self.get_declare_parameter('k2', rclpy.Parameter.Type.DOUBLE, checker=lambda x: True) - def image_processing(self, frame): - self.publish_coordinte(self.get_island, "black_position_side", frame, True) - self.publish_coordinte(self.get_bous, "blue_bou", frame, True) - self.publish_coordinte(self.get_bous, "yellow_bou", frame, True) + def image_processing(self, frame, debug_frame): + self.publish_coordinte(self.get_island, "black_position_side", frame, debug_frame) + self.publish_coordinte(self.get_bous, "blue_bou", frame, debug_frame) + self.publish_coordinte(self.get_bous, "yellow_bou", frame, debug_frame) def main(args=None): -- GitLab From bf3138241d5889b85f5a96b714ca5b8812245780 Mon Sep 17 00:00:00 2001 From: "toropov.nik" Date: Sat, 12 Oct 2024 12:23:40 +0300 Subject: [PATCH 19/23] make vlad24 in virtual pool --- ws_nodes_py/CVs/CVCamera.py | 14 +- ws_nodes_py/CVs/videoD.py | 5 +- ws_nodes_py/CVs/videoF.py | 6 +- ws_nodes_py/CVs/videoS.py | 2 + ws_nodes_py/StateMachine/Controller.py | 11 ++ ws_nodes_py/StateMachine/DefaultStates.py | 1 + ws_nodes_py/StateMachine/MotionStates.py | 21 ++- ws_nodes_py/StateMachine/SpecialStates.py | 98 ++++++++++- ws_nodes_py/StateMachine/state_machine.py | 24 +++ ws_nodes_py/StateMachine/test.py | 5 - .../__pycache__/compass.cpython-310.pyc | Bin 6194 -> 6979 bytes .../__pycache__/regulator.cpython-310.pyc | Bin 6712 -> 6611 bytes .../__pycache__/state_machine.cpython-310.pyc | Bin 14247 -> 0 bytes .../__pycache__/videoD.cpython-310.pyc | Bin 1245 -> 0 bytes .../__pycache__/videoF.cpython-310.pyc | Bin 1577 -> 0 bytes .../__pycache__/videoS.cpython-310.pyc | Bin 1463 -> 0 bytes ws_nodes_py/__pycache__/world.cpython-310.pyc | Bin 4679 -> 4550 bytes ws_nodes_py/compass.py | 50 ++++-- ws_nodes_py/default/ObjectsDetection.py | 87 +++++----- .../ObjectsDetection.cpython-310.pyc | Bin 6385 -> 6825 bytes ws_nodes_py/regulator.py | 24 +-- .../robot_settings.cpython-310.pyc | Bin 379 -> 412 bytes ws_nodes_py/settings/mission.py | 152 ++++++++++++++---- ws_nodes_py/settings/robot_settings.py | 13 +- ws_nodes_py/world.py | 6 +- 25 files changed, 392 insertions(+), 127 deletions(-) delete mode 100644 ws_nodes_py/StateMachine/test.py delete mode 100644 ws_nodes_py/__pycache__/state_machine.cpython-310.pyc delete mode 100644 ws_nodes_py/__pycache__/videoD.cpython-310.pyc delete mode 100644 ws_nodes_py/__pycache__/videoF.cpython-310.pyc delete mode 100644 ws_nodes_py/__pycache__/videoS.cpython-310.pyc diff --git a/ws_nodes_py/CVs/CVCamera.py b/ws_nodes_py/CVs/CVCamera.py index 8cbaff5..e15ed2b 100644 --- a/ws_nodes_py/CVs/CVCamera.py +++ b/ws_nodes_py/CVs/CVCamera.py @@ -11,7 +11,7 @@ from cv_bridge import CvBridge class CVCamera(GetParameterNode, ObjectsDetection): - def __init__(self, name, camera_angle, size=(640, 320)): + def __init__(self, name, camera_angle, size=(640, 480)): super().__init__(name) self.set_pool_masks() @@ -79,7 +79,7 @@ class CVCamera(GetParameterNode, ObjectsDetection): break # TODO расчёт дистанции - dist = 1 + dist = 1 if int(cv2.moments(contour)["m00"]) < 10_000 else 0.1 course = self.get_course(contour) @@ -200,11 +200,13 @@ class CVCamera(GetParameterNode, ObjectsDetection): continue # Вычисление положения - lx = (-cy + self.height / 2) * self.meter_per_pix - ly = (cx - self.width / 2) * self.meter_per_pix - dist = (lx**2 + ly**2)**0.5 + lx = -(cy - self.height / 2) + ly = (cx - self.width / 2) + ln = (lx**2 + ly**2)**0.5 + dist = ln * self.meter_per_pix + # self.get_logger().info(f"{lx=}x{ly=} {ln=} {dist=}") if dist != 0: - course = degrees(acos(lx/dist)) + course = degrees(acos(lx/ln)) if ly < 0: course = 360 - course else: diff --git a/ws_nodes_py/CVs/videoD.py b/ws_nodes_py/CVs/videoD.py index a7df50a..4376390 100644 --- a/ws_nodes_py/CVs/videoD.py +++ b/ws_nodes_py/CVs/videoD.py @@ -9,7 +9,10 @@ from math import radians, tan class RosOpencvToBinary(CVCamera): def __init__(self): super().__init__('camcvD', -16 if is_real else 0) - self.pool_depth = 1.4 # in meters + if is_real: + self.pool_depth = 1.4 # in meters + else: + self.pool_depth = 6 # in meters self.meter_per_pix = (tan(radians(self.fov/2)) * self.pool_depth) / (self.width / 2) def image_processing(self, frame, debug_frame): diff --git a/ws_nodes_py/CVs/videoF.py b/ws_nodes_py/CVs/videoF.py index e781954..b011f29 100644 --- a/ws_nodes_py/CVs/videoF.py +++ b/ws_nodes_py/CVs/videoF.py @@ -11,11 +11,11 @@ class RosOpencvToBinary(CVCamera): self.k2 = self.get_declare_parameter('k2', rclpy.Parameter.Type.DOUBLE, checker=lambda x: True) def image_processing(self, frame, debug_frame): - self.publish_coordinte(self.get_poster, "red_poster", frame, debug_frame) + self.publish_coordinte(self.get_poster, "red_poster", frame, None) # , debug_frame) self.publish_coordinte(self.get_poster, "green_poster", frame, debug_frame) self.publish_coordinte(self.get_island, "black_position_front", frame, debug_frame) - # self.publish_coordinte(self.get_gates, "yellow_gate", frame, debug_frame) - # self.publish_coordinte(self.get_gates, "blue_gate", frame, debug_frame) + self.publish_coordinte(self.get_gates, "yellow_gate", frame, debug_frame) + self.publish_coordinte(self.get_gates, "blue_gate", frame, debug_frame) def main(args=None): diff --git a/ws_nodes_py/CVs/videoS.py b/ws_nodes_py/CVs/videoS.py index 3c283bb..822b78e 100644 --- a/ws_nodes_py/CVs/videoS.py +++ b/ws_nodes_py/CVs/videoS.py @@ -14,8 +14,10 @@ class RosOpencvToBinary(CVCamera): def image_processing(self, frame, debug_frame): self.publish_coordinte(self.get_island, "black_position_side", frame, debug_frame) + self.publish_coordinte(self.get_poster, "green_poster", frame, None) self.publish_coordinte(self.get_bous, "blue_bou", frame, debug_frame) self.publish_coordinte(self.get_bous, "yellow_bou", frame, debug_frame) + self.publish_coordinte(self.get_poster, "red_poster_side", frame, debug_frame) def main(args=None): diff --git a/ws_nodes_py/StateMachine/Controller.py b/ws_nodes_py/StateMachine/Controller.py index 3655c03..2a71e57 100644 --- a/ws_nodes_py/StateMachine/Controller.py +++ b/ws_nodes_py/StateMachine/Controller.py @@ -28,6 +28,8 @@ class Controller(Node): self.detected_objects = [] self.detected_objects_lifetime = 1 # int sec + self.settings = {} + self.running = True self.heading = 0 @@ -70,6 +72,12 @@ class Controller(Node): break else: self.detected_objects.append(msg) + + def set_settings(self, key, val): + self.settings[key] = val + + def get_settings(self): + return self.settings def get_detected_objects(self): self.__remove_old_detected_objects() @@ -115,6 +123,9 @@ class Controller(Node): def heading_callback(self, msg): self.heading = msg.data + + def get_heading(self): + return self.heading def get_declare_parameter(self, name, checker, error_text): value = self.get_parameter(name).value diff --git a/ws_nodes_py/StateMachine/DefaultStates.py b/ws_nodes_py/StateMachine/DefaultStates.py index 5e8f732..8a86f5e 100644 --- a/ws_nodes_py/StateMachine/DefaultStates.py +++ b/ws_nodes_py/StateMachine/DefaultStates.py @@ -27,6 +27,7 @@ class Wait(State): def execute(self, userdata): self.state_change_time = self.cntr.get_clock().now() + self.cntr.get_logger().info(f'Start wait for {self.time} seconds') while not self.cntr.shutdown(): # rclpy.spin_once(self.cntr) if (self.cntr.get_clock().now() - self.state_change_time) > Duration(seconds=self.time): diff --git a/ws_nodes_py/StateMachine/MotionStates.py b/ws_nodes_py/StateMachine/MotionStates.py index 2eef8bb..d04da47 100644 --- a/ws_nodes_py/StateMachine/MotionStates.py +++ b/ws_nodes_py/StateMachine/MotionStates.py @@ -9,7 +9,7 @@ from smach import State class MoveToGoal(State): - def __init__(self, c, goal:str | list=[0, 0], dist_treshold: float=None, navigation_marker: str=None, is_real_compass: bool=None, is_update_pos: bool=None): + def __init__(self, c, goal=[0, 0], dist_treshold: float=None, navigation_marker: str=None, is_real_compass: bool=None, is_update_pos: bool=None): State.__init__(self, outcomes=['complite'], input_keys=['sm_counter'], output_keys=['sm_goal', 'sm_pos', 'sm_error_dist', 'sm_error_angle'],) @@ -26,12 +26,13 @@ class MoveToGoal(State): if not self.__is_started: self.cntr.next_step("MOVE TO", self.goal, self.is_real_compass, self.navigation_marker, self.is_update_pos) self.__is_started = True - + self.cntr.get_logger().info(f'Move to position {self.goal[0], self.goal[1]} m from position {round(self.cntr.position.x, 1), round(self.cntr.position.y, 1)} m, heading {self.cntr.get_heading()} deg') while not self.cntr.shutdown(): self.cntr.update() rclpy.spin_once(self.cntr) error_dist = ((self.goal[0] - self.cntr.position.x)**2 + (self.goal[1] - self.cntr.position.y)**2)**0.5 + # self.cntr.get_logger().info(f"{round(self.angle_error(self.angle_point_error(), self.cntr.heading), 0), round(self.angle_point_error(), 0), round(self.cntr.heading, 0)}") userdata.sm_error_angle = f'{round(self.angle_error(self.angle_point_error(), self.cntr.heading), 0)} deg' userdata.sm_goal = f'{round(self.goal[0], 1)} m\t{round(self.goal[1], 1)} m' userdata.sm_pos = f'{round(self.cntr.position.x, 1)} m\t{round(self.cntr.position.y, 1)} m' @@ -63,7 +64,7 @@ class MoveToGoal(State): class MoveByCircle(State): - def __init__(self, c, goal:str | list=[0, 0], dist:int=None, out_angle:int=None, navigation_marker: str=None, is_real_compass: bool=None, is_update_pos: bool=None): + def __init__(self, c, goal=[0, 0], dist:int=None, out_angle:int=None, navigation_marker: str=None, is_real_compass: bool=None, is_update_pos: bool=None): State.__init__(self, outcomes=['complite'], input_keys=['sm_counter'], output_keys=['sm_goal', 'sm_pos', 'sm_error_dist', 'sm_error_angle'],) @@ -78,6 +79,7 @@ class MoveByCircle(State): def execute(self, userdata): self.cntr.next_step("MOVE CIRCLE", self.goal, self.is_real_compass, self.navigation_marker, self.is_update_pos) + self.cntr.get_logger().info(f'Move circle around position {self.goal[0], self.goal[1]} m with radius: {self.goal[2]} m from position {round(self.cntr.position.x, 1), round(self.cntr.position.y, 1)} m, heading {self.cntr.get_heading()} deg') while not self.cntr.shutdown(): self.cntr.update() rclpy.spin_once(self.cntr) @@ -114,7 +116,20 @@ class MoveByTwist(State): def execute(self, userdata): self.cntr.next_step("MOVE TWIST", self.goal) + self.cntr.get_logger().info(f'Move by twist {round(self.goal[0] * 100), round(self.goal[1] * 100)} %') while not self.cntr.shutdown(): self.cntr.update() rclpy.spin_once(self.cntr) return 'complite' + + +class MoveByVariableTwist(MoveByTwist): + def __init__(self, c, var=None, **kwargs): + super().__init__(c) + self.param_name = var + self.variants = kwargs + + def execute(self, userdata): + self.goal = self.variants[self.cntr.get_settings()[self.param_name]] + self.cntr.get_logger().info(f'Move by twist {round(self.goal[0] * 100), round(self.goal[1] * 100)} %, use {self.cntr.get_settings()[self.param_name]} parameter') + return super().execute(userdata) \ No newline at end of file diff --git a/ws_nodes_py/StateMachine/SpecialStates.py b/ws_nodes_py/StateMachine/SpecialStates.py index e761129..23cf55f 100644 --- a/ws_nodes_py/StateMachine/SpecialStates.py +++ b/ws_nodes_py/StateMachine/SpecialStates.py @@ -61,29 +61,44 @@ class DropBox(State): class FindObject(State): - def __init__(self, c, object_name: str="", min_max_angle: list=[0, 359]): + def __init__(self, c, object_name: str="", min_max_angle: list=[0, 359], max_dist: float=None, time:int=None): State.__init__(self, outcomes=['complite']) self.cntr = c self.object_name = object_name self.min_angle, self.max_angle = min_max_angle + self.max_dist = max_dist + self.time = time def execute(self, userdata): - self.cntr.get_logger().info(f'Start seraching {self.object_name} in [{self.min_angle}; {self.max_angle}] deg') + self.cntr.get_logger().info(f'Start seraching {self.object_name} in [{self.min_angle}; {self.max_angle}] deg and {self.max_dist} m') + is_last_time_detected = False + detect_time = None while not self.cntr.shutdown(): - # rclpy.spin_once(self.cntr) - # self.cntr.get_logger().info(" ".join(obj.header.frame_id for obj in self.cntr.detected_objects)) + is_detected = False for obj in self.cntr.get_detected_objects(): if self.object_name in obj.header.frame_id: if ((self.max_angle > self.min_angle and self.max_angle >= (obj.point.z % 360) >= self.min_angle or self.max_angle < self.min_angle and (self.min_angle <= (obj.point.z % 360) or self.max_angle >= (obj.point.z % 360)))): - self.cntr.get_logger().info(f'Detect {self.object_name} at {obj.point.z % 360} deg') - return 'complite' + if self.max_dist is None or self.max_dist >= obj.point.x: + is_detected = True + if self.time is None: + self.cntr.get_logger().info(f"{self.object_name} detected at {obj.point.z} deg, {obj.point.x} m") + return 'complite' + elif not is_last_time_detected: + detect_time = self.cntr.get_clock().now() + is_last_time_detected = True + elif (self.cntr.get_clock().now() - detect_time) > Duration(seconds=self.time): + self.cntr.get_logger().info(f"{self.object_name} detected at {obj.point.z} deg, {obj.point.x} m for {self.time} seconds") + return 'complite' + if not is_detected: + detect_time = None + is_last_time_detected = False return 'complite' class IsAchievePosition(State): - def __init__(self, c, goal:str | list=None, dist_treshold: float=None): + def __init__(self, c, goal=None, dist_treshold: float=None): State.__init__(self, outcomes=['complite'], input_keys=['sm_counter'], output_keys=['sm_goal', 'sm_pos', 'sm_error_dist', 'sm_error_angle'],) @@ -97,6 +112,7 @@ class IsAchievePosition(State): self.distance_threshold = dist_treshold def execute(self, userdata): + self.cntr.get_logger().info(f'Start waiting for {self.goal} m position with distance_threshold: {self.distance_threshold} m') while not self.cntr.shutdown(): error_dist = ((self.goal[0] - self.cntr.position.x)**2 + (self.goal[1] - self.cntr.position.y)**2)**0.5 @@ -105,6 +121,7 @@ class IsAchievePosition(State): userdata.sm_error_dist = f'{round(error_dist, 1)} m' if error_dist < self.distance_threshold: + self.cntr.get_logger().info(f"Target position is achived with error_dist: {error_dist}") return 'complite' return 'complite' @@ -124,7 +141,72 @@ class IsAchieveHeading(State): self.delta_angle = delta_angle def execute(self, userdata): + self.cntr.get_logger().info(f'Start waiting for {self.target_angle} deg heading with delta angle: {self.delta_angle} deg') while not self.cntr.shutdown(): - if abs(self.cntr.get_heading() - self.target_angle) < self.delta_angle: + err_ang = self.angle_error(self.cntr.get_heading(), self.target_angle) + if abs(err_ang) < self.delta_angle: + self.cntr.get_logger().info(f"Target angle is achived, exit_err_angle: {err_ang}") return 'complite' return 'complite' + + def angle_error(self, target_angle, source_angle): + result = (source_angle - target_angle) % 360 + if result > 180: + result -= 360 + return result + +class GetPostersDirection(State): + def __init__(self, c): + State.__init__(self, outcomes=['complite']) + self.cntr = c + + def execute(self, userdata): + self.cntr.get_logger().info(f'Start checking posters direction, default: right') + self.cntr.set_settings("move_side", "right") + is_checked = False + while not self.cntr.shutdown(): + if is_checked: + continue + + red_poster = None + green_poster = None + for obj in self.cntr.get_detected_objects(): + if red_poster is None and "red_poster" in obj.header.frame_id: + red_poster = obj + if green_poster is None and "green_poster" in obj.header.frame_id: + green_poster = obj + if red_poster is not None and green_poster is not None: + angle_err = self.angle_error(red_poster.point.z, green_poster.point.z) + if angle_err >= 0: + self.cntr.set_settings("move_side", "right") + else: + self.cntr.set_settings("move_side", "left") + is_checked = True + self.cntr.get_logger().info(f'Posters direction: {self.cntr.get_settings()["move_side"]}') + break + + return 'complite' + + def angle_error(self, target_angle, source_angle): + result = (source_angle - target_angle) % 360 + if result > 180: + result -= 360 + return result + + +class WorldSettings(State): + def __init__(self, c, is_real_compass: bool=None, is_update_pos: bool=None): + State.__init__(self, outcomes=['complite']) + + self.cntr = c + self.is_real_compass = is_real_compass + self.is_update_pos = is_update_pos + + def execute(self, userdata): + self.cntr.get_logger().info(f'Change navigation settings at is_real_compass: {self.is_real_compass}, is_update_pos_and_heading: {self.is_update_pos}') + self.cntr.next_step(is_real_compass=self.is_real_compass, is_update_pos=self.is_update_pos) + + while not self.cntr.shutdown(): + pass + + return 'complite' diff --git a/ws_nodes_py/StateMachine/state_machine.py b/ws_nodes_py/StateMachine/state_machine.py index 52d9598..81234b7 100644 --- a/ws_nodes_py/StateMachine/state_machine.py +++ b/ws_nodes_py/StateMachine/state_machine.py @@ -1,10 +1,34 @@ #!/usr/bin/env python +import rclpy.logging from ws_nodes_py.settings.mission import mission from ws_nodes_py.StateMachine.MissionMachine import MissionMachine +import smach +import rclpy + + +def loginfo(msg): + # print("[ INFO ] : "+str(msg)) + pass + + +def logwarn(msg): + # print("[ WARN ] : "+str(msg)) + pass + + +def logdebug(msg): + # print("[ DEBUG ] : "+str(msg)) + pass + + +def logerr(msg): + # print("[ ERROR ] : "+str(msg)) + pass def main(): + smach.set_loggers(loginfo, logwarn, logdebug, logerr) mm = MissionMachine() mm.load_mission(mission) mm.execute() diff --git a/ws_nodes_py/StateMachine/test.py b/ws_nodes_py/StateMachine/test.py deleted file mode 100644 index bc848fa..0000000 --- a/ws_nodes_py/StateMachine/test.py +++ /dev/null @@ -1,5 +0,0 @@ -for i in range(10): - # break - a = 0 -else: - print("HELLOW") \ No newline at end of file diff --git a/ws_nodes_py/__pycache__/compass.cpython-310.pyc b/ws_nodes_py/__pycache__/compass.cpython-310.pyc index 1f3a10577e2ce55a48a1c733bcd23794f367fd8a..6c57b398f182160c65f89dd3ce12be496fa23c9a 100644 GIT binary patch delta 3216 zcma)8-ESLN6`wmZ9*@W0PW+K?r%fnsmu6E`Qo3m)Q<@UCwA&_Kz$_XZ&&0JeW4mX@ zWLr5HNF^RnAF7K$NIbOru%Z$Vhzg0fJ|OXcgpgLeF#kc`mRE$doO7Kzt+iU!*8Jw& zd(OFY&$+*Q?)}r-fBjmb9giCl{yr`m_40|wiD~w?$7jyvKG zqhh$xN>ucPotPW1#D%Oo2{&0ux~WP^v?EU1%~Uc%Hk>hcyfQB2sFQVbm7I`cPTnn4 z3QW2tS#c}zP_h!Xe4tb&tfZBCC{0!-?I|n0D?`etXR-@fQg|gvxYZTzCKjo<50sk4ZGuD_j{!pqUe<7(- zCABG)viwuFsin~h(tJa{&9eM=@+H>cfo%8dk`?)g^&~T88Hi~=18Y_krLmSWc>5V_ zjbs0Jds0)jV!qN;o1tbFBULFq`{(B`hmG`Hu0;*@Mi4pTz{!eA<=COY6RsEAWzi1WFZ{C`JI{3f|{O}3ud9bo-Clv;!{RTh3Qz}NpvrRS4Jv3`=pn`sdAw{5%;!moTt-{x9w8Z~DnggHK)zufM6s<$*^_ zo8f;CN0-G!S)f`~zY3>G_c=);A2C5qTIct)i88!lM%Ki~5FSL9_PpigrH0!*q*}l= zy>#vk2l>X!K}uiZRs9-^@@M*&nZZBN&nM#MmA2cddY)fYw_`JGli!VfJRw$cWVLRg{z&|n`Q zuYeGfH8~@zO#Vz|nqtV4DCd-z{GdQ<&5h(@pEipK?$;>vSQG0uMnTnzK^#%GWQ-3> zBudJWhU$(=wPt9)ME2g|(EhZLFf*`Zh0r=QwAQe1^{6l0ldhWDP^F`4<46@jC!rA~ z&A1W72i&`BSFJ{?4wP2-KEVzGzn+@86NY7KA1Ad=&EqsulqLB%jicN1UDW>yA1{Zn zz+&-1DBukihDFj{kgJRId;;Xozs9-Er(4ANHt~DEmG%w!6N4 z6`&0e!_Jp^%#V!Dr?S)RFZ@FGe7{D_die{ZKmiJ-eS!ipr^y7$0u_O3UEP=cP)|N! zA1M3EzPcakA={do#a5*r9>XZfC?Tg%QdT8D?Bk(K@`QB24p>yO)SR@hS$(yqsgfe? zhTtMnIs|JiWpF_H5hx4JX~;3W6FiCbw4*H@^KRi_aSJ!H_r8#buHPeTKY{J!0p28< zse|UTk+i`+Wr{UM`2Fm^F9yG~y zgcAu-_J>9K1)6w8glU#whWHoc9F`Atw3OtIj*YXg4JR}@<%e4YRJ7n-{vx~h0}25|sNX>;K&sZTEP6HDaZK}-NpF#zI6zV;Q7|5*QZh| delta 2468 zcma)8O>7%Q6yDiguh;AU*l`@kanhfNZcCdq{ikU{sM53|ZKWhFk}K*unQd^oYn$0k ze?q_&tppOvAq+@J6;&%Cq@q#@IB@_}xFB%`p}lcILY#WzfEwN#hotqPYOMYC&HJ18 z-pqTOpZ0$}5UWL_h6I0O56IlP?v+@ce6g})ut99Cs8NXq?4Y9;bthB|IY!YS67V!T z>_mzYK?m)q6D!7?crh;4x}9*6#iXD^cAJwbrie5yQGGdD__`@1{B0Me~B&EwDg4mNj8r z0Ixl?cg;e9y?U0UJH6m%(_Mm;`%<3NT$yL_O50gv+`1+7tc4laKIR9b5 z%}o31=$Yx$e)QPs$)jVXvnM8}&x}s+4+2vKVV4obSqJpMvH*Mdzk!AA&s5oux}5+f zVLkkoc3|gbQ_CyS7i;<$4yS&5j?&Xoal$y8uV zD|qO6w>;R~@|WPT=e0Q0v{==;`8hqjTLemcm6p9S+lI2I%Ps;$8xf!;9pjI5bMQF} za#-vIU~c9Up>bmHk3!o>h<_K_$B8jb0{pBoSkO^R1Mp+aaxJekTej_)^6VuR1SbF8 zxbTXINu1#zu<#t+0e}d}DH@5%2GL{6u!9`{(z7iPej`{ zcY&#IG%wEN*yzOETj>Yme=L4B!U`ZU!)%m~M|*O+V8LESU8OwZvOZi6Aq?~D(Qa~* z-;RFXDN<}H*{h&5!e5Uy3>B!O@VM9cpRu7$(vo~v7X6^O+u-i2RJ|m-I|UVhDbUYH ziZku2UOc#(+!Q0bz??_6Z8h0+KZ^ z8d`#vhHyiJ;3T;2Ly0!vfg zIHuO#8wdF^JZ=tPE6I=uK{Vi}t)u#QF@t9VH((cp#lKJfMhg7nw&Q!Sw}s1l3X22n zD{h_o#B15c4-L&(RqJY<4d3U9)RlwlcC7Nhho*_>KpS}X;c@YD%hxj!!|33K4)Y&U z6NA_xrYdssHCn>L;oGY?mLGy~SaR!@MV}0TA4%uQEnZCXp%Y-t0sy{Jsd^0S)4b3V zNIiXT{b;FFUzia)z`;G>X8XGt4BZT{8FUe>*Xp))(PMA&hwbTn7)5KvPPe$@kJ|I( zD;~@2lg$Jl&GzkZP@{@&0t}+ISREM`5O4%B>~V&j!^Zhawok*3V`cs@n>xG^<|$-~ zqD6tiBM?2NODc3#8&RRJ^cVC4dWRm!HCVD7=S(BYb}_fe{sN+>Ut%nr7f|Jpe_=to?Smf$<&?!u1|mRgKnF>3wf zoK(-Q3o0Zt6kMP7p!rb`Zo>kllLOkSA3K-OQpOrYdwY zPzILow4q`VoFWWH56;UDUc`$Bdk}UI@g@$2cvR4rfOQY@`{n)lem~xqY>st``kbz- zjAFc?R-bI>2yjEVa0IfV1?dy0FS1Gx8fX%$$T3N*p@k{p1g6m@)-i()v9ZKtRxzj8 zp`17_t=%!|a*a;ss4WX<74^aOsTqaVYC0<_l=SZbeHX0(VuayetI^4 zN_}0b7Q^VYZ|h3{&qd5E7u*pq=F1I0d6v27=mywz<`!w2p1MS+x@W~Ta{!{EY4)aK z=R?)%>PisrOJ}}C#g@4v4}&6!b$(EM;amDm@?B5}Vvvx;tK`YkGn62Nddy9F-~;(d zv$D&5_EhqrMZKD}r#?i^5slF|0h$^Vqt-)ymqZpgfea)Js(5F;g$Lq(>Tvr_>MKA~ zJhI2duHCS<PwoO>yNiumP}HN{fm pil)RA&c#&rg}okl20x`jo+>Eh>ox_?eI~cv+(bOD#8r_0`~|kxwMPH| delta 843 zcmYjQOKTHR6uxKXCX;3|uOySCwM`$^R~Loa;z|gHf(orr1Q*=|<)oz#Qn?w$#iT_N zDJn(rf?yFeA}Hv>l1*0y-H01qxa&gDomk!Y1N6*Plip#zIrlr?dELu=9^alYRtzJ~ zXsv$Y^KZ5c1lW-RMxjqOA)P6Am9-*6WB)tH(L}4k(~_7jXk8WF>-)a@DTPA>#Q@*W|J4$iM;#?FV+$@UNO=1SxLu44d_+ET>`C| zN;~cuKC!-n3A8sj=}cnC=DdY`?=@1j>ZbX5F-Xvx)i@A{R4$w>1 z8M;#k1U*5Ep`foLX!}!`$9^0L{evM6wbQ3P7Z2mGEUAO~ktP5`Mgtyg-BmZi6hKEl zzc;g;zx89Q&^jIYt=Z$@Y8B=;ABC$`FxCN{$@k`TzB)pqsY^_pr=i(3CdO!hu9`v^ z7iZ-utACW&m6}#jd_tc+HGR#GG?jlYs zGHFFjKFm(*Jx(kd(=*W_J1>7_U%IbJPz`>N81)*b^Gbk_@>T8&NO`BbsP)IRIPgDz E0d5P>ApigX diff --git a/ws_nodes_py/__pycache__/state_machine.cpython-310.pyc b/ws_nodes_py/__pycache__/state_machine.cpython-310.pyc deleted file mode 100644 index 1b0307c3d86099e052ddafa38898c1634d94e55b..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 14247 zcmb_jd5m1iS$|i*etphN&uM!+ZhP(B?ImN!UT1Rn0V6?ne)vNldUY*}v)vZh>so^Pp=fUdS2Q}>*lo@<1WX>UYFX@^lG@;kEhF;Vg zM$u@PMYCZQtwyXEYuH7*5iiCYPSMe+o>5N}6L_2TWFu8fg|H;Uta`eUDP|ZJt7jWy z#j(bCalA26oM_~VxyEF1lFRJ+RAah09ez7goZ)t7i?evg>vQxCmpS!)js3;_jRVC4 zoKK)lw54m$Y9c98cQlc5Gq=s+Jo0IgK|bTo)D8l&07zDh0Wuas4goSQCIFcTA$dS@ zViJ(a5ONrhDKQPmbO<>D$c&f;WHy960?3@$2gtqV6>}&Q)#s~yy{vrh?V_v^HgAZ)n;H;T3!&BWl=3Rkr3{hbX_m7 z%78Bw^uT#$U6%c7tBJI=)DmtX7Q`>rTV?;L(?RTFqwQ}5*3+$4J&0Xy`X`?X?5n=4 zHrG%+bF(GuqSPqM>#p<)y3EjEj3Bmrv+DUl?1fgf=>wbvxIzORd{SY?dkqO!E9yck z8bU9cpVn4&VF>e%Q?x{0#DslEFUCY%I9Lc<92QA8j?HmI+D%|n;v(xNR+C~(jNgeB zlVU>TP%kAW#T4FYF)e2B&WKqthj&)&6Z`QV69?S!RYS~+gLkargjf)V0L^_`v$W#m zRjqI&NM2|)ec7tlU3sYjJdPepI|wF%_s_`r+HGx17y3P2*Sgx4aZlSa?;%yPkc%N_ z3xmr1_-%qZ;X5Hr3%Lv0q3c7 zIx0QDrK;sL@SjR&qtba*3Mx=N9pwB1HcjOZh57wqK9Bt2a3l$FfLoiVG1iWthQ(`k z*k3@cSy1&sE??mCM^rhl`%!KMD1h&d4Scs>p#9pT;xM%&G(wA|V9N8$z6+$UT&p(S zlJC~tR=YEOm7w_$nnxY!yCSfzE?;?JInX2_O)%DI-M|+cZA7hhwGt%PTIG6}&bC`# zmFO_cXRf)R+2&f9%~e{BcG>d=KTk>5bN#S_ajmlhO{v$b2yYTc?jm_#U%HPd&~M4Z z0CDu~XHQ&fHQW=jcD+-+Ay3Gbck0AVuXNKpv9eyR3raV^7rat?V<)wg-y5B0bWoyMP3E66&xGAf?+nEka z2~=8X-BRGUy*N)r!=1!%f$l@YeA~^xJ8FIw&4<)8)J115oLGOxCC_boEg8g^Vz-H4 z3XdVT-gaf+^zb!EtbxudK-%kp*=*emCKT>hu9cf>Am(br1udgWy|uQ653Oo*wIwIf zSm5-?InlX} zZQ_JM8p}`uqo}WxvGU_Rju=6bSgl0u|=ZpmUw@``7F<^ui~93 zVLyqVmK5J$M)3{|a2COPAELCL#m_Mu!`8!}k<$l%3+94Ozj6NMjXdh*^clS~xeL$x zCp5`Vg_v>gp)@hd3kV+Vx36{e+d9OKUNVq2!nBFB8Kx}~Ma=S`s>N1GRg+KRQ~3y@ zK;H=TPQj8(J&3$Ysbz{Ho-b*-fkV=RHuem1o{cE2r!kkI@%QTNpwAfU^RPdF1SBXz zP4^AN<~@@nZ_V0*K2x(py@()j-$9&E<@`mGzeo|JKdp3N0)yl^G|Y%9PgCkVq9Bd} zLSZ%IrwLB#rF<7f&rn3GD4(ZjWV*JZEgrLj@|=-X+chz;U8>HK>XBEKTjoZIfmLXk zdJm8$=4UF>6tlo6|Id=_eZ*A&{iE@!%=M8&$;N8}c|qyXAYQ&4TY z8)ImpOR8WTP%LPPR4raOKS_%ypAu$M7go58se5D+ka`kZl7zm?L9)+Ag7|9n7V(op zS}vkcl5mzR4tZ67kg~&blpi4&t&>{)A3~~ND~#fAc3GFv#ZENcTRy1HZ9j*vz5R%? zAfzM+$|j=HZ4eg4`}MTZnHt!E=)(#TM(?3?L?|=JboH7h6BJ{CU_8JgipatUEO60s z;JD55O5GKug2AH?bTKkc`5`p*aRf$x8|ZI6=*$j`W{`dCJs?t1c((@zq03W%_Ift4 z1EYNt;Rx!RCYIEP`W@tad-t2hhl5KX^8ldBm++D#5buW*HyhsCX!n;AB))6C-tt?XI&YR zJ_Ew+M!CN3N>ZVM7$g*=a^m8xip!#)5K|jyH6i+fc;%W~fz25txgM18TfVH|!_g_8 z1+0eP(FUb;hZwqU>7D$*RCkquJGdQb20UL{2%&@6jHOGeyWMz=<6T?#MeAnsW$K;w z7rhROg^3NbxpD~pV2;+e9731ZQ8yC zW)kb#x4}(cp6Ke}zZR{2pEB&gA_W`tQLXsgcMyZh*MMgLjK1vqQhpR&7UF8TW6DlR z5H+#op)jkADuH?3-Pm=}iF^169$_bul%tP>)n@fhc3|1T$M?o-(oRSc)>synjQ#}7 zz*>PbLXCr1i~Kl6!>nGyE2V8B@XAA=4es}@dIG!@A4dIxxwN!Ym?F)O5Uo@Sk|k(D zVjUVpDM*(}FRz#DsziQ_`X!A)G7BXp36F~p?GN(Fm$dQnM<^l|BZ<}}QJf^4lvRqZ zQ^dTM#v5cpm89a<>piuE=XHhZ9i?ar5e4R_4sa@MBp-8P`M5LVSdNKbJZp|8xE|5N z&|@NZ4#8VM6nR|Y>}!GFMOY#RABZJvAh#35u96LVsX|}%9!eAbUqE>C2qGUMzXk~< z^fmZwUL_}ml6fSxSWm%<$^vRa<#JlgUIN+H`W zm?77$$WNfg=qA%ww-MeXF`R?$1s0m+QAdX64h)M3ia3to!ayqOwP7P!4Or^aduE z`)((rRwJwhZ{MZ+F)68ek0K&cg~2~TsXh&HiV^M>3Oe%a_KG<7}r|01kekPwivtf$c-0gB(pwRuKMy7d;-1%D{4<{lN+dW-&_jfR1JtE zI{-i`bn%iz>S`jqzCT4p3U|0|Nq~Xr@yrN&ewN@rj;NENfjk7-v{CtgK%1vV(MI|I zNF4+U#pg7o{|fU}{kkPja7#%b6#Re8r(?S!ls$k@pd)bVN~!h~bYSmoVL!uv}Zj;0w0l7mmC3YF0QR0fb8^XFmMG?8Jv( zn4OvM3y%W~)sG6D>};H|VG8g-D3vT#+@U693(awFAdd)+TVgnK~>CWA*<;}6_hQ%rk^tc7-sd?LCriqAwi1-cEP8}sb=NdpRw_^nQE04qbj zSOMxqPYNkQOh(F|MuInuRVQqoUNa42Qzs;-Swd$Nv4pWt1Jr~?Wp?#B4Vt|%M-b@w za1xk6D4Sv+kOc&acjNK`ct-qywnB zA*8rP(FP*y*62#VfmXhP;LRbLfdeh4Pl5U!{79K0iw)M({JSV5uBXqzUSFa`)Wi?{wB&f{1FO6o75-uCYx;?dn#4zwT*rHc$i?l4Gut#t`r zr*bwJ-gC|4l@lxuFgeX)$sRSx;m+MXJqzB10^Uj=k}Y> zas?d6-rk09Z_%ExB=sFQF$~l;N-SM#$*ojZ9gC+?VzN|)9kx${m)k65!@L>h$HKf7 z=Etes+63BQON_^ogA|?&<>wUSG+a=%84+VSUFI6Is4*wlAx#E|Pe;`_sIK8q8m-OW zgSQ>*))EdFrKB4C73Lx*A{m)j>ZDjkUby_?g%4dE<(#jHGD)5z1brXqlq(6pgralLTXP=be-~i3lur#Z!!e*&De9LwA~F8Au)> zkC}RqQktWpQO1`((h$?xM(~Kg%taWQ1(vDbEeMzo$_Vw{^55x|2@ZlAslYp@&4mIRtMWQJ>2R1H8**1C_iVFu@9T zU|(XheTV|+`-nSH8+&ml<}|QXP1yd*PPKMpGNEf&amfDMXnC2#8WAHdh;tX$1}lu@H{60%&Q>Sf7(C=5$Aw8u?hnxvaAm}}j8JP& z%hA*^4#GtmI$4d@Tb*n;fgT>nPosQS+4Tn~cncx43!w#Al3+V_)CBMx0{f!+JL;Oa z*+VyoaM3LEYT*_U_B9@4pTU{-3)RYXct+>|lFog7cgX)rE8g4YH_6B$wYh}D8^k!@ zs+l;>8FhH!&<1{OQta;Aiu}He^lqCTDi#A`k!$X<{Yk7L>kuRd7zJjB2fsTi(3(W3 z@F&RZjS5@9dX!>~aJ3Aa5@$;Gm$WySEB81bE}RT-afUiTokYlIp-TEGK%o;5pY_gh zdj{Hjj_TGj>aC6fgv02T0O!q;G@!bW2V-1wd}~6<1Im&7kokz`CI=iKow(Y%JS(h_ zu-MSR!fl%CBPL5BH-NW&l;!0fVY_=W(dV#!d~B_l@>NqB9=0 z`w&?A8>r8_zAM9uu++fO$BeLmrLi4Y`jLmkQk%7b^V}OBPl-6=qywDH4B#ZL-vN8a zw#N6w$;2Q|CI}1jxGIwmtQOS6Aco5#aM})Q1BHpVkC9`v@Gntxi6Yhz{+v=Hn?Zq>or2avECN-Fi-x)a7*0gQG)#yqJy-zf7#0oAr{%U8H5W6UJZ!j^C38# zDRg#)%?;%Z-;wlVHQb2AfdRaroA8B^Pk0SZ4RYCuaGO^ z6K+1Z_^1RILeb{L-B?xMg!kExi`+ik0mg3yA8q*=v7~>j;_);p>J&c}?+%?1_=ozC%4@biuj)|k(gC!mnkD;Ao+;2iW zj$Wx=0reEDpkSyswo5%~Pt`jb)_a0iX0*M9Xl4uC@Aut}!%JYpE1Xsf`N;@hw~^5M#HEXSCP!j~&6uqO+NRA(cQMClMel zUeo0}!ZuEI9U?|Q4Vn04SnD)A7^?oeqSRAS>fKT5Jy7Q3;=SS&lzZ+Ks=vdn9}(|s znq5bX&BE#RhS3|{%ikCnM}}**N5%od-DMo#ANBeEC^a;;J;t&65aT$+ zuG?Nb8KgFF7~Z;B!u3^m4Xxn$+q2saUfbV$@ZiDFLh$rQ&L+ic1G93WRHj@GUQ%QaRPYlf4Eqw8@Kpi)Jr<`E~Tt zi39x(BLvRXYf|90$WubltT;>m&@q~yEwsW^0< zq7VCOaTnPj4b|FrpI2Agug|o2)1B z86F#KINGLT{t~W~2a}iS?-X9!t! zqg-vCA`R+2h;UO4N_$o}VzzG3&#=C2B*L_AeaCQiPut%#ju`lszd=3gfsyXz44vAb z66<@0t={&xjM-i}wK`l%wXC=7e>0Bu>Tny$pJpDiea&GI$p<0AZ{l1!pFNGc%xGeMI23Fo|z{~~#7-f_C?mZm01 zB~{l$S#2JtY6}B@XE=^KPb4wC8`8~u*R zYpZ;8Oi_Flfmpnv0&=%2@*oGs0FBVPC^L_h?0!V*HvUPWE*+f#>S^97M33XrBpoKC m_4)8nByEl(HE!jaxomDM_wL-|xm0e-nbtnxd@zY?jQaDda@Iv-l)cD=QZj=cb0j?#ANfD(j%hY7VlPFil)=?hg;nikeFVr&t zf`pWl&^jYM$|<*IerR)>J2MhGXT%|)dq7&QxxZhi_oc|=;fLaFl1FkJ)0H<7BM11j z+J!-rDLID+wIzLS<&@hO$bvH=%+`)}C*;KA?iQKSDFtkA5EyG{*#g9TPQb0VXS_Jd z;^A)cXFi`x^wRHD?f082r~_%$jjINwkj7^$$rH_(#28Fa7DdXqDD|;)!5}d@WA}=o zaYqT)$7VefT1WJ| zQk|ngl5)g(!G&Vw_!b@}Tok+Qax76e*p&yvCiUoZdiwYdo9$)zF{ZU_h%xCdbcGop zEthmpJ^-+mk2(G%bdQXl*80)EBIjMJ$v`@;;tb_@HB*Zr}C)|M8ZGl;-d< zVTDEsJqgKI5z7f#$_12U54drwR7qC!yfGhbyU6%sN?=RL#@XEhV&uTB)`yjtb3|%Y~-FF{l}3 qu#hy;Svi%D5Zy3dp(MYrY>)XZ{aEsVpV{ diff --git a/ws_nodes_py/__pycache__/videoF.cpython-310.pyc b/ws_nodes_py/__pycache__/videoF.cpython-310.pyc deleted file mode 100644 index fb52e5fb8ab53b0c5eb4886675c4145c6b504504..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 1577 zcmZ`(L2uhO6ecA~mR%=HwzV6EVHk$?AYh2MoQt4n+7uWHcWAO6bQnR4j3Zl?G*Wiv zz&j;CPyG+cF@K5I0Rwj4ZI|vnD)!t2mBHu7kK}uN-+O%J#zu$0_sw7Lk0~SMPn@iO zfRQIK%u5iGNSc!b|Gk_}yu_mo<|qCOB7Mm&h-At;_Y)=~8OZR0CV>iNYwYQVDmW*w zpB}mqxBmuB;(raA6=7cYU%Gf z0?dhjpY%@3iR$Um_)N@nPnTx5cWU^l=?$h?E>SN^sSK~?ALz48s`6l`n%n4cE+zvh z_U@rK4+JA;-`^o;hx4q+YR-2a=VdB#vj<9Z5K_}hPgFWq8baCqXec_wYqyqn*X@<& z;z9qq3j&AU01>xsV5SwAjh5c+{h_M4RB0}>;+4=)5H)NFbecmj?Vnc&JDOL@hRX4mepBV9NBwwmFMLtABtMp_8^}sr^d`~SJOeBnG>FtrIuL%>mB5|G2G~x z$weV;8*#K?Vo!eoG1}l5lkk5{YUI&wWfL)k4Cyje#$?5C6G_1)@+ob859rxPfruEa=uDg~ytseol=5O^np;t8Y X?_qZ`TOcB@3vRdk&Sn&KD0}}O;}Un8 diff --git a/ws_nodes_py/__pycache__/videoS.cpython-310.pyc b/ws_nodes_py/__pycache__/videoS.cpython-310.pyc deleted file mode 100644 index 1c4f7a35cd0e19105c303443ca021e439640af4f..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 1463 zcmZ`(&u`l{6ecA~mJK^di+1RS9k!wvCZLG7oQt5STMQTqcc`--bRlGsNo3QaM#|0{ zc&DW3ssBTA%)i9zPCf6o!?yS6I!hCj0LjNk^8NVuJ?dVsL!gM@$EP`>dc~i|{n9;z$-#X&{x|&!%Ep7 zPsMa`i9u|kfJ54Lk)79IHraT0_YY;mg)CHVB(HM=glOPEWC{hzw12tMu>EB%UHtsj zug_k-FfrQPMw`?zE7bu2+2$O~Sj+i33Ln5-ID_z7ys<^U4!@x%Uws~p0s-4Jl=0dC zy8Rx86*)(y0Bcr2#0!SJ@fzUHM&lX5hHh)D@?y$sZOf+A)xJx{YA*R$&(qM|spn%= z+9O_QZA4i$(sdAs(yF`?E;ftnG!f1o z;sC!8mYYMH#^xSaa106UT>&NPvT|K0l*Z_#FBzBEqj@9raRs1$hvz%5Wv=kc7`(ZO zESu$Jbsr8{Bz2d@v_lixrRFhS1julo^OO$L*4p{KYn8dP`PdAm>B3^M>%;Q5%|p;# g^XJh+;GlBX@bBSYVeu`KpbKud!%iwpO=@50SIO*aHZeb$ScTO&kW=_195Q-kf>qIVn|`kW-8(da-``zL!N-a(;iO)?e zxW%59SrK25uT)&b2(;Mk7FTION@7VWP^dUWlcmTGC|KkTB4j38@#%^P0hwHkLX2EY OTo4Q-H#hPfV+H{0k1rbl delta 344 zcmX@6d|ZV$pO=@50SG!CFr;7G$ScTO&kE!@199;pAW_4Z#gM|7%~T{=!kEI4!qCbj z$xy=(&s4*(fO#Rq0+tjYD~oA1!(66?jEq3}V2BQ8pbnE1mK2tBpggAP6xO-SVD)T3 zSys3%cAzf*dOX@VpxQWL+CW-0x%_T%7Nr)amc-{K7F2P8nF>Ir!Y%f+%!>Ge{Nf@; zpjWDR6cY1N6re(Gx423RQW8s2LGo4nU@5SG99SeoldZ@OsHw;sM96>$VGtoQc|WhN Zlmw8;#mL7f#mEJO5?~TSZ{j<~3;=w>QvLt{ diff --git a/ws_nodes_py/compass.py b/ws_nodes_py/compass.py index e82666c..17c0f00 100644 --- a/ws_nodes_py/compass.py +++ b/ws_nodes_py/compass.py @@ -4,13 +4,15 @@ import rclpy from ws_nodes_py.default.GetParameterNode import GetParameterNode from ws_nodes_py.settings.robot_settings import is_real from time import time -import math +from math import degrees from std_msgs.msg import Int16, String, Bool from geometry_msgs.msg import Twist from ws_nodes_py.settings.for_world import old_motor_ang_speed, new_motor_ang_speed from collections import deque from numpy import median - +# sudo apt-get install ros--tf-transformations +from tf_transformations import euler_from_quaternion +from sensor_msgs.msg import Imu class Compass(GetParameterNode): # for compass angular speed_caltulation @@ -38,23 +40,28 @@ class Compass(GetParameterNode): self.angular_speed = 0 self.test_heading = 0 self.heading = 0 + self.__imu = [0, 0, 0] self.is_enabled = True if is_real: self.COMPASS_MODE = "AUTO" # FORCE_VIRTUAL, FORCE_REAL, AUTO else: - self.COMPASS_MODE = "FORCE_VIRTUAL" # FORCE_VIRTUAL, FORCE_REAL, AUTO + self.COMPASS_MODE = "AUTO" # FORCE_VIRTUAL, FORCE_REAL, AUTO self.IS_REAL_VORLD = is_real # for zero heading - self.compass_delta = self.get_declare_parameter('delta_compass', rclpy.Parameter.Type.INTEGER, - checker=lambda x: 360 > x > 0, error_text="delta_compass not in (0, 360)") - # self.compass_delta = 0 + if is_real: + self.compass_delta = self.get_declare_parameter('delta_compass', rclpy.Parameter.Type.INTEGER, + checker=lambda x: 360 > x > 0, error_text="delta_compass not in (0, 360)") + else: + self.compass_delta = 0 self.subscribtion(Int16, "compass_topic", self.compass_callback, checker=lambda x: x) self.subscribtion_for_parameter(Twist, "twist_topic", 'ang_throtle', checker=lambda x: x, func=lambda msg: msg.angular.z, default=0) self.subscribtion(String, "reset_topic", self.reset_callback, checker=lambda x: x) self.subscribtion_for_parameter(Bool, "compass_type_topic", 'is_use_real_compass', checker=lambda x: x, func=lambda msg: msg.data, default=False) + self.subscribtion(String, "reset_topic", self.reset_callback, checker=lambda x: x) + self.create_subscription(Imu, '/ws11/imu', self.__imu_callback, 10) self.pub_heading = self.publisher_from_declare(Int16, "heading_topic", checker=lambda x: x) # self.pub_test_heading = self.create_publisher(Int16, 'test_heading', 1) @@ -127,7 +134,7 @@ class Compass(GetParameterNode): # for heading offset self.__compass = new_compass - self.__add_new_compass_value(self, new_compass) + self.__add_new_compass_value(new_compass) if (not self.is_use_real_compass and self.COMPASS_MODE == "AUTO") or self.COMPASS_MODE == "FORCE_VIRTUAL": return @@ -140,10 +147,10 @@ class Compass(GetParameterNode): def reset_callback(self, msg): if msg.data == "reset_heading" and ((self.is_use_real_compass and self.COMPASS_MODE == "AUTO") or self.COMPASS_MODE == "FORCE_REAL"): - self.get_logger().info(f"reset real heading {self.__compass}") + # self.get_logger().info(f"reset real heading {self.__compass}") self.compass_delta = self.__compass elif msg.data == "reset_heading" and ((not self.is_use_real_compass and self.COMPASS_MODE == "AUTO") or self.COMPASS_MODE == "FORCE_VIRTUAL"): - self.get_logger().info(f"reset virtual heading {self.__compass}") + # self.get_logger().info(f"reset virtual heading {self.__compass}") self.heading = 0 elif msg.data == "fix_pos": self.heading = 0 @@ -180,6 +187,31 @@ class Compass(GetParameterNode): self.last_compass.append(compass) except AttributeError: # if first compass data self.last_compass = deque([compass, compass, compass]) + + def __imu_callback(self,data): + q = [data.orientation.x, data.orientation.y, data.orientation.z, data.orientation.w] + (roll, pitch, yaw) = euler_from_quaternion(q) + self.__imu[0] = roll + self.__imu[1] = pitch + self.__imu[2] = yaw + # self.get_logger().info(f"{-round(degrees(self.__imu[2])) % 360}") + new_compass = -round(degrees(self.__imu[2])) % 360 + + if not 360 >= new_compass >= 0 or is_real: + return + + # for heading offset + self.__compass = new_compass + + self.__add_new_compass_value(new_compass) + + if (not self.is_use_real_compass and self.COMPASS_MODE == "AUTO") or self.COMPASS_MODE == "FORCE_VIRTUAL": + return + + # median + self.heading = (median(self.last_compass) - self.compass_delta) % 360 + + self.__pub_heading() def main(): diff --git a/ws_nodes_py/default/ObjectsDetection.py b/ws_nodes_py/default/ObjectsDetection.py index 4c34d01..58f9240 100644 --- a/ws_nodes_py/default/ObjectsDetection.py +++ b/ws_nodes_py/default/ObjectsDetection.py @@ -10,7 +10,18 @@ class ObjectsDetection: self.__settings = { "yellow_gate": { - "binary": [((99, 122, 101), (154, 255, 255)), cv2.COLOR_RGB2HSV_FULL, roi_corners, False], + "binary": [((99, 122, 101), (154, 255, 255)), cv2.COLOR_RGB2HSV_FULL, self.__get_box_borders(*roi_corners), False], + "contours": [ + # Выделение контуров по размерам + 300, None, 3, True, # min_size, max_size, count_counturs, is_max_size + # Проверка, что контур не косается границы (roi_full, 0) - нет проверки + roi_corners, 0, # border, border_offset + # Сортировка пол количеству граней + 0.01, True, 2, # aprox_k is_max_vertices, count_aproxs + ], + }, + "yellow_bou": { + "binary": [((99, 122, 101), (154, 255, 255)), cv2.COLOR_RGB2HSV_FULL, self.__get_box_borders(*roi_corners), False], "contours": [ # Выделение контуров по размерам 600, None, 3, True, # min_size, max_size, count_counturs, is_max_size @@ -21,7 +32,7 @@ class ObjectsDetection: ], }, "blue_gate": { - "binary": [((0, 0, 140), (360, 110, 255)), cv2.COLOR_RGB2YUV, (0, 100, 640, 210), True], + "binary": [((0, 0, 140), (360, 110, 255)), cv2.COLOR_RGB2YUV, self.__get_box_borders(0, 100, 640, 210), True], "contours": [ # Выделение контуров по размерам 600, None, 3, True, # min_size, max_size, count_counturs, is_max_size @@ -32,7 +43,7 @@ class ObjectsDetection: ], }, "blue_bou": { - "binary": [((0, 0, 140), (360, 110, 255)), cv2.COLOR_RGB2YUV, (0, 100, 640, 210), True], + "binary": [((0, 0, 140), (360, 110, 255)), cv2.COLOR_RGB2YUV, self.__get_box_borders(0, 100, 640, 210), True], "contours": [ # Выделение контуров по размерам 300, None, 1, True, # min_size, max_size, count_counturs, is_max_size @@ -43,7 +54,7 @@ class ObjectsDetection: ], }, "red_position": { - "binary": [((168, 81, 100), (203, 255, 255)), cv2.COLOR_RGB2HSV_FULL, roi_full, False], + "binary": [((168, 81, 100), (203, 255, 255)), cv2.COLOR_RGB2HSV_FULL, self.__get_box_borders(*roi_full), False], "contours": [ # Выделение контуров по размерам 300, None, 2, True, # min_size, max_size, count_counturs, is_max_size @@ -54,10 +65,10 @@ class ObjectsDetection: ], }, "black_position_front": { - "binary": [((5, 126, 124), (221, 134, 154)), cv2.COLOR_RGB2YUV, (280, 230, 640, 340), True], + "binary": [((5, 126, 124), (221, 134, 154)), cv2.COLOR_RGB2YUV, [(0, 100), (0, 310), (200, 310), (200, 350), (640, 350), (640,100)], True], "contours": [ # Выделение контуров по размерам - 1500, None, 1, True, # min_size, max_size, count_counturs, is_max_size + 2000, None, 1, True, # min_size, max_size, count_counturs, is_max_size # Проверка, что контур не косается границы (roi_full, 0) - нет проверки roi_full, 0, # border, border_offset # Сортировка пол количеству граней @@ -65,23 +76,16 @@ class ObjectsDetection: ], }, "black_position_side": { - "binary": [((5, 126, 124), (221, 134, 154)), cv2.COLOR_RGB2YUV, (280, 250, 640, 640), True], + "binary": [((5, 126, 124), (221, 134, 154)), cv2.COLOR_RGB2YUV, self.__get_box_borders(0, 190, 640, 640), True], "contours": [ # Выделение контуров по размерам - 500, None, 1, True, # min_size, max_size, count_counturs, is_max_size + 5000, None, 1, True, # min_size, max_size, count_counturs, is_max_size # Проверка, что контур не косается границы (roi_full, 0) - нет проверки roi_full, 0, # border, border_offset # Сортировка пол количеству граней 0.01, True, 1, # aprox_k is_max_vertices, count_aproxs ], }, - # "yellow_gate": [((99, 122, 101), (154, 255, 255)), cv2.COLOR_RGB2HSV_FULL, roi_corners, False], - # "blue_bou": [((0, 0, 140), (360, 110, 255)), cv2.COLOR_RGB2YUV, (0, 100, 640, 210), True], - # "blue_gate": [((0, 0, 140), (360, 110, 255)), cv2.COLOR_RGB2YUV, (0, 100, 640, 210), True], - "red_poster": [((52, 133, 121), (120, 255, 255)), cv2.COLOR_RGB2YUV, roi_corners, False], - # "red_position": [((108, 0, 112), (360, 255, 255)), cv2.COLOR_RGB2HSV_FULL, roi_full, False], - # "red_position": [((154, 101, 100), (212, 255, 255)), cv2.COLOR_RGB2HSV_FULL, roi_full, False], - # "red_position": [((168, 81, 0), (203, 255, 255)), cv2.COLOR_RGB2HSV_FULL, roi_full, False], } def __set_virtual_pool_settings(self): @@ -93,7 +97,7 @@ class ObjectsDetection: "yellow_gate": { "binary": [ ((80, 70, 70), (100, 255, 255)), cv2.COLOR_RGB2HSV, # (min_mask, max_mask), color_space - roi_corners, False # borders, contrast + self.__get_box_borders(*roi_corners), False # borders, contrast ], "contours": [ # Выделение контуров по размерам @@ -107,7 +111,7 @@ class ObjectsDetection: "yellow_bou": { "binary": [ ((80, 70, 70), (100, 255, 255)), cv2.COLOR_RGB2HSV, # (min_mask, max_mask), color_space - roi_corners, False # borders, contrast + self.__get_box_borders(*roi_corners), False # borders, contrast ], "contours": [ # Выделение контуров по размерам @@ -119,7 +123,7 @@ class ObjectsDetection: ], }, "blue_gate": { - "binary": [((0, 83, 0), (19, 255, 255)), cv2.COLOR_RGB2HSV, (0, 100, 640, 250), False], + "binary": [((0, 83, 0), (19, 255, 255)), cv2.COLOR_RGB2HSV, self.__get_box_borders(0, 100, 640, 250), False], "contours": [ # Выделение контуров по размерам 400, None, 3, True, # min_size, max_size, count_counturs, is_max_size @@ -130,7 +134,7 @@ class ObjectsDetection: ], }, "blue_bou": { - "binary": [((0, 83, 0), (19, 255, 255)), cv2.COLOR_RGB2HSV, (0, 100, 640, 250), False], + "binary": [((0, 83, 0), (19, 255, 255)), cv2.COLOR_RGB2HSV, self.__get_box_borders(0, 100, 640, 250), False], "contours": [ # Выделение контуров по размерам 400, None, 3, True, # min_size, max_size, count_counturs, is_max_size @@ -141,7 +145,7 @@ class ObjectsDetection: ], }, "black_position_front": { - "binary": [((0, 0, 0), (180, 255, 20)), cv2.COLOR_RGB2HSV, roi_full, False], + "binary": [((0, 0, 0), (180, 255, 20)), cv2.COLOR_RGB2HSV, self.__get_box_borders(*roi_full), False], "contours": [ # Выделение контуров по размерам 400, None, 2, True, # min_size, max_size, count_counturs, is_max_size @@ -152,7 +156,7 @@ class ObjectsDetection: ], }, "black_position_side": { - "binary": [((0, 0, 0), (180, 255, 20)), cv2.COLOR_RGB2HSV, roi_full, False], + "binary": [((0, 0, 0), (180, 255, 20)), cv2.COLOR_RGB2HSV, self.__get_box_borders(*roi_full), False], "contours": [ # Выделение контуров по размерам 400, None, 2, True, # min_size, max_size, count_counturs, is_max_size @@ -163,29 +167,40 @@ class ObjectsDetection: ], }, "red_poster": { - "binary": [((170, 65, 6), (180, 255, 255)), cv2.COLOR_RGB2HSV, roi_corners, False], + "binary": [((164, 0, 80), (185, 255, 130)), cv2.COLOR_RGB2HSV_FULL, self.__get_box_borders(*roi_full), True], "contours": [ # Выделение контуров по размерам - 100, None, 2, True, # min_size, max_size, count_counturs, is_max_size + 100, None, 1, True, # min_size, max_size, count_counturs, is_max_size # Проверка, что контур не косается границы (roi_full, 0) - нет проверки roi_full, 0, # border, border_offset # Сортировка пол количеству граней - 0.01, True, 2, # aprox_k is_max_vertices, count_aproxs + 0.01, True, 1, # aprox_k is_max_vertices, count_aproxs + ], + }, + "red_poster_side": { + "binary": [((164, 0, 80), (185, 255, 130)), cv2.COLOR_RGB2HSV_FULL, self.__get_box_borders(*roi_full), True], + "contours": [ + # Выделение контуров по размерам + 500, None, 1, True, # min_size, max_size, count_counturs, is_max_size + # Проверка, что контур не косается границы (roi_full, 0) - нет проверки + roi_full, 0, # border, border_offset + # Сортировка пол количеству граней + 0.01, True, 1, # aprox_k is_max_vertices, count_aproxs ], }, "green_poster": { - "binary": [((43, 136, 5), (101, 255, 255)), cv2.COLOR_RGB2HSV, roi_corners, False], + "binary": [((73, 0, 31), (100, 255, 130)), cv2.COLOR_RGB2HSV_FULL, self.__get_box_borders(*roi_corners), True], "contours": [ # Выделение контуров по размерам - 100, None, 2, True, # min_size, max_size, count_counturs, is_max_size + 400, None, 1, True, # min_size, max_size, count_counturs, is_max_size # Проверка, что контур не косается границы (roi_full, 0) - нет проверки roi_full, 0, # border, border_offset # Сортировка пол количеству граней - 0.01, True, 2, # aprox_k is_max_vertices, count_aproxs + 0.01, True, 1, # aprox_k is_max_vertices, count_aproxs ], }, "red_position": { - "binary": [((90, 0, 0), (180, 255, 255)), cv2.COLOR_RGB2HSV, roi_full, False], + "binary": [((90, 0, 0), (180, 255, 255)), cv2.COLOR_RGB2HSV, self.__get_box_borders(*roi_full), False], "contours": [ # Выделение контуров по размерам 100, None, 2, True, # min_size, max_size, count_counturs, is_max_size @@ -195,13 +210,6 @@ class ObjectsDetection: 0.01, True, 2, # aprox_k is_max_vertices, count_aproxs ] }, - # "blue_bou": [((0, 83, 0), (19, 255, 255)), cv2.COLOR_RGB2HSV, (0, 100, 640, 250), False], - # "blue_gate": [((0, 83, 0), (19, 255, 255)), cv2.COLOR_RGB2HSV, (0, 100, 640, 250), False], - # "black_position": [((0, 0, 0), (180, 255, 20)), cv2.COLOR_RGB2HSV, roi_corners, False], - # "black_position": [((0, 0, 0), (0, 0, 0)), cv2.COLOR_RGB2HSV, roi_corners, False], - # "red_poster": [((170, 65, 6), (180, 255, 255)), cv2.COLOR_RGB2HSV, roi_corners, False], - # "green_poster": [((43, 136, 5), (101, 255, 255)), cv2.COLOR_RGB2HSV, roi_corners, False], - # "red_position": [((90, 0, 0), (180, 255, 255)), cv2.COLOR_RGB2HSV, roi_full, False], } def __get_binary(self, frame, mask, color_type, roi_corners, is_more_contrast): @@ -214,10 +222,13 @@ class ObjectsDetection: binary = cv2.inRange(frame, *mask) # Задаём рамку - binary = self.__set_borders(binary, *roi_corners) + binary = self.__set_borders(binary, roi_corners) return binary + def __get_box_borders(self, x1, y1, x2, y2): + return [[x1, y1], [x2, y1], [x2, y2], [x1, y2]] + def __get_counturs(self, binary, min_size, max_size, count_counturs, is_max_size, border, border_offset, aprox_k, is_max_vertices, count_aproxs): # Получение контуров из маски contours = cv2.findContours(binary, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_NONE)[0] @@ -248,13 +259,13 @@ class ObjectsDetection: return contours - def __set_borders(self, frame, x1, y1, x2, y2): + def __set_borders(self, frame, array): if len(frame.shape) == 3: channel_count = frame.shape[2] else: channel_count = 1 - roi_corners = np.array([[(x1,y1), (x2,y1), (x2,y2), (x1,y2),]], dtype=np.int32) + roi_corners = np.array([array], dtype=np.int32) mask = np.zeros(frame.shape, dtype=np.uint8) ignore_mask_color = (255,)*channel_count cv2.fillPoly(mask, roi_corners, ignore_mask_color) diff --git a/ws_nodes_py/default/__pycache__/ObjectsDetection.cpython-310.pyc b/ws_nodes_py/default/__pycache__/ObjectsDetection.cpython-310.pyc index 047d43a5805ca666eb04d703fbc29dbff5557fe6..f433aa5b52c830c4faaa1dd1b22a1d74e7b7eb72 100644 GIT binary patch delta 3052 zcmb7GU1(fI6uvX}XYX$A{$zKPO-xghZli8YQj*&E7b3QjR@1i(ubyMX%$ou5g!y31;H0lMA%mmygrCZh0;RxoVlCb{Q2O*J#*&F zcg~!d`OeJ#*?(zIs1OVW6!?90Pau2C{^iiKV7+s=FEh^Kbq)gKiIjt})t1+D^%wWC}VJ7o49}8sttl{idNb5h4L5QiA zlyej!gjkqGRy5`-XSJ-t8qX6MY)gnlM5~BcEit+#u>d57VKK3IHL)g~4q`Whrwl%}8dG2k+eR;G95zmI8H!U>QF20~cbOJ+ zN8BDFM(8fxeFW@co_3Vc%ykq7;=c!x^7A+}qdWxiXf<3N!xLbZQ}8eXI35`I1fdyq zaq6SI1>{;tD?(^WbuWVAj^QZInNl0wZ?Pk@kfK#|sL65z4jL)y#gI@>cL!mD6m`Er zuN{b@JPD2agtW^=@SUC=7bK_2edhzj=g`f)90M0$rD4&YnJGM-&Sr{sB~a~57UnDd z$(eaOEk05A)08eO?S|xz^pVMFd#dQ%XBVN#74qqH)-I+e3k&dZW^+e$X`^IN6t$Yn<7bKPlK*cqB zb+l$IRvU}g#;|VDPp^$3&%K&nEuEm~G!lmA-lyLX1 z`#fsM>D*;WI~cqlO^9}H1hFoyy=Xx4A6vypAYg{kP|K&B3hubk)L;P(XAVXr<-*2ANN=8Xrp<>BDKzhZ8{yWKT@teQ>f!i?sNX|*m7Py_BD&&hi z;}m%m%-;HNsoo;3Y9;6L8NT=vm>oO|jRuINMu{cH0?9JoJ*z*|@sqF9Hc(2WM642ZHJ=v}egz^1;6a!+YfA z6D|)XMJhTgF3#CN3@iq?lJcT2dMmK%ShVdXQ!<(39n4<@Q3+0+$mH|(3=|qHk7M!i zF>xW<)ho}Ub|95tI_=0;Q@)r6?mL!bh*(6UQE?^uF*z;Xk98V&WAVe{$JhuI^tSl$ z&JBTLUZ8c=9TU&R&y%R=Z0aEWVsBI5pp1f#G2F7U_|!~p*2{ZCynuYnu%k%Qun z#FINEy*A_!!mXJr6lT)18Rw*PoA@%(PlDodqN{ufQ+OA}c@*;QzJS^h6o*jAEH9$= z9*TNwsmY>2x*6~}%^*z0B<4}GWQI)D+-?TVi0QKe95E@wdolF<{OsH!e?bw;Edza9 ZM68%YvjsLkV-LegDh^T@!&)S&{tKtxj&A?} delta 2608 zcmbVNU2Gdg5Wc-TpU=+Ou^s2PI7ypE=+a+DO-o8ip+9LQsGFvRHdO85WG}8$=N#=h zG$|LQ66HrAe#$-}RiaHPDEz%3UU=mhDgh7ZC{GBGL4Bx-gw#qEGi%qOiSmLae>*$# z?ac1X?A~JE?R2yliG(HiUHgFKGW#z@Un0jYz45GiR5Fv7{_0BAgK01P-J1ZE%n>Tj z$y6yx6T~AtO;Ym)$TE%4U{1|RG`c`&-DR0RtgWwVV`805)tpM>RH9)T^kk~j5Dn5q zE+keq2!BcRM;n2YSe4U+oc*+U6;ca2@`wo5dO7GxmO>>sS6gOLWdx2>rU(~g0DfBD zsYhTsSUw^1EAkfpquhB46;&<^fXp;p@I8PTVo?|}4D)l0H3JHlEGFi0Kr>X{2vIU+ z47HNd#!o8kF^s?jw4&6;FDhM=2Ci1iEwHfsJE}1(3j*YC!Zu8mb%9*ITQi|O|2NQ0 z*;LBg(D(|%EC48GV5kJaNM_Klo3#st;yEjqE!n(7%|!h$v#B*zbW1ifRbEuv_~+^& z($DXyFCN?ni&UZ#L)8^k)@1pvdfzZKvV{NDuOdf{ulYBDU8w{(6a91ow!5OAJT1j{ zYZ@W%1yez6AwnbE)7C+M!H^0@EB%AkB{5Pjw3R}~_!q(ENPLARQCTUPL2yPIE{C8c zWYH4I3{x>lI_3v+I0Qm@5LHj0N~x5;R;|JOyRiLg@j;SA-5DN^#d*8l{z#1q)e?kS z|FCX;L~ox*LOF@fG2SOQdkO(-xu~!wVOXk}2rq~mkn=hk+W}x>bq^5*=Xq3#paS+I zF~oXc@Wa*0k6OiPhQ8QFgv|(B5VivNMvmFGQ|$+~2~-d$jh4vX!_OQZwuTSv={qFtejo4M)OXV&ECMn;?-5%h3w=g%?|v^e~viam@_tla1y{I>@>zmZd1QjTOei@kvk1ymZk6UV2R@8*T1lQzJlRvmWlybQ z0A$$%gij;^yGqr!!=o)3gzAEjbHB8K(`P2sDoc@q2v%R zw~VhBXi@ngU~MYx9WF~Sjq41(C;B1)ejEV-@K zgehRaj&<=|x}+0*NT1Q8db=LcV|vgC3x=sx-t>SoJvlYYE&$i>I_Fw^sBJ@Rxm*5b GTl>GHc`uv* diff --git a/ws_nodes_py/regulator.py b/ws_nodes_py/regulator.py index 54ec18f..2fae747 100644 --- a/ws_nodes_py/regulator.py +++ b/ws_nodes_py/regulator.py @@ -24,7 +24,7 @@ class Controller(GetParameterNode): if is_real: self.IS_WITH_PID = True ang_p = 0.015 - ang_d = 0 + ang_d = 0.002 self.IS_WITH_LIN_PID = True lin_p = 0.07 lin_d = 0 @@ -96,9 +96,9 @@ class Controller(GetParameterNode): # Настройка линейной скорости if is_real: - self.__move_to_point(goal, 0.075) + self.__move_to_point(goal, 0.09) else: - self.__move_to_point(goal, 0.025) + self.__move_to_point(goal, 0.3) def __move_to_point(self, point: Point, max_thrust:float=None): target_heading = self.__get_heading_to_point(point) @@ -107,7 +107,8 @@ class Controller(GetParameterNode): error_dist = self.__eval_dist(self.position, point) lin = 0 - if abs(sin(radians(err_angle))*error_dist) < 0.2 or err_angle < 3: + if (abs(sin(radians(err_angle))*error_dist) < 0.2 and False) or abs(err_angle) < 5: + # if abs(err_angle) < 3: if max_thrust is None: max_thrust = self.max_thrust if self.IS_WITH_LIN_PID: @@ -117,8 +118,9 @@ class Controller(GetParameterNode): lin = max_thrust self.__move_by_twist(lin, ang) - def __move_by_twist(self, lin:int | float, ang:int | float): + def __move_by_twist(self, lin, ang): msg = Twist() + # self.get_logger().info(f"{round(lin, 2), round(ang, 2)}") msg.linear.x = float(lin) msg.angular.z = float(ang) self.pub_twist.publish(msg) @@ -126,10 +128,10 @@ class Controller(GetParameterNode): def __get_angular_speed(self, err_angle): value = 0 if self.IS_WITH_PID: - if abs(err_angle) > 3: - value = -self.pid_ang(err_angle) + if abs(err_angle) > 5: + value = self.pid_ang(err_angle) else: - if abs(err_angle) > 3: + if abs(err_angle) > 5: # value = k_p_angular * err_angle value = -self.max_thrust * (1 if err_angle > 0 else -1) return copysign(min(abs(self.max_thrust), abs(value)), value) @@ -140,7 +142,7 @@ class Controller(GetParameterNode): result -= 360 return result - def __get_heading_to_point(self, goal:list | Point | PointStamped): + def __get_heading_to_point(self, goal): """ Возвращает курс на цель, если положение точки совпадает с целью, то возвращает 0 """ @@ -156,12 +158,12 @@ class Controller(GetParameterNode): return ang return 0 - def __eval_dist(self, point_1:list | Point | PointStamped, point_2:list | Point | PointStamped): + def __eval_dist(self, point_1, point_2): point_1 = self.__point_to_list(point_1) point_2 = self.__point_to_list(point_2) return ((point_1[0] - point_2[0])**2 + (point_1[1] - point_2[1])**2)**0.5 - def __point_to_list(self, point:list | Point | PointStamped): + def __point_to_list(self, point): if type(point) is Point: return [point.x, point.y] if type(point) is PointStamped: diff --git a/ws_nodes_py/settings/__pycache__/robot_settings.cpython-310.pyc b/ws_nodes_py/settings/__pycache__/robot_settings.cpython-310.pyc index 36a9fc0577f3b2e7ca29935bad13c2acda8a4a39..49ab0cef939dbba12e63e7d5c4eac9853170982e 100644 GIT binary patch delta 162 zcmey(G>6$IpO=@50SKPhaiuG;Ffcp@agYH>)&Yo%4S+-nLli>_V-#ZwQwnnm3ku0P zhjXHvi#BH!tC60eo*{%XhEPTz%0v%Jfdx(U3@0u)EGGptl!bwXiHVbm5eS(OB-;l8 FVE|sv8JYk9 delta 129 zcmbQk{F})wpO=@50SKb3S<+`QGcY^`agYH!kmCTv#acijg&~R|g)xdTg(-zOg#}8o w&S9JA<08gZ#cHHytY-+Jj3AWh#NCG_M1VS37+9E?IGGrMkO@SxeGw1<01mGc+yDRo diff --git a/ws_nodes_py/settings/mission.py b/ws_nodes_py/settings/mission.py index 84800cc..0b91e21 100644 --- a/ws_nodes_py/settings/mission.py +++ b/ws_nodes_py/settings/mission.py @@ -19,6 +19,20 @@ circle_mission = [ [MoveByCircle, {"goal": [10, 0], "dist": 1, "out_angle": 120, "is_real_compass": is_real_compass, "is_update_pos": True}] ] +on_navigation = [ + [ + [WorldSettings, {"is_real_compass": True, "is_update_pos": True}], + [Wait, {"time": 2}], + ], +] + +off_navigation = [ + [ + [WorldSettings, {"is_real_compass": False, "is_update_pos": False}], + [Wait, {"time": 2}], + ], +] + # FIRST STEP OF VLAD24 MISSION @@ -32,15 +46,16 @@ def trough_gate(color: str): ] move_to_blue_gate = [ - [ # Выезжаем из жёлтых ворот - [MoveByTwist, {"goal": [0.15, 0]}], - [Wait, {"time": 10}], - ], [ # Ищем синий шар - [MoveByTwist, {"goal": [0, 0.15]}], + # [ # Выезжаем из жёлтых ворот + # [MoveByTwist, {"goal": [0.15, 0]}], + # [Wait, {"time": 7}], + # ], + [ # Ищем синий шар + [MoveByTwist, {"goal": [0.10, 0.075 if is_real else 0.13]}], [FindObject, {"object_name": "blue_bou"}], ], [ # Ждём пока он уйдёт в корму - [MoveByTwist, {"goal": [0.15, -0.05]}], - [FindObject, {"object_name": "blue_bou", "min_max_angle": [120, 240]}], + [MoveByTwist, {"goal": [0.10, -0.075 if is_real else -0.1]}], + [FindObject, {"object_name": "blue_bou", "min_max_angle": [90, 270]}], ], # [ # Проезжаем чуть вперёд, чтобы выйти +- перпендикулярно воротам # [MoveByTwist, {"goal": [0.15, 0]}], @@ -55,8 +70,9 @@ move_to_blue_gate = [ move_to_yellow_gate = [ [ # Поворочиваемся вокруг буя, пока не увидим жёлтые ворота - [MoveByCircle, {"goal": "blue_bou", "navigation_marker": "blue_bou"}], - [FindObject, {"object_name": "yellow_gate", "min_max_angle": [350, 10]}], + # [MoveByCircle, {"goal": "blue_bou", "navigation_marker": "blue_bou"}], + [MoveByTwist, {"goal": [0.05 if is_real else 0.02, -0.14]}], + [FindObject, {"object_name": "yellow_gate", "min_max_angle": [0, 360], "time": 2}], ] # [ # Пока не видим 2 шара в боковой камере # [MoveByTwist, {"goal": [0.15, 0.00]}], @@ -70,18 +86,30 @@ move_to_yellow_gate = [ # ] ] -first_step = trough_gate("yellow") + move_to_blue_gate + trough_gate("blue") + move_to_yellow_gate + trough_gate("yellow") +first_step = off_navigation + trough_gate("yellow") + move_to_blue_gate + trough_gate("blue") + move_to_yellow_gate + trough_gate("yellow") -mission = first_step # SECOND AND THIRD STEP OF VLAD24 MISSION def get_drop_cube(): if is_real: - return [[[DropBox, {"time": 5}], [MoveByTwist, {"goal": [0.075, 0]}]]] + return [ + [ # Сближаемся с чёрным кругом + [MoveToGoal, {"goal": "black_position", "dist_treshold": 0.11, "navigation_marker": "black_position_front", "is_real_compass": False, "is_update_pos": False}], + ], + [ + [DropBox, {"time": 5}], + [MoveByTwist, {"goal": [0.075, 0]}] + ] + ] else: - return [[[DropBox, {"time": 5}], [MoveByTwist, {"goal": [0, 0]}]]] + return [ + [ # Сближаемся с чёрным кругом + [MoveToGoal, {"goal": "black_position", "dist_treshold": 0.11, "navigation_marker": "black_position_front", "is_real_compass": False, "is_update_pos": False}], + ], + [[DropBox, {"time": 5}], [MoveByTwist, {"goal": [0, 0]}]] + ] move_to_black_island_1 = [ @@ -106,15 +134,12 @@ move_to_black_island_1 = [ [Wait, {"time": 20}], ], [ # Поворачиваемся в сторону острова [MoveByTwist, {"goal": [0.0, 0.15]}], - [Wait, {"time": 20}], - # TODO выход по курсу - # WARN круг виден только в правой части экрана [0, 20-30]гр + # [Wait, {"time": 25}], + [IsAchieveHeading, {"target_angle": 0}], ], [ # Идём вперёд, пока не увидим чёрный круг [MoveByTwist, {"goal": [0.15, 0]}], [FindObject, {"object_name": "black_position", "min_max_angle": [0, 360]}], - ], [ # Сближаемся с чёрным кругом - [MoveToGoal, {"goal": "black_position", "dist_treshold": 0.11, "navigation_marker": "black_position", "is_real_compass": False, "is_update_pos": False}], - ], + ], ] move_to_black_island_2 = [ @@ -127,16 +152,17 @@ move_to_black_island_2 = [ # TODO Общий таймер на этап, если сделаем прохождение сбора шаров # (надо придумать, как это можно сделать) + *on_navigation, [ # Отходим от ворот [MoveByTwist, {"goal": [0.02, 0.15]}], - [Wait, {"time": 42}], - # TODO выход по курсу - ], [ # Идём вперёд, пока не увидим чёрный круг + # [Wait, {"time": 45}], + [IsAchieveHeading, {"target_angle": 0}], + ], + *off_navigation, + [ # Идём вперёд, пока не увидим чёрный круг [MoveByTwist, {"goal": [0.15, 0]}], [FindObject, {"object_name": "black_position", "min_max_angle": [0, 360]}], - ], [ # Сближаемся с чёрным кругом - [MoveToGoal, {"goal": "black_position", "dist_treshold": 0.11, "navigation_marker": "black_position", "is_real_compass": False, "is_update_pos": False}], - ], + ], ] move_around_black_island = [ @@ -158,7 +184,10 @@ move_around_black_island = [ ] move_to_red_point = [ - [MoveToGoal, {"goal": "red_position", "dist_treshold": 0.15, "navigation_marker": "red_position", "is_real_compass": False, "is_update_pos": False}], + [ + [MoveToGoal, {"goal": "red_position", "navigation_marker": "red_position", "is_real_compass": False, "is_update_pos": False}], + [FindObject, {"max_dist": 0.3, "object_name": "red_position"}], + ] ] drop_stakan = [ @@ -173,18 +202,71 @@ drop_stakan = [ second_step = move_to_black_island_2 + get_drop_cube() + move_around_black_island + move_to_red_point + drop_stakan -mission = first_step + second_step -test = [ +# FOURTH STEP OF VLAD24 MISSION + +set_to_course = [ + *on_navigation, [ - [MoveByCircle, {"goal": "black_position", "navigation_marker": "black_position", "is_real_compass": False, "is_update_pos": False}], - [Wait, {"time": 120}], - # Если шар под кругом, то через него мы не идём - # Для теста смотрим только спереди - [FindObject, {"object_name": "red_position", "min_max_angle": [270, 20]}], + [MoveByTwist, {"goal": [0, 0.15]}], + [IsAchieveHeading, {"target_angle": 155}], + ], + *off_navigation, +] + +move_to_balls = [ + [ + [MoveByTwist, {"goal": [0.15, 0]}], + [FindObject, {"object_name": "red_poster", "min_max_angle": [0, 360], "time": 10}], + ], [ + [MoveToGoal, {"goal": "red_poster", "navigation_marker": "red_poster", "is_real_compass": False, "is_update_pos": False}], + [FindObject, {"object_name": "red_poster", "min_max_angle": [0, 360], "max_dist": 0.5}], + [GetPostersDirection, {}], + ] +] + +balls_collecting = [ + [ + [MoveByTwist, {"goal": [0.01, 0.2]}], + [FindObject, {"object_name": "red_poster", "min_max_angle": [130, 230]}], + ], + [ + [MoveByTwist, {"goal": [0.01, 0.2]}], + [Wait, {"time": 15}], + ] +] + +balls_delivery = [ + [ + [MoveByVariableTwist, {"var": "move_side", "right": [0.04, 0.2], "left": [0.04, -0.2]}], + [FindObject, {"object_name": "green_poster", "min_max_angle": [320, 60]}], + ], [ + [MoveByVariableTwist, {"var": "move_side", "right": [0.04, 0.2], "left": [0.04, -0.2]}], + [Wait, {"time": 10}], ] ] -mission = second_step + + + + + +# mission = trough_gate("blue") +# mission = second_step # mission = move_around_black_island # mission = move_to_yellow_gate -# mission = move_to_red_point + drop_stakan \ No newline at end of file +# mission = move_to_red_point + drop_stakan + +# mission = get_drop_cube() # отладка сброса куба +# mission = move_to_red_point # отладка выхода на красный круг +# mission = move_to_red_point + drop_stakan # отладка выхода на красный круг со сбросом + +# mission = move_to_red_point + +third_step = set_to_course + move_to_balls + balls_collecting + balls_delivery +# mission = move_to_balls + balls_collecting + balls_delivery +# mission = balls_collecting + balls_delivery +# mission = second_step +# balls_delivery + + +mission = first_step + second_step + third_step \ No newline at end of file diff --git a/ws_nodes_py/settings/robot_settings.py b/ws_nodes_py/settings/robot_settings.py index 4fd79e7..bbe73cd 100644 --- a/ws_nodes_py/settings/robot_settings.py +++ b/ws_nodes_py/settings/robot_settings.py @@ -9,14 +9,17 @@ cameras_ports = { # "2.4.6": "camera_down", # R5 # "2.4.5": "camera_down", # третья камера от провода юсб хаба, воткнутого в красный юсб jetson'a - "2.3.1": "camera_front", # в хабе - "2.3.2": "camera_side", # в хабе - "2.3.5": "camera_down", # в хабе + "2.1.1": "camera_front", # в хабе + "2.1.3": "camera_side", # в хабе + "2.1.2": "camera_down", # в хабе + + "2.4.2": "camera_front", # в хабе + "2.4.3": "camera_side", # в хабе + "2.4.1": "camera_down", # в хабе "14.0-3.4.2": "camera_front", # в хабе "14.0-3.4.3": "camera_side", # в хабе - "14.0-3.4.1": "camera_down", # в хабе - + "14.0-3.4.1": "camera_down", # в хабе } camcv = {'F':'camera_front', 'S':'camera_side', 'D':'camera_down'} \ No newline at end of file diff --git a/ws_nodes_py/world.py b/ws_nodes_py/world.py index 7e449f3..b1f68c6 100644 --- a/ws_nodes_py/world.py +++ b/ws_nodes_py/world.py @@ -79,14 +79,14 @@ class World(GetParameterNode): def reset_callback(self, msg): if msg.data == "reset_map": self.position = start_position[:] - self.get_logger().info(f"reset map {self.position}") + # self.get_logger().info(f"reset map {self.position}") elif msg.data == "fix_pos": self.position = [0, 0] - self.get_logger().info(f"reset map {self.position} and fix_pos") + # self.get_logger().info(f"reset map {self.position} and fix_pos") self.is_update_pos = False elif msg.data == "update_pos": self.position = [0, 0] - self.get_logger().info(f"reset map {self.position} and update_pos") + # self.get_logger().info(f"reset map {self.position} and update_pos") self.is_update_pos = True def update_position(self, msg): -- GitLab From b9454440d3e94d364d8634b6e26d9a2df52c13cc Mon Sep 17 00:00:00 2001 From: "toropov.nik" Date: Fri, 18 Oct 2024 15:56:10 +0300 Subject: [PATCH 20/23] last pool settings --- text.adoc | 1 + ws_nodes_py/CVs/CVCamera.py | 11 ++- ws_nodes_py/CVs/videoD.py | 2 +- ws_nodes_py/CVs/videoF.py | 10 +- ws_nodes_py/CVs/videoS.py | 10 +- ws_nodes_py/StateMachine/Controller.py | 4 + ws_nodes_py/StateMachine/SpecialStates.py | 13 +++ ws_nodes_py/compass.py | 15 +-- ws_nodes_py/default/ObjectsDetection.py | 109 +++++++++++++++++----- ws_nodes_py/regulator.py | 6 +- ws_nodes_py/settings/mission.py | 70 +++++++++++--- ws_nodes_py/settings/robot_settings.py | 2 +- 12 files changed, 192 insertions(+), 61 deletions(-) create mode 100644 text.adoc diff --git a/text.adoc b/text.adoc new file mode 100644 index 0000000..4f84a22 --- /dev/null +++ b/text.adoc @@ -0,0 +1 @@ +sad \ No newline at end of file diff --git a/ws_nodes_py/CVs/CVCamera.py b/ws_nodes_py/CVs/CVCamera.py index e15ed2b..5faaace 100644 --- a/ws_nodes_py/CVs/CVCamera.py +++ b/ws_nodes_py/CVs/CVCamera.py @@ -79,9 +79,10 @@ class CVCamera(GetParameterNode, ObjectsDetection): break # TODO расчёт дистанции - dist = 1 if int(cv2.moments(contour)["m00"]) < 10_000 else 0.1 + x, y, w, h = cv2.boundingRect(contour) + dist = 1 if w <= 40 else 0.1 - course = self.get_course(contour) + course = (self.get_course(contour) + 10) % 360 posters.append((dist, course)) @@ -111,10 +112,10 @@ class CVCamera(GetParameterNode, ObjectsDetection): break # TODO расчёт дистанции - is_collide_checker = lambda contour: any(point[0][1] >= 330 and point[0][0] <= 476 for point in contour) + is_collide_checker = lambda contour: any(point[0][1] >= 335 and point[0][0] <= 345 for point in contour) dist = 0.05 if is_collide_checker(contour) else 1 - course = (self.get_course(contour)-10) % 360 + course = (self.get_course(contour) + 10) % 360 islands.append((dist, course)) @@ -204,7 +205,7 @@ class CVCamera(GetParameterNode, ObjectsDetection): ly = (cx - self.width / 2) ln = (lx**2 + ly**2)**0.5 dist = ln * self.meter_per_pix - # self.get_logger().info(f"{lx=}x{ly=} {ln=} {dist=}") + # self.get_logger().info(f"{round(lx)}x{round(ly)} ln={round(ln)} dist={round(dist, 1)}") if dist != 0: course = degrees(acos(lx/ln)) if ly < 0: diff --git a/ws_nodes_py/CVs/videoD.py b/ws_nodes_py/CVs/videoD.py index 4376390..f80bffc 100644 --- a/ws_nodes_py/CVs/videoD.py +++ b/ws_nodes_py/CVs/videoD.py @@ -10,7 +10,7 @@ class RosOpencvToBinary(CVCamera): def __init__(self): super().__init__('camcvD', -16 if is_real else 0) if is_real: - self.pool_depth = 1.4 # in meters + self.pool_depth = 0.9 # in meters else: self.pool_depth = 6 # in meters self.meter_per_pix = (tan(radians(self.fov/2)) * self.pool_depth) / (self.width / 2) diff --git a/ws_nodes_py/CVs/videoF.py b/ws_nodes_py/CVs/videoF.py index b011f29..d1f9331 100644 --- a/ws_nodes_py/CVs/videoF.py +++ b/ws_nodes_py/CVs/videoF.py @@ -11,11 +11,11 @@ class RosOpencvToBinary(CVCamera): self.k2 = self.get_declare_parameter('k2', rclpy.Parameter.Type.DOUBLE, checker=lambda x: True) def image_processing(self, frame, debug_frame): - self.publish_coordinte(self.get_poster, "red_poster", frame, None) # , debug_frame) - self.publish_coordinte(self.get_poster, "green_poster", frame, debug_frame) - self.publish_coordinte(self.get_island, "black_position_front", frame, debug_frame) - self.publish_coordinte(self.get_gates, "yellow_gate", frame, debug_frame) - self.publish_coordinte(self.get_gates, "blue_gate", frame, debug_frame) + self.publish_coordinte(self.get_poster, "red_poster_front", frame, debug_frame if True else None) + self.publish_coordinte(self.get_poster, "green_poster", frame, debug_frame if False else None) + self.publish_coordinte(self.get_island, "black_position_front", frame, debug_frame if False else None) + self.publish_coordinte(self.get_gates, "yellow_gate", frame, debug_frame if False else None) + self.publish_coordinte(self.get_gates, "blue_gate", frame, debug_frame if False else None) def main(args=None): diff --git a/ws_nodes_py/CVs/videoS.py b/ws_nodes_py/CVs/videoS.py index 822b78e..c9478e3 100644 --- a/ws_nodes_py/CVs/videoS.py +++ b/ws_nodes_py/CVs/videoS.py @@ -13,11 +13,11 @@ class RosOpencvToBinary(CVCamera): self.k2 = self.get_declare_parameter('k2', rclpy.Parameter.Type.DOUBLE, checker=lambda x: True) def image_processing(self, frame, debug_frame): - self.publish_coordinte(self.get_island, "black_position_side", frame, debug_frame) - self.publish_coordinte(self.get_poster, "green_poster", frame, None) - self.publish_coordinte(self.get_bous, "blue_bou", frame, debug_frame) - self.publish_coordinte(self.get_bous, "yellow_bou", frame, debug_frame) - self.publish_coordinte(self.get_poster, "red_poster_side", frame, debug_frame) + self.publish_coordinte(self.get_island, "black_position_side", frame, debug_frame if True else None) + self.publish_coordinte(self.get_poster, "green_poster", frame, debug_frame if False else None) + self.publish_coordinte(self.get_bous, "blue_bou", frame, debug_frame if False else None) + self.publish_coordinte(self.get_bous, "yellow_bou", frame, debug_frame if False else None) + self.publish_coordinte(self.get_poster, "red_poster_side", frame, debug_frame if False else None) def main(args=None): diff --git a/ws_nodes_py/StateMachine/Controller.py b/ws_nodes_py/StateMachine/Controller.py index 2a71e57..0feccf2 100644 --- a/ws_nodes_py/StateMachine/Controller.py +++ b/ws_nodes_py/StateMachine/Controller.py @@ -50,6 +50,7 @@ class Controller(Node): self.pub_navigation_marker = self.create_publisher(String, 'navigation_marker', 10) self.drop_stakan = self.create_publisher(Int16, "modbus/auto_probe_drop/write", 10) self.drop_box = self.create_publisher(Int16, "modbus/drop_box/write", 10) + self.drop_zabralo = self.create_publisher(Int16, "modbus/catch_balls/write", 10) self.create_subscription(Point, self.get_declare_parameter('position_topic', lambda x: x, "no pos is described"), self.pos_callback, 10) self.create_subscription(Int16, self.get_declare_parameter('heading_topic', lambda x: x, "no heading is described"), self.heading_callback, 10) @@ -60,6 +61,9 @@ class Controller(Node): # self.create_timer(1 / self.get_declare_parameter('hz', lambda x: x > 0, "no hz is described"), self.update) + def pub_zabralo(self, is_drop): + self.pub(self.drop_zabralo, Int16, 10 if is_drop else 40) + def __remove_old_detected_objects(self): for i in range(len(self.detected_objects) - 1, -1, -1): if self.detected_objects[i].header.stamp.sec + self.detected_objects_lifetime < self.get_clock().now().seconds_nanoseconds()[0]: diff --git a/ws_nodes_py/StateMachine/SpecialStates.py b/ws_nodes_py/StateMachine/SpecialStates.py index 23cf55f..eff92aa 100644 --- a/ws_nodes_py/StateMachine/SpecialStates.py +++ b/ws_nodes_py/StateMachine/SpecialStates.py @@ -60,6 +60,19 @@ class DropBox(State): return 'complite' +class DropZabralo(State): + def __init__(self, c, is_close: bool=None): + State.__init__(self, outcomes=['complite']) + self.cntr = c + + self.is_close = is_close if is_close is not None else True + + def execute(self, userdata): + self.cntr.get_logger().info(f'Drop zabralo') + self.cntr.pub_zabralo(self.is_close) + return 'complite' + + class FindObject(State): def __init__(self, c, object_name: str="", min_max_angle: list=[0, 359], max_dist: float=None, time:int=None): State.__init__(self, outcomes=['complite']) diff --git a/ws_nodes_py/compass.py b/ws_nodes_py/compass.py index 17c0f00..96694e1 100644 --- a/ws_nodes_py/compass.py +++ b/ws_nodes_py/compass.py @@ -10,9 +10,11 @@ from geometry_msgs.msg import Twist from ws_nodes_py.settings.for_world import old_motor_ang_speed, new_motor_ang_speed from collections import deque from numpy import median -# sudo apt-get install ros--tf-transformations -from tf_transformations import euler_from_quaternion -from sensor_msgs.msg import Imu +if not is_real: + # sudo apt-get install ros--tf-transformations + from tf_transformations import euler_from_quaternion + from sensor_msgs.msg import Imu + class Compass(GetParameterNode): # for compass angular speed_caltulation @@ -56,12 +58,13 @@ class Compass(GetParameterNode): else: self.compass_delta = 0 - self.subscribtion(Int16, "compass_topic", self.compass_callback, checker=lambda x: x) + if is_real: + self.subscribtion(Int16, "compass_topic", self.compass_callback, checker=lambda x: x) + else: + self.create_subscription(Imu, 'imu', self.__imu_callback, 10) self.subscribtion_for_parameter(Twist, "twist_topic", 'ang_throtle', checker=lambda x: x, func=lambda msg: msg.angular.z, default=0) self.subscribtion(String, "reset_topic", self.reset_callback, checker=lambda x: x) self.subscribtion_for_parameter(Bool, "compass_type_topic", 'is_use_real_compass', checker=lambda x: x, func=lambda msg: msg.data, default=False) - self.subscribtion(String, "reset_topic", self.reset_callback, checker=lambda x: x) - self.create_subscription(Imu, '/ws11/imu', self.__imu_callback, 10) self.pub_heading = self.publisher_from_declare(Int16, "heading_topic", checker=lambda x: x) # self.pub_test_heading = self.create_publisher(Int16, 'test_heading', 1) diff --git a/ws_nodes_py/default/ObjectsDetection.py b/ws_nodes_py/default/ObjectsDetection.py index 58f9240..de493f2 100644 --- a/ws_nodes_py/default/ObjectsDetection.py +++ b/ws_nodes_py/default/ObjectsDetection.py @@ -5,12 +5,14 @@ from ws_nodes_py.settings.robot_settings import is_real class ObjectsDetection: def __set_real_pool_settings(self): - roi_corners = (0, 100, 640, 340) + roi_corners = (0, 0, 640, 340) roi_full = (0, 0, 640, 640) + collide_down = (-100, 0, 1000, 480) + collide_side = (0, -100, 640, 10000) self.__settings = { "yellow_gate": { - "binary": [((99, 122, 101), (154, 255, 255)), cv2.COLOR_RGB2HSV_FULL, self.__get_box_borders(*roi_corners), False], + "binary": [((66,109,0),(109,255,255)), cv2.COLOR_RGB2HSV, self.__get_box_borders(*roi_corners), False, True], "contours": [ # Выделение контуров по размерам 300, None, 3, True, # min_size, max_size, count_counturs, is_max_size @@ -21,7 +23,7 @@ class ObjectsDetection: ], }, "yellow_bou": { - "binary": [((99, 122, 101), (154, 255, 255)), cv2.COLOR_RGB2HSV_FULL, self.__get_box_borders(*roi_corners), False], + "binary": [((66,109,0),(109,255,255)), cv2.COLOR_RGB2HSV, self.__get_box_borders(*roi_corners), False, True], "contours": [ # Выделение контуров по размерам 600, None, 3, True, # min_size, max_size, count_counturs, is_max_size @@ -32,54 +34,109 @@ class ObjectsDetection: ], }, "blue_gate": { - "binary": [((0, 0, 140), (360, 110, 255)), cv2.COLOR_RGB2YUV, self.__get_box_borders(0, 100, 640, 210), True], + "binary": [((0, 0, 155), (160, 255, 255)), cv2.COLOR_RGB2YUV, self.__get_box_borders(0, 0, 640, 225), False, True], "contours": [ # Выделение контуров по размерам - 600, None, 3, True, # min_size, max_size, count_counturs, is_max_size + 400, None, 3, True, # min_size, max_size, count_counturs, is_max_size # Проверка, что контур не косается границы (roi_full, 0) - нет проверки - (0, 100, 640, 210), 0, # border, border_offset + (0, 0, 640, 225), 0, # border, border_offset # Сортировка пол количеству граней 0.01, True, 2, # aprox_k is_max_vertices, count_aproxs ], }, "blue_bou": { - "binary": [((0, 0, 140), (360, 110, 255)), cv2.COLOR_RGB2YUV, self.__get_box_borders(0, 100, 640, 210), True], + "binary": [((0, 0, 155), (160, 255, 255)), cv2.COLOR_RGB2YUV, self.__get_box_borders(0, 0, 640, 125), False, True], "contours": [ # Выделение контуров по размерам - 300, None, 1, True, # min_size, max_size, count_counturs, is_max_size + 400, None, 1, True, # min_size, max_size, count_counturs, is_max_size # Проверка, что контур не косается границы (roi_full, 0) - нет проверки - (0, 100, 640, 210), 0, # border, border_offset + (0, 0, 640, 125), 0, # border, border_offset # Сортировка пол количеству граней 0.01, True, 1, # aprox_k is_max_vertices, count_aproxs ], }, "red_position": { - "binary": [((168, 81, 100), (203, 255, 255)), cv2.COLOR_RGB2HSV_FULL, self.__get_box_borders(*roi_full), False], + "binary": [((38, 0, 0), (360, 255, 110)), cv2.COLOR_RGB2HSV, self.__get_box_borders(*roi_full), True, True], "contours": [ # Выделение контуров по размерам - 300, None, 2, True, # min_size, max_size, count_counturs, is_max_size + 1_000, None, 2, True, # min_size, max_size, count_counturs, is_max_size # Проверка, что контур не косается границы (roi_full, 0) - нет проверки roi_full, 0, # border, border_offset # Сортировка пол количеству граней 0.01, True, 1, # aprox_k is_max_vertices, count_aproxs ], }, + # "black_position_front": { + # "binary": [((5, 126, 124), (221, 134, 154)), cv2.COLOR_RGB2YUV, [(0, 100), (0, 310), (200, 310), (200, 350), (640, 350), (640,100)], True], + # "contours": [ + # # Выделение контуров по размерам + # 2000, None, 1, True, # min_size, max_size, count_counturs, is_max_size + # # Проверка, что контур не косается границы (roi_full, 0) - нет проверки + # roi_full, 0, # border, border_offset + # # Сортировка пол количеству граней + # 0.01, True, 1, # aprox_k is_max_vertices, count_aproxs + # ], + # }, "black_position_front": { - "binary": [((5, 126, 124), (221, 134, 154)), cv2.COLOR_RGB2YUV, [(0, 100), (0, 310), (200, 310), (200, 350), (640, 350), (640,100)], True], + "binary": [(0, 87), cv2.COLOR_RGB2GRAY, [(0, 200), (0, 325), (295, 325), (295, 375), (640, 375), (640,200)], True, False], "contours": [ # Выделение контуров по размерам - 2000, None, 1, True, # min_size, max_size, count_counturs, is_max_size + 3000, None, 1, True, # min_size, max_size, count_counturs, is_max_size # Проверка, что контур не косается границы (roi_full, 0) - нет проверки - roi_full, 0, # border, border_offset + collide_side, 10, # border, border_offset # Сортировка пол количеству граней 0.01, True, 1, # aprox_k is_max_vertices, count_aproxs ], }, + # "black_position_side": { + # "binary": [((5, 126, 124), (221, 134, 154)), cv2.COLOR_RGB2YUV, self.__get_box_borders(0, 190, 640, 640), True], + # "contours": [ + # # Выделение контуров по размерам + # 5000, None, 1, True, # min_size, max_size, count_counturs, is_max_size + # # Проверка, что контур не косается границы (roi_full, 0) - нет проверки + # roi_full, 0, # border, border_offset + # # Сортировка пол количеству граней + # 0.01, True, 1, # aprox_k is_max_vertices, count_aproxs + # ], + # }, "black_position_side": { - "binary": [((5, 126, 124), (221, 134, 154)), cv2.COLOR_RGB2YUV, self.__get_box_borders(0, 190, 640, 640), True], + "binary": [(0, 87), cv2.COLOR_RGB2GRAY, self.__get_box_borders(0, 150, 640, 640), False, False], + "contours": [ + # Выделение контуров по размерам + 5_000, None, 1, True, # min_size, max_size, count_counturs, is_max_size + # Проверка, что контур не косается границы (roi_full, 0) - нет проверки + collide_down, 10, # border, border_offset + # Сортировка пол количеству граней + 0.01, True, 1, # aprox_k is_max_vertices, count_aproxs + ], + }, + "red_poster_front": { + "binary": [((62, 0, 0), (360, 255, 129)), cv2.COLOR_RGB2HSV, self.__get_box_borders(100, 120, 540, 240), False, True], + "contours": [ + # Выделение контуров по размерам + 100, None, 1, True, # min_size, max_size, count_counturs, is_max_size + # Проверка, что контур не косается границы (roi_full, 0) - нет проверки + roi_full, 0, # border, border_offset + # Сортировка пол количеству граней + 0.01, True, 1, # aprox_k is_max_vertices, count_aproxs + ], + }, + "red_poster_side": { + "binary": [((62, 0, 0), (360, 255, 129)), cv2.COLOR_RGB2HSV, self.__get_box_borders(*roi_corners), False, True], "contours": [ # Выделение контуров по размерам - 5000, None, 1, True, # min_size, max_size, count_counturs, is_max_size + 500, None, 1, True, # min_size, max_size, count_counturs, is_max_size + # Проверка, что контур не косается границы (roi_full, 0) - нет проверки + roi_full, 0, # border, border_offset + # Сортировка пол количеству граней + 0.01, True, 1, # aprox_k is_max_vertices, count_aproxs + ], + }, + "green_poster": { + "binary": [((0, 17, 95), (55, 255, 112)), cv2.COLOR_RGB2HSV, self.__get_box_borders(0, 120, 640, 240), False, False], + "contours": [ + # Выделение контуров по размерам + 20, None, 1, True, # min_size, max_size, count_counturs, is_max_size # Проверка, что контур не косается границы (roi_full, 0) - нет проверки roi_full, 0, # border, border_offset # Сортировка пол количеству граней @@ -96,7 +153,7 @@ class ObjectsDetection: # "yellow_gate": [((0, 0, 0), (255, 255, 255)), cv2.COLOR_RGB2HSV, roi_corners, False], "yellow_gate": { "binary": [ - ((80, 70, 70), (100, 255, 255)), cv2.COLOR_RGB2HSV, # (min_mask, max_mask), color_space + ((70, 115, 0), (93, 255, 255)), cv2.COLOR_RGB2HSV, # (min_mask, max_mask), color_space self.__get_box_borders(*roi_corners), False # borders, contrast ], "contours": [ @@ -110,7 +167,7 @@ class ObjectsDetection: }, "yellow_bou": { "binary": [ - ((80, 70, 70), (100, 255, 255)), cv2.COLOR_RGB2HSV, # (min_mask, max_mask), color_space + ((70, 115, 0), (93, 255, 255)), cv2.COLOR_RGB2HSV, # (min_mask, max_mask), color_space self.__get_box_borders(*roi_corners), False # borders, contrast ], "contours": [ @@ -123,7 +180,7 @@ class ObjectsDetection: ], }, "blue_gate": { - "binary": [((0, 83, 0), (19, 255, 255)), cv2.COLOR_RGB2HSV, self.__get_box_borders(0, 100, 640, 250), False], + "binary": [((0, 83, 0), (19, 255, 255)), cv2.COLOR_RGB2HSV, self.__get_box_borders(0, 100, 640, 250), False, True], "contours": [ # Выделение контуров по размерам 400, None, 3, True, # min_size, max_size, count_counturs, is_max_size @@ -134,7 +191,7 @@ class ObjectsDetection: ], }, "blue_bou": { - "binary": [((0, 83, 0), (19, 255, 255)), cv2.COLOR_RGB2HSV, self.__get_box_borders(0, 100, 640, 250), False], + "binary": [((0, 83, 0), (19, 255, 255)), cv2.COLOR_RGB2HSV, self.__get_box_borders(0, 100, 640, 250), False, True], "contours": [ # Выделение контуров по размерам 400, None, 3, True, # min_size, max_size, count_counturs, is_max_size @@ -145,7 +202,7 @@ class ObjectsDetection: ], }, "black_position_front": { - "binary": [((0, 0, 0), (180, 255, 20)), cv2.COLOR_RGB2HSV, self.__get_box_borders(*roi_full), False], + "binary": [((0, 0, 0), (180, 255, 20)), cv2.COLOR_RGB2HSV, self.__get_box_borders(*roi_full), False, False], "contours": [ # Выделение контуров по размерам 400, None, 2, True, # min_size, max_size, count_counturs, is_max_size @@ -212,10 +269,12 @@ class ObjectsDetection: }, } - def __get_binary(self, frame, mask, color_type, roi_corners, is_more_contrast): + def __get_binary(self, frame, mask, color_type, roi_corners, is_more_contrast, is_more_shapen): # Добавляем контрастности через цветовое пространство LAB if is_more_contrast: frame = self.__set_more_contrast(frame) + if is_more_shapen: + frame = self.__set_more_sharpen(frame) # Переходим в другое цветовое пространство и выделяем маску frame = cv2.cvtColor(frame, color_type) @@ -226,6 +285,12 @@ class ObjectsDetection: return binary + def __set_more_sharpen(self, img): + img = cv2.medianBlur(img, 5) + kernel = np.array([[0,-1,0],[-1,5,-1], [0,-1,0]]) + image = cv2.filter2D(img, -1, kernel) + return image + def __get_box_borders(self, x1, y1, x2, y2): return [[x1, y1], [x2, y1], [x2, y2], [x1, y2]] diff --git a/ws_nodes_py/regulator.py b/ws_nodes_py/regulator.py index 2fae747..874acfc 100644 --- a/ws_nodes_py/regulator.py +++ b/ws_nodes_py/regulator.py @@ -26,7 +26,7 @@ class Controller(GetParameterNode): ang_p = 0.015 ang_d = 0.002 self.IS_WITH_LIN_PID = True - lin_p = 0.07 + lin_p = 0.75 lin_d = 0 else: self.IS_WITH_PID = False @@ -107,7 +107,8 @@ class Controller(GetParameterNode): error_dist = self.__eval_dist(self.position, point) lin = 0 - if (abs(sin(radians(err_angle))*error_dist) < 0.2 and False) or abs(err_angle) < 5: + # self.get_logger().info(f"{abs(sin(radians(err_angle))*error_dist)} {abs(err_angle)}") + if (abs(sin(radians(err_angle))*error_dist) < 0.1 and abs(err_angle) < 90) or abs(err_angle) < 5: # if abs(err_angle) < 3: if max_thrust is None: max_thrust = self.max_thrust @@ -116,6 +117,7 @@ class Controller(GetParameterNode): lin = copysign(min(abs(max_thrust), abs(value)), 1) else: lin = max_thrust + # self.get_logger().info(f"{lin} {ang}") self.__move_by_twist(lin, ang) def __move_by_twist(self, lin, ang): diff --git a/ws_nodes_py/settings/mission.py b/ws_nodes_py/settings/mission.py index 0b91e21..30c19db 100644 --- a/ws_nodes_py/settings/mission.py +++ b/ws_nodes_py/settings/mission.py @@ -41,7 +41,10 @@ def trough_gate(color: str): [ # Двигаемся к воротам пока не увидим, что шар ушёл в корму [Wait, {"time": 100}], [MoveToGoal, {"goal": f"{color}_gate", "navigation_marker": f"{color}_gate", "is_real_compass": False, "is_update_pos": False}], - [FindObject, {"object_name": f"{color}_bou", "min_max_angle": [90, 270]}], + [FindObject, {"object_name": f"{color}_bou", "min_max_angle": [0, 360], "time": 2}], + ], [ + [MoveByTwist, {"goal": [0.10, 0]}], + [FindObject, {"object_name": f"{color}_bou", "min_max_angle": [90, 270], "time": 2}], ] ] @@ -71,7 +74,7 @@ move_to_blue_gate = [ move_to_yellow_gate = [ [ # Поворочиваемся вокруг буя, пока не увидим жёлтые ворота # [MoveByCircle, {"goal": "blue_bou", "navigation_marker": "blue_bou"}], - [MoveByTwist, {"goal": [0.05 if is_real else 0.02, -0.14]}], + [MoveByTwist, {"goal": [0.05 if is_real else 0.02, -0.17]}], [FindObject, {"object_name": "yellow_gate", "min_max_angle": [0, 360], "time": 2}], ] # [ # Пока не видим 2 шара в боковой камере @@ -186,7 +189,7 @@ move_around_black_island = [ move_to_red_point = [ [ [MoveToGoal, {"goal": "red_position", "navigation_marker": "red_position", "is_real_compass": False, "is_update_pos": False}], - [FindObject, {"max_dist": 0.3, "object_name": "red_position"}], + [FindObject, {"max_dist": 0.15, "object_name": "red_position"}], ] ] @@ -216,32 +219,32 @@ set_to_course = [ move_to_balls = [ [ - [MoveByTwist, {"goal": [0.15, 0]}], - [FindObject, {"object_name": "red_poster", "min_max_angle": [0, 360], "time": 10}], + [MoveByTwist, {"goal": [0.10, 0]}], + [FindObject, {"object_name": "red_poster_front", "min_max_angle": [0, 360], "time": 1}], ], [ - [MoveToGoal, {"goal": "red_poster", "navigation_marker": "red_poster", "is_real_compass": False, "is_update_pos": False}], - [FindObject, {"object_name": "red_poster", "min_max_angle": [0, 360], "max_dist": 0.5}], + [MoveToGoal, {"goal": "red_poster", "navigation_marker": "red_poster_front", "is_real_compass": False, "is_update_pos": False}], + [FindObject, {"object_name": "red_poster_front", "min_max_angle": [340, 20], "max_dist": 0.5}], [GetPostersDirection, {}], ] ] balls_collecting = [ [ - [MoveByTwist, {"goal": [0.01, 0.2]}], - [FindObject, {"object_name": "red_poster", "min_max_angle": [130, 230]}], + [MoveByTwist, {"goal": [0.03, 0.2]}], + [FindObject, {"object_name": "red_poster_side", "min_max_angle": [80, 230]}], ], [ - [MoveByTwist, {"goal": [0.01, 0.2]}], + [MoveByTwist, {"goal": [0.03, 0.2]}], [Wait, {"time": 15}], ] ] balls_delivery = [ [ - [MoveByVariableTwist, {"var": "move_side", "right": [0.04, 0.2], "left": [0.04, -0.2]}], + [MoveByVariableTwist, {"var": "move_side", "right": [0.08, 0.2], "left": [0.08, -0.2]}], [FindObject, {"object_name": "green_poster", "min_max_angle": [320, 60]}], ], [ - [MoveByVariableTwist, {"var": "move_side", "right": [0.04, 0.2], "left": [0.04, -0.2]}], + [MoveByVariableTwist, {"var": "move_side", "right": [0.08, 0.2], "left": [0.08, -0.2]}], [Wait, {"time": 10}], ] ] @@ -251,10 +254,10 @@ balls_delivery = [ # mission = trough_gate("blue") +mission = first_step # mission = second_step # mission = move_around_black_island # mission = move_to_yellow_gate -# mission = move_to_red_point + drop_stakan # mission = get_drop_cube() # отладка сброса куба # mission = move_to_red_point # отладка выхода на красный круг @@ -269,4 +272,43 @@ third_step = set_to_course + move_to_balls + balls_collecting + balls_delivery # balls_delivery -mission = first_step + second_step + third_step \ No newline at end of file +# mission = first_step + second_step + third_step + + +around_island_in_real = [ + [ + [MoveByTwist, {"goal": [0.15, 0]}], + [Wait, {"time": 5}], + ], + [ + [MoveByTwist, {"goal": [0.01, 0.15]}], + # [Wait, {"time": 12}], + [FindObject, {"object_name": "black_position_side", "min_max_angle": [330, 90]}], + ], + [ + [MoveByTwist, {"goal": [0.03, -0.15]}], + [Wait, {"time": 90}], + [FindObject, {"object_name": "red_position", "min_max_angle": [270, 20]}], + ], +] + +# mission = [ +# [ +# [MoveByTwist, {"goal": [0.15, 0]}], +# [FindObject, {"object_name": "black_position_front", "min_max_angle": [0, 360]}], +# ]] + get_drop_cube() + +# mission = around_island_in_real + move_to_red_point + drop_stakan + +# mission = [[ +# [MoveByTwist, {"goal": [0.15, 0]}], +# [FindObject, {"object_name": "black_position", "min_max_angle": [0, 360]}], +# ]]+ get_drop_cube() + around_island_in_real + move_to_red_point + drop_stakan + +# mission = [ +# [DropZabralo, {"is_close": False}], +# [Wait, {"time": 5}], +# [DropZabralo, {"is_close": True}], +# ] + +mission = move_to_balls + balls_collecting + [[DropZabralo, {"is_close": True}],] + balls_delivery \ No newline at end of file diff --git a/ws_nodes_py/settings/robot_settings.py b/ws_nodes_py/settings/robot_settings.py index bbe73cd..fb36464 100644 --- a/ws_nodes_py/settings/robot_settings.py +++ b/ws_nodes_py/settings/robot_settings.py @@ -1,4 +1,4 @@ -is_real = False +is_real = True NS = 'ws11' -- GitLab From 0c76737a10fe9bfb230fcfe0d13dc18db4c6a7e1 Mon Sep 17 00:00:00 2001 From: "toropov.nik" Date: Fri, 18 Oct 2024 17:17:46 +0300 Subject: [PATCH 21/23] add mission for tests --- .../__pycache__/compass.cpython-310.pyc | Bin 6979 -> 6887 bytes .../__pycache__/regulator.cpython-310.pyc | Bin 6611 -> 6627 bytes ws_nodes_py/default/ObjectsDetection.py | 20 +-- .../ObjectsDetection.cpython-310.pyc | Bin 6825 -> 7672 bytes .../robot_settings.cpython-310.pyc | Bin 412 -> 412 bytes ws_nodes_py/settings/mission.py | 117 +++++++++++++----- ws_nodes_py/settings/robot_settings.py | 2 +- 7 files changed, 95 insertions(+), 44 deletions(-) diff --git a/ws_nodes_py/__pycache__/compass.cpython-310.pyc b/ws_nodes_py/__pycache__/compass.cpython-310.pyc index 6c57b398f182160c65f89dd3ce12be496fa23c9a..a05162e0e78e872132e67124656ff9dafa774593 100644 GIT binary patch delta 1007 zcmYk4O=uHQ5XX0RUpDJDY11a@rrV@#Qmn+PNQ+t#zoND(QmR%2KN@1U#%N9J%W4$@ zt?0!=g*XQVLG+;B1PQ3%K~(VI#ghkvf+t0K^5De}bY^R-yS!g!{%>aI&Ahx^`*}@p zA{aCkdgeZg{IW_g0k13Fm&BMT4OSkkgVBedgbEhUsT*Mg)7oRMh|TD_g7*@dj66L%6$ zo2RyW>{!f7xiKf{r2b}TWhL8L##s@9PP`>@&~eT81J>hD-!*F!bt2=0G{_|V)U?9s zvtLZSsgJ-`{H`A%9`qL>gRlG&wvm#;!$u5NVA8Mx+bE-?5_F{>zZyfZqncT=9Pqx` zgX87~d}kJXYiT=<19d%hLbahuZG#xJRWH}Q0O-eoV7uKz0dfbSrsa;6WSZSAj111u zc!%*;@E&ZYdSTx_l6O^)*7pMRVx^%^pU=fN4VJajOJR%<(wZD*dnNvA=7XBCHIJ(rQ{BC6E>@Q(*!}mF?c}D_g|Be+K~k<)v9S#B2t(nEFv*9qOC&#{RT2bs zrdg2*GSViU|5~BKGQ-%9{%8`OU^;rG;Uw?- zCnqMy$Bw)5JU)-M(kwniSHsh4eRBpNk9)1w>23~#DOu0?d>`^0!^hAVURwpW{l>O_?@!8-@wu_aC=o+m8SM delta 1170 zcmZuwO>7fa5Z&?cSIC0F{CjW#~CbSTuG)Upk38Y1|>Q4|7hqBt_InE--HqV<{ zKp;VI;SkYukaC1nDiXJr6>3kdM5|tU>9u>VRHcU^aRmuxHsMEzmEQMezL_^OZ{F;` zqrZJF~G={awwNQn-S4%rcU#pLaXImsnIDwmqo`d^2;&Fl?>de&Td#r|83e2VL$?8JQsn>10wm4(Y7aieN zN_j(0<1zDI_6U2oy@C#wz95+LpPM59!#L7Nfp(VJIs7ZxKRL|es~TmA)z?ubNzF&Lqp<0%T22vn+aGOP0`T!c`?U|M z?{JdD`LyaPL1blzBD+YSI~LGMeWG(S@+w{UVQLD#!)GbXeMXej2!g0v@g!HVb*nG& zen%fX#6LT34!LZP*Q&L0an6$s)>;alC*7GvuXsYrs+0*LxqMts?}wlLAJP-BJjYxr zXs$}PSn(XstyW}?55q;75BG*_pHhCGi~t$z3wH>}c7og;Yc8YgNQj9E20!)(|1lDlp0%eP`L4^9`_hMp`0|Zz(lPcp&$}@{gswYPaZnjAQNu~mc zm;Zr4ldZ@KB#;gwY(Ru8h~NVe89+jlxyTj7&H@p6lf8u08Ji{-30-9D-RvSUpO=@50SFwGxYFmzY~)j9Wq%X6L$3Ap#?5A|6F3={O+L=&r>IuKR>RWF zn8K3HP~=_1l)_NTP-I-gki}Wc1SGkNJSRKxdorK2cbhzk|DAveBL@>3BO4-S# zW};e*6DKE%t`MpMsRWr{R1G9F8H;KrON&`BE}R@9b`I!IW$`?Le2}aki0}assgoCq z3p3VC-YRa-C^Y%2_(sunK;dFgfN=o*$HmA2^kdP)$-5=)u}uVN=$JfT(w1G3gPn_! Guo0jC6KZ#D>eqp?cs9B*_n0E zP$HRG_#q|Y#YPTEE_MLX$VHc&oWw>R=lp;if(8QE$RXKVfaIhAQoib$eUVECMwXcB z?wabF>ZZU(3lGpH?;G^}wzNTq^qe1`Az~Ci3!3hAaapr2bQf8?Je1wLT!OX+JYXj z4>X?UnR<$6p^6;OM2zPlRfZSqIX?1{K9sh!1?@KQH6rby4jy@4=G)iJn0sC0qaQQH zZzIi)KAN$FW@lGE9r9(Ae7o??BIG09B;W2tzCEBd!GAC2W_TJa*$2wqpfvgZ&6J6B z2OwQpeVJxFlguzS%s3d!f%P2Xhxym|ct=)@<8N$~CYdpy*&|!f9_@0^gL^sQPV#(x ziy6m0%14@&;u^f**{O;t$m|NV-+>>tZ(#hW4=YT1PoQG9KYIG~X-xYMYzgOc&~`Xv zI%H1ySzz^w(LN7UyFqyRM#T`c$=Xi|Q2@An!#R!@ZQ5Ll;_p8k`_+{%R`0*qrVa5( zm+4u@tBMtwt@&Q)FNwgpg*U`o(Z`Z}2wHsXA#owLF(854UjS5c?VsQ)Q=*8$jgGV= zP`t7+>Z#!f5r`6Cy9H3miJfSs+VpbB#9cu5Puxv&XxYcQFK8wZ^JtY+ZI51M3B=U# z3J`I$O1h%Qqd(Co>0iRkir#*f7(5f>529(`Rrq)Thhn@k(*g9IPoagjQuWVixQ2bK zo3XNth;2#=&b4zvM6kBHT}S$_#?+=-D+*IJGSNotS)zKzy=Pt zWjUD`+9Dy_)`w9f#k#O0n*`N@}OZpv|MV-U2gx*b}x z{xTlHZ4vZOdHvO!vee^nW%}Ifa{mw(fgOg9R}U&FX$H1CCkw)NteP)8Vwk1D+>-0c zV$FA5EXU&hU9UGH@gHe|cF*JKiFvZ z&6R;Zx96%$ZaA@VeJ7eLa*t&}LY0Z29^a*rl9#q{a0kF-@!y!4HB3FN>yOPRW5r&7 zUop+hCo$~DkM8cH>F@W=?Wf<@hcEtkwX#Xttt1`DRTMwEx#}-)*j}#c<^al3CN~im z5&0;eAO~bAS0UOXk0M7IThJm;bZH9-ZLu45Ro3O(5YxY$`&XY)?#e^?tH*tqJYR)& zJNP2MBD=(Sk#sTwos9Oab2M4!Fxnkk(2jNalCCpdT@T|sn49CZkqpDuwoBzJQ{}7O z-F$TvHjl#}RVpG4pkmx*blL%-;uzO{lltAXa9M@)I2FX9kb=J0Mt}l;H?_292$k?4 z=j!ng-EX%a_fP2$t?f^`Q;sJz?Ozg#IJU17%4mN^2o-+cqaI}hfz%l<N=m4}{yZUG z0Fa}-fQOK2U4HfIRdI$W4e?zfQd#9i>QTUo0acBNj4I6BaYVRObq6EOPeBm;7=YqT zI$dP?lWaPPH2+DL2gm!i^+lDwh|wQO{-1vFXRF7zh&)4uuTh-r{3w~5cxcJN_W+`l z$e=*fBDRi_IZBHXMl2qM`a>gPk+H6~Sg0?U5v#LE|Cmi{A80L_m5NYu;%z3%%xI6; z6!O;5;R(iS_`*g`@j_7zLV4uNsDk;bXxg5#ENZDc;RWCI1&$-$bk(cd^57PelN4CV zDoe}MoTxTzX*R0CqRb4rc70Z zsG^9Sb6k`nlNV8g($0~F)2PezqK&e^l_{rDt=k*bGaBz9rPsuYhe=7~bwzf7m7u28 zBSVx!3BG?;jn>imQJ)^AA}Sf-lX_i`j0BqKvk0(m&S>IQ5Gc8@J|(xld`jvoc$O#8 zPHvQW`=%P|7>P_aI>1)PhX}i&5w>-;Dx(`pSo!RG+6IgxB&umxk?;B2qfDy$(v zEHSZv&4}9v#shOs=Sb`Kv#p}HFL{3qSeQ7$ZcC_7JurYxC0IIc8uztUF*L(eR1DKm z0bj~QIC#Gg?veQ;eXn*;zsFjI2ke24uVmHC5k)@|72M$hq7FSaN7hRmLEtBqM+ zJv-5{;JCJWY6}b2{3S2M8STUw*RNIG;4C`v>`hA{W2&W$qN5C&EMyJKfXg+37J@(s zP+{UO_0JO^xda_jPC=x9kyDLooA^@<=<**_)o}h)^lspP8vMly@SC`kF=p@t^clM1 zfvRuBvq{{zHz@3L9sQZe?$TJaS7b)mcW@lAH?i*uzX!}h?Sb}y!N=$_C>v=PwR`Ly z-6|PV0~1LRH1RZ^fYkS{7HMyuZm~67Q|;B(*luic>d~Xzh538&4$ia2_GpXD)5&f8 z)4;(w`{*%B=uhFDF-U(CbKcT9!UTc?=|xphaVbOR?J1=UELlHsQl^@|GajtBTrH+pCB35Q;6+RbiE0TZh22RFMVZ7SuK6f%38 zYLOC|9>Qf)7GV1H!e+2ox3WxUwHZ4 zD^u3F>({5R-Lj^xO!27($C*y117CzTmxUzWoWtd3r4*Nqsu)n)n)aS7spx@v zCXOMpK(#{>n#4Px5bqLLBie1rJ8YlOso4RYoH5N(DhmW%Qpho&C>W`TX4U5H8uF~% zn+Red)Fr-`yi)pr57P(wzNg}Xl2XOPQk>d4QdUS|M=@iUUN*`si*&3WeVmnyl2JBv zmVSzp^3zchHO!&^;j}d}VCP3EubZOhF$l4L7qIm?Thzq}^=Y5B3{Tz2qL`%Qt%^!+ zzOT1TU{XYdHmxu?`9{W=)=Gz&huJ6Zg_B>PmS=&YpUo3h?UhBu}-)p>}+ zl$mj?xQ|ArR9PoD`odNS?C#T(GPP>snnI?QFnm6#IpGj4`MNC3I^kU>u&t`cle$v; zpvz&THedBn4aI&CA1&S>*4v%BNA)h1o_I;sYmxf6J{np#jl^|RQovpzJ5j1Bu_80| zQGHa|34OMDaEMi?cuJ-Yl$La-)-}p440^Q))JFv*jIhq$K>64*TXaEgQrzYS43du0 zsP3uI%z!RUb$*nZqj6So0bIy{OD@V`znW8Zu&3Wkxc(z&5FY~|kmhQx)4b|5oKO}+ z$F;8rhtD8?Cp%V@H!6dP1B&$j`D^oSMI&Vw8=F*-=9b2?L5Y&`G>Kmeq+|gIY z(iWs63*9pZ6sk(B%t~gSm9b@DkfU1|WZ-Nuuvci`Q0}8G8e9Z`KWhux9Zl@xsuv;4 z(Fj;7bh|jpe(~XJMhj6aM(jgmxL+V-zsFz#bzpgHc>dwl9fuw7aC{kkxN}>EXhG*- z`OJ{hQ}1jdbC;HDc9SkR)m{*v{lK3r6qwpYei21SpH~LnQmfxAZNXnvZ zYGSAF(UrnBpB?*WY)b1oW-Ct`Lnp-55*^>yky+8|)T#p!EJ%d1GGsXvKIq?kgI6B% z_!gu??-&qk)fU{~7bW7Y?rbUg1xp>@FcHwQ!&1y3{ZyqSvzF!I7;jlJZ{di@mt5-O z9R3z=Y2r8H_gI?v1A$)?c#QyE28quIsB5R2)Kg~t9rgZ5peL;AkroAQPV(MhA3#>i zGVSebDH~u*(9?XuQmnr5IT_qi-iS<>e(!(zHpMq+Pde-JibNcBJcCJ4=;qbn0sBwkl5R(lqZaOV_@H zn{>2Ywu%#xF^1cPA+ljxo!ae#h#>5Pe%c4oA)=u0Kp2R)Fc8sw7z&-|+?OV8=RO#C z=bn4+@BDe^oO@pGKe4#eoA!8I0{m`1=}I>3yWl-ZhA*6bHF`);YfRtZ=$M|!W%uei zSW;;tK@eK}*5wN3u9uQ zoEJ0+knTqyJ%F_3g*g!{c(qE+H!o?9q?8o2ss+MrJqj@^Xw^lCzvNCVy2HR+5Ez=p zzyigIH_h%J+q2%uPc(DA@B zP(Uw3Gz@fhB_@Ixe3_4dDI5U8or2&Sk3KYnyXgNS& zkuP+DdsQrag{baL)3p%-I)FZB1Dn^Tsed_nKxroHxbF`3~c+te9 z#O^wpa0w}G)-J+El_0Vw6@KuMwftZ$50oanbACpgA%DQSVa2ajz%Q!31z!~SZ z{ZxV%p^zb~e$N9_=tI#i6xpoaDb=qp^)_VM>YQ@@X%#B?nCx2bD?y80$2we%J5gA` zrsF;G0iv;yXg7*%g&W9o%xq!bSnzBA!$OQ zB6$o+3lP&qbuE@jXLC9=E6aQ=o6>aCous;M6pv{$;8i z&JyWj2Pzt7Ux9rV`xm=HmR&?#QsiQ1{3H8T@57bl%7<5U#L70VZ#!FstT-Jeen_NRG_Gl+N4cMdV=?lO)ND3*?}1UN|KKPRh%92~Op_ zd`MU%18`|JU-X1nGU5?wE4$C_|pI7Z_eh519F!WCpCD;XDfVEZEY~Xg#@S6C! z*?R&@RNtw>q027>x8(NVI23r+ zqrqO4Oxw6_K;eu)tFL5>!k0YV%TCkD$*|))+o#+q>Nl{)#jNq2*^1^tn5+>4R|-L%i>G@Ui=ldydp0v`_5 zZ{~sHw4-mJt|xIUZW#J_jC-aXcyNGS32j-=^HfTf>50X%*tAxE$?SIMCOOZphBwMM zIp}ljR=Cf}o7I+@NV2*}XT=H&Y&a5zbWBDTNHq)BHjpUmu5Ih&sso@81J}}xrzR+m zZiO<&=_=otMs}rkzRizb=Yh-w8&XC?B5mZ-khlg2l7F{UiC#qr4lZ*8eAq(!mXk~0 z2N|66RPdpU9!+N|TfCENng9kSy!o1ig-=Y@2huyYU|p4!dj(5JgakGNj}c quVPcyD;~wCINUCZ7*)20y4{$Z$V}5S0(+;v-D>}`*)C?*um2Ydv~Cpu diff --git a/ws_nodes_py/settings/__pycache__/robot_settings.cpython-310.pyc b/ws_nodes_py/settings/__pycache__/robot_settings.cpython-310.pyc index 49ab0cef939dbba12e63e7d5c4eac9853170982e..91dc7a2cb7458e7579788c1134967cba11cbef23 100644 GIT binary patch delta 20 acmbQkJcpS(pO=@50SL||3vJ|{$Or%}1_W*Z delta 20 acmbQkJcpS(pO=@50SKPhac$(D$Or%|ngmw> diff --git a/ws_nodes_py/settings/mission.py b/ws_nodes_py/settings/mission.py index 30c19db..7084759 100644 --- a/ws_nodes_py/settings/mission.py +++ b/ws_nodes_py/settings/mission.py @@ -145,7 +145,17 @@ move_to_black_island_1 = [ ], ] -move_to_black_island_2 = [ +set_course_to_black_island = [ + *on_navigation, + [ # Отходим от ворот + [MoveByTwist, {"goal": [0.02, 0.15]}], + # [Wait, {"time": 45}], + [IsAchieveHeading, {"target_angle": 0}], + ], + *off_navigation, +] + +move_to_black_island_2 = set_course_to_black_island + [ # TODO время подобранно для симулятора # WARN в реальности это не прокатит @@ -155,13 +165,6 @@ move_to_black_island_2 = [ # TODO Общий таймер на этап, если сделаем прохождение сбора шаров # (надо придумать, как это можно сделать) - *on_navigation, - [ # Отходим от ворот - [MoveByTwist, {"goal": [0.02, 0.15]}], - # [Wait, {"time": 45}], - [IsAchieveHeading, {"target_angle": 0}], - ], - *off_navigation, [ # Идём вперёд, пока не увидим чёрный круг [MoveByTwist, {"goal": [0.15, 0]}], [FindObject, {"object_name": "black_position", "min_max_angle": [0, 360]}], @@ -186,6 +189,23 @@ move_around_black_island = [ ], ] +around_island_in_real = [ + [ + [MoveByTwist, {"goal": [0.15, 0]}], + [Wait, {"time": 5}], + ], + [ + [MoveByTwist, {"goal": [0.01, 0.15]}], + # [Wait, {"time": 12}], + [FindObject, {"object_name": "black_position_side", "min_max_angle": [330, 90]}], + ], + [ + [MoveByTwist, {"goal": [0.03, -0.15]}], + [Wait, {"time": 90}], + [FindObject, {"object_name": "red_position", "min_max_angle": [270, 20]}], + ], +] + move_to_red_point = [ [ [MoveToGoal, {"goal": "red_position", "navigation_marker": "red_position", "is_real_compass": False, "is_update_pos": False}], @@ -195,18 +215,18 @@ move_to_red_point = [ drop_stakan = [ [ - [DropPickStakan, {"time": 5, "state": 1, "is_wait_pickup": True}], + [DropPickStakan, {"time": 15, "state": 1, "is_wait_pickup": True}], [MoveByTwist, {"goal": [0, 0]}] ], [ - [DropPickStakan, {"time": 15, "state": 0}], + [DropPickStakan, {"time": 40, "state": 0}], [MoveByTwist, {"goal": [0, 0]}] ] ] -second_step = move_to_black_island_2 + get_drop_cube() + move_around_black_island + move_to_red_point + drop_stakan +second_step = move_to_black_island_2 + get_drop_cube() + (move_around_black_island if not is_real else around_island_in_real) + move_to_red_point + drop_stakan -# FOURTH STEP OF VLAD24 MISSION +# THIRD STEP OF VLAD24 MISSION set_to_course = [ *on_navigation, @@ -249,12 +269,62 @@ balls_delivery = [ ] ] +third_step = set_to_course + move_to_balls + balls_collecting + balls_delivery + +# финальная миссия +# mission = first_step + second_step + third_step + +""" +0) Для всех этапов надо записать БЭКИ +1) Откалибровать компасс +2) Надо проверить корректность работы магнитного компаса в нужных точках (во всём бассейне) +""" + +# Отладка ПЕРВОГО этапа +""" +1) Требуется подстройка масок под бассейн (жёлтые и синие ворота), если они не подходят +(цветовой сегментации, рамок и границ за которыми изображение отбрасывается) +2) Отработка прохождения первого этапа +""" +# mission = first_step # проход через жёлтые, через синие, через жёлтые +# mission = trough_gate("blue") + move_to_yellow_gate + trough_gate("yellow") # через синие, через жёлтые +# mission = trough_gate("yellow") # отдельный проход через жёлтые +# mission = trough_gate("blue") # отдельно проход через синие + +# Отладка ВТОРОГО этапа +""" +0) Настройка НАПРАВЛЕНИЯ камер, на этом этапе это ВАЖНО +1) Требуется подстройка масок под бассейн (чёрный и красный круг), если они не подходят +(цветовой сегментации, рамок и границ за которыми изображение отбрасывается) +2) Настройка угла выхода для направления в район предполагаемого нахождения нахождения чёрного круга +3) Отработать сброс куба и пробоотборника +4) Проверить выход на цель (правильность границ срабатывания) +5) Проверить выполнение второго этапа +""" +# mission = second_step # выход к месту предполагаемого нахождения нефтяного пятна + сброс куба + сброс пробоотборника +# mission = [[DropBox, {"time": 5}],] # сборс куба +# mission = drop_stakan # сборс стакана +# mission = set_course_to_black_island # выход на курс предполагаемого нахождения чёрного круга +# mission = around_island_in_real # движение вокруг нефтяного пятна +# mission = move_to_red_point + drop_stakan # движения к красному кругу + сброс пробоотборника + +# Отладка ТРЕТЬЕГО этапа +""" +1) Требуется подстройка масок под бассейн (красный и зелёный плакат), если они не подходят +2) Настройка угла выхода для направления в район предполагаемого нахождения причала +3) Отработать сбор шаров и доставку в зону разгрузки +4) Проверить выполнение третьего этапа +""" +# mission = third_step +# mission = set_to_course # выход на курс к предполагаемому месту нахождения причальной стенки +# mission = move_to_balls + balls_collecting # отладка сбора шаров +# mission = move_to_balls + balls_collecting + balls_delivery # отладка сбора шаров и доставки в зону разгрузки # mission = trough_gate("blue") -mission = first_step +# mission = first_step # mission = second_step # mission = move_around_black_island # mission = move_to_yellow_gate @@ -265,7 +335,6 @@ mission = first_step # mission = move_to_red_point -third_step = set_to_course + move_to_balls + balls_collecting + balls_delivery # mission = move_to_balls + balls_collecting + balls_delivery # mission = balls_collecting + balls_delivery # mission = second_step @@ -274,24 +343,6 @@ third_step = set_to_course + move_to_balls + balls_collecting + balls_delivery # mission = first_step + second_step + third_step - -around_island_in_real = [ - [ - [MoveByTwist, {"goal": [0.15, 0]}], - [Wait, {"time": 5}], - ], - [ - [MoveByTwist, {"goal": [0.01, 0.15]}], - # [Wait, {"time": 12}], - [FindObject, {"object_name": "black_position_side", "min_max_angle": [330, 90]}], - ], - [ - [MoveByTwist, {"goal": [0.03, -0.15]}], - [Wait, {"time": 90}], - [FindObject, {"object_name": "red_position", "min_max_angle": [270, 20]}], - ], -] - # mission = [ # [ # [MoveByTwist, {"goal": [0.15, 0]}], @@ -311,4 +362,4 @@ around_island_in_real = [ # [DropZabralo, {"is_close": True}], # ] -mission = move_to_balls + balls_collecting + [[DropZabralo, {"is_close": True}],] + balls_delivery \ No newline at end of file +# mission = move_to_balls + balls_collecting + [[DropZabralo, {"is_close": True}],] + balls_delivery \ No newline at end of file diff --git a/ws_nodes_py/settings/robot_settings.py b/ws_nodes_py/settings/robot_settings.py index fb36464..bbe73cd 100644 --- a/ws_nodes_py/settings/robot_settings.py +++ b/ws_nodes_py/settings/robot_settings.py @@ -1,4 +1,4 @@ -is_real = True +is_real = False NS = 'ws11' -- GitLab From 233871d559cc4888d96bdca43a14029aa82e0e36 Mon Sep 17 00:00:00 2001 From: "toropov.nik" Date: Sun, 20 Oct 2024 08:28:07 +0300 Subject: [PATCH 22/23] release --- ws_nodes_py/CVs/CVCamera.py | 4 +- ws_nodes_py/CVs/videoS.py | 2 +- ws_nodes_py/StateMachine/MotionStates.py | 31 +- ws_nodes_py/StateMachine/SpecialStates.py | 2 +- .../__pycache__/regulator.cpython-310.pyc | Bin 6627 -> 6976 bytes ws_nodes_py/regulator.py | 13 + ws_nodes_py/settings/mission.py | 454 +++++++++++------- 7 files changed, 325 insertions(+), 181 deletions(-) diff --git a/ws_nodes_py/CVs/CVCamera.py b/ws_nodes_py/CVs/CVCamera.py index 5faaace..602d6e6 100644 --- a/ws_nodes_py/CVs/CVCamera.py +++ b/ws_nodes_py/CVs/CVCamera.py @@ -80,9 +80,9 @@ class CVCamera(GetParameterNode, ObjectsDetection): # TODO расчёт дистанции x, y, w, h = cv2.boundingRect(contour) - dist = 1 if w <= 40 else 0.1 + dist = 1 if cv2.moments(contour)["m00"] >= 500 else 0.1 - course = (self.get_course(contour) + 10) % 360 + course = (self.get_course(contour) - 10) % 360 posters.append((dist, course)) diff --git a/ws_nodes_py/CVs/videoS.py b/ws_nodes_py/CVs/videoS.py index c9478e3..807290b 100644 --- a/ws_nodes_py/CVs/videoS.py +++ b/ws_nodes_py/CVs/videoS.py @@ -6,7 +6,7 @@ from ws_nodes_py.CVs.CVCamera import CVCamera class RosOpencvToBinary(CVCamera): def __init__(self): - super().__init__('camcvS', 90) + super().__init__('camcvS', -90) # см. ~/Desktop/python/calc.py self.k1 = self.get_declare_parameter('k1', rclpy.Parameter.Type.DOUBLE, checker=lambda x: True) diff --git a/ws_nodes_py/StateMachine/MotionStates.py b/ws_nodes_py/StateMachine/MotionStates.py index d04da47..b3c7cb1 100644 --- a/ws_nodes_py/StateMachine/MotionStates.py +++ b/ws_nodes_py/StateMachine/MotionStates.py @@ -130,6 +130,31 @@ class MoveByVariableTwist(MoveByTwist): self.variants = kwargs def execute(self, userdata): - self.goal = self.variants[self.cntr.get_settings()[self.param_name]] - self.cntr.get_logger().info(f'Move by twist {round(self.goal[0] * 100), round(self.goal[1] * 100)} %, use {self.cntr.get_settings()[self.param_name]} parameter') - return super().execute(userdata) \ No newline at end of file + if self.param_name in self.cntr.get_settings(): + self.goal = self.variants[self.cntr.get_settings()[self.param_name]] + else: + self.goal = self.variants[self.cntr.get_settings()[self.variants.keys()[0]]] + # self.cntr.get_logger().info(f'Move by twist {round(self.goal[0] * 100), round(self.goal[1] * 100)} %, use {self.cntr.get_settings()[self.param_name]} parameter') + return super().execute(userdata) + + +class MoveByHeading(State): + def __init__(self, c, tar_heading:int=None, thrust:float=None): + State.__init__(self, outcomes=['complite'], input_keys=['sm_counter'], + output_keys=['sm_goal', 'sm_pos', 'sm_error_dist', 'sm_error_angle'],) + + self.cntr = c + + if tar_heading is None: + raise Exception("TARGET ANGLE IS NOT DESCRIBED") + target_heading = tar_heading + thrust = thrust if thrust is float or thrust else 0.1 + self.goal = [thrust, 0, target_heading] + + def execute(self, userdata): + self.cntr.next_step("MOVE HEADING", self.goal) + self.cntr.get_logger().info(f'Move by heading {round(self.goal[0] * 100)} % {round(self.goal[2])} deg') + while not self.cntr.shutdown(): + self.cntr.update() + rclpy.spin_once(self.cntr) + return 'complite' \ No newline at end of file diff --git a/ws_nodes_py/StateMachine/SpecialStates.py b/ws_nodes_py/StateMachine/SpecialStates.py index eff92aa..1be7925 100644 --- a/ws_nodes_py/StateMachine/SpecialStates.py +++ b/ws_nodes_py/StateMachine/SpecialStates.py @@ -68,7 +68,7 @@ class DropZabralo(State): self.is_close = is_close if is_close is not None else True def execute(self, userdata): - self.cntr.get_logger().info(f'Drop zabralo') + self.cntr.get_logger().info(f'{"Drop" if self.is_close else "Up"} zabralo') self.cntr.pub_zabralo(self.is_close) return 'complite' diff --git a/ws_nodes_py/__pycache__/regulator.cpython-310.pyc b/ws_nodes_py/__pycache__/regulator.cpython-310.pyc index 4d4e2a9273d23e4012336257d0040b8c80cafa86..62a35ad09c95d6e8f246e79c573085bbdee03b23 100644 GIT binary patch delta 1042 zcmY+D-%C_c5XaBl-|dglMkhDJ@tdlhlrqmB8nv>=plRv3VOGhyGeN9eD2KLxpTi~?%eMOKOVLVe!qv| zvOhQ7T;KD?PD9@#bTcdgoF$Y+mIV%6S+Ei+SGl&p5*pXJfmfZI86&H3i|<&_5+>h8 zKKEv7?r9>wH_MW(StV3;{A)t^wYU|CLjvPaD_yREDOFwTY<+KlUy ziinrl3UGrPtIF1$?Y?8mU!K^m8Q+D$(RgcL{LI<&1Lwp6)GYwvSaPqIx74D^okrG94SORsH8oKbVan=9IEILm-6ey8wj1hHE-#wNcM(CWW^X^-8H^QegaI}l z&Zk(QDFK*?{_jTD_9trkbRm~7r4{+oj#U462S69YSU|@8FPod z_DR?;hA_P0w!1;3(RTAhD_NHboUo5DijZF$x}7odb`!1;t`e>xYMzV`1(D>rVo4-0 zAbSHHP?Gt;+ji0+PE(ROeQ#naSG?tD^@dQQ`?%6r*bYhW27i!Tq;eMV7WTdGz~l5 znlvsyaO3)RN~GkMo!8--Jlxy~D>BvmB3Qv2?%M_ZQ4WY%*%CSeujNSSnV(wW;HXp1 z7|M^qnv8`HL8H7H-hc2Sg-yaBK@bKAG!XUn(pRbX-EH)&Tnl&GG!7Q?fz@W&wpacR nuQh+bP?3Id3k;kF3w$7!Haz6$Ti delta 676 zcmY+COK1~O6o&6f=H<*Jlb6Y)5?myqj7Y60BGOtx1Q(?x6l)z3vEICFZQ_jzwvC7c ziVHVAD?xnV!qSCQqli02*DghZh@c=@xpF7gdnPLn+>iPHx&N7a5A*iW_XE*JBw{jI znIB4ZJoh|$5l&v0vdnzo%nz-x3UJ_|j^T%Sm@6ITD_kwBl@Qms-cfvwyQKM!vR+ZR z$*qp+8{FoR4#PLF$6R9y^PL&yWxKzjZNM#TnU&qZRjxh`0po0jt+Ge|X-~t1mAA!y zc@6;*o{rNN$B$M~Pe^Hki+`;`w-#wYxChJlC7Q>3mV;02IGWBRl<>536DGQ!oTHHL zt1{9L+Bo72we%(Mgr-&0|UM4IMh7oopaVrsn6?~WIj&U-}68&vio+gfv zQ6-EKa)cm?TsPG}*z7gUPlrmL0Xz(lnS2Mj>kBRQA=*Eorpqr!znU6AZ9H XtfM>hRa>Y09sDyiuICNi1ZDdlCN7*_ diff --git a/ws_nodes_py/regulator.py b/ws_nodes_py/regulator.py index 874acfc..341e55e 100644 --- a/ws_nodes_py/regulator.py +++ b/ws_nodes_py/regulator.py @@ -55,11 +55,24 @@ class Controller(GetParameterNode): self.__move_to_point(self.goal) elif self.type_move == "MOVE CIRCLE": self.__move_around(self.goal) + elif self.type_move == "MOVE HEADING": + self.__move_heading(self.goal) elif self.type_move == "STOP": self.__move_by_twist(0, 0) else: self.__move_by_twist(0, 0) + def __move_heading(self, goal): + target_heading = goal.z + err_angle = self.__angle_error(target_heading, self.heading) + ang = self.__get_angular_speed(err_angle) + + if abs(err_angle) < 5: + lin = goal.x + else: + lin = 0 + self.__move_by_twist(lin, ang) + def __move_around(self, point: Point): target_dist = point.z diff --git a/ws_nodes_py/settings/mission.py b/ws_nodes_py/settings/mission.py index 7084759..8488c3c 100644 --- a/ws_nodes_py/settings/mission.py +++ b/ws_nodes_py/settings/mission.py @@ -1,27 +1,13 @@ from ws_nodes_py.settings.robot_settings import is_real -from ws_nodes_py.settings.for_world import world_markers from ws_nodes_py.StateMachine.MotionStates import * from ws_nodes_py.StateMachine.SpecialStates import * from ws_nodes_py.StateMachine.DefaultStates import * -# TEST MISSIONS - -is_real_compass = True -square_mission = [ - [MoveToGoal, {"goal": [0, 2], "dist_treshold": 0.3, "navigation_marker": "", "is_real_compass": is_real_compass, "is_update_pos": True}], - [MoveToGoal, {"goal": [2, 2], "dist_treshold": 0.3}], - [MoveToGoal, {"goal": [2, 0], "dist_treshold": 0.3}], - [MoveToGoal, {"goal": [0, 0], "dist_treshold": 0.3}], -] - -circle_mission = [ - [MoveByCircle, {"goal": [10, 0], "dist": 1, "out_angle": 120, "is_real_compass": is_real_compass, "is_update_pos": True}] -] - on_navigation = [ [ [WorldSettings, {"is_real_compass": True, "is_update_pos": True}], + [MoveByTwist, {"goal": [0, 0]}], [Wait, {"time": 2}], ], ] @@ -29,71 +15,136 @@ on_navigation = [ off_navigation = [ [ [WorldSettings, {"is_real_compass": False, "is_update_pos": False}], + [MoveByTwist, {"goal": [0, 0]}], [Wait, {"time": 2}], ], ] +timer_thrust = 0.2 +is_anticlockwise = True -# FIRST STEP OF VLAD24 MISSION +def set_course(tar_course:int, is_clockwise:bool=False, thrust:float=None): + thrust = timer_thrust if thrust is None else thrust + thrust = -thrust if is_clockwise else thrust + return [ + [ + [MoveByTwist, {"goal": [0, thrust]}], + [IsAchieveHeading, {"target_angle": tar_course}], + ] + ] -def trough_gate(color: str): + +def move_by_time(goal:list, time:int): return [ - [ # Двигаемся к воротам пока не увидим, что шар ушёл в корму - [Wait, {"time": 100}], - [MoveToGoal, {"goal": f"{color}_gate", "navigation_marker": f"{color}_gate", "is_real_compass": False, "is_update_pos": False}], - [FindObject, {"object_name": f"{color}_bou", "min_max_angle": [0, 360], "time": 2}], - ], [ - [MoveByTwist, {"goal": [0.10, 0]}], - [FindObject, {"object_name": f"{color}_bou", "min_max_angle": [90, 270], "time": 2}], + [ + [MoveByTwist, {"goal": goal}], + [Wait, {"time": time}], ] ] + +def move_forward_by_time(time:int, thrust:float=None): + thrust = timer_thrust if thrust is None else thrust + return move_by_time([thrust, 0], time) + + +def get_clock_ang(ang): + if is_anticlockwise: + return (360 - ang) % 360 + else: + return ang + + +def inv_clock(val): + if is_anticlockwise: + return -val + else: + return val + + +# FIRST STEP OF VLAD24 MISSION + move_to_blue_gate = [ - # [ # Выезжаем из жёлтых ворот - # [MoveByTwist, {"goal": [0.15, 0]}], - # [Wait, {"time": 7}], - # ], [ # Ищем синий шар - [MoveByTwist, {"goal": [0.10, 0.075 if is_real else 0.13]}], + [MoveByTwist, {"goal": [0.10, inv_clock(0.075 if is_real else 0.13)]}], [FindObject, {"object_name": "blue_bou"}], ], [ # Ждём пока он уйдёт в корму - [MoveByTwist, {"goal": [0.10, -0.075 if is_real else -0.1]}], + [MoveByTwist, {"goal": [0.10, inv_clock(-0.075 if is_real else -0.1)]}], [FindObject, {"object_name": "blue_bou", "min_max_angle": [90, 270]}], - ], - # [ # Проезжаем чуть вперёд, чтобы выйти +- перпендикулярно воротам - # [MoveByTwist, {"goal": [0.15, 0]}], - # [Wait, {"time": 7}], - # ], - [ # Разворачиваемся к воротам - [MoveByTwist, {"goal": [0.0, -0.15]}], + ], [ # Разворачиваемся к воротам + [MoveByTwist, {"goal": [0.0, inv_clock(-0.15)]}], [FindObject, {"object_name": "blue_gate", "min_max_angle": [0, 360]}], ] - ] move_to_yellow_gate = [ [ # Поворочиваемся вокруг буя, пока не увидим жёлтые ворота # [MoveByCircle, {"goal": "blue_bou", "navigation_marker": "blue_bou"}], - [MoveByTwist, {"goal": [0.05 if is_real else 0.02, -0.17]}], + [MoveByTwist, {"goal": [0.05 if is_real else 0.02, inv_clock(-0.17)]}], [FindObject, {"object_name": "yellow_gate", "min_max_angle": [0, 360], "time": 2}], ] - # [ # Пока не видим 2 шара в боковой камере - # [MoveByTwist, {"goal": [0.15, 0.00]}], - # [FindObject, {"object_name": "yellow_bou_1", "min_max_angle": [0, 360]}], - # ], [ # Проежаем чуть вперёд, чтобы отойти от синих шаров - # [MoveByTwist, {"goal": [0.15, -0.05]}], - # [Wait, {"time": 5}], - # ], [ # Поворочиваемся лицом к воротам - # [MoveByTwist, {"goal": [0.0, -0.15]}], - # [FindObject, {"object_name": "yellow_gate", "min_max_angle": [0, 360]}], - # ] ] -first_step = off_navigation + trough_gate("yellow") + move_to_blue_gate + trough_gate("blue") + move_to_yellow_gate + trough_gate("yellow") +move_trouth_yellow_gate_extrapolations = [ + [ # Поворочиваемся вокруг буя, пока не увидим жёлтые ворота + [MoveByTwist, {"goal": [0.05 if is_real else 0.02, inv_clock(-0.17)]}], + [FindObject, {"object_name": "yellow_gate", "min_max_angle": [0, 360], "time": 2}], + [IsAchieveHeading, {"target_angle": 170}], + ], [ + [MoveByTwist, {"goal": [0.10, 0]}], + [FindObject, {"object_name": "yellow_bou", "min_max_angle": [90, 270], "time": 2}], + ] +] + +def trough_gate(color: str): + return [ + [ # Двигаемся к воротам пока не увидим, что шар ушёл в корму + [MoveToGoal, {"goal": f"{color}_gate", "navigation_marker": f"{color}_gate", "is_real_compass": False, "is_update_pos": False}], + [FindObject, {"object_name": f"{color}_bou", "min_max_angle": [0, 360], "time": 2}], + ], [ + [MoveByTwist, {"goal": [0.10, 0]}], + [FindObject, {"object_name": f"{color}_bou", "min_max_angle": [90, 270], "time": 2}], + ] + ] + +move_by_nav = [ + *on_navigation, + *set_course(20, True), + # TODO + [ + [MoveByHeading, {"tar_heading": 20, "thrust": 0.2}], + # [MoveByTwist, {"goal": [timer_thrust, 0]}], + [FindObject, {"object_name": "blue_bou", "min_max_angle": [140, 220], "time": 2}], + [Wait, {"time": 40}], # TODO remove + ], + *set_course(260, False), + [ + [MoveByHeading, {"tar_heading": 270, "thrust": 0.2}], + # [MoveByTwist, {"goal": [timer_thrust, 0]}], + [Wait, {"time": 20}], # TODO remove + ], [ # Поворочиваемся вокруг буя, пока не увидим жёлтые ворота + [MoveByTwist, {"goal": [0, inv_clock(-0.2)]}], + [FindObject, {"object_name": "yellow_gate", "min_max_angle": [0, 360], "time": 2}], + ], + *trough_gate("yellow"), +] +first_step = off_navigation + trough_gate("yellow") + move_to_blue_gate + trough_gate("blue") + move_to_yellow_gate + trough_gate("yellow") +first_step_nav = trough_gate("yellow") + move_by_nav +first_step_short = [ + *trough_gate("yellow"), + [ + [MoveByTwist, {"goal": [0.2, 0]}], + [Wait, {"time": 5 if is_real else 10}], + ], [ # Поворочиваемся вокруг буя, пока не увидим жёлтые ворота + [MoveByTwist, {"goal": [0, inv_clock(-0.2)]}], + [FindObject, {"object_name": "yellow_gate", "min_max_angle": [0, 360], "time": 2}], + ], + *trough_gate("yellow"), +] -# SECOND AND THIRD STEP OF VLAD24 MISSION +# SECOND OF VLAD24 MISSION def get_drop_cube(): if is_real: @@ -114,80 +165,24 @@ def get_drop_cube(): [[DropBox, {"time": 5}], [MoveByTwist, {"goal": [0, 0]}]] ] - -move_to_black_island_1 = [ - # TODO время подобранно для симулятора - # WARN в реальности это не прокатит - - # TODO отсюда экстраполируем своё положение (подставляем координаты жёлтых ворот) - # WARN если включаем экстраполяцию положения, то не стоит одновременно использовать linear и angular - # WARN если используем виртуальный компас, то лучше использовать константную угловую скорость 0.15 (отключить пиды) - - # TODO Общий таймер на этап, если сделаем прохождение сбора шаров - # (надо придумать, как это можно сделать) - [ # Отходим от ворот - [MoveByTwist, {"goal": [0.15, 0]}], - [Wait, {"time": 10}], - ], [ # Поворачиваем на "90" градусов - [MoveByTwist, {"goal": [0.0, 0.15]}], - [Wait, {"time": 22}], - # TODO выход по курсу - ], [ # Проходим ещё чуть вперёд - [MoveByTwist, {"goal": [0.15, 0]}], - [Wait, {"time": 20}], - ], [ # Поворачиваемся в сторону острова - [MoveByTwist, {"goal": [0.0, 0.15]}], - # [Wait, {"time": 25}], - [IsAchieveHeading, {"target_angle": 0}], - ], [ # Идём вперёд, пока не увидим чёрный круг - [MoveByTwist, {"goal": [0.15, 0]}], - [FindObject, {"object_name": "black_position", "min_max_angle": [0, 360]}], - ], -] - -set_course_to_black_island = [ +move_and_drop_cube = [ *on_navigation, + [DropZabralo, {"is_close": False}], [ # Отходим от ворот - [MoveByTwist, {"goal": [0.02, 0.15]}], + [MoveByTwist, {"goal": [0.05 if is_real else 0.02, 0.17]}], # [Wait, {"time": 45}], - [IsAchieveHeading, {"target_angle": 0}], + [IsAchieveHeading, {"target_angle": 60 if is_real else 0}], # TODO check + ], [ # Идём вперёд, пока не увидим чёрный круг + [MoveByHeading, {"tar_heading": 60 if is_real else 0, "thrust": 0.15}], + [FindObject, {"object_name": "black_position_front", "min_max_angle": [0, 360]}], ], - *off_navigation, + *get_drop_cube(), ] -move_to_black_island_2 = set_course_to_black_island + [ - # TODO время подобранно для симулятора - # WARN в реальности это не прокатит +second_step = move_and_drop_cube - # TODO отсюда экстраполируем своё положение (подставляем координаты жёлтых ворот) - # WARN если включаем экстраполяцию положения, то не стоит одновременно использовать linear и angular - # WARN если используем виртуальный компас, то лучше использовать константную угловую скорость 0.15 (отключить пиды) - - # TODO Общий таймер на этап, если сделаем прохождение сбора шаров - # (надо придумать, как это можно сделать) - [ # Идём вперёд, пока не увидим чёрный круг - [MoveByTwist, {"goal": [0.15, 0]}], - [FindObject, {"object_name": "black_position", "min_max_angle": [0, 360]}], - ], -] -move_around_black_island = [ - [ # Отходим от острова - [MoveByTwist, {"goal": [-0.15, 0]}], - [Wait, {"time": 3}], - ], [ # Разворачиваемся перпендикулярно острову - [MoveByTwist, {"goal": [0, 0.15]}], - [FindObject, {"object_name": "black_position", "min_max_angle": [90, 110]}], - ], - [ # Движемся вокруг острова - [MoveByCircle, {"goal": "black_position", "navigation_marker": "black_position", "is_real_compass": False, "is_update_pos": False}], - [Wait, {"time": 120}], - # Если шар под кругом, то через него мы не идём - # [FindObject, {"object_name": "red_position", "min_max_angle": [180, 360]}], - # Для теста смотрим только спереди - [FindObject, {"object_name": "red_position", "min_max_angle": [270, 20]}], - ], -] +# THIRD OF VLAD24 MISSION around_island_in_real = [ [ @@ -195,12 +190,12 @@ around_island_in_real = [ [Wait, {"time": 5}], ], [ - [MoveByTwist, {"goal": [0.01, 0.15]}], + [MoveByTwist, {"goal": [0.01, inv_clock(0.15)]}], # [Wait, {"time": 12}], - [FindObject, {"object_name": "black_position_side", "min_max_angle": [330, 90]}], + [FindObject, {"object_name": "black_position_side", "min_max_angle": [270, 90]}], ], [ - [MoveByTwist, {"goal": [0.03, -0.15]}], + [MoveByTwist, {"goal": [0.03, inv_clock(-0.15)]}], [Wait, {"time": 90}], [FindObject, {"object_name": "red_position", "min_max_angle": [270, 20]}], ], @@ -218,62 +213,80 @@ drop_stakan = [ [DropPickStakan, {"time": 15, "state": 1, "is_wait_pickup": True}], [MoveByTwist, {"goal": [0, 0]}] ], [ - [DropPickStakan, {"time": 40, "state": 0}], + [DropPickStakan, {"time": 40, "state": 0, "is_wait_pickup": True}], [MoveByTwist, {"goal": [0, 0]}] ] ] -second_step = move_to_black_island_2 + get_drop_cube() + (move_around_black_island if not is_real else around_island_in_real) + move_to_red_point + drop_stakan +third_step = [] +# third_step += around_island_in_real +third_step += move_to_red_point + drop_stakan -# THIRD STEP OF VLAD24 MISSION -set_to_course = [ +# FOURTH STEP OF VLAD24 MISSION + +move_to_red_poster = [ *on_navigation, + [DropZabralo, {"is_close": False}], [ [MoveByTwist, {"goal": [0, 0.15]}], - [IsAchieveHeading, {"target_angle": 155}], - ], - *off_navigation, -] - -move_to_balls = [ - [ - [MoveByTwist, {"goal": [0.10, 0]}], + [IsAchieveHeading, {"target_angle": 170 if is_real else 160}], # TODO + ], [ # Идём вперёд, пока не увидим красный плакат + [MoveByHeading, {"tar_heading": 170 if is_real else 160, "thrust": 0.15}], [FindObject, {"object_name": "red_poster_front", "min_max_angle": [0, 360], "time": 1}], ], [ [MoveToGoal, {"goal": "red_poster", "navigation_marker": "red_poster_front", "is_real_compass": False, "is_update_pos": False}], [FindObject, {"object_name": "red_poster_front", "min_max_angle": [340, 20], "max_dist": 0.5}], - [GetPostersDirection, {}], + # [GetPostersDirection, {}], ] ] balls_collecting = [ [ - [MoveByTwist, {"goal": [0.03, 0.2]}], - [FindObject, {"object_name": "red_poster_side", "min_max_angle": [80, 230]}], - ], - [ - [MoveByTwist, {"goal": [0.03, 0.2]}], + [MoveByTwist, {"goal": [0.03, -0.2]}], + [FindObject, {"object_name": "red_poster_side", "min_max_angle": [get_clock_ang(80), get_clock_ang(230)]}], + ], [ + [MoveByTwist, {"goal": [0.03, -0.2]}], [Wait, {"time": 15}], ] ] balls_delivery = [ + [DropZabralo, {"is_close": True}], [ - [MoveByVariableTwist, {"var": "move_side", "right": [0.08, 0.2], "left": [0.08, -0.2]}], + [MoveByTwist, {"goal": [0.03, 0.2]}], + # [MoveByVariableTwist, {"var": "move_side", "right": [0.08, 0.2], "left": [0.08, inv_clock(-0.2)]}], [FindObject, {"object_name": "green_poster", "min_max_angle": [320, 60]}], ], [ - [MoveByVariableTwist, {"var": "move_side", "right": [0.08, 0.2], "left": [0.08, -0.2]}], + [MoveByTwist, {"goal": [0.03, 0.2]}], + # [MoveByVariableTwist, {"var": "move_side", "right": [0.08, 0.2], "left": [0.08, inv_clock(-0.2)]}], [Wait, {"time": 10}], ] ] -third_step = set_to_course + move_to_balls + balls_collecting + balls_delivery +fourth_step = move_to_red_poster + balls_collecting + balls_delivery +fourth_step_short = [ + *on_navigation, + *set_course(180, False), + [ + [MoveByHeading, {"tar_heading": 180, "thrust": 0.2}], + # [MoveByTwist, {"goal": [0.2, 0]}], + [Wait, {"time": 150}], # TODO remove + ], +] # финальная миссия -# mission = first_step + second_step + third_step +# mission = first_step_short + second_step + third_step + fourth_step + +# Отдельные этапы для отработки +# mission = first_step_short + second_step + third_step + fourth_step_short +# mission = first_step_nav + second_step + third_step + fourth_step_short +mission = first_step + second_step + third_step + fourth_step_short +# mission = second_step +# mission = third_step +# mission = fourth_step """ 0) Для всех этапов надо записать БЭКИ @@ -288,7 +301,8 @@ third_step = set_to_course + move_to_balls + balls_collecting + balls_delivery 2) Отработка прохождения первого этапа """ # mission = first_step # проход через жёлтые, через синие, через жёлтые -# mission = trough_gate("blue") + move_to_yellow_gate + trough_gate("yellow") # через синие, через жёлтые +# mission = trough_gate("blue") + move_to_yellow_gate_anticlockwise + trough_gate("yellow") # через синие, через жёлтые +# mission = trough_gate("blue") + on_navigation + move_trouth_yellow_gate_anticlockwise_extrapolations # через синие, через жёлтые # mission = trough_gate("yellow") # отдельный проход через жёлтые # mission = trough_gate("blue") # отдельно проход через синие @@ -302,7 +316,7 @@ third_step = set_to_course + move_to_balls + balls_collecting + balls_delivery 4) Проверить выход на цель (правильность границ срабатывания) 5) Проверить выполнение второго этапа """ -# mission = second_step # выход к месту предполагаемого нахождения нефтяного пятна + сброс куба + сброс пробоотборника +# mission = second_step # выход к месту предполагаемого нахождения нефтяного пятна + сброс куба # mission = [[DropBox, {"time": 5}],] # сборс куба # mission = drop_stakan # сборс стакана # mission = set_course_to_black_island # выход на курс предполагаемого нахождения чёрного круга @@ -321,45 +335,137 @@ third_step = set_to_course + move_to_balls + balls_collecting + balls_delivery # mission = move_to_balls + balls_collecting # отладка сбора шаров # mission = move_to_balls + balls_collecting + balls_delivery # отладка сбора шаров и доставки в зону разгрузки +# mission = miss -# mission = trough_gate("blue") -# mission = first_step -# mission = second_step -# mission = move_around_black_island -# mission = move_to_yellow_gate +""" +mission = trough_gate("blue") +mission = first_step +mission = second_step +mission = move_around_black_island +mission = move_to_yellow_gate -# mission = get_drop_cube() # отладка сброса куба -# mission = move_to_red_point # отладка выхода на красный круг -# mission = move_to_red_point + drop_stakan # отладка выхода на красный круг со сбросом +mission = get_drop_cube() # отладка сброса куба +mission = move_to_red_point # отладка выхода на красный круг +mission = move_to_red_point + drop_stakan # отладка выхода на красный круг со сбросом -# mission = move_to_red_point +mission = move_to_red_point -# mission = move_to_balls + balls_collecting + balls_delivery -# mission = balls_collecting + balls_delivery -# mission = second_step -# balls_delivery +mission = move_to_balls + balls_collecting + balls_delivery +mission = balls_collecting + balls_delivery +mission = second_step +balls_delivery + + +mission = first_step + second_step + third_step + +mission = [ + [ + [MoveByTwist, {"goal": [0.15, 0]}], + [FindObject, {"object_name": "black_position_front", "min_max_angle": [0, 360]}], + ]] + get_drop_cube() + +mission = around_island_in_real + move_to_red_point + drop_stakan + +mission = [[ + [MoveByTwist, {"goal": [0.15, 0]}], + [FindObject, {"object_name": "black_position", "min_max_angle": [0, 360]}], + ]]+ get_drop_cube() + around_island_in_real + move_to_red_point + drop_stakan + +mission = [ + [DropZabralo, {"is_close": False}], + [Wait, {"time": 5}], + [DropZabralo, {"is_close": True}], +] + +mission = move_to_balls + balls_collecting + [[DropZabralo, {"is_close": True}],] + balls_delivery + +move_to_black_island_1 = [ + # TODO_ время подобранно для симулятора + # WARN в реальности это не прокатит + + # TODO_ отсюда экстраполируем своё положение (подставляем координаты жёлтых ворот) + # WARN если включаем экстраполяцию положения, то не стоит одновременно использовать linear и angular + # WARN если используем виртуальный компас, то лучше использовать константную угловую скорость 0.15 (отключить пиды) + + # TODO_ Общий таймер на этап, если сделаем прохождение сбора шаров + # (надо придумать, как это можно сделать) + [ # Отходим от ворот + [MoveByTwist, {"goal": [0.15, 0]}], + [Wait, {"time": 10}], + ], [ # Поворачиваем на "90" градусов + [MoveByTwist, {"goal": [0.0, 0.15]}], + [Wait, {"time": 22}], + # TODO_ выход по курсу + ], [ # Проходим ещё чуть вперёд + [MoveByTwist, {"goal": [0.15, 0]}], + [Wait, {"time": 20}], + ], [ # Поворачиваемся в сторону острова + [MoveByTwist, {"goal": [0.0, 0.15]}], + # [Wait, {"time": 25}], + [IsAchieveHeading, {"target_angle": 0}], + ], [ # Идём вперёд, пока не увидим чёрный круг + [MoveByTwist, {"goal": [0.15, 0]}], + [FindObject, {"object_name": "black_position", "min_max_angle": [0, 360]}], + ], +] -# mission = first_step + second_step + third_step +# TEST MISSIONS -# mission = [ -# [ -# [MoveByTwist, {"goal": [0.15, 0]}], -# [FindObject, {"object_name": "black_position_front", "min_max_angle": [0, 360]}], -# ]] + get_drop_cube() +is_real_compass = True +square_mission = [ + [MoveToGoal, {"goal": [0, 2], "dist_treshold": 0.3, "navigation_marker": "", "is_real_compass": is_real_compass, "is_update_pos": True}], + [MoveToGoal, {"goal": [2, 2], "dist_treshold": 0.3}], + [MoveToGoal, {"goal": [2, 0], "dist_treshold": 0.3}], + [MoveToGoal, {"goal": [0, 0], "dist_treshold": 0.3}], +] -# mission = around_island_in_real + move_to_red_point + drop_stakan +circle_mission = [ + [MoveByCircle, {"goal": [10, 0], "dist": 1, "out_angle": 120, "is_real_compass": is_real_compass, "is_update_pos": True}] +] -# mission = [[ -# [MoveByTwist, {"goal": [0.15, 0]}], -# [FindObject, {"object_name": "black_position", "min_max_angle": [0, 360]}], -# ]]+ get_drop_cube() + around_island_in_real + move_to_red_point + drop_stakan -# mission = [ -# [DropZabralo, {"is_close": False}], -# [Wait, {"time": 5}], -# [DropZabralo, {"is_close": True}], -# ] +set_course_to_black_island = [ + *on_navigation, + [ # Отходим от ворот + [MoveByTwist, {"goal": [0.02, 0.15]}], + # [Wait, {"time": 45}], + [IsAchieveHeading, {"target_angle": 80}], # TODO_ + ], + *off_navigation, +] + +move_to_black_island_2 = set_course_to_black_island + [ + # TODO_ время подобранно для симулятора + # WARN в реальности это не прокатит + + # TODO_ отсюда экстраполируем своё положение (подставляем координаты жёлтых ворот) + # WARN если включаем экстраполяцию положения, то не стоит одновременно использовать linear и angular + # WARN если используем виртуальный компас, то лучше использовать константную угловую скорость 0.15 (отключить пиды) + + # TODO_ Общий таймер на этап, если сделаем прохождение сбора шаров + # (надо придумать, как это можно сделать) + [ # Идём вперёд, пока не увидим чёрный круг + [MoveByTwist, {"goal": [0.15, 0]}], + [FindObject, {"object_name": "black_position", "min_max_angle": [0, 360]}], + ], +] -# mission = move_to_balls + balls_collecting + [[DropZabralo, {"is_close": True}],] + balls_delivery \ No newline at end of file +move_around_black_island = [ + [ # Отходим от острова + [MoveByTwist, {"goal": [-0.15, 0]}], + [Wait, {"time": 3}], + ], [ # Разворачиваемся перпендикулярно острову + [MoveByTwist, {"goal": [0, inv_clock(0.15)]}], + [FindObject, {"object_name": "black_position", "min_max_angle": [get_clock_ang(90), get_clock_ang(110)]}], + ], + [ # Движемся вокруг острова + [MoveByCircle, {"goal": "black_position", "navigation_marker": "black_position", "is_real_compass": False, "is_update_pos": False}], + [Wait, {"time": 120}], + # Если шар под кругом, то через него мы не идём + # Для теста смотрим только спереди + [FindObject, {"object_name": "red_position", "min_max_angle": [270, 20]}], + ], +] +""" \ No newline at end of file -- GitLab From dd50668f9cfd60e68baaab71467307d9b961fc16 Mon Sep 17 00:00:00 2001 From: Rjkzavr Date: Sun, 3 Nov 2024 19:16:25 +0300 Subject: [PATCH 23/23] add movement by points --- setup.py | 1 + .../StateMachine/mission_generators.py | 10 ++++ .../__pycache__/compass.cpython-310.pyc | Bin 6887 -> 6887 bytes .../__pycache__/regulator.cpython-310.pyc | Bin 6976 -> 7015 bytes .../twist_controller.cpython-310.pyc | Bin 2347 -> 2790 bytes ws_nodes_py/__pycache__/world.cpython-310.pyc | Bin 4550 -> 4913 bytes ws_nodes_py/orientation_by_mrmr.py | 52 ++++++++++++++++++ ws_nodes_py/regulator.py | 1 + .../robot_settings.cpython-310.pyc | Bin 412 -> 412 bytes ws_nodes_py/settings/mission.py | 4 ++ ws_nodes_py/twist_controller.py | 26 +++++++-- ws_nodes_py/world.py | 5 ++ 12 files changed, 93 insertions(+), 6 deletions(-) create mode 100644 ws_nodes_py/StateMachine/mission_generators.py create mode 100644 ws_nodes_py/orientation_by_mrmr.py diff --git a/setup.py b/setup.py index 4356ffe..5ff94cd 100644 --- a/setup.py +++ b/setup.py @@ -24,6 +24,7 @@ setup( 'point_logger = ws_nodes_py.point_logger:main', 'nmea_reader = ws_nodes_py.nmea_parser:main', # sensors + 'orientaion_by_mrmr = ws_nodes_py.orientation_by_mrmr:main', 'compass = ws_nodes_py.compass:main', # propultion and regulators 'twist_controller = ws_nodes_py.twist_controller:main', diff --git a/ws_nodes_py/StateMachine/mission_generators.py b/ws_nodes_py/StateMachine/mission_generators.py new file mode 100644 index 0000000..c8663aa --- /dev/null +++ b/ws_nodes_py/StateMachine/mission_generators.py @@ -0,0 +1,10 @@ +from typing import List + +from ws_nodes_py.StateMachine.MotionStates import * + + +def gals_hodit_plavat_kursach_delat(points: List[List[float]], dist_treshold: float=0.11, navigation_marker: str="mrmr_position"): + mission = [] + for point in points: + mission.append([MoveToGoal, {"goal": point, "dist_treshold": dist_treshold, "navigation_marker": navigation_marker, "is_real_compass": True, "is_update_pos": True}]) + return mission \ No newline at end of file diff --git a/ws_nodes_py/__pycache__/compass.cpython-310.pyc b/ws_nodes_py/__pycache__/compass.cpython-310.pyc index a05162e0e78e872132e67124656ff9dafa774593..5309f368e6cef1346ede5f261df4284f74f2dcfe 100644 GIT binary patch delta 20 acmaEE`rMQ|pO=@50SJmqls9tUkpcid9R-sB delta 20 acmaEE`rMQ|pO=@50SI0{VA;rhM+yK#QU+K6 diff --git a/ws_nodes_py/__pycache__/regulator.cpython-310.pyc b/ws_nodes_py/__pycache__/regulator.cpython-310.pyc index 62a35ad09c95d6e8f246e79c573085bbdee03b23..273f29cef073a4f8a8b9c9bf9a406cdc96f04997 100644 GIT binary patch delta 282 zcmX?L_S}p&pO=@50SKfPsHeZ&$ZIXYcxtkzz)H1+3=8;c*k?1Ou+^~6W|+&okdcw0 zhGBs~4SNmiLMBFr6oz01P4-EGa;(1?6*L7WD+s!CIs?rqasd*P(*%=bWq=GWMm|O$ zW@2G9VB%n6V`O9EVU%KIVUn19PDp(6FTrDsos$m=-4)6Qs?}sJas?R>0U`<~GfE3j zJ}fN4IAQV);fsv3H?I=mVPaf3dAryOp*oO?To6$YBs3X|8YUNtTQF{%yh{8Wft OY}pMt*tr;4{sRE;XG9MG delta 239 zcmaEEcEF4`pO=@50SNf63#Z@P$ZIXYxM{Mdz)HRv_8QiOOpFXE48aVVY?CDf<(M@2 zCtC};Pwo*+l#>GL;9}%s1Y#x@Mgt}eCN@SkMjl2fMiwRkrpdfQw-`GnzYw}Blm}F( z$z0?Lq>3UyM8V|i!s?9ulm81}WSq76q6iNY-XOWGt$mJVV@q zal_<`;^!E*PF^5U%vdy8L{f>dezLiwBcsUVO396)dx1)eLGI@OTF=GE0dzvql*#r| X_t>U@OzNJjEN#nfz`@SN$nqZm*UUJ4 diff --git a/ws_nodes_py/__pycache__/twist_controller.cpython-310.pyc b/ws_nodes_py/__pycache__/twist_controller.cpython-310.pyc index 4a0710694d130dbb6918fdc5b72a6585df8f4d13..a16d7077b85258d358a262385b4666991f43d2f8 100644 GIT binary patch delta 1222 zcmZuxK~EDw6rS0g-5s{=wp1xqK!k#!YCuesa3Y2a8e>c_#`Ln5&Vnrq#n~k!m81uV zoH&>{=|wnrK;nNeF){HUm^EHWJop#X_qwfM<0S9BnfKnzeDl7ydz?M#bLy#-O<*j% zEUcVZubi*V`>^szi3(EE1TE=%WSI(0=)1hc1oMn#O&G%5WhJw{vIK{fC3=LtOG;cM zL=tSz9$}HIe=p>xgwyuva7uQY9GpAsB?sq4`jF!0gFe`pQ{U((np1DJk90si)jc|>KI@O^ zuo_|o+OO`gk&(1t4njAqu6b^V@0JFK8mA5j|IPKho~Jq8(K1v$H)fkTNC`=+Kon4F+Z6E%vPfhWLPcFN zFftD+8gRRqaNWQItcdDJlh-hD5>?M%g)rnOs4GaDeKNsJ#;H& z-(M)V*I*vYT zV{^%`2G1n2(_&X%#o1Z#cVS%w=XH=a`Yg?Aja)bSnDUASd7XiClzEU~LLD2UO<>kF zu!2SH!4HfT%@`1Zl$Sw8R(XA8(=W@2Hez7^DQJ{9L-#min>#kc&$U)`v7bd#pjQYuEtSQcI=!ZX%zz5C% delta 823 zcmY*Xy>1gh5Z<}lyIY@s;~3itiNP2W9Rvdf0whF3Lq(GaAvcHZwZV?D(fR_&STa%~ zrHTaFf+mI25%B{2G&}`DrlUaO8Bi2v&K4GTns0Yzc4xks*)a*URS;g64o|q&zU$pwc z-Cj3Py-ug0>OkRVAMI@v6Q%zKAz(Xfj42KQm^6mWH%3F&##{k!TW!1Tg!!1nJo_Bo zB8bEot}dR61~DmcjmrpfV$$8`fWFWk#Wf*g|JjpxJ}wh8n~70ULpEMGWaG3NGaHc8$L0fA(wn>li~1!m>M#7cr7UExuUQ-5=>w~@5~YeV2r;nD z1PBl(!pxg_$eL+G|FmvFSzj0DMyzd(?cuauOm}45|83wc&ruu~l_w}5Oq^=>@ph-G zCNNN?(C}=#)@k*h92shrPI&~8e-NYHSp>S>7YG7uNZ~(No{vNy*z2R)=*0ezK#m2* z4%=dC88uU7hS0{&OUN+V`4(F3u`?$uk+iyq0X#7!OPQwJDY057xJ*E#Rhi%l!JO%x zkZKW)=TZ9f{L(Y3{CH59lMUZbS@mkYz^_+3om#cNmTu9|8B#j^}Srzq}$Vy?2 z^e{NRF^W6w1I{F|nAMLW4{?m2BKKA=k&FD)A{U1h14Yk%B7Fa8wbk`~FRGUGn)}Ko Qv%0F^yEmMo5H9eOzlYtVNB{r; diff --git a/ws_nodes_py/__pycache__/world.cpython-310.pyc b/ws_nodes_py/__pycache__/world.cpython-310.pyc index 0ab533d7e70d3a763bdbf16a334662630dc4cd7f..c067c2a5ae1093ce2b7a17fd72aed58f345aad29 100644 GIT binary patch delta 695 zcmY+Azi-n}5Xav+j^o&I>?TczK3S^eVtvz6K{UG)c7~X(nC}nKad#q^|Eih1W5-%`@@6XPzDzv^*f5Nh?$t5?XvI z0~wGVi`vnQm+`XREUiwJ`H{?CMY#cS=At~ZfLX;6**QQLMgW8^Qnf^Y^pvPH&B3lB z$E!q1TB8U5c8_7y#>y%x^P{9FBskQe-CN4fn40VO!F|_v_&4o3oad*tCS2few72>d zT#cI?jo{@%<#?o&5vJDBfc0spN3wlAZ+f)*Y|q#!LRpnxseAWngQPwnNWZOSOK z9O9$I7KMM5kJN0ufo_BUFwU(@=n1=s7M?`ltc1eD7{k3iG5`-eX`8SjdNEI)GTxDJ zGes=(&+5`OwuaU91pk%J7bfw3(CtOc#t^FtOb2O;$5(FP{SI2FAPO)C>i(0;X25)U z{Qpgzeh_w}ZqVa}^w$b|Ao_F!t|BH0*6#amtIf9X#!F@+zK)K|`{ptv`8)GSL;NHr zR%PNzCgh3XI4$1|!&ck(9fvIoVTyagJ0@TtbhI#qP>BkuCDm39OXilf2^)ONx&&Q5 nwz8RROqi&S^T_S?9H*XT8~ls)K@nlrY5QcY5?y}x_NiI?2N1{<751@T$P7q5iv^oTTg5V#}-3Tts%eynr%q)ACdhQ7p3_~O0Tqm`ikH}qc6kr6W zU`RVBezG1|Ch7Y7Q@HfO1hy$}CzTM3yJ5$4B2JVgm|7;Rqw42pZexj9O>VQ$Scz0x zs-)7&I;#MLB^hleK=P{FR%6=#7$@%5vo6+Mo6&mV6WN65>?jfEuaAh`J@cM&RN(t; z#nauRT=BlFe){l9TZBn$*01zg@p3*YB%QBfL!y<|)%o{O0H(p{jEH2q5 z$=301>HG1KMwJC&7qA>0M-098dg`v7Wj6QdHqRc|R*vyG0eAs!AMR(jY)?5XFEh`=6OE?+1Rb`s*;a6lVutozK_z00aa8 delta 20 acmbQkJcpS(pO=@50SL||3vJ|{$Or%}1_W*Z diff --git a/ws_nodes_py/settings/mission.py b/ws_nodes_py/settings/mission.py index 8488c3c..76ebf63 100644 --- a/ws_nodes_py/settings/mission.py +++ b/ws_nodes_py/settings/mission.py @@ -2,6 +2,7 @@ from ws_nodes_py.settings.robot_settings import is_real from ws_nodes_py.StateMachine.MotionStates import * from ws_nodes_py.StateMachine.SpecialStates import * from ws_nodes_py.StateMachine.DefaultStates import * +from ws_nodes_py.StateMachine.mission_generators import * on_navigation = [ @@ -338,6 +339,9 @@ mission = first_step + second_step + third_step + fourth_step_short # mission = miss +mission = gals_hodit_plavat_kursach_delat([[4, 0], [4, -4], [0, -4], [0, 0]] * 2) + + """ mission = trough_gate("blue") mission = first_step diff --git a/ws_nodes_py/twist_controller.py b/ws_nodes_py/twist_controller.py index 53b699a..9dfc36e 100644 --- a/ws_nodes_py/twist_controller.py +++ b/ws_nodes_py/twist_controller.py @@ -8,11 +8,16 @@ from geometry_msgs.msg import Twist class TwistController(GetParameterNode): + lifetime = 1/5 def __init__(self): super().__init__('twist_controller') # params self.last_time_joy = 0 + + self.last_time_twist = 0 + self.last_twist = Twist() + self.delta_time = self.get_declare_parameter('delta_time', rclpy.Parameter.Type.DOUBLE, checker=lambda x: x > 0, error_text="delta_time not more than zero") @@ -26,18 +31,27 @@ class TwistController(GetParameterNode): self.pub_twist = self.publisher_from_declare(Twist, "twist_topic", checker=lambda x: x) self.get_logger().info('Twist controller started') + self.create_update(self.update) def joy_callback(self, msg: Twist): self.last_time_joy = time() - if self.is_double_angular_speed_for_simulation: - msg.angular.z = msg.angular.z * 2 - self.pub_twist.publish(msg) + self.__set_twist(msg) def robot_callback(self, msg: Twist): if self.last_time_joy + self.delta_time < time(): - if self.is_double_angular_speed_for_simulation: - msg.angular.z = msg.angular.z * 2 - self.pub_twist.publish(msg) + self.__set_twist(msg) + + def update(self): + if self.last_time_twist + self.lifetime > time(): + self.pub_twist.publish(self.last_twist) + else: + self.pub_twist.publish(Twist()) + + def __set_twist(self, msg): + self.last_time_twist = time() + if self.is_double_angular_speed_for_simulation: + msg.angular.z = msg.angular.z * 2 + self.last_twist = msg def main(): diff --git a/ws_nodes_py/world.py b/ws_nodes_py/world.py index b1f68c6..39ea8a3 100644 --- a/ws_nodes_py/world.py +++ b/ws_nodes_py/world.py @@ -60,6 +60,7 @@ class World(GetParameterNode): self.subscribtion_for_parameter(String, "navigation_marker_topic", 'navigation_marker', checker=lambda x: x, func=lambda msg: msg.data, default="") self.subscribtion(String, "reset_topic", self.reset_callback, checker=lambda x: x) self.subscribtion(PointStamped, "object_position_topic", self.update_position, checker=lambda x: x) + self.subscribtion(PointStamped, "global_position_topic", self.update_gloabal_position, checker=lambda x: x) # Имеется в виду новая насадка на мотор или нет (новая - от Кирилла (обтекаемая) или старая - от трионикса (белая)) if self.get_declare_parameter('is_new_motor', rclpy.Parameter.Type.BOOL, lambda x: True, "is_new_motor can be 0 or 1"): @@ -89,6 +90,10 @@ class World(GetParameterNode): # self.get_logger().info(f"reset map {self.position} and update_pos") self.is_update_pos = True + def update_gloabal_position(self, msg): + if self.navigation_marker != "" and self.navigation_marker in msg.header.frame_id: + self.position = [msg.point.x, msg.point.y] + def update_position(self, msg): if self.navigation_marker != "" and self.navigation_marker in msg.header.frame_id: obj_pos = self.world_markers[self.navigation_marker.replace("_front", "").replace("_side", "")] -- GitLab