ROS2 で navigation2 の Active SLAM を試す。
--- ROS2 Gazbo Turtlebot3 で、Mapping しながら、 Navigationでロボットを動かす。 ---
(SLAM) Navigating While Mapping を参考に、ROS2 Gazebo Turtlebot3 を、動かしてみました。
PC 環境:
Ubuntu Mate 20.04
ROS2 :foxy
1. Gazebo turtlebot3_house Waffle の準備。
$ ros2 launch turtlebot3_gazebo turtlebot3_house.launch.py
で、Gazebo House が起動出来ますが、ロボットの起点がちょっと、ROS と比べて変なので、修正します。
~/colcon_ws/src/turtlebot3_simulations/turtlebot3_gazebo/worlds/turtlebot3_houses/waffle.model
注)ロボットの pose(位置と向き) は、tf base_footprint が、 tf odom の pose からの距離と向きの差で決まる。ので、tf odom を起点とした値を使います。
tf odom の x 軸は、ロボットの前、後、 y軸は、ロボットの左右になっていて、Rviz2 の開始時の画面が、90[度] 左に回転しているのがややこしいが、
Rviz2 の開始時の画面を、90[度] 右に回転すれば、理解しやすい。
tf base_footprint は、tf odom から、後ろに 3[m] (x軸で、-3.0) 、左に 1.0[m] (y軸で、+1.0) になります。これが、起動時のロボットの位置になります。
z軸=0.01 は、ちょっと浮かせているのかな? 向きは、変わらず。
tf odom は、tf map からの距離と、向が与えられる。
tf map は、 Map原点(左、下が 0,0) からの距離が、Map データ上から与えられる。
注)全ては、MAP 情報の定義で自由に決まる。
Turtlebot3 Gazebo の MAP 定義が上記のようになっているだけです。
2. (SLAM) Navigating While Mapping の 4- Getting Started Simplification を参考に、
$ ros2 launch nav2_bringup tb3_simulation_launch.py slam:=True
すれば、Gazbo も一緒に起動できるようですが、
なぜか、おんちゃんが試すと、Gazebo が起動途中でだんまりになります。
なので、今回は、先に、自分で Gazebo house を起動して、その後で、tb3_simulation_launch.py で、
Gazebo を起動させないように指定します。
実際の手順。
term1
$ export TURTLEBOT3_MODEL=waffle
$ ros2 launch turtlebot3_gazebo turtlebot3_house.launch.py
term2
$ export TURTLEBOT3_MODEL=waffle
$ ros2 launch nav2_bringup tb3_simulation_launch.py slam:=True use_simulator:=False
これで、OK です。
後は、Rviz2 上で、目的地を指定すると、自動で動きます。
上のアイコンの "Navigation2 Goal" を使って操作します。
3. Map の保存。
Map の保存は、
$ ros2 run nav2_map_server map_saver_cli -f ~/map
で行くよと書いていますが、これを行うとエラーになります。
こちらに回答がありました。
timeout error while saving map
$ ros2 run nav2_map_server map_saver_cli -f my_map --ros-args -p save_map_timeout:=10000.0
で、保存出来ました。
4. C++ プログラムから、navigation2 を使って、ロボットを動かす。
此処まで出来たので、後は、ROS でも試しましたが、
C++ プログラムから、リクエスト送って、ロボットを動かします。
ROS2 からは、navigation2 になった様です。
ROS2 Navigation2 Action Clientを使用してゴールを送信する を参考に、以前に作ったプログラムを移植します。
サンプルは、下記の方が良いかも。
1) navigation2/nav2_util/test/test_actions.cpp
の TEST_F(ActionTest, test_simple_action)、TEST_F(ActionTest, test_simple_action_with_feedback)
2) navigation2/nav2_rviz_plugins/src/nav2_pnale.cpp
の Nav2Panel::startWaypointFollowing(std::vector
auto goal_handle_ = client_ptr_->async_send_goal(...);
の後は、
rclcpp::spin_until_future_complete(node_,goal_handle_);
で、少し回さないと、関連する、イベントコールバックが、有効にならないみたいぞね。
注) 後日 github にアプロードします。
ROS2用に移植してテストをしていますが、なんだかチョットおかしい。
上述のように、turtlebot3_house.launch.py で、起点をずらして、Gazebo Tutlebot3 Houseで、
ros2 launch nav2_bringup tb3_simulation_launch.py slam:=True use_simulator:=False
が立ち上げて、そのままの場所で
ロボットを左 90[度] 、 右 90[度] 回転させてみましたが、
左 90[度] を指定すると、 tf map の方向に 90[度] の所で止まります。
右 90[度] を指定すると、ロボットの前方から約90[度] の所で止まります。
散々、自分のプログラムを見直しましたが、問題解決せずに、
試しに、Rviz2 から、"Navigation2 Goal" で同じ操作をすると、こちらでも同じ状況です。
原因がわかりました。nav2_params.yaml を調整しないといけない様です。
~/colcon_ws/src/navigation2/nav2_bringup/bringup/params/nav2_params.yaml
i) yaw_goal_tolerance: 0.08
ii) ついでに、ロボットの回転速度を落とします。
max_vel_theta: 0.25
iii) ついでに、到着距離を小さくします。add by nishi 2022.12.17
goal_checker:
xy_goal_tolerance: 0.05
FollowPath:
xy_goal_tolerance: 0.05
注) DWBLocalPlanner の場合、必ず、2箇所を同時に変更します。
goal_checker: だけ変更して、実機でテストしたら、なかなか、 Reached the goal! にならないので、苦労しました。