通过gmapping和伪造的odom，完成Kinect建图

传感器信息:

1. 环境深度信息:sensor_msgs/laserScan -----> RGBD三维点云数据：通过ros功能包depthimage to laserscan完成深度相机数据转换成激光数据
2. 里程计信息:机器人发布的nav_msgs/Odemetry(pose:x,y,z三轴位置与方向以及矫正误差的协方差矩阵；twist)：通过伪造的节点发布odom数据

发布gmapping需要的传感器信息和里程计消息：

• $ catkin_create_pkg odom_tf_package std_msgs rospy roscpp sensor_msgs tf nav_msgs
• $ touch odom_tf_node.cpp

　　 #include <tf/transform_broadcaster.h>
    #include <nav_msgs/Odometry.h>
    //需要实现“odom”参考系到“base_link”参考系的变换，以及nav_msgs/Odometry消息的发布
 
    int main(int argc, char** argv)
    {
        //定义一个消息发布者来发布“odom”消息，在定义一个tf广播，来发布tf变换信息
        ros::init(argc, argv, "odometry_publisher");
        ros::NodeHandle n;
        ros::Publisher odom_pub = n.advertise<nav_msgs::Odometry>("odom", );
        tf::TransformBroadcaster odom_broadcaster;
 
        // 默认机器人的起始位置是odom参考系下的0点
        double x = 0.0;
        double y = 0.0;
        double th = 0.0;
        // 让机器人的base_link参考系在odom参考系下以x轴方向0.1m/s，Y轴速度-0.1m/s，角速度0.1rad/s的状态移动
        double vx = 0.1;
        double vy = -0.1;
        double vth = 0.1;
 
        ros::Time current_time, last_time;
        current_time = ros::Time::now();
        last_time = ros::Time::now();
        //使用1Hz的频率发布odom消息，在实际系统中，往往需要更快的速度进行发布
        ros::Rate r(1.0);
        while(n.ok())
        {
        ros::spinOnce();
        current_time = ros::Time::now();
 
        //积分计算里程计信息
        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;
 
        //为了兼容二维和三维的功能包，让消息结构更加通用，里程计的偏航角需要转换成四元数才能发布，辛运的是，ROS为我们提供了偏航角与四元数相互转换的功能
        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();
 
        }
    }

编译源码：在odom_tf_package/CMakeLists.txt添加编译选项：

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

在odom_tf_package中添加launch文件：

• mkdir -p launch
• cd launch
• touch gamppping_slam.launch添加代码如下：

  <launch>
   
  <node pkg="tf" type="static_transform_publisher" name="base_to_laser_broadcaster" args="0 0 0 0 0 0 /base_link /laser_frame 100" />
  <node pkg="tf" type="static_transform_publisher" name="base_footprint_2_base_link" args="0 0 0 0 0 0 /odom /base_link 100"/>
  <node pkg="tf" type="static_transform_publisher" name="base_link_2_base_stabilized_link" args="0 0 0 0 0 0 /base_link /camera_link 100"/>
   
  </launch>

启动kinect相机：$ roslaunch freenect_launch freenect.launch

完成深度信息转换成/scan信息：$ rosrun depthimage_to_laserscan depthimage_to_laserscan image:=/camera/depth/image_raw

发布Odom信息：$ rosrun odom_tf_package odom_tf_node

启动launch：$ roslaunch odom_tf_package gmapping_slam.launch

启动gmapping建图：$ rosrun gmapping slam_gmapping scan:=scan

在rviz中观察建图过程：$ rviz

查看tree图 ：$ rosrun rqt_tf_tree rqt_tf_tree