ROSARIAを使ってオドメトリを取る(1)

●MobileRobot社のpioneerロボットを使うため、
　ROSARIAからオドメトリ情報を取得する

 http://www.ros.org/wiki/ROSARIA/Tutorials/How%20to%20use%20ROSARIA
※p2osのlanuchファイルがどうしても使えなかったので、ROSARIAを使ってみる

おそらく出来る
しかも、あまりいじらないで出来ると思う

なぜなら、nav_msgs/Odometoryを使っているから。
それを出力するだけでも十分いけるように思う


○ROSARIA odomでググったら

http://www.ist.tugraz.at/robotics/bin/view/Main/Poineer_mapping

また、git-hubで

https://github.com/giorgosera/HCR-project/blob/master/src/KeyBoardController.cpp


○拡張できそうなソースコードが揃った

ROSARIAのパッケージの、RosAriaがあるフォルダに、

theo_key.cpp（キーボード操作）
https://github.com/giorgosera/HCR-project/blob/master/src/KeyBoardController.cpp
reb_teleop.cpp（キーボード操作？）
http://www.ist.tugraz.at/robotics/bin/viewfile/Main/Poineer_mapping?rev=1;filename=rob_teleop.cpp
transform.cpp（odom出力？）
http://www.ist.tugraz.at/robotics/bin/viewfile/Main/Poineer_mapping?rev=1;filename=transform.cpp

を追加


○CMakeLists.txtを開いて、追加

だいたい44行めあたりに（見たらなんとなくわかる）

add_executable(theo_key theo_key.cpp)#
add_dependencies(theo_key ROSARIA_gencfg)#
add_executable(rob_teleop rob_teleop.cpp)#
add_dependencies(rob_teleop ROSARIA_gencfg)#
add_executable(transform transform.cpp)#
add_dependencies(transform ROSARIA_gencfg)#

target_link_libraries(theo_key ${catkin_LIBRARIES} ${Boost_LIBRARIES} Aria pthread dl rt)#
set_target_properties(theo_key PROPERTIES COMPILE_FLAGS "-fPIC")#
target_link_libraries(rob_teleop ${catkin_LIBRARIES} ${Boost_LIBRARIES} Aria pthread dl rt)#
set_target_properties(rob_teleop PROPERTIES COMPILE_FLAGS "-fPIC")#
target_link_libraries(transform ${catkin_LIBRARIES} ${Boost_LIBRARIES} Aria pthread dl rt)#
set_target_properties(transform PROPERTIES COMPILE_FLAGS "-fPIC")#

を追加


○catkin_wsに戻って

$catkin_make

※rob_teleopをコンパイルするとエラーが出てくるので、エラーが出てる箇所を削除してみる
http://answers.ros.org/question/11980/teleop_base-compile-errors-from-boost/


※トピックが違うので変更してあげる（適切に指定しないと動かない）
rob_teleopだいたい82行め

pub_ = n_.advertise<geometry_msgs::Twist>("/cmd_vel",1);
↓
pub_ = n_.advertise<geometry_msgs::Twist>("/RosAria/cmd_vel",1);


※データが古いので以下のような警告がでるかもしれない

警告: ‘tf::Quaternion::Quaternion(const tfScalar&, const tfScalar&, const tfScalar&)’ は廃止されました (宣言位置 /opt/ros/groovy/include/tf/LinearMath/Quaternion.h:51) [-Wdeprecated-declarations]

tf::Quaternion(0, 0, 0)
↓
tf::Quaternion(0, 0, 0, 1)
にしたら直った


○実行してみる

$ roscore
$ rosrun ROSARIA RosAria
$ rosrun ROSARIA reb_teleop

動かないときは、トピックの指定が違っているかも
/cmd_vel→/RosAria/cmd_vel

theo_keyでも動く

$ rosrun ROSARIA theo_key

矢印キーを押したら動く（加速するので注意）
で、次に自己位置データ

$ rosrun ROSARIA transform
[ INFO] [1371822464.617404922]: odometry frame sent
[ INFO] [1371822464.715979980]: odometry frame sent
[ INFO] [1371822464.816018004]: odometry frame sent
[ INFO] [1371822464.915978903]: odometry frame sent
[ INFO] [1371822465.015940424]: odometry frame sent
[ INFO] [1371822465.115958098]: odometry frame sent
[ INFO] [1371822465.215978116]: odometry frame sent
[ INFO] [1371822465.315948702]: odometry frame sent
[ INFO] [1371822465.416017841]: odometry frame sent
[ INFO] [1371822465.515966884]: odometry frame sent
[ INFO] [1371822465.615961342]: odometry frame sent
[ INFO] [1371822465.716013449]: odometry frame sent
[ INFO] [1371822465.815990326]: odometry frame sent
[ INFO] [1371822465.915962577]: odometry frame sent
...
...
...
何かいっぱい出た！
しかし、自己位置を取っているのかは不明

次は、キーボードでロボットを動かして、そのときの自己位置を出力するように改造してみる