ROS2 自作 Turtlebot3(foxbot_core3_r2) Rtabmap_ros with Stereo rgbd and GPS.

ROS2 自作 Turtlebot3(foxbot_core3_r2) Rtabmap_ros with Stereo rgbd and GPS.
--- GPS を付けて、robot_localization ekf で試す。 ---

ROS2 自作 Turtlebot3(foxbot_core3_r2) Rtabmap_ros with Stereo rgbd. の続きです。
自作 Turtlebot3 自律走行に向けたプログラム。#14 の ROS2版です。

1. 環境。
1) Remote PC
Ubuntu Mate 20.04
ros2: foxy
rtabmap_ros, navigation2, Rviz2
注1) foxy navigation2 には、不具合があるので、パッチが必要。後述。
注2) ubuntu 20.04 で、ros2 galactic が使えるみたい。こちらの方が良いと思います。by nishi 2023.1.17

2) SBC
Jetson Nanao 2G
Jet Pack4 .x and Ubuntu 20.04
ros2: foxy
micro-ROS Agent
Single USB Stereo Camera
GPS: AE-GYSFDMAXB

3) SOC
ESP32 Arduino
micro-ROS
IMU

2. foxbot_core3_r2.ino のビルド。
foxbot_core3_r2_config.h を、下記にして、ESP32 へ書き込みます。
詳しい事は、ROS2 で、ESP32(Arduino) を使う。 を参照。

foxbot_cor3_r2 からは、tf-base_footprint を出力しません。
IMU は、出力します。

3. Remote PC でのインストール。
無し。

4. SBC(Jetson Nano 2G) でのインストール。
4.1 robot localization
cra-ros-pkg/robot_localization/

'wheel': '/odom_fox'
'odom_gps': '/odom_gps'
'imu0': '/imu'
を入力させて、
tf-base_footprint
/odom
を、パブリッシュさせます。

4.2 GPS node のインストール。
tosa-no-onchan/gysfdmaxb_gps

5. 実行
rtabmap_ros_my/launch/ratbmap_stero_rgbd_gps.launch.py を実行します。
実行方法は、上記ファイルの上部に記載してあります。

local_planner は、dwb_core::DWBLocalPlanner 、teb_local_planner::TebLocalPlannerROS どちらでも、動作確認できました。

これで、ROS版 自作 Turtlebot3 自律走行に向けたプログラム。#14 の続きが、やっと ROS2 版で出来ます。

後は、Remote PC T100TA で試してみて、動作が安定すれば、やっと、屋外で試せます。

6. 追記。
teb_local_planer を、T100TA で試したら、navigation2 のエラーが出て上手く行かなかった。
試しに、再度、ここの、PC で試しても、同じ様なエラーが出だしました。


Extrapolation Error: Lookup would require extrapolation into the future.
のエラーが常に出るようになった。

ネットで、検索するどうやら、tf map->odom のpublish が間に合っていないとの事。
しかし、tf map->odom のpublish rate は、rtabmap_ros では、20[hz] なので、これ以上、上がりそうにない。
実際は、20[hz] も出ていないが、現実として、これ以上、改善できそうもない。
dwb_core::DWBLocalPlanner では、出ないのに、teb_local_planner::TebLocalPlannerROSでは、でるみたい。
少し、両者のソースを比べてみて、
teb_local_planner/teb_local_planner/src/teb_local_planner_ros.cpp
を少し修正したら、でなくなりました。
tf2::durationFromSec(0.5) を、追加したら、OK になりました。
良く調べたら、下記にあった。
teb_local_planner/issues/273
書き込もうと、おもったら、既に指摘する投稿があった。

これで、PC では、teb_local_planner で、動くようになった。by nishi 2023.1.9
後は、remote PC を、T100TA にしてでの、確認じゃ。
おんちゃんの今の感想は、もっと GPS の性能を上げたい。これに尽きる。

6.1 現状で、rtabmap_ros を、SBC(Jetson Nano 2G) で起動して、テスト。
OK でした。C++ プログラムでの自動走行で、2[M] 前進して、Uターンして、帰ってきました。

6.2 さらに、Navigation2 も、SBC(Jetson Nano 2G) で起動して、テスト。
これは、一応動くが、途中の、
幾つかのポイントに着けないでエラーで、pass する場合がでた。
やはり、CPU パワー不足か。


"send_goal failed" を出している所を、探してみる。

navigation2/nav2_behavior/include/nav2_behavior_tree/bt_action_node.hpp
line 233
if (rclcpp::spin_until_future_complete(node_, future_goal_handle, server_timeout_) !=
rclcpp::FutureReturnCode::SUCCESS)
server_timeout_ が小さいのでは、試しに大きくしてみる。

上記修正を入れて、再度、SBC(Jetson Nano 2G) で、navigation2 を動かしてみたら。エラーは出なくなった。
うまく、完走できるようになった。

さて、server_timeout_ を設定している箇所は、
同じく、navigation2/nav2_behavior/include/nav2_behavior_tree/bt_action_node.hpp の line42

のようだが、このパラメータの与え方が、判らない。
navaigation2 のパラメータに関しては、下記、navigation2/doc/parameters/param_list に記載があるが?
誰か、具体的やり方を知っていれば、お教えて欲しい。

bt_navigator:
ros__parameters:
default_bt_xml_filename: "navigate_w_replanning_and_recovery.xml"

navigation2/nav2_bt_navigator/behavior_trees/navigate_w_replanning_and_recovery.xml
で、指定するのだろうか?

The value of server_timeout_ of on_new_goal_received() in bt_action_node.hpp is too short. #3355 で、問い合わせてみたが、
具体的には、記載が無いので、わからん。
You just need to set the server_timeout port on the BT nodes where you have timing issues which use the BT Action Node (probably FollowPath and ComputePathToPose) in your BT XML.
もしかして、port は、part の誤りか?
とは、どこじゃ?

nav2_params.yaml


なので、彼の言う、BT XML ファイルは、
navigation2/nav2_bt_navigator/behavior_trees/navigate_w_replanning_and_recovery.xml に該当しそうだが、
どう記述すれば良いのか?


この中の、設定項目は、
navigation2/parameters/param_list の、下記か?

しかし、気になるのは、RecoveryNode の定義になっていること。しかし、global plan の処理の気がするが?
This Behavior Tree replans the global path periodically at 1 Hz となっているから、此処でよいかも?
global_costmap の方だから、ComputePathToPose の設定の様だ。
local_costmap が、FollowPath の方みたいぞね。
goal=、planner_id= の指定があるから、下記で良いのかも?


Galactic 以降だと、 the default_server_timeout で、指定できるとのこだ。
どうも、担当者は、他のQ/A でもそうだが、foxy に関しては、冷ややかだ!!
しきりに、 Galactic 以降の場合を回答してくる。
ここに、記述があったみたい。
Configuration Guide / Behavior Tree XML Nodes
Configuration Guide / Behavior Tree XML Nodes / ComputePathToPose

ともあれ、これで、SBC(Jetson Nano 2G) 上で、Single USB Stereo Camera、GPS 、rtabmap_ros with Stereo sync、
robot_localization/ekf_node、navigation2+teb_local_planner を起動して、動くようになった。

6.3 最後に、SBC(Jetson Nano 2G) 上で、 C++ プログラムも起動してみます。
$ ros2 run turtlebot3_navi_my multi_goals4_nav2
OK でした。

あとは、Remote PC を、T100TA にして、動作確認できれば、屋外で走行ができます。
T100TA 上では、起動の操作(Python GUI scripts で作成)と、Rviz2 の表示だけになるぞね。
主要なノードは、全て、SBC(Jetson Nano 2G) 上で、動かせます。
もう少しぞね。by nishi 2023.1.10

6.4 T100TA で、まるきり、同じ事を試してみました。
navigation2 の ログに、今度は、follow_path の方が出ています。

やはり、local_costmap の方も、server_timeout="20" にしたほうが良さそうぞね。by nishi 2023.1.12
<FollowPath path="{path}" controller_id="FollowPath" server_timeout="20" />

上記追加で、やっと、 T100TA(Remote PC) で、完走できました。しかし、 Navigation2 のログに、結構 ERROR が、まだ、出ている。
今の問題点。
voxel_layer で、床を、障害物と判断されるので、どうも、工合が悪い。
やれやれぞね!!

その後は、GPS を、みちびきの L6 信号を受信できるようにして、 6[cm] 級にしたいぞね。
そうすると、rtabmap_ros with Stereo sync は、止めて、Camera 入力での 機械学習で、動かしてみたい。

参考ページ。
navigation2 parameter list
navigation2/parameters/param_list
navigation.ros.org/configuration
Configuration Guide / Costmap 2D / Voxel Layer Parameters

このブログ記事について

このページは、おんちゃんが2022年12月19日 17:51に書いたブログ記事です。

ひとつ前のブログ記事は「ROS2 自作 Turtlebot3(foxbot_core3_r2) Rtabmap_ros with Stereo rgbd. 」です。

次のブログ記事は「とほほの、Windows XP マザーボード交換。」です。

最近のコンテンツはインデックスページで見られます。過去に書かれたものはアーカイブのページで見られます。

カテゴリ

ウェブページ

サイトナビ