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

3. Remote PC でのインストール。
3.1 navigation2 のインストール。
Remote PC に navigaton2 をインストールします。 nav2 build and install
パッチを当てないといけないので、source build します。
3. 1.1 navigatino2 のパッチ。
foxy navigation2 には、不具合があるので、パッチが必要。
bt_navigator-4] [INFO] [1671009087.452352684] [bt_navigator]: Begin navigating from current location to (1.00, 0.00) [controller_server-1] [INFO] [1671009087.474864910] [controller_server]: Received a goal, begin computing control effort. [bt_navigator-4] [ERROR] [1671009087.474989733] [bt_navigator]: Action server failed while executing action callback: "send_goal failed" [bt_navigator-4] [WARN] [1671009087.475026190] [bt_navigator]: [navigate_to_pose] [ActionServer] Aborting handle. [bt_navigator-4] [INFO] [1671009088.513209802] [bt_navigator]: Begin navigating from current location to (0.11, -0.00)
Would not halt all actions while exception thrown during running BT #2272
BehaviorTreeEngine::run() に、パッチを入れる。
Galactic 版に、実際のパッチが入っているので、それを、foxy-devel 版に、back port すれば良い。
behavior_tree_engine.cpp
こちらの方が、ログが出てくるので、良い。

2箇所必要なので、具体的に書きました。 by nishi 2022.12.25
navigation2/nav2_behavior_tree/src/behavior_tree_engine.cpp
... BtStatus BehaviorTreeEngine::run( BT::Tree * tree, std::function<void()> onLoop, std::function<bool()> cancelRequested, std::chrono::milliseconds loopTimeout) { rclcpp::WallRate loopRate(loopTimeout); BT::NodeStatus result = BT::NodeStatus::RUNNING; // Loop until something happens with ROS or the node completes //while (rclcpp::ok() && result == BT::NodeStatus::RUNNING) { // if (cancelRequested()) { // tree->rootNode()->halt(); // return BtStatus::CANCELED; // } // // result = tree->tickRoot(); // // onLoop(); // // loopRate.sleep(); //} // Loop until something happens with ROS or the node completes try { while (rclcpp::ok() && result == BT::NodeStatus::RUNNING) { if (cancelRequested()) { tree->rootNode()->halt(); return BtStatus::CANCELED; } result = tree->tickRoot(); onLoop(); loopRate.sleep(); } } catch (const std::exception & ex) { RCLCPP_ERROR( rclcpp::get_logger("BehaviorTreeEngine"), "Behavior tree threw exception: %s. Exiting with failure.", ex.what()); return BtStatus::FAILED; } return (result == BT::NodeStatus::SUCCESS) ? BtStatus::SUCCEEDED : BtStatus::FAILED; } ... // In order to re-run a Behavior Tree, we must be able to reset all nodes to the initial state void BehaviorTreeEngine::haltAllActions(BT::TreeNode * root_node) { // add by nishi 2022.12.25 // back port from galactic if (!root_node) { return; } // this halt signal should propagate through the entire tree. root_node->halt(); // but, just in case... auto visitor = [](BT::TreeNode * node) { if (node->status() == BT::NodeStatus::RUNNING) { node->halt(); } }; BT::applyRecursiveVisitor(root_node, visitor); } ..
BehaviorTreeEngine::run()
BehaviorTreeEngine::haltAllActions()
の2箇所に、galactic版の、back port パッチを入れます。
上記、BehaviorTreeEngine::haltAllActions() に、パッチを入れても、下記エラーを吐いて、bt_navigator が、die します。
[bt_navigator-4] [ERROR] [1671942409.346953865] [BehaviorTreeEngine]: Behavior tree threw exception: send_goal failed. Exiting with failure. [ERROR] [bt_navigator-4]: process has died [pid 8407, exit code -11, cmd '/home/nishi/colcon_ws/install/nav2_bt_navigator/lib/nav2_bt_navigator/bt_navigator --ros-args -r __node:=bt_navigator --params-file /tmp/tmpb8jl5dgy -r /tf:=tf -r /tf_static:=tf_static'].
これは、特殊な場合の様です。
T100TA、あるいは、SBC(Jetson Nano 2G) で、navigation2 and teb_local_planner を起動すると、出る様です。
CPUが非力な場合の様です。

navigation2/nav2_bt_navigator/src/bt_navigator.cpp に、loop.sleep() を入れて様子見です。
注) galactic 版だと、navigation2/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp になる。
void BtNavigator::navigateToPose() { ... // memo by nishi // on galactic // navigation2/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp // line 239 // Execute the BT that was previously created in the configure step nav2_behavior_tree::BtStatus rc = bt_->run(&tree_, on_loop, is_canceling); // add by nishi if(rc==nav2_behavior_tree::BtStatus::FAILED){ using namespace std::chrono_literals; //rclcpp::WallRate loop(10); rclcpp::WallRate loop(5); // update by nishi 2023.1.15 loop.sleep(); } // Make sure that the Bt is not in a running state from a previous execution // note: if all the ControlNodes are implemented correctly, this is not needed. bt_->haltAllActions(tree_.rootNode());
注) delay を増やしました。
rclcpp::WallRate loop(5); // update by nishi 2023.1.15

注) たまに、navigation2 の方で、下記ログが出てくるので、問題は残っています。
[controller_server-1] [INFO] [1671177881.620306978] [local_costmap.local_costmap_rclcpp_node]: Message Filter dropping message: frame 'camera' at time 1671177881.186 for reason 'Unknown'
time stamp がどうなの!!
$ python >>> import time >>> import datetime >>> t1=1671177881.186 >>> dt1 = datetime.datetime.fromtimestamp(t1) >>> print(dt1) 2022-12-16 17:04:41.186000
問題ないか?
注) もしかしたら、nav2_params.yaml の記述の誤りか?
local_costmap: local_costmap: local_costmap_client: local_costmap_rclcpp_node: # ここの indent が、左にずれていたかも? ros__parameters: #use_sim_time: True use_sim_time: False # indent がずれて、この値が無視されていたかも?

3.2 ROS2 foxy 版、Rtamap_ros をインストール。
introlab / rtabmap_ros

3.3 ROS2 foxy 版、turtlebot3_navi_my をインストール。
tosa-no-onchan/turtlebot3_navi_my

4. SBC でのインストール。
4.1 OpenCV のビルド。
チョット古いですが、Jetson Nano 2G OpenCV4.1.1 Build を参照。
Jetson Nano 2G が、 JetPack 4.X + Ubuntu 20.04 であれば、以前よりは、簡単ぞね。
Ubuntu 20.04 のデフォルトバージョン OpenCV 4.2.0 ? にします。

4.2 Ros2 版 Single USB Stereo Camera のインストール。
uvc_camera_single

4.3 Ros2 stereo_image_proc のインストール。
パッケージ版では、上手く動かないので、source build 版をインストール。
ros-perception/image_pipeline
foxy 版をインストール。

4.4 ROS2 foxy 版、Rtamap_ros をインストール。
introlab / rtabmap_ros
注) SBC 側にも必要でした。
ROS2 で、Rtabmap_ros を試す。 を参照。

5. rtabmap_ros_my のインストール。
Remote PC and SBC
tosa-no-onchan/rtabmap_ros_my

6. 実行
rtabmap_ros_my/launch/ratbmap_stero_rgbd.launch.py を実行します。
実行方法は、上記ファイルの上部に記載してあります。
local_planner は、dwb_core::DWBLocalPlanner 、teb_local_planner::TebLocalPlannerROS どちらでも、動作確認できました。

7. teb_local_planner (Remote PC)
パッケージ版が無いので、source build します。
rst-tu-dortmund/teb_local_planner
注) costmap_converter も必要です。

やれやれ。書くのも大変じゃ。

8. galactic に移行しました。
結論を先に言えば、galactic 版だと、foxy で現れた、Packageのバグがでなくて、より簡単ぞね。
上記の様な、Packageの修正は何もなく、取り敢えず、DWBLocalPlanner で、完走できました。by nishi 2023.1.20

8.1 teb_local_planner で試す。by nishi 2023.1.21
teb_local_planner も、Navigation2 の controller に、 追加のplugin として組み込んでいるだけみたい。
/plugin_tutorials/docs/writing_new_nav2controller_plugin/#3- Pass the plugin name through the params file

サンプルの teb_params.yam を流用すれば、簡単に動かせました。
但し、xy_goal_tolerance、yaw_goal_tolerance には、別途 goal checker plugin の記述が必要でした。

controller_server: ros__parameters: ... # add by nishi 2023.1.21 goal_checker_plugins: ["general_goal_checker"] # "precise_goal_checker" ... # add by nishi 2023.1.21 general_goal_checker: stateful: True plugin: "nav2_controller::SimpleGoalChecker" #xy_goal_tolerance: 0.25 # changed by nishi xy_goal_tolerance: 0.05 #yaw_goal_tolerance: 0.25 # changed by nishi yaw_goal_tolerance: 0.08 ...

1) global planner が、具合が悪いみたい。
途中で、機能不全になる。

planner_server: ros__parameters: expected_planner_frequency: 20.0 #use_sim_time: True use_sim_time: False planner_plugins: ["GridBased"] GridBased: # https://navigation.ros.org/configuration/packages/configuring-navfn.html plugin: "nav2_navfn_planner/NavfnPlanner" #tolerance: 0.5 # test by nishi tolerance: 0.6 use_astar: false allow_unknown: true

tolerance: 0.5
では、目的地へのコースを算出できないみたい。
問題点としては、global costmap で、
obstacle_layer
data_type: "PointCloud2"
を使っていて、ロボットが前傾した時に、部屋の床が、障害物になっている所為のような気がします。
此処を、対処することが先決か?

local costmap も同様に、床が障害物判定される影響で、通り道が塞がれるので、Success!! にならない。

voxel pulgin、 obstacle plugin の、
<voxel layer>. <data source>.min_obstacle_height: 0.6
<obstacle layer>. <data source>.min_obstacle_height: 0.6
にした。
max_obstacle_height、min_obstacle_height をなるべく近づけて、上下の幅を狭くして、LaserScan のように使うほうがよいかも。

あと、ロボットの半径と障害物の半径を、下記にして、
robot_radius: 0.18 -- ロボットが、回転する際に描く円の半径。
inflation_radius: 0.2 -- 0.0 でも良い気がするが。
実際の、foxbot_core3 ロボットサイズに近づけて、大体、完走できるようになりました。
local costmap、global costmap に障害物が検出されて、目的地へのコースが塞がれると、右往左往します。