大家好呀,我是一个SLAM方向的在读博士,深知SLAM学习过程一路走来的坎坷,也十分感谢各位大佬的优质文章和源码。随着知识的越来越多,越来越细,我准备整理一个自己的激光SLAM学习笔记专栏,从0带大家快速上手激光SLAM,也方便想入门SLAM的同学和小白学习参考,相信看完会有一定的收获。如有不对的地方欢迎指出,欢迎各位大佬交流讨论,一起进步。博主创建了一个科研互助群Q:772356582,欢迎大家加入讨论。
目录
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之间,我们可以不懂理论,可以不懂代码,但我们一定得会编译是不是(狗头),直接跑大佬们的代码爽到飞好不好,这节将讲述如何编译。
这节所用到的代码在:git@github.com: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="castiel_liu@outlook.com">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="shant@mit.edu">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中调用的。文章来源:https://www.toymoban.com/news/detail-497396.html
# 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
这个文件中可以看到一些作者的信息,并且告诉你了编译依赖和运行依赖都有什么。文章来源地址https://www.toymoban.com/news/detail-497396.html
<?xml version="1.0"?>
<package>
<name>lio_sam</name>
<version>1.0.0</version>
<description>Lidar Odometry</description>
<maintainer email="shant@mit.edu">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++编译基础的文章就介绍完了。如果您还想了解更多内容,请在右上角搜索TOY模板网以前的文章或继续浏览下面的相关文章,希望大家以后多多支持TOY模板网!