⛄
new_Базовый курс по ROS
  • Обложка
  • Урок 1. Введение в ROS
    • Что такое ROS ?
    • Знакомство с роботом TurtleBro
    • Настройка ПК
    • Установка ROS
    • Настройка робота TurtleBro
    • Настройка VSCode
    • Практические задания
  • Урок 2. Базовый понятия ROS
    • Основы теории управления
    • Сущности ROS как элементы ТАУ
    • Базовые утилиты ROS
    • Практические задания
  • Урок 3. Издатель
    • Издатель (Publisher)
    • Издатель команд для робота
    • Практические задания
  • Урок 4. Подписчик
    • Подписчик (Subscriber)
    • Подписчик на данные робота
    • Практические задания
  • Урок 5. Подписчик и Издатель
    • Взаимодействие Подписчика и Издателя в рамках одной ноды. Часть 1
    • Практические задания
    • Взаимодействие Подписчика и Издателя в рамках одной ноды. Часть 2
    • Практические задания
      • Решение
  • Урок 6. Работа с Arduino
    • Работа с Arduino
    • Создание Издателя
    • Создание Подписчика
    • Практические задания
  • Курсовая работа
    • Задание
  • Итог
    • Заключение
Powered by GitBook
On this page

Was this helpful?

  1. Урок 5. Подписчик и Издатель
  2. Практические задания

Решение

import rospy
from geometry_msgs.msg import Twist
from geometry_msgs.msg import Pose2D

class RobotMover():
    def __init__(self):
        rospy.init_node("one_meter_move")
        self.vel = Twist()
        self.pub = rospy.Publisher("/cmd_vel", Twist, queue_size=10)
        rospy.Subscriber("/odom_pose2d", Pose2D, self.cb_manager)
        self.passed_path = 0
        self.angle = 0
        
        self.state = 0  # 0 - вперед, 1 - разворот, 2 - назад, 3 - разворот обратно, 4 - стоп
        self.start_x = None
        self.turn_start_angle = None

    def cb_manager(self, msg):
        self.passed_path = msg.x
        self.angle = msg.theta
        print(f"проехали: {self.passed_path}, угол:  {self.angle}")


    def vel_publish(self, value_x, value_z):
        self.vel.linear.x = value_x
        self.vel.angular.z = value_z
        self.pub.publish(self.vel)
    
    def move(self):
        print(f"проехали: {self.passed_path}, угол: {self.angle}")

        if self.start_x is None:
            self.start_x = self.passed_path

        distance = self.passed_path - self.start_x

        if self.state == 0:  # Едем вперед
            if distance >= 1.0:
                self.vel_publish(0, 0)
                self.turn_start_angle = self.angle
                self.state = 1
                rospy.sleep(1)
            else:
                self.vel_publish(0.2, 0)

        elif self.state == 1:  # Разворот на 180
            angle_diff = abs(self.angle - self.turn_start_angle)
            if angle_diff >= 3.12:
                self.vel_publish(0, 0)
                self.state = 2
                rospy.sleep(1)
            else:
                self.vel_publish(0, 0.5)

        elif self.state == 2:  # Едем назад 
            if self.passed_path <= self.start_x:
                self.vel_publish(0, 0)
                self.state = 3
                rospy.sleep(1)
            else:
                self.vel_publish(0.2, 0)

        elif self.state == 3:  # Разворот в исходную позицию
            angle_diff = abs(self.angle - self.turn_start_angle)
            if angle_diff <= 0.05:
                self.vel_publish(0, 0)
                self.state = 4
            else:
                self.vel_publish(0, 0.5)

        elif self.state == 4:  # Конец — остановка
            self.vel_publish(0, 0)

rb = RobotMover()

while not rospy.is_shutdown():
    rospy.sleep(0.1)
    rb.move()
PreviousПрактические заданияNextРабота с Arduino

Last updated 2 days ago

Was this helpful?