ChoreonoidをROSで使う場合に，外部のノードからカメラのオン・オフを切り替えられないでしょうか？RTミドルウェアのVisionSensorIoRTC.cppを使う場合には，「カメラ名_switch」という入力データポートが設けられ，ここにブール値を送ることによってカメラのオン・オフを切り替えていました．同じようなことをROSでやるにはどうすればいいでしょうか？
こちらについても現状ではシンプルコントローラを用いて自前で実装するのが早いかと思います。
やり方としてはトピックを用いる方法とサービスを用いる方法が考えられます。
トピックの場合は操作側でトピックを生成してオン／オフの状態をPublishするようにしておき、シンプルコントローラ側でトピックをSubscribeして、そのオン／オフ状態をカメラのオン／オフに反映させます。
サービスの場合は、シンプルコントローラ側でサービスを生成しておき、操作側でそのサービスに接続して操作します。
いずれの場合も、メッセージ型としてはboolがひとつあれば足りると思うので、std_msgs/Bool型などを使えるのではないかと思います。これは
rosmsg show std_msgs/Bool
で確認できます。
このケースではどちらかと言えばサービスの方がマッチしているかもしれませんね。でもどちらでも実現は可能だと思います。
「ROS版Tankチュートリアル」ではトピックの例しか紹介していませんが、サービスについても同様の方法で実装できるかと思います。シンプルコントローラでroscppの関数を使う方法はこのチュートリアルで分かるかと思うので、あとはroscppをどう使うかという問題になりますので。そちらはroscppのマニュアルもあたってみてください。
https://choreonoid.org/ja/documents/latest/ros/tank-tutorial/index.html
返信ありがとうございます．
画像情報をパブリッシュするにはBodyROSItemを使い，複数のカメラのオン・オフを行いたいので，BodyROSItemにコードを追加しようと考えています．BodyROSItem::createSensors()の中でサービスの登録を行い，そのコールバックの中でCameraクラスのメンバ関数on()を呼び出しても大丈夫でしょうか？
BodyRosItemにセンサのon/offの機能を追加するというご提案に賛成します。
私の方でも同様の機能の追加を検討をしておりました。
@masutani 先生 が仰った仕様で問題ないかと思います。2週間お待ちいただけるのであればこちらで実装します。もしそちらの開発の方が早く、かつコード公開に差し支えがなければ、Githubにてプルリクエストを投げていただければと思います。
よろしくお願いします。
1 Like
大西さんありがとうございます。
先日の相談結果を踏まえた方針について、ここにまとめておきます。
- 各センサ／デバイスごとにstd_msgs/Bool型のROSサービスを生成する
- デフォルトで全てオフ or オンにするオプションをBodyROSItemのプロパティで設定できるようにする
なおオプションの件については、何も設定しなければオフにしようかという話でしたが、何も設定しなければシミュレーション開始時の状態（読み込まれているBodyItemの元々の状態）がよいかと思いました。オン／オフはBodyファイルでも初期状態を記述できるので、そちらを優先した方がよいかと。現状のBodyROSItemではそうなっているはずです。
その上で、BodyROSItemに「デバイスの有効化／無効化」といったプロパティを追加し、選択肢として「なし（何もしない）」、「全て有効化」、「全て無効化」から選択できるとよいかと思います。デフォルトは「なし」とします。
以上ご検討いただければ幸いです。
よろしくお願いします。
1 Like
お待たせして申し訳ありません．実装が予想以上に難航しておりました．
以下のプルリクエストにて追加しております．当該のブランチに切り替えていただければ，すぐにお手元で試せるようになるかと思います．
こちらの手元ではよく動いているように見えますが，何かバグ等発見されましたらお知らせください．サービス名への意見も歓迎です．
@Yuki_Onishi 様，ありがとうございます．
自分たちのコードにマージしようとしていて気付いたのですが，以下の行は正しいですか？
https://github.com/choreonoid/choreonoid_ros/blob/feature/switch-sensors-via-rosservice/src/plugin/BodyROSItem.cpp#L217
range_vision_sensor_switch_servers_ ではないですか？
ご指摘のとおりです．
見落としでした，ありがとうございます．
（実用上動作への影響が生じない誤りでしたので，動作チェックは通ったのだと思います．）
早速修正しておきました．
@Yuki_Onishi 様，対応ありがとうございます．確認いたしました．
[ERROR] [1631611146.548325131]: Tried to advertise a service that is already advertised in this node [/DoubleArmV7/FRAME_FRONT_DEPTH_CAMERA/switch]
というエラーが出ます．
type: Camera
format: COLOR_DEPTH
の場合，visionSensors_にもvisionSensors_にも含まれていて，switchサービスが重複してしまいませんか？
@Yuki_Onishi 様，
以下の部分をコメントアウトして，range_vision_sensor_switch_servers_に対するpush_back()をやめてみたところ，エラーは出なくなりましたし，COLOR_DEPTHのカメラに対してswitch機能は正常に動作しているようです．
https://github.com/choreonoid/choreonoid_ros/blob/feature/switch-sensors-via-rosservice/src/plugin/BodyROSItem.cpp#L225-L228
@Yuki_Onishi 様，
type: Camera
format: COLOR
なデバイスのトピック /ロボット名/デバイス名/image_raw/compressed をサブスクライブしている場合，switchサービスにFalseを送ると，パブリッシャ（Choreonoid）側とサブスクライバ側の双方でエラーが表示されます．処理が停止されるわけではありませんが…
パブリッシャ（Choreonoid）側
OpenCV Error: Unknown error code -10 (Raw image encoder error: Empty JPEG image (DNL not supported)) in throwOnEror, file /build/opencv-L2vuMj/opencv-3.2.0+dfsg/modules/imgcodecs/src/grfmt_base.cpp, line 139
[ERROR] [1631878485.826977756]: /build/opencv-L2vuMj/opencv-3.2.0+dfsg/modules/imgcodecs/src/grfmt_base.cpp:139: error: (-10) Raw image encoder error: Empty JPEG image (DNL not supported) in function throwOnEror
サブスクライバ側
OpenCV Error: Assertion failed (!buf.empty() && buf.isContinuous()) in imdecode_, file /build/opencv-L2vuMj/opencv-3.2.0+dfsg/modules/imgcodecs/src/loadsave.cpp, line 637
[ERROR] [1631878485.827966195]: /build/opencv-L2vuMj/opencv-3.2.0+dfsg/modules/imgcodecs/src/loadsave.cpp:637: error: (-215) !buf.empty() && buf.isContinuous() in function imdecode_
詳細に調べていませんが，image_transportを困らせているのではないかと思います．
@masutani バグ報告ありがとうございます．
全体としてスイッチのON/OFFが動作していたため，エラーメッセージを見逃しておりました．
同一ブランチの，以下のコミットで例外対応を行いました．
Depthカメラ単体のときにもスイッチ機能が死なないようにしております．
https://github.com/choreonoid/choreonoid_ros/pull/8/commits/205b279d3eb648264dfce191c658231ca3772998
こちらについてもご報告ありがとうございます．
私自身，image_transportに詳しいわけではないのですが，Choreonoid側の当該エラーは空の画像をpubしようとして失敗しているようです．
スイッチをOFFにすれば，センサの計測の更新が行われず，したがってそのコールバックであるpubも行われないという認識でおりましたが，実際の挙動が異なる可能性がありますね．
調査はしたいと思いますが，専門外な部分もあり，こちらはすぐの解決ができないと思われます．
実用上，支障があるようでしたらお知らせください．
@Yuki_Onishi 様，
適切な対処かどうかわかりませんが，BodyROSItem::updateVisionSensor()の中の
https://github.com/choreonoid/choreonoid_ros/blob/feature/switch-sensors-via-rosservice/src/plugin/BodyROSItem.cpp#L354-L356
の部分を以下のように変更するとエラーは出なくなりました．
if (vision.step * vision.height != 0) {
vision.data.resize(vision.step * vision.height);
std::memcpy(&(vision.data[0]), &(sensor->image().pixels()[0]), vision.step * vision.height);
publisher.publish(vision);
}
情報共有ありがとうございます．
はい，取り急ぎではその対応で良いのですが，本質的にはupdateVisionSensor関数が，デバイスOFF状態でも呼び出されるところに問題があると考えております．
言い換えると，そのコードをBodyROSItemに公式に入れるのには，現時点では早計かなと考えております．
したがって，ひとまずの対応としては，お手元でそちらのパッチを当てておいていただくのが良いかなと思います．
大西さん、升谷先生、本件ご対応いただきありがとうございます。こちらプルリクのマージが遅くなりまして申し訳ありません。
マージをしようと思うのですが、その前に一点、生成されるサービス名について、現状で"switch"となっておりますが、こちらを"set_enabled"に変更させていただけますでしょうか。
理由は以下になります。
-
switchはC、C++を含む多くの言語で予約後になっていて、避けたほうが無難（サービス名をそのままサービスオブジェクト名にしようとしても、予約語なのでできない。まあ普通は多少名前を変えたりもしますが…。）
-
〜のスイッチをオン／オフするという意味では、switch 〜 on / off という言い方になるが、それと関数の構文(switch(true / false) )が若干マッチしていない（気が個人的にはする）。
-
今後「オン／オフの状態を返す」というサービスを作る時に、この関数名とマッチするよい名前が思いつかない（is_switchedとかだと意味が分かりにくいし、分かりやすい名前にすると両者がマッチしなくなる、気がする。）
set_enabled(true/false)であれば引数のtrue/falseでenabledな状態にするか／しないかという対応が分かりやすく、状態を返す関数も “is_enabled” と素直に定義できて、またこの関数名のパターンはいろいろなライブラリでよくみるものなので、こちらの方がよいのではないかと思いました。
この旨もっと早くからコメントできていればよかったのですが、当初認識が十分ではなく、このようなタイミングになりましてすみません。競技に向けて変更が難しい場合はこれまでのブランチをお使いいただくことになってしまいますが、masterブランチにマージするバージョンとしては、以上の関数名にさせていただければと思いました。
この方針について何かありましたらお知らせください。例えば実際にswtichというROSサービスが存在するようでしたら、そちらも検討材料になるかと思います。よろしくお願いします。
公式リポジトリ側の，set_enabledへの変更対応は問題ないかと思います．
理由についても正当性のあるものだと思いました．
こちらで修正すればよろしいでしょうか？
（Githubにも書きましたがPR7とPR8（本件）がコンフリクトするので様子見しています）