diff --git a/ws_nodes_py/CVs/CVCamera.py b/ws_nodes_py/CVs/CVCamera.py index 602d6e6936b28b66d5d95eddc913e7146f6b114b..96fe82fe1106fdacef51d69428c513aff0ade34e 100644 --- a/ws_nodes_py/CVs/CVCamera.py +++ b/ws_nodes_py/CVs/CVCamera.py @@ -95,10 +95,10 @@ class CVCamera(GetParameterNode, ObjectsDetection): # Вывести расчётную дистанцию до контура 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) + cv2.rectangle(frame, (x, y), (x+w, y+h), (0, 255, 0), 2) + #Оси через центр контура + cv2.line(frame, (x, 0), (x, self.height), (0, 255, 0), 1) + cv2.line(frame, (0, y), (self.width, y), (0, 255, 0), 1) if len(posters) >= 1: return posters[:1] diff --git a/ws_nodes_py/CVs/videoD.py b/ws_nodes_py/CVs/videoD.py index f80bffcde2dda013f75c4471703633e829fbfa31..64738530277a0161448ccc0c90d0a80e10a730f8 100644 --- a/ws_nodes_py/CVs/videoD.py +++ b/ws_nodes_py/CVs/videoD.py @@ -17,6 +17,8 @@ class RosOpencvToBinary(CVCamera): def image_processing(self, frame, debug_frame): self.publish_coordinte(self.get_bottom, "red_position", frame, debug_frame) + self.publish_coordinte(self.get_bottom, "black_position", frame, debug_frame) + # self._logger.info(self.get) def main(args=None): diff --git a/ws_nodes_py/StateMachine/Controller.py b/ws_nodes_py/StateMachine/Controller.py index 0feccf2167b4129e0c8baec99f9bd11be49dc4e9..d271d990240f6c723a6760df3100bc28cae83651 100644 --- a/ws_nodes_py/StateMachine/Controller.py +++ b/ws_nodes_py/StateMachine/Controller.py @@ -16,7 +16,7 @@ class Controller(Node): def __init__(self): super().__init__('state_machine_teleop') - self.get_logger().info('State machine started') + self.position = Point() @@ -29,6 +29,7 @@ class Controller(Node): self.detected_objects_lifetime = 1 # int sec self.settings = {} + self.__new_taget_point = Point() self.running = True @@ -41,7 +42,9 @@ class Controller(Node): 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.declare_parameter("name", rclpy.Parameter.Type.STRING) + self.waterstrider_name = self.declare_parameter("name", "01").value + self.get_logger().info(f'State machine started, my name is: {self.waterstrider_name}') 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) @@ -51,7 +54,10 @@ class Controller(Node): 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.ask_new_taget_publisher = self.create_publisher(PointStamped, "/ask_new_target", 1) + + self.create_subscription(PointStamped, "/global_goal", self.global_goal_callback, 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) @@ -76,6 +82,17 @@ class Controller(Node): break else: self.detected_objects.append(msg) + + def global_goal_callback(self, msg:PointStamped): + self.get_logger().info(f"{msg.header.frame_id}") + if msg.header.frame_id == self.waterstrider_name: + self.__new_taget_point.x, self.__new_taget_point.y = msg.point.x, msg.point.y + + def ask_new_target_point(self): + askMsg = PointStamped() + askMsg.header.frame_id = self.waterstrider_name + askMsg.point.x, askMsg.point.y = self.position.x, self.position.y + self.ask_new_taget_publisher.publish(askMsg) def set_settings(self, key, val): self.settings[key] = val @@ -158,3 +175,11 @@ class Controller(Node): def shutdown(self): return not self.running + + @property + def new_target_point(self): + return self.__new_taget_point + + @new_target_point.setter + def new_target_point(self, point): + self.__new_taget_point = point \ No newline at end of file diff --git a/ws_nodes_py/StateMachine/MissionPlanners/SimpleMissionPlanner.py b/ws_nodes_py/StateMachine/MissionPlanners/SimpleMissionPlanner.py new file mode 100644 index 0000000000000000000000000000000000000000..7b7f2b724bf527946b058824e2df87d5ad779ce5 --- /dev/null +++ b/ws_nodes_py/StateMachine/MissionPlanners/SimpleMissionPlanner.py @@ -0,0 +1,40 @@ +import math +class SimpleMissionPlanner: + def __init__(self, tack_width, start_position, area_size): + self.tack_width = tack_width + self.current_lat = start_position[0] + self.current_lon = start_position[1] + self.area_width = area_size[1] + self.area_height = area_size[0] + self.target_lat = self.current_lat + self.target_lon = self.current_lon + self.path = list() + + def make_tack_path(self, type): + if type == 'vertical': + self.tack_count = math.trunc(self.area_width/self.tack_width) // 2 + for i in range(self.tack_count): + self.current_lat += self.area_height + self.path.append([self.current_lat, self.current_lon]) + self.current_lon -= self.tack_width + self.path.append([self.current_lat, self.current_lon]) + self.current_lat -=self.area_height + self.path.append([self.current_lat, self.current_lon]) + self.current_lon -= self.tack_width + self.path.append([self.current_lat, self.current_lon]) + + elif type == 'horizontal': + self.tack_count = math.trunc(self.area_height / self.tack_width)// 2 + for i in range(self.tack_count): + self.current_lon += self.area_width + self.path.append([self.current_lat, self.current_lon]) + self.current_lat -= self.tack_width + self.path.append([self.current_lat, self.current_lon]) + self.current_lon -= self.area_width + self.path.append([self.current_lat, self.current_lon]) + self.current_lat -= self.tack_width + self.path.append([self.current_lat, self.current_lon]) + # self.current_lat -=self.area_height + # self.path.append([self.current_lat, self.current_lon]) + # self.current_lon -= self.tack_width + # self.path.append([self.current_lat, self.current_lon]) \ No newline at end of file diff --git a/ws_nodes_py/StateMachine/MotionStates.py b/ws_nodes_py/StateMachine/MotionStates.py index b3c7cb18754f82fec5c9b7944a6f89c932492f9c..fd34f164433bd4a1ac87b5d3e57447ca9bf56371 100644 --- a/ws_nodes_py/StateMachine/MotionStates.py +++ b/ws_nodes_py/StateMachine/MotionStates.py @@ -1,6 +1,8 @@ -import rclpy.duration +from rclpy.duration import Duration +import rclpy from math import pi, copysign, atan2, sin, cos, radians, degrees, acos from ws_nodes_py.settings.for_world import world_markers +from geometry_msgs.msg import Point from smach import State # TODO состояние движения занимается движением @@ -157,4 +159,81 @@ class MoveByHeading(State): while not self.cntr.shutdown(): self.cntr.update() rclpy.spin_once(self.cntr) - return 'complite' \ No newline at end of file + return 'complite' + + +class MoveToPoint(State): + def __init__(self, c, 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 + # Получаем координаты объекта из данных о метке или задём координаты вручную + self.distance_threshold = dist_treshold + self.__is_started = False + self.__is_running = False + self.__is_asked = False + self.__target_point = Point() + + def execute(self, userdata): + if not self.__is_started: + self.cntr.next_step("STOP", [0, 0]) + self.__is_started = True + self.cntr.get_logger().info(f'Wait target from global planner') + while not self.cntr.shutdown(): + self.cntr.update() + rclpy.spin_once(self.cntr) + if not self.__is_running and not self.__is_asked: + self.cntr.ask_new_target_point() + self.cntr.get_logger().info(f'Wait new target point') + self.cntr.next_step("STOP", [0, 0]) + self.state_change_time = self.cntr.get_clock().now() + self.__is_asked = True + elif not self.__is_running and self.__is_asked: + if (self.cntr.get_clock().now() - self.state_change_time) > Duration(seconds=10): + self.cntr.ask_new_target_point() + self.state_change_time = self.cntr.get_clock().now() + self.cntr.get_logger().info(f'Ask again') + self.cntr.get_logger().info(f'{self.__target_point.x, self.__target_point.y}, {self.cntr.new_target_point.x, self.cntr.new_target_point.y}') + if self.cntr.new_target_point != self.__target_point: + self.__target_point.x,self.__target_point.y = self.cntr.new_target_point.x, self.cntr.new_target_point.y + self.cntr.get_logger().info(f'My life - is my rules. The new target - is my target {self.__target_point.x}, {self.__target_point.y}') + self.__is_running = True + self.__is_asked = False + elif self.__is_running: + self.cntr.next_step("MOVE TO", [self.__target_point.x, self.__target_point.y], True, "mrmr_position", True) + # self.cntr.get_logger().info(f"I'm start walking to") + error_dist = self.calculate_dist(self.cntr.position, self.__target_point) + + # 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: + self.__is_running = False + 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 + + def calculate_dist(self, point1, point2): + return ((point2.x - point1.x)**2 + (point2.y - point1.y)**2)**0.5 \ No newline at end of file diff --git a/ws_nodes_py/StateMachine/mission_generators.py b/ws_nodes_py/StateMachine/mission_generators.py index c8663aad07395832ea878bc688be701873e958fb..dd69a1ad49202f315f23e07d66d7c0eecabb8e64 100644 --- a/ws_nodes_py/StateMachine/mission_generators.py +++ b/ws_nodes_py/StateMachine/mission_generators.py @@ -3,7 +3,7 @@ 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"): +def diplom_mission(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}]) diff --git a/ws_nodes_py/StateMachine/state_machine.py b/ws_nodes_py/StateMachine/state_machine.py index 81234b754a90e15d1aefebb48230356cadf5a603..0a84af91af25df7a4c90a42dbf82b4c4f9944e05 100644 --- a/ws_nodes_py/StateMachine/state_machine.py +++ b/ws_nodes_py/StateMachine/state_machine.py @@ -1,7 +1,7 @@ #!/usr/bin/env python import rclpy.logging -from ws_nodes_py.settings.mission import mission +from ws_nodes_py.settings.mission import diplom_mission as mission from ws_nodes_py.StateMachine.MissionMachine import MissionMachine import smach import rclpy diff --git a/ws_nodes_py/__pycache__/__init__.cpython-310.pyc b/ws_nodes_py/__pycache__/__init__.cpython-310.pyc index 6f3e9bb01dacb26cdc3b2bf0a4fb3c3601a3e9b5..2d097b5712cae105a2239f3f7449ae94f11f07a1 100644 Binary files a/ws_nodes_py/__pycache__/__init__.cpython-310.pyc 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 index 5309f368e6cef1346ede5f261df4284f74f2dcfe..394c4b00339a3ec94dbfc3c327babaa8a44e73fe 100644 Binary files a/ws_nodes_py/__pycache__/compass.cpython-310.pyc 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 index 273f29cef073a4f8a8b9c9bf9a406cdc96f04997..963da691bd9c5a0c53f07ef5fd4a10a01bc01717 100644 Binary files a/ws_nodes_py/__pycache__/regulator.cpython-310.pyc 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 index a16d7077b85258d358a262385b4666991f43d2f8..7d3fc96514ccb5d94923d11637c4799b2acf3a6c 100644 Binary files a/ws_nodes_py/__pycache__/twist_controller.cpython-310.pyc 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 index c067c2a5ae1093ce2b7a17fd72aed58f345aad29..913292fbfdb46e04cbe25b286e9dd242ac788172 100644 Binary files a/ws_nodes_py/__pycache__/world.cpython-310.pyc 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 index 96694e12df6c2b2e6114efffccb293343e1356b6..048ba30b25c313c3b0e76d916633e9c5e80eef32 100644 --- a/ws_nodes_py/compass.py +++ b/ws_nodes_py/compass.py @@ -9,8 +9,14 @@ 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 +from numpy import * + if not is_real: + import numpy as np + np.float = float + np.int = int #module 'numpy' has no attribute 'int' + np.object = object #module 'numpy' has no attribute 'object' + np.bool = bool # sudo apt-get install ros--tf-transformations from tf_transformations import euler_from_quaternion from sensor_msgs.msg import Imu diff --git a/ws_nodes_py/default/ObjectsDetection.py b/ws_nodes_py/default/ObjectsDetection.py index d3bf092085973146f13c2e2a529b2c1097445ee4..d1a439a0c5ad34b4e63c313a068cc751d7b4bf9c 100644 --- a/ws_nodes_py/default/ObjectsDetection.py +++ b/ws_nodes_py/default/ObjectsDetection.py @@ -267,6 +267,17 @@ class ObjectsDetection: 0.01, True, 2, # aprox_k is_max_vertices, count_aproxs ] }, + "black_position": { + "binary": [((49, 0, 0), (64, 133, 135)), cv2.COLOR_RGB2YUV, self.__get_box_borders(*roi_full), False, False], + "contours": [ + # Выделение контуров по размерам + 1000, 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): diff --git a/ws_nodes_py/default/__pycache__/GetParameterNode.cpython-310.pyc b/ws_nodes_py/default/__pycache__/GetParameterNode.cpython-310.pyc index 23ca236b27894714d2a9edad37c9bda57fd7c77c..9e419ae84b12a4d746019e65254046bd4f238a5a 100644 Binary files a/ws_nodes_py/default/__pycache__/GetParameterNode.cpython-310.pyc and b/ws_nodes_py/default/__pycache__/GetParameterNode.cpython-310.pyc differ diff --git a/ws_nodes_py/default/__pycache__/ObjectsDetection.cpython-310.pyc b/ws_nodes_py/default/__pycache__/ObjectsDetection.cpython-310.pyc index 786aa1e074d57ba5307e633a857f2ce9b9c9b953..455aad08782e999d3b977c328fc896b9ededfd55 100644 Binary files a/ws_nodes_py/default/__pycache__/ObjectsDetection.cpython-310.pyc and b/ws_nodes_py/default/__pycache__/ObjectsDetection.cpython-310.pyc differ diff --git a/ws_nodes_py/orientation_by_mrmr.py b/ws_nodes_py/orientation_by_mrmr.py index 5586f9441ff03e8b5791ddce83ad4ce862711055..5b005bd44f8ddfc38e8b52348c21c6231b92719d 100644 --- a/ws_nodes_py/orientation_by_mrmr.py +++ b/ws_nodes_py/orientation_by_mrmr.py @@ -28,9 +28,11 @@ class MrMrPosition(GetParameterNode): 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.get_logger().info(f"{position.x, x, position.y, y}") self.__send_position(x, y) def __send_position(self, x, y): diff --git a/ws_nodes_py/settings/__pycache__/for_world.cpython-310.pyc b/ws_nodes_py/settings/__pycache__/for_world.cpython-310.pyc index 22e7e213de182100eb8ed7791b632463451f97ba..b8e60a88e3378756f048d721582feaa82e56dbe8 100644 Binary files a/ws_nodes_py/settings/__pycache__/for_world.cpython-310.pyc and b/ws_nodes_py/settings/__pycache__/for_world.cpython-310.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 index 4bcd6b01b89cc1b76c17df35c6b6fa9914c8aa75..a6a417f14e3b2dbc523ee037b538285e16a76d63 100644 Binary files a/ws_nodes_py/settings/__pycache__/robot_settings.cpython-310.pyc and b/ws_nodes_py/settings/__pycache__/robot_settings.cpython-310.pyc differ diff --git a/ws_nodes_py/settings/mission.py b/ws_nodes_py/settings/mission.py index 76ebf632493e30a3eb2d6bc4445c1ead5c1dbfa2..ef5c190bb39624564ad72421c7b0f65db4f48fa4 100644 --- a/ws_nodes_py/settings/mission.py +++ b/ws_nodes_py/settings/mission.py @@ -3,6 +3,7 @@ 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 * +from ws_nodes_py.StateMachine.MissionPlanners.SimpleMissionPlanner import * on_navigation = [ @@ -285,6 +286,11 @@ fourth_step_short = [ # 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 + +diplom_mission = [ + [ + [MoveToPoint, {"dist_treshold":0.2}], + ]] # mission = second_step # mission = third_step # mission = fourth_step @@ -339,8 +345,15 @@ mission = first_step + second_step + third_step + fourth_step_short # mission = miss -mission = gals_hodit_plavat_kursach_delat([[4, 0], [4, -4], [0, -4], [0, 0]] * 2) +# mission = diplom_mission([[4, 0], [4, -4], [0, -4], [0, 0]] * 2) + +# simple_planer = SimpleMissionPlanner(1,(20, -8), (20, 8)) +# simple_planer.make_tack_path('horizontal') +# mission = diplom_mission(simple_planer.path) +# simple_planer = SimpleMissionPlanner(1,(0,0), (20, 8)) +# simple_planer.make_tack_path('vertical') +# mission = diplom_mission(simple_planer.path) """ mission = trough_gate("blue") diff --git a/ws_nodes_py/settings/robot_settings.py b/ws_nodes_py/settings/robot_settings.py index bbe73cddd30dfb06c1ff814f795a48264c5b8765..f1d9b79bb381f4ec0ea754f20db88150722cb892 100644 --- a/ws_nodes_py/settings/robot_settings.py +++ b/ws_nodes_py/settings/robot_settings.py @@ -22,4 +22,4 @@ cameras_ports = { "14.0-3.4.1": "camera_down", # в хабе } -camcv = {'F':'camera_front', 'S':'camera_side', 'D':'camera_down'} \ No newline at end of file +camcv = {'D':'camera_down'} \ No newline at end of file diff --git a/ws_nodes_py/world.py b/ws_nodes_py/world.py index 39ea8a3044d84b13137d28249dea4eda58ac9419..492f653c369f842b1f6a73eb7a571f81038f1f91 100644 --- a/ws_nodes_py/world.py +++ b/ws_nodes_py/world.py @@ -10,36 +10,36 @@ 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() + # 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 + # 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) + # # Линейная интерполяция + # speed = round(self.motor_speed[low] + ((med_signal - low) / (high - low) * (self.motor_speed[high] - self.motor_speed[low]) if high != low else 0), 2) - if self.throtle < 0: - speed = -speed + # if self.throtle < 0: + # speed = -speed - # Расчёт смещения - dx = speed * cos(radians(self.heading)) * delta_time - dy = speed * sin(radians(self.heading)) * delta_time - # Обновление позиции - self.position[0] += dx - self.position[1] += dy + # # Расчёт смещения + # dx = speed * cos(radians(self.heading)) * delta_time + # dy = speed * sin(radians(self.heading)) * delta_time + # # Обновление позиции + # self.position[0] += dx + # self.position[1] += dy msg = Point() msg.x = float(self.position[0]) @@ -81,14 +81,14 @@ class World(GetParameterNode): if msg.data == "reset_map": self.position = start_position[:] # self.get_logger().info(f"reset map {self.position}") - elif msg.data == "fix_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 + # 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: