Робот-тележка на ROS. Часть 5. Работаем в rviz и gazebo: xacro, новые сенсоры / forpes.ru

Главная
Робот-тележка на ROS. Часть 5. Работаем в rviz и gazebo: xacro, новые сенсоры

Робот-тележка на ROS. Часть 5. Работаем в rviz и gazebo: xacro, новые сенсоры +8

27.09.2019 06:08

zoldaten 0 955 Источник

Продолжение цикла статей.

Предыдущие посты серии:

1. Часть 4
2. Часть 3
3. Часть 2
4. Часть 1

В прошлый раз удалось добиться следующих целей:

визуализировать робота, создав xacro-файл, содержащий urdf-описание робота;
создать два launch файла, один из которых позволяет разместить робота в редакторе-симуляторе Gazebo;
поуправлять роботом в симуляторе Gazebo с клавиатуры.

В этом посте оформим проект в виде модулей xacro, чтобы он стал более читаемым (ранее для наглядности мы затолкали все описание в один xacro файл). Добавим виртуальную видеокамеру и imu. Просмотрим как добавлять в gazebo посторонние предметы окружающего мира.

Для начала проверим, работает ли в терминале перемещение по ROS-системе с помощью команд roscd:

roscd rosbots_description/launch

Если не работает, то зайдем в папку с catkin_ws и выполним команду:

source devel/setup.bash

Теперь зайдем в папку с описанием робота:

roscd rosbots_description/launch

В ранее созданном файле spawn.launch было обозначено следующее:

<param name="robot_description" command="cat '$(find rosbots_description)/urdf/rosbots.xacro'" />

Использовалась команда cat для загрузки содержимого rosbots.xacro в параметр robot_description.

Для полного использования преимуществ формата команд xacro, код необходимо немного поправить. Заменим вышеуказанную строку на следующую:

<param name="robot_description" command="$(find xacro)/xacro.py '$(find rosbots_description)/urdf/rosbots.xacro'" />

Теперь система будет использовать xacro.py для выполнения rosbots.xacro.

Такие же изменения потребуются и для второго launch файла — rviz.launch.

Поменяем

<param name="robot_description" command="cat '$(find rosbots_description)/urdf/rosbots.xacro'" />

на

<param name="robot_description" command="$(find xacro)/xacro.py  '$(find rosbots_description)/urdf/rosbots.xacro'"/>

Проверим, что все работает с новым синтаксисом.

Сперва посмотрим на модель в редакторе rviz:

roslaunch rosbots_description rviz.launch

Затем, закрыв rviz, проверим в gazebo.

1-й терминал:

roslaunch gazebo_ros empty_world.launch

2-й:

roslaunch rosbots_description spawn.launch

*Можно увидеть сообщения

желтого цвета

xacro: Traditional processing is deprecated. Switch to --inorder processing!
To check for compatibility of your document, use option --check-order.
For more infos, see http://wiki.ros.org/xacro#Processing_Order
xacro.py is deprecated; please use xacro instead

Погоды они не далают, поэтому можно не обращать внимание.

Итак, все работает как и прежде, только теперь используется формат xacro.
Что он дает? Данный формат позволяет реорганизовать код. По мере роста проекта, это позволит в дальнейшем лучше ориентироваться.

Работаем с xacro

Теперь пришло время разделить rosbots.xacro на составные части и использовать преимущества xacro.

Переместим все, что касается редактора gazebo (теги gazebo) из rosbots.xacro в новый файл.
Создадим файл rosbots.gazebo.xacro в папке urdf:

nano rosbots.gazebo.xacro

И поместим туда код:

rosbots.gazebo.xacro

<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="rosbots" >
  
  <gazebo>
    <plugin name="differential_drive_controller" filename="libgazebo_ros_diff_drive.so">
      <legacyMode>false</legacyMode>
      <alwaysOn>true</alwaysOn>
      <publishWheelTF>true</publishWheelTF>
      <publishTf>1</publishTf>
      <publishWheelJointState>true</publishWheelJointState>
      <updateRate>100.0</updateRate>
      <leftJoint>wheel_left_joint</leftJoint>
      <rightJoint>wheel_right_joint</rightJoint>
      <wheelSeparation>1.1</wheelSeparation>
      <wheelDiameter>0.52</wheelDiameter>
      <wheelAcceleration>1.0</wheelAcceleration>
      <torque>20</torque>
      <commandTopic>/part2_cmr/cmd_vel</commandTopic>
      <odometryTopic>odom</odometryTopic>
      <odometryFrame>odom</odometryFrame>
      <robotBaseFrame>base_link</robotBaseFrame>
    </plugin>
  </gazebo>
</robot>

Этот же код в пределах тега

<gazebo> </gazebo>

удалим из файла rosbots.xacro.

Теперь вновь созданный файл «привяжем» к rosbots.xacro. Откуда-то же сведения о gazebo-составляющей rosbots.xacro должен получать!

Соответственно добавим в rosbots.xacro:

<xacro:include filename="$(find rosbots_description)/urdf/rosbots.gazebo.xacro" />

Данную строку вставим после строки с тегом

<robot>

. Теперь начало файла выглядит так:

<robot name="rosbots" xmlns:xacro="http://www.ros.org/wiki/xacro">
<xacro:include filename="$(find rosbots_description)/urdf/rosbots.gazebo.xacro" />

Еще раз проверим, что все работает в новом формате:

1-й терминал:

roslaunch gazebo_ros empty_world.launch

2-й:

roslaunch rosbots_description spawn.launch

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

Добавим новые сенсоры

Теперь, когда проект принял более-менее структурированный вид, навесим дополнительные сенсоры.

Чтобы пока сильно не загромождать робота «обвесами» добавим всего всего лишь два сенсора: камеру и imu(инерциальный измерительный модуль или гироскоп).

Для этих целей потребуется поправить файлы rosbots.xacro и rosbots.gazebo.xacro.

Начнем с камеры и файла rosbots.xacro. Для того, чтобы все получилось, для сенсора необходимо добавить:

связь (link). Она будет представлено файлом формата dae.
сустав (joint), который прикрепит камеру к телу робота.

В другом файле — rosbots.gazebo.xacro — мы добавим:

плагин, который определит связь (link), созданную выше, в качестве сенсора.

Разместим в rosbots.xacro в пределах тега (для удобства можно добавить в конце):

rosbots.xacro

<joint name="camera_joint" type="fixed">
  <origin xyz="0.49 -0.03 0.75" rpy="0 0.21 0" />
  <parent link="base_link"/>
  <child link="camera_link" />
</joint>   
<link name="camera_link">
  <visual>
    <geometry>
      <mesh filename="package://rosbots_description/meshes/camera.dae" scale="4.0 4.0 4.0"/>
    </geometry>
    <origin xyz="0.0 0 0" rpy="0 0 0"/>
  </visual>
  <collision>
    <geometry>
      <mesh filename="package://rosbots_description/meshes/camera.dae" scale="4.0 4.0 4.0"/>
    </geometry>
    <origin xyz="0.0 0 0" rpy="0 0 0"/>
  </collision>
</link>

Код выше добавляет link и joint для нашей камеры, позволяя ее визуализировать.

Проверим это.

1-й терминал:

roslaunch gazebo_ros empty_world.launch

2-й:

roslaunch rosbots_description spawn.launch

Если все верно, то можно увидеть добавленную камеру на роботе (белого цвета):

Вроде все просто. Однако надо понимать, что была добавлена только визуализация камеры. Как эта камера будет себя вести в мире физических вещей пока не ясно. Ее поведение не определено. Камера пока не способна делать фото или снимать видео.

Пришла очередь поработать над файлом с gazebo.

Добавим в

rosbots.gazebo.xacro

внутри тегов

<robot> </robot>

добавим:

<gazebo reference="camera_link">
    <sensor type="camera" name="camera1">
      <update_rate>30.0</update_rate>
      <camera name="head">
        <horizontal_fov>1.04</horizontal_fov>
        <image>
          <width>320</width>
          <height>240</height>
          <format>R8G8B8</format>
        </image>
        <clip>
          <near>0.1</near>
          <far>50</far>
        </clip>
      </camera>
      <plugin name="camera_controller" filename="libgazebo_ros_camera.so">
        <alwaysOn>true</alwaysOn>
        <updateRate>0</updateRate>
        <cameraName>camera1</cameraName>
        <imageTopicName>image_raw</imageTopicName>
        <cameraInfoTopicName>camera_info</cameraInfoTopicName>
        <frameName>camera</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>
</gazebo>

Как несложно догадаться в коде мы определили параметры камеры:

update_rate: как часто будут поступать данные
width/height: разрешение снимков. В данном случае, 320x240.
format: формат видео (R8G8B8).
imageTopicName: наименование топика, куда будут посылаться данные
frameName: link-связь, к которой будет привязана камера.

Теперь все готово как для визализации камеры, так и ее симуляции.

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

rostopic list

/rosbots/camera1/camera_info
/rosbots/camera1/image_raw
/rosbots/camera1/image_raw/compressed
/rosbots/camera1/image_raw/compressed/parameter_descriptions
/rosbots/camera1/image_raw/compressed/parameter_updates
/rosbots/camera1/image_raw/compressedDepth
/rosbots/camera1/image_raw/compressedDepth/parameter_descriptions
/rosbots/camera1/image_raw/compressedDepth/parameter_updates
/rosbots/camera1/image_raw/theora
/rosbots/camera1/image_raw/theora/parameter_descriptions
/rosbots/camera1/image_raw/theora/parameter_updates
/rosbots/camera1/parameter_descriptions
/rosbots/camera1/parameter_updates

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

Изображение в rviz из симулятора gazebo

*здесь надо оговориться что в текущей конфигурации образа для VMWare Workstation gazebo вылетает при попытке запустить трансляцию в rviz с виртуальной видеокамеры. Возможное решение указано в конце поста в разделе ошибки.

Для большей наглядности при работе с камерой в симуляции запустим rviz и поместим какой-нибудь объект перед роботом.

Для этого сперва понадобится сам объект, который будет добавлен в gazebo.

Скачайте файл object.urdf и положите его в ~/catkin_ws/src/

Запустим.

1-й терминал:

roslaunch gazebo_ros empty_world.launch

2-й (разместим модели):

rosrun gazebo_ros spawn_model -file /home/pi/catkin_ws/src/object.urdf -urdf -x 1 -y 0 -z 1 -model my_object

roslaunch rosbots_description spawn.launch

В симуляции получим такую картинку:

Модель робота и столб, который был так же добавлен как модель.

Предметы в редактор gazebo можно добавлять и более простым способом со вкладки внутри редактора «insert»:

Теперь посмотрим, что видит робот.

Не закрывая два предыдущих терминала, запустим rviz с описанием робота:

roslaunch rosbots_description rviz.launch

И добавим в нем новый Display с названием «Image»:

Появится новый дисплей с изображением с камеры и… вылетит редактор gazebo.

К сожалению, при работе на виртуальной машине с образом VMWare добавление трансляции с виртуальной камеры приводит к ошибке.

Если работа ведется не на виртуальной машине, а на реальной, то получим изображение с виртуальной камеры в gazebo с изображением фигуры столба:

Теперь добавим к модели IMU.

IMU (гироскоп)

Процесс добавления imu аналогичен добавлению камеры.

Сперва откроем rosbots.gazebo.xacro и внесем

код

<gazebo>
    <plugin name="gazebo_ros_imu_controller" filename="libgazebo_ros_imu.so">
      <!-- <robotNamespace></robotNamespace> -->
      <topicName>imu/data</topicName>
      <serviceName>imu/service</serviceName>
      <bodyName>base_link</bodyName>
      <gaussianNoise>0</gaussianNoise>
      <rpyOffsets>0 0 0</rpyOffsets>
      <updateRate>30.0</updateRate>
      <alwaysOn>true</alwaysOn>
      <gaussianNoise>0</gaussianNoise>
    </plugin>
  </gazebo>

Данный код, как и код для добавленной камеры, внесем в пределах тега

 <robot></robot>

Как не сложно догадаться из кода, он будет публиковать данные в топик /imu/data.

Теперь, если заново разместить модель робота в gazebo и выполнить в соседнем терминале команду: rostopic list, можно увидеть топик с imu данными среди прочих:

Также можно взглянуть, чтО он публикует, выполнив команду:

rostopic echo /imu/data -n1

Если кратко, то imu публикует следующую информацию:

orientation: ориентация робота по осям x, y, z и w.
angular_velocity: угловая скорость робота.
linear_acceleration: линейное ускорение.

Остался небольшой штрих

.
Добавим в rosbots.gazebo.xacro все также в пределах тега

код

<gazebo reference="wheel_left_link">
  <mu1>1.0</mu1>
  <mu2>1.0</mu2>
  <kp>1000000.0</kp>
  <kd>100.0</kd>
  <minDepth>0.001</minDepth>
  <maxVel>1.0</maxVel>
</gazebo>
<gazebo reference="wheel_right_link">
  <mu1>1.0</mu1>
  <mu2>1.0</mu2>
  <kp>1000000.0</kp>
  <kd>100.0</kd>
  <minDepth>0.001</minDepth>
  <maxVel>1.0</maxVel>
</gazebo>

<gazebo reference="base_link">
  <material>Gazebo/Blue</material>
  <mu1>0.3</mu1>
  <mu2>0.3</mu2>

  <sensor type="contact" name="bumpers">
    <always_on>1</always_on>
    <update_rate>50.0</update_rate>
    <visualize>true</visualize>
    <contact>
      <collision>base_footprint_collision_base_link</collision>
    </contact>
  </sensor>
</gazebo>

<gazebo reference="camera_link">
  <mu1>0.2</mu1>
  <mu2>0.2</mu2>
</gazebo>

Этот код определит дополнительные параметры робота: коэффициенты трения для колес, цвета в gazebo, контактный датчик. Контактный датчик будет срабатывать сразу после того, как бампер робота коснется препятствия.

Теперь заново запустим gazebo, разместим модель, а в rviz добавим imu display как и ранее, добавляли display с камерой:

Если все прошло удачно, то увидим, что imu публикует сообщения в топик.

В завершение поуправляем роботом в симуляции и посмотрим как изменяются данные с imu:

1-й терминал:

roslaunch gazebo_ros empty_world.launch

2-й:

roslaunch rosbots_description spawn.launch

roslaunch rosbots_description rviz.launch

3-й:

rosrun teleop_twist_keyboard teleop_twist_keyboard.py /cmd_vel:=/part2_cmr/cmd_vel

Возможные ошибки при работе:

1. Модель робота не появляется в gazebo (Package[rosbots_description] does not have a path) — закрыть gazebo, выполнить в терминале source devel/setup.bash, заново запустить gazebo.
2.

gzserver: /build/ogre-1.9-mqY1wq/ogre-1.9-1.9.0+dfsg1/OgreMain/src/OgreRenderSystem.cpp:546: virtual void Ogre::RenderSystem::setDepthBufferFor(Ogre::RenderTarget*): Assertion `bAttached && "A new DepthBuffer for a RenderTarget was created, but after creation" "it says it's incompatible with that RT"' failed.
Aborted (core dumped)

Возможное решение (не проверялось):

https://bitbucket.org/osrf/gazebo/issues/1837/vmware-rendering-z-ordering-appears-random

Робот-тележка на ROS. Часть 5. Работаем в rviz и gazebo: xacro, новые сенсоры +8

Работаем с xacro

Добавим новые сенсоры

Изображение в rviz из симулятора gazebo

IMU (гироскоп)

Остался небольшой штрих

Комментарии (0)