Урок 10
Введение: Данная задача, является продолжением задач из серии задач в которой мы будем интегрировать ROS и Arduino. В этой задаче мы рассмотрим как при помощи данных имеющихся в РОСе управлять периферией на низком уровне. А именно как при помощи данных лидара, зажигать соответствующие светодиоды на LED-ленте, подключенной к Arduino.
Подготовка:
Аналогична предыдущей задаче. Если вы создавали какие-то сообщения, которые хотите использовать в ваших скетчах под Arduino, актуализируйте библиотеку ros_lib в соответствии с инструкцией и убедитесь что Arduino подключена к Raspberry. Кроме того, дополнительно для решения именно этой задачи, необходимо установить библиотеку FastLED в Ардуино IDE. Для установки этой библиотеки воспользуйтесь стандартным механизмом поиска и добавления библиотек через менеджер библиотек Arduino, в нем есть несколько вариантов этой библиотеки, мы рекомендуем вот эту версию, она должна быть первой в списке поиска.
Для проверки правильности работы и верности установки библиотеки FastLED, попросите учащихся загрузить на своих роботов тестовую прошивку включающую светодиоды. После загрузки проверьте работу светодиодной ленты по внешней камере полигона. Лента должна переливаться RGB цветами.
Задача: Написать программу для РОС и прошивку для Ардуино, чтобы при приближении к роботу объектов определяемых лидаром, светодиоды LED-ленты зажигались соответствующим цветом. Близко - красным, в средней зоне желтым, далеко - зеленым.
Общий алгоритм решения:
Решение будет состоять из двух программ - одна для raspberry под ROS, вторая для Arduino. Программа на raspberry будет читать данные лидара и готовить данные для Arduino, а программа для Arduino будет читать подготовленные для нее данные из топика и при помощи библиотеки для общения с LED-лентой, зажигать светодиоды.
Программа для РОС
- Берем массив точек с лидара, нарезаем его на 24 сектора, соответствующие количеству светодиодов
- В каждом секторе определяем минимальное значение, и определяем для него цвет светодиода.
- Собираем массив из 24-х значений цветов для светодиодов и передаем его в топик на который подписана Arduino.
Скетч для Arduino:
1. Настраиваем связь Arduino и ROS, в соответствии с инструкцией к роботу
2. В основном цикле топик в который передается массив с цветом светодиодов
3. Передаем это значение на LED-ленту при помощи библиотеки FAST_LED
Решение:
Программа для ROS: Для начала импортируем все библиотеки и структуры данных, которые нам понадобятся.
import rospy
from sensor_msgs.msg import LaserScan
from std_msgs.msg import BInt16MultiArray
Создадим класс LedBlink, в котором будем реализовывать всю логику.
def __init__(self):
self.pub = rospy.Publisher("/led_line", ByteMultiArray, queue_size=1)
NUMBER_OF_LEDS = 24
MINRANGE = 0.4 #distance in m - Full Red
MIDRANGE = 1 #Full yellow
MAXRANGE = 1.4 #Full green
self.minimum_of_each_sector_of_laser_scan_array = [0 for i in range(self.NUMBER_OF_LEDS)]
rospy.Subscriber('/scan', LaserScan, self.laser_callback)
rospy.loginfo("Init done")
Создадим экземпляр Издателя, который будет публиковать результаты обработки данных лидара в топик "led_line". Для публикации мы будем использовать байтовый массив Int16MultyArray, для согласования размерности кодировки цветов на стороне РОС и на стороне Arduino. А так же подписчика на топик /scan, чтобы получать данные для анализа.
Объявим несколько констант:
NUMBER_OF___LEDS - количество светодиодов - для того, чтобы разрезать весь scan.ranges на равное этому количеству число секторов.
MIN, MID и MAXRANGE - это пороговые значения для наших цветов. Т.е. больше максимума - зеленый, меньше минимума - красный, посередине - желтый.
self.filled_LED_RGB_array = [0 for i in range(self.number_of_LEDs * 3)]
Это предварительное создание и заполнение нулями массива со значениями RGB для каждого светодиода, который мы и будем в дальнейшем публиковать для Arduino
Теперь опишем функцию обратного вызова. В отличие от прошлых примеров, где зачастую функция обратного вызова только лишь заполняла какую-то переменную, в этот раз мы заставим ее поработать, и разрезать массив ranges на 24 сектора, затем определить в каждом из них минимальное значение, и записать эти минимальные значения в новый массив из 24-х элементов.
def laser_callback(self, msg):
for i in range(self.number_of_LEDs):
self.minimum_of_each_sector_of_laser_scan_array[i] = min(
msg.ranges[i*(len(msg.ranges)/self.NUMBER_OF_LEDS)
:(i + 1)*(len(msg.ranges)/self.NUMBER_OF_LEDS)])
Теперь в массиве minimum_of_each_sector_of_laser_scan_array находятся минимумы расстояний до ближайших к роботу предметов.
Теперь опишем функцию-обертку над Издателем. Ее логика работы и создания очень похожа на те, которые мы уже писали. Просто передаем ей какой-то аргумент, а она сама уже заполняет нужную структуру данных и публикует
def LED_publisher(self,array_of_leds_colors):
leds_colors = Int16MultiArray()
leds_colors.data = array_of_leds_colors
self.pub.publish(leds_colors)
Теперь опишем функцию, которая из массива 24-х расстояний до объектов будет делать 24 тройки чисел, кодирующих цвет каждого светодиода в цветовой модели RGB, где значения R-красного, G-зеленого и B-голубого могут изменяться от 0 минимум, то 255 - максимум.
При этом мы будем заполнять цвета соответственно расстоянию до робота. Чем ближе тем краснее, чем дальше тем зеленее.
def color_definition(self, array_of_minimums):
array_of_colors_for_led = []
for k in range(len(array_of_minimums)):
if 0 <= array_of_minimums[k] < self.MINRANGE:
array_of_colors_for_led.append(255)
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(255)
array_of_colors_for_led.append(int(round(
(array_of_minimums[k]-self.MINRANGE)/self.MIDRANGE*255)))
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)*255)))
array_of_colors_for_led.append(255)
array_of_colors_for_led.append(0)
else:
array_of_colors_for_led.append(0)
array_of_colors_for_led.append(255)
array_of_colors_for_led.append(0)
return array_of_colors_for_led
Все что нам осталось сделать в классе, это объявить функцию контроллер, которая будет все это сводить воедино и которую мы будем вызывать в нашем основном цикле.
def controller(self):
self.LED_publisher(self.color_definition(
self.minimum_of_each_sector_of_laser_scan_array))
В ней мы берем заполненный в подписчике массив минимумов до препятствий, передаем его в функцию, которая определяет цвета, и затем результат ее работы передаем в функцию-обертку над Издателем.
Теперь инициализируем ноду, создадим экземпляр нашего класса и в цикле будем вызывать его функцию контроллер.
rospy.init_node('turtled')
l = LedBlink()
while not rospy.is_shutdown():
l.controller()
rospy.sleep(0.1)
Вот и все с программой на python. В итоге ваша программа должна будет выглядеть вот так:
import rospy
from sensor_msgs.msg import LaserScan
from std_msgs.msg import Int16MultiArray
class LedBlink():
def __init__(self):
self.pub = rospy.Publisher("/led_line", Int16MultiArray, queue_size=1)
NUMBER_OF_LEDS = 24
MINRANGE = 0.4 #distance in m - Full Red
MIDRANGE = 1 #Full yellow
MAXRANGE = 1.4 #Full green
self.minimum_of_each_sector_of_laser_scan_array = [0 for i in range(self.NUMBER_OF_LEDS)]
rospy.Subscriber('/scan', LaserScan, self.laser_callback)
rospy.loginfo("Init done")
def laser_callback(self, msg):
for i in range(self.number_of_LEDs):
self.minimum_of_each_sector_of_laser_scan_array[i] = min(
msg.ranges[i*int(round(len(msg.ranges)/self.NUMBER_OF_LEDS))
:(i + 1)*int(round(len(msg.ranges)/self.NUMBER_OF_LEDS))]))
def color_definition(self, array_of_minimums):
array_of_colors_for_led = []
for k in range(len(array_of_minimums)):
if 0 <= array_of_minimums[k] < self.MINRANGE:
array_of_colors_for_led.append(255)
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(255)
array_of_colors_for_led.append(int(round(
(array_of_minimums[k]-self.MINRANGE)/self.MIDRANGE*255)))
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)*255)))
array_of_colors_for_led.append(255)
array_of_colors_for_led.append(0)
else:
array_of_colors_for_led.append(0)
array_of_colors_for_led.append(255)
array_of_colors_for_led.append(0)
return array_of_colors_for_led
def LED_publisher(self,array_of_leds_colors):
leds_colors = Int16MultiArray()
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))
rospy.init_node('turtled')
l = LedBlink() #class invoker
while not rospy.is_shutdown():
l.controller()
rospy.sleep(0.1)
Теперь перейдем к Arduino, тут все несколько проще, потому что основная логика реализовывается на raspberry. Нам надо инициализировать все структуры данных, ноду, подписчика и подключить библиотеки ros_lib и FastLED
#include <ros.h>
#include <FastLED.h>
#include "std_msgs/Int16MultiArray.h"
#define DATA_PIN 30
#define NUM_LEDS 24
#define BRIGHTNESS 200
CRGB leds[NUM_LEDS];
class NewHardware : public ArduinoHardware
{
public:
NewHardware():ArduinoHardware(&Serial1, 115200){};
};
CRGB leds[NUM_LEDS];
Это специальный класс библиотеки FastLED, который нам и надо заполнить, чтобы наши светодиоды загорелись.
Напишем функцию обратного вызова для подписчика на обработанные на raspberry данные. В цикле мы будем последовательно читать тройки из RGB чисел, которые мы создали в Питоне, и записывать их в массив leds соответственно в r, g и b поля элементов массива. Однако в связи с тем, что ориентация осей в ROS и в FastLED разная, на придется инвертировать массив и записать первый элемент полученный в ROSе, в последний элемент массива leds и далее пройтись по обоим этим массивам но во встречном направлении. Т.к. переменная итерации цикла i у нас одна, то для массива leds мы применим инверсию по порядковому номеру т.е. в элемент под номером 23 мы будем записывать 0-й элемент массива из ROSа.
void messageCb(const std_msgs::Int16MultiArray& arrscan)
{
int pincolorred;
int pincolorgreen;
int pincolorblue;
for(int i=0; i<NUM_LEDS; i++)
{
pincolorred = arrscan.data[i*3];
pincolorgreen = arrscan.data[i*3+1];
pincolorblue = arrscan.data[i*3+2];
leds[23-i].r = pincolorred;
leds[23-i].g = pincolorgreen;
leds[23-i].b = pincolorblue;
FastLED.show();
}
}
Кроме того, в этом же цикле мы будем вызывать функцию FastLED.show(), которая и отвечает за включение светодиодов по тем параметрам, которые мы для нее заполнили.
Дальше достаточно стандартно для программ под Arduino создадим экземпляр класса соединяющего Arduino c ROS, и экземпляр класса Подписчика.
ros::NodeHandle_<NewHardware> nh;
ros::Subscriber<std_msgs::Int16MultiArray> sub("led_line", &messageCb);
В секции setup, мы сделаем 1 секундную задержку, по рекомендациям библиотеки FastLED, после чего, вызовем две функции FastLED.addLeds и FastLED.setBrightness, первая возьмет наш массив с LEDами и будет проводить с ним свою библиотечную магию, вторая установит яркость светодиодов на 200 из 255.
void setup()
{
delay(1000); // sanity delay
FastLED.addLeds<NEOPIXEL, DATA_PIN>(leds, NUM_LEDS);
FastLED.setBrightness( BRIGHTNESS );
nh.initNode();
nh.subscribe(sub);
}
Здесь же инициализируем ноду и зарегистрируем Подписчика.
Ну вот и все. Так как основная логика у нас реализована в функции обратного вызова нашего подписчика, то в основном цикле нам надо просто крутить вызовы подписчика при помощи функции nh.spinOnce().
void loop()
{
nh.spinOnce();
delay(50);
}
В итоге ваша программа для Arduino должна выглядеть следующим образом:
#include <ros.h>
#include <FastLED.h>
#include "std_msgs/Int16MultiArray.h"
#define DATA_PIN 30
#define NUM_LEDS 24
#define BRIGHTNESS 200
CRGB leds[NUM_LEDS];
class NewHardware : public ArduinoHardware
{
public:
NewHardware():ArduinoHardware(&Serial1, 115200){};
};
void messageCb(const std_msgs::Int16MultiArray& arrscan)
{
int pincolorred;
int pincolorgreen;
int pincolorblue;
int i = 0;
for(i=0;i<NUM_LEDS;i++)
{
pincolorred = arrscan.data[i*3];
pincolorgreen = arrscan.data[i*3+1];
pincolorblue = arrscan.data[i*3+2];
leds[23-i].r = pincolorred;
leds[23-i].g = pincolorgreen;
leds[23-i].b = pincolorblue;
FastLED.show();
}
}
ros::NodeHandle_<NewHardware> nh;
ros::Subscriber<std_msgs::Int16MultiArray> sub("led_line", &messageCb);
void setup()
{
delay(1000); // sanity delay
FastLED.addLeds<NEOPIXEL, DATA_PIN>(leds, NUM_LEDS);
FastLED.setBrightness( BRIGHTNESS );
nh.initNode();
nh.subscribe(sub);
}
void loop()
{
nh.spinOnce();
delay(50);
}
Проверка решения:
Попросите учащихся залить скетч для Arduino на робота по процедуре описанной в разделе подготовка, перекиньте на робота программу на Python. Попросите учащихся запустить программу на Python и при помощи веб-интерфейса перемещать робота по полигону, наблюдая как горят светодиоды в зависимости от того как близко располагаются препятствия. Проконтролируйте работу учащихся в режиме трансляции их экрана через Discord.