我正在使用MD49电机驱动器及其电机
https://www.robot-electronics.co.uk/htm/md49tech.htm
http://wiki.ros.org/md49_base_controller
如何从md49_base_controller包订阅(encoder_l和encoder_r(并将(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-里程计(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()
问题出现在串行通信设置中，而不是针对代码。
在raspi 3 GPIO 上设置UART
出于某种奇怪的原因，使用最新4.4.9内核的Pi3的默认设置是DISABLE UART。要启用它，您需要在/boot/config.txt中更改enable_uart=1。(不再需要添加core_freq=250来固定核心频率以获得稳定的波特率。(如果您不使用蓝牙(或有无要求的用途(，则可以在设备树中重新交换端口。这里有一个pi3 miniuart bt和pi3 disable bt模块，在/boot/overloads/README中进行了描述。
如前所述，默认情况下，新的GPIO UART未启用，因此首先要做的是编辑config.txt文件以启用串行UART:
sudo nano/boot/config.txt
并添加行(在底部(：
enable_uart=1
您必须重新启动才能使更改生效。
要检查串行端口指向的位置，可以使用带有长"-l"列表格式的list命令：ls-l/dev
在长长的列表中，您会发现：serial0->ttyS0serial1->ttyAMA0
因此，在树莓派3和树莓派Zero W上，serial0将指向GPIO J8引脚8和10，并使用/dev/ttyS0。在其他树莓派上，它将指向/dev/ttyAMA0。
因此，在可能的情况下，通过其别名"serial0"引用串行端口，您的代码应该在Raspberry Pi 3和其他Raspberrry 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并保存并重新启动以使更改生效：
重新启动
首先，您需要导入nav_msgs/Odometry，如下所示：
从nav_msgs.msg导入Odometry
你必须有一个函数来执行这些转换，然后在rospy中。订阅服务器导入这些变量，如下所示：
def example(data):
data.vx=<conversion>
data.vth=<conversion>
def listener():
rospy.Subscriber('*topic*', Odometry, example)
rospy.spin()
if __name__ == 'main':
listener()
我认为这会起作用