ROS操作系统快速入门

一、简介

手机界的安卓操作系统就和机器人界的ros操作系统一样，向上可以扩展不同应用，向下可以适应不同的硬件设备

模块化、分布式的系统设计

• 模块化：ros中每个.lanuch文件相当于一个机器人，每个node节点相当于一个模块

• 分布式：可以使用多个计算机（实际上是cpu）来处理不同模块的计算任务

ros操作系统要跑在运算能力强的芯片上，例如瑞芯微、树莓派、电脑等，通过数据链路（usb、串口）与单片机进行通信传输指令与数据，在ros的应用中开发能够进行串口通信的node，例如GMapping（建图）、Move_base（自主导航）

二、安装虚拟机与ROS系统安装

虚拟机的缺点

• 运行效率低【两个系统之间隔着虚拟机】
• 调用宿主硬件的方法比较繁琐【利用宿主电脑的显卡GPU进行CUDA加速，映射操作比较复杂】

安装ubuntu20.04

ubuntu20.04安装教程

一些注意点：

1. 关于20.04版本：

   • 可以在这个页面下载：https://releases.ubuntu.com/20.04.5，选“64-bit PC (AMD64) desktop image”版本

2. 修改分辨率

   • 因为分辨率看不见按钮，通过命令行修改分辨率
   • 按Ctrl+Alt+T，出现终端窗口，然后在终端窗口输入命令：
   • xrandr -s 1280x800后回车（需要注意的是1280和800中间的x是字母x）
     

3. 安装过程跳过更新
   

4. 虚拟机备份

   • 备份虚拟机，避免一些问题重复安装虚拟机
   • 查看克隆虚拟机章节

5. 安装vmware tools工具

   • 安装步骤
   • 注意：由于文件夹由于容量过小，不能在这个文件夹解压，需要将文件夹移动到用户目录下

6. Ubuntu20.04使用过程中总是弹出检测到系统程序出现问题的解决方法

   • 解决方法链接

7. Ubuntu已安装VMware Tools仍无法粘贴复制主机文件夹

   • 解决方法链接

8. 刚建立好的linux虚拟机使用NAT方式可以连接外网，系统重启几次，系统无法上网，这是什么问题导致的呢？

   • 解决ubuntu20.04虚拟机无法上网的问题
   • sudo dhclient -v 动态分配地址，但是每次开机都需要执行该指令

其他教程：

• 参考连接
• 参考连接

三、ROS系统安装

切换镜像源

1. 要知道当前系统的代号，可以用以下命令：

lsb_release -a

2. 换镜像源教程

关于无法保存只读文件问题，可以在home目录下创建好该文件然后复制过去

3. 刷新列表，一定要执行刷新

sudo apt-get update//必须执行
sudo apt-get upgrade//非必须执行

视频教程

教程链接

四、ROS应用商城APT源

简介与指令介绍

ubuntu有自带的下载源，到指定的应用商城下载软件包

ROS官方应用商城网站：index.ros.org

添加不同的源，就类似于添加不同的应用商城

下载指令格式：

suod apt install ros-noetic-软件包名称

软件包需要ros系统启动起来才能使用，另外开终端输入

roscore

报错的解决方法

运行软件包指令

rosrun 包的名称 节点的名称

案例

安装如下两个软件包进行测试

sudo apt install ros-noetic-turtlesim
sudo apt install ros-noetic-rqt-robot-steering

运行两个软件包

rosrun turtlesim turtlesim_node
rosrun rqt_robot_steering rqt_robot_steering

输入框中可以修改速度发送的主题名称

ros应用商城介绍

点击软件包的名称可以查看详细信息，例如源代码存放位置是github

右侧的website内有软件包的使用细节介绍

五、GIthub

在github中下载的是源码，没有编译，需要在本地编译，编译时有指定的文件格式，所以需要建立一个工作空间，所谓的工作空间就是符合某种要求的目录结构，编译器会安装这个目录结构去检索源代码，完成编译工作

建立如下目录结构：

安装git与克隆软件包

sudo apt install git

在src目录下克隆软件包

git clone https://github.com/6-robot/wpr_simulation.git

执行安装依赖包的脚本

下载后的目录结构中有scripts目录，【用于放置脚本文件，执行脚本可以为下载安装和编译需要用到的依赖项，为实体机器人映射端口】，根据ros版本执行不同的脚本，此类操作只需要执行一次

./install_for_noetic.sh

编译

cd ~/catkin_ws
catkin_make

注意：该指令（catkin_make）只能在catkin_ws路径下，会对src里面的源代码进行编译

编译完成后会得到如下几个文件

然后是将catkin_ws工作空间里面的环境变量加载到终端里面，使用source指令载入工作空间的环境设置

source ~/catkin_ws/devel/setup.bash

使用roslaunch运行编译好的ros程序

roslaunch wpr_simulation wpb_simple.launch

通过上面安装的速度控制工具可以控制机器人运动，运行软件包

rosrun rqt_robot_steering rqt_robot_steering

通常会把设置工作空间环境参数的source指令放到终端启动的初始化脚本文件bashrc中

gedit ~/.bashrc

在最末尾添加

source ~/catkin_ws/devel/setup.bash

在终端输入

source ~/.bashrc

六、安装vscode

快速下载教程

下载后的路径在

/home/fang/下载

执行解压

sudo dpkg -i 压缩包名称

运行vscode

code

在vscode软件中将/home/fang/catkin_ws/src添加为工作空间

安装插件：

• chinese
• ros（会顺带安装c++和python等插件）
  
• cmake（安装CMake Tools会自动安装CMake）
  
• bracket【vscode已经内置了，无须安装】
• C/C++ Extension Pack【它包含了 vscode 编写 C/C++ 工程需要的插件（C/C++、C/C++ Themes、CMake、CMake Tools和Better C++ Syntax等）】

其他插件：地址

设置编译快捷键:

按下：ctrl+shift+b
选择第一项：

此时编译工作空间里面的软件包

当然也可以设置第一项为默认编译选择，点击右侧齿轮按钮

此时工作目录中出现的tasks.json文件，修改group选项

设置拼写错误检查

• 出现头文件引用问题是，c/c++插件没有找到，删除左侧的c__cpp_properties.json文件，然后关闭vscode再打开，此时ros插件会初始化，会修改c__cpp_properties.json文件，自动将头文件添加到路径中

• 特殊情况：ros插件无法识别，则需要手动关闭报错，按住ctrl+shift+p，输入：error squiggles，选择禁用错误波形曲线，此时左侧会多一个setting.json文件， 如果要启动检查则改为true

七、超级终端Terminator

安装Terminator

sudo apt install terminator

ctrl+AIT+T启动Terminator，在运行其他任务的时候可以分割界面

快捷键

• ctrl+shifr+E：分割为左右
• ctrl+shifr+O：分割为上下
• ALT+方向键：切换焦点窗口
• ctrl+shifr+W：删除刚刚切割的窗口

注意：ctrl+shifr+E可能会激活ubuntu输入法的符号模式，需要取消输入法对该快捷键的占用，终端输入：ibus-setup，删除该快捷键的占用，操作步骤如下

八、Node节点与Package包

基本单元：node

一个机器人由多个节点组成

节点的组织形式：package

使用apt下载的，虽然只使用了一个软件包里面的某些节点的功能，但是下载的还是整个软件包（package）

ros采用cmake和catkin作为编译工具，引入了package的概念（ros中各个节点采用模块化的思想），可以将各个节点的关联性进行分类，一次只安装一个包（一组节点），此时包可以理解为节点的容器

九、写一个Node节点

创建超声波节点的软件包

进入catkin_ws/src目录：

catkin_create_pkg ssr_pkg rospy roscpp std_msgs 

catkin_create_pkg指令：

catkin_create_pkg <包名> <依赖项列表>

ps：依赖项是通用的依赖项，也可以成为通用节点

• rospy：ros和python结合
• roscpp：ros和cpp结合
• std_msgs ：标准消息

此时会创建两个文件，两个目录

ps：

• 凡是catkin软件包里面一定含有package.xml
• CMakeLists.txt是指令和注释，可以作为编译项进行复制，避免写错，##表示注释 ，#表示指令

roscd指令

该指令可以在终端中进入指定软件包的文件地址

路径：/opt/ros/noetic/share/roscpp 里面都是软件包，来源于安装ros的desktop-full基础包或独立扩展包
catkin_ws工作空间中的软件包则是源码，而在此目录下是可执行文件

在bashrc中source /opt/ros/noetic/setup.bash和source ~/catkin_ws/devel/setup.bash就分别是软件包的两个安装空间

创建超声波node节点

在srv目录下创建chao_node.cpp文件

#include 

int main(int argc, char *argv[])//argv没有const
{
    /* code */
    ros::init(argc,argv,"chao_node");//节点初始化后才能与ros系统产生连接，才能调用ros的核心功能
    printf("hello world");
    //节点启动后一直运行，true可以替换为ros::ok()函数，避免节点不响应任何外部信号，例如ctrl+c
    while(ros::ok()){
        printf("hhhhh");
    }
    return 0;
}

为新建节点添加编译规则

打开CMakeLists.txt，添加如下两条

# 为这个包的节点添加可执行指令
# 前一项：指定可执行文件的名字，可以和节点名保持一致
# 后一项：指定从哪个代码文件进行编译
add_executable(chao_node src/chao_node.cpp)

# 链接ros::init函数所在的库文件
target_link_libraries(chao_node
  ${catkin_LIBRARIES}
)

运行节点

1. 启动roscore
2. 终端输入：rosrun ssr_pkg chao_node

rosrun <包名> <节点文件名>

十、Topic话题与Message消息

Topic 话题

1. 话题Topic是节点间进行持续通讯的一种形式
2. 话题通讯的两个节点通过话题的名称建立起话题通讯连接话题中通讯的数据，叫做消息Message
3. 消息Message通常会按照一定的频率持续不断的发送，以保证消息数据的实时性
4. 消息的发送方叫做话题的发布者Publisher。
5. 消息的接收方叫做话题的订阅者Subsciber。
6. 一个ROS节点网络中，可以同时存在多人话题
7. 一个话题可以有多个发布者，也可以有多个订阅者2
8. 一个节点可以对多个话题进行订阅，也可以发布多个话题
9. 不同的传感器消息通常会拥有各自独立话题名称，每个话题只4有一个发布者。
10. 机器人速度指令话题通常会有多个发布者，但是同一时间只能有一个发言人。

一个话题多个发布者的情况是存在的，例如底盘驱动需要速度指令，该指令可以由跟踪或者导航节点发布，注意的是要控制发布的顺序

Message 消息

不同的消息包有不同的消息类型，标准消息包std_msgs就包含多个基本消息类型

可以将消息类型理解为C语言中的结构体

十一、Publisher 发布者和Subscriber 订阅者的实现

Publisher 发布者的实现

chao_node.cpp

1. 确定话题名称和消息类型
2. 在代码文件中include消息类型对应的头文件
3. 在main函数中通过NodeHandler大管家发布一个话题并得到消息发送对象。
4. 生成要发送的消息包并进行发送数据的赋值
5. 调用消息发送对象的publish()函数将消息包发送到话题当中。

#include 
#include

int main(int argc, char *argv[])
{
    /* code */
    ros::init(argc,argv,"chao_node");

    ros::NodeHandle nh;//该对象是节点与ROS通讯的关键，理解为管家,通过管家发布一个话题并得到一个消息发送对象
    ros::Publisher pub = nh.advertise<std_msgs::String>("connect",10);//pub理解为由管家发的手机,该函数是泛型函数，尖括号参数是消息类型，参数是话题名称和，消息缓存长度

    ros::Rate loop_rate(10);//生成频率对象，括号内是每秒执行次数，即为10hz

    while(ros::ok()){
        printf("wait for connect!\n");
        std_msgs::String msg;
        msg.data = "we can hhhhhh !!!!";
        pub.publish(msg);
        loop_rate.sleep();//短时间阻塞

    } 
    return 0;
}

消息缓存长度可以避免等待订阅者订阅消息的时间问题

编译：

ctrl+shift+b

运行：

roscore

rosrun ssr_pkg chao_node

运行结果：

rostopic指令

查看当前系统中所有活跃的话题的指令

rostopic list

查看指定话题中消息包发送频率的指令

rostopic hz /话题名称

查看指定话题中发送消息包内容的指令

rostopic echo /话题名称

将unicode字符转化为中文的指令

echo -e “Unicode命令”

Subscriber 订阅者的实现

ma_node.cpp

1. 创建atr_pkg软件包
2. 创建ma_node.cpp节点
3. 确定话题名称和消息类型
4. 在代码文件中include 和消息类型对应的头文件
5. 在main函数中通过NodeHandler大管家订阅一个话题并设置消息接收回调函数。
6. 定义一个回调函数，对接收到的消息包进行处理
7. main函数中需要执行ros:spinOnce()，让回调函数能够响应接收到的消息包。

#include 
#include

//系统自动调用
void chao_callback(std_msgs::String msg){
    ROS_INFO(msg.data.c_str());//将字符数组转化为string类型,并打印时间措
}

int main(int argc, char *argv[])
{
    /* code */
    setlocale(LC_ALL,"");//接受发布者的中文内容
    ros::init(argc,argv,"ma_node");

    ros::NodeHandle nh;//该对象是节点与ROS通讯的关键，理解为管家,通过管家发布一个话题并得到一个消息发送对象
    ros::Subscriber sub = nh.subscribe("connect",10,chao_callback);//pub理解为由管家发的手机,该函数是泛型函数，尖括号参数是消息类型，参数是话题名称和，消息缓存长度

    while(ros::ok()){
        ros::spinOnce();//调用该函数会注意到消息包的接收

    } 
    return 0;
}

缓存长度

图形化显示话题通讯关系工具

保持话题发布，然后在终端执行

rqt_graph

十二、使用launch文件启动多个节点

XML语法：

编写launch文件

在任意一个软件包（例如atr_pkg）中创建launch文件夹，新建launch文件

auto_run.launch

<launch>
	//roscode不需要描述，因为只要运行launch文件就会自动启动roscode
	<node pkg="ssr_pkg" type="yao_ndoe" name="yao_node"/>
	<node pkg="ssr_pkg" type="chao_ndoe" name="chao_node" launch-prefix="gnome-terminal -e"/>//表示使用一个新的终端去运行节点
	<node pkg="atr_pkg" type="ma_ndoe" name="ma_node" output="screen"/>//订阅者
</launch>

节点名称name是为了避免同名节点

运行launch文件

先删除注释，避免报错

roslaunch atr_pkg auto_run.launch

1. 使用launch文件，可以通过roslaunch指令一次启动多个节点
2. 在launch文件中，为节点添加 output=”screen”属性，可以让节点信息输出在终端中。(ROS WARN不受该属性控制)
3. 在launch文件中，为节点添加 launch-prefix=”gnome-terminal -e属性，可以让节点单独运行在一个独立终端中。

十三、发布者节点的应用：底盘运动控制

机器人的运动方向设定

速度消息包的内容

在index.ros.org网站，搜索软件包：geometry_msgs，找到ros版本为noetic，找到速度控制的消息类型：Twist ，包含两个数据成员linear（线性）和angular（角度），分别对应矢量速度和旋转速度

运动控制的实现

流程图，有两个节点，之间通过话题进行通信，速度话题的名称一般是command_velocity

首先，下载wpr_simulation源码包，使用教程可以查看上面

运行wpr_simulation分两个终端执行如下代码

roslaunch wpr_simulation wpb_simple.launch

rosrun wpr_simulation demo_vel_ctrl

编写运动控制案例

进入catkin_ws/src目录：

catkin_create_pkg vel_pkg rospy roscpp geometry_msgs

进入src目录创建vel_node.cpp

#include 
#include 

int main(int argc, char** argv)
{
  ros::init(argc, argv, "demo_vel_ctrl");

  ros::NodeHandle n;
  ros::Publisher vel_pub = n.advertise<geometry_msgs::Twist>("/cmd_vel", 10);

  geometry_msgs::Twist vel_msg;
  vel_msg.linear.x = 0.1;
  vel_msg.linear.y = 0.0;
  vel_msg.linear.z = 0.0;
  vel_msg.angular.x = 0;
  vel_msg.angular.y = 0;
  vel_msg.angular.z = 0;

  ros::Rate r(30);
  while(ros::ok())
  {
    vel_pub.publish(vel_msg);
    r.sleep();
  }

  return 0;
}

修改CMakeLists.txt

add_executable(vel_node src/vel_node.cpp)
add_dependencies( vel_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
target_link_libraries(vel_node 
  ${catkin_LIBRARIES}
)

编译：
运行：

roslaunch wpr_simulation wpb_simple.launch

rosrun vel_pkg vel_node

十四、激光雷达

• 按照测量维度可以分为单线雷达和多线雷达
• 按照测量原理可以分为三角测距雷达和TOF雷达
• 按照工作方式可以分为机械旋转雷达和固态雷达

十五、使用Rviz观测传感器数据

Rviz是可视化工具，方便对机器人状态实时观测的辅助工具

执行仿真工具

roslaunch wpr_simulation wpb_simple.launch

拆分终端输入

rviz

修改fixed frame为base_footprint，然后点击左侧add添加机器人模型

添加激光雷达显示条

修改激光雷达的话题名称和显示点大小

rviz仿真工具与gazebo可视化工具的关系

保存可视化工具的配置
手动：File->save config as->在主目录下保存lidar.rivz，下次使用要手动点open config
自动：在launch中加载rviz配置文件

• 运行：roslaunch wpr_simulation wpr_rviz.launch

十六、激光雷达消息包数据格式

传感器数据包：sensor_msgs，找到消息类型：LaserScan

在运行仿真和可视化工具的前提下，显示scan话题里面的消息

rostopic echo /scan --noarr #把数组折叠起来

参数解释

起始角度和终止角度【对ranges数组有用】

测距的旋转角度和时间差【用于修正高速运动的机器雷达测距点阵的形变】

两次扫描时间差为雷达转一圈所耗费的时间，

ranges数组

• 每旋转一度就进行一次测距行为，数组的第一个成员是起始点
  

intensities表示信号的强度，强度越强，数据值有效性越高

十七、订阅者节点的应用：获取激光雷达数据

获取激光雷达数据的实现

分两个终端执行如下代码

roslaunch wpr_simulation wpb_simple.launch

rosrun wpr_simulation demo_lidar_data

流程图

编写获取激光雷达数据的案例

进入catkin_ws/src目录：

catkin_create_pkg lidar_pkg rospy roscpp sensor_msgs

进入src目录创建lidar_node.cpp

#include 
#include 
#include 

void LidarCallback(const sensor_msgs::LaserScan msg)
{
    int nNum = msg.ranges.size();
    
    int nMid = nNum/2;//180°为正前方
    float fMidDist = msg.ranges[nMid];
    ROS_INFO("前方测距 ranges[%d] = %f 米", nMid, fMidDist); 
}

int main(int argc, char** argv)
{
    setlocale(LC_ALL,"");//设置中文编码
    ros::init(argc,argv,"demo_lidar_data");
    
    ros::NodeHandle n;
    ros::Subscriber lidar_sub = n.subscribe("/scan", 10, &LidarCallback);

    ros::spin();//保持节点运行不退出
}

修改CMakeLists.txt

add_executable(lidar_node src/lidar_node.cpp)
target_link_libraries(lidar_node 
  ${catkin_LIBRARIES}
)

编译
运行：

roslaunch wpr_simulation wpb_simple.launch

rosrun lidar_pkg lidar_node

十八、节点发布+订阅的应用：激光雷达避障

修改lidar_node.cpp

#include 
#include 
#include 
#include 

ros::Publisher vel_pub;
static int nCount = 0;

void LidarCallback(const sensor_msgs::LaserScan msg)
{
    int nNum = msg.ranges.size();
    
    int nMid = nNum/2;
    float fMidDist = msg.ranges[nMid];
    ROS_INFO("前方测距 ranges[%d] = %f 米", nMid, fMidDist); 

    if(nCount > 0)
    {
        nCount--;
        return;
    }

    geometry_msgs::Twist vel_cmd;
    if(fMidDist < 1.5f)
    {
        vel_cmd.angular.z = 0.3;
        nCount = 50;
    }
    else
    {
        vel_cmd.linear.x = 0.05;
    }
    vel_pub.publish(vel_cmd);
}

int main(int argc, char** argv)
{
    setlocale(LC_ALL,"");
    ros::init(argc,argv,"demo_lidar_behavior");
    
    ros::NodeHandle n;
    ros::Subscriber lidar_sub = n.subscribe("/scan", 10, &LidarCallback);
    vel_pub = n.advertise<geometry_msgs::Twist>("/cmd_vel",10);

    ros::spin();
}

编译
运行

roslaunch wpr_simulation wpb_simple.launch

rosrun lidar_pkg lidar_node

十九、IMU惯性测量单元消息包

六轴IMU消息包格式

orientation：融合姿态，对角速度和加速度进行融合换算得到
angular_velocity：角速度
liner_acceleration：线性加速度

四元数描述法

欧拉旋转

• 优点：

  • 很容易理解，形象直观；表示更方便，只需要3个值（分别对应x、y、z轴的旋转角度）；但按我的理解，它还是转换到了3个3*3的矩阵做变换，效率不如四元数；

• 缺点：

  • 之前提到过这种方法是要按照一个固定的坐标轴的顺序旋转的，因此不同的顺序会造成不同的结果；

  • 会造成万向节锁（Gimbal Lock）的现象。这种现象的发生就是由于上述固定坐标轴旋转顺序造成的。理论上，欧拉旋转可以靠这种顺序让一个物体指到任何一个想要的方向，但如果在旋转中不幸让某些坐标轴重合了就会发生万向 节锁，这时就会丢失一个方向上的旋转能力，也就是说在这种状态下我们无论怎么旋转（当然还是要原先的顺序）都不可能得到某些想要的旋转效果，除非我们打破原先的旋转顺序或者同时旋转3个坐标轴。

  • 由于万向节锁的存在，欧拉旋转无法实现球面平滑插值；

四元数旋转

• 优点：

  • 可以避免万向节锁现象；

  • 只需要一个4维的四元数就可以执行绕任意过原点的向量的旋转，方便快捷，在某些实现下比旋转矩阵效率更高；

  • 可以提供平滑插值；

• 缺点：

  • 比欧拉旋转稍微复杂了一点点，因为多了一个维度；
  • 理解更困难，不直观；

万向节锁

• 其实就是一个物体在一个3D世界里面随着旋转顺序和旋转角度的改变，导致物体只能在一个固定的平面旋转，无法旋转到你预想的角度。. 由于物体的旋转，物体的坐标轴方向也发生了改变，导致其中2条坐标轴发生了 重合 。

• 

• 视频解读

九轴IMU消息包格式

二十、订阅者节点应用：获取IMU数据的节点

• 消息话题名有三个，一般订阅的是第二个话题

获取IMU数据的实现

流程图

实现步骤

1. 构建一个新的软件包，包名叫做imu_pkg
2. 在软件包中新建一个节点，节点名叫做imu_node。
3. 在节点中，向ROS大管家NodeHandle申请订阅话题/imu/data并设置回调函数为IMUCallback0)。
4. 构建回调函数IMUCallback() ，用来接收和处理IMU数据
5. 使用TF工具将四元数转换成欧拉角。
6. 调用ROS_INFOO显示转换后的欧拉角数值

编写获取IMU数据的案例

进入catkin_ws/src目录：

catkin_create_pkg imu_pkg rospy roscpp sensor_msgs

进入src目录创建imu_node.cpp

#include 
#include 
#include //可将四元数转化为欧拉角

void IMUCallback(const sensor_msgs::LaserScan msg)
{
	// 检测消息包中四元数数据是否存在
    if(msg.orientation_covariance[0] < 0)
        return;
    // 四元数转成欧拉角
    tf::Quaternion quaternion(
        msg.orientation.x,
        msg.orientation.y,
        msg.orientation.z,
        msg.orientation.w
    );
    //定义三个double变量，装载欧拉角结果
    double roll, pitch, yaw;
    tf::Matrix3x3(quaternion).getRPY(roll, pitch, yaw);
    // 弧度换算成角度
    roll = roll*180/M_PI;
    pitch = pitch*180/M_PI;
    yaw = yaw*180/M_PI;
    ROS_INFO("滚转= %.0f 俯仰= %.0f 朝向= %.0f", roll, pitch, yaw);
}

int main(int argc, char** argv)
{
    setlocale(LC_ALL,"");//设置中文编码
    ros::init(argc,argv,"imu_node");
    
    ros::NodeHandle n;
    ros::Subscriber lidar_sub = n.subscribe("/imu/data", 10, &IMUCallback);

    ros::spin();//保持节点运行不退出
    return  0;
    
}

修改CMakeLists.txt

add_executable(imu_nodesrc/imu_node.cpp)
add_dependencies( imu_node${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
target_link_libraries(imu_node 
  ${catkin_LIBRARIES}
)

编译：
运行：

roslaunch wpr_simulation wpb_simple.launch
rosrun imu_pkg imu_node

二十一、节点发布+订阅的应用：IMU航向锁定

IMU航向锁定的实现

实现步骤：

1. 让大管家 NodeHandle 发布速度控制话题 /cmd_vel
2. 设定一个目标朝向角，当姿态信息中的朝向角和目标朝向角不致时，控制机器人转向目标向角。

编写IMU航向锁定案例

修改imu_node.cpp

#include "ros/ros.h"
#include "sensor_msgs/Imu.h"
#include "tf/tf.h"
#include "geometry_msgs/Twist.h"

// 速度消息发布对象（全局变量）
ros::Publisher vel_pub;

// IMU 回调函数
void IMUCallback(const sensor_msgs::Imu msg)
{
    // 检测消息包中四元数数据是否存在
    if(msg.orientation_covariance[0] < 0)
        return;
    // 四元数转成欧拉角
    tf::Quaternion quaternion(
        msg.orientation.x,
        msg.orientation.y,
        msg.orientation.z,
        msg.orientation.w
    );
    double roll, pitch, yaw;
    tf::Matrix3x3(quaternion).getRPY(roll, pitch, yaw);
    // 弧度换算成角度
    roll = roll*180/M_PI;
    pitch = pitch*180/M_PI;
    yaw = yaw*180/M_PI;
    ROS_INFO("滚转= %.0f 俯仰= %.0f 朝向= %.0f", roll, pitch, yaw);
    
    // 速度消息包
    geometry_msgs::Twist vel_cmd;
    // 目标朝向角
    double target_yaw = 90;
    // 计算速度
    double diff_angle = target_yaw - yaw;
    vel_cmd.angular.z = diff_angle * 0.01;
    vel_cmd.linear.x = 0.1;
    vel_pub.publish(vel_cmd);
}

int main(int argc, char **argv)
{
    setlocale(LC_ALL, "");
    ros::init(argc,argv, "demo_imu_behavior"); 

    ros::NodeHandle n;
    // 订阅 IMU 的数据话题
    ros::Subscriber sub = n.subscribe("imu/data", 100, IMUCallback);
    // 发布速度控制话题
    vel_pub = n.advertise<geometry_msgs::Twist>("/cmd_vel",10);
    ros::spin();

    return 0;
}

编译：
运行：

roslaunch wpr_simulation wpb_simple.launch
rosrun imu_pkg imu_node

二十二、ROS消息包

ROS消息包分为

• std_msgs：基础数值类型
• common_msgs：常规消息包，与应用密切相关

ROS标准消息包std_msgs

• Empty不携带数据，但可以把消息包当做一个信号来用

ROS常规消息包common_msgs

geometry_msgs几何消息包

sensor_msgs传感器消息包

自定义消息类型

生成自定义消息的步骤

1. 创建新软件包xx_msgs，依赖项roscpp、 rospy、 std_msgs 、message_generation、message_runtime【消息包也是软件包，以msg结尾来区分】
2. 软件包添加msg目录，新建自定义消息文件，以.msg结尾。
3. 在CMakeLists.txt中，将新建的.msg文件加入add_message_files()
4. 在CMakeLists.txt中，去掉generate_messages()注释符号，将依赖的其他消息包名称添加进去。
5. 在CMakeLists.txt中，将 message_runtime 加入 catkin_package()的CATKIN DEPENDS。
6. 在package.xml中，确认message_generation、message_runtime都加入依赖项中和中
7. 编译软件包，生成新的自定义消息类型

消息包格式

流程：

catkin_create_pkg new_msgs roscpp rospy std_msgs message_generation message_runtime

软件包添加msg目录

新建自定义消息文件new.msg

new.msg文件

//定义消息类型
string name
int64 age
string data

如图所示：

修改CMakeLists.txt

## Generate messages in the 'msg' folder
add_message_files(
  FILES
  new.msg
)

新的消息类型需要依赖的其他消息包列表，string是std_msgs中的数据类型

## Generate added messages and services with any dependencies listed here
generate_messages(
  DEPENDENCIES
  std_msgs
)

依赖的软件包能够在启动的时候使用新定义的消息类型

catkin_package(
#  INCLUDE_DIRS include
#  LIBRARIES qq_msg
 CATKIN_DEPENDS message_generation message_runtime roscpp rospy std_msgs
#  DEPENDS system_lib
)

在package.xml中，确认message_generation、message_runtime都加入依赖项中和中

查看新的消息类型是否进入消息列表

rosmsg show new_msgs/new

消息包名称/消息类型

自定义类型在C++节点中的应用

1. 在节点代码中，先include新消息类型的头文件
2. 在发布或订阅话题的时候，将话题中的消息类型设置为新的消2息类型o
3. 按照新的消息结构，对消息包进行赋值发送或读取解析
4. 在CMakeList.txt文件的find_package()中，添加新消息包名称作为依赖项。
5. 在节点的编译规则中，添加一条add dependencies0，将新消息软件包名称_generate messages_cpp作为依赖项。
6. 在package.xml中，将新消息包添加到和中夫
7. 运行 catkin make 重新编译

修改发布者节点的消息发布类型

chao_node.cpp

#include 
#include 
#include 

int main(int argc, char *argv[])
{
    /* code */
    ros::init(argc,argv,"chao_node");

    ros::NodeHandle nh;//该对象是节点与ROS通讯的关键，理解为管家,通过管家发布一个话题并得到一个消息发送对象
    ros::Publisher pub = nh.advertise<new_msgs::new>("connect",10);//pub理解为由管家发的手机,该函数是泛型函数，尖括号参数是消息类型，参数是话题名称和，消息缓存长度

    ros::Rate loop_rate(10);//生成频率对象，括号内是每秒执行次数，即为10hz

    while(ros::ok()){
        printf("wait for connect!\n");
        new_msgs::new msg;
        msg.name = "woshishui";
        msg.age = 18;
        msg.data = "we can hhhhhh !!!!";
        pub.publish(msg);
        loop_rate.sleep();//短时间阻塞

    } 
    return 0;
}

修改chao_node的编译规则

使其先去编译新的消息包，然后再编译节点

find_package(catkin REQUIRED COMPONENTS
  roscpp
  rospy
  std_msgs
  new_msgs
) 

添加节点编译的依赖项

add_dependencies(chao_node new_msgs_generate_messages_cpp)

修改package.xml

• 在package.xml中，确认new_msgs都加入依赖项中和中

修改订阅者节点的消息接收类型

ma_node.cpp

#include 
#include
#include
//系统自动调用
void chao_callback(new_msgs::new msg){
	ROS_WARN(msg.name.c_str());//将字符数组转化为string类型,并打印时间措
	ROS_INFO("年龄为：%d",msg.age);
    ROS_INFO(msg.data.c_str());//将字符数组转化为string类型,并打印时间措
}

int main(int argc, char *argv[])
{
    /* code */
    setlocale(LC_ALL,"");//接受发布者的中文内容
    ros::init(argc,argv,"ma_node");

    ros::NodeHandle nh;//该对象是节点与ROS通讯的关键，理解为管家,通过管家发布一个话题并得到一个消息发送对象
    ros::Subscriber sub = nh.subscribe("connect",10,chao_callback);//pub理解为由管家发的手机,该函数是泛型函数，尖括号参数是消息类型，参数是话题名称和，消息缓存长度

    while(ros::ok()){
        ros::spinOnce();//调用该函数会注意到消息包的接收

    } 
    return 0;
}

修改ma_node的编译规则

find_package(catkin REQUIRED COMPONENTS
  roscpp
  rospy
  std_msgs
  new_msgs
) 

添加节点编译的依赖项

add_dependencies(chao_node new_msgs_generate_messages_cpp)

修改package.xml

• 在package.xml中，确认new_msgs都加入依赖项中和中

二十三、ROS中栅格地图格式

• 发布节点是：map_server
• 发布的数据包是：OccupancyGrid.msg
• 发布的话题名是：/map

栅格尺寸 = 栅格边长=地图分辨率，体现地图的精细程度，ros中默认是0.05米

OccupancyGrid.msg数据包的描述

• 包含三个成员
• 第一个数据类型是另一个数据包Header，包含frame_id和stamp
• 第三个数据类型是数组
  
• 第二个数据类型是另一个数据包
  

二十四、发布者节点应用：发布地图数据

发布地图数据的实现

确定地图数据的规划

实现步骤：

1. 构建一个软件包map_pkg，依赖项里加上nav_msgs
2. 在map_pkg里创建一个节点map_pub_node2
3. 在节点中发布话题/map，消息类型为nav_msgs::OccupancyGridY
4. 构建一个nav_msgs::OccupancyGrid地图消息包，并对其进行赋值
5. 将地图消息包发送到话题/map。
6. 编译并运行节点
7. 启动RViz，订阅话题/map，显示地图

编写发布地图数据案例

编写map_pub_node.cpp

# include 
# include 
# include 

int main(int argc, char** argv)
{
    ros::init(argc, argv, "demo_map_pub");

    ros::NodeHandle n;
    ros::Publisher pub = n.advertise<nav_msgs::OccupancyGrid>("/map", 10);

    ros::Rate r(1);
    while (ros::ok())
    {
        nav_msgs::OccupancyGrid msg;
        // header
        msg.header.frame_id = "map";
        msg.header.stamp = ros::Time::now();
        // 地图描述信息
        msg.info.origin.position.x = 0;
        msg.info.origin.position.y = 0;
        msg.info.resolution = 1.0;
        msg.info.width = 4;
        msg.info.height = 2;
        // 地图数据
        msg.data.resize(4*2);
        msg.data[0] = 100;
        msg.data[1] = 100;
        msg.data[2] = 0;
        msg.data[3] = -1;
        // 发送
        pub.publish(msg);
        r.sleep();
    }
    
    return 0;
}

修改CMakeLists.txt

add_executable(map_pub_node src/map_pub_node.cpp)

target_link_libraries(map_pub_node
  ${catkin_LIBRARIES}
)

编译

终端运行三个语句

roscore
rosrun map_pkg map_pub_node 
rviz

添加坐标系和地图标识，修改话题名称为/map，结果如图所示

二十五、slam

• 栅格地图是通过slam生成的
• slam主要是：定位与建图
• 参照物

参考视频：机器人操作系统 ROS 快速入门教程