04-ros1-occupancy_grid_map

Ros ros slam occupancy_grid_map

发布时间 : 2025-03-10 09:53

字数:2.4k 阅读 :

ROS中的栅格地图

地图数据格式

ROS中默认的栅格为 0.05m

栅格地图转换为一个 二维数组 —–> 一维数组

nav_msgs

这种数组 + 行列数消息便是 ros中 map_server 中 nav_msgs

header
- seq
- stamp
- frame_id
info 栅格地图信息
- load_time
- resolution
- width
- height
[]data 一维数组
- [0,0] 位置是栅格地图 左下角 的位置

发布地图消息包

栅格地图样式

C++实现

map_pub_node.cpp

创建软件包

catkin_create_pkg map_pkg rospy roscpp nav_msgs

map_pub_node.cpp

#include <ros/ros.h>
#include <nav_msgs/OccupancyGrid.h>

int main(int argc, char *argv[])
{
    ros::init(argc,argv,"map_pub_node");
    ros::NodeHandle n;
    ros::Publisher pub = n.advertise<nav_msgs::OccupancyGrid>("/map",10);

    ros::Rate r(1);

    while (ros::ok())
    {
        nav_msgs::OccupancyGrid msg;
        msg.header.frame_id = "map";
        // 设置时间
        msg.header.stamp = ros::Time::now();
        
        // 设置起始点
        msg.info.origin.position.x = 0,
        msg.info.origin.position.y = 0,
        // 设置地图尺寸
        msg.info.resolution = 1.0;
        msg.info.width = 4;
        msg.info.height = 2;

        msg.data.resize(4*2);
        msg.data[0]= 100;
        msg.data[1]= 100;
        msg.data[2]= 0;
        msg.data[3]= -1;
        pub.publish(msg);
        ROS_INFO("RUNNING!");
        r.sleep();
    }   
    return 0;
}

CmakeList.txt

末尾添加上

add_executable(map_pub_node src/map_pub_node.cpp)
add_dependencies(map_pub_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
target_link_libraries(map_pub_node
  ${catkin_LIBRARIES}
)

运行效果

Python实现

map_pub_node.py

#!/usr/bin/env python3
# coding = utf-8
import rospy
from nav_msgs.msg import OccupancyGrid

def main():
    rospy.init_node("map_pub_node")
    pub = rospy.Publisher("/map",OccupancyGrid,queue_size=10)
    rate = rospy.Rate(1) # 1hz
    while not rospy.is_shutdown():
        msg = OccupancyGrid()

        msg.header.frame_id = "map"
        msg.header.stamp = rospy.Time.now()
        # 地图描述信息
        msg.info.origin.position.x = 0
        msg.info.origin.position.y = 0
        msg.info.resolution = 1.0 # 地图分辨率
        msg.info.width = 4
        msg.info.height = 2
        # 地图数据的赋值
        msg.data =[0]*4*2
        msg.data[0] = 100
        msg.data[1] = 100
        msg.data[2] = 0
        msg.data[3] = -1

        pub.publish(msg)
        rospy.loginfo("running")
        rate.sleep()


if __name__ == "__main__":
    main()

SLAM

SLAM —-> Simultaneous Localization And Mapping

同时定位与地图创建

Localization && Mapping

Hector_Mapping 年轻人的第一次SLAM建图

SLAM节点，我们直接复用别人的 SLAM ，等我们学会了再去研究这个节点写吧！！！复用！

下载与启动仿真环境

Hector_mapping 下载

sudo apt install ros-noetic-hector-mapping

roslaunch wpr_simulation wpb_stage_slam.launch

启动slam建图程序

rosrun hector_mapping hector_mapping

启动 rviz 程序

rviz

启动控制程序

rosrun rqt_robot_steering rqt_robot_steering

使用控制程序来控制其运动

通过launch文件来启动Hector_Mapping建图

创建 slam_pkg 包

catkin_create_pkg slam_pkg roscpp rospy std_msgs

将这些指令全部写入到一个launch文件中

hector.launch

<launch>
    <include file="$(find wpr_simulation)/launch/wpb_stage_slam.launch" />

    <node pkg="hector_mapping" type="hector_mapping" name="hector_mapping" />

    <node pkg="rviz" type="rviz" name="rviz" />

    <node pkg="rqt_robot_steering" type="rqt_robot_steering" name="rqt_robot_steering" />
</launch>

"$(find wpr_simulation)"

编译

编译的目的是为了让这个 slam_launch 文件进入 ros工作环境

保存 `rviz` 配置

在rviz下，选择 save config as

存储到

~/catkin_ws/src/slam_pkg/rviz/

rviz 加载配置信息启动

rosrun rviz rviz -d /home/ysc/catkin_ws/src/slam_pkg/rviz/slam.rviz

修改 launch信息

hector.launch

<launch>
    <!-- 载入机器人 和 SLAM 的仿真场景  -->
    <!-- 实体机器人就启动实体的启动程序 -->
    <include file="$(find wpr_simulation)/launch/wpb_stage_slam.launch" />

    <!-- Hector SLAM -->
    <node pkg="hector_mapping" type="hector_mapping" name="hector_mapping" />

    <!-- Rviz 显示 -->
    <node pkg="rviz" type="rviz" name="rviz" args="-d $(find slam_pkg)/rviz/slam.rviz" />

    <!-- 运动控制 -->
    <node pkg="rqt_robot_steering" type="rqt_robot_steering" name="rqt_robot_steering" />
</launch>

Hector_Mapping 参数

我们修改这几个参数

修改 hector.launch

<launch>
    <!-- 载入机器人 和 SLAM 的仿真场景  -->
    <!-- 实体机器人就启动实体的启动程序 -->
    <include file="$(find wpr_simulation)/launch/wpb_stage_slam.launch" />

    <!-- Hector SLAM -->
    <node pkg="hector_mapping" type="hector_mapping" name="hector_mapping" output="screen">
        <param name="map_update_distance_thresh" value="0.1" />
        <param name="map_update_angle_thresh" value="0.1" />
        <param name="map_pub_period" value="0.1" />
    </node>

    <!-- Rviz 显示 -->
    <node pkg="rviz" type="rviz" name="rviz" args="-d $(find slam_pkg)/rviz/slam.rviz" />

    <!-- 运动控制 -->
    <node pkg="rqt_robot_steering" type="rqt_robot_steering" name="rqt_robot_steering" />
</launch>

怎么看出差距

比较一下呗

创建一个 companies.launch

<launch>

  <!-- 第一个 Hector_Mapping 建图节点 -->
  <group ns="slam_1">
    <node pkg="hector_mapping" type="hector_mapping" name="hector_mapping_1">

      <param name="map_update_distance_thresh" value="0.5"/>
      <param name="map_update_angle_thresh" value="0.5" />
      <param name="map_pub_period" value="0.2" />
      
      <param name="map_frame" value="slam_1/map" />
      <param name="base_frame" value="slam_1/base_footprint" />
      <param name="odom_frame" value="slam_1/odom" />
    </node>
  </group>

  <!-- 第二个 Hector_Mapping 建图节点 -->
  <group ns="slam_2">
    <node pkg="hector_mapping" type="hector_mapping" name="hector_mapping_2">

      <param name="map_update_distance_thresh" value="0.1"/>
      <param name="map_update_angle_thresh" value="0.1" />
      <param name="map_pub_period" value="0.2" />

      <param name="map_frame" value="slam_2/map" />
      <param name="base_frame" value="slam_2/base_footprint" />
      <param name="odom_frame" value="slam_2/odom" />
    </node>
  </group>

  <!-- **************************** 分割线 **************************** -->

  <!-- 载入 SLAM 的仿真场景 -->
  <include file="$(find gazebo_ros)/launch/empty_world.launch">
  <arg name="world_name" value="$(find wpr_simulation)/worlds/slam_simple.world"/>
  <arg name="paused" value="false"/>
  <arg name="use_sim_time" value="true"/>
  <arg name="gui" value="true"/>
  <arg name="recording" value="false"/>
  <arg name="debug" value="false"/>
  </include>

  <!-- 载入 1号机器人 -->
  <include file="$(find wpr_simulation)/launch/wpb_slam_template.launch">
      <arg name="robot_namespace" value="slam_1" /> 
      <arg name="local_x" value="0" /> 
      <arg name="local_y" value="-0.3" /> 
      <arg name="local_yaw" value="0" /> 
  </include>

  <!-- 载入 2号机器人 -->
  <include file="$(find wpr_simulation)/launch/wpb_slam_template.launch">
      <arg name="robot_namespace" value="slam_2" /> 
      <arg name="local_x" value="0" /> 
      <arg name="local_y" value="0.3" /> 
      <arg name="local_yaw" value="0" /> 
  </include>

  <!-- 运动控制 -->
  <node pkg="rqt_robot_steering" type="rqt_robot_steering" name="rqt_robot_steering"/>

  <!-- 速度话题分流 -->
  <node pkg = "topic_tools" type = "relay" name = "relay_1" args="/cmd_vel /slam_1/cmd_vel" />
  <node pkg = "topic_tools" type = "relay" name = "relay_2" args="/cmd_vel /slam_2/cmd_vel" />

</launch>

定位

机器人地盘的节点

`base_footprint·
- 约定俗成的节点，发布机器人的地盘信息

父子坐标系

坐标系起始点为机器人 初始位置

位置描述方式

距离
角度

TF [TransForm]

Transform 坐标系变换

TF数据格式

Header
- seq
- stamp
- frame_id 父坐标名称
child_frame_id 子坐标系名称
Transform
- translation
  
  表示 子坐标系 相对于 父坐标系 的坐标距离偏差量
  - x
  - y
  - z
- rotation 四元数据格式 —->
  
  表示 子坐标系 相对于 父坐标系 的角度偏差值
  - x
  - y
  - z
  - w

查看 `/tf` 话题

rostopic echo /tf

查看各个tf的父子关系

tf tree

rosrun rqt_tf_tree rqt_tf_tree

里程计 odom

一种算法

减少单一SLAM 误差问题

开启一个仿真程序

roslaunch wpr_simulation wpb_corridor_hector.launch

这个建图会出现 机器人还在走，但是 rviz 图不更新了

激光雷达每一帧都完全相同，无法估计自己的位移

无法找到自己的区别

Gmapping 算法

使用 odom 【里程计】
误差使用 odom 来修正

启动仿真环境

roslaunch wpr_simulation wpb_corridor_gmapping.launch

里程计只是一套软件

理论值和实际值有区别！！！

轮子会打滑！！

障碍物点云配准算法

修正

Hector_mapping 算法

不考虑里程计的数据
odom是假里程计
为了符合rviz，勉为其难得加了个odom得 tf 节点

odom 在 Hector_mapping 算法下会不断往后增长

为了抵消 scanmatcher_frame 的坐标

只考虑了 RViz中显示的情况

如何使用Gampping进行SLAM建图

需求

雷达坐标系 —> base_link
base_link —> odom
/scan （话题）

启动仿真环境

roslaunch wpr_simulation wpb_stage_robocup.launch

这样的场景

查看依赖

终端输入

查看消息话题

rostopic list

查看tf 树

rosrun rqt_tf_tree rqt_tf_tree

都符合需求

启动Gampping节点

rosrun gmapping slam_gmapping

启动 rviz

运行键盘控制机器人节点

rosrun wpr_simulation keyboard_vel_ctrl

编写launch文件一键启动Gampping建图

创建文件 slam_pkg

catkin_create_pkg slam_pkg roscpp rospy std_msgs

参数设置

栅格默认 0.05m

lskip 跳线

每隔多少个采纳一条射线—-> 降低算力消耗

throttle_scans = 1

选择第几帧处理，剩下全部抛弃，转几圈处理一次

gampping.launch 内容

<launch>
    <!-- 载入机器人 和 SLAM 的仿真场景  -->
    <!-- 实体机器人就启动实体的启动程序 -->
    <include file="$(find wpr_simulation)/launch/wpb_stage_robocup.launch" />

    <!-- Gamppping SLAM -->
    <node pkg="gmapping" type="slam_gmapping" name="slam_gmapping" />

    <!-- 运动控制 -->
    <node pkg="wpr_simulation" type="keyboard_vel_ctrl" name="keyboard_vel_ctrl" launch-prefix="gnome-terminal -e">
        <param name="maxUrange" value="3.0"/>
        <param name="map_update_interval" value="0.5"/>
        <param name="linearUpdate" value="0.1"/>
    </node>

    <!-- Rviz 显示 -->
    <node pkg="rviz" type="rviz" name="rviz" args="-d $(find slam_pkg)/rviz/slam.rviz" />
</launch>

如何保存建立好的地图

map_server

将节点保存到地图里

rosrun map_server map_server -f 【文件名称】

地图文件就长这样

map.pgm
map.yaml
- 解释文件
- origin 相对于 map初始位置的偏移
  - [-100.000000,-100.000000,0.000000] x,y,偏移角

加载地图

在终端上运行，map.yaml 文件里

rosrun map_server map_server map.yaml

直接载入成功

04-ros1-occupancy_grid_map

ROS中的栅格地图

地图数据格式

nav_msgs

发布地图消息包

C++实现

map_pub_node.cpp

CmakeList.txt

运行效果

Python实现

map_pub_node.py

SLAM

Hector_Mapping 年轻人的第一次SLAM建图

下载 与 启动仿真环境

通过launch文件来启动Hector_Mapping建图

保存 rviz 配置

修改 launch信息

Hector_Mapping 参数

怎么看出差距

定位

TF [TransForm]

TF数据格式

查看 /tf 话题

查看各个tf的父子关系

里程计 odom

Gmapping 算法

Hector_mapping 算法

如何使用Gampping进行SLAM建图

启动仿真环境

查看依赖

启动Gampping节点

编写launch文件一键启动Gampping建图

参数设置

如何保存建立好的地图

map_server

加载地图

下载与启动仿真环境

保存 `rviz` 配置

查看 `/tf` 话题