Kinect ROS Gmapping 2D地图

时间 2021-08-15

原文地址：http://blog.csdn.net/u010925447/article/details/56494680

在ROS下手持kinect1构建2D地图(--由于只有一个kinect,没有可移动的底盘或者小车,于是就手持kinect --).

环境及设备为: linux14.04, ROS-indigo.kinect一代.

主要内容:

(一) 准备工作:搭建熟悉ROS系统,

(二) 安装kinect驱动,将深度图转为伪激光图(安装depthimage_to_laserscan包.)

(三) 熟悉理解ROS下构建2d地图原理

(四)代码实现

-------------------------------------------------------------

(一) 准备工作:搭建熟悉ROS系统,

1 熟悉ROS操作系统. 首先练习了两遍wiki的20偏开始文档.了解了工作间的创建,package的创建,node,topic,launch文件,msg,发布和订阅.然后,又下载了一本ROS by example的电子书,跟着做了前七章的内容,对ROS系统更加熟悉. 后面在群里发现一本更适合新手的书:<<机器人操作系统(ROS)浅析 [美] Jason M. O'Kane 著肖军浩译>>,内容相当于是对wiki的20篇开始文档的详解,特别不错.

(二) 安装kinect驱动,将深度图转为伪激光图

2.1 在ros indigo 中安装kinect一代的驱动,当时也折腾了两天,主要是找的博客不太合适....最后还是请求外援才搞定的..后来发现好像也不用那么麻烦.当时主要按照这个博客中的步骤安装的:http://blog.csdn.NET/zqxf123456789/article/details/52015850

2.2安装depthimage_to_laserscan包. 源码编译,注意版本....

$.Git clone --branch indigo-devel https://github.com/ros-perception/depthimage_to_laserscan

$catkin_make

$source devel/setup.bash

$rospack profile

启动命令

$roslaunch openni_launch openni.launch

$rosrun depthimage_to_laserscan depthimage_to_laserscan image:=/camera/depth/image_raw
$rviz

在rviz中Fixed Frame选择camera_depth_optical_frame，Add选择By topic，选择LaserScan，在LaserScan中的Color Transformer中选择AxisColor。

会看到距离近的物体的点是红色，距离远的是紫色。

(三) 熟悉理解ROS下构建2d地图原理

转自知乎大神的回答:链接：https://www.zhihu.com/question/37143050/answer/103899813

ros中常用的2d slam算法主要有gmapping和hector_slam，其中hector_slam是个性能非常好的算法，但是作者在论文中说的很明白，hector_slam通过最小二乘法匹配扫描点，且依赖高精度的激光雷达数据，但是不需要里程计。因此扫描角很小且噪声较大的kinect是不行的，我试过，匹配的时候会陷入局部点，地图非常混乱gmapping是一个比较早的算法，核心思想是粒子滤波并且需要里程计，但并不要求很高性能的传感器，初学者一般都是先来玩这个，不过请注意一点，gmapping只是mapping，定位还需要amcl这个蒙特卡洛算法包配合使用，最后才能接入navigation stack..

gmapping的wiki这样写道

这里说明，这个包需要订阅2个话题，其中scan是激光雷达数据，穷学生买不起激光雷达（去年那会还不知道国产廉价雷达），使用的方法是用ros中的depthimage_to_laserscan包，这个包可以将kinect发布出来的深度图转换成激光雷达扫描数据。

接下来是第二个主题tf，tf是ros中必用的部分，说简单一些，tf类中定义了两个刚体之间的旋转与平移矩阵，并且重载了乘法运算符，这样我们就可以通过相乘两个tf来沿着tf树的方向求末段执行器相对世界坐标的位置与方向

因此，gmapping需要的里程计odom便是通过我们自己发布tf树的形式告诉gmapping，而我们该如何获得这个里程计，这就需要我们自己完成这一部分了，通常做法是在移动平台上安装电机编码器与电子罗盘，在移动平台上的嵌入式

因此，gmapping需要的里程计odom便是通过我们自己发布tf树的形式告诉gmapping，而我们该如何获得这个里程计，这就需要我们自己完成这一部分了，通常做法是在移动平台上安装电机编码器与电子罗盘，在移动平台上的嵌入式单片机（此处与ros无关）内完成里程计的制作与pid调试，然后，再用树莓派连接单片机串口，单片机将里程计与航向角发给树莓派，树莓派上需要自己写一个nod.
     总结如下：
1.单片机与树莓派之前完成数据与控制通信
2.树莓派上写出一个发布tf的node
对没错，如果只是gmapping那么就只需要做这两点！！！

(四)代码实现.
主要是根据古月居的博客 :http://www.guyuehome.com/column/ros-explore ----ros探索总结系列 (十六)到(二十二) .的博客.
最后,写了自己的myodom节点模拟机器人运动,提供gmapping需要的base_link和odom的关系,写了tf_broadcaster节点,发布gmapping需要的tf关系.详细代码如下:

4.1在catkin_ws中创建myodom包,添加 myodom.cpp文件,修改CMakiists.txt,修改package.xml.
myodom.cpp文件为:
#include <ros/ros.h>
#include <tf/transform_broadcaster.h>
#include <nav_msgs/Odometry.h>
int main(int argc, char** argv)
{

      ros::init(argc, argv, "odometry_publisher");

      ros::NodeHandle n;

      ros::Publisher odom_pub = n.advertise<nav_msgs::Odometry>("odom", 50);

      tf::TransformBroadcaster odom_broadcaster;

      double x = 0.0;

      double y = 0.0;

      double th = 0.0;

      double vx = 0.01;

      double vy = -0.01;

      double vth = 0.01;

      ros::Time current_time, last_time;

      current_time = ros::Time::now();

      last_time = ros::Time::now();

      ros::Rate r(1.0);

      while(n.ok())
{

        ros::spinOnce();               // check for incoming messages

        current_time = ros::Time::now();

        //compute odometry in a typical way given the velocities of the robot

        double dt = (current_time - last_time).toSec();

        double delta_x = (vx * cos(th) - vy * sin(th)) * dt;

        double delta_y = (vx * sin(th) + vy * cos(th)) * dt;

        double delta_th = vth * dt;

        x += delta_x;

        y += delta_y;

        th += delta_th;



        //since all odometry is 6DOF we'll need a quaternion created from yaw

        geometry_msgs::Quaternion odom_quat = tf::createQuaternionMsgFromYaw(th);

        //first, we'll publish the transform over tf

        geometry_msgs::TransformStamped odom_trans;

        odom_trans.header.stamp = current_time;

        odom_trans.header.frame_id = "odom";

        odom_trans.child_frame_id = "base_link";

        odom_trans.transform.translation.x = x;

        odom_trans.transform.translation.y = y;

        odom_trans.transform.translation.z = 0.0;

        odom_trans.transform.rotation = odom_quat;

        //send the transform

        odom_broadcaster.sendTransform(odom_trans);



        //next, we'll publish the odometry message over ROS

        nav_msgs::Odometry odom;

        odom.header.stamp = current_time;

        odom.header.frame_id = "odom";



        //set the position

        odom.pose.pose.position.x = x;

        odom.pose.pose.position.y = y;

        odom.pose.pose.position.z = 0.0;

        odom.pose.pose.orientation = odom_quat;



        //set the velocity

        odom.child_frame_id = "base_link";

        odom.twist.twist.linear.x = vx;

        odom.twist.twist.linear.y = vy;

        odom.twist.twist.angular.z = vth;



        //publish the message

        odom_pub.publish(odom);



        last_time = current_time;

        r.sleep();

      }

    }
CmakeList.txt为:
cmake_minimum_required(VERSION 2.8.3)
project(myodom)

find_package(catkin REQUIRED COMPONENTS nav_msgs roscpp tf)

catkin_package( )

include_directories( ${catkin_INCLUDE_DIRS})

add_executable(myodom src/myodom.cpp)
target_link_libraries(myodom ${catkin_LIBRARIES})

package.xml文件内容为:
<?xml version="1.0"?>
<package>
<name>myodom</name>
<version>0.0.0</version>
<description>The myodom package</description>
<maintainer email="[email protected]">zhao</maintainer>
<license>TODO</license>
<buildtool_depend>catkin</buildtool_depend>
<build_depend>nav_msgs</build_depend>
<build_depend>roscpp</build_depend>
<build_depend>tf</build_depend>
<run_depend>nav_msgs</run_depend>
<run_depend>roscpp</run_depend>
<run_depend>tf</run_depend>

<export>
</export>
</package>

----最后catkin_make 得到myodom可运行节点.------
4.2 同样创建settf包,添加tf_broadcaster.cpp .修改相应的CMakeList.txt package.xml.
----tf_broadcaster.cpp文件为:---------(注意修改古月君博客中的base_laser,改为camera_link.有试过改成camera_depth_optical_frame,运行不成功)
#include <ros/ros.h>
#include <tf/transform_broadcaster.h>

    int main(int argc, char** argv)
{

      ros::init(argc, argv, "robot_tf_publisher");

      ros::NodeHandle n;
      ros::Rate r(100);

      tf::TransformBroadcaster broadcaster;

      while(n.ok())
{

        broadcaster.sendTransform(tf::StampedTransform(tf::Transform(tf::Quaternion(0, 0, 0, 1), tf::Vector3(0.1, 0.0, 0.2)),ros::Time::now(),"base_link", "camera_link"));

        r.sleep();

      }

    }
------------同样catkin_make得到可运行节点tf_broadcaster.

4.3 最后运行一系列的节点:
$roscore
$rosrun myodom myodom
$rosrun settf tf_broadcaster
$roslaunch opnni_launch openni.launch
$rosrun depthimage_to_laserscan depthimage_to_laserscan image:=/camera/depth/image_raw
$rosrun gmapping slam_gmapping scan:=scan
$rviz
4.4 最后效果图: