search

ShunjiHashimoto / tang_task

人追従ロボットTANGの開発タスクを管理するリポジトリです
0 stars 0 forks source link

現在のtang_lidarに論文内容を実装する #34

Closed ShunjiHashimoto closed 8 months ago

ShunjiHashimoto commented 9 months ago

https://www.eureka-moments-blog.com/entry/2022/07/25/170130 占有格子マップの理解はこれがわかりやすそう

ShunjiHashimoto commented 9 months ago

https://daobook.github.io/ros2-docs/xin/Tutorials/Tf2/Writing-A-Tf2-Listener-Cpp.html tf2_rosのROS2での書きあkた

ShunjiHashimoto commented 9 months ago

https://fer.gs/ros2_cookbook/client_libraries/rclcpp/tf2.html#get-latest-transform tf2のgetYaw関数の使い方が書いてある

ShunjiHashimoto commented 9 months ago

まずは占有格子マップ部分をROS1からROS2に対応させた 次は、人の足検出を行っている、detect_leg_clusters.cppをROS 2対応させながらコードの理解を進める
人の足検出ができれば、その結果をもとに、人の足でない領域がわかり、占有格子マップを更新することができる https://github.com/ShunjiHashimoto/leg_tracker_ros2/pull/1

ShunjiHashimoto commented 9 months ago 
bool operator ()(const leg_tracker_ros2::msg::Leg &a, const leg_tracker_ros2::msg::Leg &b) const{
   float rel_dist_a = pow(a.position.x*a.position.x + a.position.y*a.position.y, 1./2.);
   float rel_dist_b = pow(b.position.x*b.position.x + b.position.y*b.position.y, 1./2.);          
   return rel_dist_a < rel_dist_b;
 }

detect_leg_clusters.cppにおいて、上記のように、メソッドの末尾にconstをつけることでエラーが解消されました。
エラー：https://github.com/mowito/ros2_leg_detector/issues/2 const修飾子をつけることで、メンバ変数が変更されないことが保証され、より安全にメソッドを呼び出すことができます。

ShunjiHashimoto commented 9 months ago

人の足をランダムフォレストを用いて検出するところまでのコードをROS 2に移植してビルドが成功するところまで確認できた。あとは、結果をPubするのみ。 次のステップとして、tracker部分を実装する必要がある。

ShunjiHashimoto commented 9 months ago

BSDライセンスに必要な表記について 公開する際の具体的なステップ ファイルのヘッダーにライセンスを含める： 元のコードと同様に、あなたが作成または修正したすべてのファイルの先頭に、適切な著作権表示とライセンス条項を含めてください。これにより、あなたのコードがBSDライセンスの条件下で配布されることが他のユーザーに明確に伝わります。

ライセンスファイルの追加： プロジェクトのルートディレクトリにLICENSEファイルを追加し、そこに完全なBSDライセンスのテキストを含めることをお勧めします。

READMEの更新： README.mdファイル（またはプロジェクトの説明文書）に、プロジェクトがBSDライセンスの下で配布されていること、およびその主な条件を簡潔に説明するセクションを追加してください。これにより、プロジェクトを利用する他の開発者がライセンス条件を理解しやすくなります。

ShunjiHashimoto commented 9 months ago

一旦、 人の脚の特徴をもとに、人の足を検出→RViz上に表示までの実装ができた。 使用するコードは、 cluster_features.cpp LiDARデータをもとに特徴量を計算 laser_processor.cpp LiDARデータの前処理 detect_leg_clusters.cpp 上記のライブラリを使用して、人の足の検出を行う

その他のコード local_occupancy_grid_mapping.cpp　占有格子マップを作成、tracker時に使用する、non_leg_clustersをsubして、マップを作る joint_leg_tracker.py　人の足の検出結果をもとに追跡をおこなう、subは占有格子マップmsgとdetected_leg_clusters msgの２つをもとに追跡する。結果としては、人の座標をpubする。その結果をもとにロボットに速度指令を与える

ShunjiHashimoto commented 9 months ago

今後の実装方針としては、人の足を検出して結果をRViz上に表示するのを目標とする。 次にtracker部分の実装に進む。

ShunjiHashimoto commented 9 months ago 
sudo apt install python3-pykdl
sudo apt install python3-pip
pip install pykalman

不足していたパッケージ

ShunjiHashimoto commented 8 months ago

現在はleg_tracker_ros2で人検出がうまく行かなかった原因を探るため、まずは動いているros2_leg_detectorを調査中。点群クラスタの検出数は同じであった

ShunjiHashimoto commented 8 months ago

https://qiita.com/iwatake2222/items/20fcad3eb68bb1e03792 これみてデバッグ環境頑張ろうと思ったけどやっぱり難しい、

ShunjiHashimoto commented 8 months ago

https://github.com/athackst/vscode_ros2_workspace これ試してみる

ShunjiHashimoto commented 8 months ago

デバッグ環境を頑張って作る

ShunjiHashimoto commented 8 months ago

https://qiita.com/ossyaritoori/items/f4f07708d0f27b33f89b こちらの記事を参考にできた。 chatgptの回答も参考にした VS Codeでのリモート開発設定 VS Codeに「Remote - Containers」拡張機能をインストールします。

VS Codeのコマンドパレット（Ctrl+Shift+P）を開き、「Remote-Containers: Attach to Running Container...」を選択します。

起動したコンテナのリストから、デバッグを行いたいコンテナを選択します。

コンテナにアタッチした後、VS Codeでros2_ws/src/ros2_leg_detector/src/leg_detectorフォルダを開きます。これは、デバッグしたいプログラムが格納されているディレクトリです。

launch.jsonの設定 VS Codeで、デバッグを行いたいC++プログラム用にlaunch.jsonファイルを設定します。この設定ファイルは、デバッグの構成情報をVS Codeに伝えるものです。.vscodeフォルダ内にlaunch.jsonを作成し、適切な設定を行います。

Remote -containersではなく、Dev -COntainersであることに注意すること

ShunjiHashimoto commented 8 months ago

ノードを実行してから、デバッグ機能の再生ボタンを押すとどのノードをデバッグするか選択できる

ShunjiHashimoto commented 8 months ago

デバッグ環境は整った。
ブレイクポイントを設けながらデバッグ中。 やはりdetect_leg_clusters.cppの296で、検出結果がpubされているはずなのに、RViz上には表示されない。原因は不明なので調査が必要。 デバッグのやり方は、GUI上のデバッグボタンを押して、launchファイルを実行後、別のターミナルでrviz2とrosbagのコマンドを実行する。明日中にはlidar動かしたいね。

ShunjiHashimoto commented 8 months ago

image 無事人検出はできていそうだ

ShunjiHashimoto commented 8 months ago

残すros2対応していないのは、人物トラッカーのjoint_leg_tracker.pyとinflated_human_scan.cppの２つのみ。 これらを実装できれば、一通り実装して理解したこととなる

ShunjiHashimoto commented 8 months ago

デバッグモードでlaunchを実行できるようになった、足りないmsgであるPersonArrayを追加する

ShunjiHashimoto commented 8 months ago

if self.publish_occluded or track.seen_in_current_scan:
ps = PointStamped() ps.header.frame_id = self.fixed_frame ps.header.stamp = tf_time ps.point.x = track.pos_x ps.point.y = track.pos_y try: ps = self.buffer.transform(ps, self.publish_people_frame) except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException): continue

この行で、下記の例外エラーがでる。


lass TransformRegistration():
    __type_map = {}

    def print_me(self) -> None:
        print(TransformRegistration.__type_map)

    def add(
        self,
        key: TransformableObjectType,
        callback: Callable[[TransformableObject, TransformStamped], TransformableObject]
    ) -> None:
        TransformRegistration.__type_map[key] = callback

    def get(
        self,
        key: TransformableObjectType
    ) -> Callable[[TransformableObject, TransformStamped], TransformableObject]:
        if not key in TransformRegistration.__type_map:
            raise TypeException('Type %s is not loaded or supported' % str(key))
ShunjiHashimoto commented 8 months ago 
import tf2_geometry_msgs

を追加すれば上記のエラーはなくなったが、 人のマーカーが描画されない。

ShunjiHashimoto commented 8 months ago 
if (transform_available) {
    RCLCPP_INFO(this->get_logger(), "Transform Available");
    // TODO: 追跡された足の座標変換、どのscanデータが人に対応するかを決定し、それらのエリアはfree-space１とする
    // TODO: non_legsを定義
    std::vector<bool> is_sample_human;                 
    is_sample_human.resize(scan_msg->ranges.size(), false);
    sensor_msgs::msg::LaserScan scan = *scan_msg;
    // TODO: non_legsをもとに、人の足に一致するスキャンデータがどれか割り当てる

ここを追加しないとか。ただそれだとなぜ、わからんのでまた明日にするか

ShunjiHashimoto commented 8 months ago 
[Processing: leg_tracker_ros2]                       
--- stderr: leg_tracker_ros2
/usr/bin/ld: CMakeFiles/occupancy_grid_mapping.dir/src/local_occupancy_grid_mapping.cpp.o: in function `OccupancyGridMapping::laserAndLegCallback(std::shared_ptr<sensor_msgs::msg::LaserScan_<std::allocator<void> > const>, std::shared_ptr<leg_tracker_ros2::msg::LegArray_<std::allocator<void> > const> const&)':
/root/ros2_ws/src/leg_tracker_ros2/src/local_occupancy_grid_mapping.cpp:147: undefined reference to `laser_processor::ScanProcessor::ScanProcessor(sensor_msgs::msg::LaserScan_<std::allocator<void> > const&)'
/usr/bin/ld: /root/ros2_ws/src/leg_tracker_ros2/src/local_occupancy_grid_mapping.cpp:148: undefined reference to `laser_processor::ScanProcessor::splitConnected(float)'
/usr/bin/ld: /root/ros2_ws/src/leg_tracker_ros2/src/local_occupancy_grid_mapping.cpp:149: undefined reference to `laser_processor::ScanProcessor::removeLessThan(unsigned int)'
/usr/bin/ld: /root/ros2_ws/src/leg_tracker_ros2/src/local_occupancy_grid_mapping.cpp:152: undefined reference to `laser_processor::SampleSet::getPosition()'
/usr/bin/ld: /root/ros2_ws/src/leg_tracker_ros2/src/local_occupancy_grid_mapping.cpp:328: undefined reference to `laser_processor::ScanProcessor::~ScanProcessor()'
/usr/bin/ld: /root/ros2_ws/src/leg_tracker_ros2/src/local_occupancy_grid_mapping.cpp:328: undefined reference to `laser_processor::ScanProcessor::~ScanProcessor()'
collect2: error: ld returned 1 exit status
gmake[2]: *** [CMakeFiles/occupancy_grid_mapping.dir/build.make:223: occupancy_grid_mapping] Error 1
gmake[1]: *** [CMakeFiles/Makefile2:600: CMakeFiles/occupancy_grid_mapping.dir/all] Error 2
gmake: *** [Makefile:146: all] Error 2
---
Failed   <<< leg_tracker_ros2 [48.8s, exited with code 2]

Summary: 0 packages finished [49.8s]
  1 package failed: leg_tracker_ros2
  1 package had stderr output: leg_tracker_ros2

このエラーはなにか

ShunjiHashimoto commented 8 months ago

上記のエラーは、CMakelistでlocal_occupancy_mapとlaser_processor.cppにtarget_linkの紐付けがされていなかった

ただ、次に、dist_travelledの値が常に0になっていた、原因調査が必要 dist_travelled = min(track_1.dist_travelled - track_1_initial_dist, track_2.dist_travelled - track_2_initial_dist)

ShunjiHashimoto commented 8 months ago

subscriberのsyncの記述方法がros2_leg_detectorと異なっていたため、修正したら上記のエラーはなくなった。そもそもsubscriberが機能していなかった。 どこの記述が良くなくて、どう修正すればうまくいくかは後ほど調べる。

ShunjiHashimoto commented 8 months ago

auto timer_interface = std::make_shared( this->get_node_base_interface(), this->get_node_timers_interface()); tfbuffer->setCreateTimerInterface(timer_interface);

このコメントアウトされているコードは、tf2_ros::Bufferにカスタムタイマーインターフェースを設定するためのものです。ROS 2では、tf2_ros::Bufferのインスタンスがトランスフォームをリッスンする際に、内部でタイマーを使用してトランスフォームの有効期限を管理します。デフォルトでは、tf2_ros::BufferはROS 2のノードタイマーを使用しますが、特定のシナリオではカスタムタイマーインターフェースを設定する必要があります

ShunjiHashimoto commented 8 months ago 
OccupancyGridMapping() : Node("occupancy_grid_mapping"), 
                        scan_sub_(this, std::string("scan")), 
                        legs_sub_(this, "non_leg_clusters"),
                        sync(scan_sub_, legs_sub_, 100)

このコードで、メンバ変数の初期化処理を実装している。

ShunjiHashimoto commented 8 months ago

tfのbufferにあるカスタムタイマインターフェースを使用することによる利点

トランスフォームの可用性の定期的なテスト

カスタムタイマーインターフェースを使用することで、tf2_ros::Bufferはノードから定期的にコールバックを受け取り、トランスフォーム（座標変換情報）が利用可能かどうかをテストできます。これにより、特定のトランスフォームが利用可能になった瞬間を正確に把握でき、それに応じて処理を進めることが可能になります。

非同期クエリのサポート

tf2_ros::Bufferは、トランスフォームの非同期クエリ（非同期に座標変換情報を問い合わせること）をサポートします。カスタムタイマーインターフェースを設定することで、これらの非同期クエリを、自身でスレッドを起動することなく実行できます。これは、リソースの使用効率を高め、アプリケーションの複雑性を低減させる効果があります。

リソース管理の最適化

自身でスレッドを管理する必要がなくなることは、リソース管理の観点からも大きな利点です。スレッドを起動し、管理することはリソース消費が大きく、特に組み込みシステムやリアルタイムシステムのようなリソースが限られた環境では望ましくありません。カスタムタイマーインターフェースを使用することで、tf2_ros::Bufferはノードの既存のイベントループやコールバックシステムを活用して効率的に動作できます。

この説明は、tf2_ros::Bufferのカスタムタイマーインターフェースを設定することによる主な利点を明確にしています。この機能を利用することで、トランスフォーム情報の管理がより効率的かつ柔軟になり、ROS 2アプリケーションの全体的なパフォーマンスと信頼性が向上します。

ShunjiHashimoto commented 8 months ago

https://github.com/ShunjiHashimoto/leg_tracker_ros2/pull/3 trackerの部分も実装完了した。 次は実際にLiDARを接続して実験を行う。別チケットで起票するため、本チケットはClosedとする