Вот полный скрипт для тайм-аута и обратного узла. После листинга мы будем разбивать его на более мелкие части.
Ссылка на источник: 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, который запускает скрипт (и робота).