传感器信息:
- 环境深度信息: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", 50);
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