Navigation

# 2015-06-04 AND

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

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

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

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

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

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

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

ターミナル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