0

我正在使用 MD49 电机驱动器及其电机

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

http://wiki.ros.org/md49_base_controller

如何从 md49_base_controller 包订阅（encoder_l 和 encoder_r）并以 odom（nav_msgs/Odometry）的形式发布（vx、vy 和 vth）？

有两个问题：

1-首先是编码器输出不正确“需要修改包。

2-第二个是我想创建一个包，订阅左右轮编码器计数（encoder_l 和 encoder_r）并以 odom（nav_msgs/Odometry）的形式发布（vx、vy 和 vth）以与 imu 兼容MPU9250

http://wiki.ros.org/robot_pose_ekf

建议的包是：

1- 我们必须将 (encoder_l and encoder_r) 转换为 (RPM_l and 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, and 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()
4

2 回答 2

1

The problem was in serial comunication setup not for the code.

Setup UART on the raspi 3 GPIO

For some strange reason the default for Pi3 using the latest 4.4.9 kernel is to DISABLE UART. To enable it you need to change enable_uart=1 in /boot/config.txt. (There is no longer necessary to add core_freq=250 to fix the core frequency to get stable baudrate.) If you don’t use Bluetooth (or have undemanding uses) it is possible to swap the ports back in the Device Tree. There is a pi3-miniuart-bt and pi3-disable-bt module which are described in /boot/overlays/README here.

As mentioned, by default the new GPIO UART is not enabled so the first thing to do is to edit the config.txt file to enable the serial UART:

sudo nano /boot/config.txt

and add the line (at the bottom):

enable_uart=1

You have to reboot for the changes to take effect.

To check where your serial ports are pointing you can use the list command with the long “-l” listing format: ls -l /dev

In the long listing you will find: serial0 -> ttyS0 serial1 -> ttyAMA0

Thus on a Raspberry Pi 3 and Raspberry Pi Zero W, serial0 will point to GPIO J8 pins 8 and 10 and use the /dev/ttyS0. On other Raspberry Pi’s it will point to /dev/ttyAMA0.

So where possible refer to the serial port via it’s alias of “serial0” and your code should work on both Raspberry Pi 3 and other Raspberry Pi’s.

You also need to remove the console from the cmdline.txt. If you edit this with:

sudo nano /boot/cmdline.txt

you will see something like this:

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

remove the line: console=serial0,115200 and save and reboot for changes to take effect:

reboot

于 2020-05-10T16:05:00.383 回答
0

首先，您需要导入 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()

我认为这会奏效

于 2020-04-09T13:28:30.320 回答