7.6.3. Скрипт с выдержкой времени и обратно

Вот полный скрипт для тайм-аута и обратного узла. После листинга мы будем разбивать его на более мелкие части.

Ссылка на источник: timed_out_and_back.py​

#!/usr/bin/env pythonimport rospyfrom geometry_msgs.msg import Twist from math import piclass OutAndBack(): def __init__(self):        # Give the node a namerospy.init_node('out_and_back', anonymous=False)# Set rospy to execute a shutdown function when exiting rospy.on_shutdown(self.shutdown)  # Publisher to control the robot's speed self.cmd_vel = rospy.Publisher('/cmd_vel', Twist, queue_size=5)​ # How fast will we update the robot's movement? rate = 50​ # Set the equivalent ROS rate variable r = rospy.Rate(rate)​ # Set the forward linear speed to 0.2 meters per second linear_speed = 0.2​ # Set the travel distance to 1.0 meters goal_distance = 1.0​ # How long should it take us to get there? linear_duration = goal_distance / linear_speed​ # Set the rotation speed to 1.0 radians per second angular_speed = 1.0​ # Set the rotation angle to Pi radians (180 degrees) goal_angle = pi​ # How long should it take to rotate? angular_duration = goal_angle / angular_speed​ # Loop through the two legs of the trip for i in range(2): # Initialize the movement command move_cmd = Twist()​ # Set the forward speed move_cmd.linear.x = linear_speed​ # Move forward for a time to go 1 meter ticks = int(linear_duration * rate)​ for t in range(ticks): self.cmd_vel.publish(move_cmd) r.sleep()​ # Stop the robot before the rotation move_cmd = Twist() self.cmd_vel.publish(move_cmd) rospy.sleep(1)​ # Now rotate left roughly 180 degrees 63 # Set the angular speed move_cmd.angular.z = angular_speed​ # Rotate for a time to go 180 degrees ticks = int(goal_angle * rate)for t in range(ticks): self.cmd_vel.publish(move_cmd) r.sleep()            # Stop the robot before the next legmove_cmd = Twist() self.cmd_vel.publish(move_cmd) rospy.sleep(1)        # Stop the robotself.cmd_vel.publish(Twist())def shutdown(self):# Always stop the robot when shutting down the node. rospy.loginfo("Stopping the robot...") self.cmd_vel.publish(Twist())rospy.sleep(1)if __name__ == '__main__': try:        OutAndBack()    except:rospy.loginfo("Out-and-Back node terminated.")

Так как это наш первый скрипт, давайте посмотрим на него построчно, начиная сверху:

#!/usr/bin/env python import rospy

Если вы делали Учебники для начинающих ROS в Python, вы уже знаете, что все наши узлы ROS начинаются с этих двух строк. Первая строка гарантирует, что программа будет работать как скрипт Python, а вторая импортирует основную библиотеку ROS для Python.

from geometry_msgs.msg import Twistfrom math import pi

Здесь мы позаботимся о любом другом импорте, который нам нужен для скрипта. В этом случае нам понадобится тип сообщения Twist из пакета ROS geometry_msgs и константа pi из математического модуля Python. Обратите внимание, что общий источник ошибок при импорте - забыть включить необходимую строку ROS в файл package.xml вашего пакета. В этом случае наш файл package.xml должен содержать строку:

<run_depend>geometry_msgs</run_depend>

так что мы можем импортировать Twist из geometry_msgs.msg.

class OutAndBack():def __init__(self):

Здесь мы начинаем основную часть нашего узла ROS, определяя его как класс Python вместе со стандартной линией инициализации класса.

# Give the node a namerospy.init_node('out_and_back', anonymous=False)# Set rospy to execute a shutdown function when exitingrospy.on_shutdown(self.shutdown)

Каждый узел ROS требует вызова rospy.init_node (), и мы также устанавливаем обратный вызов для функции on_shutdown (), чтобы мы могли выполнить любую необходимую очистку, когда сценарий завершается, например когда пользователь нажимает Ctrl-C. В случае мобильного робота самая важная задача очистки - остановить робота! Мы увидим, как это сделать позже в скрипте.

# Publisher to control the robot's speedself.cmd_vel = rospy.Publisher('/cmd_vel', Twist, queue_size=5)# How fast will we update the robot's movement?rate = 50# Set the equivalent ROS rate variabler = rospy.Rate(rate)

Здесь мы определяем нашего издателя ROS для отправки команд Twist в раздел / cmd_vel. Мы также устанавливаем скорость, с которой мы хотим обновить движение робота, 50 раз в секунду, а параметр queue_size равным 5.

Параметр queue_size был введен в ROS Hydro и будет обязательным в ROS Jade. В ROS Indigo ваш код выдаст предупреждение, если издатель инициализируется без этого параметра. Вы можете узнать больше о параметре queue_size в ROS Wiki. Самый важный факт, который нужно иметь в виду, это то, что если этот параметр не указан или не установлен в значение «Нет», издатель будет вести себя синхронно. Это означает, что если имеется несколько подписчиков на тему издателя, и один из подписчиков зависает - например, он находится на другом конце ненадежного беспроводного соединения - тогда публикация будет блокировать всех подписчиков этой темы, пока подписчик не зависнет возвращается к жизни. Как правило, это нежелательный результат. Установка для параметра queue_size числового значения приводит к тому, что издатель ведет себя асинхронно, так что каждый подписчик получает сообщения в отдельном потоке без подписчика и блокирует всю систему.

# Set the forward linear speed to 0.2 meters per secondlinear_speed = 0.2# Set the travel distance to 1.0 metersgoal_distance = 1.0# How long should it take us to get there?linear_duration = linear_distance / linear_speed

Мы инициализируем скорость движения вперед относительно безопасную 0,2 метра в секунду и целевое расстояние до 1,0 метра. Затем мы вычисляем, сколько времени это займет.

# Set the rotation speed to 1.0 radians per secondangular_speed = 1.0# Set the rotation angle to Pi radians (180 degrees)goal_angle = pi# How long should it take to rotate?angular_duration = angular_distance / angular_speed

Аналогично, мы устанавливаем скорость вращения 1,0 радиан в секунду, а целевой угол - 180 градусов или Pi радиан.

# Loop through the two legs of the tripfor i in range(2):# Initialize the movement commandmove_cmd = Twist()# Set the forward speedmove_cmd.linear.x = linear_speed# Move forward for a time to go the desired distanceticks = int(linear_duration * rate)for t in range(ticks):self.cmd_vel.publish(move_cmd)r.sleep()

Это петля, которая фактически перемещает робота - один цикл для каждой из двух ног. Напомним, что некоторые роботы требуют, чтобы сообщение Twist постоянно публиковалось, чтобы оно продолжало двигаться. Поэтому, чтобы продвинуть робота вперед метры linear_distance со скоростью метра linear_speed в секунду, мы публикуем сообщение move_cmd каждые 1 раз в секунду в течение соответствующей продолжительности. Выражение r.sleep () является сокращением для rospy.sleep (1 / скорость), так как мы определили переменную r = rospy.Rate (скорость).

# Now rotate left roughly 180 degrees 63# Set the angular speedmove_cmd.angular.z = angular_speed# Rotate for a time to go 180 degreesticks = int(goal_angle * rate)for t in range(ticks):self.cmd_vel.publish(move_cmd)r.sleep()

Во второй части цикла мы вращаем робота со скоростью angular_speed радиан в секунду в течение соответствующей продолжительности (в данном случае Pi секунд), которая должна дать 180 градусов.

# Stop the robot.self.cmd_vel.publish(Twist())

Когда робот завершил обратный путь, мы останавливаем его, публикуя пустое сообщение Twist (все поля равны 0).

def shutdown(self):# Always stop the robot when shutting down the node.rospy.loginfo("Stopping the robot...")self.cmd_vel.publish(Twist())rospy.sleep(1)

Это наша функция обратного вызова выключения. Если выполнение сценария прекращается по какой-либо причине, мы останавливаем робота, публикуя пустое сообщение Twist.

if __name__ == '__main__': try:OutAndBack()except rospy.ROSInterruptException:rospy.loginfo("Out-and-Back node terminated.")

Наконец, это стандартный блок Python для запуска скрипта. Мы просто создаем экземпляр класса OutAndBack, который запускает скрипт (и робота).