Программа движения робота

Мы разобрались, как управлять роботом и получать данные о его положении в симуляторе turtlesim. Давайте рассмотрим программу, которая заставит робота проехать 3 метра и остановиться. На базе этой программы вам будет необходимо выполнить зачетное домашнее задание.

import rospy, math

from turtlesim.msg import Pose
from geometry_msgs.msg import Twist

class Turtle():

    def __init__(self):

        rospy.on_shutdown(self.shutdown)
        self.pub = rospy.Publisher('/turtle1/cmd_vel', Twist, queue_size=10)
        rospy.Subscriber("turtle1/pose", Pose, self.pose_callback)
        self.pose = None

    def shutdown(self):
        self.pub.publish(Twist())    

    def pose_callback(self, pose):
        self.pose = pose        

    def wait_turtle(self):
        while self.pose is None:
            rospy.loginfo('Wait turtle')
            rospy.sleep(0.1)



    def move_forward(self, dist):

        start_pose = self.pose
        move_dist = 0
        cmd_vel = Twist()

        while not rospy.is_shutdown():

            move_dist = self.getMoveDist(start_pose, self.pose)

            if (move_dist < dist):                    
                cmd_vel.linear.x = 1
                self.pub.publish(cmd_vel)
            else:
                cmd_vel.linear.x = 0
                self.pub.publish(cmd_vel)
                return  

            rospy.sleep(0.2)


    def getMoveDist(self, start_pose, current_pose):

        move_dist = math.sqrt(math.pow(start_pose.x - current_pose.x,2) + math.pow(start_pose.y - current_pose.y,2))
        rospy.loginfo("Dist :%s", move_dist)
        return move_dist


rospy.init_node('move_forward')
rospy.loginfo("Start Node")

turtle = Turtle()   
turtle.wait_turtle()
turtle.move_forward(3)

Программа выглядит уже сложнее и больше, чем мы видели раньше. Но такая программа уже больше похоже на те, какие реально работают на роботах.

Алгоритм программы по сути довольно простой. Если у нас есть координаты, то зная текущую и стартовую координаты, мы можем вычислить расстояние, которое проехала черепаха. Если расстояние меньше требуемого, то продолжать ехать. Если больше, то остановиться и выйти. Но есть нюансы реализации.

Конструктор класса - создание Издателя для управления перемещением робота и подписка на данные Одометрии.

rospy.on_shutdown(self.shutdown)
self.pub = rospy.Publisher('/turtle1/cmd_vel', Twist, queue_size=10)
rospy.Subscriber("turtle1/pose", Pose, self.pose_callback)
self.pose = None

Новый для нас метод. Он нам нужен для правильного завершения программы. Мы говорим, что если мы решили отключиться от ROS, то нужно вызвать метод класса shutdown

rospy.on_shutdown(self.shutdown)

Вызвать такой метод необходимо, чтобы остановить робота. Иначе программа завершится, а робот может продолжить свое движение, это будет неожидаемым поведением.

Также мы инициализировали переменную, в которой собираемся хранить текущее положение робота.

self.pose = None

Функция "отключения" робота. Мы создаем пустой объект Twist. И отправляем его через Издателя. Пустой Twist содержит все нулевые скорости, поэтому робот остановится.

def shutdown(self):
    self.pub.publish(Twist())

Функция обратного вызова, мы просто сохраним положение в атрибуте класса pose, потом мы будем использовать эту переменную для расчета пройденного расстояния.

def pose_callback(self, pose):
    self.pose = pose

Метод, который позволит нашей программе работать правильно.

def wait_turtle(self):
    while self.pose is None:
        rospy.loginfo('Wait turtle')
        rospy.sleep(0.1)

По ходу выполнения нашей программы нам важно, чтобы в самом начале работы мы знали положение робота. Но у нас может быть ситуация, что программа уже запустилась и готова рассчитать расстояние, а мы еще не получили "первый" callback с данными положения робота.

Поэтому нам необходима функция, которая позволит работать программе только после того, как мы получили первые данные о положении робота.

Сама главная функция "ехать вперед на расстояние dist"

def move_forward(self, dist):

    start_pose = self.pose
    move_dist = 0
    cmd_vel = Twist()

    while not rospy.is_shutdown():

        move_dist = self.getMoveDist(start_pose, self.pose)

        if (move_dist < dist):                    
            cmd_vel.linear.x = 1
            self.pub.publish(cmd_vel)
        else:
            cmd_vel.linear.x = 0
            self.pub.publish(cmd_vel)
            return  

        rospy.sleep(0.2)

Логика работы этой функции описана в самом алгоритме программы. Мы сохраняем точку старту, и раз в rospy.sleep(0.2) в цикле рассчитываем расстояние self.getMoveDist и принимаем решение, что делать: ехать дальше или выходить из метода.

# Установить линейную скорость по оси робота X 1 м/с
cmd_vel.linear.x = 1
# Обнулит скорость
cmd_vel.linear.x = 0

Функция getMoveDist производит расчет пройденного расстояния робота по теореме Пифагора. На вход мы передаем стартовую и текущую координату, а функция выдает расстояние между двумя этими точками.

    def getMoveDist(self, start_pose, current_pose):

        move_dist = math.sqrt(math.pow(start_pose.x - current_pose.x,2) + math.pow(start_pose.y - current_pose.y,2))
        rospy.loginfo("Dist :%s", move_dist)
        return move_dist

Ну и запуск нашей программы

turtle = Turtle()   
turtle.wait_turtle()
turtle.move_forward(3)

Мы создаем объект turtle. Ожидаем получения первых данных о положении робота, едем на 3 метра.