多传感器融合-搭建ROS工程

说明:针对任乾大佬的知乎专栏,详细记录如何用vscode搭建ros工程

一 安装Terminator

sudo apt install terminator

二 vscode创建功能包

这里参考奥特学园的视频,简直太详细了->ROS理论与实践

1. 创建ros工作空间

mkdir -p localization_ws/src
cd localization_ws
catkin_make

2. 启动vscode

cd localization_ws
code .

3. vscode中编译ros配置
   设置快捷键 ctrl + shift + B 调用编译，选择:catkin_make:build
   可以点击配置设置为默认，修改.vscode/tasks.json 文件

{
// 有关 tasks.json 格式的文档，请参见
    // https://go.microsoft.com/fwlink/?LinkId=733558
    "version": "2.0.0",
    "tasks": [
        {
            "label": "catkin_make:debug", //代表提示的描述性信息
            "type": "shell",  //可以选择shell或者process,如果是shell代码是在shell里面运行一个命令，如果是process代表作为一个进程来运行
            "command": "catkin_make",//这个是我们需要运行的命令
            "args": [],//如果需要在命令后面加一些后缀，可以写在这里，比如-DCATKIN_WHITELIST_PACKAGES=“pac1;pac2”
            "group": {"kind":"build","isDefault":true},
            "presentation": {
                "reveal": "always"//可选always或者silence，代表是否输出信息
            },
            "problemMatcher": "$msCompile"
        }
    ]
}

4. 创建ROS功能包
   选定 src 右击 —> create catkin package.
   设置包名 添加依赖,先加roscpp rospy std_msgs,其他包可以之后用到再添加.

    自动增加了include和src文件夹,以及CMakeList.txt和package.xml
   

5. 添加源文件
6.配置CMakeLists.txt
是功能包里边自动生成的CmakeLists.txt

add_executable(节点名称
  src/C++源文件名.cpp
)
target_link_libraries(节点名称
  ${catkin_LIBRARIES}
)

三 ROS相关基础

1.添加源文件

	先增加一个简单的cpp测试一下.

#include "ros/ros.h"

int main(int argc, char *argv[])
{
    setlocale(LC_ALL,"");
    //执行节点初始化
    ros::init(argc,argv,"HelloVSCode");

    //输出日志
    ROS_INFO("Hello VSCode!!!哈哈哈哈哈哈哈哈哈哈");
    return 0;
}

	配置功能包里的CMakeLists.txt

	ctrl shift b编译

	启动roscore

	设置环境变量
	运行节点

2.添加launch文件

type是跟CMakeLists.txt中add_executable的名字相同的.
ros::init的名字跟name是一个,但是name可以覆盖ros::init的名字.

<launch>
    <node pkg="lidar_localization" type="hahaha" name="hello" output="screen" />
</launch>

node —> 包含的某个节点

pkg -----> 功能包

type ----> 被运行的节点文件

name --> 为节点命名,这个名字可以覆盖代码中的名字,以这个为准

output-> 设置日志的输出目标

3.ROS话题通讯

发布者

1.初始化ROS节点
2.向ROS Master注册节点信息
3.按照一定频率循环发布消息

订阅者

1.初始化ROS节点
2.订阅需要的话题
3.循环等待话题消息,接收到消息后进入回调函数
4.在回调函数中完成消息处理

大佬把这种消息发布和订阅的方式做了修改:我们把每一类信息的订阅和发布封装成一个类，它的callback做为类内函数存在，这样我们在node文件中想要订阅这个消息的时候只需要在初始化的时候定义一个类的对象，就可以在正常使用过程中从类内部直接取它的数据了。

四 添加代码

1. 发布者publisher

一般的发布者实现(ros基础):

/*
    需求: 实现基本的话题通信，一方发布数据，一方接收数据，
         实现的关键点:
         1.发送方
         2.接收方
         3.数据(此处为普通文本)
         PS: 二者需要设置相同的话题
    消息发布方:
        循环发布信息:HelloWorld 后缀数字编号
    实现流程:
        1.包含头文件 
        2.初始化 ROS 节点:命名(唯一)
        3.实例化 ROS 句柄
        4.实例化 发布者 对象
        5.组织被发布的数据，并编写逻辑发布数据
*/
// 1.包含头文件 
#include "ros/ros.h"
#include "std_msgs/String.h" //普通文本类型的消息
#include <sstream>

int main(int argc, char  *argv[])
{   
    //设置编码
    setlocale(LC_ALL,"");

    //2.初始化 ROS 节点:命名(唯一)
    // 参数1和参数2 后期为节点传值会使用
    // 参数3 是节点名称，是一个标识符，需要保证运行后，在 ROS 网络拓扑中唯一
    ros::init(argc,argv,"talker");
    //3.实例化 ROS 句柄
    ros::NodeHandle nh;//该类封装了 ROS 中的一些常用功能

    //4.实例化 发布者 对象
    //泛型: 发布的消息类型
    //参数1: 要发布到的话题
    //参数2: 队列中最大保存的消息数，超出此阀值时，先进的先销毁(时间早的先销毁)
    ros::Publisher pub = nh.advertise<std_msgs::String>("chatter",10);

    //5.组织被发布的数据，并编写逻辑发布数据
    //数据(动态组织)
    std_msgs::String msg;
    // msg.data = "你好啊！！！";
    std::string msg_front = "Hello 你好！"; //消息前缀
    int count = 0; //消息计数器

    //逻辑(一秒10次)
    ros::Rate r(1);

    //节点不死
    while (ros::ok())
    {
        //使用 stringstream 拼接字符串与编号
        std::stringstream ss;
        ss << msg_front << count;
        msg.data = ss.str();
        //发布消息
        pub.publish(msg);
        //加入调试，打印发送的消息
        ROS_INFO("发送的消息:%s",msg.data.c_str());

        //根据前面制定的发送贫频率自动休眠 休眠时间 = 1/频率；
        r.sleep();
        count++;//循环结束前，让 count 自增
        //暂无应用
        ros::spinOnce();
    }
    return 0;
}

我们把每一类信息的订阅和发布封装成一个类，它的callback做为类内函数存在(任大佬的修改):

a.点云发布类CloudPublisher:

//构造函数,需要传入句柄nh,话题名,有了这两个参数,就可以想一般的发布一样如发布信息了
CloudPublisher::CloudPublisher(ros::NodeHandle& nh,
                               std::string topic_name,
                               size_t buff_size,
                               std::string frame_id)
    :nh_(nh), frame_id_(frame_id) {
    publisher_ = nh_.advertise<sensor_msgs::PointCloud2>(topic_name, buff_size);
}

void CloudPublisher::Publish(CloudData::CLOUD_PTR  cloud_ptr_input) {
	...
    publisher_.publish(*cloud_ptr_output);
}

b.里程计发布类OdometryPublisher:

同上,略

c.如何发布

在普通的方式中:

while (ros::ok())
{
	pub.publish(msg);
}

改进后的发布方式:

int main(int argc, char *argv[]) {
// 定义类对象指针
	//订阅者
    std::shared_ptr<CloudSubscriber> cloud_sub_ptr = std::make_shared<CloudSubscriber>(nh, "/kitti/velo/pointcloud", 100000);
    std::shared_ptr<IMUSubscriber> imu_sub_ptr = std::make_shared<IMUSubscriber>(nh, "/kitti/oxts/imu", 1000000);
    std::shared_ptr<GNSSSubscriber> gnss_sub_ptr = std::make_shared<GNSSSubscriber>(nh, "/kitti/oxts/gps/fix", 1000000);
    std::shared_ptr<TFListener> lidar_to_imu_ptr = std::make_shared<TFListener>(nh, "velo_link", "imu_link");
	//发布者
    std::shared_ptr<CloudPublisher> cloud_pub_ptr = std::make_shared<CloudPublisher>(nh, "current_scan", 100, "/map");
    std::shared_ptr<OdometryPublisher> odom_pub_ptr = std::make_shared<OdometryPublisher>(nh, "lidar_odom", "map", "lidar", 100);
   //订阅数据,发布数据
    while (ros::ok()) {
    	ros::spinOnce();
    	//取数据
    	cloud_sub_ptr->ParseData(cloud_data_buff);
        imu_sub_ptr->ParseData(imu_data_buff);
        gnss_sub_ptr->ParseData(gnss_data_buff);
        //经过一些计算...
        
		//发布数据
		cloud_pub_ptr->Publish(cloud_data.cloud_ptr);
		odom_pub_ptr->Publish(odometry_matrix);
    	rate.sleep();
	}
    return 0;
}

2. 订阅者subscriber

一般订阅者的实现:

/*
    需求: 实现基本的话题通信，一方发布数据，一方接收数据，
         实现的关键点:
         1.发送方
         2.接收方
         3.数据(此处为普通文本)


    消息订阅方:
        订阅话题并打印接收到的消息

    实现流程:
        1.包含头文件 
        2.初始化 ROS 节点:命名(唯一)
        3.实例化 ROS 句柄
        4.实例化 订阅者 对象
        5.处理订阅的消息(回调函数)
        6.设置循环调用回调函数

*/
// 1.包含头文件 
#include "ros/ros.h"
#include "std_msgs/String.h"

void doMsg(const std_msgs::String::ConstPtr& msg_p){
    ROS_INFO("我听见:%s",msg_p->data.c_str());
    // ROS_INFO("我听见:%s",(*msg_p).data.c_str());
}
int main(int argc, char  *argv[])
{
    setlocale(LC_ALL,"");
    //2.初始化 ROS 节点:命名(唯一)
    ros::init(argc,argv,"listener");
    //3.实例化 ROS 句柄
    ros::NodeHandle nh;

    //4.实例化 订阅者 对象
    ros::Subscriber sub = nh.subscribe<std_msgs::String>("chatter",10,doMsg);
    //5.处理订阅的消息(回调函数)

    //     6.设置循环调用回调函数
    ros::spin();//循环读取接收的数据，并调用回调函数处理

    return 0;
}

改进成一个类后的实现:

a.gnss订阅者

//构造函数,传入句柄和话题
GNSSSubscriber::GNSSSubscriber(ros::NodeHandle& nh, std::string topic_name, size_t buff_size) 
    :nh_(nh) {
    subscriber_ = nh_.subscribe(topic_name, buff_size, &GNSSSubscriber::msg_callback, this);
}
//其中msg_callback就是它的callback函数，也就是接收和处理信息的地方
void GNSSSubscriber::msg_callback(const sensor_msgs::NavSatFixConstPtr& nav_sat_fix_ptr) {
    //...
    new_gnss_data_.push_back(gnss_data);
}
//函数ParseData就是实现从类里取数据的功能
void GNSSSubscriber::ParseData(std::deque<GNSSData>& gnss_data_buff) {
    if (new_gnss_data_.size() > 0) {
        gnss_data_buff.insert(gnss_data_buff.end(), new_gnss_data_.begin(), new_gnss_data_.end());
        new_gnss_data_.clear();
    }
}

b.cloud订阅者

同上,略

c.imu订阅者

同上,略

d.如何订阅

见上边的"c.如何发布"

3. sensor_data

包含点云, gnss, imu数据

4. tf_listener

五 CMakeLists文件规划

这里的CMakeLists.txt指的还是功能包里的那个,目前只有一个功能包lidar_localization.我们把每个包对应的这些操作放在cmake文件夹下对应的XX.cmake文件中，然后在CMakeLists中 include(cmake/XX.cmake)一行代码就可以搞定。

CMakeLists详解:

cmake_minimum_required(VERSION 2.8.3)
project(lidar_localization) 	#ros包的名称,在vscode中创建ros功能包时输入的那个名称,之后在输入了roscpp,rospy等依赖项

SET(CMAKE_BUILD_TYPE "Release")
SET(CMAKE_CXX_FLAGS_RELEASE "$ENV{CXXFLAGS} -O3 -Wall")
add_compile_options(-std=c++11)
add_definitions(-std=c++11)

find_package(catkin REQUIRED COMPONENTS
  roscpp
  rospy
  std_msgs
  pcl_ros 	# 创建lidar_localization功能包时,只添加了上边三个依赖,遇到新的依赖,要在这里加进去
  geometry_msgs
  tf
  eigen_conversions
)

set(ALL_TARGET_LIBRARIES "") 	#为了避免target_link_libraries后面跟很长一串库的名字，而且库增减的时候它也得跟着增减，我们在CMakeLists文件一开始定义一个变量

include(cmake/glog.cmake) #变量ALL_TARGET_LIBRARIES在.cmake会添加新的内容
include(cmake/PCL.cmake)
include(cmake/eigen.cmake)
include(cmake/geographic.cmake)

include_directories(include ${catkin_INCLUDE_DIRS})
include(cmake/global_defination.cmake)
catkin_package()

# 除了库对应的变量，还有文件名字对应的变量，我们在add_executable的时候要把所需要的cpp文件路径都要写进去，文件多的时候也是太麻烦，所以可以使用下面的指令把所有cpp文件合并到一个变量中
file(GLOB_RECURSE ALL_SRCS "*.cpp")
#但是，当工程中有多个node文件的时候，就要把他们从这个变量中踢出去，因为多个node文件编到一个可执行文件中会出错。用下面的代码踢
file(GLOB_RECURSE NODE_SRCS "src/*_node.cpp")
file(GLOB_RECURSE THIRD_PARTY_SRCS "third_party/*.cpp")
list(REMOVE_ITEM ALL_SRCS ${NODE_SRCS}) #剔除src文件夹内的_node.cpp,都剔除,别担心,后边添加add_executable把需要的node.cpp单独加上就行
list(REMOVE_ITEM ALL_SRCS ${THIRD_PARTY_SRCS}) #剔除third_party文件夹内的.cpp

add_executable(test_frame_node src/test_frame_node.cpp ${ALL_SRCS})
target_link_libraries(test_frame_node ${catkin_LIBRARIES} ${ALL_TARGET_LIBRARIES})

关于catkin CMakeLists.txt

关于catkin CMakeList.txt的一些补充知识(引用):
catkin是ROS官方的一个编译构建系统，是原本的ROS的编译构建系统rosbuild的发展。catkin_make是将cmake与make的编译方式做了一个封装的指令工具，规范了工作路径与生成文件路径。
catkin是ROS官方的一个编译构建系统，是原本的ROS的编译构建系统rosbuild的发展。catkin_make是将cmake与make的编译方式做了一个封装的指令工具，规范了工作路径与生成文件路径。

总体结构:

• 必需的CMake版本：cmake_minimum_required()
• 软件包名：project()
• 查找编译依赖的其他CMake/Catkin包（声明依赖库）：find_package()
• 启动Python模块支持：catkin_python_package()
• 消息/服务/操作(Message/Service/Action)生成器：add_message_files(),add_service_files(),add_action_files()
• 调用消息/服务/操作生成：generate_messages()
• 指定包编译信息导出：catkin_package()
• 添加要编译的库和可执行文件：add_library()/add_executable()/target_link_libraries()
• 测试编译：catkin_add_gtest()
• 安装规则：install()

cmake版本

软件包名

CMake中，可以通过使用变量 ${PROJECT_NAME}在CMake脚本后面的任何位置引用项目名称。

查找编译依赖的CMake包

编译一个项目，需要使用CMake 的 find_package函数确定依赖的其他CMake包并找到它们，一般情况下至少会有一个catkin依赖：

find_package(catkin REQUIRED)

除此之外，项目依赖的其他软件包，都会自动成为catkin的组件（components）（就CMake而言）。因此可以将这些依赖包指定为catkin的组件，而不必再使用find_package，这样将会变得简单

find_package(catkin REQUIRED COMPONENTS
  roscpp
  rospy
  std_msgs
  pcl_ros
  geometry_msgs
  tf
  eigen_conversions
)

find_package()查找到一个软件包，它就会创建几个CMake环境变量，以提供有关已查找到的软件包的信息。这些环境变量可以在后面的CMake脚本中使用，它们表示软件包导出的头文件所在的位置、源文件所在的位置、软件包依赖的库以及这些库的查找路径.

//catkin_INCLUDE_DIRS就是通过find_package()获得的
include_directories(include ${catkin_INCLUDE_DIRS})

如果不用组件的方式,也单独使用,比如.cmake中的:

find_package(PCL 1.7 REQUIRED)
list(REMOVE_ITEM PCL_LIBRARIES "vtkproj4")
include_directories(${PCL_INCLUDE_DIRS})
list(APPEND ALL_TARGET_LIBRARIES ${PCL_LIBRARIES})

以catkin的组件的方式 find_package它们是有好处的，因为这个过程以catkin_prefix的形式创建了一组环境变量,这就意味着nodelet导出的头文件路径、库等都会附加到 catkin_variables上，比如，catkin_INCLUDE_DIRS不仅包含catkin的头文件路径，也包含了nodelet软件包的头文件路径，这在后面会派上用场。
Boost库:
如果使用C++和Boost库，需要在Boost上调用 find_package()，并指定Boost中将要作为组件的那部分。例如，如果想要使用Boost的线程，可以用：

find_package(Boost REQUIRED COMPONENTS thread)

catkin_package()

catkin_package()是一个由catkin提供的CMake宏。需要指定特定的catkin信息到编译系统，而这些信息又会被用于生成pkg-config和CMake文件。

该函数必须在使用 add_library()或add_executable()声明任何targets之前调用。其5个可选参数：

• INCLUDE_DIRS：软件包导出的头文件路径（例如cflags）
• LIBRARIES：项目导出的库
• CATKIN_DEPENDS：当前项目依赖的其他catkin项目
• DEPENDS：当前项目依赖的非catkin CMake项目，详细解释参见这里
• CFG_EXTRAS：其他的配置选项

头文件目录

include_directories的参数应该是由调用find_package生成的* _INCLUDE_DIRS变量以及需要包含的任何其他目录。如果使用catkin和Boost，include_directories()的调用为：

include_directories(include {Boost_INCLUDE_DIRS} {catkin_INCLUDE_DIRS})

第一个参数“include”表示包中的include/目录也是路径的一部分。

库目录

CMake的 link_directories()函数可以添加其他的库目录，然而，并不推荐这么做。所有的catkin和CMake包在find_package时都会自动添加链接信息。只需链接到target_link_libraries()中的库。
添加其他库目录:

link_directories(~/my_libs)

库目标(生成库)

CMake函数 add_library()指定用于编译的库文件，默认情况下，catkin编译共享库。

add_library({PROJECT_NAME} {${PROJECT_NAME}_SRCS})

target_link_libraries

使用 target_link_libraries()函数指定可执行目标所要链接的库，即告诉CMake当链接此可执行文件时需要链接哪些库（这些库在上面的find_package中定义），通常在调用完add_executable()后被调用。如果出现ros未定义的引用错误，则添加${catkin_LIBRARIES}。

target_link_libraries(<executableTargetName>, <lib1>, <lib2>, ... <libN>)

完整的例子:

//将 foo与libmoo.so链接起来
add_executable(foo src/foo.cpp)
add_library(moo src/moo.cpp)
target_link_libraries(foo moo)