diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000000000000000000000000000000000000..e99122e29b211689d0cef18875d69340c821e0ab --- /dev/null +++ b/.gitignore @@ -0,0 +1,2 @@ +*__pycache__* +*.pyc \ No newline at end of file diff --git a/package.xml b/package.xml new file mode 100644 index 0000000000000000000000000000000000000000..37762f7f85c010f7d2cb8e780b7583517bb865d6 --- /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 0000000000000000000000000000000000000000..3e992abf6b7467700dd4fd9e09bce69a350c2ed2 --- /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 0000000000000000000000000000000000000000..e69de29bb2d1d6434b8b29ae775ad8c2e48c5391 diff --git a/setup.cfg b/setup.cfg new file mode 100644 index 0000000000000000000000000000000000000000..b5128d1739a869e19263bc45046a6e5d2ecd357f --- /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 0000000000000000000000000000000000000000..5ff94cd56c0a54a38905cada6fd41bd38817e9af --- /dev/null +++ b/setup.py @@ -0,0 +1,41 @@ +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': [ + # open water + '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', + 'regulator = ws_nodes_py.regulator:main', + 'to_berlin = ws_nodes_py.legacy.to_berlin:main', + # mission + '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.StateMachine.state_machine:main', + ], + }, +) diff --git a/test/test_copyright.py b/test/test_copyright.py new file mode 100644 index 0000000000000000000000000000000000000000..97a39196e84db97954341162a6d2e7f771d938c0 --- /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 0000000000000000000000000000000000000000..27ee1078ff077cc3a0fec75b7d023101a68164d1 --- /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 0000000000000000000000000000000000000000..b234a3840f4c5bd38f043638c8622b8f240e1185 --- /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/text.adoc b/text.adoc new file mode 100644 index 0000000000000000000000000000000000000000..4f84a22ba9f9aa90121af852336fc405c19b90a4 --- /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 new file mode 100644 index 0000000000000000000000000000000000000000..602d6e6936b28b66d5d95eddc913e7146f6b114b --- /dev/null +++ b/ws_nodes_py/CVs/CVCamera.py @@ -0,0 +1,236 @@ +#! /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, 480)): + 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_topic", 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, debug_frame): + binary = self.get_binary(name, frame) + self.__publish_coordinte(func(self.get_counturs(name, binary, True), debug_frame), name) + + 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) + 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): + 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) + debug_frame = frame.copy() + self.mask_id = 0 + self.image_processing(frame, debug_frame) + # Отладочное изображение + self.__pub_debug.publish(self.br.cv2_to_compressed_imgmsg(debug_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 расчёт дистанции + x, y, w, h = cv2.boundingRect(contour) + dist = 1 if cv2.moments(contour)["m00"] >= 500 else 0.1 + + course = (self.get_course(contour) - 10) % 360 + + 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 расчёт дистанции + 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 + + 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, 0.01 * 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) + ly = (cx - self.width / 2) + ln = (lx**2 + ly**2)**0.5 + dist = ln * self.meter_per_pix + # 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: + 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/CVs/videoD.py b/ws_nodes_py/CVs/videoD.py new file mode 100644 index 0000000000000000000000000000000000000000..f80bffcde2dda013f75c4471703633e829fbfa31 --- /dev/null +++ b/ws_nodes_py/CVs/videoD.py @@ -0,0 +1,31 @@ +#! /usr/bin/env python3 + +import rclpy +from ws_nodes_py.CVs.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', -16 if is_real else 0) + if is_real: + 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) + + def image_processing(self, frame, debug_frame): + self.publish_coordinte(self.get_bottom, "red_position", frame, debug_frame) + + +def main(args=None): + rclpy.init(args=args) + RealOpenCV = RosOpencvToBinary() + rclpy.spin(RealOpenCV) + RealOpenCV.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() diff --git a/ws_nodes_py/CVs/videoF.py b/ws_nodes_py/CVs/videoF.py new file mode 100644 index 0000000000000000000000000000000000000000..d1f93312110a540e8833eda06cd4ef9169ed033d --- /dev/null +++ b/ws_nodes_py/CVs/videoF.py @@ -0,0 +1,30 @@ +#! /usr/bin/env python3 + +import rclpy +from ws_nodes_py.CVs.CVCamera import CVCamera + + +class RosOpencvToBinary(CVCamera): + def __init__(self): + 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) + + def image_processing(self, 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): + rclpy.init(args=args) + RealOpenCV = RosOpencvToBinary() + rclpy.spin(RealOpenCV) + RealOpenCV.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() diff --git a/ws_nodes_py/CVs/videoS.py b/ws_nodes_py/CVs/videoS.py new file mode 100644 index 0000000000000000000000000000000000000000..807290b3eff6e3daaa58654fd18ee6e00cb24c83 --- /dev/null +++ b/ws_nodes_py/CVs/videoS.py @@ -0,0 +1,32 @@ +#! /usr/bin/env python3 + +import rclpy +from ws_nodes_py.CVs.CVCamera import CVCamera + + +class RosOpencvToBinary(CVCamera): + def __init__(self): + super().__init__('camcvS', -90) + + # см. ~/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) + + def image_processing(self, 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): + rclpy.init(args=args) + RealOpenCV = RosOpencvToBinary() + rclpy.spin(RealOpenCV) + RealOpenCV.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() diff --git a/ws_nodes_py/StateMachine/Controller.py b/ws_nodes_py/StateMachine/Controller.py new file mode 100644 index 0000000000000000000000000000000000000000..0feccf2167b4129e0c8baec99f9bd11be49dc4e9 --- /dev/null +++ b/ws_nodes_py/StateMachine/Controller.py @@ -0,0 +1,160 @@ +#!/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.settings = {} + + 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.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) + 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 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]: + 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 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() + 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_heading(self): + return self.heading + + 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 0000000000000000000000000000000000000000..8a86f5ec129804440195af09904637ba0163769c --- /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() + 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): + 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() + 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 0000000000000000000000000000000000000000..5ac4476d8114b360e8673970d34ac1f020b33967 --- /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 0000000000000000000000000000000000000000..b3c7cb18754f82fec5c9b7944a6f89c932492f9c --- /dev/null +++ b/ws_nodes_py/StateMachine/MotionStates.py @@ -0,0 +1,160 @@ +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 + +# TODO состояние движения занимается движением +# TODO убрать настройки навигации в отдельное состояние +# TODO убрать объекты, относительно которы навигируемся в отдельное состояние + + +class MoveToGoal(State): + 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'],) + + 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 + 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' + 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=[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 if dist is not None else 0) + 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) + 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) + + 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) + 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): + 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 new file mode 100644 index 0000000000000000000000000000000000000000..1be79251b590889d5a56e04889e87ef80e009452 --- /dev/null +++ b/ws_nodes_py/StateMachine/SpecialStates.py @@ -0,0 +1,225 @@ +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 + + + +class DropPickStakan(State): + def __init__(self, c, time, state:int, is_wait_pickup:bool=None): + State.__init__(self, outcomes=['complite']) + self.cntr = c + + 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) + 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 + + 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) + 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 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" if self.is_close else "Up"} 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']) + 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 and {self.max_dist} m') + is_last_time_detected = False + detect_time = None + while not self.cntr.shutdown(): + 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)))): + 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=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): + 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 + + 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: + self.cntr.get_logger().info(f"Target position is achived with error_dist: {error_dist}") + 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): + 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(): + 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/mission_generators.py b/ws_nodes_py/StateMachine/mission_generators.py new file mode 100644 index 0000000000000000000000000000000000000000..c8663aad07395832ea878bc688be701873e958fb --- /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/StateMachine/state_machine.py b/ws_nodes_py/StateMachine/state_machine.py new file mode 100644 index 0000000000000000000000000000000000000000..81234b754a90e15d1aefebb48230356cadf5a603 --- /dev/null +++ b/ws_nodes_py/StateMachine/state_machine.py @@ -0,0 +1,37 @@ +#!/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() + +if __name__ == '__main__': + main() diff --git a/ws_nodes_py/__init__.py b/ws_nodes_py/__init__.py new file mode 100644 index 0000000000000000000000000000000000000000..e69de29bb2d1d6434b8b29ae775ad8c2e48c5391 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 Binary files /dev/null and b/ws_nodes_py/__pycache__/__init__.cpython-310.pyc differ 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..5309f368e6cef1346ede5f261df4284f74f2dcfe Binary files /dev/null and b/ws_nodes_py/__pycache__/compass.cpython-310.pyc differ diff --git a/ws_nodes_py/__pycache__/regulator.cpython-310.pyc b/ws_nodes_py/__pycache__/regulator.cpython-310.pyc new file mode 100644 index 0000000000000000000000000000000000000000..273f29cef073a4f8a8b9c9bf9a406cdc96f04997 Binary files /dev/null and b/ws_nodes_py/__pycache__/regulator.cpython-310.pyc differ diff --git a/ws_nodes_py/__pycache__/twist_controller.cpython-310.pyc b/ws_nodes_py/__pycache__/twist_controller.cpython-310.pyc new file mode 100644 index 0000000000000000000000000000000000000000..a16d7077b85258d358a262385b4666991f43d2f8 Binary files /dev/null and b/ws_nodes_py/__pycache__/twist_controller.cpython-310.pyc differ 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..c067c2a5ae1093ce2b7a17fd72aed58f345aad29 Binary files /dev/null and b/ws_nodes_py/__pycache__/world.cpython-310.pyc differ diff --git a/ws_nodes_py/compass.py b/ws_nodes_py/compass.py new file mode 100644 index 0000000000000000000000000000000000000000..96694e12df6c2b2e6114efffccb293343e1356b6 --- /dev/null +++ b/ws_nodes_py/compass.py @@ -0,0 +1,229 @@ +#!/usr/bin/python + +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 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 +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 + 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') + + # Имеется в виду новая насадка на мотор или нет (новая - от Кирилла (обтекаемая) или старая - от трионикса (белая)) + 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 = 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 = "AUTO" # FORCE_VIRTUAL, FORCE_REAL, AUTO + self.IS_REAL_VORLD = is_real + + # for zero heading + 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 + + 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.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): + if self.IS_REAL_VORLD: # real settings + default_target_angl_speed = 0 + zero_throtle_multiplier = 0.6 + full_throtle_multiplier = 1 + full_throtle_angle_speed = 60 + is_with_accel = True + else: # sim settings + default_target_angl_speed = 0 + zero_throtle_multiplier = 25 + full_throtle_multiplier = 25 + full_throtle_angle_speed = 14 + is_with_accel = False + + # self.get_logger().info(f"{full_throtle_angle_speed=}") + + try: + dt = time() - self.last_time + except Exception: + dt = 0 + self.last_time = time() + + if (self.is_use_real_compass and self.COMPASS_MODE == "AUTO") or self.COMPASS_MODE == "FORCE_REAL": + 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 full_throtle_multiplier) + else: + self.angular_speed = target_angle_speed + + if is_with_accel: + self.heading += self.angular_speed * dt + else: + self.heading += target_angle_speed * dt + + self.heading = self.heading % 360 + + self.__pub_heading() + + def compass_callback(self, msg): + # Проверка на правильность преобразования + assert -10%360 == 350 + + new_compass = msg.data + + if not 360 >= new_compass >= 0: + 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() + # 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"): + # 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.heading = 0 + elif msg.data == "fix_pos": + self.heading = 0 + self.is_enabled = False + elif msg.data == "update_pos": + self.heading = 0 + self.is_enabled = True + + def __compass_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) + 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 __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(): + 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 0000000000000000000000000000000000000000..58d02aa12dda4e467e40bd07bc4f078765f610d1 --- /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 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 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 + """ + + 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/ObjectsDetection.py b/ws_nodes_py/default/ObjectsDetection.py new file mode 100644 index 0000000000000000000000000000000000000000..d3bf092085973146f13c2e2a529b2c1097445ee4 --- /dev/null +++ b/ws_nodes_py/default/ObjectsDetection.py @@ -0,0 +1,377 @@ +import cv2 +import numpy as np +from ws_nodes_py.settings.robot_settings import is_real + + +class ObjectsDetection: + def __set_real_pool_settings(self): + 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": [((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 + # Проверка, что контур не косается границы (roi_full, 0) - нет проверки + roi_corners, 0, # border, border_offset + # Сортировка пол количеству граней + 0.01, True, 2, # aprox_k is_max_vertices, count_aproxs + ], + }, + "yellow_bou": { + "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 + # Проверка, что контур не косается границы (roi_full, 0) - нет проверки + roi_corners, 0, # border, border_offset + # Сортировка пол количеству граней + 0.01, True, 2, # aprox_k is_max_vertices, count_aproxs + ], + }, + "blue_gate": { + "binary": [((0, 0, 155), (160, 255, 255)), cv2.COLOR_RGB2YUV, self.__get_box_borders(0, 0, 640, 225), False, True], + "contours": [ + # Выделение контуров по размерам + 400, None, 3, True, # min_size, max_size, count_counturs, is_max_size + # Проверка, что контур не косается границы (roi_full, 0) - нет проверки + (0, 0, 640, 225), 0, # border, border_offset + # Сортировка пол количеству граней + 0.01, True, 2, # aprox_k is_max_vertices, count_aproxs + ], + }, + "blue_bou": { + "binary": [((0, 0, 155), (160, 255, 255)), cv2.COLOR_RGB2YUV, self.__get_box_borders(0, 0, 640, 125), False, True], + "contours": [ + # Выделение контуров по размерам + 400, None, 1, True, # min_size, max_size, count_counturs, is_max_size + # Проверка, что контур не косается границы (roi_full, 0) - нет проверки + (0, 0, 640, 125), 0, # border, border_offset + # Сортировка пол количеству граней + 0.01, True, 1, # aprox_k is_max_vertices, count_aproxs + ], + }, + "red_position": { + "binary": [((38, 0, 0), (360, 255, 110)), cv2.COLOR_RGB2HSV, self.__get_box_borders(*roi_full), True, True], + "contours": [ + # Выделение контуров по размерам + 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": [(0, 87), cv2.COLOR_RGB2GRAY, [(0, 200), (0, 325), (295, 325), (295, 375), (640, 375), (640,200)], True, False], + "contours": [ + # Выделение контуров по размерам + 3000, None, 1, True, # min_size, max_size, count_counturs, is_max_size + # Проверка, что контур не косается границы (roi_full, 0) - нет проверки + 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": [(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": [ + # Выделение контуров по размерам + 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 + # Сортировка пол количеству граней + 0.01, True, 1, # aprox_k is_max_vertices, count_aproxs + ], + }, + } + + def __set_virtual_pool_settings(self): + roi_corners = (0, 100, 640, 340) + roi_full = (0, 0, 640, 640) + + self.__settings = { + # "yellow_gate": [((0, 0, 0), (255, 255, 255)), cv2.COLOR_RGB2HSV, roi_corners, False], + "yellow_gate": { + "binary": [ + ((70, 115, 0), (93, 255, 255)), cv2.COLOR_RGB2HSV, # (min_mask, max_mask), color_space + self.__get_box_borders(*roi_corners), False, False # borders, contrast + ], + "contours": [ + # Выделение контуров по размерам + 400, None, 3, True, # min_size, max_size, count_counturs, is_max_size + # Проверка, что контур не косается границы (roi_full, 0) - нет проверки + roi_full, 0, # border, border_offset + # Сортировка пол количеству граней + 0.01, True, 3, # aprox_k is_max_vertices, count_aproxs + ], + }, + "yellow_bou": { + "binary": [ + ((70, 115, 0), (93, 255, 255)), cv2.COLOR_RGB2HSV, # (min_mask, max_mask), color_space + self.__get_box_borders(*roi_corners), False, 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, self.__get_box_borders(0, 100, 640, 250), False, False], + "contours": [ + # Выделение контуров по размерам + 400, None, 3, True, # min_size, max_size, count_counturs, is_max_size + # Проверка, что контур не косается границы (roi_full, 0) - нет проверки + roi_full, 0, # border, border_offset + # Сортировка пол количеству граней + 0.01, True, 3, # aprox_k is_max_vertices, count_aproxs + ], + }, + "blue_bou": { + "binary": [((0, 83, 0), (19, 255, 255)), cv2.COLOR_RGB2HSV, self.__get_box_borders(0, 100, 640, 250), False, False], + "contours": [ + # Выделение контуров по размерам + 400, None, 3, True, # min_size, max_size, count_counturs, is_max_size + # Проверка, что контур не косается границы (roi_full, 0) - нет проверки + roi_full, 0, # border, border_offset + # Сортировка пол количеству граней + 0.01, True, 3, # aprox_k is_max_vertices, count_aproxs + ], + }, + "black_position_front": { + "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 + # Проверка, что контур не косается границы (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, self.__get_box_borders(*roi_full), False, 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 + ], + }, + "red_poster_front": { + "binary": [((164, 0, 80), (185, 255, 130)), cv2.COLOR_RGB2HSV_FULL, self.__get_box_borders(*roi_full), True, False], + "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": [((164, 0, 80), (185, 255, 130)), cv2.COLOR_RGB2HSV_FULL, self.__get_box_borders(*roi_full), True, False], + "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": [((73, 0, 31), (100, 255, 130)), cv2.COLOR_RGB2HSV_FULL, self.__get_box_borders(*roi_corners), True, False], + "contours": [ + # Выделение контуров по размерам + 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, 1, # aprox_k is_max_vertices, count_aproxs + ], + }, + "red_position": { + "binary": [((90, 0, 0), (180, 255, 255)), cv2.COLOR_RGB2HSV, self.__get_box_borders(*roi_full), False, False], + "contours": [ + # Выделение контуров по размерам + 100, 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 + ] + }, + } + + 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) + binary = cv2.inRange(frame, *mask) + + # Задаём рамку + binary = self.__set_borders(binary, roi_corners) + + 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]] + + 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] + + # Трешхолд по размеру контура + if min_size is not None and max_size is not None: + couturs_size_checker = lambda contour: max_size >= 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, array): + if len(frame.shape) == 3: + channel_count = frame.shape[2] + else: + channel_count = 1 + + 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) + 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, 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_counturs(self, aim, frame, is_binary): + if not is_binary: + frame = self.get_binary(aim, frame) + + if aim in self.__settings and "contours" in self.__settings[aim]: + return self.__get_counturs(frame, *self.__settings[aim]["contours"]) + + raise Exception(f"no {aim} in contours settings") + return [] + + 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__/GetParameterNode.cpython-310.pyc b/ws_nodes_py/default/__pycache__/GetParameterNode.cpython-310.pyc new file mode 100644 index 0000000000000000000000000000000000000000..23ca236b27894714d2a9edad37c9bda57fd7c77c Binary files /dev/null and b/ws_nodes_py/default/__pycache__/GetParameterNode.cpython-310.pyc differ 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..994cfb3497bb7a70515272ee5fa52b30c1615475 Binary files /dev/null and b/ws_nodes_py/default/__pycache__/GetParameterNode.cpython-38.pyc differ 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..786aa1e074d57ba5307e633a857f2ce9b9c9b953 Binary files /dev/null and b/ws_nodes_py/default/__pycache__/ObjectsDetection.cpython-310.pyc differ diff --git a/ws_nodes_py/legacy/regulator copy.py b/ws_nodes_py/legacy/regulator copy.py new file mode 100644 index 0000000000000000000000000000000000000000..149987de52e271d1123d046fab3fdab2931ef24a --- /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 0000000000000000000000000000000000000000..b662d2bb87ba15f47cd5e443411165d533eacb64 --- /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/nmea_parser.py b/ws_nodes_py/nmea_parser.py new file mode 100644 index 0000000000000000000000000000000000000000..eda1d0c53ae90e2b512feccf4d8c14de14a380d8 --- /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/orientation_by_mrmr.py b/ws_nodes_py/orientation_by_mrmr.py new file mode 100644 index 0000000000000000000000000000000000000000..5586f9441ff03e8b5791ddce83ad4ce862711055 --- /dev/null +++ b/ws_nodes_py/orientation_by_mrmr.py @@ -0,0 +1,52 @@ +#!/usr/bin/python + +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 nav_msgs.msg import Odometry +from math import degrees, acos +from geometry_msgs.msg import PointStamped + + +class MrMrPosition(GetParameterNode): + def __init__(self): + super().__init__("orientation_by_mrmr") + if is_real: + 1/0 + # add mrmr recieve + self.create_subscription('', '', self.__mrmr_callback, 10) + else: + self.create_subscription(Odometry, 'pose_gt', self.__odom_callback, 10) + + self.__pub_pos = self.publisher_from_declare(PointStamped, "global_position_topic", checker=lambda x: x, count=10) + + def __mrmr_callback(self, msg): + # TODO + pass + + def __odom_callback(self, odom): + position = odom.pose.pose.position + x, y = position.x, position.y + # go pool coord + x = x + 10 + y = -(y + 4) + self.__send_position(x, y) + + def __send_position(self, x, y): + msg = PointStamped() + msg.header.stamp = self.get_clock().now().to_msg() + msg.header.frame_id = f"mrmr_position" + msg.point.x, msg.point.y = x, y + self.__pub_pos.publish(msg) + + +def main(): + rclpy.init() + mrmr = MrMrPosition() + rclpy.spin(mrmr) + mrmr.destroy_node() + rclpy.shutdown() + +if __name__ == '__main__': + main() diff --git a/ws_nodes_py/point_logger.py b/ws_nodes_py/point_logger.py new file mode 100644 index 0000000000000000000000000000000000000000..adfde327d81ce2f280786f2dee1cae735230b9e2 --- /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 0000000000000000000000000000000000000000..868cf8429fd05bf37d0178e986f7cb94eee8e3af --- /dev/null +++ b/ws_nodes_py/regulator.py @@ -0,0 +1,198 @@ +#!/usr/bin/env python3 + +import rclpy +from ws_nodes_py.default.GetParameterNode import GetParameterNode +from ws_nodes_py.settings.robot_settings import is_real +from std_msgs.msg import Int16, String +from geometry_msgs.msg import Twist, Point, PointStamped +from math import acos, degrees, sin, cos, radians, copysign +# pip3 install simple_pid==2.0.1 +from simple_pid import PID +# from nonius_msgs.msg import Track + + +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) + + if is_real: + self.IS_WITH_PID = True + ang_p = 0.015 + ang_d = 0.002 + self.IS_WITH_LIN_PID = True + lin_p = 0.75 + 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()) + 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) + + self.create_update(self.__update) + self.get_logger().info('Auto teleoperation started') + + def __update(self): + if self.type_move == "MOVE TWIST": + self.__move_by_twist(self.goal.x, self.goal.y) + elif self.type_move == "MOVE TO": + 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 + + 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 = 20 + if mode == "clockwise": + angle = around_heading + point_target_delta_degrees + elif mode == "anticlockwise": + angle = around_heading - point_target_delta_degrees + else: + if self.__angle_error(heading_to_point, self.heading) % 360 <= 180: + angle = around_heading + point_target_delta_degrees + else: + angle = around_heading - point_target_delta_degrees + + if target_dist == 0: + target_dist = self.__eval_dist(self.position, point) + + 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(goal, 0.09) + else: + 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) + 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 + # 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 + + if self.IS_WITH_LIN_PID: + value = -self.pid_lin(error_dist) + 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): + 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) + + def __get_angular_speed(self, err_angle): + value = 0 + if self.IS_WITH_PID: + if abs(err_angle) > 5: + value = self.pid_ang(err_angle) + else: + 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) + + def __angle_error(self, target_angle, source_angle): + result = (target_angle - source_angle) % 360 + if result > 180: + result -= 360 + return result + + def __get_heading_to_point(self, goal): + """ + Возвращает курс на цель, если положение точки совпадает с целью, то возвращает 0 + """ + goal = self.__point_to_list(goal) + dx, dy = goal[0] - self.position.x, goal[1] - self.position.y + 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, 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): + 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(): + 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/settings/__pycache__/for_world.cpython-310.pyc b/ws_nodes_py/settings/__pycache__/for_world.cpython-310.pyc new file mode 100644 index 0000000000000000000000000000000000000000..22e7e213de182100eb8ed7791b632463451f97ba Binary files /dev/null and b/ws_nodes_py/settings/__pycache__/for_world.cpython-310.pyc differ 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..abb19df6977577029c8f8919f299e576fb73737c Binary files /dev/null and b/ws_nodes_py/settings/__pycache__/for_world.cpython-38.pyc differ diff --git a/ws_nodes_py/settings/__pycache__/robot_settings.cpython-310.pyc b/ws_nodes_py/settings/__pycache__/robot_settings.cpython-310.pyc new file mode 100644 index 0000000000000000000000000000000000000000..4bcd6b01b89cc1b76c17df35c6b6fa9914c8aa75 Binary files /dev/null and b/ws_nodes_py/settings/__pycache__/robot_settings.cpython-310.pyc differ diff --git a/ws_nodes_py/settings/for_world.py b/ws_nodes_py/settings/for_world.py new file mode 100644 index 0000000000000000000000000000000000000000..3080e5be2d1618759e1e41ac86304943366556c2 --- /dev/null +++ b/ws_nodes_py/settings/for_world.py @@ -0,0 +1,79 @@ +world_markers = { + "start_point": [0, 0], + "yellow_gate": [6, 0], + "blue_gate": [8, 0], + "blue_bou": [10, 0], + "black_position": [10, 10], + "red_position": [9, 3], #???? + "red_poster": [10, 0], + "trash": [0, 10], +} + +start_position = [0, 0] + +zero_speed_signal = 1500 + + +sim_motor_speed = { +# signal: speed (in m/s) + 1500: 0, + 1540: 0, + 1537: 0.10, + 1570: 0.11, + 1580: 0.11, + 1600: 0.40, + 1700: 0.73, + 1800: 0.95, + 1900: 1.04, + 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, + # 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", + "red_poster": "red_poster", +} \ No newline at end of file diff --git a/ws_nodes_py/settings/mission.py b/ws_nodes_py/settings/mission.py new file mode 100644 index 0000000000000000000000000000000000000000..76ebf632493e30a3eb2d6bc4445c1ead5c1dbfa2 --- /dev/null +++ b/ws_nodes_py/settings/mission.py @@ -0,0 +1,475 @@ +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 = [ + [ + [WorldSettings, {"is_real_compass": True, "is_update_pos": True}], + [MoveByTwist, {"goal": [0, 0]}], + [Wait, {"time": 2}], + ], +] + +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 + +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 move_by_time(goal:list, time:int): + return [ + [ + [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.10, inv_clock(0.075 if is_real else 0.13)]}], + [FindObject, {"object_name": "blue_bou"}], + ], [ # Ждём пока он уйдёт в корму + [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.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, inv_clock(-0.17)]}], + [FindObject, {"object_name": "yellow_gate", "min_max_angle": [0, 360], "time": 2}], + ] +] + +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 OF VLAD24 MISSION + +def get_drop_cube(): + if is_real: + 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 [ + [ # Сближаемся с чёрным кругом + [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_and_drop_cube = [ + *on_navigation, + [DropZabralo, {"is_close": False}], + [ # Отходим от ворот + [MoveByTwist, {"goal": [0.05 if is_real else 0.02, 0.17]}], + # [Wait, {"time": 45}], + [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]}], + ], + *get_drop_cube(), +] + +second_step = move_and_drop_cube + + +# THIRD OF VLAD24 MISSION + +around_island_in_real = [ + [ + [MoveByTwist, {"goal": [0.15, 0]}], + [Wait, {"time": 5}], + ], + [ + [MoveByTwist, {"goal": [0.01, inv_clock(0.15)]}], + # [Wait, {"time": 12}], + [FindObject, {"object_name": "black_position_side", "min_max_angle": [270, 90]}], + ], + [ + [MoveByTwist, {"goal": [0.03, inv_clock(-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}], + [FindObject, {"max_dist": 0.15, "object_name": "red_position"}], + ] +] + +drop_stakan = [ + [ + [DropPickStakan, {"time": 15, "state": 1, "is_wait_pickup": True}], + [MoveByTwist, {"goal": [0, 0]}] + ], [ + [DropPickStakan, {"time": 40, "state": 0, "is_wait_pickup": True}], + [MoveByTwist, {"goal": [0, 0]}] + ] +] + + +third_step = [] +# third_step += around_island_in_real +third_step += move_to_red_point + drop_stakan + + +# FOURTH STEP OF VLAD24 MISSION + +move_to_red_poster = [ + *on_navigation, + [DropZabralo, {"is_close": False}], + [ + [MoveByTwist, {"goal": [0, 0.15]}], + [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, {}], + ] +] + +balls_collecting = [ + [ + [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}], + [ + [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]}], + ], [ + [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}], + ] +] + +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_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) Для всех этапов надо записать БЭКИ +1) Откалибровать компасс +2) Надо проверить корректность работы магнитного компаса в нужных точках (во всём бассейне) +""" + +# Отладка ПЕРВОГО этапа +""" +1) Требуется подстройка масок под бассейн (жёлтые и синие ворота), если они не подходят +(цветовой сегментации, рамок и границ за которыми изображение отбрасывается) +2) Отработка прохождения первого этапа +""" +# mission = first_step # проход через жёлтые, через синие, через жёлтые +# 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") # отдельно проход через синие + +# Отладка ВТОРОГО этапа +""" +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 = miss + + +mission = gals_hodit_plavat_kursach_delat([[4, 0], [4, -4], [0, -4], [0, 0]] * 2) + + +""" +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 = move_to_red_point + +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]}], + ], +] + + +# 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}] +] + + +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]}], + ], +] + +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 diff --git a/ws_nodes_py/settings/robot_settings.py b/ws_nodes_py/settings/robot_settings.py new file mode 100644 index 0000000000000000000000000000000000000000..bbe73cddd30dfb06c1ff814f795a48264c5b8765 --- /dev/null +++ b/ws_nodes_py/settings/robot_settings.py @@ -0,0 +1,25 @@ +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.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", # в хабе +} + +camcv = {'F':'camera_front', 'S':'camera_side', 'D':'camera_down'} \ 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 0000000000000000000000000000000000000000..9dfc36e74ef7441dc5bf90bd3d6591c1b5468323 --- /dev/null +++ b/ws_nodes_py/twist_controller.py @@ -0,0 +1,66 @@ +#!/usr/bin/python + +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 + + +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") + + self.is_double_angular_speed_for_simulation = not is_real + + # 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) + + # publisher + 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() + self.__set_twist(msg) + + def robot_callback(self, msg: Twist): + if self.last_time_joy + self.delta_time < time(): + 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(): + 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/world.py b/ws_nodes_py/world.py new file mode 100644 index 0000000000000000000000000000000000000000..39ea8a3044d84b13137d28249dea4eda58ac9419 --- /dev/null +++ b/ws_nodes_py/world.py @@ -0,0 +1,113 @@ +#!/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 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): + try: + delta_time = time() - self.last_time + except AttributeError: + self.get_logger().info("start updating") + delta_time = 0 + self.last_time = time() + + 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 + + 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.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) + 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"): + 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.position = start_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.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.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", "")] + 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(): + rclpy.init() + world = World() + rclpy.spin(world) + world.destroy_node() + rclpy.shutdown() + +if __name__ == '__main__': + main()