Rodney - 漫长的自主机器人之路（第六部分）。
5.00/5 (5投票s)
关于 ROS（机器人操作系统）家用机器人的系列文章的第六部分。
引言
Rodney 机器人项目是一个业余机器人项目，旨在设计和构建一个使用 ROS（机器人操作系统）的自主家用机器人。本文是描述该项目的系列文章中的第六篇。
背景
在第 1 部分中，为了帮助定义我们机器人的需求，我们选择了我们的第一个任务，并将其分解为多个设计目标，使其更易于管理。
该任务取自文章让我们建造一个机器人！，任务是：给……传达消息 - 既然机器人将[拥有]识别家庭成员的能力，那么它能否充当“消息传递者和提醒者”呢？我可以对机器人说“机器人，提醒（人名）在下午 6 点到车站接我”。然后，即使那个家庭成员的手机处于静音状态，或者正在听嘈杂的音乐，或者（插入任何不来车站接我的理由），机器人也可以在房子里漫游，找到那个人，并向他们传达消息。
该任务的设计目标是
- 设计目标1：能够使用摄像头环顾四周，搜索人脸，尝试识别任何看到的人，并显示识别出来的信息
- 设计目标2：面部表情和语音合成。罗德尼需要能够传递信息
- 设计目标3：通过远程键盘和/或操纵杆控制移动
- 设计目标4：增加激光测距仪或类似的测距传感器以辅助导航
- 设计目标5：自主移动
- 设计目标6：任务分配和完成通知
在上一部分中，我们添加了电机控制和里程计反馈来完成设计目标 3。在这一部分中，我将添加一个旋转 LIDAR（光检测和测距）来完成设计目标 4，并添加一个 IMU 以改进里程计。在添加 IMU 的过程中，我还会将 Arduino Nano 更换为 Teensy 3.5。
添加 LIDAR
ROS 中自主导航最常用的方法需要订阅 /scan 和 /tf 主题。/tf 主题用于获取里程计变换，我们已在上一篇文章中开始从 ekf_localization_node 广播它。/scan 主题包含来自激光扫描设备的数据。
您可以在开发中的自动驾驶汽车上看到一些大型且非常昂贵的 360 度扫描设备，但由于 Rodney 将被限制在房子里，我们可以大幅降低价格范围。Slamtec 生产了许多旋转 LIDAR，他们的RPLidar A1 价格合理，约为 100 英镑。RPLidar 的量程为 12 米，提供完整的 360 度扫描，并使用串行接口进行通信。但还有更好的，他们甚至开发了一个 ROS 节点，可以从 Slamtec GitHub 页面下载。因此，我们可以将此设备直接插入我们的支持 ROS 的机器人，并开始在 /scan 主题上接收消息，而无需弄清楚它正在传输什么串行数据。
下面是安装在 Rodney 上的 RPLidar 的照片。
我购买的 RPLidar 版本是开发套件，带有一个 USB 串口设备和连接此设备到 LIDAR 的电缆。除了 Tx/Rx 数据线，它还为 LIDAR 电机和 LIDAR 核心提供 5V 电源。还有一个 PWM 输入到核心，可用于控制电机速度，如果您使用提供的 USB 串口设备，此线连接到 DTR 信号，可用于启用/禁用设备。
由于我试图在 Rodney 上不同的电源之间分配负载，我决定从不同的电源为电机和核心供电，并且只通过 USB 电源从 Raspberry Pi 为 USB 串口设备供电。因此，我制作了一个简单的分线板，以便我可以单独为 LIDAR 供电。下图显示了安装 LIDAR 的平台后部的分线板和 USB 串口设备。单独的电源通过照片中未连接的 3 个引脚接入。平台和支架的 stl 文件可在本文随附的 3D 打印 zip 文件中找到。
现在，一直关注这些文章的一些人可能已经发现了一个问题。尽管我将 RPLidar 安装得足够高，以便后端平台上的电子设备低于其激光器，但 Rodney 的脖子会挡住激光器，从而导致在 360 度视野中始终检测到。有一个现有的名为 laser filters 的 ROS 包，它允许我们使用一个名为 LaserScanAngularBoundsFilter 的插件来移除消息中覆盖颈部区域的点。我们将使用它来移除 180 度标记两侧各 10 度的点。
为了添加 RPLidar 的代码，我们将编辑本文前面部分创建的一些包，即 rodney 和 rodney_missions。
激光滤波器节点将从我们的 rondey.launch 文件启动，并且需要一个我们将加载到 ROS 参数服务器的配置文件。因此，我们需要将以下名为 laser_filter_config.yaml 的文件添加到 rodney/config 文件夹中。
scan_filter_chain:
- name: angle
type: laser_filters/LaserScanAngularBoundsFilter
params:
lower_angle: -2.96706
upper_angle: 2.96706
下限和上限角度以弧度表示，这将把我们的视野限制在 0 到 -170 度和 0 到 +170 度之间。
为了将这些参数加载到参数服务器并启动扫描过滤器，我们将在 rodney/launch 文件夹中的 rodney.launch 文件中添加以下几行。我们将对启动文件进行更多更改，因此我将在本文后面完整展示它。
<node pkg ="laser_filters" type="scan_to_scan_filter_chain"
name="scan_to_scan_filter_chain" output="screen">
<rosparam command="load" file="$(find rodney)/config/laser_filter_config.yaml"/>
<remap from="scan" to="scan_filter_input"/>
<remap from="scan_filtered" to="scan"/>
</node>
从上面可以看出，我们需要对主题名称进行一些重新映射，节点的输出主题是 scan_filtered，但导航包将订阅 scan。我们还将 RPLidar 发布的主题从 scan 重新映射到 scan_filter_input。
接下来，我们需要启动 Slamtec 提供的 RPLidar 节点。这里有一个小问题，提供的串行设备被 Linux 系统识别为 /dev/ttyUSBn。这是一个常见的名称，如果您的系统中有一个以上名为 ttyUSBn 的设备，可能会使系统混淆。
我们可以通过添加一些 udev 规则来避免这种不确定性，这些规则将创建一组符号链接，我们将在启动文件中使用这些链接。在 rodney/scripts 文件夹中，我在 rodney_udev.rules 文件中创建了 udev 规则。
# Set the udev rules.
#
# Arduino Nano
KERNEL=="ttyUSB*", ATTRS{idVendor}=="1a86", ATTRS{idProduct}=="7523", MODE:="0777", SYMLINK+="nano"
#
# Teensy
KERNEL=="ttyACM*", ATTRS{idVendor}=="16c0", ATTRS{idProduct}=="0483", MODE:="0777", SYMLINK+="teensy"
#
# RPLidar
KERNEL=="ttyUSB*", ATTRS{idVendor}=="10c4", ATTRS{idProduct}=="ea60", MODE:="0777", SYMLINK+="rplidar"
使用 sudo 复制命令，将此文件复制到 Raspberry Pi 上的 /etc/udev/rules.d 文件夹。我还创建了一个名为 create_udev_rules.sh 的脚本文件，它将为您完成复制。
接下来，我们可以将启动 RPLidar 节点的命令添加到 rodey.launch 文件中。
<node pkg="rplidar_ros" type="rplidarNode" name="rplidar_node" output="screen">
<param name="serial_port" type="string" value="/dev/rplidar"/>
<param name="serial_baudrate" type="int" value="115200"/>
<param name="frame_id" type="string" value="laser"/>
<remap from="scan" to="scan_filter_input"/>
</node>
我们将在本文后面的“使用代码”部分构建和运行所有代码，但下图显示了在 rviz 中可视化的激光扫描消息。
IMU（惯性测量单元）
在第 5 部分中，我们开始广播源自电机编码器的原始里程计数据，并包含了 ekf_localization_node，我们说它将用于将原始里程计与 IMU 数据融合以改进机器人的里程计。
对于 IMU，我们将使用 SparkFun MPU-9250 分线板。
我想将 IMU 连接到微控制器，而不是直接连接到 Raspberry Pi。由于 Arduino Nano 内存已经不足，我们需要一个替代解决方案。您可以向项目中添加第二个 Nano 或使用更大的 Arduino 板，但我决定在我的项目中将 Nano 替换为 Teensy 3.5。Teensy 比 Arduino 更快，内存也更多，但与 Arduino 软件和库兼容。您可以下载一个插件，以便您可以继续使用 Arduino IDE 进行开发。我选择了 Teensy 3.5，因为它与更快的 3.6 不同，其数字输入支持 5V 电压。
下图显示了图像下部的 IMU（MPU-9250 分线板），Teensy 显示在左上角。
Arduino 草图更新
由于 Teensy 速度更快，我们也将趁此机会提高与 Raspberry Pi 串行接口的波特率。随着机器人的发展，我们可能还需要在 Teensy 和 Pi 之间传输更大的消息。为了更改缓冲区大小和波特率，您需要更改 ROS 串行库中的 ros.h 和 ArduinoHardware.h 文件。您可以直接在库中进行更改，但如果您重新编译库，例如添加新消息，那么这些更改将丢失。因此，我已在 sketch 文件夹中重新创建了这些文件。
该草图仍然包含伺服电机和霍尔传感器的代码，但已更新以包含 IMU 功能。
在 setup 函数中，我们现在还确保可以与 IMU 通信，并从我使用 Library Manager 安装到 Arduino IDE 的 SparkFun MPU-9250 9 DOF IMU Breakout 库中调用设置过程。请注意，这里我们还设置了 IMU 的磁力计部分，尽管我们目前没有在任何主题上广播磁力计数据。在 setup 函数的末尾，我们现在只有在成功设置 IMU 后才打开板载 LED。
在 loop 函数中，我添加了代码，如果 IMU 设置失败，则记录一条消息。以前，我发现草图 setup 部分中进行的任何日志调用都不会被记录。在每次调用 loop 期间，我们都会检查 IMU 寄存器是否包含新数据，如果是，则读取加速度计、陀螺仪和磁力计数据。如果到了发布 IMU 数据的时间，我们就会形成一个 sensor_msgs/Imu 消息并在 imu/data_raw 主题上广播它。
rodney_control.ino
#include <PWMServo.h> // Use PWMServo on Teensy
#include <MPU9250.h>
// Use "ros.h" not <ros.h> so that by using our local version
// we can increase/decrease buffer size if required and
// increased the baud rate on faster boards.
#include "ros.h"
#include <servo_msgs/servo_array.h>
#include <tacho_msgs/tacho.h>
#include <sensor_msgs/Imu.h>
#include <sensor_msgs/MagneticField.h>
void servo_cb( const servo_msgs::servo_array& cmd_msg);
void WheelSpeed0();
void WheelSpeed1();
#define LED_PIN 13 // Onboard LED
#define GEAR_BOX_COUNTS_PER_REV 1440.0f
// Define the period in milliseconds between tacho messages
#define TACHO_PERIOD_MS 50 // Publish at 20Hz
// Define the PWM pins that the other servos are connected to
#define SERVO_0 23
#define SERVO_1 22
#define SERVO_2 21
#define SERVO_3 20
// Define pins used for two Hall sensors
#define ENCODER0_PINA 0 // Interrupt
#define ENCODER0_PINB 1 // Digital pin
#define ENCODER1_PINA 3 // Interrupt
#define ENCODER1_PINB 4 // Digital pin
PWMServo servo0;
PWMServo servo1;
PWMServo servo2;
PWMServo servo3;
#define G_TO_MS2 9.80665
#define I2Cclock 400000
#define I2Cport Wire
#define MPU9250_ADDRESS MPU9250_ADDRESS_AD0
MPU9250 myIMU(MPU9250_ADDRESS, I2Cport, I2Cclock);
tacho_msgs::tacho tachoMsg;
sensor_msgs::Imu imuMsg;
ros::NodeHandle nh;
ros::Publisher tachoPub("tacho", &tachoMsg);
ros::Publisher imuPub("imu/data_raw", &imuMsg);
ros::Subscriber<servo_msgs::servo_array> subServo("servo", servo_cb);
bool imuTestPassed;
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
unsigned long publisherTime;
unsigned long currentTime;
unsigned long lastTime;
char imu_link[] = "imu";
void setup()
{
Wire.begin();
nh.initNode();
nh.advertise(tachoPub);
nh.advertise(imuPub);
nh.subscribe(subServo);
// Attach servos
servo0.attach(SERVO_0); //attach it to the pin
servo1.attach(SERVO_1);
servo2.attach(SERVO_2);
servo3.attach(SERVO_3);
servo0.write(90);
servo1.write(120);
servo2.write(90);
servo3.write(90);
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(digitalPinToInterrupt(ENCODER0_PINA), WheelSpeed0, CHANGE);
attachInterrupt(digitalPinToInterrupt(ENCODER1_PINA), WheelSpeed1, CHANGE);
imuTestPassed = true;
// Read the WHO_AM_I register of the IMU, this is a good test of communication
byte c = myIMU.readByte(MPU9250_ADDRESS, WHO_AM_I_MPU9250);
if(c == 0x71) // WHO_AM_I should always be 0x71
{
// Start by performing self test
myIMU.MPU9250SelfTest(myIMU.selfTest);
for(int i = 0; i < 6; i++)
{
if(abs(myIMU.selfTest[i]) > 14.0f)
{
imuTestPassed = false;
}
}
// Calibrate gyro and accelerometers, load biases in bias registers
myIMU.calibrateMPU9250(myIMU.gyroBias, myIMU.accelBias);
// Initialize device for active mode read of acclerometer, gyroscope, and temperature
myIMU.initMPU9250();
// Read the WHO_AM_I register of the magnetometer, this is a good test of communication
byte d = myIMU.readByte(AK8963_ADDRESS, WHO_AM_I_AK8963);
if(d == 0x48)
{
// Get magnetometer calibration from AK8963 ROM
// Initialize device for active mode read of magnetometer
myIMU.initAK8963(myIMU.factoryMagCalibration);
// Get sensor resolutions, only need to do this once
myIMU.getAres();
myIMU.getGres();
myIMU.getMres();
}
else
{
imuTestPassed = false;
}
}
else
{
imuTestPassed = false;
}
if(imuTestPassed == true)
{
// Turn on the onboard LED to show we are running
pinMode(LED_PIN, OUTPUT);
digitalWrite(LED_PIN, HIGH);
}
}
void loop()
{
static bool setup = false;
if(setup == false)
{
// Log only gets reported in loop
nh.loginfo("Teensy code started");
if(imuTestPassed == false)
{
nh.loginfo("IMU self test failed");
}
setup = true;
}
// Is it time to publish the tacho message
if(millis() > publisherTime)
{
float deltaTime;
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;
tachoPub.publish(&tachoMsg);
publisherTime = millis() + TACHO_PERIOD_MS;
}
// IMU
if(imuTestPassed == true)
{
// Check to see if all data registers have new data
if (myIMU.readByte(MPU9250_ADDRESS, INT_STATUS) & 0x01)
{
myIMU.readAccelData(myIMU.accelCount); // Read the x/y/z adc values
// Now we'll calculate the accleration value into actual g's
// This depends on scale being set
myIMU.ax = (float)myIMU.accelCount[0] * myIMU.aRes;
myIMU.ay = (float)myIMU.accelCount[1] * myIMU.aRes;
myIMU.az = (float)myIMU.accelCount[2] * myIMU.aRes;
myIMU.readGyroData(myIMU.gyroCount); // Read the x/y/z adc values
// Calculate the gyro value into actual degrees per second
// This depends on scale being set
myIMU.gx = (float)myIMU.gyroCount[0] * myIMU.gRes;
myIMU.gy = (float)myIMU.gyroCount[1] * myIMU.gRes;
myIMU.gz = (float)myIMU.gyroCount[2] * myIMU.gRes;
myIMU.readMagData(myIMU.magCount); // Read the x/y/z adc values
// Reading mag data but not currently publishing it
// Calculate the magnetometer values in milliGauss
// Include factory calibration per data sheet and user environmental corrections
// Get actual magnetometer value, this depends on scale being set
myIMU.mx = (float)myIMU.magCount[0] * myIMU.mRes
* myIMU.factoryMagCalibration[0] - myIMU.magBias[0];
myIMU.my = (float)myIMU.magCount[1] * myIMU.mRes
* myIMU.factoryMagCalibration[1] - myIMU.magBias[1];
myIMU.mz = (float)myIMU.magCount[2] * myIMU.mRes
* myIMU.factoryMagCalibration[2] - myIMU.magBias[2];
}
// Is it time to publish IMU data
myIMU.delt_t = millis() - myIMU.count;
if (myIMU.delt_t > 50)
{
// IMU
imuMsg.header.frame_id = imu_link;
imuMsg.header.stamp = nh.now();
// We are not providing orientation so the
// first element of the this matrix should be -1
imuMsg.orientation_covariance[0] = -1;
imuMsg.angular_velocity.x = myIMU.gx * DEG_TO_RAD;
imuMsg.angular_velocity.y = myIMU.gy * DEG_TO_RAD;
imuMsg.angular_velocity.z = myIMU.gz * DEG_TO_RAD;
// angular velocity covariance
imuMsg.angular_velocity_covariance[0] = 0.003;
imuMsg.angular_velocity_covariance[4] = 0.003;
imuMsg.angular_velocity_covariance[8] = 0.003;
imuMsg.linear_acceleration.x = myIMU.ax * G_TO_MS2;
imuMsg.linear_acceleration.y = myIMU.ay * G_TO_MS2;
imuMsg.linear_acceleration.z = myIMU.az * G_TO_MS2;
// linear acceleration covariance
imuMsg.linear_acceleration_covariance[0] = 0.1;
imuMsg.linear_acceleration_covariance[4] = 0.1;
imuMsg.linear_acceleration_covariance[8] = 0.1;
imuPub.publish(&imuMsg);
myIMU.count = millis();
}
}
nh.spinOnce();
}
// Callback for when servo array message received
void servo_cb( const servo_msgs::servo_array& cmd_msg)
{
/* Which servo to drive */
switch(cmd_msg.index)
{
case 0:
servo0.write(cmd_msg.angle); //set servo 0 angle, should be from 0-180
break;
case 1:
servo1.write(cmd_msg.angle); //set servo 1 angle, should be from 0-180
break;
case 2:
servo2.write(cmd_msg.angle); //set servo 2 angle, should be from 0-180
break;
case 3:
servo3.write(cmd_msg.angle); //set servo 3 angle, should be from 0-180
break;
default:
nh.logdebug("Error incorrect servo index");
break;
}
}
// ISR
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
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--;
}
}
ros.h
#ifndef _ROS_H_
#define _ROS_H_
// As we are no longer including any .h file in the root of the
// ros_lib library the IDE can't find ros/node_handle.h
// Add the dummy.h empty file in the ros_lib root
#include <dummy.h>
#include <ros/node_handle.h>
#include "ArduinoHardware.h"
namespace ros
{
#if defined(__MK64FX512__) || defined(__MK66FX1M0__)
// Teensy 3.5 or 6.3
typedef NodeHandle_<ArduinoHardware, 25, 25, 512, 512> NodeHandle;
#elif defined(__AVR_ATmega328P__)
// arduino Nano
// 10 publishers, 15 subscribers, 128 bytes input buffer and 256 bytes output buffer
typedef NodeHandle_<ArduinoHardware, 10, 15, 128, 256> NodeHandle;
#else
typedef NodeHandle_<ArduinoHardware> NodeHandle; // default 25, 25, 512, 512
#endif
}
#endif
ArduinoHardware.h
#ifndef ROS_ARDUINO_HARDWARE_H_
#define ROS_ARDUINO_HARDWARE_H_
#if ARDUINO>=100
#include <Arduino.h> // Arduino 1.0
#else
#include <WProgram.h> // Arduino 0022
#endif
#if defined(__MK20DX128__) || defined(__MK20DX256__) ||
defined(__MK64FX512__) || defined(__MK66FX1M0__) || defined(__MKL26Z64__)
#if defined(USE_TEENSY_HW_SERIAL)
#define SERIAL_CLASS HardwareSerial // Teensy HW Serial
#else
#include <usb_serial.h> // Teensy 3.0 and 3.1
#define SERIAL_CLASS usb_serial_class
#endif
#elif defined(_SAM3XA_)
#include <UARTClass.h> // Arduino Due
#define SERIAL_CLASS UARTClass
#elif defined(USE_USBCON)
// Arduino Leonardo USB Serial Port
#define SERIAL_CLASS Serial_
#elif (defined(__STM32F1__) and !(defined(USE_STM32_HW_SERIAL))) or defined(SPARK)
// Stm32duino Maple mini USB Serial Port
#define SERIAL_CLASS USBSerial
#else
#include <HardwareSerial.h> // Arduino AVR
#define SERIAL_CLASS HardwareSerial
#endif
class ArduinoHardware {
public:
#if defined(__MK64FX512__) || defined(__MK66FX1M0__)
ArduinoHardware(SERIAL_CLASS* io , long baud= 500000){
iostream = io;
baud_ = baud;
}
ArduinoHardware()
{
#if defined(USBCON) and !(defined(USE_USBCON))
/* Leonardo support */
iostream = &Serial1;
#elif defined(USE_TEENSY_HW_SERIAL) or defined(USE_STM32_HW_SERIAL)
iostream = &Serial1;
#else
iostream = &Serial;
#endif
baud_ = 500000;
}
#else // Not a Teensy
ArduinoHardware(SERIAL_CLASS* io , long baud= 57600){
iostream = io;
baud_ = baud;
}
ArduinoHardware()
{
#if defined(USBCON) and !(defined(USE_USBCON))
/* Leonardo support */
iostream = &Serial1;
#elif defined(USE_TEENSY_HW_SERIAL) or defined(USE_STM32_HW_SERIAL)
iostream = &Serial1;
#else
iostream = &Serial;
#endif
baud_ = 57600;
}
#endif // defined(__MK64FX512__) || defined(__MK66FX1M0__)
ArduinoHardware(ArduinoHardware& h){
this->iostream = h.iostream;
this->baud_ = h.baud_;
}
void setBaud(long baud){
this->baud_= baud;
}
int getBaud(){return baud_;}
void init(){
#if defined(USE_USBCON)
// Startup delay as a fail-safe to upload a new sketch
delay(3000);
#endif
iostream->begin(baud_);
}
int read(){return iostream->read();};
void write(uint8_t* data, int length){
for(int i=0; i<length; i++)
iostream->write(data[i]);
}
unsigned long time(){return millis();}
protected:
SERIAL_CLASS* iostream;
long baud_;
};
#endif
串行节点波特率
在 ArduinoHardware.h 文件中，我们将串口的波特率更改为 500000 波特。当启动 ROS 串行节点时，我们需要在 Raspberry Pi 端反映此更改。我对 rodney.launch 文件进行了另一项更改以设置波特率。
!-- Teensy.
Use the defaults /dev/ttyACM0 (or teensy if dev rules updated) and 500000 -->
<arg name="serial_port" default="/dev/teensy"/>
<arg name="baud_rate" default="500000"/>
<node pkg="rosserial_python" type="serial_node.py" name="serial_node" output="screen">
<param name="port" value="$(arg serial_port)"/>
<param name="baud" value="$(arg baud_rate)"/>
</node>
IMU 校准
当我们设置 IMU 时，它包括加载一些出厂校准值，但我们可以通过包含额外的校准来改进 IMU 数据。
为此，我们将使用一个可从 这个 GitHub 站点获取的 ROS 包。我们还将分支该存储库并自行进行一些代码更改。未更改的包包含两个节点。其中第一个计算加速度计校准参数并将它们保存到 YAML 文件中。我们只需要运行一次，并且由于我们必须将 IMU 以相当精确的定位放置在六个方向上，因此最好在将 IMU 安装到机器人之前完成此操作。第二个节点使用第一个节点创建的校准文件将校准应用于未校准的 IMU 主题以生成校准后的 IMU 主题。第二个节点还可以选择在启动时计算陀螺仪偏差，然后将这些值从原始数据中减去。尽管草图内部的 IMU 设置也执行此操作，但我发现如果启用此选项，我获得了更好的结果。
此外，我将对第二个节点进行更改。尽管我们已经校准了 IMU，但一旦安装到机器人上，它可能不会完全垂直于地面。这将导致加速度计漂移。现在，由于我只期望在家里使用 Rodney，并且不期望他攀爬或下降任何斜坡，我们也可以抵消这个偏移。
为了进行一次性校准并使 IMU 沿着其每个轴达到相当准确的位置，我将其安装到我 3D 打印的校准立方体中。尽管该立方体是用于不同型号的 IMU，但稍加填充，我成功安装了我的 IMU。
图片显示了安装在校准立方体中的 IMU。IMU 由一个备用 Teensy 控制，但您可以使用安装在机器人中的那个。
校准节点 do_calib 期望 IMU 数据在主题 imu 中广播。为了启动 ROS 串行节点并重新映射主题，我创建了一个启动文件 cal_imu.launch，它位于 rodney/launch 文件夹中。
<?xml version="1.0" ?>
<launch>
<!-- Teensy.
Use the defaults /dev/ttyACM0 (or teensy if dev rules updated) and 500000 -->
<arg name="serial_port" default="/dev/ttyACM0"/>
<arg name="baud_rate" default="500000"/>
<node pkg="rosserial_python" type="serial_node.py"
name="serial_node" output="screen">
<param name="port" value="$(arg serial_port)"/>
<param name="baud" value="$(arg baud_rate)"/>
<remap from="/imu/data_raw" to="imu"/>
</node>
<!-- now "rosrun imu_calib do_calib" in a shell -->
</launch>
串行节点运行后，在另一个终端中，输入以下命令：
$ rosrun imu_calib do_calib
然后按照终端上给出的说明进行操作。生成校准文件后，将其复制到 rodney/config 文件夹。
我对 imu_calib 包所做的更改仅限于 apply_calib.cpp 和 .h 文件。
在构造函数中，我添加了从参数服务器读取参数的功能，以便可以启用或禁用此功能。
nh_private.param<bool>("null_accelerometer", null_accelerometer_, true);
nh_private.param<int>("null_accelerometer_samples", null_accelerometer_samples_, 100);
在函数 rawImuCallback 中，我添加了代码以在启动时计算平均加速度计偏移量。
if(null_accelerometer_ == true)
{
ROS_INFO_ONCE("Nulling accelerometer; do not move the IMU");
// Recursively compute mean accelerometer measurements from corrected acceleration readings
sensor_msgs::Imu corrected = *raw;
accel_sample_count_++;
calib_.applyCalib(raw->linear_acceleration.x,
raw->linear_acceleration.y, raw->linear_acceleration.z,
&corrected.linear_acceleration.x, &corrected.linear_acceleration.y,
&corrected.linear_acceleration.z);
accel_bias_x_ = ((accel_sample_count_ - 1) *
accel_bias_x_ + corrected.linear_acceleration.x) / accel_sample_count_;
accel_bias_y_ = ((accel_sample_count_ - 1) *
accel_bias_y_ + corrected.linear_acceleration.y) / accel_sample_count_;
accel_bias_z_ = ((accel_sample_count_ - 1) * accel_bias_z_ +
(corrected.linear_acceleration.z-9.80665)) / accel_sample_count_;
if (accel_sample_count_ >= null_accelerometer_samples_)
{
ROS_INFO("Nulling accelerometers complete! (bias = [%.3f, %.3f, %.3f])",
accel_bias_x_, accel_bias_y_, accel_bias_z_);
null_accelerometer_ = false;
}
}
然后将这些值从校准后的值中减去。
corrected.linear_acceleration.x -= accel_bias_x_;
corrected.linear_acceleration.y -= accel_bias_y_;
corrected.linear_acceleration.z -= accel_bias_z_;
更新后的软件包可在代码 zip 文件中的 imu_calib 文件夹下找到。
以下内容已添加到 rodney.launch 文件中，以启动更新的节点并传递校准文件的位置。
<!-- Add calibration to raw imu data -->
<node pkg="imu_calib" type="apply_calib" name="imu_calib" output="screen">
<param name="calib_file" value="$(find rodney)/config/imu_calib.yaml"/>
</node>
在本文的上一部分，我们添加了 robot_localization 包中的 ekf_localization_node。该节点将融合 IMU 数据与原始里程计数据，以生成里程计数据，该数据将用于我将在下一篇文章中介绍的导航系统。
此数据融合的配置包含在 robot_localization.yaml 文件中，该文件位于 rodney/config 文件夹中。我们曾在上一篇文章中简要讨论过此文件，但在此处添加一些详细信息是值得的。
配置矩阵中的数据表示以下内容：
[ x position, y position, z position,
roll, pitch, yaw,
x velocity, y velocity, z velocity,
roll velocity, pitch velocity, yaw velocity,
x acceleration, y acceleration, z, acceleration]
矩阵中的“true”值表示该位置的数据将由扩展卡尔曼滤波器使用。
现在对于原始里程计数据，我有：
odom0_config: [false, false, false,
false, false, false,
true, true, false,
false, false, true,
false, false, false]
虽然里程计消息也包含位置数据，但它与速度数据来源于同一来源，因此没有必要两次使用这些数据。也可以说，既然我们总是将 Y 轴速度设置为零，我们就可以将这个矩阵值设置为 false。但是，如果您想从 IMU 中包含 Y 轴速度，那么通过在里程计矩阵中包含此参数，有助于抵消 IMU 的漂移。
现在，对于 IMU 矩阵，我尝试了两种不同的配置。首先是一种只使用 IMU 数据中的偏航速度的配置。
imu0_config: [false, false, false,
false, false, false,
false, false, false,
false, false, true,
false, false, false]
您也可以尝试使用 x 和 y 速度的配置，但您需要确保首先减少数据中的任何噪声/误差，否则里程计将受到 x 和/或 y 轴漂移的影响。
imu0_config: [false, false, false,
false, false, false,
true, true, false,
false, false, true,
false, false, false]
静态变换
在上一部分中，我们引入了机器人底部相对于地面的高度的静态变换。此闩锁变换广播是通过 rodney.launch 文件使用以下内容设置的：
<node pkg="tf2_ros" type="static_transform_publisher"
name="base_footprint_broadcaster" args="0 0 0.09 0 0 0 /base_footprint /base_link"/>
现在我们已经添加了 LIDAR 和 IMU，我们需要添加更多的静态变换。计算是从机器人中心进行的，因此扫描消息中报告的任何距离都需要从 LIDAR 的位置转换为机器人中心。
我们可以像上面那样在启动文件中添加两个更多的静态变换，但我们将删除现有的一个并编写一个简单的节点，我们将把它包含在 rodney 包中，以处理所有三个静态变换。
在 rodney/src 文件夹中，我添加了 static_broadcaster.py 文件。这个 Python 脚本将创建一个负责广播三个静态变换的节点。这个消息是闩锁的，因此需要变换的节点将在启动时获得数据。
# Rodney robot static transform broadcaster
import sys
import rospy
import tf
import tf2_ros
import geometry_msgs.msg
def main(args):
rospy.init_node('rodney_static_broadcaster', anonymous=False)
rospy.loginfo("rodney static broadcaster node started")
broadcaster = tf2_ros.StaticTransformBroadcaster()
# Static transform for the base_footprint to base_link
st1 = geometry_msgs.msg.TransformStamped()
st1.header.stamp = rospy.Time.now()
st1.header.frame_id = "/base_footprint"
st1.child_frame_id = "/base_link"
st1.transform.translation.x = 0.0
st1.transform.translation.y = 0.0
st1.transform.translation.z = 0.09
quat = tf.transformations.quaternion_from_euler(0.0, 0.0, 0.0)
st1.transform.rotation.x = quat[0]
st1.transform.rotation.y = quat[1]
st1.transform.rotation.z = quat[2]
st1.transform.rotation.w = quat[3]
# Static transform for the baselink to laser
st2 = geometry_msgs.msg.TransformStamped()
st2.header.stamp = rospy.Time.now()
st2.header.frame_id = "/base_link"
st2.child_frame_id = "/laser"
st2.transform.translation.x = 0.085
st2.transform.translation.y = 0.0
st2.transform.translation.z = 0.107
quat = tf.transformations.quaternion_from_euler(0.0, 0.0, 0.0)
st2.transform.rotation.x = quat[0]
st2.transform.rotation.y = quat[1]
st2.transform.rotation.z = quat[2]
st2.transform.rotation.w = quat[3]
# Static transform for the baselink to imu
st3 = geometry_msgs.msg.TransformStamped()
st3.header.stamp = rospy.Time.now()
st3.header.frame_id = "/base_link"
st3.child_frame_id = "/imu"
st3.transform.translation.x = 0.0
st3.transform.translation.y = 0.0
st3.transform.translation.z = 0.058
quat = tf.transformations.quaternion_from_euler(0.0, 0.0, 0.0)
st3.transform.rotation.x = quat[0]
st3.transform.rotation.y = quat[1]
st3.transform.rotation.z = quat[2]
st3.transform.rotation.w = quat[3]
broadcaster.sendTransform([st1, st2, st3])
# static transforms are latched so all we need to do here is spin
rospy.spin()
if __name__ == '__main__':
main(sys.argv)
该节点在 rodney.launch 文件中启动
<!-- Static transforms in the system -->
<node pkg="rodney" type="static_broadcaster.py" name="static_broadcaster_node"/>
启动文件
我对 rodney.launch 文件做的另一个更改是删除了...
output="screen"
...从不再处于开发或测试阶段的每个节点中。这减少了我们启动机器人代码的终端中显示的日志消息。但是，您仍然可以在远程终端上使用 rqt_console 来监视所有日志消息。
完整的 rodney.launch 文件现在看起来像这样：
<?xml version="1.0" ?>
<launch>
<!-- Static transforms in the system -->
<node pkg="rodney" type="static_broadcaster.py" name="static_broadcaster_node"/>
<!-- Load each of the config files into the parameter server -->
<rosparam command="load" file="$(find pan_tilt)/config/config.yaml"/>
<rosparam command="load" file="$(find face_recognition)/config/config.yaml"/>
<rosparam command="load" file="$(find head_control)/config/config.yaml"/>
<rosparam command="load" file="$(find rodney_missions)/config/config.yaml"/>
<!-- Launch the camera node from one of its launch files -->
<include file="$(find raspicam_node)/launch/camerav2_1280x960.launch" />
<!-- Start all the nodes that make up Rondey -->
<!-- Starting with those written for the project -->
<node pkg="pan_tilt" type="pan_tilt_node" name="pan_tilt_node"/>
<node pkg="face_recognition" type="face_recognition_node.py" name="face_recognition_node"/>
<node pkg="head_control" type="head_control_node" name="head_control_node"/>
<node pkg="speech" type="speech_node" name="speech_node"/>
<node pkg="rodney_missions" type="rodney_missions_node.py"
name="rodney_missions" output="screen"/>
<node pkg="rodney" type="rodney_node" name="rodney" output="screen">
<rosparam command="load" file="$(find rodney)/config/config.yaml"/>
</node>
<node pkg="thunderborg" type="thunderborg_node.py" name="thunderborg_node">
<rosparam command="load" file="$(find thunderborg)/config/config.yaml"/>
</node>
<!-- Teensy.
Use the defaults /dev/ttyACM0 (or teensy if dev rules updated) and 500000 -->
<arg name="serial_port" default="/dev/teensy"/>
<arg name="baud_rate" default="500000"/>
<node pkg="rosserial_python" type="serial_node.py" name="serial_node" output="screen">
<param name="port" value="$(arg serial_port)"/>
<param name="baud" value="$(arg baud_rate)"/>
</node>
<!-- The RPLidar and laser filter node
Have created symbolic link for /dev/ttyUSBn to be rplidar -->
<node pkg="rplidar_ros" type="rplidarNode" name="rplidar_node" output="screen">
<param name="serial_port" type="string" value="/dev/rplidar"/>
<param name="serial_baudrate" type="int" value="115200"/>
<param name="frame_id" type="string" value="laser"/>
<remap from="scan" to="scan_filter_input"/>
</node>
<node pkg ="laser_filters" type="scan_to_scan_filter_chain"
name="scan_to_scan_filter_chain" output="screen">
<rosparam command="load" file="$(find rodney)/config/laser_filter_config.yaml"/>
<remap from="scan" to="scan_filter_input"/>
<remap from="scan_filtered" to="scan"/>
</node>
<!-- The robot face -->
<node pkg="homer_robot_face" type="RobotFace" name="RobotFace"/>
<!-- Add calibration to raw imu data -->
<node pkg="imu_calib" type="apply_calib" name="imu_calib" output="screen">
<param name="calib_file" value="$(find rodney)/config/imu_calib.yaml"/>
</node>
<!-- Node to fuse motor encoder and IMU data for odom -->
<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>
</launch>
微小代码更改
我还对代码进行了一些小的更改，主要是为了能够在机器人处于手动模式时启用/禁用 LIDAR，并确保在远程控制时机器人不会失控。
rodney_missions 节点更新
我做了一些小改动，以便该节点可以通过启动和停止 LIDAR 电机来启用/禁用 LIDAR。请求来自 rodney 节点，但在这里进行控制很重要，这样该节点才能确保在将来请求自主导航时 LIDAR 正在运行。
电机的启动和停止是通过 ROS 服务调用 LIDAR 节点来完成的。第一个更改在 MissionsHelper 类的 __init__ 函数中。我们等待服务可用，创建代理调用以便我们可以访问服务，然后调用 LidarEnable 辅助函数以确保 LIDAR 当前正在运行。
# RPLidar services to start and stop the motor
rospy.wait_for_service('stop_motor')
rospy.wait_for_service('start_motor')
self.__rplidar_stop_motor_srv = rospy.ServiceProxy('stop_motor', std_srvs.srv.Empty)
self.__rplidar_start_motor_srv = rospy.ServiceProxy('start_motor', std_srvs.srv.Empty)
# LIDAR should be running but make sure
self.LidarEnable()
接下来，我向 MissionHelper 类添加了三个辅助函数。它们调用相关的服务并跟踪 LIDAR 电机的当前状态。
# Function to enable the RPLidar
def LidarEnable(self):
self.__rplidar_start_motor_srv()
self.__lidar_on = True
# Function to disable the RPLidar
def LidarDisable(self):
self.__rplidar_stop_motor_srv()
self.__lidar_on = False
# Function to Toggle RPLidar on/off
def ToggleLidar(self):
if(self.__lidar_on == True):
self.LidarDisable()
else:
self.LidarEnable()
最后的更改在 Prepare 类中，并向 execute 函数添加了一个新的 elif 构造。如果收到的作业 ID 为 'J4'，则请求切换 LIDAR 的当前状态。
elif parameters[0] == 'J4':
# Simple job to toggle the LIDAR on/off
self.__helper_obj.ToggleLidar()
rodney 包更新
我们需要对 rodney 节点进行一些更改，以便我们可以在手动模式下使用连接到远程工作站的操纵杆和/或键盘来启用/禁用 LIDAR。
此外，在我手动控制机器人时，我的家庭网络偶尔会中断几秒钟。这里的问题是机器人将继续使用从上次操纵杆或键盘输入创建的速度。因此，我在 rodney 包中添加了一个新节点（remote_heartbeat_node），该节点在远程工作站上运行并发布心跳消息。在机器人硬件上运行的 rodney 节点监视此消息，如果机器人在手动模式下并且一秒钟内未收到消息，则速度将设置为零。
rodney_node.cpp 文件的更改。
我们现在可以从操纵杆启用/禁用 LIDAR，因此我们需要一种方法来配置哪个按钮控制此功能。将以下行添加到 RodneyNode 构造函数中。
nh_.param("/controller/buttons/lidar_enable", lidar_enable_select_, 2);
在 joystickCallback 函数中，添加以下“if”构造以请求在按下相关操纵杆按钮时切换 LIDAR 电机状态。
// Button on controller selects to enable/disable the lidar function
if((manual_locomotion_mode_ == true) && (msg->buttons[lidar_enable_select_] == 1))
{
std_msgs::String mission_msg;
// Toggle the LIDAR on/off
mission_msg.data = "J4";
mission_pub_.publish(mission_msg);
last_interaction_time_ = ros::Time::now();
}
同样，如果按下键盘上的“l”键，则电机状态将被切换。将以下“else if”构造添加到 keyboardCallBack 函数中。
else if((msg->code == keyboard::Key::KEY_l) &&
((msg->modifiers & ~RodneyNode::SHIFT_CAPS_NUM_LOCK_) == 0))
{
if(manual_locomotion_mode_ == true)
{
std_msgs::String mission_msg;
// Toggle the LIDAR on/off
mission_msg.data = "J4";
mission_pub_.publish(mission_msg);
last_interaction_time_ = ros::Time::now();
}
}
接下来，节点需要通过将以下内容添加到 RodneyNode 构造函数来订阅新的 heartbeat 消息。
remote_heartbeat_sub_ = nh_.subscribe
("remote_heartbeat", 1, &RodneyNode::remHeartbeatCallback, this);
当接收到此主题上的消息时，将有一个回调函数来存储最后一条消息的时间。
// Callback for remote heartbeat
void RodneyNode::remHeartbeatCallback(const std_msgs::Empty::ConstPtr& msg)
{
// Remote heartbeat received store the time
remote_heartbeat_time_ = ros::Time::now();
}
在 sendTwist 函数中，如果自上次心跳消息收到已超过一秒，我们需要将速度设置为零。下面是 sendTwist 函数的完整新版本。
void RodneyNode::sendTwist(void)
{
geometry_msgs::Twist target_twist;
// If in manual locomotion mode use keyboard or joystick data
if(manual_locomotion_mode_ == true)
{
// Only allow stored keyboard or joystick values to set
// the velocities if the remote heartbeat is running
if((ros::Time::now() - remote_heartbeat_time_).toSec() < 1.0)
{
// 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
{
// Lost connection with remote workstation so zero the velocities
target_twist.linear.x = 0.0;
target_twist.angular.z = 0.0;
}
}
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);
}
}
接下来，我们将 remote_heartbeat_node.cpp 文件添加到 rodney/src 文件夹中。请记住，此节点不会在机器人硬件上运行，而是在用于手动驾驶机器人的远程工作站上运行。这个简单的节点只是以 5Hz 的频率广播心跳消息。
// This heartbeat node is not to be run on the robot platform but on a remote worksation
// when either the keyboard or joystick nodes are being used to teleop the robot. If the
// message sent by this node is missed for 1 second, the robot will stop using the keyboard
// and joystick stored values to drive the motors.
#include <ros/ros.h>
#include <std_msgs/Empty.h>
int main(int argc, char **argv)
{
ros::init(argc, argv, "remote_heartbeat");
ros::NodeHandle n;
ros::Publisher remote_heartbeat_pub = n.advertise<std_msgs::Empty>("remote_heartbeat", 1);
std::string node_name = ros::this_node::getName();
ROS_INFO("%s started", node_name.c_str());
ros::Rate r(5); // 5Hz
std_msgs::Empty beat;
while(ros::ok())
{
remote_heartbeat_pub.publish(beat);
ros::spinOnce();
r.sleep();
}
return 0;
}
对 rodney 包的最后一次更改是添加一个启动文件，该文件可用于同时启动键盘、操纵杆和心跳节点。文件名是 remote.launch，它位于 rodney/launch 文件夹中。
<?xml version="1.0" ?>
<launch>
<node pkg="joystick" type="joystick_node" name="joystick_node"/>
<node pkg="keyboard" type="keyboard" name="keyboard"/>
<node pkg="rodney" type="remote_heartbeat_node" name="remote_heartbeat_node" output="screen"/>
</launch>
机器人硬件
当前电路图的全尺寸图像可在图表 zip 文件夹中找到，以及显示所有节点和主题的 rqt_graph 图像的全尺寸副本。
到目前为止，该项目的完整物料清单可在此处获取。
在本文的第 1 部分中，我提到了我在树莓派上使用的 Ubiquity 机器人镜像。有关如何安装镜像、安装额外软件和配置它以用于项目的说明可在此处获取。
Using the Code
像往常一样，我将在机器人硬件上运行代码，并在Linux PC上运行测试工具和手动控制节点。在下面的细节中，我将这台PC称为工作站。
在Pi上构建ROS包（机器人硬件）
如果尚未完成，请在 Raspberry Pi 上创建一个 catkin 工作区，并使用以下命令初始化它
$ mkdir -p ~/rodney_ws/src
$ cd ~/rodney_ws/
$ catkin_make
将 face_recognition、face_recognition_msgs、head_control、imu_calib、pan_tilt、rodney、rodney_missions、ros-keyboard、rplidar-ros、servo_msgs、speech、tacho_msgs 和 thunderborg 包复制到 ~/rodney_ws/src 文件夹中。
使用以下命令构建代码
$ cd ~/rodney_ws/
$ catkin_make
检查构建是否无任何错误地完成。
您还需要编译并将草图下载到 Teensy 3.5。
在工作站上构建ROS包
在工作站上，我们希望运行键盘、操纵杆和心跳节点，以便能够远程控制实际的机器人硬件。
使用以下命令创建一个工作空间
$ mkdir -p ~/test_ws/src
$ cd ~/test_ws/
$ catkin_make
将 rodney、joystick、odom_test 和 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键自动补全rosrun rosserial_python。
开始编码
在机器人硬件上，运行以下命令启动系统中所有当前节点：
$ source rodney_ws/devel/setup.bash
$ roslaunch rodney rodney.launch
在工作站上，运行以下命令启动远程控制节点：
$ source test_ws/devel/setup.bash
$ export ROS_MASTER_URI=http://ubiquityrobot:11311
$ roslaunch rodney remote.launch
一个标题为“ROS 键盘输入”的小窗口应该正在运行。输入键盘按键时，请确保小窗口处于焦点。
在工作站上的另一个终端中，使用以下命令启动 rviz：
$ source test_ws/devel/setup.bash
$ export ROS_MASTER_URI=http://ubiquityrobot:11311
$ roslaunch rodney rviz.launch
测试 LIDAR
配置 rviz，使其固定帧为 base_link，并使 LaserScan 显示 /scan 主题。您应该会在 rviz 中看到扫描消息的可视化表示。
还应该可以使用操纵杆控制器或键盘远程控制机器人，以避开激光器检测到的障碍物。
在工作站的一个新终端中，输入以下命令禁用 LIDAR：
$ source test_ws/devel/setup.bash
$ export ROS_MASTER_URI=http://ubiquityrobot:11311
$ rosservice call /stop_motor
LIDAR 应该停止旋转，并且 rviz 中的扫描显示不应更新。
在终端中，输入以下命令启用 LIDAR：
$ rosservice call /start_motor
LIDAR 应该开始旋转，并且 rviz 中的扫描显示应该会更新。
测试 IMU
为了确保原始和融合里程计均为零，请关闭启动代码的机器人硬件上的终端。
在机器人硬件上，在一个新终端中，使用以下命令重新启动：
$ source rodney_ws/devel/setup.bash
$ roslaunch rodney rodney.launch
配置 rviz，以便：
- 固定帧为
odom - 显示变换 (TF)
base_link和odom轴 - 里程计 /raw_odom 和 /odom 以不同颜色显示
标记机器人在地板上的位置。使用键盘或操纵杆移动机器人。
在工作站上的一个终端中，输入以下内容以查看当前里程计消息：
$ source test_ws/devel/setup.bash
$ export ROS_MASTER_URI=http://ubiquityrobot:11311
$ rostopic echo /odom
从消息中，记下当前姿态位置 x 和 y。测量机器人从标记位置移动的距离，并确认它是一个很好的近似值。
使用键盘或操纵杆旋转机器人。姿态方向不容易比较，因为它是一个四元数。您可以在 rviz 中看到方向，但您也可以使用测试脚本 odom_test 来确认角度（以度为单位）。
在工作站的终端中，按 Ctrl-C，然后输入以下命令：
$ rosrun odom_test odom_test1_node.py
每秒一次，节点将以度数打印出原始和融合里程计消息的姿态方向。
在 rviz 显示中，您将看到两组箭头，显示原始和融合里程计。此外，当机器人返回到其起始位置时，base_link 和 odom 变换应该非常接近。
图片显示了原始里程计（红色箭头）和融合里程计（绿色箭头）的轨迹
关注点
在这一部分中，我们完成了设计目标 4，并添加了 IMU 以改进里程计。
在下一篇文章中，我们将添加使用 LIDAR 进行自主导航的包，包括从 LIDAR 和里程计变换数据创建地图。我们将使用 rviz 设置目标姿态，机器人将自主导航到设定的位置。
历史
- 2019/04/15：首次发布