ROSのTFへOdometoryを入力する

##目的
ROSのgmappingなどでSLAMを行うためには、
TFで、"odom"→"base_link(or base_footprint)"の発行を行う必要がある。

ROSのLidarSLAMまとめ

##実装
TFで、"odom"→"base_link(or base_footprint)"の発行を行うソースは以下

#include <ros/ros.h>
#include <tf/transform_broadcaster.h>
#include <nav_msgs/Odometry.h>

void odom_calc(double &state_odom_x,double &state_odom_y,double &state_odom_th){
	//ここでエンコーダ値を読み取り、オドメトリ変換する演算を行う。
}

int main(int argc, char **argv){
	tf::TransformBroadcaster odom_broadcaster;
	ros::Time current_time;
	current_time = ros::Time::now();

	ros::Rate r(30.0);
	
	double state_odom_x=0.0;//オドメトリX座標[m]
	double state_odom_y=0.0;//オドメトリY座標[m]
	double state_odom_th=0.0; //オドメトリ姿勢[rad]
	
	while(nh.ok()){
	odom_calc(state_odom_x,state_odom_y,state_odom_th);
	current_time = ros::Time::now();
			
	//tf odom->base_link
	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 = state_odom_x;
	odom_trans.transform.translation.y = state_odom_y;
	odom_trans.transform.translation.z = 0.0;
	geometry_msgs::Quaternion odom_quat = tf::createQuaternionMsgFromYaw(state_odom_th);
	odom_trans.transform.rotation = odom_quat;
	odom_broadcaster.sendTransform(odom_trans);

	ros::spinOnce();
	r.sleep();
	}
}

###確認
rvizでtfが発行されていることを確認する。
座標系があっているかをよく確認する。

##参考：
ROS wiki Publishing Odometry Information over ROS
http://wiki.ros.org/ja/navigation/Tutorials/RobotSetup/Odom