Решение
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?