卡尔曼滤波

卡尔曼滤波（Kalman Filter）是一种常用的状态估计方法，其主要应用于从一系列的不完全和有噪声的数据中提取系统状态的信息，预测物体的位置和速度。

原理

卡尔曼滤波是一种基于概率统计的最优估计算法，用于估计线性系统的状态量。它包含两个主要的步骤：预测和更新。

预测步骤包括状态的预测和预测协方差的计算。状态的预测是通过系统的动态模型和当前状态来计算出下一个时刻的状态值。预测协方差则是通过系统的动态模型和先前的协方差计算出的预测误差协方差。

更新步骤则是基于传感器测量值来修正状态估计和协方差。具体来说，通过比较预测值和测量值之间的差异，计算卡尔曼增益，然后使用该增益来更新状态估计和协方差。

扩展卡尔曼滤波

扩展卡尔曼滤波（Extended Kalman Filter, EKF）是卡尔曼滤波的一种非线性扩展，用于处理非线性系统的状态估计问题。由于许多实际系统（如飞行器、无人机、机器人等）的状态更新和测量模型都是非线性的，EKF通过对非线性函数进行局部线性化来近似解决这些问题。

如何使用机器人姿态EKF

安装功能包：

sudo apt-get install ros-melodic-robot-pose-ekf

到达安装路径/opt/ros/melodic/share/robot_pose_ekf，修改robot_pose_ekf.launch文件。

配置 EKF节点的默认启动文件可以在robot_pose_ekf包目录中找到。启动文件包含多个可配置参数：

  • freq: 滤波器的更新和发布频率。注意，较高的频率会让你在较短时间内获得更多的机器人姿态，但不会提高每个姿态估计的准确度。

  • sensor_timeout: 当传感器停止向滤波器发送信息时，滤波器等待该传感器的时间。

  • odom_used, imu_used, vo_used: 启用或禁用输入。 这些配置可以在启动文件中修改，启动文件类似如下：

<launch>

  <node pkg="robot_pose_ekf" type="robot_pose_ekf" name="robot_pose_ekf">
    <param name="output_frame" value="/odom"/>
    <param name="base_footprint_frame" value="/base_link"/>
    <param name="freq" value="30.0"/>
    <param name="sensor_timeout" value="1.0"/>  
    <param name="odom_used" value="true"/>
    <param name="imu_used" value="true"/>
    <param name="vo_used" value="false"/>

    <remap from="/odom" to="/mobile_base/mobile_base_controller/odom" />
  </node>

</launch>

启动机器人仿真测试去EKF

roslaunch handsfree_gazebo gazebo_mini.launch

运行机器人姿态EKF：

roslaunch robot_pose_ekf robot_pose_ekf.launch

通过处理话题打印处理好的位置数据：

rostopic echo /robot_pose_ekf/odom_combined

节点

robot_pose_ekf``robot_pose_ekf实现了一个扩展卡尔曼滤波器来确定机器人的姿态。

订阅的主题：

  • odom (nav_msgs/Odometry)
    2D姿态（用于轮式里程计）：包含机器人在地面平面的位置信息和方向，以及该姿态的不确定性。该消息实际上代表3D姿态，但忽略z、滚转和俯仰。

  • imu_data (sensor_msgs/Imu)
    3D方向（用于IMU）：提供机器人基坐标系相对于世界参考系的滚转、俯仰和偏航角。滚转和俯仰被解释为绝对角度（IMU有重力参考），而偏航角是相对角度。不确定性由协方差矩阵表示。EKF只接收该主题时不会启动，它还需要接收到vo或odom主题的信息。

  • vo (nav_msgs/Odometry)
    3D姿态（用于视觉里程计）：代表机器人的完整位置和方向，以及姿态的不确定性。若传感器仅测量部分3D姿态（例如轮式里程计仅测量2D姿态），则未测量的部分可以设置较大的协方差。

robot_pose_ekf节点不要求所有传感器都时刻可用。每个传感器提供一个姿态估计及其不确定性，这些传感器的工作频率和延迟不同。传感器可随时启用或禁用，节点会自动检测和使用可用的传感器。若要添加自定义的传感器输入，请查看“添加GPS传感器”教程。

发布的主题：

  • robot_pose_ekf/odom_combined (geometry_msgs/PoseWithCovarianceStamped)
    滤波器的输出（估计的3D机器人姿态）。

提供的TF变换：

  • odom_combined → base_footprint

如何工作

  • 姿态解释：
    发送到滤波器节点的所有传感器源都有自己的世界参考系，这些参考系会随着时间漂移。因此，不同传感器发送的绝对姿态不能直接比较。节点使用每个传感器的相对姿态差来更新扩展卡尔曼滤波器。

  • 协方差解释：
    随着机器人移动，世界参考系下的姿态不确定性会越来越大。因此，直接发布姿态的协方差没有意义，传感器源发布的是速度的协方差变化。

  • 时间：
    假设滤波器上次在时间t_0更新。只有当每个传感器都接收到比t_0更晚的时间戳时，滤波器才会更新。例如，odom主题接收到时间戳t_1 > t_0，imu_data主题接收到t_2 > t_1 > t_0时，滤波器将更新至t_1。

ROS中建立MQTT通信

MQTT介绍

MQTT 是 Message Queuing Telemetry Transport 的缩写，是一种轻量级的、基于发布/订阅模式的物联网通信协议。它具有以下特点：

  • 简单易用：MQTT 的协议规范很简单，易于学习和使用。

  • 可靠性高：MQTT 使用了 TCP 协议进行传输，具有较高的可靠性。

  • 低延迟：MQTT 使用了发布/订阅模式，可以减少消息传递的延迟。

在机器人应用中，MQTT 可以用于以下场景：

  • 传感器数据上传：使用 MQTT 可以将传感器数据上传到云端或其他机器人系统。

  • 机器人控制：使用 MQTT 可以远程控制机器人。

  • 机器人协作：使用 MQTT 可以实现多个机器人之间的协作。

MQTT 协议由三个主要部分组成：

  • 客户端： MQTT 客户端是发送和接收消息的应用程序。

  • 服务器： MQTT 服务器是处理消息的应用程序。

  • 主题： 主题是消息的路径，用于区分不同类型的消息。

MQTT 协议使用发布/订阅模式进行通信。发布者将消息发布到主题，订阅者订阅主题，并接收发布者发布的消息。 这一点跟ros2里面的topic非常类似

1. MQTT 客户端

MQTT 客户端可以是任何类型的应用程序，包括嵌入式设备、PC 应用程序和 Web 应用程序。MQTT 客户端需要实现 MQTT 协议的三个主要功能：

  • 连接： 客户端连接到 MQTT 服务器。

  • 发布： 客户端发布消息到主题。

  • 订阅： 客户端订阅主题，并接收发布者发布的消息。

2. MQTT 服务器

MQTT 服务器是一个运行 MQTT 协议的应用程序。MQTT 服务器需要实现 MQTT 协议的三个主要功能：

  • 连接： 服务器接受客户端的连接请求。

  • 发布： 服务器将发布者发布的消息传递给订阅者。

  • 订阅： 服务器将订阅者的订阅信息存储起来。

3. MQTT 主题

MQTT 主题是消息的路径，用于区分不同类型的消息。主题的格式为：

/topic/[topic_name]

其中，topic_name 是主题名称。主题名称可以包含字母、数字、下划线和点。

4. MQTT 消息

MQTT 消息由两部分组成：

  • 报头： 报头包含消息的标识符、主题、QoS 等信息。

  • 数据： 数据是消息的内容。

QoS 是消息质量等级，用于控制消息的可靠性。QoS 有三个级别：

  • 0： 最多一次传递。

  • 1： 至少一次传递。

  • 2： 只有一次传递。

ROS通信机制

ROS中的主要通信机制有以下几种：

  1. 话题 (Topics)

    • 发布/订阅模型（Publish/Subscribe）：这种通信方式中，节点可以发布消息到一个主题，或者订阅一个主题以接收消息。这种方式是匿名的，发布者和订阅者不需要知道彼此的存在。

    • 消息（Messages）：在主题上发送的数据结构，被定义为简单的数据类型（如整数、浮点数、字符串等）的组合，也可以是更复杂的嵌套结构。

  2. 服务 (Services)

    • 请求/响应模型（Request/Response）：服务是一种同步通信方式，一个节点可以向另一个节点发出请求，并等待响应。服务定义了请求和响应的消息类型。

    • 服务客户端和服务服务器（Service Client/Service Server）：节点可以作为服务服务器来提供服务，或者作为服务客户端来调用服务。

  3. 动作 (Actions)

    • 目标/结果模型（Goal/Result）：用于处理可能需要较长时间才能完成的任务。客户端发送一个目标给动作服务器，并且可以选择接收反馈、取消目标或者等待最终结果。

    • 动作客户端和动作服务器（Action Client/Action Server）：动作客户端可以请求动作服务器执行某项任务，并根据需要获取反馈信息或结果。

  4. 参数服务器 (Parameter Server)

    • 参数存储：节点可以存储和检索全局参数。这个服务器用于存储配置参数，可以被任何节点访问。

那么这些通信方式是如何实现的呢？

  1. 话题 (Topics)

    • 主题通信是基于发布/订阅模式实现的。当一个节点（Node）发布一个消息到某个主题时，ROS中间件会将这个消息传递给订阅了该主题的所有节点。

    • Publisher：节点通过一个发布者（Publisher）对象将消息发布到一个特定的主题。

    • Subscriber：节点通过一个订阅者（Subscriber）对象从一个特定的主题接收消息。

    • Master：ROS master提供名称解析服务，让发布者和订阅者能够找到对方。

    • roscore：在ROS中启动时首先运行的是roscore，它启动了一个Master，以及参数服务器（Parameter Server）和rosout日志节点。

底层实现是通过TCP/IP（默认）或者UDP（通过UDP ROS数据包协议，UDPROS）进行通信。当一个节点向ROS Master注册自己为某个主题的发布者或订阅者时，Master会帮助发布者和订阅者相互发现，并建立一个直接的通信链接。

  1. 服务 (Services)

    • 服务是基于请求/响应模式实现的。一个节点作为服务器提供服务，另一个节点作为客户端可以调用这个服务并等待响应。

    • Service Server：提供特定服务的节点，它注册到ROS master，等待服务请求。

    • Service Client：需要服务的节点，它会询问ROS master服务器的地址，并直接向服务器发送服务请求，并等待回应。

服务通信通常也是通过TCP/IP进行的，确保了通信的可靠性，因为服务调用通常需要得到确定的响应。

  1. 动作 (Actions)

    • 动作通信是一个更复杂的通信机制，用于处理可能需要长时间执行的任务。它在服务通信的基础上增加了中间反馈的可能性。

    • Action Server：执行长时间运行的任务，并提供中间反馈以及最终结果。

    • Action Client：发送任务请求给动作服务器，可以选择接收反馈、取消任务或等待结果。

动作通信内部实际上使用了多个主题来实现其通信模型，包括用于发送目标、接收反馈和结果的主题。

  1. 参数服务器 (Parameter Server)

    • 参数服务器是ROS中用于存储参数值的中心化服务，节点可以读写这些参数。

    • Parameter Server：作为存储全局参数的服务运行，可以在roscore启动时一起启动。

    • Nodes：各个节点可以通过ROS提供的API来获取和设置参数服务器中的参数值。

参数服务器的操作通常通过XML-RPC实现，一个简单的远程过程调用协议，运行在HTTP之上。

可以看到，大多数通信方式的实现依赖TCP通信，那么ROS中的TCP通信是如何实现的呢？

ROS中进行TCP依赖于TCPROS协议，TCPROS是ROS用于节点间通信的主要协议

TCPROS：

当ROS节点需要通过主题通信或服务通信进行交互时，TCPROS被用作默认的传输协议。以下是TCPROS工作的基本步骤：

  • 节点注册：当节点启动并创建一个发布者或订阅者时，它会在ROS Master上注册自己，声明它将发布或订阅的主题名称和消息类型。

  • 连接建立：订阅者节点会要求ROS Master提供所有发布给特定主题的节点的信息。使用这些信息，订阅者节点直接与发布者节点建立TCP连接。

  • 消息传输：一旦TCP连接建立，发布者节点就可以开始将消息流式传输给订阅者节点。每个消息都保证会按顺序且完整地到达订阅者节点。

  • 服务调用：在服务通信中，服务客户端节点会直接与服务服务器节点建立一个TCP连接，通过该连接发送一个服务请求，并等待服务器处理并返回响应。

TCPROS的特点

  • 可靠性：由于TCPROS基于TCP，它保证了消息的可靠传递。如果数据包在传输过程中丢失或损坏，TCP协议会自动重传这些数据包。

  • 连接导向：TCPROS需要在通信的节点之间建立一个持久的连接。这个连接会一直保持，直到主题的发布或订阅结束，或服务调用完成。

  • 流控制和拥塞控制：TCPROS继承了TCP的流控制功能，可以根据网络的状况动态地调整数据的发送速率，以避免网络拥塞。

  • 消息序列化：在ROS中，消息在通过TCPROS发送前需要序列化。序列化后的消息是一个字节流，这个字节流包括了消息的长度和消息本身，以确保接收方可以正确地反序列化并解析消息。

ROS中的通信与MQTT通信异同点：

相同点

  • 发布/订阅模型：ROS和MQTT都使用了发布/订阅（pub/sub）的消息通信模型。在这种模型中，发布者（publisher）发送消息，而订阅者（subscriber）接收消息，发布者和订阅者之间不需要彼此了解。

  • 消息传递：两者都是通过发送和接收消息来实现通信的，而不是使用传统的客户端-服务器模型。

  • 解耦：在ROS和MQTT的通信中，组件间是解耦的。发送者和接收者不需要直接的网络连接，发布者不需要知道谁是订阅者，反之亦然。

区别

  • 设计目的：

    • ROS：专为机器人应用设计，提供了一套完整的工具和库来帮助开发者构建复杂的机器人应用程序。

    • MQTT：是一个轻量级的消息协议，设计用于物联网(IoT)应用中各种设备的通信，尤其是在网络带宽有限或不可靠的环境中。

  • 协议层级：

    • ROS：运行在应用层，使用TCP（默认）或UDP进行节点间的通信，并且提供了一系列的工具和库来帮助开发。

    • MQTT：是一个基于TCP/IP的应用层协议，通常运行在较低的带宽、高延迟或不可靠的网络上。

  • 网络模式：

    • ROS：主要用于局域网（LAN）内部通信，通常用于单个机器人系统或机器人之间的通信。

    • MQTT：设计用于广域网（WAN），可以在互联网上进行通信，适用于远程监控和控制。

  • 可靠性：

    • ROS：依赖于TCPROS，提供了可靠的消息传输。但是，它不原生支持消息的持久化或保证消息传递。

    • MQTT：提供了不同级别的服务质量（QoS）保证。例如，QoS1保证至少一次消息传递，QoS2保证消息仅传递一次。

  • 中间件：

    • ROS：使用ROS Master作为节点发现的中间件，但每个节点之间的通信是直接进行的。

    • MQTT：使用中央代理（broker）来处理所有消息的传递。所有的通信都通过这个中央代理进行。

ROS中建立MQTT通信有什么优点或者应用场景：

  • 进行跨网络通信，MQTT是设计用于跨网络边界进行通信的，这意味着通过MQTT，ROS系统可以轻松地与互联网上的其他系统或设备进行通信。这对于需要远程监控或控制机器人的应用场景非常有用。

  • 并且在实际应用中，结合MQTT与ROS可以在确保消息传递可靠性的同时，扩展ROS的通信能力到互联网层面，实现更广泛的设备互联和数据交换。这种结合特别适合于需要远程操作、云服务集成或大规模设备管理的机器人应用。

环境准备

  1. 安装 MQTT 客户端 需要安装 paho-mqtt Python 库来处理 MQTT 消息：
sudo apt install python-pip
pip install paho-mqtt
  1. 安装 ROS-MQTT 桥 ROS 和 MQTT 之间需要有一个桥接，可以使用 ros_mqtt_bridge 来进行集成。可以创建一个新的 ROS 包并在其中集成 MQTT。

使用方法

1. 创建 ROS 包

创建一个新的 ROS 包，假设包名为 ros_mqtt_bridge：

cd ~/catkin_ws/src
catkin_create_pkg ros_mqtt_bridge std_msgs rospy

2. 编写 MQTT 与 ROS 的桥接代码

编辑 ros_mqtt_bridge 包中的 Python 脚本，假设文件名为 mqtt_bridge.py。

#!/usr/bin/env python
#coding=utf-8

import rospy
import paho.mqtt.client as mqtt
from std_msgs.msg import String

# MQTT 相关配置
MQTT_BROKER = "test.mosquitto.org"  # 使用公共的 MQTT 服务器地址
MQTT_PORT = 1883
MQTT_TOPIC = "mqtt_ros_bridge/test"

# 当接收到 ROS 话题的消息时，将消息发送到 MQTT
def ros_to_mqtt_callback(ros_msg):
    mqtt_msg = ros_msg.data
    rospy.loginfo(f"Sending message to MQTT: {mqtt_msg}")
    client.publish(MQTT_TOPIC, mqtt_msg)

# MQTT 收到消息后发布到 ROS 话题
def on_mqtt_message(client, userdata, msg):
    rospy.loginfo(f"Received MQTT message: {msg.payload.decode()}")
    ros_msg = String()
    ros_msg.data = msg.payload.decode()
    ros_pub.publish(ros_msg)

# 连接到 MQTT 服务器时的回调函数
def on_connect(client, userdata, flags, rc):
    rospy.loginfo("Connected to MQTT broker")
    client.subscribe(MQTT_TOPIC)

if __name__ == '__main__':
    # 初始化 ROS 节点
    rospy.init_node('mqtt_bridge_node')

    # 创建 ROS 发布者
    ros_pub = rospy.Publisher('mqtt_to_ros', String, queue_size=10)

    # 创建 ROS 订阅者，订阅 ROS 话题
    rospy.Subscriber('ros_to_mqtt', String, ros_to_mqtt_callback)

    # 初始化 MQTT 客户端并设置回调
    client = mqtt.Client()
    client.on_connect = on_connect
    client.on_message = on_mqtt_message

    # 连接到 MQTT 服务器
    client.connect(MQTT_BROKER, MQTT_PORT, 60)

    # 启动一个线程处理 MQTT 连接
    client.loop_start()

    # ROS 主循环
    rospy.spin()

    # 当 ROS 停止时关闭 MQTT 连接
    client.loop_stop()
    client.disconnect()

3. 编译代码

在 catkin_ws 工作空间下编译代码：

cd ~/catkin_ws
catkin_make
source devel/setup.bash

4. 启动 ROS-MQTT 桥

启动你编写的 ROS 节点：

rosrun ros_mqtt_bridge mqtt_bridge.py

5. 发布 ROS 话题消息到 MQTT

你可以通过以下命令向 ROS 话题 ros_to_mqtt 发布消息，并查看它是否被桥接到 MQTT：

rostopic pub /ros_to_mqtt std_msgs/String "Hello from ROS"

6. 订阅 MQTT 消息并发布到 ROS

你可以通过 MQTT 客户端发布消息到 MQTT 话题，然后查看消息是否被桥接回 ROS。例如，使用命令行 MQTT 客户端：

mosquitto_pub -h test.mosquitto.org -t "mqtt_ros_bridge/test" -m "Hello from MQTT"

然后查看 ROS 话题 /mqtt_to_ros 是否接收到了消息：

rostopic echo /mqtt_to_ros

总结

这个简单的 ROS-MQTT 桥接案例展示了如何将 ROS 话题消息通过 MQTT 发送到另一系统，以及如何将 MQTT 消息发布到 ROS 系统中。在实际应用中，你可以根据具体需求进行扩展和优化。

ROS上快速验证PID算法

PID算法简介

  • 比例（Proportional）(P)：
计算当前偏差（设定值与实际值的差距），并乘以比例增益（Kp）。

输出对系统的当前状态进行纠正。
  • 积分（Integral)（I）：
累积偏差随时间的总和，并乘以积分增益（Ki）。

解决由于比例控制引入的静态误差，确保系统最终能够达到设定值。
  • 微分（Derivative）（D）：
计算偏差变化率，并乘以微分增益（Kd）。

抑制系统的振荡，提高系统的稳定性。

这里有一张比较形象的图可以用于简述其工作状态。

PID控制器包是一个比例-积分-微分（PID）控制器的实现，适用于简单的控制问题，直接应用PID循环即可解决。该包专注于单一目的，并致力于做好这件事。它提供了多种功能，使得添加控制器和调整控制循环变得更加容易，同时还提供了多个示例。

该页面使用了控制系统理论中的几个术语：

  • 设定值：指控制过程的期望值，例如机械臂的位置、马达的速度等。

  • 植物状态：指控制过程的实际值。植物是指过程本身，它的状态就是要控制的属性，例如机械臂的位置、马达的速度等。

  • 控制量：指控制器施加给控制过程的力量，目的是使植物状态等于设定值。控制量可以通过改变电压、占空比等方式来调节。

功能

  • 易于接口：使用std_msgs/Float64来处理设定值、植物状态和控制量。

  • 动态调整Kp、Ki和Kd，方便实时调试。

  • 误差导数中带有可调节截止频率的低通滤波器，提供更平滑的导数项。

  • ROS节点私有参数可配置所有控制器参数。

  • 支持多控制器。

  • 支持比时钟快的仿真。

  • 自动/手动模式。

  • 通过仿真一阶和二阶植物来评估控制器功能。

  • Ziegler-Nichols自动调谐器。

  • 支持不连续角度测量。

安装包

可以通过二进制安装PID包，也可以从源码构建。如果通过二进制安装，下面提到的示例文件将位于/opt/ros/<release>/share/pid目录下。

二进制安装

在命令中替换"melodic"为你使用的版本：

sudo apt-get install ros-melodic-pid

或者选择源码安装与构建

cd catkin_ws/src
git clone https://bitbucket.org/AndyZe/pid.git
cd ..
catkin_make

示例：运行servo_sim.launch

在pid/launch目录中提供了多个启动文件，这些文件展示了控制器的功能。servo_sim.launch就是其中之一，PID控制器控制一个模拟伺服器的位置，该控制循环以100Hz运行。

运行命令：

roslaunch pid servo_sim.launch

多个窗口将打开，其中一个rqt_plot窗口显示了控制器的输入和输出。使用缩放按钮查看详细信息。

  • /setpoint/data：设定值，表示PID控制器期望的伺服器位置。

  • /state/data：植物状态，表示PID控制器输入的伺服器当前位置。

  • /control_effort/data：控制量，表示PID控制器输出的用于调节伺服器位置的控制量。

动态调整PID增益

servo_sim.launch还会打开一个rqt_reconfigure窗口，点击"left_wheel_pid"条目以动态调整Kp、Ki和Kd参数。

自动调谐

在运行servo_sim.launch时，可以通过以下命令启动Ziegler-Nichols自动调谐器：

rosrun pid autotune

控制器节点

控制器节点是包中的核心节点，负责实现PID算法，并通过调整控制量使植物状态等于设定值。

主题

PID控制器订阅并发布以下主题：

  • setpoint：控制器订阅此主题，读取设定值。

  • state：控制器订阅此主题，读取植物状态。

  • control_effort：控制器发布此主题，输出控制量。

  • pid_enable：控制器订阅此主题以启用或禁用控制器。

参数

控制器节点启动时读取以下私有参数：

  • Kp, Ki, Kd：比例、积分和微分增益的值。

  • upper_limit, lower_limit：控制量的上下限。

  • windup_limit：积分误差的上限。

  • cutoff_frequency：导数项的低通滤波器截止频率。

  • setpoint_timeout：设定值超时参数，确定最后一次设定值消息后控制量消息的发布时间。

动态重配置

可以通过rosrun rqt_reconfigure rqt_reconfigure命令动态调整PID增益参数。

支持多控制器

在实际应用中，通常会有多个PID控制回路，每个控制器需要订阅其对应的植物状态，并在不同的control_effort主题上发布控制量。可以通过命名空间或参数等方式配置多个控制器。

手动/自动模式

手动模式下，PID控制器停止发布control_effort消息，误差积分保持为零。

跨ROS系统通信

当涉及到在机器人操作系统（ROS）环境中的通信时，标准做法通常是在同一个ROS网络内通过话题和服务进行。但在某些特定情况下，比如当你有两个分布在不同网络中的ROS系统时，标准的通信方法可能不太适用。此时，一个可行的解决方案是建立一个直连的TCP通信，允许跨ROS系统的节点之间直接传输消息。本文将实现一个进行TCP通信节点示例程序

场景概述

想象一下，有两台计算机A和B，每台上都运行着一个独立的ROS系统。我们的目标是让系统A中的节点监听话题testa，并将接收到的消息通过TCP发送给系统B中的节点。同样，系统B中的节点将监听话题testb并通过TCP将消息回传给系统A。

这种设置的一个典型应用场景是多机器人协作，其中机器人位于不同的网络环境中，或者需要与不支持ROS的遗留系统交互。

实现步骤

1. 准备工作

首先，确保两台计算机都安装了ROS，并且需要使两台设备能互ping 在本示例中我是在局域网中的两台设备中进行演示的 闲言少叙，我们直接开始分析以下的程序

将两个设备都用下面的指令关闭防火墙

sudo ufw disable

系统A

在系统A上，创建一个节点tcp_publisher_a.py，它订阅话题testa并将消息发送给系统B。 完整程序如下：

#!/usr/bin/env python
#coding=utf-8
import rospy
from std_msgs.msg import String
import socket

def on_message_received(msg):
    rospy.loginfo("Received message on 'testa': %s", msg.data)
    try:
        # 创建TCP socket
        tcp_socket = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
        # 连接到系统B的TCP服务器
        tcp_socket.connect(('192.168.7.244', 10000))
        rospy.loginfo("Sending message to System B")
        tcp_socket.send(msg.data.encode('utf-8'))
        tcp_socket.close()  # 显式关闭socket
    except Exception as e:
        rospy.logerr("Could not send message to System B: %s", e)

def testa_listener():
    rospy.init_node('testa_listener', anonymous=True)
    rospy.Subscriber('testa', String, on_message_received)
    rospy.spin()

if __name__ == '__main__':
    testa_listener()

这个Python程序是为了作为机器人操作系统网络的一部分而设计的。它监听特定的ROS主题上的消息，并在收到消息后，通过TCP套接字连接将该消息转发到另一个系统（称为"System B"）。该程序使用了rospy库，这是ROS的Python客户端库，以及socket库用于网络通信。

运行示例：

以下是对程序各个部分的详细分析：

导入模块

import rospy
from std_msgs.msg import String
import socket

  • rospy是ROS的Python客户端API，提供了与ROS交互所需的功能。

  • std_msgs.msg是提供标准ROS消息类型的包。这里导入了消息类型String。

  • socket是Python模块，提供了对BSD套接字接口的访问，允许通过网络进行通信。

回调函数

def on_message_received(msg):
    rospy.loginfo("Received message on 'testa': %s", msg.data)
    try:
        # 创建TCP socket
        tcp_socket = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
        # 连接到系统B的TCP服务器
        tcp_socket.connect(('192.168.3.21', 10000))
        rospy.loginfo("Sending message to System B")
        tcp_socket.send(msg.data.encode('utf-8'))
        tcp_socket.close()  # 显式关闭socket
    except Exception as e:
        rospy.logerr("Could not send message to System B: %s", e)

  • on_message_received是一个函数，当在ROS主题'testa'上接收到新消息时调用。

  • rospy.loginfo用于记录接收到的消息。

  • 使用

    • AF_INET表示使用的是IPv4。

    • SOCK_STREAM表示它是一个TCP套接字。

  • 套接字尝试连接到'SYSTEM_B_IP'的10000端口。'SYSTEM_B_IP'应该替换为System B的实际IP地址。

  • 成功连接后，会打印另一个日志消息，表示消息正在发送到System B。

  • 从ROS主题收到的消息通过.encode('utf-8')转换为UTF-8编码的字节串，然后通过套接字发送。

  • 如果在尝试建立连接或发送数据时出现socket.error异常，将通过rospy.logerr记录错误信息。

监听器函数

def testa_listener():
    rospy.init_node('testa_listener', anonymous=True)
    rospy.Subscriber('testa', String, on_message_received)
    rospy.spin()

  • listener函数初始化了一个名为'testa_listener'的ROS节点。

  • rospy.init_node用于初始化节点，anonymous=True表示在节点名称后添加随机数以确保名称的唯一性。

  • rospy.Subscriber创建了一个订阅者，订阅名为'testa'的主题，并指定当有消息到达时调用on_message_received函数。

  • rospy.spin()是一个不会返回的循环，它会保持程序运行并等待消息到达。

主函数

if __name__ == '__main__':
    listener()

当Python脚本作为主程序运行时（而不是被导入到另一个脚本中），listener()函数会被调用，启动整个流程。

注意事项

  • 需要确保ROS环境已经设置好，并且此脚本在ROS网络中的适当位置运行。

  • 程序没有处理发送消息后的响应或确认，只是单向发送。

  • rospy.spin()会阻塞当前线程，所以如果需要执行其他任务，应该考虑将监听过程放在另一个线程或使用异步方式。

系统B

在系统B上，创建一个节点tcp_publisher_b.py，它订阅话题testb并将消息发送到系统A。

#!/usr/bin/env python
import rospy
from std_msgs.msg import String
import socket

def tcp_server():
    rospy.init_node('tcp_server', anonymous=True)
    pub = rospy.Publisher('testb', String, queue_size=10)

    server_socket = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
    server_socket.bind(('', 10000))  # 绑定所有可用的接口
    server_socket.listen(1)
    rospy.loginfo("TCP Server listening on port 10000")

    while not rospy.is_shutdown():
        rospy.loginfo("Waiting for connection...")
        connection, client_address = server_socket.accept()
        rospy.loginfo("Connected with %s", client_address)
        try:
            while True:
                data = connection.recv(1024)
                if data:
                    message = data.decode('utf-8')
                    rospy.loginfo("Publishing received message to 'testb': %s", message)
                    pub.publish(message)
                else:
                    break
        finally:
            connection.close()

if __name__ == '__main__':
    tcp_server()

服务端接收信息示例：

分析： 这个Python程序是一个基于ROS (Robot Operating System) 的TCP服务器，它监听特定端口上的TCP连接，并将接收到的数据发布到ROS主题。以下是对程序的详尽分析：

导入模块

import rospy
from std_msgs.msg import String
import socket

  • rospy 是ROS的Python客户端API，提供了与ROS交互所需的功能。

  • std_msgs.msg 是提供标准ROS消息类型的包。这里导入了消息类型 String。

  • socket 是Python模块，提供了对BSD套接字接口的访问，允许通过网络进行通信。

TCP服务器函数

def tcp_server():
    rospy.init_node('tcp_server', anonymous=True)
    pub = rospy.Publisher('testb', String, queue_size=10)

    server_socket = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
    server_socket.bind(('', 10000))  # 绑定所有可用的接口
    server_socket.listen(1)
    rospy.loginfo("TCP Server listening on port 10000")

    while not rospy.is_shutdown():
        rospy.loginfo("Waiting for connection...")
        connection, client_address = server_socket.accept()
        rospy.loginfo("Connected with %s", client_address)
        try:
            while True:
                data = connection.recv(1024)
                if data:
                    message = data.decode('utf-8')
                    rospy.loginfo("Publishing received message to 'testb': %s", message)
                    pub.publish(message)
                else:
                    break
        finally:
            connection.close()

  • tcp_server 函数首先初始化一个名为 'tcp_server' 的ROS节点。anonymous=True 参数确保每次启动该节点时都有一个独一无二的名称，避免节点名称冲突。

  • 创建了一个 Publisher 对象 pub，用于发布消息到名为 'testb' 的ROS主题，队列大小设置为10。

  • 创建了一个TCP套接字，并绑定到主机的空字符串地址和端口10000上。空字符串意味着服务器将接受任何可用网络接口的连接。

  • 通过 server_socket.listen(1) 开始监听传入的连接，这里的参数1指定了操作系统可以挂起的最大连接数。

  • 进入一个循环，循环体内首先检查 rospy.is_shutdown()，以确定是否应该退出循环，这通常是响应于ROS节点关闭的信号。

  • 使用 server_socket.accept() 阻塞等待一个新的连接。当一个新的客户端连接时，它返回一个新的套接字对象 connection 和客户端的地址 client_address。

  • 一旦建立连接，程序将打印客户端地址，并进入另一个循环接收数据。使用 connection.recv(1024) 接收数据，这里的1024表示最大数据量的字节数。

  • 如果收到数据，则将其解码为UTF-8格式的字符串，并打印日志信息，然后通过之前创建的 Publisher 对象将消息发布到 'testb' 主题。

  • 如果没有收到数据（data 为空），则退出接收循环。

  • 无论是因为接收到空数据还是由于其他异常，最终都会关闭与客户端的连接。

主函数

if __name__ == '__main__':
    tcp_server()

当Python脚本作为主程序运行时，tcp_server() 函数会被调用，启动TCP服务器。

注意事项

  • 需要确保ROS环境已经设置好，并且此脚本在ROS网络中的适当位置运行。

  • 服务器使用 rospy 的日志功能来记录信息，便于调试和跟踪。

  • TCP服务器会一直运行，直到ROS节点被关闭。

  • 这个程序仅支持同时处理一个客户端连接,如果需要同时处理多个连接，可以考虑使用多线程或异步IO来接收来自不同客户端的连接。

  • 如果客户端发送的数据超过1024字节，数据将被截取，只有前1024字节会被接收和处理。如果需要处理更大的数据，需要增加recv方法的缓冲区大小或实现某种形式的分块接收逻辑。

补充 tcp_server() 函数完成的工作如下：

  1. 使用 rospy.init_node 初始化了一个名为 'tcp_server' 的ROS节点。anonymous=True 参数使得每次启动该节点时，都会在节点名称后附加一个随机数，以确保节点名称的唯一性。

  2. 创建了一个 Publisher 对象 pub，用于将接收到的字符串消息发布到名为 'testb' 的ROS主题。queue_size=10 表示消息队列的大小，这决定了在处理消息之前可以积累多少未处理的消息。

  3. 创建了一个TCP套接字 server_socket，使用IPv4(socket.AF_INET) 和TCP 协议(socket.SOCK_STREAM)。

  4. 套接字绑定到所有可用接口的端口10000上。在 server_socket.bind(('', 10000)) 调用中，空字符串 '' 表示服务器将接受任何可用网络接口上的连接。

  5. 通过 server_socket.listen(1)，服务器开始监听传入的连接请求。参数 1 指定了操作系统可以挂起的最大连接数，即服务器未处理的连接队列长度。

  6. 进入一个循环，循环的继续条件是 rospy.is_shutdown() 返回 False，这是一个检查ROS节点是否接收到了关闭信号（如Ctrl+C）的方法。

  7. 在循环内部，首先打印等待连接的日志信息，然后使用 server_socket.accept() 阻塞等待一个新的连接。当有新的连接请求时，accept() 方法返回一个新的套接字对象 connection 和客户端的地址 client_address。

  8. 当有客户端连接后，进入另一个无限循环，不断地接收来自客户端的数据。使用 connection.recv(1024) 接收最多1024字节的数据。

  9. 如果接收到数据，它将数据解码为UTF-8格式的字符串，打印相关的日志信息，然后将解码后的字符串作为 String 类型的消息发布到 'testb' 主题。

  10. 如果接收到的数据为空，这意味着客户端已经关闭了连接，于是退出接收循环。

ROS中常用的几种SLAM算法

文章将介绍使用的基于机器人操作系统（ROS）框架工作的SLAM算法。 在ROS中提供的五种基于2D激光的SLAM算法分别是：HectorSLAM，Gmapping，KartoSLAM，CoreSLAM和LagoSLAM。当然最后还有比较经典的google开源的cartographer，虽然不是基于ROS的但是大牛们已经将它修改为基于ROS的版本的cartographer_ros,

机器人爆炸式增长的一个主要问题是不能在不同的机器人平台上重复使用代码。然而，ROS中的硬件抽象层及其消息服务允许创建可用于许多不同机器人平台的新代码。而且，ROS提供了一套稳定的机器人软件包，公认的SLAM评估方法都依赖于机器人社区可用的标准数据集。 本文中研究了基于激光的主要二维SLAM算法，所有SLAM的结果都使用占用网格作为最终输出，使用地图相似性的性能指标进行分析。 重点是放在地图质量，而不是姿态估计误差，因为映射输出受到本地化问题的高度影响。 主要目标是提供ROS中所有五种算法的优缺点概述，提供简单而准确的定量比较，从而为ROS开发者定义一个通用的指导方针，以选择最符合他们需求的算法。

（1）HectorSLAM

HectorSLAM是一种结合了鲁棒性较好的扫描匹方法2D SLAM方法和使用惯性传感系统的导航技术。传感器的要求：高更新频率小测量噪声的激光扫描仪.不需要里程计,使空中无人机与地面小车在不平坦区域运行存在运用的可能性.作者利用现代激光雷达的高更新率和低距离测量噪声，通过扫描匹配实时地对机器人运动进行估计。所以当只有低更新率的激光传感器时，即便测距估计很精确，对该系统都会出现一定的问题，如下图是该系统生成的二维地图。

无标题

利用已经获得的地图对激光束点阵进行优化, 估计激光点在地图的表示,和占据网格的概率.，其中扫描匹配利用的是高斯牛顿的方法进行求解. 找到激光点集映射到已有地图的刚体转换(x,y,theta).具体的公式如下：

下图二维地图是hectorSLAM 多分辨率地图的表示。 网格单元格长度（从左到右）：20厘米，10厘米和5厘米 ，使用多分辨率地图表示，以避免陷入局部最小化。 这个解决方案背后的想法是在内存中有不同的地图，同时使用之前估计的姿势来更新它们。 计算成本 仍然很低，所以地图总是一致的

最后，3D空间导航状态估计是基于EKF滤波器。 但是，这种情况是仅在IMU存在时才需要，例如在空中机器人的情况下。 因此，这里我们只讨论二维的SLAM

它会将不再这项工作中使用。如下图显示了HectorSLAM方法的概述。

（2）Gmapping

Gmapping是一种基于激光的SLAM算法，它已经集成在ROS中，是移动机器人中使用最多的SLAM算法。这个算法已经由Grisetti等人提出是一种基于 Rao-Blackwellized的粒子滤波的 SLAM方法。基于粒子滤波的算法用许多加权粒子表示路径的后验概率，每个粒子都给出一个重要性因子。但是，它们通常需要大量的粒子才能获得比较好的的结果，从而增加该算法的的计算复杂性。此外，与PF重采样过程相关的粒子退化耗尽问题也降低了算法的准确性。粒子退化问题包括在重采样阶段从样本集粒子中消除大量的粒子。发生这种情况是因为它们的重要性权重可能变得微不足道。因此，这意味着有一定的小概率时间会消除正确的假设的粒子。为了避免粒子的退化问题，已经开发了自适应重采样技术。

作者还提出了一种计算精确分布的方法，不仅考虑机器人平台的运动，还考虑最近的观察。提出了在使用测距运动模型时结合观测值提出建议分布。然而，当一个移动机器人装备一个非常精确的传感器LRF时，可以使用该传感器的模型，因为它可以实现了极高的似然函数。 基于此，作者整合了最近的传感器观测zt，并且他们计算高斯分布近似有效地获得下一代粒子分布。

这种自适应重采样降低了PF预测步骤中机器人姿态的不确定性。 结果，由于扫描匹配过程，所需的粒子数量减少，因为不确定性较低。 实验中，Gmapping使用的粒子数量是30，与普通PF方法相比，这是非常低的。

（3）LagoSLAM

LagoSLAM 是线性近似图优化,不需要初始假设。基本的图优化slam的方法就是利用最小化非线性非凸代价函数.每次迭代, 解决局部凸近似的初始问题来更新图配置,过程迭代一定次数直到局部最小代价函数达到. (假设起始点经过多次迭代使得局部代价函数最小). 。假设图中每个节点的相对位置和方向都是独立的，作者求解了一个等价于非凸代价函数的方程组。为此，提出了一套基于图论的程序，通过线性定位和线性位置估计，得到非线性系统的一阶近似。

（4）KartoSLAM

KartoSLAM是基于图优化的方法，用高度优化和非迭代 cholesky矩阵进行稀疏系统解耦作为解．图优化方法利用图的均值表示地图，每个节点表示机器人轨迹的一个位置点和传感器测量数据集，箭头的指向的连接表示连续机器人位置点的运动，每个新节点加入，地图就会依据空间中的节点箭头的约束进行计算更新．

　　KartoSLAM的ROS版本，其中采用的稀疏点调整（the Spare Pose Adjustment(SPA)）与扫描匹配和闭环检测相关．landmark越多,内存需求越大,然而图优化方式相比其他方法在大环境下制图优势更大.在某些情况下KartoSLAM更有效,因为他仅包含点的图(robot pose),求得位置后再求map.

（5）CoreSLAM

    COreSLAM最小化性能损失的一种slam算法.将算法简化为距离计算与地图更新的两个过程,  第一步,每次扫描输入,基于简单的粒子滤波算法计算距离,粒子滤波的匹配器用于激光与地图的匹配,每个滤波器粒子代表机器人可能的位置和相应的概率权重,这些都依赖于之前的迭代计算. 选择好最好的假设分布,即低权重粒子消失,新粒子生成..在更新步骤,扫描得到的线加入地图中,当障碍出现时,围绕障碍点绘制调整点集,而非仅一个孤立点。

以上各种激光测距仪的ＳＬＡＭ的算法的对比：

（6）cartographer

cartographer是Google的实时室内建图项目，传感器安装在背包上面，可以生成分辨率为5cm的2D格网地图。获得的每一帧laser scan数据，利用scan match在最佳估计位置处插入子图（submap）中，且scan matching只跟当前submap有关。在生成一个submap后，会进行一次局部的回环（loop close），利用分支定位和预先计算的网格，所有submap完成后，会进行全局的回环。

ros相机标定

安装相机标定库

sudo apt-get install ros-melodic-camera-calibration

启动摄像头驱动

roslaunch handsfree_camera camera.launch

开始标定

　　1.准备标定纸

　　2.启动标定程序

rosrun camera_calibration cameracalibrator.py --size 8x6 --square 0.024  image:=/camera/rgb/image_raw

　　size 是指棋盘内部角点个数，我是用的标定板公有12x8个角点（即12列8行），”x”是字母,而不是乘号…square指方形的边长，我使用的标定板方形边长为24mm,image为输出图像的话题，usb_cam为相机的名称（即在/image_raw前面部分）

运行如下语句： 角点数 棋盘格大小 topic映射

rosrun camera_calibration cameracalibrator.py --size 9x7 --square 0.108 image:=/camera/rgb/image_raw   --no-service-check

　　3.开始标定

在出现上述窗口后，拿标定板在窗口中移动，

我们从图1中可以看到，在GUI的右侧有X，Y，Size，Skew这几个标志条
它们的含义是：
（1）x:标定板在图像中的左右位置，从左到右x由小变大。
（2）y:标定板在图像中的上下位置，从上到下y由小变大。
（3）Size：标定板在图像中的大小，由远及近，Size由小变大。
（4）Skew:标定板在图像中倾斜的角度大小

操作过程：

（1）为了得到尽量丰富的x,y值:标定的过程中要尽量缓慢移动标定板，使标定板的位置出现在图像中的各个地方（上中下左右）。
（2）为了得到丰富的Size值缓慢移动标定板，使标定板由远及近，采集到丰富的尺寸信息，最好有一张，标定板占据视野绝大部分的图像。
（3）为了得到丰富的Skew信息，缓慢移动标定板，使标定板的俯仰,偏航，滚动变化。
注：在操作的过程中，标定板移动不要过快，并且，每移动到一个位置要停顿下，如果发现这四标志条都变绿了说明采集到了足够的信息。并且如果可以执行标定，CALIBRATE就会变亮。

　　4．执行标定，得到结果

CALIBRATE选项变亮后，就可以点击，执行标定（最好各标志条也都变绿），此时图像会变灰，等待几分钟，标定完成后，再把标定板拿到摄像机前会出现如下画面：

results matching ""

    No results matching ""