Партнерка на США и Канаду по недвижимости, выплаты в крипто
- 30% recurring commission
- Выплаты в USDT
- Вывод каждую неделю
- Комиссия до 5 лет за каждого referral
Вводное руководство по работе с Gazebo
Версия 1.0 от 01.01.2001
Москва 2015
Оглавление
Введение. 2
Работа с Gazebo. 2
Gazebo и ROS. 2
Установка и настройка. 3
ROS. 3
Gazebo. 3
Создание робота. 4
Описание модели робота. 4
Введение в sfd формат. 5
Создание sdf файла модели робота с помощью утилиты xacro. 7
Создание launch файла. 13
Добавление камеры.. 13
Добавление двигателя. 16
Написание плагина. 18
Вращение камерой. 20
Список литературы.. 22
Ссылки. 22
Введение
Gazebo - это программный пакет, моделирующий взаимодействие робота или даже популяции роботов с физическим миром. Детально описав робота можно тестировать как работу алгоритмов, так и физическую реализацию робота в виртуальной среде, до того, как собирать его в железе. Физический движок использующийся в Gazebo позволяет учитывать такие тонкости взаимодействия робота со средой как трение, мощность моторов, имитация сенсоров и камеры и т. д.
Работа с Gazebo
Сначала в xml-формате создается модель робота, каждая функциональная часть (база, колеса, камера и т. д.) описывается отдельно, затем эти части соединяются вместе в модель робота.
К таким частям работа, как камеры, моторы, сенсоры и т. д. необходимо подключать плагины, которые пишутся на C++ и описывают функциональность этих частей. Например, простейший плагин на мотор прикладывает момент силы к некоторой оси при получении соответствующей команды (например, ROS-сообщения).
Можно подключать плагины не только непосредственно к модели и сенсорам, но и к миру целиком.
Gazebo и ROS
Gazebo не обязателен ROS для функционирования, в нем успешно можно работать вообще без ROS'а. Однако это менее удобно. Именно наличие некоторого количества уже написанных плагинов взаимодействующих методами ROS (сообщения, сервисы, параметры) и интегрируют Gazebo в ROS. ROS Indigo работает с Gazebo 2.2, ROS Jade работает с Gazebo 5.0.
Установка и настройка
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
На этом первоначальную настройку можно считать законченной. Далее создадим модель робота.
Создание робота
На примере создания небольшой рабочей модели робота познакомимся с основными моментами 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>*****@***email</email>
</author>
<description>
My awesome robot.
</description>
</model>
В этом файле самое главное указать путь до sdf файла модели.
Введение в 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 для определения позиции относительно начала координат модели, этот тэг используется очень часто и определяет позицию относительно родительского тэга.
• 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 позволяет импортировать один 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>
<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>
</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 и посмотрим, что получилось.

Добавление колес
Так как колеса отдельные физические объекты в нашем роботе, их надо добавлять в свои линки. Добавим для этого макросы в файл 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>
Описываем визуальную и инерциальную часть:
<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 определяет возникающее сопротивление пропорционально скорости. Варьированием этого параметра и момента силы которая будет прикладываться по оси задается максимальная скорость вращения сочленения и его динамика. Подробнее тут (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>
<!-- 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>
<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,
• 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, добавляем нашего робота, и ставим напротив него какой-нибудь объект, примерно вот так.

Теперь, если открыть утилиту rqt в консоле, и выбрать там image_view, то можно увидеть видеоизображение нашего объекта. Мы взаимодействуем с нашим роботом из ROS'а.

Если вам интересна документация по имеющимся плагинам, вы можете изучить исходники вот тут
(https:///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>
#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. Более подробно в комментариях в коде.
Теперь создадим файл 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 = bscribe(_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;
}
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. /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-сообщение. Для проверки отправим из терминала сообщение.
$ rostopic pub -1 /camera_pos_plugin/set_camera_angle std_msgs/Float32 "data: 0.2"
Увидим, что камера действительно повернулась:

В этом (https:///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. /en/content/75-gazebo-and-ros.
[GazeboSim] http://gazebosim. org/tutorials
[osrf-distibutions] https://osrfdistributions. /gazebo/api/dev
[ROS] http://wiki. ros. org/ROS/Tutorials
[Gazebo-ros-pkg] https:///ros-simulation/gazebo_ros_pkgs/tree/jade-devel/gazebo_plugins/src
[SDFormat] http://sdformat. org/spec


