Решение

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()

Last updated

Was this helpful?