Сурцуков М.М. Вводное руководство по работе с Gazebo , М

М.М. Сурцуков
Вводное руководство по работе с Gazebo
Версия 1.0 от 20.11.2015
Москва 2015
1
Оглавление
Введение ........................................................................................................................ 2
Работа с Gazebo ......................................................................................................... 2
Gazebo и ROS ............................................................................................................ 2
Установка и настройка.................................................................................................. 3
ROS ............................................................................................................................ 3
Gazebo ........................................................................................................................ 3
Создание робота ............................................................................................................ 4
Описание модели робота .......................................................................................... 4
Введение в sfd формат .............................................................................................. 5
Создание sdf файла модели робота с помощью утилиты xacro ............................ 6
Создание launch файла ........................................................................................... 12
Добавление камеры ..................................................................................................... 13
Добавление двигателя ............................................................................................ 17
Написание плагина ...................................................................................................... 17
Вращение камерой .................................................................................................. 20
Список литературы ..................................................................................................... 21
Ссылки ......................................................................................................................... 21
Введение
Gazebo - это программный пакет, моделирующий взаимодействие робота или
даже популяции роботов с физическим миром. Детально описав робота можно
тестировать как работу алгоритмов, так и физическую реализацию робота в
виртуальной среде, до того, как собирать его в железе. Физический движок
использующийся в Gazebo позволяет учитывать такие тонкости взаимодействия
робота со средой как трение, мощность моторов, имитация сенсоров и камеры и т.д.
Работа с Gazebo
Сначала в xml-формате создается модель робота, каждая функциональная часть
(база, колеса, камера и т.д.) описывается отдельно, затем эти части соединяются
вместе в модель робота.
К таким частям работа, как камеры, моторы, сенсоры и т.д. необходимо подключать
плагины, которые пишутся на C++ и описывают функциональность этих частей.
Например, простейший плагин на мотор прикладывает момент силы к некоторой оси
при получении соответствующей команды (например, ROS-сообщения).
Можно подключать плагины не только непосредственно к модели и сенсорам, но и к
миру целиком.
Gazebo и ROS
Gazebo не обязателен ROS для функционирования, в нем успешно можно
работать вообще без ROS'а. Однако это менее удобно. Именно наличие некоторого
количества уже написанных плагинов взаимодействующих методами ROS
(сообщения, сервисы, параметры) и интегрируют Gazebo в ROS. ROS Indigo работает с
Gazebo 2.2, ROS Jade работает с Gazebo 5.0.
2
Установка и настройка
ROS
Если у вас настроена полная версия ROS, то пропускаем этот шаг.
• Устанавливаем
ROS-indigo-desktop-full,
как
написано
здесь
(http://wiki.ros.org/indigo/Installation/Ubuntu)
• Настраиваем
рабочую
папку
catkin,
как
написано
здесь
(http://wiki.ros.org/ROS/Tutorials/InstallingandConfiguringROSEnvironment)
• Если вы не знакомы с ROS, то желательно сначала пройти уроки для
начинающих здесь (http://wiki.ros.org/ROS/Tutorials)
Gazebo
Gazebo установился вместе с ROS'ом на предыдущем шаге. Убедитесь, что он
работает, введя команду в терминал:
$roslaunch gazebo_ros empty_world.launch
Вы должны увидеть, как запускается окно Gazebo.
Запуск через roslaunch запускает как ядро ROS roscore, так и пару клиент-сервер
Gazebo. Обновите путь, по которому Gazebo ищет модели:
$ mkdir ~/catkin_ws/src/models
$ echo “export GAZEBO_MODEL_PATH=~/catkin_ws/src/models:$GAZEBO_MODEL_PATH”
>> ~/.bashrc
3
На этом первоначальную настройку можно считать законченной. Далее создадим
модель робота.
Создание робота
На примере создания небольшой рабочей модели робота познакомимся с
основными моментами Gazebo. Начнем с создания структуры файлов и пакетов для
нашего робота в рабочей папке. В терминале:
$ mkdir ~/catkin_ws/src/mobot
$ cd ~/catkin_ws/src/mobot
$ catkin_create_pkg mobot_gazebo gazebo_ros
$ mkdir mobot_gazebo/launch mobot_gazebo/worlds
$ cd ../models
$ catkin_create_pkg mobot_description xacro
$ mkdir mobot_description/sdf
В папке src/mobot будут находится пакеты связанные с роботом, включая
mobot_gazebo,
который
интегрирует
робота
в
Gazebo,
в
папке
src/models/mobot_description будет находится описание модели робота.
Описание модели робота
3-х мерная физическая модель робота в Gazebo описывается с помощью файла
формата sdf, близкого родственника urdf, который обычно используется в ROS’е. В
сети множество туториалов как использовать urdf в Gazebo, однако все они основаны
на забагованной стандартной утилите gzsdf, которая преобразует один формат в
другой. На момент написания руководства данная утилита находится в нерабочем
состоянии, во всяком случае ее версия для ROS Indigo и Gazebo 2.2 и вряд ли для
данной версии Gazebo будет исправлена в будущем. Пользоваться ей не
рекомендуется, если urdf файл все же необходим, то проще написать его отдельно.
Создадим
содержанием:
файл
model.config
в
папке
mobot_description
<?xml version="1.0"?>
<model>
<name>Mobot</name>
<version>1.0</version>
<sdf version='1.4'>sdf/model.sdf</sdf>
<author>
<name>My Name</name>
<email>me@my.email</email>
</author>
<description>
My awesome robot.
</description>
</model>
В этом файле самое главное указать путь до sdf файла модели.
4
со
следующим
Введение в sfd формат
Создадим файл sfd/model.sdf и добавим в него содержимое:
<?xml version='1.0'?>
<sdf version='1.4'>
<model name="mobot" xmlns:xacro="http://www.ros.org/wiki/xacro">
</model>
</sdf>
Описание самой модели прописывается между тэгами model, добавим между тэгами
следующий код:
<link name='chassis'>
<pose>0 0 0.1 0 0 0</pose>
<collision name='collision'>
<geometry>
<box>
<size>0.5 0.2 0.1</size>
</box>
</geometry>
</collision>
<visual name='visual'>
<geometry>
<box>
<size>0.5 0.2 0.1</size>
</box>
</geometry>
<material>
<script>
<name>Gazebo/Orange</name>
<uri>__default__</uri>
</script>
</material>
</visual>
<inertial>
<mass>5</mass>
<inertia>
<ixx>0.2</ixx>
<iyy>0.3</iyy>
<izz>0.2</izz>
<ixy>0</ixy>
<ixz>0</ixz>
<iyz>0</iyz>
</inertia>
</inertial>
</link>
Тэг link определяет базовый структурный элемент, их может быть несколько в одной
моделе. Внутри link нужно определить тэги:
• pose в формате xyz rpy для определения позиции относительно начала
координат модели, этот тэг используется очень часто и определяет позицию
5
относительно родительского тэга.
collision (их может быть много внутри однога линка) используется для
просчета столкновений, в нем можно использовать как стандартные
примитивы, так и импортировать модель форматов .dae или .stl. Здесь же
описываются свойства поверхности вроде трения. Добавив сюда pose можно
сдвинуть мэш относительно родительского линка
• visual (их так же может быть много) добавляется для рендеринга, здесь также
можно использовать как стандартные примитивы, так и импортировать
модель форматов .dae или .stl. Здесь же добавляются текстуры. Добавив
сюда pose можно сдвинуть мэш относительно родительского линка
• inertial (он в линке один) описывает физические инерциальные свойства
линка: его массу, тензор инерции и т.д. Добавив сюда pose можно сдвинуть
координаты центра масс относительно родительского линка
Тэги link, collision, visual обязательно должны быть снабжены уникальными
именами. Подробнее про спецификации sdf формата написано здесь
(http://sdformat.org/spec) (в нашей версии Gazebo нужно выбрать 1.4 версию sdf).
•
Запустим Gazebo командой:
$ roslaunch gazebo_ros empty_world.launch
Выберем вкладку insert и добавим оттуда модель Mobot. Должно появиться что-то
такое:
Создание sdf файла модели робота с помощью
утилиты xacro
Теперь, когда структура sdf файла стала примерно понятна, продолжим её
изучение, попутно познакомившись с утилитой xacro.
Название xacro происходит от Xml mACROs. Это простая утилита, однако, она
существенно помогает в создании сложных xml файлов в том числе sdf и urdf. Xacro
6
позволяет импортировать один sdf файл в другой, определять переменные, создавать
макросы, производить не сложные арифметические вычисления и т.д.
Удалим файл model.sdf и создадим в папке sdf файл с названием model.sdf.xacro,
добавим в него следующее содержание:
<?xml version='1.0'?>
<sdf version='1.4'>
<model name="mobot" xmlns:xacro="http://www.ros.org/wiki/xacro">
Далее добавим определения переменных:
<xacro:property name="PI" value="3.1415926535897931"/>
<xacro:property name="chassisHeight" value="0.1"/>
<xacro:property name="chassisLength" value="0.4"/>
<xacro:property name="chassisWidth" value="0.2"/>
<xacro:property name="chassisMass" value="50"/>
<xacro:property name="casterRadius" value="0.05"/>
<xacro:property name="casterMass" value="5"/>
<xacro:property name="wheelWidth" value="0.05"/>
<xacro:property name="wheelRadius" value="0.1"/>
<xacro:property name="wheelPos" value="0.12"/>
<xacro:property name="wheelMass" value="5"/>
<xacro:property name="wheelDamping" value="1"/>
<xacro:property name="cameraSize" value="0.05"/>
<xacro:property name="cameraMass" value="0.1"/>
Импортируем файл с макросами, который мы напишем позже, обратите внимание на
$(find mobot_description). Оно выполняют функцию поиска адреса пакета.
<xacro:include filename="$(find mobot_description)/sdf/macros.xacro" />
Ниже добавим описание линка базы робота, тэг static указывает на то, что модель
пока статична, это полезно на момент формирования модели
<static>True</static>
Обратите внимание, что в тексте ниже несколько visual и collision в одном линке, так
же в collision кастера изменены свойства поверхности, а именно добавлено
проскальзывание и убрано трение. Кастер - это сфера без трения, которая добавлена,
чтобы роботу хватило только двух колес.
<link name='chassis'>
<pose>0 0 ${wheelRadius} 0 0 0</pose>
<collision name='collision'>
<geometry>
<box>
<size>${chassisLength} ${chassisWidth} ${chassisHeight}</size>
</box>
</geometry>
</collision>
7
<visual name='visual'>
<geometry>
<box>
<size>${chassisLength} ${chassisWidth} ${chassisHeight}</size>
</box>
</geometry>
<material>
<script>
<name>Gazebo/Orange</name>
<uri>__default__</uri>
</script>
</material>
</visual>
<inertial>
<mass>${chassisMass}</mass>
<box_inertia m="${chassisMass}" x="${chassisLength}"
y="${chassisWidth}" z="${chassisHeight}"/>
</inertial>
<collision name='caster_collision'>
<pose>${-chassisLength/3} 0 ${-chassisHeight/2} 0 0 0</pose>
<geometry>
<sphere>
<radius>${casterRadius}</radius>
</sphere>
</geometry>
<surface>
<friction>
<ode>
<mu>0</mu>
<mu2>0</mu2>
<slip1>1.0</slip1>
<slip2>1.0</slip2>
</ode>
</friction>
</surface>
</collision>
<visual name='caster_visual'>
<pose>${-chassisLength/3} 0 ${-chassisHeight/2} 0 0 0</pose>
<geometry>
<sphere>
<radius>${casterRadius}</radius>
</sphere>
</geometry>
<material>
<script>
<name>Gazebo/Red</name>
<uri>__default__</uri>
</script>
8
</material>
</visual>
</link>
</model>
</sdf>
Теперь создадим там же файл model.xacro и определим в нем несколько макросов,
синтаксис говорит сам за себя.
<?xml version='1.0'?>
<model>
<macro name="cylinder_inertia" params="m r h">
<inertia>
<ixx>${m*(3*r*r+h*h)/12}</ixx>
<iyy>${m*(3*r*r+h*h)/12}</iyy>
<izz>${m*r*r/2}</izz>
<ixy>0</ixy>
<ixz>0</ixz>
<iyz>0</iyz>
</inertia>
</macro>
<macro name="box_inertia" params="m x y z">
<inertia>
<ixx>${m*(y*y+z*z)/12}</ixx>
<iyy>${m*(x*x+z*z)/12}</iyy>
<izz>${m*(x*x+y*y)/12}</izz>
<ixy>0</ixy>
<ixz>0</ixz>
<iyz>0</iyz>
</inertia>
</macro>
<macro name="sphere_inertia" params="m r">
<inertia>
<ixx>${2*m*r*r/5}</ixx>
<iyy>${2*m*r*r/5}</iyy>
<izz>${2*m*r*r/5}</izz>
<ixy>0</ixy>
<ixz>0</ixz>
<iyz>0</iyz>
</inertia>
</macro>
</model>
Далее с помощью xacro создаем sdf файл:
$ rosrun xacro xacro -o model.sdf model.sdf.xacro
Запустим Gazebo и посмотрим, что получилось.
9
Добавление колес
Так как колеса отдельные физические объекты в нашем роботе, их надо
добавлять в свои линки. Добавим для этого макросы в файл macros.xacro:
<macro name="wheel" params="lr tY">
<link name="${lr}_wheel">
<pose>${wheelPos} ${tY*wheelWidth/2+tY*chassisWidth/2} ${wheelRadius} 0 ${PI/2}
${PI/2}</pose>
<collision name="${lr}_wheel_collision">
<geometry>
<cylinder>
<radius>${wheelRadius}</radius>
<length>${wheelWidth}</length>
</cylinder>
</geometry>
Обратите внимание на параметры. "lr" следует задать "left" или "rigth", а tY смещение по оси Y, 1 или -1 в зависимости от колеса. Зададим колесам трение:
<surface>
<friction>
<ode>
<mu>1.0</mu>
<mu2>1.0</mu2>
<slip1>0.0</slip1>
<slip2>0.0</slip2>
</ode>
</friction>
</surface>
</collision>
10
Описываем визуальную и инерциальную часть:
<visual name="${lr}_wheel_visual">
<geometry>
<cylinder>
<radius>${wheelRadius}</radius>
<length>${wheelWidth}</length>
</cylinder>
</geometry>
<material>
<script>
<name>Gazebo/Black</name>
<uri>__default__</uri>
</script>
</material>
</visual>
<inertial>
<mass>${wheelMass}</mass>
<cylinder_inertia m="${wheelMass}" r="${wheelRadius}" h="${wheelWidth}"/>
</inertial>
</link>
Мы задали колеса, однако теперь их надо соединить с базой:
<joint name="${lr}_wheel_hinge" type="revolute">
<parent>chassis</parent>
<child>${lr}_wheel</child>
<pose>0 0 ${wheelWidth/2} 0 0 0</pose>
<axis>
<xyz>0 1 0</xyz>
<dynamics>
<damping>${wheelDamping}</damping>
</dynamics>
<limit>
<effort>100</effort>
<velocity>100</velocity>
</limit>
</axis>
</joint>
</macro>
Тэг joint используется, чтобы соединять линки между собой. Ему нужно задать
уникальное имя и тип. Наиболее часто испольюзуются типы revolute и continuous.
child крепится к 'parent'
• xyz определяет по какой оси будет вращение,
• limit задает ограничение на сочленение:
• effort - максимальные моменты сил Н*м которые можно задать по оси,
• velocity - максимальную скорость
• есть также upper и lower, они определят максимальный и минимальный угол
поворота
• damping определяет возникающее сопротивление пропорционально
11
скорости. Варьированием этого параметра и момента силы которая будет
прикладываться по оси задается максимальная скорость вращения
сочленения
и
его
динамика.
Подробнее
тут
(http://sdformat.org/spec?ver=1.4&elem=joint#joint_type).
•
Теперь добавим эти колеса в основной файл сразу после "chassis" линка:
<wheel lr="left" tY="1"/>
<wheel lr="right" tY="-1"/>
Снова вызываем утилиту xacro - не забываем это делать каждый раз, когда меняем
файлы xacro.
$ rosrun xacro xacro -o model.sdf model.sdf.xacro
Импортируем модель в Gazebo и смотрим, что получилось:
Создание launch файла
Для того чтобы не приходилось каждый раз вручную добавлять робота, напишем
launch файл. В пакете mobot_gazebo создадим файл launch/mobot.launch и добавим в
него:
<launch>
<include file="$(find gazebo_ros)/launch/empty_world.launch">
</include>
12
<!-- urdf xml robot description loaded on the Parameter Server, converting the xacro into a
proper sdf/urdf file-->
<param name="robot_description" command="$(find xacro)/xacro.py '$(find
mobot_description)/sdf/model.sdf.xacro'" />
<!-- push robot_description to factory and spawn robot in gazebo -->
<node name="mobot_spawn" pkg="gazebo_ros" type="spawn_model" output="screen"
args="-sdf -param robot_description -model mobot" />
</launch>
Здесь мы сначала запускаем пустой мир, как мы делали обычно. Потом в ROSпараметр записываем созданный с помощью xacro sdf текст. (здесь мы не обновяем
model.sdf, а сразу записываем в параметр, если добавлять через include, то все же надо
запускать в терминале xacro, как мы делали это раньше). Далее запускаем утилиту
spawn_model, которой передаем параметр, содержащий sdf текст нашей модели, и она
его добавляет в мир.
Теперь создадим в папке worlds файл mobot.world:
<?xml version="1.0" ?>
<sdf version="1.4">
<world name="mobot">
<include>
<uri>model://ground_plane</uri>
</include>
<include>
<uri>model://sun</uri>
</include>
</world>
</sdf>
Внутрь тэгов include можно так же добавить тэг pose чтобы задать положение, сюда
можно добавить все необходимые объекты, чтобы каждый раз не добавлять их
вручную.
Теперь запустить Gazebo с роботом можно так:
$ roslaunch mobot_gazebo mobot.launch
В следующей части добавим камеру и нужные плагины в модель, чтобы
взаимодействовать с ROS.
Добавление камеры
Сначала добавим в робота условную физическую модель камеры, для этого в
файл model.sdf.xacro вставим следующий код:
<link name="camera_link">
<pose>${chassisLength/2 - cameraSize/2} 0 ${wheelRadius +
chassisHeight/2 + cameraSize/2} 0 0 0</pose>
<inertial>
<mass>${cameraMass}</mass>
13
<box_inertia m="${cameraMass}" x="${cameraSize}" y="${cameraSize}"
z="${cameraSize}"/>
</inertial>
<collision name='camera_collision'>
<geometry>
<box>
<size>${cameraSize} ${cameraSize} ${cameraSize}</size>
</box>
</geometry>
</collision>
<visual name='camera_visual'>
<geometry>
<box>
<size>${cameraSize} ${cameraSize} ${cameraSize}</size>
</box>
</geometry>
<material>
<script>
<name>Gazebo/Red</name>
<uri>__default__</uri>
</script>
</material>
</visual>
В коде выше ничего нового. Ниже добавляем объект сенсор. Их много типов,
рассмотрим
на
примере
камеры.
Еще
примеры
(http://gazebosim.org/tutorials?tut=sensor_noise&cat=sensors).
<sensor name='camera1' type='camera'>
<update_rate>30</update_rate>
<camera name='head'>
<horizontal_fov>1.39626</horizontal_fov>
<image>
<width>800</width>
<height>800</height>
<format>R8G8B8</format>
</image>
<clip>
<near>0.02</near>
<far>300</far>
</clip>
<noise>
<type>gaussian</type>
<mean>0</mean>
<stddev>0.007</stddev>
</noise>
</camera>
Обсудим тэги:
• horizontal_fov задает широкоугольность камеры,
• clip - область рендеринга, то есть камера видит объекты от near до far,
14
• noise - добавляет шум.
Теперь добавим плагин, который подключит камеру к ROS.
<plugin name='camera_controller' filename='libgazebo_ros_camera.so'>
<alwaysOn>true</alwaysOn>
<updateRate>0.0</updateRate>
<cameraName>mobot/camera1</cameraName>
<imageTopicName>image_raw</imageTopicName>
<cameraInfoTopicName>camera_info</cameraInfoTopicName>
<frameName>camera_link</frameName>
<hackBaseline>0.07</hackBaseline>
<distortionK1>0.0</distortionK1>
<distortionK2>0.0</distortionK2>
<distortionK3>0.0</distortionK3>
<distortionT1>0.0</distortionT1>
<distortionT2>0.0</distortionT2>
</plugin>
</sensor>
</link>
Name - название плагина, а filename - имя скомпилированного файла плагина. Тэги
внутри plugin, это указания на входные параметры в исполняемый код плагина, будет
более понятно, когда мы напишем свой простенький плагин.
Теперь присоединим нашу камеру к базе робота:
<joint name="camera_joint" type="revolute">
<child>camera_link</child>
<parent>chassis</parent>
<axis>
<xyz>0 0 1</xyz>
<limit>
<upper>0</upper>
<lower>0</lower>
</limit>
</axis>
</joint>
Мы уже встречались с таким типом соединения. К сожалению, в sdf нету жесткого
соединения, обычно можно обойтись добавлением объектов прямо в нужный линк
напрямую. Когда же это все-таки нужно, то можно использовать соединение типа
revolute с нулевыми пределами.
Запускаем Gazebo, добавляем нашего робота, и ставим напротив него какой-нибудь
объект, примерно вот так.
15
Теперь, если открыть утилиту rqt в консоле, и выбрать там image_view, то можно
увидеть видеоизображение нашего объекта. Мы взаимодействуем с нашим роботом из
ROS'а.
Если вам интересна документация по имеющимся плагинам, вы можете изучить
исходники вот тут
16
(https://github.com/ros-simulation/gazebo_ros_pkgs/tree/jade-devel/gazebo_plugins/src),
на момент написания данного руководства официальная документация отсутствует.
Добавление двигателя
Изображение с камеры мы получили. Теперь добавим возможность ездить.
Подключим к модели плагин, вращающий колеса:
<plugin name="differential_drive_controller"
filename="libgazebo_ros_diff_drive.so">
<alwaysOn>true</alwaysOn>
<updateRate>100</updateRate>
<leftJoint>left_wheel_hinge</leftJoint>
<rightJoint>right_wheel_hinge</rightJoint>
<wheelSeparation>${chassisWidth+wheelWidth}</wheelSeparation>
<wheelDiameter>${2*wheelRadius}</wheelDiameter>
<torque>20</torque>
<commandTopic>mobot/cmd_vel</commandTopic>
<odometryTopic>mobot/odom_diffdrive</odometryTopic>
<odometryFrame>odom</odometryFrame>
<robotBaseFrame>chassis</robotBaseFrame>
</plugin>
Названия входных параметров говорят сами за себя. leftJoint и rightJoint это ссылки
на соединители колес с базой. torque - вращающий момент силы. commandTopic топик на который ему надо посылать команды и т.д.
Заново добавим нашу модель, предварительно удалив старую. В новом терминале
введем команду:
$ rosrun turtlesim turtle_teleop_key /turtle1/cmd_vel:=/mobot/cmd_vel
Это небольшая учебная утилита, которая отправляет geometry_msgs/Twist при нажатии
клавиш стрелок на клавиатуре, попробуйте! Особенно интересно управлять им, если
смотреть на изображение с камеры.
Написание плагина
В этой части мы напишем небольшой плагин на C++, который будет менять угол
поворота камеры. Для этого перейдем в папку src/mobot и создадим там новый пакет:
$ catkin_init_pkg mobot_plugins roscpp gazebo_ros
$ cd mobot_plugins
$ mkdir include src
Создадим файл include/camera_rotaion_plugin.h и добавим в него следующее
содержимое:
#include <string>
#include <boost/bind.hpp>
#include <gazebo/gazebo.hh>
#include <gazebo/physics/physics.hh>
#include <gazebo/common/common.hh>
17
#include <stdio.h>
#include <ros/ros.h>
#include <std_msgs/Float32.h>
namespace gazebo {
class CameraPos : public ModelPlugin {
public:
// Конструктор
CameraPos();
// Вызывается после констуктора, получает указатель на sdf описание модели, с
которой будет взаимодействовать
void Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf);
public:
// Вызывается во время обновления мира
void OnUpdate(const common::UpdateInfo & /*_info*/);
void setAngle(double angle);
// Вызывается, когда получено ROS сообщение
void callBack(const std_msgs::Float32::ConstPtr& msg);
private:
// Указатель на модель
physics::ModelPtr model;
// Указатель на сочленение
physics::JointPtr _joint;
// Указатель на sdf представление параметров
sdf::ElementPtr sdf;
// Указатель на соединение с движком Gazebo
event::ConnectionPtr updateConnection;
double _updateRate;
// Угол сочленения, который мы хотим установить
double _angle;
std::string _jointName, _topicName;
// NodeHandler и Subscriber для связи с ROS
ros::NodeHandle _nh;
ros::Subscriber sub;
};
}
Функция Load переопределяется, чтобы получить указатель на физическую модель
physics::ModelPtr model, с которой можно взаимодействовать, а так же чтобы
получить параметры sdf::ElementPtr sdf из sdf файла, из которого плагин
запускается. Функция onUpdate будет вызываться Gazebo пре пересчете параметров
мира с частотой _updateRate. Более подробно в комментариях в коде.
18
Теперь создадим файл src/camera_rotation_plugin.cpp с таким содержимым (не
забудьте поменять путь в своему .h файлу):
#include
"/home/USER/catkin_ws/src/mobot/mobot_plugins/include/camera_rotation_plugin.h"
namespace gazebo {
// Конструктор с инициализацией NodeHandler
CameraPos::CameraPos():_nh("camera_pos_plugin") {
_angle = 0.0;
};
void CameraPos::setAngle(double angle) {
ROS_INFO("was angle: %g", this->_angle);
ROS_INFO("setting angle: %g", angle);
this->_angle = angle;
}
// Вызывается, когда получено ROS сообщение.
void CameraPos::callBack(const std_msgs::Float32::ConstPtr& msg) {
this->setAngle(msg->data);
}
// Вызывается для загрузки плагина
void CameraPos::Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf) {
// Сохраним указатель на модель
this->model = _parent;
this->sdf = _sdf;
// Получим параметры из sdf кода
if(!this->sdf->HasElement("cameraJointName")) {
ROS_FATAL_STREAM("cameraJointName tag is not specified, quitting");
exit(1);
} else {
this->_jointName = this->sdf->Get<std::string>("cameraJointName");
ROS_INFO("inside");
this->_joint = this->model->GetJoint(this->_jointName);
}
if(!this->sdf->HasElement("topicName")) {
// По умолчанию
this->_topicName = "set_camera_angle";
} else {
this->_topicName = this->sdf->Get<std::string>("topicName");
}
// Создадим подписчика для связи с ROS
sub = _nh.subscribe(_topicName, 2, &CameraPos::callBack, this);
if(!this->sdf->HasElement("updateRate")) {
ROS_INFO("joint trajectory plugin missing <updateRate>, defaults"
" to 0.0 (as fast as possible)");
this->_updateRate = 0;
}
19
else
this->_updateRate = this->sdf->Get<double>("updateRate");
// Убедимся, что ROS уже был инициализирован
if (!ros::isInitialized()) {
ROS_FATAL_STREAM("A ROS node for Gazebo has not been initialized, unable to load
plugin. "
<< "Load the Gazebo system plugin 'libgazebo_ros_api_plugin.so' in the gazebo_ros
package)");
return;
}
// Слушаем событие обновления среды, оно вызывается каждую
// итерацию симуляции.
this->updateConnection = event::Events::ConnectWorldUpdateBegin(
boost::bind(&CameraPos::OnUpdate, this, _1));
}
// Вызывается при обновлении мира
void CameraPos::OnUpdate(const common::UpdateInfo & /*_info*/) {
// Поменяем угол сочленения
this->_joint->SetAngle(0, _angle);
}
// Регистрируем плагин в симуляторе
GZ_REGISTER_MODEL_PLUGIN(CameraPos)
}
Изучая код, обратите внимание на строчку:
this->_joint->SetAngle(0, _angle);
Взаимодействие с объектами обычно происходит с помощью подобных методов.
Поподробнее прочитать, что и как можно задать или считать можно вот здесь
(https://osrf-distributions.s3.amazonaws.com/gazebo/api/dev/classgazebo_1_1physics_1_1Joint.html).
Там же находится и остальное API взаимодействия Gazebo и C++. Однако за
актуальность не могу ручаться. Но это единственное место где я нашел описание API.
В принципе этот код может являться основой для написания своих плагинов.
Осталось только подключить этот плагин в sdf файле модели, откроем его и допишем
в конец за предыдущим плагином вот такой код:
<plugin name="camera_pos" filename="libmobot_plugins.so">
<cameraJointName>camera_joint</cameraJointName>
</plugin>
camera_joint это названия того самого сочленения, которым мы будем управлять.
Так как мы писали код на C++, то пакет надо откомпилировать с помощью
catkin_make.
Вращение камерой
Для того, что повернуть камеру теперь достаточно отправить ROS-сообщение.
Для проверки отправим из терминала сообщение.
20
$ rostopic pub -1 /camera_pos_plugin/set_camera_angle std_msgs/Float32 "data: 0.2"
Увидим, что камера действительно повернулась:
В этом (https://github.com/urtrial/gazebo_intro) репозитории вы можете скачать
папку src со всем кодом.
Список литературы
[Joseph, 2015] Joseph L., Learning Robotics Using Python, 2015.
[Fernandez, 2015] Fernandez E., Crespo L., Mahtani A., Martinez A., Learning ROS for
Robotics Programming, 2015.
Ссылки
[GenerationRobots] http://www.generationrobots.com/en/content/75-gazebo-and-ros.
[GazeboSim] http://gazebosim.org/tutorials
[osrf-distibutions] https://osrfdistributions.s3.amazonaws.com/gazebo/api/dev
[ROS] http://wiki.ros.org/ROS/Tutorials
[Gazebo-ros-pkg] https://github.com/ros-simulation/gazebo_ros_pkgs/tree/jadedevel/gazebo_plugins/src
[SDFormat] http://sdformat.org/spec
21