傳感器信息:
- 環境深度信息: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