search

ShunjiHashimoto / tang_task

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

【親タスク】LiDARを使った人追従実装 #29

Open ShunjiHashimoto opened 11 months ago

ShunjiHashimoto commented 11 months ago

https://github.com/ShunjiHashimoto/tang_lidar/commit/d1975a4ee53957bd35cc4bc459e2da2891e98ea0 まずはros2のパッケージ作成と、entrypointを追加した

ShunjiHashimoto commented 11 months ago

https://zenn.dev/uchidaryo/articles/ros2-programming-5 これを参考に、ros2＋cppの簡単なpubsubを作ろう''

ShunjiHashimoto commented 11 months ago

sensor_msgsを使用する場合は、package.xmlにdependを追加するのと、CMakeListに、find_package()に追加するのと、依存関係を解決するために、ament_target_dependencies()にも追加する

ShunjiHashimoto commented 11 months ago

rviz2の画面が出ない場合

X11 Forwardingの設定: DockerコンテナがホストマシンのXサーバーを利用できるようにするために、X11 Forwardingを設定する必要があります。これには、ホストマシンでxhostコマンドを使用し、コンテナからの接続を許可することが含まれます。

xhost +local:docker
ShunjiHashimoto commented 11 months ago

点群のサイズは以下の通り

[tang_lidar-1] [INFO] [1700891100.518220410] [tang_lidar]: Received a scan
[tang_lidar-1] [INFO] [1700891100.518259377] [tang_lidar]: Size of weight_x: 527076
ShunjiHashimoto commented 11 months ago

shared_ptrを使った配列の宣言はこちらのとおり

std::shared_ptr<XYTL_t[]> p2 {new XYTL_t[URG_ANUM]};
// だめな例：std::shared_ptr<XYTL_t[]> U = std::make_shared<XYTL_t[]>(URG_ANUM);

参考）https://cpprefjp.github.io/reference/memory/shared_ptr/op_at.html

ShunjiHashimoto commented 11 months ago

ros2のノードを実行したときにログを出力するための設定

node1 = Node(
        package = 'tang_lidar',
        executable = 'tang_lidar', 
        output='screen',
        emulate_tty=True,
        arguments=[('__log_level:=debug')], 
    )

参考）https://answers.ros.org/question/332829/no-stdout-logging-output-in-ros2-using-launch/

ShunjiHashimoto commented 11 months ago

https://docs.ros.org/en/iron/How-To-Guides/Setup-ROS-2-with-VSCode-and-Docker-Container.html VSCode＋ROS 2の環境設定方法

ShunjiHashimoto commented 11 months ago

std::vectorとstd::shared_ptrの使う違い

std::vector と std::shared_ptr の配列のどちらがより適しているかは、使用するコンテキストや必要とする機能に大きく依存します。以下に、それぞれの特徴と適した使用状況を示します。

std::vector 用途: 動的なサイズの配列。要素の追加や削除が頻繁に行われる場合に適しています。 メモリ管理: 自動的に行われます。要素が追加または削除されると、必要に応じてメモリが再割り当てされます。 アクセス: operator[] を使って直接アクセスできます。 適用例: 配列のサイズが実行時に変更される可能性がある場合や、要素の挿入・削除操作が頻繁に必要な場合。

std::shared_ptr<配列タイプ> 用途: 固定サイズの配列で、その配列への参照を複数のオブジェクト間で共有する場合に適しています。 メモリ管理: 参照カウントに基づいて行われます。最後の shared_ptr がスコープを抜けると、自動的にメモリが解放されます。 アクセス: ポインタとしてアクセスします。operator[] でもアクセス可能です。 適用例: 複数のオブジェクトや関数間で配列を共有したい場合。または、配列の生存期間が複数のスコープにまたがる場合。 選択基準 サイズの柔軟性: サイズが動的に変更される必要がある場合は std::vector を選びます。 共有の必要性: 配列の所有権を複数のオブジェクトで共有する必要がある場合は std::shared_ptr を使った配列が適しています。 パフォーマンス: 高頻度でサイズが変更される場合は std::vector の方が効率的ですが、大きな配列を複数のオブジェクトで共有する必要がある場合は std::shared_ptr の方がメモリ使用量を節約できます。

ShunjiHashimoto commented 11 months ago

https://github.com/ShunjiHashimoto/tang_lidar/commit/d30c557823b3d12d0ee87058108b6483ff16a5f9 前処理の修正、クラスタリング結果の可視化の実装までが完了した。

次のやること 描画コードのリファクタリング, RViz2の設定ファイル保存、同時にrvizを立ち上げる

ShunjiHashimoto commented 11 months ago 
class ColorDefinitions {
public:
    static std_msgs::msg::ColorRGBA Red();
    static std_msgs::msg::ColorRGBA Green();
    static std_msgs::msg::ColorRGBA Blue();
    static std_msgs::msg::ColorRGBA Yellow();

private:
    static std_msgs::msg::ColorRGBA CreateColor(float r, float g, float b, float a);  // staticに変更
};

staticを使わない場合は、わざわざcolor_definitons = ColorDefinitions()みたいに初期化する必要があるが、staticを使うことで、ColorDefinitions::Red()みたいに呼び出せる

ShunjiHashimoto commented 11 months ago

rvizの設定ファイルをlaunchに読み込むには、CMakeListにrvizをビルドして、get_packages_shre_directoryを使用できるようにする。 CMakeList

install(DIRECTORY rviz DESTINATION share/${PROJECT_NAME}/)

launch

# RViz2 Node
    rviz_config_path = join(get_package_share_directory('tang_lidar'), 'rviz', 'tang_lidar.rviz')
    rviz_node = Node(
        package='rviz2',
        executable='rviz2',
        arguments=['-d', rviz_config_path],
        output='screen'
    )
ShunjiHashimoto commented 11 months ago

https://github.com/ShunjiHashimoto/tang_lidar/pull/3 プルリク出せて、マージした。 これでURGデータの前処理、クラスタリングの実装が完了した。残りのコードも引き続き実装していく

ShunjiHashimoto commented 11 months ago

山彦の参考資料がいくつか見つかった、 spur やまびこで調べたら出てきた https://www.roboken.iit.tsukuba.ac.jp/lectures/software_science_experiment/2017/document/robot_control.pdf
https://www.roboken.iit.tsukuba.ac.jp/lectures/software_science_experiment/2017/document/robot_hardware.pdf
https://www.roboken.iit.tsukuba.ac.jp/platform/wiki/yp-spur/index

ShunjiHashimoto commented 10 months ago

移動ロボットのTFのベストプラクティス https://qiita.com/motoms/items/4c45f75911e210088ea1 https://memo.soarcloud.com/ros%E3%81%AE%E5%BA%A7%E6%A8%99%E5%A4%89%E6%8F%9Btf%E3%81%AB%E3%81%A4%E3%81%84%E3%81%A6/ https://robo-marc.github.io/navigation_documents/navigation_overview.html

ShunjiHashimoto commented 10 months ago

山彦の参考資料がいくつか見つかった、 spur やまびこで調べたら出てきた https://www.roboken.iit.tsukuba.ac.jp/lectures/software_science_experiment/2017/document/robot_control.pdf https://www.roboken.iit.tsukuba.ac.jp/lectures/software_science_experiment/2017/document/robot_hardware.pdf https://www.roboken.iit.tsukuba.ac.jp/platform/wiki/yp-spur/index

今回は、ロボットの向きとLiDARの向きを完全に一緒にしているのでURG_ChangeCoordinateは実装しないでおく。
今後IMUを

ShunjiHashimoto commented 10 months ago

https://github.com/ShunjiHashimoto/tang_lidar/commit/702ed882b132beb4bd29bbf1dcd6bcb77fbdf25d 速度ベクトルの計算の実装はとりあえず行ったが、正確な動作確認がまだ。

ShunjiHashimoto commented 10 months ago

https://github.com/ShunjiHashimoto/tang_lidar/commit/b0f8fb2876171ccec57e782cea9a96e2194510d4 速度ベクトルの計算の詳細を理解しながらリファクタを実装した。内容自体はおおよそ把握したが、なぜかTF近くにmarkerが表示される。色も想定している色の挙動になっていないので、少しデバッグは必要そう。　 これが終われば、後は追従対象を決めて、速度制御する流れになる。後少し

ShunjiHashimoto commented 10 months ago

同じクラスタかどうかを判別して、同じクラスタ番号のものを追従すればよいのかを考察中 今のアルゴリズムだと、点群データ全てに対してクラスタ番号を降っているので、それが番号が少しでもずれるとクラスタ番号も変わってしまう。どうやって同じクラスタを追従するのか？

ShunjiHashimoto commented 10 months ago

前回の点群データと今の点群データの差分を見て、クラスタ番号をふるかどうかを決定しているので、登録されたクラスタ番号近くの物体と障害物がかなって、二股に別れていく挙動をした場合は、クラスタ番号を降ってしまうかもしれない。
この挙動は理解できた。 また、上記コメントについては、クラスタ番号があるということはすべて追跡対象で、そのクラスタ番号が振られている座標の重心を目標位置としている。だいぶわかってきました。

ShunjiHashimoto commented 9 months ago

ロボゼミで現在の実装内容を発表した https://github.com/ShunjiHashimoto/tang_task/issues/31 次は認識精度向上のため、論文内容を実装する https://github.com/ShunjiHashimoto/tang_task/issues/32

ShunjiHashimoto commented 7 months ago

https://github.com/ShunjiHashimoto/tang_task/issues/34 論文内容の実装は、一通り理解しながら実装が完了した。次は、LiDAR使った実験に移る