传感器信息:
- 环境深度信息:sensor_msgs/laserScan -----> RGBD三维点云数据：通过ros功能包depthimage to laserscan完成深度相机数据转换成激光数据
- 里程计信息:机器人发布的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