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

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 で試しても、同じ様なエラーが出だしました。
[bt_navigator-4] [INFO] [1672134986.027965494] [bt_navigator]: Begin navigating from current location to (0.00, 0.00) [controller_server-1] [INFO] [1672134986.049144731] [controller_server]: Received a goal, begin computing control effort. [controller_server-1] [INFO] [1672134986.049834538] [controller_server]: Passing new path to controller. [controller_server-1] [ERROR] [1672134986.100037145] [controller_server]: Extrapolation Error: Lookup would require extrapolation into the future. Requested time 1672134986.211491 but the latest data is at time 1672134986.176166, when looking up transform from frame [odom] to frame [map]

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
を少し修正したら、でなくなりました。 line 685 bool TebLocalPlannerROS::transformGlobalPlan(const std::vector<geometry_msgs::msg::PoseStamped>& global_plan, const geometry_msgs::msg::PoseStamped& global_pose, const nav2_costmap_2d::Costmap2D& costmap, const std::string& global_frame, double max_plan_length, std::vector<geometry_msgs::msg::PoseStamped>& transformed_plan, int* current_goal_idx, geometry_msgs::msg::TransformStamped* tf_plan_to_global) const { ... line 718 //let's get the pose of the robot in the frame of the plan //geometry_msgs::msg::PoseStamped robot_pose = tf_->transform(global_pose, plan_pose.header.frame_id); // test by nishi geometry_msgs::msg::PoseStamped robot_pose = tf_->transform(global_pose, plan_pose.header.frame_id, tf2::durationFromSec(0.5)); ...
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 パワー不足か。
[bt_navigator-4] [INFO] [1673249101.955052970] [bt_navigator]: navigateToPose() : #1 passed! [bt_navigator-4] [INFO] [1673249101.955157764] [bt_navigator]: Begin navigating from current location to (0.27, -0.14) [bt_navigator-4] [INFO] [1673249101.959357500] [bt_navigator]: navigateToPose() : #3 passed! [bt_navigator-4] [ERROR] [1673249101.970249323] [BehaviorTreeEngine]: Behavior tree threw exception: send_goal failed. Exiting with failure. [bt_navigator-4] [INFO] [1673249101.970308387] [bt_navigator]: navigateToPose() : #4 passed! [bt_navigator-4] [INFO] [1673249102.070816674] [bt_navigator]: navigateToPose() : #5 passed! [bt_navigator-4] [ERROR] [1673249102.070936417] [bt_navigator]: Navigation failed [bt_navigator-4] [WARN] [1673249102.070980793] [bt_navigator]: [navigate_to_pose] [ActionServer] Aborting handle.

"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_ が小さいのでは、試しに大きくしてみる。
line 233 //if (rclcpp::spin_until_future_complete(node_, future_goal_handle, server_timeout_) != // rclcpp::FutureReturnCode::SUCCESS) // test by nishi if (rclcpp::spin_until_future_complete(node_, future_goal_handle, server_timeout_*2) != rclcpp::FutureReturnCode::SUCCESS) { throw std::runtime_error("send_goal failed"); }
上記修正を入れて、再度、SBC(Jetson Nano 2G) で、navigation2 を動かしてみたら。エラーは出なくなった。
うまく、完走できるようになった。

さて、server_timeout_ を設定している箇所は、
同じく、navigation2/nav2_behavior/include/nav2_behavior_tree/bt_action_node.hpp の line42
line 42 // Get the required items from the blackboard server_timeout_ = config().blackboard->template get<std::chrono::milliseconds>("server_timeout"); getInput<std::chrono::milliseconds>("server_timeout", server_timeout_);
のようだが、このパラメータの与え方が、判らない。
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_navigator: ros__parameters: .... default_bt_xml_filename: "navigate_w_replanning_and_recovery.xml"

なので、彼の言う、BT XML ファイルは、
navigation2/nav2_bt_navigator/behavior_trees/navigate_w_replanning_and_recovery.xml に該当しそうだが、
どう記述すれば良いのか?
<!-- This Behavior Tree replans the global path periodically at 1 Hz and it also has recovery actions. --> <root main_tree_to_execute="MainTree"> <BehaviorTree ID="MainTree"> <RecoveryNode number_of_retries="6" name="NavigateRecovery"> <PipelineSequence name="NavigateWithReplanning"> <RateController hz="1.0"> <RecoveryNode number_of_retries="1" name="ComputePathToPose"> <ComputePathToPose goal="{goal}" path="{path}" planner_id="GridBased"/> <ClearEntireCostmap name="ClearGlobalCostmap-Context" service_name="global_costmap/clear_entirely_global_costmap"/> </RecoveryNode> </RateController> <RecoveryNode number_of_retries="1" name="FollowPath"> <FollowPath path="{path}" controller_id="FollowPath"/> <ClearEntireCostmap name="ClearLocalCostmap-Context" service_name="local_costmap/clear_entirely_local_costmap"/> </RecoveryNode> </PipelineSequence> <ReactiveFallback name="RecoveryFallback"> <GoalUpdated/> <SequenceStar name="RecoveryActions"> <ClearEntireCostmap name="ClearLocalCostmap-Subtree" service_name="local_costmap/clear_entirely_local_costmap"/> <ClearEntireCostmap name="ClearGlobalCostmap-Subtree" service_name="global_costmap/clear_entirely_global_costmap"/> <Spin spin_dist="1.57"/> <Wait wait_duration="5"/> </SequenceStar> </ReactiveFallback> </RecoveryNode> </BehaviorTree> </root>

この中の、設定項目は、
navigation2/parameters/param_list の、下記か?
BT Node ComputePathToPose goal N/A Goal pose planner_id N/A Mapped name to the planner plugin type to use, e.g. GridBased server_name N/A Action server name server_timeout 10 Action server timeout (ms)
しかし、気になるのは、RecoveryNode の定義になっていること。しかし、global plan の処理の気がするが?
This Behavior Tree replans the global path periodically at 1 Hz となっているから、此処でよいかも?
global_costmap の方だから、ComputePathToPose の設定の様だ。
local_costmap が、FollowPath の方みたいぞね。
goal=、planner_id= の指定があるから、下記で良いのかも?
<RecoveryNode number_of_retries="1" name="ComputePathToPose"> <!-- <ComputePathToPose goal="{goal}" path="{path}" planner_id="GridBased"/> --> <ComputePathToPose goal="{goal}" path="{path}" planner_id="GridBased" server_timeout="20" />

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 の方が出ています。
[controller_server-1] [ERROR] [1673498895.283545619] [controller_server]: Failed to make progress [controller_server-1] [WARN] [1673498895.283888128] [controller_server_rclcpp_node]: [follow_path] [ActionServer] Aborting handle. [controller_server-1] [INFO] [1673498895.301376398] [local_costmap.local_costmap]: Received request to clear entirely the local_costmap [controller_server-1] [INFO] [1673498895.303294104] [controller_server]: Received a goal, begin computing control effort. [controller_server-1] [INFO] [1673498895.504881301] [controller_server]: Passing new path to controller.
やはり、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