search

rt-net / raspimouse_slam_navigation_ros2

ROS 2 SLAM and Navigation packages for Raspberry Pi Mouse
Apache License 2.0
8 stars 2 forks source link

ROS 2 Jazzy上のSLAMが動作しない #12

Closed KuraZuzu closed 1 week ago

KuraZuzu commented 2 weeks ago

不具合の概要

ROS 2 Jazzy上でGazebo上でのSLAMが動作しません（Raspberry Pi Mouse実機を用いないシミュレーション上）。 また、実機でのSLAMの動作確認は未実施です。

ros2 launch raspimouse_slam pc_slam.launch.py で生成されるGazebo上ではロボットが動かず、マップも生成されません。また、LiDARからの情報である距離情報のパーティクルも表示されません。

実行環境

再現方法

  1. ROS 2 のワークスペースを作成し、そのディレクトリに移動します。
$ mkdir -p ~/ros2_ws/src
$ cd ~/ros2_ws/src
  1. Jazzyに対応中の各種パッケージをros2_wsにインストールします。
$ git clone -b feature/support-jazzy https://github.com/rt-net/raspimouse2.git
$ git clone -b feature/support-jazzy https://github.com/rt-net/raspimouse_description.git
$ git clone -b feature/support-jazzy https://github.com/rt-net/raspimouse_ros2_examples.git
$ git clone -b feature/support-jazzy https://github.com/rt-net/raspimouse_slam_navigation_ros2.git
$ git clone -b feature/support-jazzy https://github.com/rt-net/raspimouse_sim.git
$ git clone -b https://github.com/rt-net/rt_usb_9axisimu_driver.git  # このパッケージはmasterブランチから変更無し

~/ros2_ws/srcから~/ros2_wsに移動してビルドします。

$ cd ~/ros2_ws
$ source /opt/ros/jazzy/setup.bash
$ colcon build --symlink-install
  1. 環境のモデルlakehouseでRaspberry Pi MouseのLiDARにurgを設定してRvizとGazeboシミュレータを起動します。
$ source ~/ros2_ws/install/setup.bash
$ ros2 launch raspimouse_gazebo raspimouse_with_lakehouse.launch.py lidar:=urg
  1. ターミナル2つ目を立ち上げ、キーボード操作を起動します。
$ source ~/ros2_ws/install/setup.bash
$ ros2 run teleop_twist_keyboard teleop_twist_keyboard --ros-args -p stamped:=true
  1. ターミナル3つ目を立ち上げ、SLAMを起動します。マッピングした情報が表示されるRvizが立ち上がります。
$ source ~/ros2_ws/install/setup.bash
$ ros2 launch raspimouse_slam pc_slam.launch.py
  1. 5.で立ち上げたRviz上でSLAMが動作しません。

期待する動作

ログ・画像

コマンドを実行すると、queueがいっぱいになるというエラーが出力されます。

$ ros2 launch raspimouse_slam pc_slam.launch.py
[INFO] [launch]: All log files can be found below /home/kuradesk/.ros/log/2024-10-01-15-02-42-524724-kuradesk-18044
[INFO] [launch]: Default logging verbosity is set to INFO
[INFO] [sync_slam_toolbox_node-1]: process started with pid [18047]
[INFO] [rviz2-2]: process started with pid [18048]
[sync_slam_toolbox_node-1] [INFO] [1727762562.939960245] [slam_toolbox]: Node using stack size 40000000
[rviz2-2] [INFO] [1727762563.484638118] [rviz2]: Stereo is NOT SUPPORTED
[rviz2-2] [INFO] [1727762563.484736054] [rviz2]: OpenGl version: 4.6 (GLSL 4.6)
[rviz2-2] [INFO] [1727762563.544172631] [rviz2]: Stereo is NOT SUPPORTED
[rviz2-2] [INFO] [1727762566.031346932] [rviz2]: Message Filter dropping message: frame 'laser' at time 6.400 for reason 'discarding message because the queue is full'
[rviz2-2] [INFO] [1727762566.231160729] [rviz2]: Message Filter dropping message: frame 'laser' at time 6.500 for reason 'discarding message because the queue is full'
[rviz2-2] [INFO] [1727762566.231212770] [rviz2]: Message Filter dropping message: frame 'odom' at time 6.864 for reason 'discarding message because the queue is full'
[rviz2-2] [INFO] [1727762566.430510081] [rviz2]: Message Filter dropping message: frame 'laser' at time 6.700 for reason 'discarding message because the queue is full'
[rviz2-2] [INFO] [1727762566.430562234] [rviz2]: Message Filter dropping message: frame 'odom' at time 7.064 for reason 'discarding message because the queue is full'

4.で立ち上げたターミナルからキーボードで操作したとき、3.で立ち上げたRvizとGazeboでは操作したとおりに動きます。 しかし、5.で立ち上げたRviz上では動作せず、SLAMの実行画面上ではLiDARのセンサ情報である赤いパーティクルも表示されません。また、ロボットのモデルも白色になってしまいます。

image

その他

わかっていることを記述します。

slam_toolboxがうまく動作していない

Node:/slam_toolboxは起動できているものの、Topic:/mapに正しくpublish/subscribeが出来ていないようです。 image

/mapと/odomの関係を一時的に固定してみるとqueueがいっぱいになるというエラーは消え、SLAM上でモデルが正しく表示されます。また、LiDARから得たセンサ情報の赤いパーティクルも表示されます。キーボードで操作したとおりに動きますが、しかし、マッピングはされません。

image

Humbleでも重複してSLAMを起動するとJazzyでも動作する

同じネットワーク上でHumble（rosdistro経由のaptでインストールできるもの）のSLAMを起動すると、Jazzy上のSLAMのRvizで想定した動作をし、マッピングもされます。

$ source ~/ros2_ws/install/setup.bash
$ ros2 launch raspimouse_gazebo raspimouse_with_lakehouse.launch.py lidar:=urg
$ source ~/ros2_ws/install/setup.bash
$ ros2 run teleop_twist_keyboard teleop_twist_keyboard --ros-args -p stamped:=true
$ ros2 launch raspimouse_slam pc_slam.launch.py
$ source ~/ros2_ws/install/setup.bash
$ ros2 launch raspimouse_slam pc_slam.launch.py

JazzyのSLAMのRvizが正常に動作し、Humbleと同期した動きをします。このことから、raspimouse_descriptionのURDFではなく、Jazzy上でのslam_toolboxの扱いに間違いがあると考えています。

KuraZuzu commented 2 weeks ago

Raspberry Pi Mouse V3実機で動作確認をしてみました。

実行した内容はこちら（lidarをldsにして実施してます）

# Raspberry Pi Mouse V3側で実行
ros2 launch raspimouse_slam robot_bringup.launch.py lidar:=rplidar joyconfig:=f710

# リモートPC側で以下のコマンドを実行
ros2 launch raspimouse_slam pc_slam.launch.py

RaspBerry Pi Mouse V3側ではLiDARやモータ・ジョイコンが動作するものの、リモートPC側のRvizで失敗しました。シミュレーションのみのときと同様に/mapが無いと言われるようです。

ros2 launch raspimouse_slam pc_slam.launch.pyのコマンドでRvizの表示が失敗 image

Node・Topicリスト

$ ros2 node list
/hlds_laser_publisher
/joint_state_publisher
/joy_node
/joystick_control
/launch_ros_2931
/raspimouse
/robot_state_publisher

$ ros2 topic list
/buzzer
/cmd_vel
/diagnostics
/joint_states
/joy
/joy/set_feedback
/leds
/light_sensors
/map
/map_metadata
/odom
/parameter_events
/pose
/raspimouse/transition_event
/robot_description
/rosout
/scan
/slam_toolbox/feedback
/slam_toolbox/graph_visualization
/slam_toolbox/scan_visualization
/slam_toolbox/update
/switches
/tf
/tf_static

rqt_graph

rqt_graphではこのようになっています。シミュレーションのみのときとの違いとして/slam_toolboxの有無があります。先程のNodeリストからもわかるように、/slam_toolboxが存在しません。こちらはシミュレーションのみのときは合ったはずです。

image

Topic:/mapはこのようになっていました。

$ ros2 topic info /map
Type: nav_msgs/msg/OccupancyGrid
Publisher count: 1
Subscription count: 1
$ ros2 topic echo /map
A message was lost!!!
    total count change:1
    total count: 1---
header:
  stamp:
    sec: 1727775415
    nanosec: 909966583
  frame_id: map
info:
  map_load_time:
    sec: 0
    nanosec: 0
  resolution: 0.009999999776482582
  width: 585
  height: 481
  origin:
    position:
      x: -2.1770256830467316
      y: -1.4195476215536593
      z: 0.0
    orientation:
      x: 0.0
      y: 0.0
      z: 0.0
      w: 1.0
data:
- -1
- -1
- -1
- -1
- -1
- -1
（以下同じ...）
YusukeKato commented 2 weeks ago

slam_toolboxはJazzyからLifecycleNodeを使っているようです。 https://github.com/SteveMacenski/slam_toolbox/blob/jazzy/launch/online_sync_launch.py#L40

参考：Humbleのとき↓ https://github.com/SteveMacenski/slam_toolbox/blob/humble/launch/online_sync_launch.py#L24

KuraZuzu commented 1 week ago

LifeCycleNodeに変更し、無事動作しました。