Как публиковать одом (nav_msgs/Odometry) из выходов кодеров MD49?

Question

Я использую MD49 Motor Drive с его моторами

https://www.robot-electronics.co.uk/htm/md49tech.htm

http://wiki.ros.org/md49_base_controller

Как подписаться (encoder_l и encoder_r) из пакета md49_base_controller и опубликовать (vx, vy и vth) как форму odom (nav_msgs/Odometry)?

Есть две проблемы:

1-Во-первых, выходы энкодеров неверны "пакет необходимо изменить.

2-Второй - это то, что я хочу создать пакет, который подписывает счетчики кодировщика правого и левого колес (encoder_l и encoder_r) и публикует (vx, vy и vth) как форму odom (nav_msgs / Odometry), чтобы быть совместимым с imu MPU9250

http://wiki.ros.org/robot_pose_ekf

Предлагаемый пакет:

1- Мы должны преобразовать (encoder_l и encoder_r) в (RPM_l и RPM_r) следующим образом:

if (speed_l>128){newposition1 = encoder_l;}
else if  (speed_l<128){ newposition1 = 0xFFFFFFFF-encoder_l;}
else if  (speed_l==128) {newposition1=0;}

newtime1 = millis();
RPM_l = ((newposition1-oldposition1)*1000*60)/((newtime1-oldtime1)*980);
oldposition1 = newposition1;
oldtime1 = newtime1;
delay(250);

if (speed_r>128){ newposition2 = encoder_r;}
else if  (speed_r<128){ newposition2 = 0xFFFFFFFF-encoder_r;}
else if   (speed_r==128) { newposition2=0;}
newtime2 = millis();
RPM_r = ((newposition2-oldposition2)*1000*60)/((newtime2-oldtime2)*980);
oldposition2 = newposition2;
oldtime2= newtime2;
delay(250);

2- Мы должны преобразовать (RPM_l и RPM_r) в (vx, vy и vth) следующим образом:

vx=(r/2)*RPM_l*math.cos(th)+(r/2)*RPM_r*math.cos(th);
vx=(r/2)*RPM_l*math.sin(th)+(r/2)*RPM_r*math.sin(th);
vth=(r/B)*omega_l-(r/B)*omega_r;

Подсказка: r и B - радиус колеса и ширина автомобиля соответственно.

3- Пакет odom (nav_msgs / Odometry):

#!/usr/bin/env python

import math
from math import sin, cos, pi

import rospy
import tf
from nav_msgs.msg import Odometry
from geometry_msgs.msg import Point, Pose, Quaternion, Twist, Vector3
from md49_messages.msg import md49_encoders

rospy.init_node('odometry_publisher')

odom_pub = rospy.Publisher("odom", Odometry, queue_size=50)
odom_broadcaster = tf.TransformBroadcaster()


x = 0.0
y = 0.0
th = 0.0

vx =0.1
vy = -0.1
vth = 0.1

current_time = rospy.Time.now()
last_time = rospy.Time.now()

r = rospy.Rate(1.0)
while not rospy.is_shutdown():
    current_time = rospy.Time.now()


    # compute odometry in a typical way given the velocities of the robot
    dt = (current_time - last_time).to_sec()
    delta_x = (vx * cos(th) - vy * sin(th)) * dt
    delta_y = (vx * sin(th) + vy * cos(th)) * dt
    delta_th = vth * dt

    x += delta_x
    y += delta_y
    th += delta_th

    # since all odometry is 6DOF we'll need a quaternion created from yaw
    odom_quat = tf.transformations.quaternion_from_euler(0, 0, th)

    # first, we'll publish the transform over tf
    odom_broadcaster.sendTransform(
        (x, y, 0.),
        odom_quat,
        current_time,
        "base_link",
        "odom"
    )

    # next, we'll publish the odometry message over ROS
    odom = Odometry()
    odom.header.stamp = current_time
    odom.header.frame_id = "odom"

    # set the position
    odom.pose.pose = Pose(Point(x, y, 0.), Quaternion(*odom_quat))

    # set the velocity
    odom.child_frame_id = "base_link"
    odom.twist.twist = Twist(Vector3(vx, vy, 0), Vector3(0, 0, vth))

    # publish the message
    odom_pub.publish(odom)

    last_time = current_time
    r.sleep()

Answer 1

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

Настраиваем UART на распи 3 GPIO

По какой-то странной причине по умолчанию для Pi3 с использованием последней версии ядра 4.4.9 UART отключен. Чтобы включить его, вам нужно изменить enable_uart=1 в /boot/config.txt. (Больше нет необходимости добавлять core_freq=250, чтобы зафиксировать частоту ядра, чтобы получить стабильную скорость передачи.) Если вы не используете Bluetooth (или у вас нетребовательное использование), можно снова поменять местами порты в дереве устройств. Существуют модули pi3-miniuart-bt и pi3-disable-bt, которые описаны здесь в /boot/overlays/README.

Как уже упоминалось, по умолчанию новый GPIO UART не включен, поэтому первое, что нужно сделать, это отредактировать файл config.txt, чтобы включить последовательный UART:

sudo nano /boot/config.txt

и добавьте строку (внизу):

enable_uart=1

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

Чтобы проверить, куда указывают ваши последовательные порты, вы можете использовать команду list с длинным списком в формате "-l": ls -l /dev

В длинном списке вы найдете: serial0 -> ttyS0 serial1 -> ttyAMA0

Таким образом, на Raspberry Pi 3 и Raspberry Pi Zero W serial0 будет указывать на контакты 8 и 10 GPIO J8 и использовать /dev/ttyS0. На других Raspberry Pi он будет указывать на /dev/ttyAMA0.

Поэтому по возможности обращайтесь к последовательному порту через псевдоним "serial0", и ваш код должен работать как на Raspberry Pi 3, так и на других Raspberry Pi.

Также нужно удалить консоль из cmdline.txt. Если вы отредактируете это с помощью:

sudo nano /boot/cmdline.txt

вы увидите что-то вроде этого:

dwc_otg.lpm_enable=0 console=serial0,115200 console=tty1 root=/dev/mmcblk0p2 rootfstype=ext4 elevator=deadline fsck.repair=yes root wait

удалите строку: console=serial0,115200 и сохраните и перезагрузитесь, чтобы изменения вступили в силу:

перезагружать

Answer 2

Во-первых, вам нужно импортировать nav_msgs/Odometry следующим образом:

из nav_msgs.msg импортировать одометрию

У вас должна быть функция, которая выполняет эти преобразования, а затем в rospy.Subscriber импортировать эти переменные, например:

def example(data):
    data.vx=<conversion>
    data.vth=<conversion>

def listener():
    rospy.Subscriber('*topic*', Odometry, example)
    rospy.spin()

if __name__ == 'main':
    listener()

Я думаю это сработает