Введение в современную робототехнику на основе ROS

с камерой
Практикум: патрулирование
Практикум: Работа с удаленным роботом
Итоговая работа
Обсуждение работы, аттестация

инженеров Terminal это один из основных инструментов работы.

Терминал вызывается нажатием клавиш Ctrl + Alt + T
Tab - автодополнение, наберите начало команды или пути и нажмите Tab, если такая команда только одна оболочка ее дополнит. Если доступно несколько вариантов, нажмите два раза Tab чтобы их увидеть
Стрелка вверх - предыдущая команда в истории
Стрелка вниз - следующая команда в истории
Ctrl + C - прервать выполнение программы
Ctrl + Z - приостановить выполнение программы
Ctrl + D - завершить текущий сеанс связи
Ctrl + Shift + C Скопировать
Ctrl + Shift + V Вставить

списка файлов в текущей директории.

pwd (print working directory) выводит текущую директорию

cd (Change Directory) - сменить директорию

mkdir (Make Directory) - создать директорию

touch (Коснуться) можно создать пустой файл

mv (Move) - переместить файлы или директории

cp (Copy) скопировать файлы или директории

rm (Remove) - удалить файлы или директории

sudo (Substitute User & DO) - подменить пользователя
и выполнить

find (Найти) предназначена для поиска файлов

cat (Catenate - связывать) вывод содержимого файла на экран

grep (global regular expression print) применение регулярных выражений

nano - консольный текстовый редактор

| - конвейер перенаправления вывода

scp копирование файлов по сети

apt — менеджер пакетов

SSH— доступ к удаленному терминалу

= 3
c = a + b
print(c)

Отступы - основа разметки
if (a > b):
print("a > b")
else:
print("b > a")

- float

Операции над числами:
+ сложение
- вычитание
* умножение
/ деление
** возведение в степень
// целочисленное деление с отбрасыванием остатка
% получение остатка от деления

условие верно, если первый операнд меньше второго
> “Больше” — условие верно, если первый операнд больше второго
<= “Меньше или равно”
>= “Больше или равно”
== “Равенство” Условие верно, если два операнда равны
!= “Неравенство” Условие верно, если два операнда неравны

moon_to_earth = moon_to_earth + 1

- 9) * 4)
print(3 ** 4) # две звёздочки (**) - оператор возведения в степень
print(69 / 4) # косая черта (/) - оператор деления
print(69 // 4) # две косые черты (//) -
оператор возвращающий частное от деления нацело
print(57 % 7) # процент (%) - оператор остатка от деления нацело

Вывод print()

Ввод input()

a = input("Введите a")
b = input("Введите b")
s = a + b
print(s)

типов переменной используются функции с названиями типов

int() - целое число

float() - действительное число

str() - строка

и др.

13]
Rainbow = ['Red', 'Orange', 'Yellow', 'Green']

a = Primes[2]

Список

Доступ к элементу списка или строки

s = "Hello robot"

Строка

+ b
print(c)

print("Основной путь ветвления")
else:
print("Условие == false")
print("Альтернативный путь ветвления")

НЕ (отрицание)

логическое И - and (a and b) == True, a == True and b == True

логическое ИЛИ - or (a or b) == True, a == True or b == True

логическое НЕ - not (not a) == True, a == False

участием переменной

Непосредственно работа.
Возврат - Значение возвращаемое в точку вызова функции
Передача параметра в функцию - arg

def Foo(arg):
return 1
a = Foo()

print(a)
def function2():
print(a)
function1()
function2()

NameError: name 'a' is not defined

function2():
global a
print(a)
function1()
function2()

>>> 5

>>> 5

dict(), tuple(), range(), sum(), min(), max() ….

числа равна 0.5, то число округляется до ближайшего четного числа.
abs(x)Модуль (абсолютная величина). Это — стандартная функция.
sqrt(x)Квадратный корень. Использование: sqrt(x)
log(x)Натуральный логарифм. При вызове в виде log(x, b) возвращает логарифм по основанию b.
e Основание натуральных логарифмов e = 2,71828… pi Константа π = 3.1415...
sin(x)Синус угла, задаваемого в радианах
cos(x)Косинус угла, задаваемого в радианах
tan(x)Тангенс угла, задаваемого в радианах
asin(x)Арксинус, возвращает значение в радианах
acos(x)Арккосинус, возвращает значение в радианах
atan(x)Арктангенс, возвращает значение в радианах
atan2(y, x) Полярный угол (в радианах) точки с координатами (x, y).
degrees(x)Преобразует угол, заданный в радианах, в градусы.
radians(x)Преобразует угол, заданный в градусах, в радианы.

import math

компоненты компьютерной программы моделируются на основе реальных объектов. Объект — это что-либо, у чего есть какие-либо характеристики и то, что может выполнить какую-либо функцию.

это некий чертеж будущего объекта. Как план строительства дома, или чертеж детали. А экземпляр - это конкретная реализация чертежа т.е. построенный дом или выточенная деталь.

class Book():
title = ""
author = ""
def print_book(self):
print(self.title, self.author)
b = Book()
b.print_book()

print_book() - метод класса

title, author - поле, переменная, атрибут класса

+ 10 * math.cos(x)) * math.e**x / (math.sqrt(t) - abs(math.sin(t)))
print(round(z,2))

print(tuple(new_triple))
def rotate_right(triple):
new_triple = list(triple)
new_triple = [new_triple[2],new_triple[0],new_triple[1]]
print(tuple(new_triple))
triple = ('A','B','C')
rotate_left(triple)
rotate_right(triple)

Вам предстоит реализовать две функции, которые "вращают" тройку влево и вправо. Как это выглядит, вы можете понять из пары примеров:
>>> triple = ('A', 'B', 'C')
>>> rotate_left(triple)
('B', 'C', 'A')
>>> rotate_right(triple)
('C', 'A', 'B')

окружения http://wiki.ros.org/ROS/Tutorials/InstallingandConfiguringROSEnvironment
Запуск мастер-ноды ROS: roscore

(Subscriber)

Подписчик

Издатель

Топик

Топик

Мастер-нода

Нода

Сообщение

всех стандартов https://ros.org/reps/rep-0000.html

Оси x, y и z в ROS используют правило правой руки

ROS. Библиотека позволяет разработчикам быстро взаимодействовать с Топиками и Сервисами.
‌
Архитектура rospy отдает предпочтение скорости реализации (то есть времени, затраченному разработчиком) по сравнению с производительностью во время выполнения. Библиотека отлично подходит для быстрого прототипирования и тестирования в ROS. Многие инструменты ROS написаны на rospy, например, уже известные нам утилиты rostopic и rosservice.
‌
Библиотека ropsy устанавливается при установке ROS, поэтому отдельно устанавливать нам ничего не нужно.

queue_size=10)
s = String()
s.data = "Hello robot"
pub.publish(s)

callback)
rospy.spin()

geometry_msgs.msg import Twist
from turtlesim.msg import Pose
rospy.init_node('one_meter_node')
pub = rospy.Publisher('/turtle1/cmd_vel', Twist, queue_size=10)
def callback_regulator(msg):
vel = Twist()
if msg.x >= 7:
vel.linear.x = 0
else:
vel.linear.x = 0.1
pub.publish(vel)
rospy.Subscriber('/turtle1/pose', Pose, callback_regulator)
rospy.spin()

rospy
from geometry_msgs.msg import Twist
from nav_msgs.msg import Odometry
class RobotMover():
def __init__(self):
rospy.init_node('one_meter_node')
self.vel = Twist()
self.distance_passed = 0
self.pub = rospy.Publisher('/cmd_vel', Twist, queue_size=10)
rospy.Subscriber('/odom', Odometry, self.callback_handler)

def callback_handler(self, msg):
self.distance_passed = msg.pose.pose.position.x
def regulator(self):
print(self.distance_passed)
if self.distance_passed >= 1:
self.vel_publisher(0)
else:
self.vel_publisher(0.1)
def vel_publisher(self, vel_value):
self.vel.linear.x = vel_value
self.pub.publish(self.vel)

r = RobotMover()
while not rospy.is_shutdown():
rospy.sleep(0.2)
r.regulator()

def callback_handler(self, msg):
self.distance_passed = msg.pose.pose.position.x
def regulator(self):
print(self.distance_passed)
if self.distance_passed >= 1:
self.vel_publisher(0)
else:
self.vel_publisher(0.1)
def vel_publisher(self, vel_value):
self.vel.linear.x = vel_value
self.pub.publish(self.vel)

r = RobotMover()
while not rospy.is_shutdown():
rospy.sleep(0.2)
r.regulator()

двигалась по квадрату со сторонами равными 3 метрам.
‌
В результате работы программы черепаха должна проехать все вершины квадрата и оказаться в точке старта. Ориентация черепахи в точке финиша должна совпадать с ориентацией в момент старта.

import rospy
import math
from geometry_msgs.msg import Twist
from turtlesim.msg import Pose
rospy.init_node("mover")
first_run = True
current_pose = Pose()
init_pose = Pose()
side_len = 1
pub = rospy.Publisher("/turtle1/cmd_vel", Twist, queue_size=10)
def odom_cb(pose):
global current_pose
current_pose = pose
rospy.Subscriber("/turtle1/pose", Pose, odom_cb)
def forward(des_dist):
global init_pose, current_pose, first_run, side_len
init_pose = current_pose
dist = math.sqrt((init_pose.x-current_pose.x)**2 + (init_pose.y-current_pose.y)**2)
while dist < des_dist:
dist = math.sqrt((init_pose.x-current_pose.x)**2 + (init_pose.y-current_pose.y)**2)
publish_vel(1,0)
else:
publish_vel(0,0)
def turn(des_angle):
global init_pose, current_pose, first_run, side_len
angle_turn = True
while angle_turn:
if abs(init_pose.theta - current_pose.theta) < des_angle:
publish_vel(0,0.5)
else:
angle_turn = False
publish_vel(0,0)
def publish_vel(vel_x, vel_z):
vel = Twist()
vel.linear.x = vel_x
vel.angular.z = vel_z
pub.publish(vel)
rospy.sleep(0.2)
for i in range(4):
forward(1)
turn(math.pi/2)

handle_callculation(req):
print("Returning [%s + %s = %s]"%(req.a, req.b, (req.a + req.b)))
return AddTwoIntsResponse(req.a + req.b)
rospy.Service('sum_service', AddTwoInts, handle_callculation)
print("Ready to add two ints.")
rospy.spin()

AddTwoInts
def add_two_ints_client(a, b):
rospy.wait_for_service('sum_service')
service_caller = rospy.ServiceProxy('sum_service', AddTwoInts)
resp = service_caller(a, b)
return resp.sum
a = random.randint(-100 , 100)
b = random.randint(-100 , 100)
print("%s + %s = %s"%(a, b, add_two_ints_client(a, b)))

FibonacciAction, FibonacciFeedback, FibonacciResult
class FibonacciActionCounter():
def __init__(self):
self.feedback = FibonacciFeedback()
self.result = FibonacciResult()
self.asr = actionlib.SimpleActionServer('FB_counter', FibonacciAction, self.execute_cb)
def execute_cb(self, goal):
success = True
self.feedback.sequence = []
self.feedback.sequence.append(0)
self.feedback.sequence.append(1)
rospy.loginfo('Executing, creating fibonacci sequence of order %i' % goal.order)
for i in range(1, goal.order - 1):
if self.asr.is_preempt_requested():
rospy.loginfo('Action Preempted')
self.asr.set_preempted()
success = False
break
self.feedback.sequence.append(self.feedback.sequence[i] + self.feedback.sequence[i-1])
self.asr.publish_feedback(self.feedback)
rospy.sleep(1)
if success:
self.result.sequence = self.feedback.sequence
rospy.loginfo('Succeeded')
self.asr.set_succeeded(self.result)
if __name__ == '__main__':
rospy.init_node('fibonacci')
server = FibonacciActionCounter()
rospy.spin()

= True
self.feedback.sequence = []
self.feedback.sequence.append(0)
self.feedback.sequence.append(1)
rospy.loginfo('Executing, creating fibonacci sequence of order %i' % goal.order)
for i in range(1, goal.order - 1):
if self.asr.is_preempt_requested():
rospy.loginfo('Action Preempted')
self.asr.set_preempted()
success = False
break

rospy.sleep(1)
if success:
self.result.sequence = self.feedback.sequence
rospy.loginfo('Succeeded')
self.asr.set_succeeded(self.result)
rospy.init_node('fibonacci')
server = FibonacciActionCounter()
rospy.spin()

FibonacciAction, FibonacciGoal
class FibonacciClient():
def __init__(self):
self.client = actionlib.SimpleActionClient('FB_counter', FibonacciAction)
def fibonacci_send_goal(self):
goal = FibonacciGoal
goal.order = 10
self.client.wait_for_server()
self.client.send_goal(goal, done_cb = self.done_callback)

print(result)
rospy.init_node('fibonacci_client_py')
fib = FibonacciClient()
fib.fibonacci_send_goal()
print("Do something else")
rospy.spin()

переменные, числа, строки и тп. Он не создан для хранения сложных типов данных, например сообщений ROS.
Для работы с параметрами есть удобная консольная утилита rosparam
rosparam list: Получить список параметров
rosparam get parameter: Прочитать значение параметра
rosparam set parameter value: Установить значение параметра
rosparam delete parameter: Удалить значение параметра
rosparam dump file: Сохранить все переменные в файл
rosparam load file: Восстановить переменные из файла

Python

import rospy
rospy.init_node('params_demo')
default_param = "some_value"
try:
my_param = rospy.get_param("~my_param")
except KeyError as e:
my_param = default_param
rospy.set_param("~my_param", my_param)
print(my_param)

немного похожа на машину времени. Мы можем включить "запись" и сохранять все сообщения, которые происходят в нашей системе, а результат этой записи сохраняется в .bag файлах.
Для работы с .bag служит консольная утилита rosbag, перечислим основные ее аргументы
rosbag record [TOPIC_NAME] Начать запись .bag
rosbag play [FILE_NAME] Проиграть .bag файл
rosbag compress [FILE_NAME] Архивировать файл
rosbag decompress [FILE_NAME] Разархивировать файл
rosbag record -a Записать все сообщения

Для того, чтобы было проще ориентироваться с файлами любого пакета, сообщество разработчиков рекомендует использовать единообразную файловую структуру пакета:
bin/ Директория, в которой хранятся скомпилированные программы
include/ Директория содержит файлы с заголовками (headers) библиотек
launch/ Директория для хранения файлов конфигурации запуска .launch
msg/ Директория для сообщений (для топиков)
src/ Директория для хранения исходников программ (в том числе и скриптов)
srv/ Директория для хранения сообщений для использования Сервисами (Services)
CMakeLists.txt: Файл форматы Cmake с инструкциями для установки пакета
package.xml Файл "манифест" для описания пакета

ros-noetic-Имя
Если вы нашли пакет pkg_name на wiki, то скорее всего в системе apt будет назваться ros-noetic-pkg_name
‌Установка нового пакета происходит стандартным способом
sudo apt install ros-noetic-pkg_name

в catkin_ws/src/
catkin_make --pkg=pkg_name

помощи утилиты catkin_create_pkg, она сама создаст необходимые файлы и директории и пропишет зависимости.

Перейдем в директорию, где расположены исходники пакетов
cd ~/catkin_ws/src

И создадим наш “пока пустой” пакет
catkin_create_pkg my_first_package

описание нашего сообщения

mkdir msg cd msg nano move.msg

float32 s
float32 v

2. Модифицируем файл CMakeLists.txt

add_message_files(
FILES
move.msg)

generate_messages(
DEPENDENCIES
std_msgs
)

3. Убедимся что в файле package.xml есть следующие строки:

message_runtime

std_msgs

std_msgs

catkin_package(
CATKIN_DEPENDS std_msgs message_runtime)

4. Соберем пакет стандартным образом catkin_make --pkg=my_first_package

для РОС, по пользовательским сценариям.

Создавать лаунч файлы надо в папке launch вашего проекта, тогда при запуске при помощи утилиты roslaunch, ROS сам найдет файл, в папке.
Пример самого простого лаунч файла:

Не забудьте сделать программу исполняемой
cd ../src
chmod +x имя_файла.py

примере мета-операционной системы Robot Operating System (ROS), работающей в среде Linux.у программ для РОС, по пользовательским сценариям.
Инструкция к роботу лежит тут: https://manual.turtlebro.ru

к WiFi точке доступа с параметрами

SSID: TurtleBro5G
Pass: turtlew001

SSID: TurtleBro
Pass: turtlew001

2. Подключите работающий Raspberry к Ethernet кабелю. Определите IP адрес устройства через web-интерфейс роутера. Зайдите на ssh на устройство ssh pi@IPили pi@turtlebroNNN.local Откройте в редакторе mcedit файл wpa_supplicant.conf
sudo mcedit /etc/wpa_supplicant/wpa_supplicant.conf
В конец файла добавьте настройки WiFi вашей сети в виде:
network={
ssid="WIFI_NETWORK_NAME"
psk="wifipassword"
}
При указании пароля от wi-fi сети, обязательно наличие кавычек. Сохраните файл и перезагрузите raspberry.

По умолчанию это сеть TutleBro с паролем turtlew001
Вы знаете IP адрес робота
На вашем компьютере есть ssh клиент
Прямое подключение ssh pi@ip_robot
Пароль пользователя pi: brobro (есть в инструкции)

к плате батарейке. Тип сообщения sensor_msgs/BatteryState
Топик /cmd_vel Топик управления перемещения роботом. Тип сообщения geometry_msgs/Twist
Топик /imu Данные инерционного датчика (Inertial measurement unit), включающего в себя гироскоп, акселерометр и компас. Тип сообщения sensor_msgs/Imu
Топик /odom Данные одометрии (положения робота, рассчитанного на основании вращения колес). В текущей версии повороты робота рассчитываются по данным IMU датчика, а перемещение на основе данных энкодеров на моторах. Тип сообщения nav_msgs/Odometry
Топик /scan Данные, полученные с лидара (облако точек). Данные идут через Serial интерфейс лидара, USB-UART преобразователь и через USB hub. Тип сообщения sensor_msgs/LaserScan

/odom) и текущих установленных скоростей (/cmd_vel) робота.
Сервис /set_pid Сервис устанавливает значения ПИД регулятора для управления моторами тележки. Тип сообщения turtlebro/PidRegulator
Сервис /board_info Сервис выдает актуальную информацию о версии прошивки системной платы и уникальный номер процессора
Сервис /start_motor Сервис включает мотор вращения лидаром (для RPLidar). Без параметров
Сервис /stop_motor Сервис выключает мотор вращения лидаром (для RPLidar). Без параметров

интерпретатора python и верия библиотеки rospy
Версия дистрибутива ROS
Версия пакета turtlebro
Версия прошивки микроконтроллера материнской платы
Серийный номер системной платы робота (mcu_id)
Размер оперативной памяти
Полное / Свободное пространство на SD карте (Gb)

uname -a
python3 -V pip3 show rospy
rosversion -d
rosversion turtlebro
rosservice call /board_info {}
rosservice call /board_info {}
cat /proc/meminfo | grep MemTotal
df -h

/bat -n 1
Проверка Лидара Облако точек лидара в топике rostopic echo /scan -n 1
Проверка одометрии колес В первом терминале: Команда: rostopic echo /odom
В другом терминале команда: rostopic pub /cmd_vel geometry_msgs/Twist (x = 0.05)
Команда: rostopic pub /cmd_vel geometry_msgs/Twist (x = -0.05)
Команда: rostopic pub /cmd_vel geometry_msgs/Twist (x = 0; z = 0.5)
Команда: rostopic pub /cmd_vel geometry_msgs/Twist (x = 0; z = -0.5)
Проверка IMU датчика Данные IMU датчика в консоли rostopic echo /imu -n 1

о максимальном разрешения работы камеры
Проверить работу камеры через web интерфейс

cat /dev/ | grep video
v4l2-ctl --list-formats-ext
10.8.0.6:8080

прозрачная работа по сети.

export ROS_MASTER_URI=’http://192.168.0.145:11311’

export ROS_HOSTNAME=192.168.0.111

geometry_msgs/Vector3 angular float64 x float64 y float64 z

Топик /cmd_vel Топик управления перемещения роботом. Тип сообщения geometry_msgs/Twist

на основании вращения колес). В текущей версии повороты робота рассчитываются по данным IMU датчика, а перемещение на основе данных энкодеров на моторах. Тип сообщения nav_msgs/Odometry

Обратите внимание, что ориентация робота задается кватернионом, а не привычными нам углами Эйлера

чисел, образующая векторное пространство размерностью четыре над полем вещественных чисел. Обычно обозначаются символом H. Предложены Уильямом Гамильтоном в 1843 году.

== C, где i*i = -1

Сложение комплексных чисел = сложение векторов
(a + ib) + (c + id) = a + c + i(b + d)

Умножение комплексных чисел = сложение углов
(a + ib) * (c + id) = ac - bd + i(bc + ad)

чисел, образующая векторное пространство размерностью четыре над полем вещественных чисел. Обычно обозначаются символом H. Предложены Уильямом Гамильтоном в 1843 году.

вращения

Y

X

X = 1*cos60°

Y = 1*sin60°

X^2 + Y^2 + Z^2 = 1

W = 1 т.к. cos0 = 1
X = 1 * 1/2 = 0,5
Y = 1* 3^0,5 / 2 ~= 0.3048
Z = 0, т.к. По Z нет вращения.
Кватернион будет (1 + 0,5i + 0,3048j + 0k)

https://www.wolframalpha.com/input/?i2d=true&i=quaternion%3A1%2B0%5C%28%2C%295i%2B0%5C%28%2C%293048j%2B0k

0,707j + 0k

X^2 + Y^2 + Z^2 = 1

т.к. cos0 = 1, то вращения нет
cos Y = sin (Pi/2 - X) = 0,707
sin Y = - 0,707
Z = 0 т.к. sin Z = 0
Угол будет 45° вправо от оси X

https://www.wolframalpha.com/input/?i2d=true&i=quaternion%3A1%2B0%5C%2844%29707i-0%5C%2844%29707j%2B0k

Y

X

0,707j + 0,5k

X^2 + Y^2 + Z^2 = 1

т.к. cos0 = 1, то вращения нет
cos Y = sin (Pi/2 - X) = 0,707
sin Y = - 0,707
sin Z = 0,5 Z = 30
Угол будет 45° вправо от оси X и 30° вверх от горизонта

https://www.wolframalpha.com/input/?i2d=true&i=quaternion%3A1-0%5C%28%2C%29707i%2B0%5C%28%2C%29707j%2B0%5C%28%2C%295k

jc + dk == H, где i*j*k = -1
или {w + x + y + z}

Сложение кватернионов = "смесь" вращений, т.е. мы получим вращение, которое находится между q и q'
q + q' = [ x + x', y + y', z + z', w + w' ]

Умножение кватернионов = последовательное
вращение сначала на q, потом на q'
Скалярное произведение полезно тем, что дает косинус половины угла между двумя кватернионами
q•q' = x*x' + y*y' + z*z' + w*w'

odom.pose.pose.orientation.z + odom.pose.pose.orientation.x * odom.pose.pose.orientation.y)
t2 = +1.0 - 2.0 * (odom.pose.pose.orientation.y ** 2 + odom.pose.pose.orientation.z**2)
return math.degrees(math.atan2(t1, t2))

Для простоты мы будем использовать только один перевод из кватерниона в угол Theta

топик /scan
Читаем массив scan.ranges
Выбираем нулевой элемент
Выводим его значения на экран

Робот транспортир
Подписываемся на топик /odom
Читаем структуру Odometry
Преобразуем углы из кватернионов в углы Эйлера
Выводим значение угла по оси Z на экран

nav_msgs.msg import Odometry
rospy.init_node('transportir')
def callback(odom):
t1 = +2.0 * (odom.pose.pose.orientation.w * odom.pose.pose.orientation.z + odom.pose.pose.orientation.x * odom.pose.pose.orientation.y)
t2 = +1.0 - 2.0 * (odom.pose.pose.orientation.y ** 2 + odom.pose.pose.orientation.z**2)
print(math.degrees(math.atan2(t1, t2)))
rospy.Subscriber("/odom", Odometry, callback)
rospy.spin()

math
from sensor_msgs.msg import LaserScan
rospy.init_node('dalnomer')
def callback(scan):
print(scan.ranges[0])
rospy.Subscriber("/scan", LaserScan, callback)
rospy.spin()

Arduino Mega. Для этого микроконтроллера вы можете самостоятельно разрабатывать скетчи, и загружать их на плату TurtleBro при помощи Arduino IDE.

Для работы с различной периферией (микроконтроллеры) в составе ROS существует специальный пакет rosserial. Он позволяет подключить к ядру roscore на Raspberry микроконтроллер через Serial порт и взаимодействовать с ним как с полноценной нодой ROS.

Для работы с Arduino через ROS необходимо установить библиотеку ros_lib . Для этого надо на роботе собрать эту библиотеку:

rosrun rosserial_arduino make_libraries.py .

Дальше надо перенести собравшуюся библиотеку с робота на компьютер и подключить ее к Arduino IDE стандартным способом в
/snap/arduino/61/libraries/

Arduino IDE:

sudo chmod 777 /dev/ttyUSB0

File -> Examples -> Basic -> Blink

Board: Atmega 2560
Port: USB0

2. В терминале локального компьютера:

Upload ->

Подключаем USB провод от локального компа к роботу в порт Arduino

115200){};
};
ros::NodeHandle_ nh;

Для подключения к ROS со стороны Arduino необходимо инициализировать библиотеку ros_lib отвечающую за коммуникацию между Arduino и ROS, указав параметры Serial1 и скорость 115200.

NewHardware():ArduinoHardware(&Serial1, 115200){};
};
ros::NodeHandle_ nh;
std_msgs::String str_msg;
ros::Publisher chatter("chatter", &str_msg);
char hello[13] = "hello world!";

chatter.publish( &str_msg );
nh.spinOnce();
delay(1000);
};

NewHardware():ArduinoHardware(&Serial1, 115200){};
};
ros::NodeHandle_ nh;
void messageCb( const std_msgs::Empty& toggle_msg){
digitalWrite(13, HIGH-digitalRead(13)); // blink the led
}
ros::Subscriber sub("toggle_led", &messageCb );

delay(1);
}

ардуино

#include
#include
class NewHardware : public ArduinoHardware
{
public:
NewHardware():ArduinoHardware(&Serial1, 115200){};
};
ros::NodeHandle_ nh;
std_msgs::Int16 button_pressed;
ros::Publisher pub("/stop_topic", &button_pressed);
int inPin = 24;
void setup() {
nh.initNode();
nh.advertise(pub);
pinMode(inPin, INPUT);
}
void loop() {
button_pressed.data = digitalRead(inPin);
pub.publish(&button_pressed)
nh.spinOnce();
delay(100)}

python

import rospy
from std_msgs.msg import Int16
from geometry_msgs.msg import Twist
rospy.init_node("stoper_node")
pub = rospy.Publisher("/cmd_vel", Twist, queue_size = 10)
def stopcb(msg)
pub.publish(Twist())
rospy.Subscriber("/stop_topic", Int16, stopcb)
rospy.spin()

для Ардуино

#include "Arduino.h"
#include
#include
#include
class NewHardware : public ArduinoHardware{
public:
NewHardware():ArduinoHardware(&Serial1, 115200){};
};
ros::NodeHandle_ nh;
int angle_of_44_servo;
Servo servo44;
void messageCb( const std_msgs::Int16& msg){
angle_of_44_servo = msg.data;
servo44.write(angle_of_44_servo);
}
ros::Subscriber sub("/servo_cmd_echo", &messageCb );
void setup(){
nh.initNode();
nh.subscribe(subServo);
servo44.attach(44);servo44.write(45);
}
void loop(){
nh.spinOnce();
delay(10);}

для Python

import rospy
from std_msgs.msg import Int16
rospy.init_node("servo_node")
pub = rospy.publisher("/servo_cmd_echo", Int16, queue_size = 10)
rospy.sleep(1)
angle = Int16
angle.data = 90
pub.publish(angle)

std_msgs.msg import ByteMultiArray
class LedBlink():
def __init__(self): #init ROS node
self.pub = rospy.Publisher("/toggle_led", ByteMultiArray, queue_size=1)
rospy.init_node('turtledblink')
rospy.loginfo("Hello from PUB node")
self.number_of_LEDs = 24
self.minrange = 0.2 #distance in m - Full Red
self.midrange = 0.8 #Full yellow
self.maxrange = 1 #Full green
self.minimum_of_each_sector_of_laser_scan_array = []
self.filled_LED_RGB_array = [0 for i in range(self.number_of_LEDs * 3)]
rospy.Subscriber('/scan', LaserScan, self.laser_callback)

= []
for i in range(self.number_of_LEDs):
self.minimum_of_each_sector_of_laser_scan_array.append(
min(msg.ranges[i*(len(msg.ranges)/self.number_of_LEDs)
:(i + 1)*(len(msg.ranges)/self.number_of_LEDs)]))

= []
for k in range(len(array_of_minimums)):
if 0 <= array_of_minimums[k] < self.minrange:
array_of_colors_for_led.append(127)
array_of_colors_for_led.append(0)
array_of_colors_for_led.append(0)
elif self.minrange <= array_of_minimums[k] < self.midrange:
array_of_colors_for_led.append(127)
array_of_colors_for_led.append(int(round(
(array_of_minimums[k]-self.minrange)/self.midrange*127)))
array_of_colors_for_led.append(0)
elif self.minrange <= array_of_minimums[k] < self.maxrange:
array_of_colors_for_led.append(int(round(
(self.maxrange-array_of_minimums[k])/
(self.maxrange-self.midrange)*127)))
array_of_colors_for_led.append(127)
array_of_colors_for_led.append(0)

return array_of_colors_for_led
def LED_publisher(self,array_of_leds_colors):
leds_colors = ByteMultiArray()
leds_colors.data = array_of_leds_colors
self.pub.publish(leds_colors)
def controller(self):
self.LED_publisher(
self.color_definition(self.minimum_of_each_sector_of_laser_scan_array))

= LedBlink() #class invoker
while not rospy.is_shutdown():
l.controller()
rospy.sleep(0.1)

30
#define NUM_LEDS 24
#define BRIGHTNESS 200
CRGB leds[NUM_LEDS];

public:
NewHardware():ArduinoHardware(&Serial1, 115200){};
};
void messageCb(const std_msgs::ByteMultiArray& arrscan)
{
int pincolorred;
int pincologreen;
int pincolorblue;
int i = 0;

arrscan.data[i*3];
pincolorgreen = arrscan.data[i*3+1];
pincolorblue = arrscan.data[i*3+2];
leds[23-i].r = pincolorred*2;
leds[23-i].g = pincolorgreen*2;
leds[23-i].b = pincolorblue*2;
FastLED.show();
}
}
ros::NodeHandle_ nh;
ros::Subscriber sub("toggle_led", &messageCb);

delay
FastLED.addLeds(leds, NUM_LEDS);
FastLED.setBrightness( BRIGHTNESS );
nh.initNode();
nh.subscribe(sub);
}
void loop()
{
nh.spinOnce();
delay(1);
}

с непрерывным множеством состояний методами и средствами телемеханики. Телеуправление используется в различных технических устройствах. Одним из первых устройств, использующих технологию телеуправления, является телеграф. Сейчас самым распространенным средством телеуправления можно считать пульт дистанционного управления.

прошивку
Подключить джойстик к компьютеру, и выдать к устройству джойстика права доступа
Настроить ROS_MASTER_URI и ROS_HOSTNAME на того робота которым мы хотим управлять
Запустить ноду джойстика

перемещаться из одного места в другое, избегая столкновения с препятствиями, на основании данных с датчиков робота(внешних датчиков). При этом цель перемещения может быть выбрана как человеком, так и самим роботом. Для осуществления навигации роботам требуются следующие компоненты: Карта, Текущее положение, Путь

широкое распространение получили алгоритмы
GMapping - лидар
Cartographer - лидар + GPS + RGBD + стерео
Rtabmap - стерео + RGBD

ноду gmapping на роботе
roslaunch turtlebro_navigation turtlebro_gmapping.launch
В rviz добавим отображение карты
Add->By Topic->/map->Map

имя файла

После выполнения данной команды, будет создано два файла map.yaml и map.pgm
В файле map.yaml находятся параметры карты. В map.pgm - собственно изображение карты

“А где робот на этой карте находится?” - Ответ на этот вопрос дает локализация
Решения этой задачи возможно при помощи алгоритма AMCL (Adaptive Monte Carlo Localization / адаптивного алгоритма локализации Монте-Карло)
Принцип работы AMCL заключается в том, что алгоритм предполагает множество позиций, где может находиться робот. По мере движения робота, алгоритм сопоставляет «очертания карты» и данные с лидара. Таким образом, постепенно, множество предполагаемых позиций робота сходится к локальной точке, считающейся истинным местоположением робота в пространстве.

map.yaml и map.pgm. Файлы с информацией о карте, необходимо скопировать в директорию робота /home/pi/ros_catkin_ws/src/turtlebro_navigation/maps После этого необходимо пересобрать пакет навигации.
sudo ./src/catkin/bin/catkin_make_isolated --install -DCMAKE_BUILD_TYPE=Release --install-space /opt/ros/melodic --pkg=turtlebro_navigation
Установите робота в нулевую точку карты и очистите данные Одометрии
‌Запустим ноду amcl
roslaunch turtlebro_navigation turtlebro_amcl.launch

перемещаться из одного места в другое, избегая столкновения с препятствиями, на основании данных с датчиков робота(внешних датчиков). При этом цель перемещения может быть выбрана как человеком, так и самим роботом. Для осуществления навигации роботам требуются следующие компоненты: Карта, Текущее положение, Путь

программа rviz, которая служит для визуализации информации из различных систем ROS. В том числе, она может показать данные, которые передает лидар.

Запустим программу из консоли:
rviz

точек с данными расстояния от центра лидара до препятствия, которое встретилось на пути лазерного луча. Каждая из 360 точек соответствует одному градусу оборота лидара.
“Сырые” данные с лидара находятся в топике scan с типом данных LaserScan

header:
seq: 1998463
stamp:
secs: 1628508496
nsecs: 100944372
frame_id: "base_scan"
angle_min: -3.12413907051
angle_max: 3.14159274101
angle_increment: 0.0174532923847
time_increment: 0.000356173579348
scan_time: 0.127866312861
range_min: 0.15000000596
range_max: 12.0
ranges: [inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, 0.1379999965429306, 0.14149999618530273, inf, 0.1457500010728836, 0.1537500023841858, inf, 0.1667499989271164, inf, 0.18250000476837158, 0.20225000381469727, inf, 0.2329999953508377, inf, inf, 0.26524999737739563, inf, inf, inf, inf, inf, 0.34424999356269836, inf, inf, 0.4137499928474426, inf, 0.4129999876022339, inf, 0.4127500057220459, 0.41225001215934753, 0.4115000069141388, inf, 0.4115000069141388, 0.41324999928474426, inf, 0.414000004529953, 0.4137499928474426, inf, 0.4154999852180481, 0.41725000739097595, 0.4189999997615814, inf, 0.42124998569488525, 0.4242500066757202, inf, 0.42649999260902405, inf, 0.4300000071525574, 0.4332500100135803, 0.43700000643730164, inf, 0.4415000081062317, 0.44624999165534973, inf, 0.45100000500679016, 0.45649999380111694, inf, 0.4622499942779541, 0.4684999883174896, inf, 0.47574999928474426, 0.4830000102519989, inf, 0.4909999966621399, 0.49924999475479126, inf, 0.5074999928474426, 0.5184999704360962, inf, 0.5287500023841858, 0.5410000085830688, inf, 0.5537499785423279, 0.5669999718666077, inf, 0.5827500224113464, inf, 0.5982499718666077, 0.6157500147819519, 0.6355000138282776, inf, 0.656000018119812, inf, 0.6779999732971191, 0.703499972820282, inf, 0.7294999957084656, 0.7609999775886536, inf, 0.7952499985694885, 0.8307499885559082, inf, 0.871749997138977, 0.9182500243186951, inf, 0.96875, 1.0497499704360962, inf, inf, 1.159000039100647, inf, 1.2607500553131104, inf, 1.3697500228881836, 1.4902499914169312, inf, 1.6449999809265137, 1.8350000381469727, inf, 2.066999912261963, 2.4547500610351562, inf, 2.3559999465942383, inf, inf, inf, 2.3434998989105225, inf, 2.336750030517578, 2.33174991607666, 2.335249900817871, inf, 2.323499917984009, 2.335750102996826, inf, 2.336250066757202, 2.3389999866485596, inf, 2.3494999408721924, 2.3524999618530273, inf, 2.3615000247955322, 2.375, inf, 2.3935000896453857, 2.4094998836517334, 2.4327499866485596, inf, 2.4514999389648438, 2.4755001068115234, inf, 2.484999895095825, 2.513000011444092, inf, 2.5315001010894775, 2.5577499866485596, inf, 2.591249942779541, 2.6232500076293945, inf, 2.645250082015991, 2.700000047683716, 2.5959999561309814, inf, 2.5137500762939453, 2.4114999771118164, inf, 2.321000099182129, 2.243000030517578, inf, 2.177500009536743, 2.119499921798706, inf, 2.0577499866485596, 2.005500078201294, 1.9587500095367432, inf, 1.9142500162124634, 1.8769999742507935, inf, 1.8365000486373901, 1.874750018119812, inf, 1.75600004196167, 1.725000023841858, 1.687749981880188, inf, 1.659250020980835, 1.6352499723434448, inf, 1.6030000448226929, 1.5839999914169312, inf, 1.562999963760376, 1.5437500476837158, inf, 1.5237499475479126, 1.5077500343322754, 1.4915000200271606, inf, 1.4739999771118164, 1.46274995803833, inf, 1.4527499675750732, 1.437000036239624, inf, 1.4294999837875366, 1.4140000343322754, 1.4105000495910645, inf, 1.4049999713897705, 1.3969999551773071, inf, 1.3940000534057617, 1.3912500143051147, inf, 1.3880000114440918, 1.3839999437332153, inf, 1.377500057220459, 1.3819999694824219, inf, 1.378999948501587, inf, inf, inf, inf, inf, inf, 0.5249999761581421, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, 0.14124999940395355, 0.13899999856948853, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf]
intensities: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 13.0, 15.0, 0.0, 15.0, 15.0, 0.0, 15.0, 0.0, 15.0, 15.0, 0.0, 11.0, 0.0, 0.0, 15.0, 0.0, 0.0, 0.0, 0.0, 0.0, 15.0, 0.0, 0.0, 15.0, 0.0, 15.0, 0.0, 15.0, 15.0, 15.0, 0.0, 15.0, 15.0, 0.0, 15.0, 15.0, 0.0, 15.0, 15.0, 15.0, 0.0, 15.0, 15.0, 0.0, 15.0, 0.0, 15.0, 15.0, 15.0, 0.0, 15.0, 15.0, 0.0, 15.0, 15.0, 0.0, 15.0, 15.0, 0.0, 15.0, 15.0, 0.0, 15.0, 15.0, 0.0, 15.0, 15.0, 0.0, 15.0, 15.0, 0.0, 15.0, 15.0, 0.0, 15.0, 0.0, 15.0, 15.0, 15.0, 0.0, 15.0, 0.0, 15.0, 15.0, 0.0, 15.0, 15.0, 0.0, 15.0, 15.0, 0.0, 15.0, 15.0, 0.0, 15.0, 15.0, 0.0, 0.0, 15.0, 0.0, 15.0, 0.0, 15.0, 15.0, 0.0, 15.0, 15.0, 0.0, 15.0, 15.0, 0.0, 15.0, 0.0, 0.0, 0.0, 15.0, 0.0, 15.0, 15.0, 15.0, 0.0, 15.0, 15.0, 0.0, 15.0, 15.0, 0.0, 15.0, 15.0, 0.0, 15.0, 15.0, 0.0, 15.0, 15.0, 15.0, 0.0, 15.0, 15.0, 0.0, 15.0, 15.0, 0.0, 15.0, 15.0, 0.0, 15.0, 15.0, 0.0, 15.0, 15.0, 15.0, 0.0, 15.0, 15.0, 0.0, 15.0, 15.0, 0.0, 15.0, 15.0, 0.0, 15.0, 15.0, 15.0, 0.0, 15.0, 15.0, 0.0, 15.0, 15.0, 0.0, 15.0, 15.0, 15.0, 0.0, 15.0, 15.0, 0.0, 15.0, 15.0, 0.0, 15.0, 15.0, 0.0, 15.0, 15.0, 15.0, 0.0, 15.0, 15.0, 0.0, 15.0, 15.0, 0.0, 15.0, 15.0, 15.0, 0.0, 15.0, 15.0, 0.0, 15.0, 15.0, 0.0, 15.0, 15.0, 0.0, 15.0, 15.0, 0.0, 15.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 15.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 15.0, 15.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]

"человеческом" виде в программе rviz
Нажмем Add, выберем вкладку By topic, выбрать источник данных /scan/LaserScan
Изменить Fixed Frame на вкладке Global Option указав точку отсчета - базу робота base_footprint

данных о фреймах робота

Add->By display type->TF

Сохранение настроек rviz

File -> Save As

позволяет управлять роботом прямо из браузера. Откройте в браузере адрес http://ip_робота:8080
Управление роботом выполняется клавишами AWSD
В настройках веб приложения также видна основная телеметрия робота, и есть возможность менять скорости передвижения робота.

видео необходимое для работы web интерфейса. Для этого используется launch файл camera_stream.launch, который запускает ROS ноду mjpg_camera
Вы не можете использовать изображение с камеры для обработки в случае если включен веб-интерфейс, т.к. две ноды не могут одновременно работать с камерой. Нужно выключить веб интерфейс run_turtlebro_web и включить run_uvc_camera
Для запуска ноды камеры, которая транслирует данные в РОС необходимо переконфигурировать файл запуска /etc/ros/melodic/turtlebro.d/turtlebro.launch в части:


либо отключить запуск ноды камеры веб-интерфейса и вручную запустить ноду “РОС-камеры”
roslaunch turtlebro uvc_camera.launch

front_camera/compressed
‌Официальная документация пакета http://wiki.ros.org/uvc_camera
Данный пакет работает с камерой через библиотеку ОpenCV
Конфигурация в файле cv_camera.launch данные в формате sensor_msgs/Image в топик front_camera/image_raw Данные передаются в RAW формате (без компрессии), что удобно для дальнейшей программной обработки.
‌
Конвертация из sensor_msgs/Image в формат OpenCV возможна через библиотеку http://wiki.ros.org/cv_bridge

с камерой можно работать напрямую, подключившись к камере "стандартной" для opencv функцией вида cap = cv2.VideoCapture(0)
‌
Далее производить с видео все необходимые манипуляции, и после этого, при необходимости, публиковать видео в топики.
‌

поворота робота за шариком реализуют 2 модуля.
Первый анализирует изображение и определяет на нем местонахождение шарика, а второй доворачивает робота таким образом, чтобы изображение шарика было по центру картинки.
Сложные алгоритмы компьютерного зрения это умение загрузить изображение, изменить его размер, цвет и просто вырезать интересный нам фрагмент. Вы удивитесь, как много можно всего достичь, используя самые простые средства.
Давайте подробнее разберем код программы которая это делает

разберемся как работает модуль определения местоположения шарика на кадре. Импортируем библиотеки OpenCV и numpy для работы с данными. Фактически изображение это массив чисел, описывающих каждый пиксель для этого и нужен NumPy.

всю работу по определению места шарика на картинке

class BallProcessing:
def __init__(self):
self.yellowLower = (14, 180, 200)# dark
self.yellowUpper = (34, 255, 255) # light
self.font = cv2.FONT_HERSHEY_SIMPLEX
self.bottomLeftCornerOfText = (30,50)
self.fontScale = 0.5
self.fontColor = (255,255,255)
self.lineType = 1
self.current_data = {"obj_x":0,"obj_y":0, "obj_r":0}

Границы цвета

Параметры текста, для наложения на картинку

которая будет искать шарик

def process(self, frame):
Преобразуем изображение из формата BGR в HSV, для задач определения цвета он подходит лучше
hsv = cv2.cvtColor(frame, cv2.COLOR_BGR2HSV)

черным все, что не попадает в цветовые границы
mask = cv2.inRange(hsv, self.yellowLower, self.yellowUpper)

mask = cv2.erode(mask, None, iterations=2)
mask = cv2.dilate(mask, None, iterations=2)

маске. Контуры это границы объекта с одинаковой интенсивностью света. Нас интересуют только внешние контуры, поэтому cv2.RETR_EXTERNAL, и сохраним их в переменную cnts, c использованием cv2.CHAIN_APPROX_SIMPLE - простой отрисовки.
cnts = cv2.findContours(mask.copy(), cv2.RETR_EXTERNAL,
cv2.CHAIN_APPROX_SIMPLE)[-2]

итерацией
center = None
self.current_data = {"obj_x":0,"obj_y":0, "obj_r":0}
И если контуры найдены:
if len(cnts) > 0:
В промежуточную переменную с передается весьма интересная конструкция. С одной стороны это знакомая нам функция max, а с другой стороны в качестве именованного аргумента key = ей передается кастомная функция сортировки cv2.contourArea. В итоге на выходе в переменной с у нас будет контур с самой большой площадью. Довольно элегантно
c = max(cnts, key=cv2.contourArea)

((x, y), radius) = cv2.minEnclosingCircle(c) и опишем окружность минимального радиуса вокруг нашего контура. В принципе на этом можно было бы и остановиться, т.к. координаты центра и радиус у нас уже есть, но мы пойдем чуть дальше.

графических моментов,
M = cv2.moments(c)
center = (int(M["m10"] / M["m00"]), int(M["m01"] / M["m00"]))
Графический момент это чисто математический объект. Он определяется формулой.

Где:
i и j — порядки момента M, соответствующие координатам изображения.
I(x,y) - интенсивность пикселя
x,y - координаты пикселя

определение из Википедии:

Моменты изображения (англ. image moments) в компьютерном зрении, обработке изображений и смежных областях — некоторые частные средневзвешенные (момент) интенсивностей пикселей изображения, или функция таких моментов. Как правило, выбираются моменты, имеющие полезные свойства или интерпретации.
В самом общем смысле момент функции — это некая скалярная величина, которая характеризует эту функцию и может быть использована для артикуляции ее важных свойств. С математической точки зрения набор моментов является в некотором смысле «проекцией» функции на полиномиальный базис. Он аналогичен преобразованию Фурье, которое представляет из себя проекцию функции на базис из гармонических функций[1].
Моменты изображения полезны для описания объектов после сегментации. Простые свойства изображения, которые можно найти с помощью моментов, включают в себя площадь (или суммарную интенсивность), геометрический центр и информацию об ориентации. Кроме них в математической статистике давно применяются моменты более высоких порядков, например коэффициент асимметрии и коэффициент эксцесса[1].

первый компонент центра M["m10"] / M["m00"] , подставим его в эту формулу и применим ее ко всем пикселям нашей маски.

center = (int(M["m10"] / M["m00"]), int(M["m01"] / M["m00"]))

Теперь по русски. Возьмем определение центра изображения по графическим моментам и посмотрим что же это такое.

Что такое M["m10"]? Это сумма интенсивностей всех пикселей нашей картинки умноженная на их X-координату. А M["m00"] это просто сумма интенсивностей всех пикселей без учета координаты. Как следствие их частное будет давать Х- координату некой точки в которой суммарная интенсивность максимальная. С У-координатой тоже самое.

примере вот такого изображения.

Посчитаем Х-компоненту M["m10"] / M["m00"]:
x в степени 1 - это х, y в степени 0 - это 1, I = 0 для черного и I=1 для белого пикселя.
Т.е. по формуле
Получим для M["m10"] =
0*1*1 + 0*2*1 + 0*3*1 + 0*4*1 +
0 + 0 + 1*3*1+ 0 +
0 + 0 + 0 + 0 +
0 + 0 + 0 + 0 + = 3

А для M["m00"] = везде 0, кроме одного пикселя, где 1*1*1

Таким образом M["m10"] / M["m00"] = 3 / 1 = 3 Т.е Х-компонента центра будет 3. Магия!
С Y-координатой тоже самое.

0

Y

X

мы рассчитали центр изображения нашего шарика и теперь можем нанести на изображение текст с координатами центра, контур описывающей окружности и вместе с маской вернуть все обратно в первый файл.
if radius > 10:
self.current_data = {"obj_x": int(x),"obj_y": int(y), "obj_r": int(radius)}
cv2.circle(frame, (int(x), int(y)), int(radius),(0, 255, 255), 2)
cv2.circle(frame, center, 5, (0, 0, 255), -1)
cv2.putText(frame,"({:d},{:d},{:d})".format(int(x),int(y),int(radius)),
(int(x), int(y)),
self.font,
self.fontScale,
self.fontColor,
self.lineType)
return mask, frame

Переменные из инита нашего класса

класса, которая будет возвращать переменную self.current_data, которая имеет вид, вот такого словаря:
self.current_data = {"obj_x":0,"obj_y":0, "obj_r":0}
def get_current_data(self):
return self.current_data
Данную функцию мы используем в другом файле, чтобы определять куда поворачивать нашего робота.
params = bp.get_current_data()
if(params['obj_x'] > 0 and params['obj_y'] > 0 and params['obj_r'] > 5):
if(params['obj_x'] > 400):
move_cmd.angular.z = -angular_speed

cv2
import numpy as np

Теперь разберем код модуля получающего картинку с камеры и управляющего роботом
Импортируем библиотеки OpenCV для получения изображения, numpy для работы с данными и rospy

данных, которые мы будем использовать в РОС и создадим издателей, для публикации изображения, отредактированного изображения и команд роботу на движение

from sensor_msgs.msg import CompressedImage
from geometry_msgs.msg import Twist
from ball_processing import BallProcessing
rospy.init_node('demo_ball_tracking')
image_pub = rospy.Publisher("image/compressed", CompressedImage, queue_size=1)
mask_pub = rospy.Publisher("mask/compressed", CompressedImage, queue_size=1)
cmd_vel = rospy.Publisher('cmd_vel', Twist, queue_size=5)
angular_speed = 0.4

Кроме того введем переменную угловой скорости, будем подбирать ее значение

BallProcessing()

Создадим экземпляр класса BallProcessing, который будет обрабатывать полученное изображение и возвращать нам позицию шарика. Сам класс мы напишем в другом файле

cap = cv2.VideoCapture(0)
cap.set(3,640)
cap.set(4,480)

Метод VideoCapture() библиотеки OpenCV, получит класс изображения из нулевой камеры нашего робота. Метод set класса изображения, принимает 2 аргумента, первый - номер характеристики изображения, которую надо изменить и второй значение этой характеристики. Номера характеристик для ширины и высоты изображения 3 и 4 соответственно. В нашем случае камера выдает разрешение 640X480 пикселей, именно этот размер мы и зададим. Полный список номеров характеристик изображения доступен тут:
https://docs.opencv.org/3.4/d4/d15/group__videoio__flags__base.html#gaeb8dd9c89c10a5c63c139bf7c4f5704d

rospy.is_shutdown():

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

ret,im = cap.read()

Функция read() возвращает true в качестве первого аргумента, если удалось получить изображение, и в качестве второго аргумента возвращает собственно изображение для дальнейшей обработки. Im - это и есть переменная в которую мы сохраним изображение захваченного кадра.

изображение в модуль BallProcessing, и на выходе получим обработанное изображение frame, с нанесенными координатами шарика и mask - черно-белое изображение отфильтрованное по маске цвета, об этом подробнее чуть позже.

frame_msg = CompressedImage()
frame_msg.header.stamp = rospy.Time.now()
frame_msg.format = "jpeg"
frame_msg.data = np.array(cv2.imencode('.jpg', frame)[1]).tostring()
image_pub.publish(frame_msg)

mask, frame = bp.process(im)

Полученные из модуля изображения опубликуем при помощи ранее созданных издателей в соответствующие топики.

Заполним структуру данных CompressedImage соответствующими значениями и опубликуем ее

cv2.imencode('.jpg', frame)[1])

Кодирует изображение в буфер в памяти

np.array().tostring()

Превращает буфер из памяти в структуру numpy-массива

у BallProcessing результаты обработки изображения с координатами шарика.

params = bp.get_current_data()

А дальше на основании координат шарика будем заполнять структуру Twist значением угловой скорости (направо или налево) и публиковать ее при помощи издателя

move_cmd = Twist()
if(params['obj_x'] > 0 and params['obj_y'] > 0 and params['obj_r'] > 5):
if(params['obj_x'] > 400):
move_cmd.angular.z = -angular_speed
if(params['obj_x'] < 240):
move_cmd.angular.z = angular_speed
cmd_vel.publish(move_cmd)

передает его второму, второй чистит изображение, находит на нем шарик, и передает очищенное изображение и координаты найденного шарика обратно в первый файл. А тот уже публикует изображение в РОС и доворачивает робота направо, если шарик справа от центра изображения и налево, если слева.
Итак мы с вами быстро познакомились с основами компьютерного зрения на базе библиотеки OpenCV и посмотрели как быстро можно сопрячь ее с реальным роботом. В целом в библиотеке OpenCV есть много полезных функций, такие как распознавание лиц, улыбок, глаз, рук и т.д. В соответствии с принципами изложенными в этом упражнении вы можете использовать их в своем проекте.

некоторую территорию по заданным пользователем точкам. Точки для патрулирования можно задавать при помощи конфигурационного файла. Робот управляется при помощи команд типа std_msgs/String публикуемых в топик /patrol_control.
Принимаемые команды:
next - стартует цикл патрулирования или перенаправляет робота на следующую точку.
pause - заставляет робота сделать паузу в цикле патрулирования
shutdown - останавливает патрулирование и выходит из пакета.

робота:
cd ros_catkin_ws/src
git clone https://github.com/voltbro/turtlebro_patrol
cd ..
sudo ./src/catkin/bin/catkin_make_isolated --install -DCMAKE_BUILD_TYPE=Release --install-space /opt/ros/melodic --pkg=turtlebro_patrol

одометрии, чтобы зафиксировать начальную точку координат.
rosservice call /reset
Запуск навигации и пакета патрулирования одновременно
roslaunch turtlebro_patrol patrol.launch
Запуск только пакета патрулирования
roslaunch turtlebro_patrol patrol_run.launch

задание для робота, надо отредактировать файл с точками ~/ros_catkin_ws/src/turtlebro_patrol/data/goals.xml
В следующем формате:

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

полигона используется OpenVPN
sudo apt install openvpn
Администратор полигона предоставит вам ключ. При помощи этого ключа и OpenVPN, надо подключиться к полигону и не закрывать окно терминала с OpenVPN до окончания работы с удаленным полигоном
sudo openvpn --config <путь до файла с ключем>
Тест подключения
Initialization Sequence Completed
ping 10.8.0.1
ping 10.8.0.6