# ROS Dev Notes
Если вы изучили основы и знатно ~~зае..~~ помучались и не знаете как дальше жить, эти небрежно накиданные записки сумасшедшего для вас. Ответы на вопросы которые никто не задавал.
## Good style (maybe)
Упаковывать код в rosservice, чем больше кусков могут отработать независимо тем лучше.
Создать единый файл с неймспейсами, конфигами и тд, который пушит данные в сервер параметров, его обязаны использовать все кто работают над единым проектом. Это позволит не переписывать код лишний раз.
Не принебрегать неймспейсами.
Лучше всего добавлять всю визуализацию, которую возможно отдельной нодой.
[Советую ознакомиться с настройкой vs code](https://github.com/RoboGnome/VS_Code_ROS)
#### Создание воркспейса (catkin_make)
```bash=
mkdir <any_name>_ws # "_ws" для ясности того, что это воркспейс! (умоляю)
mkdir src
catkin_make
source devel/setup.bash
```
Еще можно сурснуть свой воркспейс с ключом ```--extend``` т.е. ```source devel/setup.bash --extend```, это позволит шелу не затереть все предыдущие сурснутые воркспейсы. Условно в системе есть воркспейсы других людей и либо вам либо ноде они требуются для работы.
**Конкретный кейс:** болезненная библиотека трансформаций ```tf``` из репозиториев скомпилирована для python-2, и есть скомпилированная под python-3 в одном из воркспейсов. Что можно сделать?
Добавить в ```.bashrc``` - ```source ~/tf_ws/devel/setup.bash``` и сурсить свой воркспейс ```source devel/setup.bash --extend```
Сверх полезная утилита для отладки ```roswtf```, отличное название, я считаю. Просто запустите, оно само все протестирует и скажет где и что сломалось в соединении между нодами. Так гораздо проще гуглить проблемы.
Для возможности запуска графических утилит на удаленном сервере по ssh, нужно добавить ключ ```-X``` и на своей машине разрешить подключения к X серверу.
```bash
xhost + # разрешает подключаться всем и отовсюду, нужно один раз разрешить
ssh -X -p <port> <username>@<ip>
```
#### Автозапуск пакетов
Roscore может стартовать при запуске системы, заодно с автостартом любых пактеов. Вроде как появляется эта возможность вместе с установкой [этого пакета](http://wiki.ros.org/robot_upstart). Он предоставляет некое апи для установки `.launch` файла в апстарт. Слабоумие и/или отвага может толкнуть к идее, что хорошо бы было стартовать собственные ноды при старте системы, если кустарная нода упадет, то утащит за собой в том числе и `roscore`. Вообще
## Настройка vscode
VSCode довольно гибкий универсальный блокнот с расширениями, это просто швейцарский нож на все случаи, можно даже смотреть мемы пока пишешь код.
Но это до безумия сложный и комплексный инструмент со своими ограничениями, большая часть настроек валяется по ```.json``` файлам, вдобавок среда позволяет их наслаивать друг на друга, от всего этого можно сойти с ума.
Главная цель, которую я хочу достичь - это понять как заставить vscode выдавать подсказки по скомпилированным ros сообщениям типа файлов ```.srv```, ```.msg``` и ```.action```, вторая цель это удобный способ откладки по шагам.
Какие расширения нам понадобятся:
1. [ROS](https://marketplace.visualstudio.com/items?itemName=ms-iot.vscode-ros)
2. [Python](https://marketplace.visualstudio.com/items?itemName=ms-python.python)
3. [пока не пробовал "task explorer"](https://marketplace.visualstudio.com/items?itemName=spmeesseman.vscode-taskexplorer)
4. [возможно это для темплейтов](https://marketplace.visualstudio.com/items?itemName=cantonios.project-templates)
5. [taskbar buttons](https://marketplace.visualstudio.com/items?itemName=spencerwmiles.vscode-task-buttons)
6. [Tasks Shell Input](https://marketplace.visualstudio.com/items?itemName=augustocdias.tasks-shell-input)
7. [~~мемы~~](https://marketplace.visualstudio.com/items?itemName=sachinsinghroxx.reddit-memes)
Главная беда расширения для питона это то, что в настройку ```extraPath``` невозможно закинуть путь относительно текущего воркспейса, этот момент я постараюсь как-нибудь обойти.
VSCode ожидает, что в директории ```.vscode``` будет лежать файл ```settings.json```, которой будет оверхедить настройки пользователя.
## Пояснение за ноды
При создании нода получает случайный порт на хост-машине, этот порт может быть указан через аргумент при запуске. Она обращается к переменной среды ```ROS_MASTER_URI``` и регистрируется в росмастере отдавая, *насколько я понимаю*, ему переменную среды ```ROS_IP```. По ней мастер-нода будет обращаться с ответом к обычной ноде. По умолчанию мастер будет обращаться по хостнейму машины, с которой пришел запрос и будет сильно удивлен не найдя этот хостнейм в ```/etc/hosts```.
Для докера на хосте **с росмастером** достаточно установить все переменные. На хосте **без росмастера** на данный момент ничего лучше, чем использовать аргумент ```--net host```, я не придумал. (Найду допишу).
Имеют вид:
```bahs
ROS_MASTER_URI=http://192.168.1.1:11311 # 11311 - дефолт порт росмастера
ROS_IP=192.168.1.2
```
Очень полезена инициализация ноды в дебаг режиме, она будет выдавать в ```stdout``` куда шлет запросы. Для питона строчка будет
```python
rospy.init_node("<name>", loglevel=rospy.DEBUG)
```
### Логирование
Логгер ROS'a предоставляет 5 уровней логирования
| | Debug | Info | Warn | Error | Fatal |
|:-------- |:-----:|:----:|:----:|:-----:|:-----:|
| stdout | | X | | | |
| stderr | | | X | X | X |
| log file | X | X | X | X | X |
| /rosout | o | X | X | X | X |
```python
rospy.loginfo()
rospy.loginfo_once()
...
```
Отладка - это процесс бесконечный, поэтому ставим дебаг левел ~~и забиваем~~. Интересный момент, дебаг-левел печает все сообщения от ноды сразу в топик ```/rosout```. Это позволяет читать сообщения от ноды из любого терминала. Если просто попытаться сделать это через ```rostopic echo /rosout``` получится какая-то каша, в перемешку с остальными нодами и ужасно неудобным видом. Есть одна утилита, на которую никто не обращает внимание - ```rosconsole```. Представляю открытие Америки:
```bash
rosconsole echo -l debug /test_log_
```
```-l``` - дебаг левел, чтобы отфильтровать ненужное
```/test_log_``` - это фильтр, чтобы печатались сообщения только с нужной ноды (может быть регуляркой).
### Как не плодить терминалы
Вот у вас есть 10 нод, которые надо иногда запускать ручками, а это на минуточку 10 окон терминала, лично мой мозг не переваривает больше пары окон. С описанным выше подходом к логированию можно вполне удобно смотреть, что там делает наш сервис, следить за его состоянием и тд. Так вот строчка для запуска в фоне:
```bash
rosrun <pkg> <exec_file> > /dev/null &
```
```&``` запускает сразу в фоне
```> /dev/null``` выкидывает весь stdout в мусорку, иначе весь вывод будет печаться в консоль
Если завершить сессию в терминале с запущенными таким образом процесами, то они завершаться вместе с ним, но можно пустить процессы в свободное плавание запустив с помощью ```disown```.
```bash
rosrun <pkg> <exec_file> > /dev/null & disown
```
## Работа с камерами глубины
У камер глубины для вычисления непосредственно координат есть топики:
* очевидно глубина
* CameraInfo
[Документация](http://docs.ros.org/en/melodic/api/sensor_msgs/html/msg/CameraInfo.html)
Из этого топика CameraInfo нужно только поле с матрицей `K`, из нее нужно достать `fx, fy, cx, cy`. Cоответственно точка вычисляется так:
```
px, py - координаты в пикселях
depth[px, py] - соответственно глубина
x = depth[px, py]*(px - cx)/fx
y = depth[px, py]*(py - cy)/fy
z = depth[px, py]
```
Камеры глубины нещадно шумят, за глубину стоит брать среднюю глубину с квадрата +-3 пикселя (я вообще беру 10х10, но это чисто по задаче подбирается), при этом отбрасывая нулевые значения (в них камера глубины просто не знает глубину).
Еще замечание: реалсенс выдает глубину в миллиметрах.
### Перевод ros сообщения камер в numpy
Вообще для перевода есть [cv_bridge](http://wiki.ros.org/cv_bridge), *но есть нюанс*... Он не всегда работает.
С ним все довольно просто
```python=
bridge = CvBridge()
cv_depth = bridge.imgmsg_to_cv2(depth_msg, desired_encoding="32FC1")
cv_image = bridge.imgmsg_to_cv2(image_msg, desired_encoding='passthrough')
```
А вот без
```python=
cv_depth = np.frombuffer(depth_msg.data, dtype=np.uint16)
cv_depth = cv_depth.reshape(depth_msg.height, depth_msg.width)
cv_img = np.frombuffer(img_msg.data, dtype=np.uint8)
cv_img = cv_img.reshape(img_msg.height, img_msg.width, -1)
```
### CompressedImage
Камеры еще имеют топик со сжатыми картинками, по структуре из себя они представляют вроде как сжатый *жипег*, он экстремально полезен, когда ширина канала связи двух машин не очень большая, облачные вычисления и прочее.
Просто сравнение частоты обычного и сжатого при соединении с "облаком" через 4g модем.
```a
subscribed to [/realsense_back/color/image_raw]
no new messages
average rate: 1.822
min: 0.516s max: 0.582s std dev: 0.03258s window: 3
average rate: 1.725
min: 0.516s max: 0.641s std dev: 0.05110s window: 4
average rate: 1.598
min: 0.516s max: 0.724s std dev: 0.07123s window: 6
```
```a
subscribed to [/realsense_back/color/image_raw/compressed]
no new messages
average rate: 33.423
min: 0.010s max: 0.045s std dev: 0.01158s window: 14
average rate: 31.075
min: 0.007s max: 0.052s std dev: 0.01086s window: 45
average rate: 30.641
min: 0.007s max: 0.052s std dev: 0.01064s window: 75
```
ВИДИТЕ ЭТОТ ПРИКОЛ? Дай бог полтора кадра в секунду против почти идеальных 30 дошедших кадров всегда! Именно поэтому стоит разобраться с их переводом в нормальный вид numpy.
```python=
np_img_arr = np.frombuffer(image_msg.data, np.uint8)
cv_img = cv2.imdecode(np_img_arr, cv2.IMREAD_COLOR)
```
## Лоботомия bash скриптами
Сразу скажу, я не эксперт в bash скриптах. Тут я постараюсь изложить интересные находки на просторах стаковерфлоу.
Например то, что можно использовать абсолютно в каждом скрипте, **получить директорию баш скрипта**:
```bash
DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" > /dev/null && pwd )"
```
Если в зависимости от задачи нужно выполнять какие-либо действия постоянно одинаковых команд с кучей аргументов, то можно просто добавить их как функции в ```.bashrc```. На названия этих функций будет работать автокомплит, жизнь это упростит знатно.
Пример
```bash=
set_rosmaster(){
export ROS_MASTER_URI=http://192.168.131.1:11311
export ROS_IP=192.168.131.5
}
```
Также bash имеет интересный аргумент ```--init-file```, с помощью него можно указать любой файл в качестве файла инициализации шела вместо ```.bashrc```.
Поиск по bash history ```ctrl+R```.
Если ваш баш скрипт должен подождать старта какого-то контейнера можно заюзать вот такой сниппет:
```bash=
until [ "`docker inspect -f {{.State.Running}} $container_name`"=="true" ]; do
sleep 0.1;
done;
```
Запуск контейнера, если он не запущен
```bash=
if ! [ "$( docker container inspect -f '{{.State.Status}}' $container_name )" == "running" ]; then
docker start $container_name
fi
```
Выполнение действий для всех поддиректорий в директории
```bash=
dirs=($(ls /home))
for dir in "${dirs[@]}"; do
# do something
done
```
Получение апишкниов хоста
```bash
ips=($(hostname -I))
ip_0=${ips[0]}
```
Получение названия директории
```bash
container_name=$(dirname $(readlink -m $DIR))
container_name=${container_name##*/}
```
## Сага о том как я клеил ros и docker
Началось с того, что мне потребовалось установить контейнер с cuda. Все готовые контейнеры с ROS и cuda на докерхабе имели либо проблемы со стартом, либо имели битый пакетный менеджер. Я бы хотел сделать его несколько универсальным чтобы адаптировать к любым своим проетам.
### Dockerfile, собирали всем селом
Конечно, каждый хотел бы схалтурить и взять уже готовый образ, видит бог, я этого не хотел, но прийдется ставить все ручками, наследуясь от безпроблемных образов. Поэтому идем на [страничку](http://wiki.ros.org/ROS/Installation) и смотрим как ставить ROS на простую систему.
Любой контейнер начинается с докерфайла, поэтому вот установка ROS в докер. Во-первых для ROS нужен полноценный контейнер, что-то типо убунты одной из не сильно допотопных версий, второе нужно выдернуть строчки из гайда по установке. Единственное что, для какогото из пакетов от ROS требуется таймзона. Аргументы в свою очередь нужны будут дальше для [фокусов](###Цыганские-фокусы), да и в целом полезно в них разобраться чтобы сделать немного динамический и универсальный докерфайл.
```dockerfile
FROM ubuntu:20.04
ARG hostname
ARG host_ip
ARG ros_master_uri
RUN apt update
ENV TZ=Europe/Kiev
RUN ln -snf /usr/share/zoneinfo/$TZ /etc/localtime && echo $TZ > /etc/timezone
RUN sh -c 'echo "deb http://packages.ros.org/ros/ubuntu focal main" > /etc/apt/sources.list.d/ros-latest.list'
RUN apt install -y curl gnupg gnupg2 gnupg1
RUN curl -s https://raw.githubusercontent.com/ros/rosdistro/master/ros.asc | apt-key add -
RUN apt update && apt install -y ros-noetic-ros-base
ENV ROS_DISTRO noetic
RUN apt install -y python3-rosdep python3-rosinstall python3-rosinstall-generator python3-wstool build-essentia
```
Для универсальности и удобства установим рабочую директорию и переменную среды с путем к проету.
```dockerfile
ENV PROJECT_DIR=/root/catkin_ws
ENV ROS_MASTER_URI=${ros_master_uri}
WORKDIR /root/catkin_ws
```
На этом шаге нам и пригодятся переменные среды выше, появляется задачка немного сложнее, чтобы подтянулись все утилиты ROS'а нужно сурснуть файлик ```/opt/ros/$ROS_DISTRO/setup.bash```. Предлагаю в проекте создать файлик ```.bashrc``` с таким содержимым:
```bash
source /opt/ros/$ROS_DISTRO/setup.bash
source $PROJECT_DIR/devel/setup.bash
export ROS_IP=$(hostname -i)
export PS1="\[\e]0;\u@\h: \w\a\]${debian_chroot:+($debian_chroot)}\[\033[0;33m\]\u@\h\[\033[00m\]:\[\033[01;34m\]\w\[\033[00m\]\$ "
```
Выставление последних двух переменных не обязательно, если при создании контейнера будет использоваться подсеть хоста (```--net host```). ```PS1``` нужна лишь для украшения нашего терминала, подчистую украдена с убунты, за исключением цвета.
Его требуется скопировать в домашнюю директорию внутри образа, отсюда появляется такая строчка
```dockerfile
COPY .bashrc /root/
```
### Цыганские фокусы
А теперь об организации проекта с докером, которую я вывел для себя как оптимальную путем экспериментов. Темплейт проекта организовать стоит как-то так
```
catkin_ws/
├── .catkin_workspace
└── src
├── CMakeLists.txt
└── ros-docker-template
├── CMakeLists.txt
├── docker
│ ├── .bashrc
│ └── Dockerfile
├── launch
│ └── default.launch
├── package.xml
├── scripts
│ ├── attach.sh
│ ├── build_docker.sh
│ ├── run_prog.sh
│ └── start.sh
└── src
└── node.py
```
Короче в чем суть, я хотел чтобы и хост система и докер видели это как ROS пакет. Запуск контейнера предполагается с помщью скрипта ```start.sh```, а ```build_docker.sh``` будет собирвать образ проекта соответственно, и все это через ```rosrun``` на хост системе. И хо-хо [эта ~~дичь~~ bash заметка](##Лоботомия-bash-скриптами) тут как нельзя кстати. Нужно всего-то смонитировать ```catkin_ws/src/project1``` в контейнер без привязки к абсолютным путям, поэтому будем использовать пути относительно скриптов, он и будет создавать контейнер и запускать его, при условии если контейнер не существует и не запущен. Штош, задача звучит уже так, что не хочется слышать, но в итоге вдоволь намучавшись она была решена.
```bash=
DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" > /dev/null && pwd )"
HOST_IP=($(hostname -I))
img_name=$(dirname $(readlink -m $DIR))
img_name=${img_name##*/}-img
docker build \
--build-arg hostname=$(hostname) \
--build-arg ros_master_uri="http://${HOST_IP[0]}:11311" \
--build-arg host_ip=${HOST_IP[0]} \
--tag $img_name \
$DIR/../docker
```
Основная фишка в том, что он называет контейнер по имени директории с проектом + '-img', эту особенность абузится и во всех остальных скриптах.
Собственно через аргумент ```--build-arg``` и передаются переменные в ```Dockerfile```, если их не передать, докер выдаст ворнинг, но все равно соберет образ. Дальше пойдет совсем жеть, просьба убрать *людей, беременных детей и женщин с тонкой душевной организацией* от экрана.
```start.sh```
```bash=
DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" > /dev/null && pwd )"
container_name=$(dirname $(readlink -m $DIR))
container_name=${container_name##*/}
img_name=$container_name-img
if [ ! "$(docker ps -a | grep $container_name)" ]; then
docker run -di \
--name $container_name \
--add-host $(hostname):$(hostname -i) \
--mount type=bind,src=$DIR/../..,dst=/root/catkin_ws/src \
--hostname ros-0 \
-P \
$img_name bash
fi
if ! [ "$( docker container inspect -f '{{.State.Status}}' $container_name )" == "running" ]; then
docker start $container_name
fi
docker exec $container_name bash -c "source /root/.bashrc; catkin_make"
```
Этот скрипт сразу поднимает/создает контейнер и монтирует директорию ```catkin_ws/src``` сразу в образ, что позволит запускать внутри контейнера и все остальные пакеты в этом воркспейсе, ну не чудно ли? Более того он на опередежение его собирает.
Для быстрого старта любого пакета из этого воркспейса была собрана из велосипедов и костылей целая консольная утилита ```run_prog.sh```.
```bash=
DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" > /dev/null && pwd )"
container_name=$(dirname $(readlink -m $DIR))
container_name=${container_name##*/}
if [ $1 = "--help" ] || [ $1 = "-h" ];
then
echo "usage
no args: rosrun $container_name node.py
1 arg : rosrun $container_name <ARG>
2 arg : rosrun <arg1> <arg2>
"
fi
if [[ $1 == "" ]] && [[ $2 == "" ]]
then
PKG=$container_name
EXEC=node.py
echo 1
elif [[ $2 == "" ]] && [[ $1 != "" ]]
then
PKG=$container_name
EXEC=$1
echo 2
else
PKG=$1
EXEC=$2
echo 3
fi
echo "launching pkg:$PKG exec:$EXEC"
docker exec $container_name bash -c "source /root/.bashrc; catkin_make; rosrun $PKG $EXEC"
```
Все про нее написано в общем-то в help, оно при запуске перекомпилирует весь воркспейс, ну прямо чудеса bash скриптов.
### Итого
Получился вот такая [репа](https://github.com/danissomo/ros-docker-template)
```bash
# Cборка образа
rosrun ros-docker-template build_docker.sh
# Запуск/создание контейнера
rosrun ros-docker-template start.sh
# Запуск любого пакета внутри контейнера
rosrun ros-docker-template run_prog.sh <pkg> <exec>
```
## Namespaces
## Rosparam
## Roslaunch
## Список источников
1. [Google](https://www.google.com/)
2. [stackoverflow](https://stackoverflow.com)
3. [answers.ros](https://answers.ros.org/)
Sign in with Wallet
Connect another wallet