====== 3. tfの設定 ======

# 2015-06-04 AND

http://wiki.ros.org/navigation/Tutorials/RobotSetup/TF

ここでは、tfを利用してURGのデータをオドメトリ座標系に変換する。

=====  tfとは =====

座標変換を簡単にやってくれるパッケージ。

詳しくは、 http://wiki.ros.org/tf を参照。

また、 http://wiki.ros.org/tf/Tutorials のチュートリアルはやっておくことをお勧めする。

===== URGのデータをtfに流す =====

URGのデータをtfに流すパッケージを作成する。

====  パッケージを作るのが面倒な人用 ====

{{ :ros:study:sensor_tf_test.tar.gz |sensor_tf_test（tar.gz）}}をダウンロードして、
~/catkin_ws/srcに展開する。

原点を車軸中心とした座標系においてのURGの搭載位置姿勢を自分のロボット用に書き直す（元々M1用の値が入っている）
  $gedit ~/catkin_ws/src/sensor_tf_test/src/sensor_tf_test.cpp

  $ cd ~/catkin_ws
  $ catkin_make
====  自分でパッケージを作りたい人用 ====

  $cd ~/catkin_ws/src
  $catkin_create_pkg sensor_tf_test roscpp tf

原点を車軸中心とした座標系においてのURGの搭載位置姿勢を自分のロボット用に書き直す（下記はM1用）


  $gedit ~/catkin_ws/src/sensor_tf_test/src/sensor_tf_test.cpp

  #include <ros/ros.h>
  #include <tf/transform_broadcaster.h>
  
  int main(int argc, char** argv){
    ros::init(argc, argv, "sensor_tf_test");
    ros::NodeHandle n;
  
    ros::Rate r(100);
  
    tf::TransformBroadcaster broadcaster;
    
    tf::Vector3 URG_position(0.15, 0.0, 0.27); //change to parameter of your URG mounting position
    
    tf::Quaternion quaternion;
    quaternion.setRPY(0, 0, 0);    //if your URG is not reversed, use this program
    //quaternion.setRPY(M_PI, 0, 0); //if your URG is reversed, use this program 
    
    while(n.ok()){
  
      broadcaster.sendTransform(
        tf::StampedTransform(
          tf::Transform(quaternion, URG_position),
          ros::Time::now(),"base_link", "laser"));
  
      r.sleep();
    }
  }

CMakeLists.txtを開き、下記の該当箇所を変更する。
  $gedit ~/catkin_ws/src/sensor_tf_test/CMakeLists.txt

  ###########
  ## Build ##
  ###########
  
  ## Specify additional locations of header files
  ## Your package locations should be listed before other locations
  # include_directories(include)
  include_directories(
    ${catkin_INCLUDE_DIRS}
  )
  
  ## Declare a cpp library
  # add_library(sensor_tf_test
  #   src/${PROJECT_NAME}/sensor_tf_test.cpp
  # )
  
  ## Declare a cpp executable
  add_executable(sensor_tf_test src/sensor_tf_test.cpp)                                                         ←この行
  
  ## Add cmake target dependencies of the executable/library
  ## as an example, message headers may need to be generated before nodes
  # add_dependencies(sensor_tf_test_node sensor_tf_test_generate_messages_cpp)
  
  ## Specify libraries to link a library or executable target against
  target_link_libraries(sensor_tf_test                                                                                            ←この行
    ${catkin_LIBRARIES}                                                                                 　                             ←この行
  )                                                                                                                                                      ←この行

パッケージをmakeする。
  $cd ~/catkin_ws
  $catkin_make

=====  tfの実行 =====

  ターミナル1
  $ roscore
  
  ターミナル2（ロボットとPCを接続しておくこと）
  $ ypspur-coordinator -p your_robot.param -d /dev/ttyACM0
  
  ターミナル3
  $ rosrun ypspur_ros_bridge ypspur_ros_bridge
  
  ターミナル4
  $ rosrun hokuyo_node hokuyo_node _port:=/dev/ttyACM1        または、    $ rosrun hokuyo_node hokuyo_node _port:=/dev/ttyACM1 _min_ang:=-2.08621382713 _max_ang:=2.08621382713
  
  ターミナル5
  $ rosrun sensor_tf_test sensor_tf_test
  
  ターミナル6
  $ rosrun joystick_commander joystick_commander /dev/input/js0
=====  rviz上で確認する =====

  ターミナル7
  $rosrun rviz rviz 

下記の画面のようにして、"/odom"を追加する。

{{:ros:study:fig3-1.png|}}

"Fixed Frame"を"odom"にするとオドメトリのデータと、オドメトリ座標系に変換されたURGのデータが表示される。

{{:ros:study:fig3-2.png|}}

下記の画面を参考に、オドメトリのデータを見やすくする。

{{:ros:study:fig3-3.png|}}

<color red>rvizを閉じる前に、設定を保存しておくこと</color>