大家好呀，我是一个SLAM方向的在读博士，深知SLAM学习过程一路走来的坎坷，也十分感谢各位大佬的优质文章和源码。随着知识的越来越多，越来越细，我准备整理一个自己的激光SLAM学习笔记专栏，从0带大家快速上手激光SLAM，也方便想入门SLAM的同学和小白学习参考，相信看完会有一定的收获。如有不对的地方欢迎指出，欢迎各位大佬交流讨论，一起进步。

1.3 利用ROS编译C++

2、ROS中自定义文件

2.1 自定义CMakeLists.txt编译文件

2.2 自定义Package.xml依赖文件

在学习SLAM之间，我们可以不懂理论，可以不懂代码，但我们一定得会编译是不是（狗头），直接跑大佬们的代码爽到飞好不好，这节将讲述如何编译。

这节所用到的代码在：[email protected]:huashu996/lidar_slam_course.git

1、如何编译C++

在进行ROS编程前，首先学会如何编译C++，这里借鉴了高翔大佬的教程。在Ubuntu中C++的编译方式有如下几种。

1.1 g++编译

代码在/lidar_slam_course/ch1/c++compile/g++

1. 编写一个 C++程序 helloSLAM.cpp

#include <iostream>
using namespace std;
int main(int argc, char **argv) {
cout << "Hello SLAM!" << endl;
return 0;
}

2. 安装 g++编译器编译文件

Sudo apt-get install g++
g++ helloSLAM.cpp

3. 运行可执行文件

编译完成后会生成.out文件，我们在终端运行它。

./out
Hello SLAM！

1.2 cmake编译

代码在/lidar_slam_course/ch1/c++compile/cmake

1.为了引用头文件编译，先自己建立库文件

建立库文件 libHelloSLAM.cpp,库文件中只有函数定义。

#include <iostream>
using namespace std;
void printHello() {
cout << "Hello SLAM" << endl;
}

2.建立头文件 libHelloSLAM.h，头文件是函数声明

#ifndef LIBHELLOSLAM_H_
#define LIBHELLOSLAM_H_
// 上面的宏定义是为了防止重复引用这个头文件而引起的重定义错误
// 打印一句 hello 的函数
void printHello();

3.建立编译文件 useHello.cpp,调用头文件中的函数

#include "libHelloSLAM.h"
// 使用 libHelloSLAM.h 中的 printHello() 函数
int main(int argc, char **argv) {
printHello();
return 0;
}

4. 建立 CMakeLists.txt 文件

# 声明要求的 cmake 最低版本
cmake_minimum_required(VERSION 2.8)
# 声明一个 cmake 工程
project(HelloSLAM)
# 设置编译模式
set(CMAKE_BUILD_TYPE "Debug")
# 添加 hello 库
add_library(hello libHelloSLAM.cpp)
# 共享库
add_library(hello_shared SHARED libHelloSLAM.cpp)# 添加可执行程序调用 hello 库中函数
add_executable(useHello useHello.cpp)
# 将库文件链接到可执行程序上
target_link_libraries(useHello hello_shared)

5. 建立build文件夹并编译

将编译中间的过程文件放在这里，会生成helloSLAM可执行文件。

mkdir build
cd build
cmake ..
make
./helloSLAM

1.3 利用ROS编译C++

1.建立工作空间

建立如下图所示结构的工作空间 ros/src/helloslam

可以用如下命令在工作空间下初始化一个名为 fun_bag 的功能包，包含
初始的CMakeLists.txt 文件和 package.xml 文件

catkin_create_pkg fun_bag pcl_conversions pcl_ros pcl_msgs sensor_msgs
roscd fun_bag
mkdir src

2.修改 CMakeLists.txt 文件和 package.xml 文件

CMakeLists.txt

# 声明要求的 cmake 最低版本
cmake_minimum_required(VERSION 2.8)


project(hello)
find_package(catkin REQUIRED COMPONENTS roscpp cv_bridge image_transport sensor_msgs roscpp rospy std_msgs)
find_package(OpenCV REQUIRED)
catkin_package(
#  INCLUDE_DIRS include
#  LIBRARIES my_image_transport
#  CATKIN_DEPENDS cv_bridge image_transport sensor_msgs
#  DEPENDS system_lib
)
# 添加hello库
add_library(hello libHelloSLAM.cpp)
# 共享库
add_library(hello_shared SHARED libHelloSLAM.cpp)

# 添加可执行程序调用hello库中函数
add_executable(useHello useHello.cpp)
# 将库文件链接到可执行程序上
target_link_libraries(useHello hello_shared)

package.xml

<?xml version="1.0"?>
<package format="2">
  <name>hello</name>
  <version>0.0.0</version>
  <description>The camera_driver package</description>
  <maintainer email="[email protected]">wendao</maintainer>  <!-- One license tag required, multiple allowed, one license per tag -->
  <license>TODO</license>  <!-- Url tags are optional, but multiple are allowed, one per tag -->
  <buildtool_depend>catkin</buildtool_depend>
  <build_depend>cv_bridge</build_depend>
  <build_depend>image_transport</build_depend>
  
  <build_depend>sensor_msgs</build_depend>
  <build_export_depend>cv_bridge</build_export_depend>
  <build_export_depend>image_transport</build_export_depend>
  
  <build_export_depend>sensor_msgs</build_export_depend>
  <exec_depend>cv_bridge</exec_depend>
  <exec_depend>image_transport</exec_depend>
  
  <exec_depend>sensor_msgs</exec_depend>
  
  <!-- The export tag contains other, unspecified, tags -->
  <export>
    <!-- Other tools can request additional information be placed here -->

  </export>
<build_depend>roscpp</build_depend>
<build_depend>rospy</build_depend>
<build_depend>std_msgs</build_depend>
<build_export_depend>roscpp</build_export_depend>
<build_export_depend>rospy</build_export_depend>
<build_export_depend>std_msgs</build_export_depend>
<exec_depend>roscpp</exec_depend>
<exec_depend>rospy</exec_depend>
<exec_depend>std_msgs</exec_depend>
</package>

如果 CMakeLists.txt 文件中没有 catkin_package()则会出现编译没问题，但 rosrun
找不到可执行文件。

3.添加代码

这里直接将刚才cmake方式建立的代码拷贝到这里

4.编译执行

catkin_make
source/devel/setup.bash
（在运行 rosrun 前需要新开终端运行 roscore）
rosrun hello useHello
(hello 为 CMakelists 文件中的 project 名字，不是文件夹的名字)

利用ROS编译C++是在SLAM工程中最常用到的方式，下面将详细介绍每个文件到底怎么配置和自定义。

2、ROS中自定义文件

首先来看这些文件中都有什么

2.1 自定义CMakeLists.txt编译文件

1.头文件

cmake_minimum_required(VERSION 2.8.3) #catkin需要的版本
project(lio_sam)  #功能包的名字

2.set编译模式

set(CMAKE_BUILD_TYPE "Debug") #设置构建类型为 Debug。这将指示 CMake 生成编译器的调试信息，同时禁用某些优化。Debug 模式下，程序会生成调试信息，并且不会进行优化，以方便程序员在调试过程中追踪代码和变量的状态。同时，Debug 模式可能会启用其他特定于调试的功能，例如对空指针的检查和断言的启用。Release 模式下，程序会进行优化以提高性能，并且不会生成调试信息。这样可以减小程序的体积和运行时开销，但也使得调试程序更加困难。
set(CMAKE_CXX_FLAGS "-std=c++14") #用于设置编译器选项。这将向编译器传递 -std=c++14 标志，表示使用 C++14 标准进行编译。
set(CMAKE_CXX_FLAGS_DEBUG "-O3 -Wall -g -pthread") #用于设置调试模式下的编译器选项。这将向编译器传递 -O3（启用高级优化级别）、-Wall（启用所有警告）、-g（生成调试信息）和 -pthread（启用线程支持）等选项。
set(CMAKE_PREFIX_PATH "/usr/local/include/eigen3/") #设置CMAKE_PREFIX_PATH可以帮助CMake找到特定的库，即使它没有安装在标准的搜索路径中，也可以让CMake找到它。一般默认路径是usr/local/include。如果不是这个路径就需要set让cmake找到。

3. 检查C++标准

这段代码的作用是检查编译器是否支持 C++11 标准，如果支持则使用 -std=c++11 标志设置编译器标准，否则检查编译器是否支持 C++0x 标准，如果支持则使用 -std=c++0x 标志设置编译器标准，如果都不支持，则输出错误信息。这段代码首先通过 INCLUDE(CheckCXXCompilerFlag) 命令引入了 CheckCXXCompilerFlag 模块，该模块提供了 CHECK_CXX_COMPILER_FLAG 命令，用于检查编译器是否支持指定的标志。接着，CHECK_CXX_COMPILER_FLAG 命令分别检查编译器是否支持 -std=c++11 和 -std=c++0x 标志，并将检查结果存储在 COMPILER_SUPPORTS_CXX11 和 COMPILER_SUPPORTS_CXX0X 变量中。

INCLUDE(CheckCXXCompilerFlag)
CHECK_CXX_COMPILER_FLAG("-std=c++11" COMPILER_SUPPORTS_CXX11)
CHECK_CXX_COMPILER_FLAG("-std=c++0x" COMPILER_SUPPORTS_CXX0X)
IF(COMPILER_SUPPORTS_CXX11)
SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11")
ELSEIF(COMPILER_SUPPORTS_CXX0X)
SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++0x")
ELSE()
  MESSAGE(ERROR "The compiler ${CMAKE_CXX_COMPILER} has no C++11 support. Please use a different C++ compiler.")
ENDIF()

4. catkin_package

catkin_package() 是用来定义一个 Catkin 包的基本信息，例如包的名称、版本号、作者、描述等等。这个命令一般放在 CMakeLists.txt 文件的最顶部，用于告诉 Catkin 构建系统有关于这个包的基本信息。

catkin_package( 
INCLUDE_DIRS include #这个包的头文件目录为 include，也就是说这个包的头文件应该放在 include 目录下。
DEPENDS PCL GTSAM #这个包依赖了 PCL 和 GTSAM 两个第三方库，也就是说在编译这个包之前需要先安装这两个库。
CATKIN_DEPENDS #这个包依赖了以下其他 Catkin 包
std_msgs 
nav_msgs 
geometry_msgs 
sensor_msgs 
message_runtime 
message_generation 
visualization_msgs)

5. find_package

用于查找和配置所需的依赖项，并将其链接到ROS节点或库中。对于ROS自带的功能包一次加入多个依赖，使用REQUIRED COMPONENTS：

find_package(catkin REQUIRED COMPONENTS ...)指令会自动包含所有指定的ROS catkin依赖项的头文件路径，因此通常不需要显式地在include_directories中添加这些路径。catkin_INCLUDE_DIRS变量包含了使用find_package(catkin REQUIRED COMPONENTS ...)指令找到的所有ROS catkin依赖项的头文件路径。

find_package(catkin REQUIRED COMPONENTS
   roscpp
   rospy
   std_msgs
   sensor_msgs
   geometry_msgs
   tf
   rostime 
   message_filters 
   message_generation
   cv_bridge
   image_transport 
   compressed_image_transport 
   compressed_depth_image_transport 
)

虽然catkin_INCLUDE_DIRS变量包含了所有使用find_package(catkin REQUIRED COMPONENTS ...)指令找到的ROS catkin依赖项的头文件路径，但并不包括其他非catkin的第三方库（例如OpenCV和PCL）的头文件路径。这些库的头文件路径需要使用find_package指令找到并存储在相应的变量中。

find_package(catkin REQUIRED) 
find_package(OpenCV REQUIRED) 
find_package(OpenMP) 
find_package(PCL REQUIRED)

某些第三方库（例如OpenCV和PCL）不属于catkin依赖项，因此它们的头文件和库文件路径不会自动包含在catkin构建系统中。因此，需要使用find_package指令来查找这些库，并指定它们的头文件和库文件路径，以便在ROS节点或库中正确链接它们。

例如find_package(OpenCV)指令将结果存储在OpenCV_INCLUDE_DIRS和OpenCV_LIBRARIES两个变量中，其中前者包含头文件路径，后者包含库文件路径。头文件是在编译时候看能否正确定位代码，库文件是代码运行时调用代码。

#添加头文件
include_directories(include
${OpenCV_INCLUDE_DIRS}
${PCL_INCLUDE_DIRS}
)
#添加库文件
link_directories(include
${PCL_LIBRARY_DIRS}
${OpenCV_LIBRARY_DIRS}
)

6.构建可执行文件并连接到库

add_executable(pointcloud_mapping  src/pointcloud_mapping.cpp  src/PointCloudMapper.cc)

使用add_executable()函数将两个源文件 src/pointcloud_mapping.cpp 和 src/PointCloudMapper.cc 编译为一个可执行文件 pointcloud_mapping，其中pointcloud_mapping为可执行文件的名字，并不代表ros节点的名字，但如果用rosrun运行时候则默认生成一个名字为pointcloud_mapping的节点。如果想自定义节点名字使用如下命令。rosrun package_name pointcloud_mapping --name my_mapping_node

target_include_directories(pointcloud_mapping
${OpenCV_INCLUDE_DIRS}
${PCL_INCLUDE_DIRS}
)

target_link_libraries(pointcloud_mapping 
${catkin_LIBRARIES} 
${OpenCV_LIBRARIES} 
${PCL_LIBRARIES} )

使用 target_link_libraries() 函数将编译后的可执行文件链接到所需的库，target_include_directories()链接头文件。可以直接使用这两个函数，不使用link_directories和include_directories以使每个代码精确的添加第三方库。

有时候还会用到add_dependencies用于添加依赖关系，即告诉 CMake，当构建一个目标时，需要先构建指定的依赖目标。

7. install

这个指令用于将生成的目标文件安装到指定的位置。在这里，我们将 "pointcloud_mapping" 可执行文件安装到了 ${CATKIN_PACKAGE_BIN_DESTINATION} 目录下，该目录通常是 ROS 程序包中的 "bin" 目录。

 install(TARGETS pointcloud_mapping
#   ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
#   LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
   RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
 )

8. 添加自定义消息与服务

当需要添加额外的消息与服务时候，需要将这几行的注释取消，并且加上消息与服务的文件名字
add_message_files(
  DIRECTORY msg
  FILES
  cloud_info.msg
)

add_service_files(
  DIRECTORY srv
  FILES
  save_map.srv
)

generate_messages(
  DEPENDENCIES
  geometry_msgs
  std_msgs
  nav_msgs
  sensor_msgs
)

2.2 自定义Package.xml依赖文件

1.基本信息

  <name>lio_sam</name>
  <version>1.0.0</version>
  <description>Lidar Odometry</description>
  <maintainer email="[email protected]">Tixiao Shan</maintainer>
  <license>TODO</license>
  <author>Tixiao Shan</author>

2.编译依赖

<buildtool_depend>catkin</buildtool_depend>
  <build_depend>roscpp</build_depend>
  <build_depend>rospy</build_depend>
  <build_depend>tf</build_depend>
  <build_depend>cv_bridge</build_depend>
  <build_depend>pcl_conversions</build_depend>
  <build_depend>std_msgs</build_depend>
  <build_depend>sensor_msgs</build_depend>
  <build_depend>geometry_msgs</build_depend>
  <build_depend>nav_msgs</build_depend>
  <build_depend>visualization_msgs</build_depend>
  <build_depend>message_generation</build_depend>
  <build_depend>message_runtime</build_depend>
  <build_depend>GTSAM</build_depend>

3.运行依赖

  <run_depend>roscpp</run_depend>
  <run_depend>rospy</run_depend>
  <run_depend>tf</run_depend>
  <run_depend>cv_bridge</run_depend>
  <run_depend>pcl_conversions</run_depend>
  <run_depend>std_msgs</run_depend>
  <run_depend>sensor_msgs</run_depend>
  <run_depend>geometry_msgs</run_depend>
  <run_depend>nav_msgs</run_depend>
  <run_depend>visualization_msgs</run_depend>
  <run_depend>message_generation</run_depend>
  <run_depend>message_runtime</run_depend>
  <run_depend>GTSAM</run_depend>

2.3 自定义 launch启动文件

<node>	运行一个节点
<param>	设置一个参数在参数服务器上
<remap>	声明重映射ros计算图资源的命名
<machine>	声明用于启动时的机器
<rosparam>	加载文件中的多个参数
<include>	包含了其他的ros launch文件
<env>	为启动节点指定一个环境变量。
<test>	运行一个测试节点，详见rostest
<arg>	声明一个参数
<group>	将共享名称空间或重新映射的封闭元素分组

1.定义功能包名字

  <arg name="project" default="lio_sam"/>

2.执行文件生成ROS节点

<node pkg="package_name" type="pointcloud_mapping" name="mapping_node" output=“log/screen” respawn=“true” args=“arg1 arg2 arg3” required=“true”/>
Node pkg---功能包名字
Type---可执行文件名字
Name---定义生成的ros节点名字
Output---如果选择了“screen”，则该节点的输出和错误都将发送到屏幕终端上现实。如果选择了“log”，则将输出和错误发送到$ROS_HOME/log下的log文件中，但错误也会继续发送到屏幕上
Respawn---如果设置为true，则当该节点退出时，重启节点，默认false
Args---将参数传递给节点
Required---设置为true时，如果该节点关闭，则关闭所有节点

3.参数服务器

--Param--
Launch文件中写的：
<node pkg="image_enhance" type="image_enhance_node" name="image_enhance_node" output="screen">
    <param name ="image_topic" value="/pub_t"/>
    <param name ="frame_rate" value="30"/>
    <param name ="mode" value="2"/>
</node>
Cpp文件中写的：
nh_private.param<std::string>("image_topic", image_topic_, "");
nh_private.param<int>("frame_rate",frame_rate_,30);
nh_private.param<int>("mode",mode_,0);

--rosparam标签--
<rosparam>标签允许使用rosparam YAML文件从ROS参数服务器加载和转储参数。它还可以用来删除参数。<rosparam>标签可以放在<node>标签中，在这种情况下，参数被视为私有的。
<rosparam file="$(find lio_sam)/config/params.yaml" command="load" />
File---引入文件路径
Command---load导入，dump存储，delete删除，默认为load。

4.消息重映射

<remap from="/different_topic" to="/needed_topic"/>
#改变topic的名字，以使更好的使用

5.启动多个launch

 <!--- LOAM -->
    <include file="$(find lio_sam)/launch/include/module_loam.launch" />
    <!--- Robot State TF -->
    <include file="$(find lio_sam)/launch/include/module_robot_state_publisher.launch" />
    <!--- Run Navsat -->
    <include file="$(find lio_sam)/launch/include/module_navsat.launch" />
    <!--- Run Rviz-->
    <include file="$(find lio_sam)/launch/include/module_rviz.launch" />

6.group

#标记使设置更容易应用于一组节点。它有一个ns属性，允许您将节点组推送到一个单独的命名空间中，ns给topic加了一个前缀更好区分。
<group ns="cam_rgb1">
  <node name="usb_cam" pkg="usb_cam" type="usb_cam_node" output="screen" >
    <param name="video_device" value="/dev/video2" />
    <param name="image_width" value="640" />
    <param name="image_height" value="480" />
    <param name="pixel_format" value="mjpeg" />
    <param name="camera_frame_id" value="cam_rgb" />
    <param name="camera_name" value="cam_rgb" />
    <param name="io_method" value="mmap"/>
  </node>
</group>
<group ns="cam_rgb2">
  <node name="usb_cam" pkg="usb_cam" type="usb_cam_node" output="screen" >
    <param name="video_device" value="/dev/video4" />
    <param name="image_width" value="640" />
    <param name="image_height" value="480" />
    <param name="pixel_format" value="mjpeg" />
    <param name="camera_frame_id" value="cam_rgb" />
    <param name="camera_name" value="cam_rgb" />
    <param name="io_method" value="mmap"/>
  </node>
</group>
#查看rostopiclist
/cam_rgb1/usb_cam/camera_info
/cam_rgb1/usb_cam/image_raw
/cam_rgb2/usb_cam/camera_info
/cam_rgb2/usb_cam/image_raw

2.4 自定义msg消息

以lio-sam为例，msg文件就是文本文件，用于描述ROS消息的字段。msg文件就是简单的文本文件，每行都有一个字段类型和字段名称。ROS中还有一个特殊的数据类型：Header，它含有时间戳和ROS中广泛使用的坐标帧信息。在msg文件的第一行经常可以看到Header header。

1.创建消息包

mkdir msg
cd msg
sudo gedit cloud_info.msg
# 创建Cloud Info消息
Header header 

int32[] startRingIndex
int32[] endRingIndex

int32[]  pointColInd # point column index in range image
float32[] pointRange # point range 

int64 imuAvailable
int64 odomAvailable

# Attitude for LOAM initialization
float32 imuRollInit
float32 imuPitchInit
float32 imuYawInit

# Initial guess from imu pre-integration
float32 initialGuessX
float32 initialGuessY
float32 initialGuessZ
float32 initialGuessRoll
float32 initialGuessPitch
float32 initialGuessYaw

# Point cloud messages
sensor_msgs/PointCloud2 cloud_deskewed  # original cloud deskewed
sensor_msgs/PointCloud2 cloud_corner    # extracted corner feature
sensor_msgs/PointCloud2 cloud_surface   # extracted surface feature

2.代码加消息头文件

#include "lio_sam/cloud_info.h
lio_sam::cloud_info cloudInfo; #创建名为cloudinfo的cloud_info类型的消息
# 定义消息初始值
        cloudInfo.startRingIndex.assign(N_SCAN, 0);
        cloudInfo.endRingIndex.assign(N_SCAN, 0);
        cloudInfo.pointColInd.assign(N_SCAN*Horizon_SCAN, 0);
        cloudInfo.pointRange.assign(N_SCAN*Horizon_SCAN, 0);

3.编译配置文件中加消息

1.确保package文件中以下代码：
<build_depend>message_generation</build_depend>
<run_depend>message_runtime</run_depend>

2.Cmakelists中添加消息
catkin_package(
  CATKIN_DEPENDS message_runtime ...
  ...)

find_package(catkin REQUIRED COMPONENTS
   roscpp
   rospy
   std_msgs
   message_generation
)

add_message_files(
  DIRECTORY msg
  FILES
  cloud_info.msg
)
generate_messages(
  DEPENDENCIES
  geometry_msgs
  std_msgs
  nav_msgs
  sensor_msgs
)

4.查看消息

$ rosmsg show [message name]
rosmsg show cloud_info/startRingIndex
rosmsg show startRingIndex  #也可以只写消息名称

2.5 自定义srv消息

一个srv文件描述一个服务。它由两部分组成：请求（request）和响应（response）。

1.创建消息

mkdir srv
cd srv
sudo gedit save_map.srv

2.添加编译

与添加msg类似，只需要把add_message换成add_service
add_service_files(
  FILES
  save_map.srv
)

3.代码添加头文件

#include "lio_sam/save_map.h"
ros::ServiceServer srvSaveMap;
srvSaveMap  = nh.advertiseService("lio_sam/save_map", &mapOptimization::saveMapService, this);

3、Topic话题编程

3.1 发布者

发布者和订阅者是ROS中最常用的功能，可以使不同传感器间数据进行处理。下面将给出如何用C++去写一个发布者，发布速率为10HZ。

#include <ros/ros.h>
#include <std_msgs/MessageType.h> // 替换为所需的消息类型

int main(int argc, char** argv)
{
  // 初始化ROS节点
  ros::init(argc, argv, "publisher_node");
  
  // 创建节点句柄
  ros::NodeHandle nh;
  
  // 创建一个发布者对象，指定要发布的消息类型和话题名称
  ros::Publisher pub = nh.advertise<std_msgs::MessageType>("topic_name", 10);
  
  // 设置循环频率为10Hz
  ros::Rate loop_rate(10);
  
  // 循环发布消息
  while (ros::ok())
  {
    // 创建要发布的消息对象
    std_msgs::MessageType msg;
    // 填充消息数据
    // 发布消息
    pub.publish(msg);
    // 等待直到下一个发布周期
    loop_rate.sleep();
  }
  return 0;
}

3.2 订阅者

#include <ros/ros.h>
#include <sensor_msgs/Image.h>

void imageCallback(const sensor_msgs::Image::ConstPtr& msg) {
  // 在这里处理接收到的图像消息
}
int main(int argc, char** argv) {
  ros::init(argc, argv, "image_subscriber");
  ros::NodeHandle nh;
  
  // 创建一个Subscriber对象来订阅/image话题
  ros::Subscriber sub = nh.subscribe<sensor_msgs::Image>("image", 100, imageCallback);

  ros::Rate loop_rate(100); // 设置循环的频率为100hz
  while (ros::ok()) {
    // 处理所有的回调函数
    ros::spinOnce();
    // 按照设定的频率睡眠一段时间
    loop_rate.sleep();
  }
  return 0;
}

在上面的代码中，我们首先初始化了ROS节点，并创建了一个名为“image_subscriber”的节点。然后，我们创建了一个Subscriber对象，订阅了名为“image”的话题，并指定了消息队列的大小为100。这意味着在没有及时处理消息的情况下，我们可以缓存最多100个未处理的消息。
接下来，我们设置了一个循环，其中我们调用ros::spinOnce()来处理所有的回调函数，并调用loop_rate.sleep()以使循环以指定的频率运行。这里，我们将频率设置为100hz，以匹配我们所需的速率。
当Subscriber订阅一个话题时，它会向ROS Master发送一个订阅请求，并创建一个缓存消息的队列。当订阅的话题有新的消息时，这些消息将被添加到队列的末尾。Subscriber会以先进先出（FIFO）的方式处理队列中的消息。如果队列已满并且有新消息到达，旧消息将被丢弃，以便为新消息腾出空间。

4、Service服务编程

4.1 服务器

#include "ros/ros.h"
#include "beginner_tutorials/AddTwoInts.h"
//定义服务
//这个函数提供了AddTwoInts服务，它接受srv文件中定义的请求（request）和响应（response）类型，并返回一个布尔值。
bool add(beginner_tutorials::AddTwoInts::Request  &req,
         beginner_tutorials::AddTwoInts::Response &res)
{
  res.sum = req.a + req.b;
  ROS_INFO("request: x=%ld, y=%ld", (long int)req.a, (long int)req.b);
  ROS_INFO("sending back response: [%ld]", (long int)res.sum);
  return true;
}
//主函数
int main(int argc, char **argv)
{
  //初始化ROS节点
ros::init(argc, argv, "add_two_ints_server");
  ros::NodeHandle n;
  //发布服务器
  ros::ServiceServer service = n.advertiseService("add_two_ints", add);
  ROS_INFO("Ready to add two ints.");
  ros::spin();

  return 0;
}

4.2 客户端

#include "ros/ros.h"
#include "beginner_tutorials/AddTwoInts.h"
#include <cstdlib>

int main(int argc, char **argv)
{
  ros::init(argc, argv, "add_two_ints_client");
  if (argc != 3)
  {
    ROS_INFO("usage: add_two_ints_client X Y");
    return 1;
  }

  ros::NodeHandle n;
  ros::ServiceClient client = n.serviceClient<beginner_tutorials::AddTwoInts>("add_two_ints");
//这里我们实例化一个自动生成的服务类，并为它的request成员赋值。一个服务类包括2个成员变量：request和response，以及2个类定义：Request和Response。
  beginner_tutorials::AddTwoInts srv;
  srv.request.a = atoll(argv[1]);
  srv.request.b = atoll(argv[2]);
  if (client.call(srv))
  {
    ROS_INFO("Sum: %ld", (long int)srv.response.sum);
  }
  else
  {
    ROS_ERROR("Failed to call service add_two_ints");
    return 1;
  }

  return 0;
}

5. LIO-SAM举例

5.1 目录介绍

关于此部分内容已经一步一步写的很详细，但在实际工程中我们还是不知道该怎么去写，博主将以LIO-SAM激光SLAM的源码来解释这些该怎么去用，首先下载LIO-SAM代码，也可以直接下载我整理的代码地址。

https://github.com/TixiaoShan/LIO-SAM.git

首先可以看到目录有哪些内容，config是参数配置文件，include是头文件，launch是启动文件，msg是自定义消息文件，src是主要通信编程文件，srv是服务编程文件，CMakeLists.txt和package.xml是编译配置文件，readme.md是对该功能包的简介。下面将介绍上述介绍的一堆自定义在实际工程中如何运用。

5.2 Launch

首先我们看一下Launch文件，launch文件能够快速的查看这个包中存在什么节点，有哪几块组成。首先打开一个总的launch文件，launch/run.launch可以看到里面包含了4个子launch文件，每个子launch文件启动一个小功能，比如有启动loam代码的有启动rviz的。

<launch>

    <arg name="project" default="lio_sam"/>
    
    <!-- Parameters -->
    <rosparam file="$(find lio_sam)/config/params.yaml" command="load" />

    <!--- LOAM -->
    <include file="$(find lio_sam)/launch/include/module_loam.launch" />

    <!--- Robot State TF -->
    <include file="$(find lio_sam)/launch/include/module_robot_state_publisher.launch" />

    <!--- Run Navsat -->
    <include file="$(find lio_sam)/launch/include/module_navsat.launch" />

    <!--- Run Rviz-->
    <include file="$(find lio_sam)/launch/include/module_rviz.launch" />

</launch>

接着在launch/include里面有4个子launch文件，我们打开module_loam.launch，可以看到这个launch文件包含了4个节点，_imuPreintegration，_imageProjection，_featureExtraction，_mapOptmization。而这些节点是通过四个可执行文件启动，那这4个可执行文件就是从CMakelists.txt编译而来，下面我们看一下LIO-SAM的CMakelists.txt文件。

<launch>

    <arg name="project" default="lio_sam"/>
    
    <node pkg="$(arg project)" type="$(arg project)_imuPreintegration"   name="$(arg project)_imuPreintegration"    output="screen" respawn="true"/>
    <node pkg="$(arg project)" type="$(arg project)_imageProjection"     name="$(arg project)_imageProjection"      output="screen"     respawn="true"/>
    <node pkg="$(arg project)" type="$(arg project)_featureExtraction"   name="$(arg project)_featureExtraction"    output="screen"     respawn="true"/>
    <node pkg="$(arg project)" type="$(arg project)_mapOptmization"      name="$(arg project)_mapOptmization"       output="screen"     respawn="true"/>
    
</launch>

5.3 CMakelists.txt

首先看到catkin_package看到这个包所依赖的所有库和消息。

catkin_package(
  INCLUDE_DIRS include
  DEPENDS PCL GTSAM

  CATKIN_DEPENDS
  std_msgs
  nav_msgs
  geometry_msgs
  sensor_msgs
  message_runtime
  message_generation
  visualization_msgs
)

接下来从find_package去找到这个包所需要的所有依赖和第三方库

cmake_minimum_required(VERSION 2.8.3)
project(lio_sam)

set(CMAKE_BUILD_TYPE "Debug")
set(CMAKE_CXX_FLAGS "-std=c++14")
set(CMAKE_CXX_FLAGS_DEBUG "-O3 -Wall -g -pthread")

find_package(catkin REQUIRED COMPONENTS
  tf
  roscpp
  rospy
  cv_bridge
  # pcl library
  pcl_conversions
  # msgs
  std_msgs
  sensor_msgs
  geometry_msgs
  nav_msgs
  message_generation
  visualization_msgs
)

find_package( GTSAMCMakeTools )
find_package(OpenMP REQUIRED)
find_package(PCL REQUIRED QUIET)
find_package(OpenCV REQUIRED QUIET)
find_package(GTSAM REQUIRED QUIET)

从add_message_files和add_service_files可以看出，此包自定义了一个cloud_info消息和一个save_map服务

add_message_files(
  DIRECTORY msg
  FILES
  cloud_info.msg
)

add_service_files(
  DIRECTORY srv
  FILES
  save_map.srv
)

generate_messages(
  DEPENDENCIES
  geometry_msgs
  std_msgs
  nav_msgs
  sensor_msgs
)

找到第三方库的头文件和库文件并与C++文件建立链接，生成可执行文件，这里生成的可执行文件正是Launch中调用的。

# include directories
include_directories(
	include
	${catkin_INCLUDE_DIRS}
	${PCL_INCLUDE_DIRS}
  ${OpenCV_INCLUDE_DIRS}
	${GTSAM_INCLUDE_DIR}
)

# link directories
link_directories(
	include
	${PCL_LIBRARY_DIRS}
  ${OpenCV_LIBRARY_DIRS}
  ${GTSAM_LIBRARY_DIRS}
)

###########
## Build ##
###########

# Range Image Projection
add_executable(${PROJECT_NAME}_imageProjection src/imageProjection.cpp)
add_dependencies(${PROJECT_NAME}_imageProjection ${catkin_EXPORTED_TARGETS} ${PROJECT_NAME}_generate_messages_cpp)
target_link_libraries(${PROJECT_NAME}_imageProjection ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${OpenCV_LIBRARIES})

# Feature Association
add_executable(${PROJECT_NAME}_featureExtraction src/featureExtraction.cpp)
add_dependencies(${PROJECT_NAME}_featureExtraction ${catkin_EXPORTED_TARGETS} ${PROJECT_NAME}_generate_messages_cpp)
target_link_libraries(${PROJECT_NAME}_featureExtraction ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${OpenCV_LIBRARIES})

# Mapping Optimization
add_executable(${PROJECT_NAME}_mapOptmization src/mapOptmization.cpp)
add_dependencies(${PROJECT_NAME}_mapOptmization ${catkin_EXPORTED_TARGETS} ${PROJECT_NAME}_generate_messages_cpp)
target_compile_options(${PROJECT_NAME}_mapOptmization PRIVATE ${OpenMP_CXX_FLAGS})
target_link_libraries(${PROJECT_NAME}_mapOptmization ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${OpenCV_LIBRARIES} ${OpenMP_CXX_FLAGS} gtsam)

# IMU Preintegration
add_executable(${PROJECT_NAME}_imuPreintegration src/imuPreintegration.cpp)
target_link_libraries(${PROJECT_NAME}_imuPreintegration ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${OpenCV_LIBRARIES} gtsam)

5.4 Package.xml

这个文件中可以看到一些作者的信息，并且告诉你了编译依赖和运行依赖都有什么。

<?xml version="1.0"?>
<package>
  <name>lio_sam</name>
  <version>1.0.0</version>
  <description>Lidar Odometry</description>

  <maintainer email="[email protected]">Tixiao Shan</maintainer>

  <license>TODO</license>

  <author>Tixiao Shan</author>

  <buildtool_depend>catkin</buildtool_depend>

  <build_depend>roscpp</build_depend>
  <run_depend>roscpp</run_depend>
  <build_depend>rospy</build_depend>
  <run_depend>rospy</run_depend>

  <build_depend>tf</build_depend>
  <run_depend>tf</run_depend>

  <build_depend>cv_bridge</build_depend>
  <run_depend>cv_bridge</run_depend>

  <build_depend>pcl_conversions</build_depend>
  <run_depend>pcl_conversions</run_depend>

  <build_depend>std_msgs</build_depend>
  <run_depend>std_msgs</run_depend>
  <build_depend>sensor_msgs</build_depend>
  <run_depend>sensor_msgs</run_depend>
  <build_depend>geometry_msgs</build_depend>
  <run_depend>geometry_msgs</run_depend>
  <build_depend>nav_msgs</build_depend>
  <run_depend>nav_msgs</run_depend>
  <build_depend>visualization_msgs</build_depend>
  <run_depend>visualization_msgs</run_depend>

  <build_depend>message_generation</build_depend>
  <run_depend>message_generation</run_depend>
  <build_depend>message_runtime</build_depend>
  <run_depend>message_runtime</run_depend>

  <build_depend>GTSAM</build_depend>
  <run_depend>GTSAM</run_depend>

</package>

从零入门激光SLAM（四）——ROS C++编译基础

1、如何编译C++

1.1 g++编译

1.2 cmake编译

1.3 利用ROS编译C++

2、ROS中自定义文件

2.1 自定义CMakeLists.txt编译文件

1.头文件

2.set编译模式

3. 检查C++标准

4. catkin_package

5. find_package

6.构建可执行文件并连接到库

7. install

8. 添加自定义消息与服务

2.2 自定义Package.xml依赖文件

1.基本信息

2.3 自定义 launch启动文件

1.定义功能包名字

2.执行文件生成ROS节点

3.参数服务器

4.消息重映射

5.启动多个launch

6.group

2.4 自定义msg消息

2.5 自定义srv消息

3、Topic话题编程

4、Service服务编程

5. LIO-SAM举例

5.1 目录介绍

5.2 Launch

5.3 CMakelists.txt

5.4 Package.xml

猜你喜欢

从零入门激光SLAM（四）——ROS C++编译基础

1、 如何编译C++

1.1 g++编译

1.2 cmake编译

1.3 利用ROS编译C++

2、ROS中自定义文件

2.1 自定义CMakeLists.txt编译文件

1.头文件

2.set编译模式

3. 检查C++标准

4. catkin_package

5. find_package

6.构建可执行文件并连接到库

7. install

8. 添加自定义消息与服务

2.2 自定义Package.xml依赖文件

1.基本信息

2.3 自定义 launch启动文件

1.定义功能包名字

2.执行文件生成ROS节点

3.参数服务器

4.消息重映射

5.启动多个launch

6.group

2.4 自定义msg消息

2.5 自定义srv消息

3、Topic话题编程

4、Service服务编程

5. LIO-SAM举例

5.1 目录介绍

5.2 Launch

5.3 CMakelists.txt

5.4 Package.xml

猜你喜欢

1、如何编译C++