Управление роботом jetbot из ROS

В статье JETBOT на базе NVIDIA Jetson Nano мы рассмотрели робота jetson на на базе NVIDIA Jetson Nano. Сейчас рассмотрим установку пакетов ROS для управления и навигации на jetbot, чтобы превратить робота в удаленно управляемую движущую платформу.

Robot Operating System (ROS) - это гибкая платформа (фреймворк) для разработки программного обеспечения роботов. Это набор разнообразных инструментов, библиотек и определенных правил, целью которых является упрощение задач разработки ПО роботов.

Используем SD-карту 64 Гб, на которой записан образ NVIDIA JetPack. Образ NVIDIA JetPack основан на базе ОС Ubuntu 18.04. Будем устанавливать версию ROS Melodic (http://wiki.ros.org/melodic).

# Добавляем все репозитории Ubuntu:

sudo apt-add-repository universe
sudo apt-add-repository multiverse
sudo apt-add-repository restricted

# добавляем репозиторий ROS 

sudo sh -c 'echo "deb http://packages.ros.org/ros/ubuntu $(lsb_release -sc) main" > /etc/apt/sources.list.d/ros-latest.list'
sudo apt-key adv --keyserver 'hkp://keyserver.ubuntu.com:80' --recv-key C1CF6E31E6BADE8868B172B4F42ED6FBAB17C654

# установим ROS Base

sudo apt-get update
sudo apt-get install ros-melodic-ros-base

# добавим пути ROS 

sudo sh -c ' echo "source /opt/ros/melodic/setup.bash" >> ~ / .bashrc '

Установим библиотеки Adafruit для поддержки драйверов двигателя TB6612/PCA9685 и отладочного OLED-дисплея SSD1306:

# установка pip

sudo apt-get install python-pip

# установка библиотек Adafruit

pip install Adafruit-MotorHAT
pip install Adafruit-SSD1306

Предоставим  пользователю доступ к шине i2c:

sudo usermod -aG i2c $USER

Перезагрузим систему, чтобы изменения вступили в силу.

Создайте рабочее пространство ROS Catkin для хранения пакетов ROS:

# создаем рабочую область catkin mkdir -p ~/workspace/catkin_ws/src
cd ~/workspace/catkin_ws
catkin_make
# добавим путь catkin_ws к bashrc sudo sh -c 'echo "source ~/workspace/catkin_ws/devel/setup.bash" >> ~/.bashrc'

Закрываем и заново открываем новое окно терминала, чтобы убедиться, что  catkin_ws виден ROS:

echo $ROS_PACKAGE_PATH

Клонируем и собираем пакет jetson-inference. В пакете используется NVIDIA TensorRT для эффективного развертывания нейронных сетей на встроенной платформе Jetson.
 

# установка git и cmake

sudo apt-get install git cmake
# клонирование репозитория и подмодулей cd ~/workspace
git clone https://github.com/dusty-nv/jetson-inference
cd jetson-inference
git submodule update --init
# сборка из исходного кода

mkdir build
cd build
cmake ../
make

# установить библиотеки 

sudo make install

Клонирование и сборка ROS пакет ros_deep_learning 

# установить зависимости

sudo apt-get install ros-melodic-vision-msgs ros-melodic-image-transport ros-melodic-image-publisher
# клонирование репозитория cd ~/workspace/catkin_ws/src
git clone https://github.com/dusty-nv/ros_deep_learning
# сборка catkin

cd ~/workspace/catkin_ws
catkin_make

# проверка, что ROS находит пакет  что пакет ros_deep_learning

rospack find ros_deep_learning

Клонирование и сборка ROS пакета  jetbot_ros

# клонирование репозитория 
cd ~/workspace/catkin_ws/src
git clone https://github.com/dusty-nv/jetbot_ros

# сборка пакета 

cd ~/workspace/catkin_ws
$ catkin_make

# проверка, что ROS находит пакет  что пакет  jetbot_ros 
$ rospack найти jetbot_ros

Тестирование ros_jetbot
Открываем терминал и запускаем 
roscore
Во втором терминале запускаем ноду jetbot_motors

И тут проблема Import error: No module named Adafruit_MotorHAT, хотя библиотеку уже устанавливали (см. выше).
Но библиотека была установлена в python3, а ROS использует python2.7
Установим библиотеку Adafruit_MotorHAT для python2.7

rosrun jetbot_ros jetbot_motors.py

И тут проблема Import error: No module named Adafruit_MotorHAT, хотя библиотеку уже устанавливали (см. выше).
Но библиотека была установлена в python3, а ROS использует python2.7
Установим библиотеку Adafruit_MotorHAT для python2.7

python2.7 -m pip install Adafruit_MotorHAT
Теперь запуск нормальный и в следующем терминале проверяем, что в ROS запущены соответствующие ноды и топики
Вот список топиков

/jetbot_motors/cmd_dir - relative heading (degree [-180.0, 180.0], speed [-1.0, 1.0])

/jetbot_motors/cmd_raw - raw L/R motor commands (speed [-1.0, 1.0], speed [-1.0, 1.0])

/jetbot_motors/cmd_str - simple string commands (left/right/forward/backward/stop)

Но, к сожалению в jetbot_motors.py прописана обработка сообщений, получаемых из топика /jetbot_motors/cmd_str 

Пробуем отправлять их из терминала (это движения вперед, назад, влево, вправо и остановка)

rostopic pub /jetbot_motors/cmd_str std_msgs/String --once "forward"
rostopic pub /jetbot_motors/cmd_str std_msgs/String --once "backward"
rostopic pub /jetbot_motors/cmd_str std_msgs/String --once "left"
rostopic pub /jetbot_motors/cmd_str std_msgs/String --once "right"
rostopic pub /jetbot_motors/cmd_str std_msgs/String --once "stop"

Опять проблема !!! Никаких признаков движения. Требуется внести изменения в файл jetbot_motors.py, изменить функции set_speed() и all_stop().

Я создал новый файл jetbot_motors_1.py и внес следующие изменения изменения

# sets motor speed between [-1.0, 1.0]
def set_speed(motor_ID, value):
  max_pwm = 200.0
  speed = int(min(max(abs(value * max_pwm), 0), max_pwm))
  a = b = 0
  if motor_ID == 1:
    motor = motor_left
    a=1
    b=0
  elif motor_ID == 2:
    motor = motor_right
    a=2
    b=3
  else:
    rospy.logerror('set_speed(%d, %f) -> invalid motor_ID=%d', motor_ID, value, motor_ID)
    return
	
  motor.setSpeed(speed)

  if value < 0:
    motor.run(Adafruit_MotorHAT.FORWARD)
    motor.MC._pwm.setPWM(a,0,0)
    motor.MC._pwm.setPWM(b,0,speed*16)
  elif value > 0:
    motor.run(Adafruit_MotorHAT.BACKWARD)
    motor.MC._pwm.setPWM(a,0,speed*16)
    motor.MC._pwm.setPWM(b,0,0)
  else:
    motor.run(Adafruit_MotorHAT.RELEASE)
    motor.MC._pwm.setPWM(a,0,0)
    motor.MC._pwm.setPWM(b,0,0)


# stops all motors
def all_stop():
	set_speed(motor_left_ID, 0.0)
	set_speed(motor_right_ID, 0.0)

Теперь jetbot реагирует на отправку сообщений, может двигаться вперед, назад, влево, вправо и стоп. Но это все с одной скоростью, регулируемой этим значением

max_pwm = 200.0

Необходимо прописать реализацию движения с разной скоростью. Пока сделаем самое простое, будем использовать отправку сообщений std_msgs/String в топик /jetbot_motors/cmd_raw

# raw L/R motor commands (speed, speed)
def on_cmd_raw(msg):
	rospy.loginfo(rospy.get_caller_id() + ' cmd_raw=%s', msg.data)
        speeds=msg.data.split(',')
	set_speed(motor_left_ID,  float(speeds[0]))
	set_speed(motor_right_ID, float(speeds[1])) 

И проверяем, отправляю подобные команды

rostopic pub /jetbot_motors/cmd_raw std_msgs/String --once " 0.9,-0.7"

 

Теперь рассмотрим отправку информационных сообщений на OLED дисплей. 

Сначала установим библиотеку 

python2.7 -m pip install Adafruit_SSD1306

Запускаем ноду jetbot_oled для отображения системной информации и пользовательского текста:

rosrun jetbot_ros jetbot_oled.py

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

Узел также будет прослушивать топик /jetbot_oled/user_text , чтобы получать строковые сообщения от пользователя и отображать на экране

rostopic pub /jetbot_oled/ user_text std_msgs/String --once "HELLO!"

 

Использование камеры

Чтобы начать потоковую передачу с камеры JetBot, запускаем ноду  jetbot_camera

rosrun jetbot_ros jetbot_camera

Видеокадры будут публиковаться в топик /jetbot_camera/raw в виде сообщений sensor_msgs::Image в кодировке BGR8. 

Установим пакет image_view , а затем подпишимся на него /jetbot_camera/raw с нового терминала:

sudo apt-get install ros-melodic-image-view
rosrun image_view image_view image:=/jetbot_camera/raw

После этого должно открыться окно, в котором будет отображаться видео с камеры в реальном времени. 

 

И еще отправку сообщений с тему /jetbot_motors/cmd_raw обязательно надо поменять. А именно тип сообщений на geometry_msgs/Twist, который широко используется в ROS.

Я создал новый файл jetbot_motors_2.py и внес следующие изменения изменения

#!/usr/bin/env python
import rospy
import time
import math 

from Adafruit_MotorHAT import Adafruit_MotorHAT
from std_msgs.msg import String
from geometry_msgs.msg import Twist

PWM_MIN=0.5
PWM_MAX=1.0

# sets motor speed between [-1.0, 1.0]
def set_speed(motor_ID, value):
  max_pwm = 200.0
  speed = int(min(max(abs(value * max_pwm), 0), max_pwm))
  a = b = 0
  if motor_ID == 1:
    motor = motor_left
    a=1
    b=0
  elif motor_ID == 2:
    motor = motor_right
    a=2
    b=3
  else:
    rospy.logerror('set_speed(%d, %f) -> invalid motor_ID=%d', motor_ID, value, motor_ID)
    return
	
  motor.setSpeed(speed)

  if value < 0:
    motor.run(Adafruit_MotorHAT.FORWARD)
    motor.MC._pwm.setPWM(a,0,0)
    motor.MC._pwm.setPWM(b,0,speed*16)
  elif value > 0:
    motor.run(Adafruit_MotorHAT.BACKWARD)
    motor.MC._pwm.setPWM(a,0,speed*16)
    motor.MC._pwm.setPWM(b,0,0)
  else:
    motor.run(Adafruit_MotorHAT.RELEASE)
    motor.MC._pwm.setPWM(a,0,0)
    motor.MC._pwm.setPWM(b,0,0)


# stops all motors
def all_stop():
	set_speed(motor_left_ID, 0.0)
	set_speed(motor_right_ID, 0.0)

# directional commands (degree, speed)
def on_cmd_dir(msg):
	rospy.loginfo(rospy.get_caller_id() + ' cmd_dir=%s', msg.data)

# raw L/R motor commands (speed, speed)
def on_cmd_raw(msg):
	rospy.loginfo("msg cmd_raw")
	rospy.loginfo(msg)
        x=max(min(msg.linear.x,1.0),-1.0)
        z=max(min(msg.angular.z,1.0),-1.0)         
        l=(x-z)/2
        r=(x+z)/2
	#rospy.loginfo(x)
        #rospy.loginfo(z)
	#rospy.loginfo(l)
        #rospy.loginfo(r)
        
        lpwm= PWM_MIN+math.fabs(l)*(PWM_MAX-PWM_MIN)
        rpwm= PWM_MIN+math.fabs(r)*(PWM_MAX-PWM_MIN)
 	#rospy.loginfo(lpwm)
        #rospy.loginfo(rpwm)
        kl=1 if l>0 else -1
        kr=1 if r>0 else -1
        if l==0 : kl=0
        if r==0 : kr=0 
	set_speed(motor_left_ID, kl*lpwm )
	set_speed(motor_right_ID, kr*rpwm) 

# simple string commands (left/right/forward/backward/stop)
def on_cmd_str(msg):
	rospy.loginfo(rospy.get_caller_id() + ' cmd_str=%s', msg.data)

	if msg.data.lower() == "left":
		set_speed(motor_left_ID,  -1.0)
		set_speed(motor_right_ID,  1.0) 
	elif msg.data.lower() == "right":
		set_speed(motor_left_ID,   1.0)
		set_speed(motor_right_ID, -1.0) 
	elif msg.data.lower() == "forward":
		set_speed(motor_left_ID,   1.0)
		set_speed(motor_right_ID,  1.0)
	elif msg.data.lower() == "backward":
		set_speed(motor_left_ID,  -1.0)
		set_speed(motor_right_ID, -1.0)  
	elif msg.data.lower() == "stop":
		all_stop()
	else:
		rospy.logerror(rospy.get_caller_id() + ' invalid cmd_str=%s', msg.data)


# initialization
if __name__ == '__main__':

	# setup motor controller
	motor_driver = Adafruit_MotorHAT(i2c_bus=1)

	motor_left_ID = 1
	motor_right_ID = 2

	motor_left = motor_driver.getMotor(motor_left_ID)
	motor_right = motor_driver.getMotor(motor_right_ID)

	# stop the motors as precaution
	all_stop()

	# setup ros node
	rospy.init_node('jetbot_motors')
	
	rospy.Subscriber('~cmd_dir', String, on_cmd_dir)
	rospy.Subscriber('~cmd_raw', Twist, on_cmd_raw)
	rospy.Subscriber('~cmd_str', String, on_cmd_str)

	# start running
	rospy.spin()

	# stop motors before exiting
	all_stop()

Ну и конечно, запуск всех нод запуском командного файла launch. Файл launch01.launch размещаем с catkin_ws/src/jetbot_ros/launch

<launch>
 <node name="jetbot_motors" pkg="jetbot_ros" type="jetbot_motors_2.py">
 </node>
 <node name="jetbot_camera" pkg="jetbot_ros" type="jetbot_camera">
 </node>
 <node name="jetbot_oled" pkg="jetbot_ros" type="jetbot_oled.py">
 </node>
</launch> 

Запуск командного файла

roslaunch jetbot_ros launch01.launch

 

Сделаем управление роботом с удаленного компьютера. Будем использовать пакет rosbridge. Rosbridge позволяет внешним клиентам (в нашем случае браузеру) иметь доступ к темам и сервисам ROS (публикация и получение из тем, вызов сервисов). Rosbridge является частью мете-пакета rosbridge_suite, включающего различные дополнительные пакеты для реализации протокола rosbridge.

Пакет rosbridge_suite - это набор пакетов, реализующих протокол rosbridge и обеспечивающих транспортный уровень WebSocket.

В пакеты входят:

• rosbridge_library - базовый пакет rosbridge. Rosbridge_library отвечает за получение строки JSON и отправку команд в ROS и наоборот.

• rosapi - делает определенные действия ROS доступными через вызовы служб, которые обычно зарезервированы для клиентских библиотек ROS . Сюда входит получение и установка параметров, получение списка тем и многое другое.

• rosbridge_server - обеспечивает соединение через WebSocket, чтобы браузеры могли "разговаривать с rosbridge". Roslibjs - это библиотека JavaScript для браузера, которая может взаимодействовать с ROS через rosbridge_server.

Установка пакета

sudo apt-get install ros-melodic-rosbridge-suite

Создаем в папке проекта jetbot_ros в папке launch и командный файл launch03.launch

<launch>
 <include file="$(find rosbridge_server)/launch/rosbridge_websocket.launch"/>
 <node name="jetbot_motors" pkg="jetbot_ros" type="jetbot_motors_2.py">
 </node>
 <node name="jetbot_camera" pkg="jetbot_ros" type="jetbot_camera">
 </node>
 <node name="jetbot_oled" pkg="jetbot_ros" type="jetbot_oled.py">
 </node>

</launch>

И запуск командного файла

1 терминал

roscore

2 терминал

roslaunch jetbot_ros launch03.launch

Для организации web-интерфейса необходимо установить web-сервер.

sudo apt-get install apache2

Теперь в папке /var/www/html будет находиться наша страница html. Библиотеку roslib.min.js поместим в папке js.

Создадим html файл index01.html, где подключимся к ROS по websocket 9090 и будем отправлять из формы сообщения в тему ROS /pub_txt_msg и получать и отображать на странице сообщения, приходящие в тему ROS /sub_txt_msg

<!DOCTYPE html>
<html>
<head>
<meta charset="utf-8" />

<script type="text/javascript" src="js/roslib.min.js"></script>
<script type="text/javascript" type="text/javascript">
  var ros = new ROSLIB.Ros({
    url : 'ws://192.168.0.111:9090'
  });

  ros.on('connection', function() {
    document.getElementById("status").innerHTML = "Connected";
  });

  ros.on('error', function(error) {
    document.getElementById("status").innerHTML = "Error";
  });

  ros.on('close', function() {
    document.getElementById("status").innerHTML = "Closed";
  });
  var txt_listener = new ROSLIB.Topic({
    ros : ros,
    name : '/sub_txt_msg',
    messageType : 'std_msgs/String'
  });

  txt_listener.subscribe(function(m) {
    document.getElementById("msg").innerHTML = m.data;
  });

  var pub1=new ROSLIB.Topic ({
     ros: ros,
     name : '/pub_txt_msg',
     messageType : 'std_msgs/String'  
  });

 function send_ros() {
    var msg=new ROSLIB.Message ({"data" : document.getElementById("putdata").value});
    pub1.publish(msg);
  }

</script>
</head>

<body>
  <h1>Simple ROS User Interface</h1>
  <p>Connection status: <span id="status"></span></p>
  <p>Last /txt_msg received: <span id="msg"></span></p>
  <p><form id=formoptions name=formoptions action="javascript:void();" onsubmit="feturn false;">
      send to /pub_txt_msg <br>
      <input  name=putdata id=putdata>
      <button id='button1' value='send' onclick='send_ros();'>Send</button>
    </form>
  </p>


</body>
</html>

Запускаем в браузере страницу index01.html

Сообщения в тему /sub_txt_msg

rostopic pub /sub_txt_msg std_msgs/String "Hello? browser"

Просмотр сообщений, отправляемых из браузера в тему /pub_txt_msg

rostopic echo /pub_txt_msg

В дальнейшем запускать все ноды будем из командных файлов

Вывод потокового изображения с камеры на web-страницу

Для вывода потокового изображения с камеры на web-страницу установим ros-пакет web_video_server

sudo apt-get install ros-melodic-web-video-server

Создаем командный файл launch03.launch

<launch>
  <include file="$(find rosbridge_server)/launch/rosbridge_websocket.launch"/>
  <node name="jetbot_motors" pkg="jetbot_ros" type="jetbot_motors_2.py">
  </node>
 <node name="jetbot_oled" pkg="jetbot_ros" type="jetbot_oled.py">
  </node>
  <node name="jetbot_camera" pkg="jetbot_ros" type="jetbot_camera">
  </node>
  <node name="web_video_server" pkg="web_video_server" type="web_video_server">
    <param name="port" value="8090"></param>
    <param name="address" value="192.168.0.111"></param>
  </node>
</launch>

Просмотр видео на странице

Создаем файл index02.html для управления движением робота и просмотра изображения с камеры. Используем библиотеку nipplejs.js

<!DOCTYPE html>
<html>
<head>
<meta charset="utf-8" />

<script type="text/javascript" src="js/roslib.min.js"></script>
<script type="text/javascript" src="js/nipplejs.js"></script>
<script type="text/javascript" type="text/javascript">
  var ros = new ROSLIB.Ros({
    url : 'ws://192.168.0.111:9090'
  });

  ros.on('connection', function() {
    document.getElementById("status").innerHTML = "Connected";
  });

  ros.on('error', function(error) {
    document.getElementById("status").innerHTML = "Error";
  });

  ros.on('close', function() {
    document.getElementById("status").innerHTML = "Closed";
  });
  var txt_listener = new ROSLIB.Topic({
    ros : ros,
    name : '/txt_msg',
    messageType : 'std_msgs/String'
  });

  txt_listener.subscribe(function(m) {
    document.getElementById("msg").innerHTML = m.data;
  });

  cmd_vel_listener = new ROSLIB.Topic({
    ros : ros,
    name : "/cmd_vel",
    messageType : 'geometry_msgs/Twist'
  });

  move = function (linear, angular) {
    var twist = new ROSLIB.Message({
      linear: {
        x: linear,
        y: 0,
        z: 0
      },
      angular: {
        x: 0,
        y: 0,
        z: angular
      }
    });
    cmd_vel_listener.publish(twist);
  }


    createJoystick = function () {
      var options = {
        zone: document.getElementById('zone_joystick'),
        threshold: 0.1,
        position: { left: 50 + '%' },
        mode: 'static',
        size: 150,
        color: '#000000',
      };
      manager = nipplejs.create(options);
      linear_speed = 0;
      angular_speed = 0;
      manager.on('start', function (event, nipple) {
        timer = setInterval(function () {
          move(linear_speed, angular_speed);
        }, 25);
      });
      manager.on('move', function (event, nipple) {
        max_linear = 1.0; // m/s
        max_angular = 1.0; // rad/s
        max_distance = 75.0; // pixels;
        linear_speed = Math.sin(nipple.angle.radian) * max_linear * nipple.distance/max_distance;
				angular_speed = -Math.cos(nipple.angle.radian) * max_angular * nipple.distance/max_distance;
      });
      manager.on('end', function () {
        if (timer) {
          clearInterval(timer);
        }
        self.move(0, 0);
      });
    }
    window.onload = function () {
      createJoystick();
    }

</script>
</head>

<body>
  <h1>Simple ROS User Interface</h1>
  <p>Connection status: <span id="status"></span></p>
  <p>Last /txt_msg received: <span id="msg"></span></p>
  <div id="zone_joystick" style="position: relative;"></div>
</body>
</html>

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

Если необходимо добавить управление с джойстика, то создадим скрипт subscriber, ловящий данные, публикуемые пакетом joy и преобразующий их в данные для ноды /jetbot_motors/cmd_raw.

Содержимое скрипта jetbot_joy.py

#!/usr/bin/env python
import rospy
import time
import math 

from sensor_msgs.msg import Joy
from std_msgs.msg import String
from geometry_msgs.msg import Twist

pub=rospy.Publisher("/jetbot_motors/cmd_raw",Twist)

def controller(data):
    rospy.loginfo(str(data.axes[0])+" "+str(data.axes[1]))
    msg=Twist()
    msg.linear.x=data.axes[0];
    msg.linear.y=0.0;
    msg.linear.z=0.0;
    msg.angular.x=0.0;
    msg.angular.y=0.0;
    msg.angular.z=data.axes[1];
    pub.publish(msg)

    
def listener():
     rospy.init_node('jetbot_joy')
     rospy.sleep(1.0)
    
     sub = rospy.Subscriber("joy",Joy,controller)
     rospy.spin()
  
if __name__ == '__main__':
   listener()

 

И командный файл launch04.launch

<launch>
 <include file="$(find rosbridge_server)/launch/rosbridge_websocket.launch"/>
 <node name="jetbot_motors" pkg="jetbot_ros" type="jetbot_motors_2.py">
 </node>
 <node name="jetbot_oled" pkg="jetbot_ros" type="jetbot_oled.py">
 </node>
 <node name="jetbot_camera" pkg="jetbot_ros" type="jetbot_camera">
 </node>
 <node name="joy_node" pkg="joy" type="joy_node">
 </node>
 <node name="jetbot_joy" pkg="jetbot_ros" type="jetbot_joy.py">
 </node>
 <node name="web_video_server" pkg="web_video_server" type="web_video_server">
  <param name="port" value="8090"></param>
  <param name="address" value="192.168.0.36"></param>
 </node>

</launch>

 

Теги:

• ROS
• jetson nano
• jetbot