Turtlebot3 の自作 #12

4. launch ファイル。
rtabmap_ros_my/launch/rtabmap-nishi-stereoB_navigation3.launch
<!-- rtabmap-nishi-stereoB_navigation3.launch --> <launch> <!-- Arguments --> <arg name="model" default="$(env TURTLEBOT3_MODEL)" doc="model type [burger, waffle, waffle_pi]"/> <arg name="move_forward_only" default="false"/> <arg name="with_camera" default="true"/> <arg name="localization" default="false"/> <arg name="database_path" default="~/.ros/rtabmap.db"/> <arg if="$(arg localization)" name="rtabmap_args" default=""/> <arg unless="$(arg localization)" name="rtabmap_args" default="--delete_db_on_start"/> <arg name="move_base" default="true"/> <arg name="stereo_odometry" default="false"/> <arg name="SBC" default="false"/> <!-- Choose visualization --> <arg name="rtabmapviz_mode" default="false" /> <arg name="rviz" default="false" /> <arg name="tb3_rviz" default="false" /> <arg name="rviz_nishi" default="true" /> <arg name="pi/2" value="1.5707963267948966" /> <arg name="optical_rotate" value="0 0 0 -$(arg pi/2) 0 -$(arg pi/2)" /> <group if="$(arg SBC)"> <include file="$(find uvc_camera)/launch/stereo_node.launch"> </include> </group> <!-- Turtlebot3 --> <include unless="$(arg SBC)" file="$(find turtlebot3_bringup)/launch/turtlebot3_remote.launch"> <arg name="model" value="$(arg model)" /> </include> <group if="$(arg SBC)"> <node if="$(arg stereo_odometry)" pkg="tf" type="static_transform_publisher" name="camera_base_link" args="$(arg optical_rotate) base_link stereo_camera 100" /> <node unless="$(arg stereo_odometry)" pkg="tf" type="static_transform_publisher" name="camera_base_link" args="$(arg optical_rotate) base_footprint stereo_camera 100" /> <node pkg="tf" type="static_transform_publisher" name="camera2_base_link" args="0 0 0 0 0 0 stereo_camera source_frame 100" /> <node pkg="tf" type="static_transform_publisher" name="camera3_base_link" args="0 0 0 0 0 0 source_frame camera 100" /> </group> <!-- Run the ROS package stereo_image_proc --> <group if="$(arg SBC)" ns="/stereo_camera" > <node name="stereo_image_proc" pkg="stereo_image_proc" type="stereo_image_proc"> <remap from="/stereo_camera/right/image_raw" to="/right/image_raw" /> <remap from="/stereo_camera/right/camera_info" to="/right/camera_info" /> <remap from="/stereo_camera/left/image_raw" to="/left/image_raw" /> <remap from="/stereo_camera/left/camera_info" to="/left/camera_info" /> </node>> <!-- Odometry --> <node if="$(arg stereo_odometry)" pkg="rtabmap_ros" type="stereo_odometry" name="stereo_odometry" output="screen"> <remap from="left/image_rect" to="left/image_rect"/> <remap from="right/image_rect" to="right/image_rect"/> <remap from="left/camera_info" to="/left/camera_info"/> <remap from="right/camera_info" to="/right/camera_info"/> <param name="frame_id" type="string" value="base_link"/> <param name="odom_frame_id" type="string" value="odom"/> <!-- <param name="approx_sync" type="bool" value="false"/> --> <param name="approx_sync" type="bool" value="true"/> <param name="queue_size" type="int" value="5"/> <param name="Odom/MinInliers" type="string" value="12"/> <param name="Odom/RoiRatios" type="string" value="0.03 0.03 0.04 0.04"/> </node> </group> <group if="$(arg SBC)" ns="rtabmap"> <node name="rtabmap" pkg="rtabmap_ros" type="rtabmap" output="screen" args="$(arg rtabmap_args)"> <param if="$(arg stereo_odometry)" name="frame_id" type="string" value="base_link" /> <param unless="$(arg stereo_odometry)" name="frame_id" type="string" value="base_footprint" /> <param name="subscribe_stereo" type="bool" value="true"/> <param name="subscribe_depth" type="bool" value="false"/> <param if="$(arg stereo_odometry)" name="approx_sync" type="bool" value="true"/> <param unless="$(arg stereo_odometry)" name="approx_sync" type="bool" value="true"/> <remap from="left/image_rect" to="/stereo_camera/left/image_rect_color"/> <remap from="right/image_rect" to="/stereo_camera/right/image_rect"/> <remap from="left/camera_info" to="/left/camera_info"/> <remap from="right/camera_info" to="/right/camera_info"/> <param name="queue_size" type="int" value="30"/> <!-- use actionlib to send goals to move_base --> <param name="use_action_for_goal" type="bool" value="true"/> <remap from="move_base" to="/move_base"/> <!-- inputs --> <!-- <remap from="scan" to="/scan"/> --> <remap if="$(arg stereo_odometry)" from="odom" to="/stereo_camera/odom"/> <remap unless="$(arg stereo_odometry)" from="odom" to="/odom"/> <!-- output --> <remap from="grid_map" to="/map"/> <!-- <remap from="proj_map" to="/map"/> --> <!-- RTAB-Map's parameters --> <param name="Vis/MinInliers" type="string" value="12"/> <!-- <param name="Reg/Strategy" type="string" value="1"/> --> <param name="Reg/Strategy" type="string" value="0"/> <!-- <param name="Reg/Force3DoF" type="string" value="true"/> --> <param name="GridGlobal/MinSize" type="string" value="20"/> <!-- localization mode --> <param if="$(arg localization)" name="Mem/IncrementalMemory" type="string" value="false"/> <param unless="$(arg localization)" name="Mem/IncrementalMemory" type="string" value="true"/> <!-- test by nishi --> <!-- Grid, CellSize, float, 0.05, "Resolution of the occupancy grid.") --> <!-- Vis, MaxFeatures, int, 1000, "0 no limits." --> <!-- <param name="Vis/MaxFeatures" type="int" value="500"/> --> </node> </group> <group if="$(arg rtabmapviz_mode)" ns="rtabmap"> <!-- Visualisation RTAB-Map --> <node pkg="rtabmap_ros" type="rtabmapviz" name="rtabmapviz" args="-d $(find rtabmap_ros)/launch/config/rgbd_gui.ini" output="screen"> <param name="subscribe_stereo" type="bool" value="true"/> <param if="$(arg stereo_odometry)" name="subscribe_odom_info" type="bool" value="true"/> <param name="queue_size" type="int" value="10"/> <param if="$(arg stereo_odometry)" name="frame_id" type="string" value="base_link"/> <param unless="$(arg stereo_odometry)" name="frame_id" type="string" value="base_footprint"/> <remap from="left/image_rect" to="/stereo_camera/left/image_rect_color"/> <remap from="right/image_rect" to="/stereo_camera/right/image_rect"/> <remap from="left/camera_info" to="/left/camera_info"/> <remap from="right/camera_info" to="/right/camera_info"/> <remap if="$(arg stereo_odometry)" from="odom_info" to="/stereo_camera/odom_info"/> <remap if="$(arg stereo_odometry)" from="odom" to="/stereo_camera/odom"/> <remap unless="$(arg stereo_odometry)" from="odom" to="/odom"/> </node> </group> <group unless="$(arg SBC)"> <!-- move_base --> <include if="$(arg move_base)" file="$(find turtlebot3_navigation)/launch/move_base.launch"> <arg name="model" value="$(arg model)" /> <arg name="move_forward_only" value="$(arg move_forward_only)"/> <!-- odom を変えられみたい。--> <arg if="$(arg stereo_odometry)" name="odom_topic" value="/stereo_camera/odom" /> <arg unless="$(arg stereo_odometry)" name="odom_topic" value="/odom" /> </include> <!-- Visualisation RVIZ for demo_stereo_outdoor --> <node if="$(arg rviz)" pkg="rviz" type="rviz" name="rviz" args="-d $(find rtabmap_ros)/launch/config/demo_stereo_outdoor.rviz"/> <!-- rviz for turtlebot3_navigation --> <node if="$(arg tb3_rviz)" pkg="rviz" type="rviz" name="rviz" required="true" args="-d $(find turtlebot3_navigation)/rviz/turtlebot3_navigation.rviz"/> <node if="$(arg rviz_nishi)" pkg="rviz" type="rviz" name="rviz" args="-d $(find rtabmap_ros_my)/launch/config/nishi-stereoB-navi.rviz"/> </group> </launch>

5. 起動方法。
1) Remote PC
$ roscore

2) SBC(Jetson Nano 2G)
$ sudo chmod 666 /dev/ttyTHS1
$ roslaunch turtlebot3_bringup turtlebot3_robot.launch
Topic の一覧が出てくることを確認します。エラーになったら、一度　Ctl+Cで中止して、再度起動します。

$ export TURTLEBOT3_MODEL=waffle
$ roslaunch rtabmap_ros_my rtabmap-nishi-stereoB_navigation3.launch SBC:=true

3) リモートPC
$ export TURTLEBOT3_MODEL=waffle
$ roslaunch rtabmap_ros_my rtabmap-nishi-stereoB_navigation3.launch

6. 感想。
問題なく動きました。

リモートPCは、CPU 負荷が軽減されて、軽くなりました。
CPU0 30%
CPU1 90%

SBC(Jetson Nano 2G) は、どうか?
rtabmap の ターミナルの表示には、取りこぼしは出ていないようです。
たまに、base_link の TF出力が、0.02[Sec] を超えた旨のエラ－が出ています。

注1) rtabmap のパラメータは、 rtabmap_rosTutorialsStereoOutdoorMapping のほうが、
Mapping の性能が良いみたい(こちらをお勧めします)。
上記の方をベースに組み込んだ方がよいみたいです。
または、 rtabmap_ros/rtabmap の 設定を使った方が良いと思います。
<!-- RTAB-Map's parameters --> <param name="Rtabmap/TimeThr" type="string" value="700"/> <param name="Grid/DepthDecimation" type="string" value="4"/> <param name="Grid/FlatObstacleDetected" type="string" value="true"/> <param name="Kp/MaxDepth" type="string" value="0"/> <param name="Kp/DetectorStrategy" type="string" value="6"/> <param name="Vis/EstimationType" type="string" value="1"/> <!-- 0=3D->3D, 1=3D->2D (PnP) --> <param name="Vis/MaxDepth" type="string" value="0"/> <param name="RGBD/CreateOccupancyGrid" type="string" value="true"/>

注2) stereo_odometry は、使わない方が良いです。
foxbot_core3 の /odom と、IMUや、地磁気センサー、GPS 等を組み合わせた方が良いと思います。

7. 余談。
今日、Jetson Community Projects を見ていたら、ROS2 、 rtabmap_ros & Jetson Nano で 3D のPointClouds が、
めちゃくちゃ綺麗に取れているのを見た。
jdgalviss/jetbot-ros2
3D PointClouds で、床がちゃんと取れているのが、本当に良いぞね。
これは、やはり、RGB-D カメラの性能に拠るところが大きいと思われます。
これくらい綺麗に取れれば、屋外での草刈りロボットで使える気がしてきました。

だけど、rtabmap_ros は、リモートPC で動かしている様なので、将来は不安だ!...

Stereo Camera だと、床は、 2D Map では、白になるみたいだ。カメラが少し上向きなので、床が取れていないみたいです。

上り坂は、どうなるのか?

8. 余談2
Gazebo & turtlebot3 & rtabmap_ros で、シュミレーションで、 Depth Camera(rgb-d) を試せるみたいなので、早速試してみました。

rtabmap_rosTutorialsMappingAndNavigationOnTurtlebot の 6.1 Turtlebot3 on Melodic and Noetic
注) scan(Lidar) データは、不可にして、rgb-d だけにします。

後、屋外の Gazebo データが無いか Google でけんさくしたら。
CLEARPATH ROBOTS GET NEW GAZEBO SIMULATION ENVIRONMENTS があって、ここからのリンクで、
Clearpath Additional Simulation Worlds が見つかりました。
大変ありがたいぞね。
但し、起伏が無いデータみたいなのが残念です。
やはり、屋外なら、起伏のあるデータが必要ぞね。

既成のロボットカーも幾つかあるみたいだ。
https://support.clearpathrobotics.com/hc/en-us
高価なんだろな!!
手頃なら、こちらの方が簡単だったが!!

後、こちらも、次のステップでは、必要になりそう。
robot_localization
どうやら、複数のセンサー情報を使ってロボットの現在位置を出すノードみたいぞね。

9. ROS2 への移植。
ROS2 対応にしました。
ROS2 で、ESP32(Arduino) を使う。