使用开源ros包实现导航功能（一）

本次程序升级过程为python转到c++环境，实现真正的可编译式编程。

由于我自己记性不是很好，所以需要每天把这种最基础的事情反复过一遍，同时，由于没有老师专门督导你的训练，所以建立了博客机制，从现在开始，一切以学习为重，以提升能力为重。

ros是什么？

是一种机器人操作系统，现在我认为他就是一个通信的框架。

正式的定义：

1ROS（Robot Operating System）是一个开源的机器人操作系统框架。

2ROS的设计目标是提供一种灵活、分布式的系统架构，使机器人开发变得更加简单、可重用和可扩展。

3基于节点（Node）的通信模型，允许不同的软件组件以节点的形式进行通信和交互。提供了丰富的软件库和工具，能够快速利用已有的软件模块和算法来加速开发过程。

如何学习ros

1官方文档：ROS官方网站提供了详细的文档和教程，包括ROS的核心概念、通信机制、软件包使用等。你可以从官方文档开始，了解ROS的基本知识和使用方法。

2ROS教程：ROS官方提供了一系列的教程，涵盖了不同领域和难度级别的主题，如ROS基础、传感器处理、导航、机器人建模等。你可以按照自己的兴趣和需求选择适合的教程进行学习。

3实践项目：通过实际项目来应用和巩固ROS的知识。你可以选择一个小型的机器人项目或仿真环境，尝试构建和控制机器人，实现一些基本的功能，如感知、运动控制、导航等。这样能够更好地理解ROS的运作方式和解决实际问题的能力。

安装ubuntu系统

双系统安装流程基本成熟。

ubuntu使用入门

linux系统初接触者需要的，一些基本的命令

ls

mkdir 文件夹名称

cd (change directory)

cd …

source 也可以启动.sh文件

如：source ~/command.sh

~/.bashrc 终端程序启动脚本——执行终端程序时都会进行初始化。

ros安装

小鱼一键安装即可

有时间可以看看那些安装指令都写的什么，应该是开源的。

pub的c++实现

#include #include int main(int argc, char *argv[])
{ ros::init(argc, argv, "chao_node");
    printf("chao_node!\n");
    ros::NodeHandle nh;
    ros::Publisher pub = nh.advertise("topic1", 10);
    while(ros::ok())
    { printf("pub_ok!\n");
        std_msgs::String msg; //初始化
        msg_data = "hhhh";
        pub.publish(msg);
    }
    return 0;
}

修改cmakelists：

有了src后：

catkin_create_pkg 包名 依赖项

以下内容也完全可以认真学习一下，这样编译就不会再出现问题了。

cmake_minimum_required(VERSION 2.8.3)
project(chao_pkg)
find_package(catkin REQUIRED COMPONENTS
  roscpp
  std_msgs
)
catkin_package(
  CATKIN_DEPENDS roscpp std_msgs
)
include_directories(
  include
  ${catkin_INCLUDE_DIRS}
)
add_executable(chao_node src/chao.cpp)
target_link_libraries(chao_node ${catkin_LIBRARIES})

这里既然遇到了，那就把cmake的内容好好整理学习一下：

Cmake入门进阶

在编译ros功能包时主要有两个关键文件，一个cmakelists.txt和package.xml，很多的开源代码无法编译与这两个文件脱不了干系，所以需要好好弄清楚这里面的参数。

指定cmake最低版本：

cmake_minimum_required(VERSION 2.8.3)

指定项目名称

project(chao_pkg)，一般就是功能包的名字。

查找并引入ros软件包：

find_package(

cakin REQUIRED COMPONENTS //查找catkin软件包

rospy

roscpp

std_msgs

)

将xxx所选内容标记为项目的依赖项，用于生成ros功能包的package.xml文件？

catkin_package(CATKIN_DEPENDS roscpp std_msgs)

指定包含文件的目录

include_directories(include c a t k i n I N C L U D E D I R S ) / / 编 译 器 找 头 文 件 其 中 {catkin_INCLUDE_DIRS}) //编译器找头文件 其中 catkinI​NCLUDED​IRS)//编译器找头文件其中{catkin_INCLUDE_DIRS}是ros依赖项的头文件所在的目录。

如果是当前项目的：

${PROJECT_SOURCE_DIR}/include也加进去即可。

然后是两个核心：

创建可执行文件：

add_executable(chao_node src/chao.cpp)

并制定了名称chao_node与可执行文件链接

指定链接的库文件:

target_link_libraries(chao_node $catkin_LIBRARIES}) //链接器找库文件

这里涉及cpp的一些知识，

什么是头文件，什么是库文件？

头文件（Header Files）：

头文件通常具有 “.h” 或 “.hpp” 的文件扩展名。

头文件包含变量、函数和类的声明，但不包含其实际的实现代码。

头文件用于在编译时告诉编译器有关函数和类的接口信息，使得在其他源文件中使用它们时能够正确编译。

头文件通常包含函数或类的声明、类型定义、常量定义和其他必要的声明。

库文件（Library Files）：

库文件是已编译和链接的二进制文件，其中包含了特定功能的可重用代码。

库文件可以是静态库（以 “.a” 或 “.lib” 为扩展名）或动态库/共享库（以 “.so”、".dll" 或 “.dylib” 为扩展名）。

静态库在链接时被完整地复制到可执行文件中，程序与静态库的代码在编译时就被连接在一起。

动态库在运行时被加载到内存中，程序在运行时使用动态库的功能。

什么是编译器，什么是链接器？

编译器（Compiler）和链接器（Linker）是软件开发中两个不可或缺的工具，用于将源代码转换为可执行程序。

编译器：

编译器是一个将高级语言（如C、C++、Java等）源代码转换为低级语言（如机器码）的工具。

编译器负责将源代码分析、词法分析、语法分析，并生成目标文件（通常是二进制文件）。

编译器执行诸如类型检查、语法验证、代码优化等任务，以确保生成的目标文件正确且高效。

链接器：

链接器是一个将目标文件（或库文件）组合在一起，生成最终可执行程序的工具。

链接器的主要任务是解决不同目标文件之间的符号引用和符号定义的关系。

链接器将符号引用与符号定义进行匹配，解析外部依赖关系，并将各个目标文件的代码和数据段组合在一起，生成最终可执行程序。

然后是package.xml的解析：

catkin

使用catkin构建工具来构建该软件包。

rospy

指定构建过程中需要的rospy库

roscpp

指定当前功能包被其他软件包构建时，这些软件包也需要使用到的依赖项。也就是说构建其他软件包的同时就会自动获得当前软件包所需要的依赖项。

std_msgs

指定运行依赖项，运行时需要std_msgs

pub、sub综合的c++实现

#include #include #include void callback(const std_msgs::String::ConstPtr& msg)
{ printf("%s\n", msg->data.c_str());
}
int main(int argc, char *argv[])
{ ros::init(argc, argv, "my_node1");
    ros::NodeHandle nh;
    ros::Publisher pub = nh.advertise("topic1", 10);
    ros::Subscriber sub = nh.subscribe("topic2", 10, callback);
    ros::Rate loop_rate(10);
    std_msgs::Int8 gps_msg;
    gps_msg.data = 1;
    while(ros::ok())
    { pub.publish(gps_msg);
        ROS_INFO("hello_ros");
        ros::spinOnce();
        loop_rate.sleep();
    }
}

cmakelists.txt和package.xml:

cmake_minimum_required(VERSION 2.8.3)
project(dynamic_tutorials)
find_package(catkin REQUIRED COMPONENTS
  dynamic_reconfigure
  roscpp
  std_msgs
)
generate_dynamic_reconfigure_options(
  cfg/DynamicParams.cfg
)
catkin_package(
  CATKIN_DEPENDS dynamic_reconfigure roscpp std_msgs
)
include_directories(
  ${catkin_INCLUDE_DIRS}
)
add_executable(dynamic_server src/dynamic_server.cpp)
add_executable(yao_node src/yao.cpp)
add_executable(gps_imu_pub src/gps_imu_pub.cpp)
add_dependencies(dynamic_server ${PROJECT_NAME}_gencfg)  #用于指定构建 dynamic_server 目标（即可执行文件）所依赖的其他目标。
target_link_libraries(dynamic_server ${catkin_LIBRARIES})
target_link_libraries(yao_node ${catkin_LIBRARIES})
target_link_libraries(gps_imu_pub ${catkin_LIBRARIES})
add_executable(cyun1 src/cyun1.cpp)
target_link_libraries(cyun1 ${catkin_LIBRARIES})

 dynamic_tutorials 0.0.0 The dynamic_tutorials package Your Name BSD dynamic_reconfigure dynamic_reconfigure roscpp roscpp std_msgs std_msgs catkin   

激光雷达数据的ros描述

index.ros.org官方文件中，（要多去官方文档学习ros）

通过rostopic info /scan

还有rostopic echo /scan --noarr折叠数组

header：时间戳和frameid（与tf有关）

float32 angle_min和angle_max #扫描的起始角度和终止角度

float32 angle_increment # 相邻两次测距的旋转夹角？

float32 time_increment # 相邻两次测距的时间差 #修正lidar运动过程中的测距矩阵的形变，低速运动可以不考虑

float32 scan_time # 两次扫描的时间差（s），其实就是lidar转一周，1/scan_time就得到了扫描频率

1/0.0877约等于11hz

float32 range_min #这两个数值之间的测距值是有效的，这是一个环形的区域，太远和太近都扫描不到

float32 range_max

最重要的数据：

floate32[] ranges #测距的数组，360度的，ranges的长度就是360个测距值，不是所有都是有效的，有些会变成infinite无穷大测不到头，单位m

float32[] intensities #测距的返回信号强度

通过c++获取激光雷达数据：

激光雷达的话题和数据一般是由产商提供。我们要做的是从lidar中获取这些数据：

#include #include void lidarCallback(const sensor_msgs::LaserScan msg)
{ float fMidDist = msg.ranges[180];
    ROS_INFO("前方测距 ranges[180] = %f 米", fMidDist);
}
int main(int argc, char *argv[])
{ ros::init(argc, argv, "lidar_node");
    ros::NodeHandle nh;
    ros::Subscriber sub = nh.subscribe("/scan", 10, &lidarCallback);
    ros::spin();
    return 0;
}

激光雷达避障的c++实现

通过cmd_vel和scan来避障，已经能获取激光雷达数据了之后，添加发布者就行了，也就是速度控制包cmd_vel，这里的速度包括角速度，所以是可以转向的。

又学到一些东西：rostopic hz /scan 可以查看数据发送的频率

rostopic info 可以查看话题的详情

rostopic echo / --noarr 折叠数组使其看起来更加清晰

同时也对c++中的全局变量有了一定的了解了。为什么变量没有更新，很可能就是被当成了全局变量。这个是我的失误。

这里就是一个很简易的避障思路，遇到障碍物进行z轴旋转一定的角度。

cmd_vel工具：

rosrun rqt_robot_steering rqt_robot_steering

#include #include #include ros::Publisher vel_pub;
static int nCount = 0;
void lidarCallback(const sensor_msgs::LaserScan msg)
{ float fMidDist = msg.ranges[180];
    geometry_msgs::Twist vel_cmd;
    ROS_WARN("前方测距 ranges[180] = %f 米", fMidDist);
    std::cout<<"nCount:"< 0)
    { nCount--;
        return;
    }
    if(fMidDist < 1.5f)
    { vel_cmd.angular.z = 0.3;  
        nCount = 50;
    }
    else
    { // vel_cmd.angular.z = 0;
        vel_cmd.linear.x = 0.05;
    }
    std::cout<<"vel_cmd:"< ros::init(argc, argv, "lidar_node");
    ros::NodeHandle nh;
    ros::Subscriber sub = nh.subscribe("/scan", 10, &lidarCallback);
    vel_pub = nh.advertise("/cmd_vel", 10);
    ros::spin();
    return 0;
}

IMU的消息包

惯性测量单元，测量空间姿态，首先，右手定则的话是以前为x，上为z，左为y。

这里面的消息格式如图所示，

head 时间戳和fram_id

orientation

angular_velocity #角速度，imu测得的

angular_velocity_covariance #协方差矩阵，按照需求使用

linear_acceleration 线加速度

linear_acceleration_covariance # 协方差矩阵

两个vector3，是六轴imu，也有九轴的。

为了方便使用，空间姿态描述orientation，直接使用就够了，x y z w 旋转偏移量，欧拉角->四元数来描述机器人的空间姿态。

IMU的解析

解析一个数据其实就是两个内容，拿到消息格式和话题名称。

按照ros官方，imu会发布这些话题：

通常就使用sensor_msgs/Imu，imu_data 融合好的四元数姿态描述。

这里将会开始涉及到TF工具的使用。

#include #include #include void imuCallback(sensor_msgs::Imu msg)
{ if (msg.orientation_covariance[0] < 0)
    return;
    tf::Quaternion quaternion(
        msg.orientation.x,
        msg.orientation.y,
        msg.orientation.z,
        msg.orientation.w
    );
    double roll, pitch, yaw;
    tf::Matrix3x3(quaternion).getRPY(roll, pitch, yaw);
    roll = roll*180/M_PI;
    pitch = pitch*180/M_PI;
    yaw = yaw*180/M_PI;
    ROS_WARN("roll:%.0f, pitch:%.0f, yaw:%0f",roll, pitch, yaw);
}
int main(int argc, char *argv[])
{ setlocale(LC_ALL, "");
    ros::init(argc, argv, "imu_node");
    ros::NodeHandle nh;
    ros::Subscriber sub = nh.subscribe("/imu/data", 10, imuCallback);
    ros::spin();
    return 0;
}

IMU航向锁定

思路就是发布cmd_vel，target_yaw，计算误差，然后进行运动控制，通过比例控制实现锁定一个target_yaw运行：

#include #include #include #include ros::Publisher vel_pub;
void imuCallback(sensor_msgs::Imu msg)
{ if (msg.orientation_covariance[0] < 0)
    return;
    tf::Quaternion quaternion(
        msg.orientation.x,
        msg.orientation.y,
        msg.orientation.z,
        msg.orientation.w
    );
    double roll, pitch, yaw;
    tf::Matrix3x3(quaternion).getRPY(roll, pitch, yaw);
    roll = roll*180/M_PI;
    pitch = pitch*180/M_PI;
    yaw = yaw*180/M_PI;
    ROS_WARN("roll:%.0f, pitch:%.0f, yaw:%0f",roll, pitch, yaw);
    double target_yaw = 90;
    double diff_angle = target_yaw - yaw;
    geometry_msgs::Twist vel_msg;
    vel_msg.angular.z = diff_angle * 0.01;
    vel_msg.linear.x = 0.05;
    vel_pub.publish(vel_msg);
}
int main(int argc, char *argv[])
{ setlocale(LC_ALL, "");
    ros::init(argc, argv, "imu_node");
    ros::NodeHandle nh;
    ros::Subscriber sub = nh.subscribe("/imu/data", 10, imuCallback);
    vel_pub = nh.advertise("/cmd_vel", 10);
    
    ros::spin();
    return 0;
}

官方消息格式集中学习

std_msgs

geometry_msgs

sensor_msgs

两大类：

std_msgs

common_msgs–geometry_msgs–sensor_msgs–nav_msgs

geometry_msgs:

sensor_msgs:

can_msgs:

自定义消息类型并以来自定义消息进行通信

这里主要是看怎么修改的cmakelist使编译更加合理，难点在于自定义的msg怎么使用。

栅格地图的发布

map_server发布的nav_msgs::OccupancyGrid，话题是/map

栅格的尺寸越小，越能显示出障碍物情况，栅格地图就是地图的分辨率，默认分辨率0.05m，即5cm。

栅格表示非常有意思，行数列数，0和100，用一维数组表示出来的栅格地图格式。

消息格式：

# This represents a 2-D grid map, in which each cell represents the probability of
# occupancy.
Header header  # 时间戳
#MetaData for the map
MapMetaData info
# The map data, in row-major order, starting with (0,0).  Occupancy
![请添加图片描述](https://img-blog.csdnimg.cn/direct/1a35b4f3b0084e8890b5e77ed9931c11.png)
# probabilities are in the range [0,100].  Unknown is -1.
int8[] data

2D栅格的表示含义。行优先顺序，0~100，障碍物未知则-1

其中的MapMetaDta info：

显示栅格地图：

发布一个4*2,2行4列的地图。

slam建立栅格地图

二维slam的原理：

xxx

slam算法，软件包的使用

hector_slam(单激光雷达)

此时就开始用上他人的软件包来干事了，只需要用launch文件即可，取官方查看接口。

ros的TF系统

地图坐标系，原点在建图起点 /map ——父坐标系

车辆坐标系 /base_footprint ————子坐标系

TF，两个坐标系的空间变换，通过tf发布节点/tf

rviz显示相对位置。

rostopic type 可以直接显示msg信息

这里就说了两个坐标系，地图和车辆

里程计的作用

hector_mapping建图出现问题，定位有问题。————检测到的平直墙面，特征不见了，所以就好像没有移动一样，缺少参照物。

所以引入了里程计odom

运动学知识，通过轮速来计算走过的距离。

通过tf消息包的形式进行发布

odom坐标系，是可以进行定位的，但是也不一定准，所以依然有点云配准算法。修正里程计的误差

odom和车辆坐标系basefootprint链接

slam是map和basefootprint

最终来说，先让map和odom，然后odom再到base_footprint 这就是gmapping的核心算法。

总的来说，这里需要清楚tf系统的意义。

里程计其实就是一个tf，记住哈。

gmapping的使用

进入index.ros.org，搜索gmapping:

API：

订阅的消息：

/tf (tf/tfMessage):laser,base,odom三个坐标系之间的tf，也就是两个tf

和

/scan (sensor_msgs/LaserScan)

发布的：

地图数据：

map #栅格地图数据 nav_msgs/OccupancyGrid

map_metadata #地图参数 nav_msgs/MapMetaDara

entropy #定位置信度 std_msgs/Float64

参数：

rviz，gazebo联合仿真还是很有意思的。

通过launch文件启动gmapping：

新建slam_pkg软件包，创建launch文件夹，launch：

gmapping建图设置参数：

包含参数名字，参数默认值，参数类型

三类参数：

接口和性能，tf坐标系名称的改变。

地图分辨率为0.05m/格，也就是5cm

lskip减少参与建图的线束，跳线处理。一帧数据是运行一周采集的数据

throttle_scans，跳帧处理。

地图更新的参数，

定位时对于算法例消耗的参数。

 #建图范围，限制在3m范围 

地图的加载和保存

终于到地图的处理了，ros中有一个map_server软件包：

订阅的是标准的map (nav_msgs/OccupancyGrid)，

发布的不是很重要

也是去官方查看这个功能包：用map_server中的map_saver

可以保存成pgm地图。首先先用gmapping建立完整的地图，在此基础上，运行保存地图的程序：rosrun map_server map_saver -f map （会保存成map.pgm，–其实就是一个图片文件和map.yaml文件–yaml的解释,地图的参数）：

image: testmap.pnm //地图图片文件

resolution: 0.1 //分辨率

origin: [0.0, 0.0, 0.0] //地图左下角像素坐标 map坐标系原点和地图边界的关系origin位置如图所示。最后一个数据指偏转的角度。

occupied_thresh: 0.65

free_thresh: 0.196

negate: 0

剩下的参数不需要修改，所以无关系。

加载地图：

rosrun map_server map_server mymap.yaml

但是他怎么直到是哪个地图文件呢？——>同一个目录。

发布之后，在通过rviz显示即可。话题默认为map

navigation与move_base

使用创建好的地图来进行导航，这也是最基础的ros开端。

这是ros的导航框架图，有点复杂，我们把他简化一下。

daw动态规划和teb动态规划，全局和局部规划太精彩了，导航失败后标记障碍物，重新找到全局路径

global_planner：全局规划，从map_server获取地图数据global_costmap

根据导航目的地move_base_simple/goal（geometry_msgs/PoseStamped）

规划好的路径internal nav_msgs/Path发送给local_planner，local_planner规划出的局部路径给到底盘base_controller（cmd_vel geometry_msgs/Twist）

这个过程中，需要激光雷达sensor sources 拿到雷达的topic:(sensor_msgs/PointCloud sensor_msgs/LaserScan)

定位使用amcl给出自身位置/tf （tf/tfMessage），里程计提高定位可靠性odemtry source ，odom的nav_msgs/Odometry数据

同时也有规划遇到问题时的重规划：

recovery_behavious，这就是整个导航机制。

对于仿真环境来说，传感器部分都准备好了，就只差move_base，map_server，以及amcl定位了，只要输入目的地就行了。所以move_base订阅的只是这个目的地

不过这里面有大量的调参数据，这里先直接使用了。

准备好环境，然后进行。

nav_pkg nav.launch

launch启动这三个节点：

除了能够使用之外，还有构建出自己的方案来。

有几个非常出众的规划方案，在这里只能先仰望了，因为无论是环境还是c++,还是ros，现在都还不够专精

首先是ego_planner

然后teb_local_planner

然后a_star

还有fast_planner

现在急需一个硬盘再装一个ubuntu18，但是不能着急，先去台式机上测试吧。