Rodney - 迟来的自主机器人（第五部分）
5.00/5 (11投票s)
关于 ROS（机器人操作系统）家用机器人系列中的第五部分
引言
罗德尼机器人项目是一个业余爱好者的机器人项目，旨在设计和建造一个使用ROS（机器人操作系统）的自主家居机器人。本文是描述该项目的系列文章的第五部分。
背景
在第一部分中，为了帮助定义我们机器人的需求，我们选择了我们的第一个任务，并将其分解为多个设计目标，使其更易于管理。
这个任务取自文章《让我们来制造一个机器人！》，内容是：“给……带个口信——由于机器人将具备识别人脸的能力，那么让它成为‘口信传递者和提醒者’怎么样？我可以说‘机器人，提醒（人名）下午6点到车站接我’。然后，即使那个家庭成员的手机静音，或者正在听响亮的音乐，或者（插入任何不来车站接我的理由），机器人也可以在房子里漫游，找到那个人，并把口信告诉他们。”
该任务的设计目标是
- 设计目标1：能够使用摄像头环顾四周，搜索人脸，尝试识别任何看到的人，并显示识别出来的信息
- 设计目标2：面部表情和语音合成。罗德尼需要能够传递信息
- 设计目标3：通过远程键盘和/或操纵杆控制移动
- 设计目标4：增加激光测距仪或类似的测距传感器以辅助导航
- 设计目标5：自主移动
- 设计目标6：任务分配和完成通知
在文章的前几部分，我们完成了设计目标1和2，我们还增加了任务控制并开始设计目标3。在这一部分中，我将添加电动机、电机控制板以及驱动控制板的软件。这将完成设计目标3，我还会简要讨论机器人到目前为止的硬件构建。
两轮驱动 vs 四轮驱动
当我刚开始研究罗德尼时，他本来会有四个轮子，并使用所谓的“滑移转向”进行转向。滑移转向对于崎岖地形很好，但在转弯时需要消耗大量电力。我在最初的实验中还发现转向有点不可预测，这可能是由于我使用的宽大带瘤轮胎抓住了地毯。现在，罗德尼一直被设计成一个家用机器人，不需要在火星上漫游，所以崎岖地形上的机动不是必需的，因此我将设计改为两轮和两个被动脚轮。这种驱动类型被称为“差速驱动”。差速驱动设计起来更容易，但不适合在颠簸和障碍物上行驶。
对于脚轮，我最初尝试了两个家具旋转脚轮，但发现它们并不总是与行驶方向对齐，导致机器人偏离航线。然后我在Thingiverse网站上找到了一个使用乒乓球作为脚轮的3D打印件。我添加了我自己的3D打印垫片以获得所需的高度。它内部没有商业滚珠脚轮那样的滚轮，但似乎工作正常。
电机控制板
下一个设计决策是使用什么作为控制板来驱动电动机。在该系列的第一篇文章中，我引用了另一个机器人项目《PiRex – 远程控制的基于Raspberry Pi的机器人》，它使用了一个简单的L293D芯片。我还考虑过使用L298N电机驱动板，但最终选择了来自PiBorg公司的更复杂的Thunderborg。尽管比其他板更昂贵，但这款板功率更高，包含欠压和短路保护，您可以读取电池电压，并且可以以这样一种方式启用它：如果我们的软件停止与板通信，它将关闭电机，从而阻止机器人失控。该板包含一个5V稳压器，我们将使用此电源为Raspberry Pi供电。
与板的通信通过I2C链接进行，您可以从PiBorg网站复制一个库，该库负责Raspberry Pi和Thunderborg之间的通信。这意味着您只需调用TB.SetMotor1(0.5)等函数即可控制电机。
这个PiBorg网站上的zip文件包含了示例代码和我将在我的ROS节点中使用的库文件。
带编码器的齿轮电机
对于这两个电动机，我将使用包含霍尔效应传感器的12V齿轮电机。我们将使用PID控制器来控制每个车轮的电机速度，尽管霍尔传感器的反馈对于PID功能很重要，但传感器的主要用途是生成里程计消息，最终将与LIDAR数据一起由ROS导航系统在自主模式下使用。
代码
将注意力转向代码，我们将编写两个新的ROS包，并主要通过更新Arduino草图来处理霍尔传感器的信号来更新Rodney包。
tacho_msgs 包
新的包中的第一个非常简单，因为它只包含一个ROS用户定义消息，该消息将用于将左右电机RPM从Arduino上运行的节点发送到将在Raspberry Pi上运行的电机控制节点。
构成这个第一个包的文件可在 tacho_msgs 文件夹中找到。根文件夹包含构成ROS包的所有常用文件。
包内的 msg 文件夹包含我们消息的定义文件，tacho.msg
float32 lwheelrpm # RPM of the left wheel
float32 rwheelrpm # RPM of the right wheel
lwheelrpm 元素将包含左侧电机的 RPM 值，而 rwheelrpm 元素将包含右侧电机的 RPM 值。
该包还在 launch 文件夹中包含了 test.launch 文件。该文件只是启动与Arduino通信的串行节点，并在包的早期测试中使用了该文件。
至此，我们完成了第一个新的 ROS 包。
thunderborg 节点包
下一个包将形成一个节点，用作 Thunderborg 控制器的驱动程序，并生成原始里程计消息。我们节点的 ROS 包名为 thunderborg，位于 thunderborg 文件夹中。该包包含所有常用的 ROS 文件。
cfg 文件夹包含文件 thunderborg.cfg。此文件由动态重新配置服务器使用，以便我们可以即时调整 PID 参数。如您所见，该文件实际上是一个 Python 脚本。
#!/usr/bin/env python
# Dynamic rconfiguration for Thunderborg node
PACKAGE = "thunderborg"
from dynamic_reconfigure.parameter_generator_catkin import *
gen = ParameterGenerator()
gen.add("p_param", double_t, 0, "P-Proportional", 0.5, 0, 5)
gen.add("i_param", double_t, 0, "I-Integral", 0.9, 0, 5)
gen.add("d_param", double_t, 0, "D-Derivative", 0.006, 0, 5)
gen.add("restore_defaults", bool_t, 0, "Restore to original configuration", False)
exit(gen.generate(PACKAGE, "thunderborg_node", "Thunderborg"))
我们在文章的前几部分中使用了动态重新配置服务器。在这里，PID 的默认参数将是比例参数为 0.5，积分参数为 0.9，微分参数为 0.006。您将能够使用 rqt_reconfiguration 应用程序调整 PID 控制器。一旦找到最佳值，请在下面显示的 config.yaml 文件中更新它们。
请注意，尽管在 thunderborg.cfg 文件中设置了默认值，但这些值将从下一个文件中给出的同名参数中设置。
config 文件夹包含文件 config.yaml。此文件将由启动文件用于在参数服务器中设置给定参数。这将允许我们配置系统而无需重新编译代码。
p_param: 0.5
i_param: 0.9
d_param: 0.006
pid:
use_pid: true
inertia_level: 0.1
wheels:
distance: 0.242
circumference: 0.317
speed:
# Plot x=thunderborg value, y=m/s
slope: 0.649776
y_intercept: -0.0788956
motor_diag_msg: false
参数如下：
p_parmPID 控制器的比例参数i_parmPID 控制器的积分参数d_parmPID 控制器的微分参数pid/use_pid如果设置为true，PID 控制器将用于控制电机速度pid/inertia_level标识一个电机速度值，在该值及以下机器人将不会移动wheels/distance是两个车轮之间的距离（米）wheels/circumference是车轮的周长（米）speed/slope是用于将速度（米/秒）转换为电机控制器使用的值的图表的斜率值speed/y_intercept是上述图表的 y 轴截距值speed/motor_diag_msg如果设置为true，系统会为每个电机发布一个诊断消息，可用于测试
ROS 节点本身的代码位于 src 子文件夹中的 thunderborg_node.py 文件中。该代码使用了库文件 ThunderBorg.py，其中包含来自 PiBorg 网站的代码，并位于 src/thunderborg_lib 子文件夹中。由于该库是用 Python 编写的，因此我们也将用 Python 编写我们的节点。
接下来，我将描述 ROS 节点本身。
main 例程初始化节点的 ROS，并创建一个 ThunderBorgNode 实例，然后进入一个循环，直到节点关闭。此循环以 20Hz 运行，并调用 ThunderBorgNode 的几个函数，这些函数将在下面描述。PublishStatus 将以 1Hz 的速率调用，而 PublishOdom 和 RunPIDs 在 20Hz 循环的每次迭代中调用。
def main(args):
rospy.init_node('thunderborg_node', anonymous=False)
rospy.loginfo("Thunderborg node started")
tbn = ThunderBorgNode()
rate = rospy.Rate(RATE)
status_time = rospy.Time.now()
while not rospy.is_shutdown():
if rospy.Time.now() > status_time:
tbn.PublishStatus()
status_time = rospy.Time.now() + rospy.Duration(1)
# Publish ODOM data
tbn.PublishOdom()
# Run the PIDs
tbn.RunPIDs()
rate.sleep()
if __name__ == '__main__':
main(sys.argv)
文件的其余部分包含 ThunderBorgNode 类。
ThunderBorgNode 的构造函数从参数服务器读取值。如果任何参数未加载到服务器中，则使用默认值。
如果 /pid/use_pid 参数设置为 true，则创建两个 PID 实例，每个电机一个。我们将使用的 PID 代码是 simple-pid 库。
构造函数然后创建一个用于与 Thunderborg 板通信的类的实例，然后检查板是否在设定的 I2C 地址上通信。
然后初始化一些变量，其中包括节点将发布和订阅的主题。
该节点将在 main_battery_status 主题上发布电池状态，在 raw_odom 主题上发布里程计数据，如果启用，则在 motor1_diag 和 motor2_diag 主题上发布电机 1 和 2 的诊断消息。
节点订阅的主题是 cmd_vel 主题，该主题由第四部分的 rodney 节点发布，并提供机器人所需的线速度和角速度，以及 tacho 主题，该主题将提供每个电机的 RPM。
class ThunderBorgNode:
def __init__(self):
self.__setup = False
# Read values from parameter server
# Using '~private_name' will prefix the parameters with the node name given in launch file
self.__use_pid = rospy.get_param('~pid/use_pid', False)
self.__wheel_distance = rospy.get_param('~wheels/distance', 0.23)
self.__wheel_circumference = rospy.get_param('~wheels/circumference', 0.34)
self.__speed_slope = rospy.get_param('~speed/slope', 1.5)
self.__speed_y_intercept = rospy.get_param('~speed/y_intercept', 0.4)
self.__inertia = rospy.get_param('~pid/inertia_level', 0.0)
self.__diag_msgs = rospy.get_param('~speed/motor_diag_msg', False)
pid_p = rospy.get_param('~p_param', 0.0)
pid_i = rospy.get_param('~i_param', 0.0)
pid_d = rospy.get_param('~d_param', 0.0)
if self.__use_pid == True:
# Configure the PIDs.
self.__pid1 = PID(pid_p, pid_i, pid_d, setpoint=0)
self.__pid1.sample_time = 0.05
# Limit the pid range. The PID will only work in positive values
self.__pid1.output_limits = (self.__inertia, 1.0)
self.__pid2 = PID(pid_p, pid_i, pid_d, setpoint=0)
self.__pid2.sample_time = 0.05
self.__pid2.output_limits = (self.__inertia, 1.0)
# We call dynamic server here after the PIDs are set up
# so the new PID values are set after the PIDs were created
srv = Server(ThunderborgConfig, self.DynamicCallback)
self.__thunderborg = thunderborg_lib.ThunderBorg() # create the thunderborg object
self.__thunderborg.Init()
if not self.__thunderborg.foundChip:
rospy.logdebug("ThunderBorg board not found")
else:
# Setup board to turn off motors if we don't send a message every 1/4 second
self.__thunderborg.SetCommsFailsafe(True)
# Motor velocity feedback values m/s
self.__feedback_velocity_right = 0.0
self.__feedback_velocity_left = 0.0
# Last motor direction
self.__fwd_right = True
self.__fwd_left = True
# Speed request in m/s
self.__speed_wish_right = 0.0
self.__speed_wish_left = 0.0
# Publish topics
self.__status_pub = rospy.Publisher("main_battery_status", BatteryState, queue_size=1)
self.__odom_pub = rospy.Publisher("raw_odom", Odometry, queue_size=50)
if self.__diag_msgs == True:
self.__diag1_pub = rospy.Publisher("motor1_diag", Vector3, queue_size=1)
self.__diag2_pub = rospy.Publisher("motor2_diag", Vector3, queue_size=1)
# ODOM values
self.__odom_x = 0.0
self.__odom_y = 0.0
self.__odom_th = 0.0
self.__last_odom_time = rospy.Time.now()
# Subscribe to topics
self.__vel_sub = rospy.Subscriber("cmd_vel",Twist, self.VelCallback)
self.__feedback_sub = rospy.Subscriber("tacho", tacho, self.TachoCallback)
函数 DynamicCallback 在初始化期间以及 PID 参数更改时被调用。此函数的主要目的是在动态重新配置期间将两个 PID 控制器的 PID 参数设置为新值。
第一次调用函数时，配置将保存为默认值。可以通过单击重新配置服务器上的“恢复到原始配置”框来返回此默认设置。
# Dynamic recofiguration of the PIDS
def DynamicCallback(self, config, level):
# First time called just store the configuration
if self.__setup == False:
self.__default_config = config
self.__setup = True
else:
# Check to restore config
if config.restore_defaults == True:
config = self.__default_config
config.restore_defaults = False
self.__pid1.tunings = (config.p_param, config.i_param, config.d_param)
self.__pid2.tunings = (config.p_param, config.i_param, config.d_param)
return config
函数 MotorSetting 是一个辅助函数，用于将所需电机速度从米/秒转换为 Thunderborg 板使用的值。Thunderborg 使用 0.0 到 1.0 的值来控制电机速度。例如，0.5 的值将使电机速度设置为其最大速度的一半。此函数使用由配置文件中给出的斜率和 y 轴截距值描述的直线图来计算电机值。如何计算斜率和 y 轴截距值将在文章后面给出。
# Function to calculate thunderborg setting from velocity
def MotorSetting(self, vel):
if vel == 0.0:
setting = 0.0
else:
setting = (abs(vel)-self.__speed_y_intercept)/self.__speed_slope
if vel < 0.0:
setting = -(setting)
return setting
每次收到 cmd_vel 主题消息时，都会调用 VelCallback 函数。根据 cmd_vel 消息中给出的速度（即线性 x 和角 z 速度），计算出左右电机所需的转速，这些是左右电机期望的转速值。
然后，辅助函数 MotorSetting 被调用两次，每个电机一次，以计算应传递给 Thunderborg 板以获得这些速度的值。
然后根据是否使用 PID 控制器以两种不同的方式使用这些电机值。
如果启用了 PID 控制器，则使用绝对值来设置相对 PID 控制器的设定点。设定点是 PID 控制器正在移动到的值。请注意，我们使用绝对值是为了简化 PID 控制器。我们稍后会添加符号以控制电机的实际方向。
如果未启用 PID 控制器，则我们直接调用 thunderborg 库来设置所需的电机速度。如果启用了诊断消息的发布，我们还将为每个电机发布一条消息。该消息包含所需的 m/s 速度、每个电机的当前反馈速度（m/s）以及传递给 Thunderborg 板的实际电机值。
# Callback for cmd_vel message
def VelCallback(self, msg):
# Calculate the requested speed of each wheel
self.__speed_wish_right = ((msg.angular.z * self.__wheel_distance) / 2) + msg.linear.x
self.__speed_wish_left = (msg.linear.x * 2) - self.__speed_wish_right
# Convert speed demands to values understood by the Thunderborg.
motor1_value = self.MotorSetting(self.__speed_wish_right)
motor2_value = self.MotorSetting(self.__speed_wish_left)
if self.__use_pid == True:
# Using the PID so update set points
self.__pid1.setpoint = abs(motor1_value)
self.__pid2.setpoint = abs(motor2_value)
if motor1_value == 0.0:
# Leave flag as is
pass
elif motor1_value < 0.0:
self.__fwd_right = False
else:
self.__fwd_right = True
if motor2_value == 0.0:
# Leave flag as is
pass
elif motor2_value < 0.0:
self.__fwd_left = False
else:
self.__fwd_left = True
else:
# Update the Thunderborg directly
self.__thunderborg.SetMotor1(motor1_value)
self.__thunderborg.SetMotor2(motor2_value)
if self.__diag_msgs == True:
motor1_state = Vector3()
motor1_state.x = self.__speed_wish_right
motor1_state.y = self.__feedback_velocity_right
motor1_state.z = motor1_value
motor2_state = Vector3()
motor2_state.x = self.__speed_wish_left
motor2_state.y = self.__feedback_velocity_left
motor2_state.z = motor2_value
self.__diag1_pub.publish(motor1_state)
self.__diag2_pub.publish(motor2_state)
每次收到 tacho 主题消息时，都会调用 TachoCallback 函数。此消息包含每个电机的 RPM，函数将这些值转换为 m/s 并存储以备后用。
# Callback for tacho message
def TachoCallback(self, msg):
# Store the feedback values as velocity m/s
self.__feedback_velocity_right = (msg.rwheelrpm/60.0)*self.__wheel_circumfrence
self.__feedback_velocity_left = (msg.lwheelrpm/60.0)*self.__wheel_circumfrence
PublishStatus 函数从主循环中每秒调用一次。它创建一个 BatteryState 消息实例，从 Thunderborg 板读取当前电池值，并用返回的值填充消息的电压元素。然后它将消息发布到 main_battery_status 主题。本主题已在本文章的第 4 部分中讨论。
# Publish the battery status
def PublishStatus(self):
battery_state = BatteryState()
# Read the battery voltage
try:
battery_state.voltage = self.__thunderborg.GetBatteryReading()
self.__status_pub.publish(battery_state)
except:
rospy.logwarn("Thunderborg node: Failed to read battery voltage");
RunPIDs 函数在主循环的每次迭代中都会被调用。它基本上将当前反馈值传递给 PID 控制器，并反过来获得当前值，因为 PID 正在向设定点移动。然后我们再将符号添加回此值，并调用以设置 Thunderborg 板的电机速度。如果启用了诊断消息，我们就会为每个电机发布诊断消息，此时，消息中的值是设定点值、PID 控制器的输出以及电机的当前反馈。
# Update the PIDs and set the motor speeds
def RunPIDs(self):
if self.__use_pid == True:
# Update PIDs and get next value.
if abs(self.__feedback_velocity_right) <= self.__inertia:
pid1_output = self.__pid1(self.__inertia)
else:
pid1_output = self.__pid1(self.MotorSetting(abs(self.__feedback_velocity_right)))
if abs(self.__feedback_velocity_left) <= self.__inertia:
pid2_output = self.__pid2(self.__inertia)
else:
pid2_output = self.__pid2(self.MotorSetting(abs(self.__feedback_velocity_left)))
if pid1_output <= self.__inertia:
motor1_speed = 0.0
elif self.__fwd_right == False:
motor1_speed = -(pid1_output)
else:
motor1_speed = pid1_output
if pid2_output <= self.__inertia:
motor2_speed = 0.0
elif self.__fwd_left == False:
motor2_speed = -(pid2_output)
else:
motor2_speed = pid2_output
# Set motor value
self.__thunderborg.SetMotor1(motor1_speed)
self.__thunderborg.SetMotor2(motor2_speed)
if self.__diag_msgs == True:
motor1_state = Vector3()
motor1_state.x = self.__pid1.setpoint
motor1_state.y = pid1_output
motor1_state.z = self.__feedback_velocity_right
motor2_state = Vector3()
motor2_state.x = self.__pid2.setpoint
motor2_state.y = pid2_output
motor2_state.z = self.__feedback_velocity_left
self.__diag1_pub.publish(motor1_state)
self.__diag2_pub.publish(motor2_state)
此类的最后一个函数是 PublishOdom 函数。此函数也会在主循环的每次迭代中被调用。根据每个电机的速度反馈值，它会计算当前的前向速度和角速度。根据这些速度以及自上次调用以来经过的时间，我们计算自上次调用以来在 x 方向上移动的距离和围绕 z 轴的旋转。然后将这些值添加到里程计值中。旋转需要采用四元数形式，因此我们使用 ROS 变换包中的辅助函数来执行转换。然后该函数会创建一个里程计消息，并用里程计数据和当前速度填充它。然后将此消息发布到 raw_odom 主题。此主题将用于生成 odom 主题，该主题将由 ROS 导航系统在手动模式下用于映射位置，并在自主模式下用于导航同一位置。当然，我们还需要来自 LIDAR（光探测和测距）的数据才能实现这些导航功能，我们将在下一篇文章中进行探讨。
本文稍后将介绍我们如何使用原始里程计数据生成导航系统使用的里程计数据。
# Publish odometry data
def PublishOdom(self):
forward_velocity = (self.__feedback_velocity_left + self.__feedback_velocity_right)/2
angular_velocity = 2*(self.__feedback_velocity_right - forward_velocity)/self.__wheel_distance
# Now the x and y velocities
velocity_x = forward_velocity
velocity_y = 0.0
# Note: As we don't receive velocity y we could remove all reference to it below, but
# setting it to zero means we can keep the code generic below for future reference
# Compute odometry from the calculate velocities
current_time = rospy.Time.now()
delta_time = (current_time - self.__last_odom_time).to_sec() # Floating point seconds
delta_x = (velocity_x * cos(self.__odom_th) - velocity_y * sin(self.__odom_th)) * delta_time
delta_y = (velocity_x * sin(self.__odom_th) + velocity_y * cos(self.__odom_th)) * delta_time
delta_th = angular_velocity * delta_time
# Add the latest calculated movement
self.__odom_x += delta_x
self.__odom_y += delta_y
self.__odom_th += delta_th
# we need Yaw in a Quaternion
odom_quat = quaternion_from_euler(0, 0, self.__odom_th)
# Next publish the odometry message over ROS
odom = Odometry()
odom.header.stamp = current_time
odom.header.frame_id = 'odom'
odom.child_frame_id = 'base_footprint'
odom.pose.pose = Pose(Point(self.__odom_x, self.__odom_y, 0.0), Quaternion(*odom_quat))
odom.pose.covariance = [0.001, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.001, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.001]
odom.twist.twist = Twist(Vector3(velocity_x, velocity_y, 0), Vector3(0, 0, angular_velocity))
odom.twist.covariance = [0.0001, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0001, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0001]
# Publish the message
self.__odom_pub.publish(odom)
self.__last_odom_time = current_time
在 src 文件夹中，还有一个非 ROS Python 脚本 test.py，我们将用它来测试计算用于将车轮速度转换为电机速度设置的图表的斜率和 y 轴截距。此文件的使用将在本文后面解释。
#!/usr/bin/env python
import thunderborg_lib
import time
# Run with different values 0.1, 0.2, 0.3, 0.4, 0.5, 0.6, 0.7, 0.8, 0.9, 1.0
# WARNING ensure your robot has room to run for 5 seconds at the set speed or change the sleep time!
speed = 0.5
TB = thunderborg_lib.ThunderBorg() # create the thunderborg object
TB.Init()
if not TB.foundChip:
print("ThunderBorg board not found")
else:
# Set both motor speeds
TB.SetMotor1(speed)
TB.SetMotor2(speed)
time.sleep(5)
TB.SetMotor1(0.0)
TB.SetMotor2(0.0)
融合里程计数据
里程计数据只是对位置和姿态的估计，并且会随时间漂移。我们可以通过使用卡尔曼扩展滤波器将原始里程计数据与IMU（惯性测量单元）的数据融合来改进它。现在我不会在这里添加IMU，那是未来的事情，但我会包含一个ROS卡尔曼扩展滤波器节点，我们将为IMU做好准备。该节点将忽略它没有接收IMU数据的事实，并将发布里程计数据。它还将广播odom转换数据。有关转换的更多信息可在ROS Wiki上找到。
我们将使用的软件包是 robot_localization 软件包，其文档可在此处找到。
在 rodney/config 文件夹中，我为节点添加了一个 robot_localization.yaml 配置文件。它基本上说明我们是一个二维规划机器人，应该使用哪些消息中的哪些数据，以及我们想要广播 tf 数据。
frequency: 40
sensor_timeout: 1
two_d_mode: true
publish_tf: true
print_diagnostics: false # Set to true for debug
odom_frame: odom
base_link_frame: base_footprint
world_frame: odom
odom0: /raw_odom
imu0: /imu/data
odom0_config: [false, false, false,
false, false, false,
true, true, false,
false, false, true,
false, false, false]
odom0_differential: false
imu0_config: [false, false, false,
false, false, false,
false, false, false,
false, false, true,
false, false, false]
imu0_differential: false
odom0_relative: false
imu0_relative: true
process_noise_covariance": [0.05, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0.05, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0.06, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0.03, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0.03, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0.06, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0.025, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0.025, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0.04, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.02, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.015]
initial_estimate_covariance: [1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9]
我们将从 rodney.launch 文件启动节点，并通过添加以下行将输出主题重映射到 odom。
<node pkg="tf2_ros" type="static_transform_publisher" name="base_footprint_broadcaster" args="0 0 0.09 0 0 0 /base_footprint /base_link"/>
<node pkg="robot_localization" type="ekf_localization_node" name="ekf_localization_node">
<remap from="odometry/filtered" to="odom"/>
<rosparam command="load" file="$(find rodney)/config/robot_localization.yaml"/>
</node>
第一行将创建一个节点，该节点将广播 base_footprint 帧和 base_link 帧之间的静态变换，这是 ekf 定位节点所必需的。
Rodney 节点包的更新
在本文的上一部分中，我们开发了 rodney 和 rodney_missions 节点，随着 Rodney 的开发进展，我们需要向这两个节点添加功能。在这里，我们将对 rodney 包进行一些更新。
主要的更改是更新在 Arduino Nano 板上运行的草图。目前，该草图订阅 servo 主题以设置云台设备的位置，在这里我们还将使用它来监测每个电机的霍尔效应传感器并发布带有每个电机 RPM 的 tacho 主题。
每个霍尔传感器都有两个输出（A和B），当电机旋转时会产生脉冲，我们可以用Arduino监测这些输出以检测速度和方向。传感器的输出A将连接到Arduino的中断引脚，输出B将连接到数字输入引脚。
在草图的 setup 函数中，我们将添加代码将 B 引脚设置为输入，并将 A 引脚连接到中断 0 和 1。
然后我们将编写两个中断服务例程，WheelSpeed0 和 WheelSpeed1，它们将记录收到的脉冲数量，并根据引脚 A 和 B 的关系计算电机方向。
在 loop 函数中，我们每隔 50 毫秒将脉冲计数转换为每个电机的 RPM 值，重置计数，并将 RPM 值发布到 tacho 主题。
完整的草图如下所示
/*
* This version controls upto four RC Servos and publishes the tacho message monitoring
* two motors.
*
* The node subscribes to the servo topic and acts on a rodney_msgs::servo_array message.
* This message contains two elements, index and angle. Index references the servos 0-3 and
* angle is the angle to set the servo to 0-180.
*
* D2 -> INT0 used for monitoring right motor speed
* D3 -> INT1 used for monitoring left motor speed
* D4 -> Digital input used for sensing right motor direction
* D5 -> PWM servo indexed 2
* D6 -> PWM servo indexed 1
* D7 -> Digital input used for sensing left motor direction
* D9 -> PWM servo indexed 0
* D10 -> PWM servo indexed 3
*/
#if (ARDUINO >= 100)
#include <Arduino.h>
#else
#include <WProgram.h>
#endif
#include <Servo.h>
#include <ros.h>
#include <servo_msgs/servo_array.h>
#include <tacho_msgs/tacho.h>
// Define the PWM pins that the servos are connected to
#define SERVO_0 9
#define SERVO_1 6
#define SERVO_2 5
#define SERVO_3 10
// Define pins used for two Hall sensors
#define ENCODER0_PINA 2 // Interrupt 0
#define ENCODER0_PINB 4
#define ENCODER1_PINA 3 // Interrupt 1
#define ENCODER1_PINB 7
#define GEAR_BOX_COUNTS_PER_REV 1440.0f
ros::NodeHandle nh;
Servo servo0;
Servo servo1;
Servo servo2;
Servo servo3;
tacho_msgs::tacho tachoMsg;
byte encoder0PinALast;
byte encoder1PinALast;
volatile int encoder0Count; // Number of pulses
volatile int encoder1Count; // Number of pulses
volatile boolean encoder0Direction; //Rotation direction
volatile boolean encoder1Direction; //Rotation direction
void servo_cb( const servo_msgs::servo_array& cmd_msg)
{
/* Which servo to drive */
switch(cmd_msg.index)
{
case 0:
nh.logdebug("Servo 0 ");
servo0.write(cmd_msg.angle); //set servo 0 angle, should be from 0-180
break;
case 1:
nh.logdebug("Servo 1 ");
servo1.write(cmd_msg.angle); //set servo 1 angle, should be from 0-180
break;
case 2:
nh.logdebug("Servo 2 ");
servo2.write(cmd_msg.angle); //set servo 2 angle, should be from 0-180
break;
case 3:
nh.logdebug("Servo 3 ");
servo3.write(cmd_msg.angle); //set servo 3 angle, should be from 0-180
break;
default:
nh.logdebug("No Servo");
break;
}
}
ros::Subscriber<servo_msgs::servo_array> sub("servo", servo_cb);
ros::Publisher pub("tacho", &tachoMsg);
void setup()
{
nh.initNode();
nh.subscribe(sub);
nh.advertise(pub);
servo0.attach(SERVO_0); //attach it to the pin
servo1.attach(SERVO_1);
servo2.attach(SERVO_2);
servo3.attach(SERVO_3);
encoder0Direction = true; // default is forward
encoder1Direction = true;
encoder0Count = 0;
encoder1Count = 0;
pinMode(ENCODER0_PINB, INPUT);
pinMode(ENCODER1_PINB, INPUT);
// Attach the interrupts for the Hall sensors
attachInterrupt(0, WheelSpeed0, CHANGE); // Int0 is pin 2
attachInterrupt(1, WheelSpeed1, CHANGE); // Int1 is pin 3
// Defaults
servo0.write(90);
servo1.write(120);
}
unsigned long publisherTime;
unsigned long currentTime;
unsigned long lastTime;
float deltaTime;
void loop()
{
// Is it time to publish the tacho message
if(millis() > publisherTime)
{
currentTime = micros();
deltaTime = (float)(currentTime - lastTime)/1000000.0;
// Right wheel speed
tachoMsg.rwheelrpm = (((((float)encoder0Count)/2.0f)/deltaTime)/GEAR_BOX_COUNTS_PER_REV)*60.0f;
encoder0Count = 0;
// Left wheel speed
tachoMsg.lwheelrpm = (((((float)encoder1Count)/2.0f)/deltaTime)/GEAR_BOX_COUNTS_PER_REV)*60.0f;
encoder1Count = 0;
lastTime = currentTime;
pub.publish(&tachoMsg);
publisherTime = millis() + 50; // Publish at 20Hz
}
nh.spinOnce();
}
// ISR 0
void WheelSpeed0()
{
int state = digitalRead(ENCODER0_PINA);
if((encoder0PinALast == LOW) && (state == HIGH))
{
int val = digitalRead(ENCODER0_PINB);
if(val == LOW && encoder0Direction)
{
encoder0Direction = false; // Reverse
}
else if (val == HIGH && !encoder0Direction)
{
encoder0Direction = true; // Forward
}
}
encoder0PinALast = state;
if(!encoder0Direction)
{
encoder0Count++;
}
else
{
encoder0Count--;
}
}
// ISR 1
void WheelSpeed1()
{
int state = digitalRead(ENCODER1_PINA);
if((encoder1PinALast == LOW) && (state == HIGH))
{
int val = digitalRead(ENCODER1_PINB);
if(val == LOW && encoder1Direction)
{
encoder1Direction = false; // Reverse
}
else if (val == HIGH && !encoder1Direction)
{
encoder1Direction = true; // Forward
}
}
encoder1PinALast = state;
if(!encoder1Direction)
{
encoder1Count++;
}
else
{
encoder1Count--;
}
}
在 rodney_node.cpp 文件中的 rodney 节点代码中，我也做了一些更改。
第一个改变与电机速度到目标值的斜坡式加速有关。此功能旨在防止如果我们在速度上进行大幅度突变，机器人可能发生的打滑和抖动。现在 PID 控制器也以受控方式改变速度，因此如果启用了 PID，我们就不需要在 rodney 节点中使用斜坡功能。
在 RodneyNode 类构造函数中，我添加了以下行以从参数服务器读取 PID 控制器是否启用。
nh_.param("/thunderborg_node/pid/use_pid", pid_enabled_, false);
为了完成此更改，我编辑了 sendTwist 函数，使其仅在未启用 PID 控制器时才使用斜坡功能。
void RodneyNode::sendTwist(void)
{
geometry_msgs::Twist target_twist;
// If in manual locomotion mode use keyboard or joystick data
if(manual_locomotion_mode_ == true)
{
// Publish message based on keyboard or joystick speeds
if((keyboard_linear_speed_ == 0) && (keyboard_angular_speed_ == 0))
{
// Use joystick values
target_twist.linear.x = joystick_linear_speed_;
target_twist.angular.z = joystick_angular_speed_;
}
else
{
// use keyboard values
target_twist.linear.x = keyboard_linear_speed_;
target_twist.angular.z = keyboard_angular_speed_;
}
}
else
{
// Use mission demands (autonomous)
target_twist.linear.x = linear_mission_demand_;
target_twist.angular.z = angular_mission_demand_;
}
// If not using the PID ramp to the target value.
if (false == pid_enabled_)
{
ros::Time time_now = ros::Time::now();
// Ramp towards are required twist velocities
last_twist_ = rampedTwist(last_twist_, target_twist, last_twist_send_time_, time_now);
last_twist_send_time_ = time_now;
// Publish the Twist message using the ramp value
twist_pub_.publish(last_twist_);
}
else
{
// Publish the Twist message using the target value
twist_pub_.publish(target_twist);
}
}
第二个变化旨在提高使用操纵杆控制机器人移动时对操纵杆输入的响应。
在之前的版本中，我们只是忽略了死区内的任何输入值，并为了缩放死区外的值，我们将操纵杆值乘以最大速度除以最大操纵杆值。这意味着移动死区的影响并不意味着死区上方的第一个值将正确映射到较低的速度。为了改进操纵杆值到正确速度的映射，我们现在使用直线图，但根据给定的死区值计算斜率和y轴截距。
在 RodneyNode 类构造函数中，我添加了以下代码来计算两个图的斜率和 y 轴截距，一个用于线速度，另一个用于角速度。
// Calculate the slope and y-intercept of the joytick input against linear speed
lslope_ = max_linear_speed_/(MAX_AXES_VALUE_-dead_zone_);
lyintercept_ = -(lslope_*dead_zone_);
// Calculate the slope and y-intercept of the joytick input against angular speed
aslope_ = max_angular_speed_/(MAX_AXES_VALUE_-dead_zone_);
ayintercept_ = -(aslope_*dead_zone_);
现在在 joystickCallback 中，如果值超出死区，我们使用相关图的斜率和 y 轴截距从操纵杆输入计算速度。
void RodneyNode::joystickCallback(const sensor_msgs::Joy::ConstPtr& msg)
{
float joystick_x_axes;
float joystick_y_axes;
// manual locomotion mode can use the joystick/game pad
joystick_x_axes = msg->axes[angular_speed_index_];
joystick_y_axes = msg->axes[linear_speed_index_];
// Check for manual movement
// Check dead zone values
if(abs(joystick_y_axes) < dead_zone_)
{
joystick_linear_speed_ = 0.0f;
}
else
{
joystick_linear_speed_ = (lslope_*(float)abs(joystick_y_axes))+lyintercept_;
if(joystick_y_axes > 0.0f)
{
joystick_linear_speed_ = -joystick_linear_speed_;
}
}
// Check dead zone values
if(abs(joystick_x_axes) < dead_zone_)
{
joystick_angular_speed_ = 0.0f;
}
else
{
joystick_angular_speed_ = (aslope_*(float)abs(joystick_x_axes))+ayintercept_;
if(joystick_x_axes > 0.0f)
{
joystick_angular_speed_ = -joystick_angular_speed_;
}
}
...
在 keyboardCallBack 函数中，我添加了一个 "else if" 结构，如果按下 'r' 或 'R' 键，则重置里程计。它调用 ekf_localization_node 的 SetPose 服务，并将位置和旋转都设置为零。
else if((msg->code == keyboard::Key::KEY_r) && ((msg->modifiers & ~RodneyNode::SHIFT_CAPS_NUM_LOCK_) == 0))
{
// 'r' or 'R', reset odometry
ros::ServiceClient client = nh_.serviceClient<robot_localization::SetPose>("set_pose");
robot_localization::SetPose srv;
srv.request.pose.header.frame_id ="odom";
client.call(srv);
}
在 rodney.launch 文件中，除了 EKF 节点的更改外，我还添加了两行新的代码来加载 thunderborg 配置并启动 thunderborg_node 节点。
<node pkg="thunderborg" type="thunderborg_node.py" name="thunderborg_node" output="screen">
<rosparam command="load" file="$(find thunderborg)/config/config.yaml"/>
</node>
Using the Code
和往常一样，我会在机器人硬件上运行代码，并在 Linux PC 上运行操纵杆节点和测试应用程序。在下面的详细信息中，我会将这台 PC 称为工作站。当然，您也可以直接将操纵杆连接到机器人，并在机器人硬件上运行操纵杆节点。我还会通过使用 test.py 脚本，展示如何计算 thunderborg config.yaml 文件的斜率和 y 轴截距值。
在Pi上构建ROS包（机器人硬件）
如果尚未完成，请在 Raspberry Pi 上创建一个 catkin 工作区，并使用以下命令初始化它
$ mkdir -p ~/rodney_ws/src
$ cd ~/rodney_ws/
$ catkin_make
将 face_recognition、face_recognition_msgs、head_control、pan_tilt、rondey、rodney_missions、servo_msgs、speech、thunderborg、tacho_msgs 和 ros-keyboard（来自https://github.com/lrse/ros-keyboard）等软件包复制到 ~/rodney_ws/src 文件夹中。
使用以下命令构建代码
$ cd ~/rodney_ws/
$ catkin_make
检查构建是否无任何错误地完成。
您还需要编译 Arduino 代码并下载到 Nano 中。
在工作站上构建ROS包
在工作站上，我们希望运行键盘和操纵杆节点，以便远程控制实际的机器人硬件。
使用以下命令创建一个工作空间
$ mkdir -p ~/test_ws/src
$ cd ~/test_ws/
$ catkin_make
将 rodney、joystick、odom_test（来自 Robotics-test-code 文件夹）和 ros-keyboard（来自https://github.com/lrse/ros-keyboard）包复制到 ~/test_ws/src 文件夹中，然后使用以下命令构建代码
$ cd ~/test_ws/
$ catkin_make
检查构建是否无任何错误地完成。
提示
当在工作站和 Raspberry Pi 上运行 ROS 代码和工具时，在多个终端中可能会有大量重复的命令输入。在接下来的部分中，我将包含完整的输入命令，但这里有一些可以为您节省大量输入时间的技巧。
在 Raspberry Pi 上，为了节省输入 "source devel/setup.bash"，我已将其添加到 Raspberry Pi 的 .bashrc 文件中。
$ cd ~/
$ nano .bashrc
然后将 "source /home/ubuntu/rodney_ws/devel/setup.bash" 添加到文件末尾，保存并退出。
在工作站上运行测试代码和工具时，它也需要知道 ROS master 的位置，所以我已经将以下内容添加到工作站的 .bashrc 文件中。
alias rodney='source ~/test_ws/devel/setup.bash; \
export ROS_MASTER_URI=http://ubiquityrobot:11311'
然后只需在终端输入 "rodney"，这两个命令就会运行，省去了大量的输入。
您还可以节省一些输入时间，因为某些 ROS 工具支持 TAB 补全。例如，在下面的命令中，输入 rosrun rosserial_，然后按 Tab 键自动补全 rosserial_python。
运行图表测试代码
以下可用于计算电机速度到电机控制值图的斜率和 y 轴截距。
将 Arduino 连接到 USB 端口后，在终端中使用以下命令在机器人硬件上启动 ROS 串行节点。
$ rosrun rosserial_python serial_node.py args="/dev/ttyUSB0"
在输出消息中应该有一条关于发布 tacho_msg 的消息。
在机器人硬件上，编辑 rodney/src/thunderborg/src 文件夹中的 test.py 文件，将 speed 变量设置为 "1.0"。这是最大速度，因此请确保机器人在最大速度下有足够的空间沿直线移动 5 秒。
在工作站上，我们将记录 tacho 消息。在工作站的终端中，输入以下命令以记录 tacho 消息。
$ cd ~/test_ws
$ source devel/setup.bash
$ export ROS_MASTER_URI=http://ubiquityrobot:11311
$ rosbag record /tacho
应该会显示一条消息，告知 rosbag 已订阅 /tacho 以及它正在记录到的文件名。
在机器人硬件的另一个终端中，输入以下命令来运行电机
$ cd ~/rodney_ws/src/thunderborg/src
$ ./test.py
机器人应该向前移动5秒。在工作站上，按 Ctrl-c 停止录制。
接下来，我们将把 rosbag 文件转换为逗号分隔变量文件，以便我们可以在像 Excel 或 LibreOffice Calc 这样的电子表格应用程序中检查它。在工作站的同一个终端中输入以下命令，将文件名替换为您启动 rosbag 时给出的文件名。
$ rostopic echo -b 2019-01-28-12-59-22.bag -p /tacho>output10.csv
在电子表格应用程序中打开 csv 文件，并告知它您的数据由逗号分隔。删除机器人开始移动前和停止移动后所有 RPM 值为零的行。然后删除机器人加速到设定速度和减速的行。
接下来，在B列和C列底部，使用电子表格计算每列的平均值，例如，公式 "=AVERAGE(B2:B102)"。
然后使用电子表格找出左右车轮速度平均值的平均值。将此值除以 60.0，并将结果乘以您的车轮周长（以米为单位）。这将为您提供以米/秒为单位的平均速度，将此值记录为测试脚本中设置的电机值的平均电机速度。
继续重复上述测试，将测试脚本 speed 值设置为 0.9、0.8、0.7、0.6、0.5、0.4、0.3、0.2 和 0.1。在某个时刻，由于您的机器人重量，机器人将不会移动，此时 speed 变量的值成为 thunderborg/config.yaml 文件中的 pid/inertia_level 值。
记录下每个变量值的所有平均速度后，再次打开电子表格，并按照如下所示在A列和B列中输入值，其中B列使用您计算出的值。
现在，为了计算 D2 单元格中的图表斜率，输入以下公式 "=SLOPE(B2:B10,A2:A10)"，在 E2 单元格中，输入 "=INTERCEPT(B2:B10,A2:A10)"。结果值将为您提供在 config.yaml 文件中输入 speed/slope 和 speed/y_intercept 的值。在您的斜率和 y 轴截距公式中只使用使机器人移动的值。您可以从电子表格中看到，电机值为 0.1 时机器人没有移动，所以我的图表从 0.2 开始。
在机器人硬件上测试 ROS 代码
在机器人硬件上，运行以下命令以启动系统中所有当前节点
$ cd ~/rodney_ws
$ source devel/setup.bash
$ roslaunch rodney rodney.launch
在工作站上，运行以下命令以启动键盘节点
$ cd ~/test_ws
$ source devel/setup.bash
$ export ROS_MASTER_URI=http://ubiquityrobot:11311
$ rosrun keyboard keyboard
一个标题为“ROS 键盘输入”的小窗口应该正在运行。
在工作站的第二个终端中，运行以下命令来启动操纵杆节点。当然，如果您没有连接操纵杆，您可以使用键盘来控制机器人。
$ cd ~/test_ws
$ source devel/setup.bash
$ export ROS_MASTER_URI=http://ubiquityrobot:11311
$ rosrun joystick joystick_node
应显示一条消息，表明节点已启动。
使用文章第四部分中给出的相同键盘命令或操纵杆操作，您现在应该能够控制机器人。
使用 rviz 和 rqt_image_view 应用程序，您应该能够远程控制机器人。在 rviz 中，您将看到机器人姿态，以便您可以检查摄像头指向何处；在 rqt_image_view 中，您可以看到来自机器人的摄像头馈送。还可以配置 rviz 以显示摄像头馈送。
要在工作站上启动 rviz，请使用以下命令包含 Rodney 的 URDF 模型。
$ cd ~/test_ws
$ source devel/setup.bash
$ export ROS_MASTER_URI=http://ubiquityrobot:11311
$ roslaunch rodney rviz.launch
要在工作站上启动 rqt_image_view，请使用以下命令
$ export ROS_MASTER_URI=http://ubiquityrobot:11311
$ rqt_image_view
下图显示了 rqt_image_view 中的摄像头画面和 rviz 中的机器人姿态。rviz 中显示的 xyz 轴是通电时的里程计值，因此您可以看到自通电以来的移动方向和旋转。
接下来，我们将测试里程计消息的生成。
确保键盘窗口获得焦点，然后按 'r' 键重置里程计数据。
在地板上标记机器人位置。使用键盘或操纵杆移动机器人。
在同一终端中，输入以下内容以查看当前里程计消息
$ rostopic echo /odom
从消息中，记下当前姿态位置，x 和 y。测量机器人从标记位置移动的距离，并确认它是一个很好的近似值。
使用键盘或操纵杆旋转机器人。姿态方向不容易比较，至少对我来说是这样，因为它是一个四元数。您可以在 rviz 中看到方向，但您也可以使用测试脚本 odom_test 来确认角度（以度为单位）。
在工作站的终端中，按 Ctrl-C，然后输入以下命令
$ rosrun odom_test odom_test1_node.py
该节点将每秒打印出以弧度和度表示的姿态方向。
如果 odometry 没有给出很好的近似值，那么您可以调整 thunderborg config.yaml 文件中的轮距和周长值，以帮助改善结果。odometry 值是使用多个积分计算得出的，因此它永远只是一个近似值。
下面的视频展示了使用摄像头画面和 rviz 远程控制 Rodney。
调整 PID
您可以使用动态重新配置即时调整 PID 控制器参数。在机器人硬件上运行 ROS 代码的同时，使用以下命令在工作站上运行动态重新配置节点
$ cd ~/test_ws
$ source devel/setup.bash
$ export ROS_MASTER_URI=http://ubiquityrobot:11311
$ rosrun rqt_reconfigure rqt_reconfigure
在左侧面板中，选择 thunderborg 以显示可动态重新配置的参数。
当调整值时，左右电机的 PID 都会重新配置，以便您可以立即看到更改的效果。一旦您对这些值满意，请编辑 config.yaml 文件，设置新的默认值。下次启动 thunderborg 节点时，它将使用新值。
如果您不想使用 PID 来控制电机，可以在 thunderborg config.yaml 文件中将 use_pid 设置为 false 来禁用它。
机器人硬件
在本文的不同部分中，我提到了何时向项目中添加了新硬件，但没有包含所有部件的列表，因此迄今为止项目的完整物料清单可在此处获取。
展望未来，当我添加 LIDAR 时，我希望将 LIDAR 的控制权交给 Arduino，这样 Raspberry Pi 将再次只使用 Arduino 生成的 ROS 消息。然而，Arduino 的内存已经不足，所以我可能会在项目中添加第二个 Arduino Nano，或者可能会将当前的 Nano 替换为 Teensy。到目前为止编写的 Arduino 代码与 Teensy 兼容，通过向 Arduino IDE 添加插件，您可以使用相同的 IDE 编译和下载代码。一些 Teensy 在信号上不耐 5V，因此可能还需要进行电平转换。
在文章的第一部分，我提到了我在 Raspberry Pi 上使用的 Ubiquity Robot Image。关于如何安装图像、安装额外软件以及为项目配置图像的说明可在此处获取。
当前电路图的全尺寸图像可在图表 zip 文件夹中找到，以及显示所有节点和主题的 rqt_graph 图像的全尺寸副本。
从电路图可以看出，10 节 AA 电池为 Thunderborg 板、电机供电，并通过 Thunderborg 向 Raspberry Pi 提供 5V 电源。Nano 由 Raspberry Pi 供电。USB 电源组提供两个不同的 5V 电轨，2.1A 输出为 RC 伺服器和电机上的霍尔传感器供电，1.0A 输出为显示器、音频放大器和 CPU 风扇供电。
与左侧电机相比，右侧电机的电源及其霍尔传感器的 A/B 信号是颠倒的。这样做的目的是为了让软件无论控制哪个电机，都能使用正值来驱动电机前进和确定霍尔传感器的前进方向。
处于当前构建状态的罗德尼机器人。
USB 电源组的两个输出连接到两个 Micro USB 分线板。
罗德尼机器人底座。迷你蓝牙键盘在不使用时，通过魔术贴垫固定收纳。该键盘省去了测试期间将全尺寸 USB 键盘连接到 Raspberry Pi 的麻烦。
后平台放置 Arduino Nano、Thunderborg 和一个连接板。
显示屏背面展示了 Raspberry Pi、摄像头和带音频放大器的 Vero 板。此板还用作 Pi 到 Thunderborg 的 I2C 连接的连接点。
现在，您可能已经看出，Rodney 很大程度上是一个原型，因此，未来已经有计划改变一些设计。用于头部的塑料管过于柔韧，未来可能会用木质销钉或扫帚柄的一部分代替。机器人底座是矩形的，因为它是为四轮设计的，现在机器人只有两轮，底座可以改为圆形设计，车轮从底部底座突出。我想用一个可现场充电的电池替换十节 AA 电池和 USB 电源组，从而允许机器人在电池需要充电时返回充电站。但正如我所说，那是未来的事情，在此之前我们需要使机器人完全自主。
关注点
在这一部分中，我们已经完成了设计目标3，即通过远程键盘和/或操纵杆控制移动。我还制作了当前结构的电路图和物料清单。
下一步将是为罗德尼添加激光测距仪以完成设计目标4，我们还将添加 IMU 以改善里程计。
历史
- 2019/02/05：首次发布
- 2019/03/21：更改了 thunderborg 节点以发布原始里程计数据，并添加了 robot_localization 包，该包将用于将原始里程计数据与 IMU 数据融合
- 2019/04/07:
增加了在重新配置中恢复默认值的功能
PID 参数现在不仅从重新配置中设置，还从配置文件中设置
参数现在使用私有名称
基座高度已移除，因为子帧是 base_footprint 而非 base_link
为 odom 消息添加了协方差数据
之前错误地计算了 y 速度。应该只使用 x 速度和绕 z 轴的速度