Getting Started with Laser SLAM from Zero (4) - ROS C++ Compilation Basics

Hello everyone, I am a doctoral candidate in the direction of SLAM. I know the ups and downs of the SLAM learning process along the way, and I am very grateful to everyone for their high-quality articles and source code. With more and more knowledge, I am going to organize a column of my own laser SLAM study notes, from 0 to take everyone to quickly get started with laser SLAM, and it is also convenient for students who want to get started with SLAM and Xiaobai study reference, I believe you can read it There will be certain gains in the end. If there is something wrong, please point it out, and welcome everyone to exchange and discuss and make progress together.

Table of contents

1. How to compile C++

1.1 g++ compile

1.2 cmake compilation 

1.3 Compile C++ with ROS

2. Custom files in ROS

2.1 Custom CMakeLists.txt compilation file

1. Header file

 2.set compilation mode

3. Check the C++ standard

4. catkin_package

5. find_package

6. Build the executable and link to the library

7. install

8. Add custom messages and services

2.2 Custom Package.xml dependency file

1. Basic information

2.3 Customize the launch file

1. Define the function package name

2. Execute the file to generate ROS nodes

3. Parameter server

4. Message remapping

5. Start multiple launches

6.group

2.4 Custom msg message

 2.5 Customize srv message

3. Topic topic programming

4. Service service programming

5. Example of LIO-SAM

5.1 Directory Introduction

5.2 Launch

5.3 CMakelists.txt

5.4 Package.xml

Between learning SLAM, we don’t need to know the theory or the code, but we must be able to compile it (dog head), and it’s cool to run the code of the big guys directly. This section will describe how to compile it.

The code used in this section is at: [email protected]:huashu996/lidar_slam_course.git

1. How to compile C++

Before programming ROS, first learn how to compile C++, here is a tutorial from Mr. Gao Xiang. There are several ways to compile C++ in Ubuntu.

1.1 g++ compile

The code is in /lidar_slam_course/ch1/c++compile/g++

1. Write a C++ program helloSLAM.cpp

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

2. Install the g++ compiler to compile the file

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

3. Run the executable

After the compilation is complete, the .out file will be generated, and we run it in the terminal.

./out
Hello SLAM！

1.2 cmake compilation 

The code is in /lidar_slam_course/ch1/c++compile/cmake

1. In order to reference the header file compilation, first create the library file yourself

        Create the library file libHelloSLAM.cpp, and there are only function definitions in the library file.

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

2. Create the header file libHelloSLAM.h, the header file is a function declaration 

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

3. Create the compiled file useHello.cpp and call the function in the header file 

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

4. Create a CMakeLists.txt file

# 声明要求的 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. Create a build folder and compile

Putting the process file in the middle of compilation here will generate the helloSLAM executable file.

mkdir build
cd build
cmake ..
make
./helloSLAM

1.3 Compile C++ with ROS

1. Create a workspace

Create a workspace ros/src/helloslam with the structure shown in the figure below

You can use the following command to initialize a function package named fun_bag in the workspace, including the
initial CMakeLists.txt file and package.xml file 

catkin_create_pkg fun_bag pcl_conversions pcl_ros pcl_msgs sensor_msgs
roscd fun_bag
mkdir src

 2. Modify the CMakeLists.txt file and package.xml file

• 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>

If there is no catkin_package() in the CMakeLists.txt file, there will be no problem with compilation, but rosrun
cannot find the executable file. 

 3. Add code

Here directly copy the code created by cmake to here

 4. Compile and execute

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

Using ROS to compile C++ is the most commonly used method in SLAM projects. The following will introduce in detail how to configure and customize each file. 

2. Custom files in ROS

Let's first look at what's in these files

2.1 Custom CMakeLists.txt compilation file

1. Header file

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

 2.set compilation mode

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. Check the C++ standard

The function of this code is to check whether the compiler supports the C++11 standard, if it supports it, use the -std=c++11 flag to set the compiler standard, otherwise check whether the compiler supports the C++0x standard, and if it supports it, use The -std=c++0x flag sets the compiler standard, and outputs an error message if none are supported. This code first introduces the CheckCXXCompilerFlag module through the INCLUDE(CheckCXXCompilerFlag) command , which provides the CHECK_CXX_COMPILER_FLAG command to check whether the compiler supports the specified flag. Next, the CHECK_CXX_COMPILER_FLAG command checks whether the compiler supports the -std=c++11 and -std=c++0x flags, respectively, and stores the check results in the COMPILER_SUPPORTS_CXX11 and COMPILER_SUPPORTS_CXX0X variables.

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() is used to define the basic information of a Catkin package, such as package name, version number, author, description, etc. This command is generally placed at the top of the CMakeLists.txt file and is used to tell the Catkin build system to have basic information about this package.

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

Used to find and configure required dependencies and link them into a ROS node or library. For the function package that comes with ROS to add multiple dependencies at a time, use REQUIRED COMPONENTS :

The find_package(catkin REQUIRED COMPONENTS ...) directive automatically includes header file paths for all specified ROS catkin dependencies, so there is usually no need to explicitly add these paths in include_directories . The catkin_INCLUDE_DIRS variable contains the header file paths for all ROS catkin dependencies found using the find_package(catkin REQUIRED COMPONENTS ...) directive.

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 
)

Although the catkin_INCLUDE_DIRS variable contains the header file paths of all ROS catkin dependencies found using the find_package(catkin REQUIRED COMPONENTS ...) directive, it does not include the header file paths of other non- catkin third-party libraries (such as OpenCV and PCL ). The header file paths of these libraries need to be found using the find_package directive and stored in the corresponding variables.

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

Some third-party libraries (such as OpenCV and PCL ) are not catkin dependencies, so their header and library file paths are not automatically included in the catkin build system. Therefore, you need to use the find_package command to find these libraries, and specify their header files and library file paths, so that they can be linked correctly in the ROS node or library.

For example, the find_package(OpenCV) command stores the results in two variables, OpenCV_INCLUDE_DIRS and OpenCV_LIBRARIES , where the former contains the header file path and the latter contains the library file path. The header file is to see whether the code can be correctly located at compile time, and the library file is to call the code when the code is running.

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

6. Build the executable and link to the library

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

Use the add_executable() function to compile the two source files src/pointcloud_mapping.cpp and src/PointCloudMapper.cc into an executable file pointcloud_mapping, where pointcloud_mapping is the name of the executable file, which does not represent the name of the ros node, but if you use rosrun When running, a node named pointcloud_mapping is generated by default. If you want to customize the node name use the following command. 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} )

Use the target_link_libraries() function to link the compiled executable to the required libraries and target_include_directories() to link the header files. These two functions can be used directly, without using link_directories and include_directories to make each code accurately add third-party libraries.

Sometimes add_dependencies is used to add dependencies, which tells CMake that when building a target, it needs to build the specified dependency target first.

7. install

This directive is used to install the generated object files to the specified location. Here, we installed the "pointcloud_mapping" executable into the ${CATKIN_PACKAGE_BIN_DESTINATION} directory, which is usually the "bin" directory in a ROS package .

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

8. Add custom messages and services

当需要添加额外的消息与服务时候，需要将这几行的注释取消，并且加上消息与服务的文件名字
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 Custom Package.xml dependency file

1. Basic information

  <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. Compile dependencies

<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. Running dependencies

  <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 Customize the launch file

<node>	run a node
<param>	Set a parameter on the parameter server
<remap>	Declare the naming of remapping ros computing graph resources
<machine>	Declare the machine to use at startup
<rosparam>	Multiple parameters in load file
<include>	Contains other ros launch files
<env>	Specify an environment variable for starting the node.
<test>	Run a test node, see rostest for details
<arg>	declare a parameter
<group>	Group enclosing elements that share a namespace or remap

1. Define the function package name

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

2. Execute the file to generate ROS nodes

<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. Parameter server

--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. Message remapping

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

5. Start multiple launches

 <!--- 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 Custom msg message

Taking lio-sam as an example, the msg file is a text file used to describe the fields of ROS messages. msg files are simply text files with each line having a field type and field name. There is also a special data type in ROS: Header, which contains timestamp and coordinate frame information widely used in ROS. The Header header can often be seen in the first line of the msg file.

1. Create a message package

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. Code plus message header file

#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. Add a message to the compiled configuration file

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. View messages

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

 2.5 Customize srv message

An srv file describes a service. It consists of two parts: request (request) and response (response).

1. Create a message

mkdir srv
cd srv
sudo gedit save_map.srv

2. Add compilation

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

3. Add the header file to the code

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

3. Topic topic programming

3.1 Publisher

       Publishers and subscribers are the most commonly used functions in ROS, enabling data processing between different sensors. The following will show how to use C++ to write a publisher with a release rate of 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 Subscribers

#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;
}

In the above code, we first initialized the ROS node and created a node called "image_subscriber". Then, we created a Subscriber object, subscribed to the topic named "image", and specified the message queue size as 100. This means that we can cache up to 100 unprocessed messages without processing them in time.
Next, we set up a loop where we call ros::spinOnce() to process all callbacks, and call loop_rate.sleep() to make the loop run at the specified frequency. Here, we set the frequency to 100hz to match our desired rate.
When a Subscriber subscribes to a topic, it sends a subscription request to the ROS Master and creates a queue for caching messages. When there are new messages for subscribed topics, these messages will be added to the end of the queue. Subscriber will process messages in the queue in a first-in-first-out (FIFO) manner. If the queue is full and a new message arrives, the old message will be discarded to make room for the new one. 

4. Service service programming

4.1 Server

#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 Client

#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. Example of LIO-SAM

5.1 Directory Introduction

        The content of this part has been written in great detail step by step, but we still don’t know how to write it in the actual project. The blogger will use the source code of LIO-SAM laser SLAM to explain how to use it. First, download the LIO-SAM code , You can also directly download the code address I organized.

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

First of all, you can see the content of the directory, config is the parameter configuration file, include is the header file, launch is the startup file, msg is the custom message file, src is the main communication programming file, srv is the service programming file, CMakeLists.txt and package .xml is the compilation configuration file, and readme.md is the introduction to the feature pack. The following will introduce how a bunch of customizations introduced above are used in actual projects.

5.2 Launch

First of all, let's take a look at the Launch file. The launch file can quickly check what nodes exist in this package and what parts it consists of. First open a general launch file, launch/run.launch, you can see that it contains 4 sub-launch files, and each sub-launch file starts a small function, such as starting loam code and starting 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>

Then there are 4 sub-launch files in launch/include, we open module_loam.launch, we can see that this launch file contains 4 nodes, _imuPreintegration, _imageProjection, _featureExtraction, _mapOptmization. And these nodes are started by four executable files, then these four executable files are compiled from CMakelists.txt, let's take a look at the CMakelists.txt file of LIO-SAM.

<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

First see catkin_package to see all the libraries and messages that this package depends on. 

catkin_package(
  INCLUDE_DIRS include
  DEPENDS PCL GTSAM

  CATKIN_DEPENDS
  std_msgs
  nav_msgs
  geometry_msgs
  sensor_msgs
  message_runtime
  message_generation
  visualization_msgs
)

Next, find all dependencies and third-party libraries required by this package from 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)

As can be seen from add_message_files and add_service_files, this package customizes a cloud_info message and a save_map service 

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
)

 Find the header file and library file of the third-party library and establish a link with the C++ file to generate an executable file. The executable file generated here is exactly what is called in 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

You can see some author's information in this file, and tell you what the compilation dependencies and runtime dependencies are.

<?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>