RefBoxのROS対応

とりあえずRobotino周りをROSに対応させたいなぁという野望の第一弾.
と思ったけど,すでにTimが作業していた….

cd git
git clone https://github.com/timn/ros-rcll_ros_msgs
git clone https://github.com/timn/ros-rcll_refbox_peer

ros-rcll_refbox_peer が,どうやらFawkes がインストールされていることを前提としているっぽいです.
protobuff_comm/peer.h が見つからないエラーに関しては,CMakeLists.txt にrcll-refbox のディレクトリ情報を追加.
Fawkes_INCLUDE_DIRS に加えときました.

elseif (RCLLRefbox_FOUND)
  list(APPEND _LIBRARIES "llsf_msgs")
  list(APPEND Fawkes_INCLUDE_DIRS "~/rcll-refbox/src/libs/")

	# Use Fawkes' protobuf_comm
  set_target_properties(rcll_refbox_peer PROPERTIES
    INCLUDE_DIRECTORIES "${Fawkes_INCLUDE_DIRS};${catkin_INCLUDE_DIRS}"

一番面倒くさそうなのが,llsf_msgs を必要としている部分で,rcll-refbox/src/msgs に置き換えるのが回避策なのかなぁ.

ryukoku@NUC8i7BEH:~/git/ros-rcll_refbox_peer$ grep fawkes-robotino * -R
cmake/FindFawkes_llsf_msgs.cmake:# - Fawkes: check for llsf_msgs (fawkes-robotino only)
cmake/FindRCLLRefbox_llsf_msgs.cmake:# - RCLLRefbox: check for llsf_msgs (fawkes-robotino only)
cmake/FindFawkes.cmake:      PATHS $ENV{HOME}/fawkes $ENV{HOME}/fawkes-robotino $ENV{HOME}/fawkes-athome
cmake/FindFawkes.cmake:            $ENV{HOME}/robotics/fawkes $ENV{HOME}/robotics/fawkes-robotino
cmake/FindFawkes.cmake:            $ENV{HOME}/robotics/fawkes-athome /opt/fawkes-robotino

こいつらをつぶしていく必要があります.

ln -s ~/rcll-refbox/src/msgs ~/rcll-refbox/src/libs/llsf_msgs

とりあえずは,これでええか.
次は,このros版は2017年で更新が止まっているので,2020年版に対応させる必要があります.
一旦,休憩(´-ω-`)


— 2021/05/21 再開 —


エラーを順に見ていきましょうか.

[ 98%] Building CXX object ros-rcll_refbox_peer/CMakeFiles/rcll_refbox_peer.dir/src/rcll_refbox_peer_node.cpp.o
/home/robotino/catkin_ws/src/ros-rcll_refbox_peer/src/rcll_refbox_peer_node.cpp  In function ‘bool srv_cb_send_machine_report(rcll_ros_msgs::SendMachineReport::Request&, rcll_ros_msgs::SendMachineReport::Response&)’:
/home/robotino/catkin_ws/src/ros-rcll_refbox_peer/src/rcll_refbox_peer_node.cpp:439:8: error: ‘class llsf_msgs::MachineReportEntry’ has no member named ‘set_type’; did you mean ‘set_name’?
   mre->set_type(rmre.type);
        ^~~~~~~~
        set_name

~/rcll-refbox/src/libs/llsf_msgs/MachineReport.proto を見ると,

message MachineReportEntry {
  enum CompType {
    COMP_ID  = 2000;
    MSG_TYPE = 60;
  }

  // Machine name and recognized type
  // and zone the machine is in
  required string name = 1;
  optional Zone   zone = 3;
  optional uint32 rotation = 4; // [0-360] in deg
}

となっていて,typeが消えていますね.
~/rcll-refbox/src/llsf-report-machine.cpp を見ると,

                       entry->set_name(machine_name_);

となっていて,nameだけを設定しています.
では,machine_name_ は何かというと,

        ArgumentParser argp(argc, argv, "T:r:z:");
...
        team_name_    = argp.items()[0];
        machine_name_ = argp.items()[1];

となっていて,引数「r」の文字列が入っているっぽいです.
r?rotation?角度?
あとで,手動で試してみる必要がありそうです.
今のところ,mre->set_type(rmre.type);をコメントアウトしてみます.

// mre->set_type(rmre.type);

次のエラーは,

[ 98%] Building CXX object ros-rcll_refbox_peer/CMakeFiles/rcll_refbox_peer.dir/src/rcll_refbox_peer_node.cpp.o
/home/robotino/catkin_ws/src/ros-rcll_refbox_peer/src/rcll_refbox_peer_node.cpp  In function ‘bool srv_cb_send_prepare_machine(rcll_ros_msgs::SendPrepareMachine::Request&, rcll_ros_msgs::SendPrepareMachine::Response&)’:
/home/robotino/catkin_ws/src/ros-rcll_refbox_peer/src/rcll_refbox_peer_node.cpp:526:8: error: ‘class llsf_msgs::PrepareInstructionDS’ has no member named ‘set_gate’
   dsi->set_gate(req.ds_gate);

set_gate が存在しないよってところですね.
PrepareInstructionDS のメッセージです.
~/rcll-refbox/src/libs/llsf_msgs/MachineInstructions.proto を見てみましょう.

message PrepareInstructionDS {
  reserved "gate";
  reserved 1;
  required uint32 order_id = 2;
}

reserved は,過去との整合性のために残しているけど,今は使っていないよ的な状態だったと思います.
~/rcll-refbox/src/tools/rcll-prepare-machine.cpp を見てみます.

                        } else if (machine_type_ == "DS") {
                                llsf_msgs::PrepareInstructionDS *prep_ds = prep.mutable_instruction_ds();
                                prep_ds->set_order_id(ds_order_id_);

DSへのコマンドは,orser_id を送るだけですね.
これは,オーダー情報にgateが含まれているから,不要になったという流れですね.
これらの定義は,src/ros-rcll_ros_msgs/srv/SendPrepareMachine.srv にあるので,

# DS: gate/slide to deliver onto
# uint32 ds_gate
uint32 ds_order_id
  // dsi->set_gate(req.ds_gate);
  dsi->set_order_id(req.ds_order_id);

としてみますか.
※これも,中身が合っているのかどうか,あとで確認が必要.
次はなんだろ.

[ 98%] Building CXX object ros-rcll_refbox_peer/CMakeFiles/rcll_refbox_peer.dir/src/rcll_refbox_peer_node.cpp.o
/home/robotino/catkin_ws/src/ros-rcll_refbox_peer/src/rcll_refbox_peer_node.cpp  In function ‘bool srv_cb_send_prepare_machine(rcll_ros_msgs::SendPrepareMachine::Request&, rcll_ros_msgs::SendPrepareMachine::Response&)’:
/home/robotino/catkin_ws/src/ros-rcll_refbox_peer/src/rcll_refbox_peer_node.cpp:529:20: error: ‘CsOp_IsValid’ is not a member of ‘llsf_msgs’
   if (! llsf_msgs::CsOp_IsValid(req.cs_operation)) {
                    ^~~~~~~~~~~~
/home/robotino/catkin_ws/src/ros-rcll_refbox_peer/src/rcll_refbox_peer_node.cpp:529:20: note: suggested alternative: ‘CSOp_IsValid’
   if (! llsf_msgs::CsOp_IsValid(req.cs_operation)) {
                    ^~~~~~~~~~~~
                    CSOp_IsValid

~/rcll-refbox/src/libs/llsf_msgs/MachineInstructions.proto には,

message PrepareInstructionCS {
  required CSOp operation = 1;
}

とありますね.
ん.
CsOp ではなく,CSOp?
その次のエラーも同様ですね.
最後は,

[100%] Linking CXX executable /home/robotino/catkin_ws/devel/lib/rcll_refbox_peer/rcll_refbox_peer
CMakeFiles/rcll_refbox_peer.dir/src/rcll_refbox_peer_node.cpp.o: In function `setup_private_peer(llsf_msgs::Team)':
rcll_refbox_peer_node.cpp:(.text+0x1b4c): undefined reference to `protobuf_comm::ProtobufBroadcastPeer::~ProtobufBroadcastPeer()'

protobuf_commへのリンクのエラー?
関係しそうなライブラリのファイルを追加.
最後のAPPEND は,Ubuntu 18.04の場合です.
Fedora 33の場合は「list(APPEND Fawkes_LIBRARIES “/usr/lib64/libprotobuf.so”)」でOKです.

elseif (RCLLRefbox_FOUND)
  list(APPEND _LIBRARIES "llsf_msgs")
  list(APPEND Fawkes_INCLUDE_DIRS "~/rcll-refbox/src/libs/")
  list(APPEND Fawkes_LIBRARIES "~/rcll-refbox/lib/libllsf_protobuf_comm.so")
  list(APPEND Fawkes_LIBRARIES "~/rcll-refbox/lib/protobuf/libllsf_msgs.so")
  list(APPEND Fawkes_LIBRARIES "~/rcll-refbox/lib/protobuf/libllsf_log_msgs.so")
  list(APPEND Fawkes_LIBRARIES "~/rcll-refbox/lib/libmps_comm.so")
  list(APPEND Fawkes_LIBRARIES "/usr/lib/x86_64-linux-gnu/libprotobuf.so")

        # Use Fawkes' protobuf_comm
  set_target_properties(rcll_refbox_peer PROPERTIES
    INCLUDE_DIRECTORIES "${Fawkes_INCLUDE_DIRS};${catkin_INCLUDE_DIRS}"
    COMPILE_FLAGS "${Fawkes_CFLAGS} ${catkin_CFLAGS}"
    LINK_FLAGS "${Fawkes_LFLAGS}"
    LINK_LIBRARIES "${Fawkes_LIBRARIES};${catkin_LIBRARIES}"
    INSTALL_RPATH "${Fawkes_LIBRARY_DIRS}"
    INSTALL_RPATH_USE_LINK_PATH TRUE
    BUILD_WITH_INSTALL_RPATH TRUE)
  message(STATUS "Using Fawkes protobuf_comm by RCLLRefBox")
else()

これで,とりあえず,catkin_make が通りました.
catkin_ws で動作させるので,source ~/catkin_ws/devel/setup.bash の実行を忘れないように!

roslaunch rcll_refbox_peer.launch num_robots:=1 team_name:=BabyTigers robot_name:=kotora robot_number:=1 crypto_key:=randomkey

(2021/06/01追記)
※launchでの起動は,上記のように引数として与える方が素直ですね.
↓ 以下では,試行錯誤的にやっていたので,launchファイルを直接書き換えて動作確認しています.
上記の引数方式に読み替えて読んで下さいませませ.

~/catkin_ws/src/ros-rcll_refbox_peer/launch/rcll_refbox_peer.launch に,ネットワークやロボットの設定を書きます.
とりあえず,babyTigers-R のテスト機の設定.

robotino@robotino:~/catkin_ws/src/ros-rcll_refbox_peer/launch$ cat rcll_refbox_peer.launch |head -n 17
<launch>
        <arg name="num_robots" default="1"/>

        <arg name="team_name" default="BabyTigers"/>
        <arg name="team_color" default="CYAN" />
        <arg name="robot_name" default="kotora"/>
        <arg name="robot_number" default="1"/>
        <arg name="crypto_key" default="randomkey"/>
        <arg name="crypto_cipher" default="aes-128-cbc"/>
        <arg name="peer_address" default="192.168.17.250" />
        <arg name="peer_public_recv_port" default="4444" />
        <arg name="peer_public_send_port" default="4444" />
        <arg name="peer_cyan_recv_port" default="4446" />
        <arg name="peer_cyan_send_port" default="4441" />
        <arg name="peer_magenta_recv_port" default="4447" />
        <arg name="peer_magenta_send_port" default="4442" />

これで,roslaunch で実行.

roslaunch rcll_refbox_peer.launch

roscoreが動き出し,

process[rcll_refbox_peer_1-1]: started with pid [21106]
[ INFO] [1621587212.611988118]: /rcll_refbox_peer_1 starting up
[ WARN] [1621587212.634310732]: Creating public peer: addr: 192.168.17.250  send: 4444  recv: 4444
[ INFO] [1621587212.886042140]: Creating private peer for CYAN
[ INFO] [1621587277.446321287]: Referee Box 2019 detected

つながりました!

robotino@robotino:~/catkin_ws/src/ros-rcll_refbox_peer/launch$ rostopic list
/rcll/beacon
/rcll/exploration_info
/rcll/game_state
/rcll/machine_info
/rcll/machine_report_info
/rcll/order_info
/rcll/ring_info
/rosout
/rosout_agg
/statistics

ROSでのやりとりは,上記の通りです.

rostopic echo /rcll/game_state

これで,試合時間(game_time)や状況(stateとphase),得点(points_cyan)などがわかります.

rostopic pub -1 /rcll/beacon rcll_ros_msgs/BeaconSignal -- "[1,1,1]" 1 "BabyTigers" Robot1" "CYAN" "[[2,2,2],[[10,10,0],[0,0,0,0]]]"

とりあえず,publish はしてくれるっぽい.
ただ,refbox には表示されない….とほほ.

robotino@robotino:~/rcll-refbox/bin$ rosnode info rcll_refbox_peer_1
--------------------------------------------------------------------------------
Node [/rcll_refbox_peer_1]
Publications:
 * /rcll/beacon [rcll_ros_msgs/BeaconSignal]
 * /rcll/exploration_info [rcll_ros_msgs/ExplorationInfo]
 * /rcll/game_state [rcll_ros_msgs/GameState]
 * /rcll/machine_info [rcll_ros_msgs/MachineInfo]
 * /rcll/machine_report_info [rcll_ros_msgs/MachineReportInfo]
 * /rcll/order_info [rcll_ros_msgs/OrderInfo]
 * /rcll/ring_info [rcll_ros_msgs/RingInfo]
 * /rosout [rosgraph_msgs/Log]

Subscriptions: None

Services:
 * /rcll/send_beacon
 * /rcll/send_machine_report
 * /rcll/send_prepare_machine
 * /rcll_refbox_peer_1/get_loggers
 * /rcll_refbox_peer_1/set_logger_level

あ,rcll/beaconではなく,rcll/send_beaconに渡さないといけないのかな.
あと,設定のポート番号も,微妙に間違えている気がする.
とりあえず,今日はここまでにします.

rosservice を使うことが判明!

robotino@robotino:~/rcll-refbox/bin$ rosservice call /rcll/send_beacon "[1,1,"btr"]" "[[2,2,"btr"],[[10,10,0],[0,0,0,0]]]"

これで,RefBox にRobotが表示されました!やったね.

コメントを残す

メールアドレスが公開されることはありません。 * が付いている欄は必須項目です

*