はじめに
前回の記事では、Gazebo上で地図を作成するための world を作成しました。
今回は、作成した world 上でロボットを走らせ、地図を作成する方法について説明します。
前提条件
前提条件は、以下の通りです。
- worldファイルの作成が完了している
- Rviz2 上で /map を表示する方法が分かる (わからない方はこちらを参考)
/map の表示に関しては、この記事に簡単に入れておきます。
slam_toolbox について
slam-toolbox は、ROS2 で SLAM を実装するためのパッケージとなります。
github のページはこちらにあります。
簡単に説明すると (詳しく説明できません) マッピングとローカリゼーションができます。
他にも様々な機能がありますが、今回は上記を使用します。
C++ での解説はまた改めてできればと思います。。。
ユーザー側の準備する内容は以下の通りです。
- SLAM 設定用の yaml ファイル
- launch ファイルにて、slam_toolbox の呼び出し
- /odom, /base_link, /scan トピックの用意
上記があれば、簡単にできてしまいます。
/odom は ekf にて出力され、/base_link, /scan は URDF にて指定します。
SLAM 設定用の YAML ファイルの作成
早速、yaml ファイルを用意していきます。
cd ~/ros_ws/src/ros2_first_test
touch config/first_slam_settings.yamlfirst_slam_settings.yaml に以下の内容を貼付してください。
slam_toolbox:
ros__parameters:
# Plugin params
solver_plugin: solver_plugins::CeresSolver
ceres_linear_solver: SPARSE_NORMAL_CHOLESKY
ceres_preconditioner: SCHUR_JACOBI
ceres_trust_strategy: LEVENBERG_MARQUARDT
ceres_dogleg_type: TRADITIONAL_DOGLEG
ceres_loss_function: None
# ROS Parameters
odom_frame: odom
map_frame: map
base_frame: base_link
scan_topic: scan
mode: mapping #localization
debug_logging: false
throttle_scans: 1
transform_publish_period: 0 #if 0 never publishes odometry
map_update_interval: 1.0
resolution: 0.05
max_laser_range: 20.0 #for rastering images
minimum_time_interval: 0.5
transform_timeout: 0.2
tf_buffer_duration: 30.
stack_size_to_use: 40000000 #// program needs a larger stack size to serialize large maps
enable_interactive_mode: true
# General Parameters
use_scan_matching: true
use_scan_barycenter: true
minimum_travel_distance: 0.5
minimum_travel_heading: 0.5
scan_buffer_size: 10
scan_buffer_maximum_scan_distance: 10.0
link_match_minimum_response_fine: 0.1
link_scan_maximum_distance: 1.5
loop_search_maximum_distance: 3.0
do_loop_closing: true
loop_match_minimum_chain_size: 10
loop_match_maximum_variance_coarse: 3.0
loop_match_minimum_response_coarse: 0.35
loop_match_minimum_response_fine: 0.45
# Correlation Parameters - Correlation Parameters
correlation_search_space_dimension: 0.5
correlation_search_space_resolution: 0.01
correlation_search_space_smear_deviation: 0.1
# Correlation Parameters - Loop Closure Parameters
loop_search_space_dimension: 8.0
loop_search_space_resolution: 0.05
loop_search_space_smear_deviation: 0.03
# Scan Matcher Parameters
distance_variance_penalty: 0.5
angle_variance_penalty: 1.0
fine_search_angle_offset: 0.00349
coarse_search_angle_offset: 0.349
coarse_angle_resolution: 0.0349
minimum_angle_penalty: 0.9
minimum_distance_penalty: 0.5
use_response_expansion: trueekfの記事から今回の分までの詳細な説明は次回に回します。
とにかく今回は /map を確認できるところまで動くようにできればと思います。
Localization 用の launch ファイル作成
Localization 用の launch ファイルは前回も紹介しましたが、再掲します。
import os
from ament_index_python.packages import get_package_share_directory
from launch_ros.actions import Node
import launch
from launch import LaunchDescription
from launch.substitutions import LaunchConfiguration
def generate_launch_description():
package_dir = get_package_share_directory("ros2_first_test")
urdf = os.path.join(package_dir, "urdf" , "first_robot.urdf")
rviz = os.path.join(package_dir, "rviz" , "first_robot.rviz")
ekf_path = os.path.join(package_dir, "config", 'first_ekf.yaml')
use_sim_time = LaunchConfiguration('use_sim_time', default='true')
world_path = os.path.join(package_dir, 'world', 'first_robot.sdf')
return LaunchDescription([
launch.actions.DeclareLaunchArgument(name='gui', default_value='True',
description='Flag to enable joint_state_publisher_gui'),
Node(
package='robot_state_publisher',
executable='robot_state_publisher',
name='robot_state_publisher',
output='screen',
arguments=[urdf],
),
Node(
package='joint_state_publisher',
executable='joint_state_publisher',
name='joint_state_publisher',
output='screen',
condition=launch.conditions.UnlessCondition(LaunchConfiguration('gui')),
),
Node(
package='robot_localization',
executable='ekf_node',
name='ekf_filter_node',
output='screen',
parameters=[ekf_path, {'use_sim_time': use_sim_time}]),
# gazebo settings
launch.actions.ExecuteProcess(
cmd=['gazebo', '--verbose', '-s', 'libgazebo_ros_init.so', '-s', 'libgazebo_ros_factory.so', world_path], output='screen'),
Node(
package="gazebo_ros",
executable="spawn_entity.py",
name="urdf_spawner",
output="screen",
parameters=[{'use_sim_time': use_sim_time}],
arguments=["-topic", "/robot_description", "-entity", "first_robot"],
),
Node(
package="rviz2",
executable="rviz2",
name="rviz2",
arguments=["-d", rviz],
parameters=[{'use_sim_time': use_sim_time}],
output='screen'),
])Mapping 用の launch ファイル作成
Mapping 用の launch ファイルでは、first_slam_settings.yaml を読み込んで、slam_toolbox に渡すようにします。
import os
from ament_index_python.packages import get_package_share_directory
from launch_ros.actions import Node
from launch import LaunchDescription
from launch.substitutions import LaunchConfiguration
def generate_launch_description():
package_dir = get_package_share_directory("ros2_first_test")
use_sim_time = LaunchConfiguration('use_sim_time', default='true')
slam_params = os.path.join(package_dir, "config", 'first_slam_settings.yaml')
return LaunchDescription([
Node(
parameters=[
{"slam_params_file": slam_params},
{'use_sim_time': use_sim_time}
],
package='slam_toolbox',
executable='async_slam_toolbox_node',
name='slam_toolbox',
output='screen'),
])以上で準備は完了です。
Gazebo シミュレーション環境で Mapping
今回は launch ファイルを 2 つ用意しました。
最初にfirst_localization.launch.py を起動して、その後に first_slam.launch.py を起動します。
cd ~/ros_ws
colcon build --packages-select ros2_first_test
source ~/.bashrc
ros2 launch ros2_first_test first_localization.launch.py上記を実行すると、Rviz2 と Gazebo が起動します。
Rviz 上で /map トピックを表示できるようにします。
まだ slam_toolbox を起動していないので、Map トピックは赤字になっていると思います。
次に、first_slam.launch.py を実行することで、Map トピックを Rviz2 がサブスクライブできるようになります。
ros2 launch ros2_first_test first_slam.launch.pyこれを実行し、Map の topic の欄に “/map” を入力すると Rviz2 上にグレースケールのようなものが表示されます。
これで Map 作成の準備は完了しました。
今回は、ひとまずここまでとさせていただきます。
おわりに
今回は Map 作成の準備までとしました。
ここに至るまでに、odom が地面の中にめり込んだり、/odom トピックが正しくパブリッシュされなかったり、多くの課題がありました。
日本語記事が少ないのも課題の一つでした。
次回はいよいよ、ロボットを動かして 完全な Map を作成していきます。
その後、yaml ファイルの詳細やパラメータ変更の影響について説明できればと思います。
コメント
コメント失礼します。
等サイト同様に自己位置推定のシミュレーションを行っています。
現在、「Gazebo シミュレーション環境で Mapping」で行き詰まっており、Rviz 上で /map と入力しても変化がなく、Map作成ができない状態です。
mapをrvizで選択し、first_slam.launch.py を実行すると、rvizのmap欄が赤色から黄色に変化しますが、warnとなります。
私は「first_slam_settings.yaml 」に原因があると思っています。作成しているurdfのロボットはlidarのみ搭載しており、IMUは搭載していません。そのため、その場合は、first_slam_settings.yaml 内のコードをどこか変更する必要があるでしょうか。
また、他にも変更したほうが良い箇所をご教授お願いしたいです。
ご確認のほどよろしくおねがいします。
コメントありがとうございます。
/map欄が黄色になるということは、おそらく /odom が正常でない可能性があります。
コンソールに /map に関するエラーが出力されていると思います。
ここからは、予想での回答になってしまいますが
https://nutritionfoodtech.com/2022/11/11/slam-%e8%87%aa%e5%b7%b1%e4%bd%8d%e7%bd%ae%e6%8e%a8%e5%ae%9a-%e3%81%ae%e3%81%9f%e3%82%81%e3%81%ae-localization-launch-%e3%83%95%e3%82%a1%e3%82%a4%e3%83%ab%e3%81%ae%e8%a8%ad%e5%ae%9a-ros2-foxy/
上記ページの first_ekf.yaml も変更する必要があるかと思われます。
センサ融合のために /imu を設定していますので、削除してみてください。
ros2 topic list で /odom があるか確認する必要もあるかと思います。
ご返信ありがとうございます。返信が遅くなり申し訳ございません。
添付して頂いたurlのfirst_ekf.yamlを作成していなかったため、imuのコードを消して作成しました。そして、first_localization.launch.pyの内容を変更したのですが、
Node(
package=’robot_localization’,
executable=’ekf_node’,
name=’ekf_filter_node’,
output=’screen’,
parameters=[ekf_path, {‘use_sim_time’: use_sim_time}]),
上記のコードをlaunchファイルに追加すると、エラーとなりgazeboが立ち上がりません。
エラー内容を以下に示します。
[ERROR] [robot_state_publisher-1]: process has died [pid 7976, exit code -2, cmd ‘/opt/ros/foxy/lib/robot_state_publisher/robot_state_publisher /home/kamalab/ros2_ws/install/ros2_first_test/share/ros2_first_test/urdf/first_robot.urdf –ros-args -r __node:=robot_state_publisher’].
[ERROR] [joint_state_publisher-2]: process[joint_state_publisher-2] failed to terminate ‘5’ seconds after receiving ‘SIGINT’, escalating to ‘SIGTERM’
[INFO] [joint_state_publisher-2]: sending signal ‘SIGTERM’ to process[joint_state_publisher-2]
[ERROR] [joint_state_publisher-2]: process has died [pid 7978, exit code -15, cmd ‘/opt/ros/foxy/lib/joint_state_publisher/joint_state_publisher /home/kamalab/ros2_ws/install/ros2_first_test/share/ros2_first_test/urdf/first_robot.urdf –ros-args -r __node:=joint_state_publisher’].
また、上記のコードを追加せずに、gazeboとrvizを立ち上げて「add」からmapを追加し、
「ros2 launch ros2_first_test first_slam.launch.py」を立ち上げると、rvizのtfが緑色から黄色となりWarnとなります。mapも同様に黄色です。
これらのエラーが表示されている場合は、どのように対処すれば良いでしょうか。
ご教授お願いいます。
コメントありがとうございます。
first_ekf.yamlでは、
/first_robot/odom
/first_robot/imu
を基に
/odom
を出力します。
あさんの場合は、
/first_robot/odom
から
/odom を出力するので
/first_robot/odomのトピック名が存在しないと、エラーが出ると思います。
/first_ekf.yamlの設定に問題はありませんでしょうか？
もしくは、URDF, World ファイルのどちらかに Imu の設定が残っているかもしれません。
ROS2 のエラー文ですが、process has died のエラーは他のエラーによって引き起こされます。
URDF Spawn あたりに大元のエラーが表示されていると思います。
お手数ですがご確認お願いします。
もう一つ、そもそも ekf_filter を使用しない方法としては
URDF, World ファイルの
/first_robot/odom
を出力する部分を
/odom
に変更する方法もあります。
tf や map が黄色になり Warn が出力されるエラーは、受信した型が不正かトピック自体が見つからないことにより引き起こされます。
ご返信ありがとうございます。
/first_robot/odomのトピックが存在していることは確認しています。
また、urdfとworldファイルにimuのコードはなかったです。
ROS2のエラー文の件ですが、「URDF Spawn」が無知で申し訳ございませんが、わからないため、どういうものか教えていただきたいです。
もしかしたらですが、GAZEBOを起動したときのターミナルが永遠とエラー（Gazeboは動かせます）となっているため、これが原因かもしれません。以下にエラー文を載せます。
[rviz2-5] Warning: TF_OLD_DATA ignoring data from the past for frame wheel_front_right at time 1167.376000 according to authority Authority undetectable
[rviz2-5] Possible reasons are listed at http://wiki.ros.org/tf/Errors%20explained
[rviz2-5] at line 332 in /tmp/binarydeb/ros-foxy-tf2-0.13.14/src/buffer_core.cpp
何度も質問で申し訳ございませんが、ご教授よろしくお願い致します。
ありがとうございます。
gazebo 起動コマンドを実行したあとに、今回でいうと
ros2 launch ros2_first_test first_localization.launch.py
を実行した後すぐの文から確認していただくと、該当するエラーがあるかと思われます。
Warning: TF_OLD_DATA ignoring data from the past for frame wheel_front_right at time 1167.376000 according to authority Authority undetectable
に関しましては、gazebo と rviz の時間軸が合わないことに起因するエラーですので
gazebo のシミュレーション時間をリセットすると、解消されると思います。
今回の場合は、無視しても大丈夫です、
私の方も解決方法を提案できず、申し訳ないです。
追加のコメント失礼します。
https://nutritionfoodtech.com/2022/11/11/slam-%e8%87%aa%e5%b7%b1%e4%bd%8d%e7%bd%ae%e6%8e%a8%e5%ae%9a-%e3%81%ae%e3%81%9f%e3%82%81%e3%81%ae-localization-launch-%e3%83%95%e3%82%a1%e3%82%a4%e3%83%ab%e3%81%ae%e8%a8%ad%e5%ae%9a-ros2-foxy/
上記のurl内「ros2 node info /ekf_filter_node」をターミナルに入力し、実行するとエラーとなります。エラーは以下に示します。
Unable to find node ‘/ekf_filter_node’
また、「ros2 topic echo /odometry/filtered」を実行すると、以下のエラーとなります。
WARNING: topic [/odometry/filtered] does not appear to be published yet
Could not determine the type for the passed topic
上記のようになります。原因を教えていただきたいです。
Unable to find node ‘/ekf_filter_node’ ということは ekf 自体が機能していませんので、
first_ekf.yaml の設定を見直す必要があります。
ros2 topic list
で出力されるトピックのリストと first_ekf.yaml の設定した部分を
送付いただくことは可能でしょうか？
ご返信ありがとうございます。
トピックリストを以下に示します。
/camera1/camera_info
/camera1/image_raw
/clicked_point
/clock
/goal_pose
/initialpose
/joint_states
/parameter_events
/robot_description
/rosout
/scan
/tf
/tf_static
/weeding_robot/cmd_vel
/weeding_robot/odom
weeding_robotというのは本サイトのfirst_robotに当たります。
また、first_ekf.yamlのコードを以下に示します。
### ekf config file ###
ekf_filter_node:
ros__parameters:
# The frequency, in Hz, at which the filter will output a position estimate. Note that the filter will not begin
# computation until it receives at least one message from one of theinputs. It will then run continuously at the
# frequency specified here, regardless of whether it receives more measurements. Defaults to 30 if unspecified.
frequency: 30.0
# ekf_localization_node and ukf_localization_node both use a 3D omnidirectional motion model. If this parameter is
# set to true, no 3D information will be used in your state estimate. Use this if you are operating in a planar
# environment and want to ignore the effect of small variations in the ground plane that might otherwise be detected
# by, for example, an IMU. Defaults to false if unspecified.
two_d_mode: false
# Whether to publish the acceleration state. Defaults to false if unspecified.
publish_acceleration: true
# Whether to broadcast the transformation over the /tf topic. Defaultsto true if unspecified.
publish_tf: true
# 1. Set the map_frame, odom_frame, and base_link frames to the appropriate frame names for your system.
# 1a. If your system does not have a map_frame, just remove it, and make sure “world_frame” is set to the value of odom_frame.
# 2. If you are fusing continuous position data such as wheel encoder odometry, visual odometry, or IMU data, set “world_frame”
# to your odom_frame value. This is the default behavior for robot_localization’s state estimation nodes.
# 3. If you are fusing global absolute position data that is subject to discrete jumps (e.g., GPS or position updates from landmark
# observations) then:
# 3a. Set your “world_frame” to your map_frame value
# 3b. MAKE SURE something else is generating the odom->base_link transform. Note that this can even be another state estimation node
# from robot_localization! However, that instance should *not* fuse the global data.
map_frame: map # Defaults to “map” if unspecified
odom_frame: odom # Defaults to “odom” if unspecified
base_link_frame: base_footprint # Defaults to “base_link” ifunspecified
world_frame: odom # Defaults to the value of odom_frame if unspecified
odom0: weeding_robot/odom
odom0_config: [true, true, false, # position of xyz
false, false, true, # angle of rpy
true, true, false, # velocity of xyz
false, false, true, # angular velocity of rpy
true, true, false,] # acceleraion of xyz
ご確認よろしくお願い致します。
ありがとうございます。
/odom が出力されていないので、ekf_filter は weeding_robot/odom のみだと
機能しないのかもしれませんね。(確認できておらず申し訳ないです)
少し手間になりますが、
urdf 及び world ファイルの/weeding_robot/odom を /odom に変更し、
ekf_filter.yaml を使用せず
→Node(
package=’robot_localization’,
executable=’ekf_node’,
name=’ekf_filter_node’,
output=’screen’,
parameters=[ekf_path, {‘use_sim_time’: use_sim_time}]),
を削除したうえで
ros2 launch ros2_first_test first_localization.launch.py
を実行するとどうでしょうか。
実行後の ros2 topic list も送付いただけると助かります。
ご返信ありがとうございます。
urdfやworldファイルに「/weeding_robot/odom」このような記述がないのですが、どこか間違っているでしょうか。
また、ekf_filter.yamlを使用せずに実行した場合のros2 topic listは以下のようになりました。
/camera1/camera_info
/camera1/image_raw
/clicked_point
/clock
/goal_pose
/initialpose
/joint_states
/map
/map_metadata
/map_updates
/parameter_events
/robot_description
/rosout
/scan
/slam_toolbox/feedback
/slam_toolbox/graph_visualization
/slam_toolbox/scan_visualization
/slam_toolbox/update
/tf
/tf_static
/weeding_robot/cmd_vel
/weeding_robot/odom
また、以下のコードを追加して、ros2を実行した場合のエラー文を示します。
Node(
package=’robot_localization’,
executable=’ekf_node’,
name=’ekf_filter_node’,
output=’screen’,
parameters=[ekf_path, {‘use_sim_time’: use_sim_time}]),
以下エラー文
[ERROR] [joint_state_publisher-2]: process[joint_state_publisher-2] failed to terminate ‘5’ seconds after receiving ‘SIGINT’, escalating to ‘SIGTERM’
[INFO] [joint_state_publisher-2]: sending signal ‘SIGTERM’ to process[joint_state_publisher-2]
[ERROR] [joint_state_publisher-2]: process has died [pid 10962, exit code -15, cmd ‘/opt/ros/foxy/lib/joint_state_publisher/joint_state_publisher /home/kamalab/ros2_ws/install/ros2_first_test/share/ros2_first_test/urdf/first_robot.urdf –ros-args -r __node:=joint_state_publisher’].
長文となり申し訳ございません。よろしくお願い致します。
topic list の送付ありがとうございます。
https://nutritionfoodtech.com/2022/10/15/gazebo%e3%81%a7%e3%83%87%e3%82%a3%e3%83%95%e3%82%a1%e3%83%ac%e3%83%b3%e3%82%b7%e3%83%a3%e3%83%ab%e3%83%89%e3%83%a9%e3%82%a4%e3%83%96%e3%80%81%e3%82%ab%e3%83%a1%e3%83%a9%e3%82%92%e3%82%b7%e3%83%9f/#toc5
こちらで、URDFにディファレンシャルドライブのプラグインを追加しています。
の
ですので、今回の場合は
削除後、ros2 topic list で確認いただければと思います。
送付いただいたエラー文はターミナルに表示されている末尾のエラーではないでしょうか？
実行後に放置しているとターミナルの履歴を超すほどのエラーが表示されるので
ros2 launch 直後の文を追えないかもしれません。
ご返信ありがとうございます。
cmd_vel:=cmd_vel
/first_robot
odom:=odom
上記のコードを削除して、実行した場合のros2 topic listは以下のようになりました。
/camera1/camera_info
/camera1/image_raw
/clicked_point
/clock
/cmd_vel
/goal_pose
/initialpose
/joint_states
/odom
/parameter_events
/robot_description
/rosout
/scan
/tf
/tf_static
この状態で
ros2 launch ros2_first_test first_localization.launch.py
ros2 launch ros2_first_test first_slam.launch.py
これらを実行してもtfとmapが黄色のままでした。。
ご確認よろしくお願い致します。
すみません。”<<>>”は削除されるみたいですね…。
もう一度掲載させていただきます。>/first_robot< >部分のみを削除してみてください。
<<>remapping>cmd_vel:=cmd_vel<<>/remapping>
<<>namespace>/first_robot<<>/namespace>
<<>remapping>odom:=odom<<>/remapping>
のうち、<
ご返信ありがとうございます。
/first_robotのみ消した場合は以下のようになりました。
/camera1/camera_info
/camera1/image_raw
/clicked_point
/clock
/cmd_vel
/goal_pose
/initialpose
/joint_states
/odom
/parameter_events
/robot_description
/rosout
/scan
/tf
/tf_static
この状態で実行しても黄色のままですね。。
ご確認よろしくお願い致します。
ありがとうございます。
すみませんが、少し手詰まり状態ですね…。
お手数ですが実行後のターミナル全文を送付いただくことは可能でしょうか？
お手数おかけして誠に申し訳ございません。
gazeboを起動したときのターミナルの全文を送らさせていただきます。
[INFO] [launch]: All log files can be found below /home/kamalab/.ros/log
[INFO] [launch]: Default logging verbosity is set to INFO
[INFO] [robot_state_publisher-1]: process started with pid [7029]
[INFO] [joint_state_publisher-2]: process started with pid [7031]
[INFO] [gazebo-3]: process started with pid [7033]
[INFO] [spawn_entity.py-4]: process started with pid [7035]
[INFO] [rviz2-5]: process started with pid [7037]
[robot_state_publisher-1] [WARN] [1705130933.537956380] [robot_state_publisher]: No robot_description parameter, but command-line argument available. Assuming argument is name of URDF file. This backwards compatibility fallback will be removed in the future.
[robot_state_publisher-1] Parsing robot urdf xml string.
[robot_state_publisher-1] The root link base_link has an inertia specified in the URDF, but KDL does not support a root link with an inertia. As a workaround, you can add an extra dummy link to your URDF.
[robot_state_publisher-1] Link base_footprint had 0 children
[robot_state_publisher-1] Link camera_joint had 1 children
[robot_state_publisher-1] Link camera_link had 0 children
[robot_state_publisher-1] Link left_joint had 1 children
[robot_state_publisher-1] Link wheel_front_left had 0 children
[robot_state_publisher-1] Link lidar_link had 0 children
[robot_state_publisher-1] Link rear_joint had 1 children
[robot_state_publisher-1] Link rear_caster had 0 children
[robot_state_publisher-1] Link right_joint had 1 children
[robot_state_publisher-1] Link wheel_front_right had 0 children
[robot_state_publisher-1] [INFO] [1705130933.539491802] [robot_state_publisher]: got segment base_footprint
[robot_state_publisher-1] [INFO] [1705130933.539516208] [robot_state_publisher]: got segment base_link
[robot_state_publisher-1] [INFO] [1705130933.539524440] [robot_state_publisher]: got segment camera_joint
[robot_state_publisher-1] [INFO] [1705130933.539531611] [robot_state_publisher]: got segment camera_link
[robot_state_publisher-1] [INFO] [1705130933.539538362] [robot_state_publisher]: got segment left_joint
[robot_state_publisher-1] [INFO] [1705130933.539545157] [robot_state_publisher]: got segment lidar_link
[robot_state_publisher-1] [INFO] [1705130933.539551616] [robot_state_publisher]: got segment rear_caster
[robot_state_publisher-1] [INFO] [1705130933.539558174] [robot_state_publisher]: got segment rear_joint
[robot_state_publisher-1] [INFO] [1705130933.539564740] [robot_state_publisher]: got segment right_joint
[robot_state_publisher-1] [INFO] [1705130933.539571583] [robot_state_publisher]: got segment wheel_front_left
[robot_state_publisher-1] [INFO] [1705130933.539578230] [robot_state_publisher]: got segment wheel_front_right
[gazebo-3] Gazebo multi-robot simulator, version 11.13.0
[gazebo-3] Copyright (C) 2012 Open Source Robotics Foundation.
[gazebo-3] Released under the Apache 2 License.
[gazebo-3] http://gazebosim.org
[gazebo-3]
[gazebo-3] Gazebo multi-robot simulator, version 11.13.0
[gazebo-3] Copyright (C) 2012 Open Source Robotics Foundation.
[gazebo-3] Released under the Apache 2 License.
[gazebo-3] http://gazebosim.org
[gazebo-3]
[spawn_entity.py-4] [INFO] [1705130933.781176507] [urdf_spawner]: Spawn Entity started
[spawn_entity.py-4] [INFO] [1705130933.781523476] [urdf_spawner]: Loading entity published on topic /robot_description
[spawn_entity.py-4] [INFO] [1705130933.782675184] [urdf_spawner]: Waiting for entity xml on /robot_description
[spawn_entity.py-4] [INFO] [1705130933.794743889] [urdf_spawner]: Waiting for service /spawn_entity, timeout = 30
[spawn_entity.py-4] [INFO] [1705130933.794917657] [urdf_spawner]: Waiting for service /spawn_entity
[rviz2-5] [INFO] [1705130933.839403205] [rviz2]: Stereo is NOT SUPPORTED
[rviz2-5] [INFO] [1705130933.839563160] [rviz2]: OpenGl version: 4.6 (GLSL 4.6)
[rviz2-5] [INFO] [1705130933.854288263] [rviz2]: Stereo is NOT SUPPORTED
[rviz2-5] Parsing robot urdf xml string.
[rviz2-5] Warning: Invalid frame ID “wheel_front_left” passed to canTransform argument source_frame – frame does not exist
[rviz2-5] at line 133 in /tmp/binarydeb/ros-foxy-tf2-0.13.14/src/buffer_core.cpp
[rviz2-5] Warning: Invalid frame ID “wheel_front_right” passed to canTransform argument source_frame – frame does not exist
[rviz2-5] at line 133 in /tmp/binarydeb/ros-foxy-tf2-0.13.14/src/buffer_core.cpp
[rviz2-5] Warning: Invalid frame ID “wheel_front_left” passed to canTransform argument source_frame – frame does not exist
[rviz2-5] at line 133 in /tmp/binarydeb/ros-foxy-tf2-0.13.14/src/buffer_core.cpp
[rviz2-5] Warning: Invalid frame ID “wheel_front_right” passed to canTransform argument source_frame – frame does not exist
[rviz2-5] at line 133 in /tmp/binarydeb/ros-foxy-tf2-0.13.14/src/buffer_core.cpp
[rviz2-5] Warning: Invalid frame ID “wheel_front_left” passed to canTransform argument source_frame – frame does not exist
[rviz2-5] at line 133 in /tmp/binarydeb/ros-foxy-tf2-0.13.14/src/buffer_core.cpp
[rviz2-5] Warning: Invalid frame ID “wheel_front_right” passed to canTransform argument source_frame – frame does not exist
[rviz2-5] at line 133 in /tmp/binarydeb/ros-foxy-tf2-0.13.14/src/buffer_core.cpp
[gazebo-3] [INFO] [1705130934.091668299] [gazebo_ros_node]: ROS was initialized without arguments.
[rviz2-5] Warning: Invalid frame ID “wheel_front_left” passed to canTransform argument source_frame – frame does not exist
[rviz2-5] at line 133 in /tmp/binarydeb/ros-foxy-tf2-0.13.14/src/buffer_core.cpp
[rviz2-5] Warning: Invalid frame ID “wheel_front_right” passed to canTransform argument source_frame – frame does not exist
[rviz2-5] at line 133 in /tmp/binarydeb/ros-foxy-tf2-0.13.14/src/buffer_core.cpp
[rviz2-5] Warning: Invalid frame ID “wheel_front_left” passed to canTransform argument source_frame – frame does not exist
[rviz2-5] at line 133 in /tmp/binarydeb/ros-foxy-tf2-0.13.14/src/buffer_core.cpp
[rviz2-5] Warning: Invalid frame ID “wheel_front_right” passed to canTransform argument source_frame – frame does not exist
[rviz2-5] at line 133 in /tmp/binarydeb/ros-foxy-tf2-0.13.14/src/buffer_core.cpp
[rviz2-5] Warning: Invalid frame ID “wheel_front_left” passed to canTransform argument source_frame – frame does not exist
[rviz2-5] at line 133 in /tmp/binarydeb/ros-foxy-tf2-0.13.14/src/buffer_core.cpp
[rviz2-5] Warning: Invalid frame ID “wheel_front_right” passed to canTransform argument source_frame – frame does not exist
[rviz2-5] at line 133 in /tmp/binarydeb/ros-foxy-tf2-0.13.14/src/buffer_core.cpp
[rviz2-5] Warning: Invalid frame ID “wheel_front_left” passed to canTransform argument source_frame – frame does not exist
[rviz2-5] at line 133 in /tmp/binarydeb/ros-foxy-tf2-0.13.14/src/buffer_core.cpp
[rviz2-5] Warning: Invalid frame ID “wheel_front_right” passed to canTransform argument source_frame – frame does not exist
[rviz2-5] at line 133 in /tmp/binarydeb/ros-foxy-tf2-0.13.14/src/buffer_core.cpp
[rviz2-5] Warning: Invalid frame ID “wheel_front_left” passed to canTransform argument source_frame – frame does not exist
[rviz2-5] at line 133 in /tmp/binarydeb/ros-foxy-tf2-0.13.14/src/buffer_core.cpp
[rviz2-5] Warning: Invalid frame ID “wheel_front_right” passed to canTransform argument source_frame – frame does not exist
[rviz2-5] at line 133 in /tmp/binarydeb/ros-foxy-tf2-0.13.14/src/buffer_core.cpp
[rviz2-5] Warning: Invalid frame ID “wheel_front_left” passed to canTransform argument source_frame – frame does not exist
[rviz2-5] at line 133 in /tmp/binarydeb/ros-foxy-tf2-0.13.14/src/buffer_core.cpp
[rviz2-5] Warning: Invalid frame ID “wheel_front_right” passed to canTransform argument source_frame – frame does not exist
[rviz2-5] at line 133 in /tmp/binarydeb/ros-foxy-tf2-0.13.14/src/buffer_core.cpp
[spawn_entity.py-4] [INFO] [1705130934.297938737] [urdf_spawner]: Calling service /spawn_entity
[rviz2-5] Warning: Invalid frame ID “wheel_front_left” passed to canTransform argument source_frame – frame does not exist
[rviz2-5] at line 133 in /tmp/binarydeb/ros-foxy-tf2-0.13.14/src/buffer_core.cpp
[rviz2-5] Warning: Invalid frame ID “wheel_front_right” passed to canTransform argument source_frame – frame does not exist
[rviz2-5] at line 133 in /tmp/binarydeb/ros-foxy-tf2-0.13.14/src/buffer_core.cpp
[gazebo-3] [Msg] Waiting for master.
[gazebo-3] [Msg] Connected to gazebo master @ http://127.0.0.1:11345
[gazebo-3] [Msg] Publicized address: 192.168.0.252
[gazebo-3] [Msg] Loading world file [/home/kamalab/ros2_ws/install/ros2_first_test/share/ros2_first_test/world/first_robot.sdf]
[gazebo-3] Warning [Model.cc:216] Non-unique name[camera_joint] detected 2 times in XML children of model with name[weeding_robot].
[gazebo-3] [Err] [Model.cc:123] Error Code 22 Msg: attached_to name[camera_joint] is identical to frame name[camera_joint], causing a graph cycle in model with name[weeding_robot].
[gazebo-3] [Err] [Model.cc:123] Error Code 22 Msg: FrameAttachedToGraph cycle detected, already visited vertex [camera_joint].
[gazebo-3] [Err] [Model.cc:123] Error Code 22 Msg: FrameAttachedToGraph cycle detected, already visited vertex [camera_joint].
[gazebo-3] [Err] [Model.cc:123] Error Code 25 Msg: relative_to name[camera_joint] is identical to frame name[camera_joint], causing a graph cycle in model with name[weeding_robot].
[gazebo-3] [Err] [Model.cc:123] Error Code 25 Msg: PoseRelativeToGraph cycle detected, already visited vertex [camera_joint].
[gazebo-3] [Err] [Model.cc:123] Error Code 25 Msg: PoseRelativeToGraph cycle detected, already visited vertex [camera_joint].
[gazebo-3] ../src/intel/isl/isl.c:2105: FINISHME: ../src/intel/isl/isl.c:isl_surf_supports_ccs: CCS for 3D textures is disabled, but a workaround is available.
[gazebo-3] [INFO] [1705130935.049086689] [camera_controller]: Publishing camera info to [/camera1/camera_info]
[spawn_entity.py-4] [INFO] [1705130935.054408003] [urdf_spawner]: Spawn status: SpawnEntity: Successfully spawned entity [weeding_robot]
[gazebo-3] [Msg] Waiting for master.
[gazebo-3] [Msg] Connected to gazebo master @ http://127.0.0.1:11345
[gazebo-3] [Msg] Publicized address: 192.168.0.252
[gazebo-3] [Err] [InsertModelWidget.cc:403] Missing model.config for model “/home/kamalab/.gazebo/models/model_board001”
[gazebo-3] [Err] [InsertModelWidget.cc:403] Missing model.config for model “/home/kamalab/.gazebo/models/model_tube254”
[INFO] [spawn_entity.py-4]: process has finished cleanly [pid 7035]
[gazebo-3] [Wrn] [SystemPaths.cc:459] File or path does not exist [“”] [tube_48_6.png]
[gazebo-3] [Err] [Material.cc:91] Unable to find texture[tube_48_6.png] in path[/home/kamalab/.gazebo/models/model_tube486/meshes]
[gazebo-3] [INFO] [1705130935.166684968] [diff_drive]: Wheel pair 1 separation set to [0.240000m]
[gazebo-3] [INFO] [1705130935.166723754] [diff_drive]: Wheel pair 1 diameter set to [0.140000m]
[gazebo-3] [INFO] [1705130935.167337552] [diff_drive]: Subscribed to [/cmd_vel]
[gazebo-3] [INFO] [1705130935.167908795] [diff_drive]: Advertise odometry on [/odom]
[gazebo-3] [INFO] [1705130935.168459044] [diff_drive]: Publishing wheel transforms between [base_link], [wheel_front_left_joint] and [wheel_front_right_joint]
[gazebo-3] ../src/intel/isl/isl.c:2105: FINISHME: ../src/intel/isl/isl.c:isl_surf_supports_ccs: CCS for 3D textures is disabled, but a workaround is available.
ご確認よろしくお願い致します。
送付ありがとうございます。
[gazebo-3] Warning [Model.cc:216] Non-unique name[camera_joint] detected 2 times in XML children of model with name[weeding_robot].
とあるので、weeding_robot もしくは camera_joint が 2回検出されていますね。
どこかに重複する部分があるかもしれません。
私の方でPCに空きがでたら再現テストもしておきます。
ご返信ありがとうございます。
このエラーはurdfから出たものでしょうか。
お手数おかけして誠に申し訳ございません。。
コメントありがとうございます。
urdfよりはworldファイル(.sdf)で出たように思えます。
1週間程度で再現してみます。
ご返信ありがとうございます。
保存しただけのworldファイルからエラーが出る場合もあるのですね。。
エラーの原因になっている可能性があるため記述しておきます。
worldファイルを保存する際は、中央に位置するロボットが存在する状態のまま保存すると、次回ワールドを起動するときにロボットが２つワールドに出現してしまうため、保存する際はロボットを削除してから保存しました。
上記のような手順でワールドを保存したのですが、エラーの原因になりうることはありますでしょうか。
再現していただき、本当にありがとうございます。
ご確認よろしくお願い致します。
お待たせしました。
現在、imuを用いてmapテストをしております。
この後、imuを削除してテストしてみます。
>>エラーの原因になっている可能性があるため記述しておきます。
worldファイルを保存する際は、中央に位置するロボットが存在する状態のまま保存すると、次回ワールドを起動するときにロボットが２つワールドに出現してしまうため、保存する際はロボットを削除してから保存しました。
→私の方ではその現象が現れませんでした。cafeオブジェクトを配置するタイミングですよね？
また、もう一つ気になる点がありました。
first_localization.launch.py first_slam.launch.py を起動したタイミングで
CPU使用率が100%を超えると、
Message Filter dropping message: frame ‘lidar_link’ at time 110.350 for reason ‘the timestamp on the message is earlier than all the data in the transform cache’
と表示され、mapデータが作成される前（map_durationにて設定）にキャッシュされたlidarデータが破棄されるため、
mapが作成できないといった現象がありました。
上記のようなメッセージは表示されていますでしょうか？
ご返信ありがとうございます。
・ロボットに関して
ロボットが２つ配置される現象は、cafeオブジェクトを配置するタイミングです。その後に、ロボットを削除せず保存して、起動するとロボットが２つ現れます。
・CPUに関して
起動時にターミナルにそのような表示はありませんでした。cpu使用率を確認しても100％には、到達していませんでした。。
ご確認よろしくお願い致します。
ご回答ありがとうございます。
・ロボットに関して
ロボットが２つ配置される現象は、cafeオブジェクトを配置するタイミングです。その後に、ロボットを削除せず保存して、起動するとロボットが２つ現れます。
→私の方では発生しなかったので、再現は難しいですね…。
imuなしでmapを確認できたので、ekf_filter.yamlを送付します。
こちらでテストしてみてください。
### ekf config file ###
ekf_filter_node:
ros__parameters:
# The frequency, in Hz, at which the filter will output a position estimate. Note that the filter will not begin
# computation until it receives at least one message from one of theinputs. It will then run continuously at the
# frequency specified here, regardless of whether it receives more measurements. Defaults to 30 if unspecified.
frequency: 10.0
# ekf_localization_node and ukf_localization_node both use a 3D omnidirectional motion model. If this parameter is
# set to true, no 3D information will be used in your state estimate. Use this if you are operating in a planar
# environment and want to ignore the effect of small variations in the ground plane that might otherwise be detected
# by, for example, an IMU. Defaults to false if unspecified.
two_d_mode: false
# Whether to publish the acceleration state. Defaults to false if unspecified.
publish_acceleration: false
# Whether to broadcast the transformation over the /tf topic. Defaultsto true if unspecified.
publish_tf: true
# 1. Set the map_frame, odom_frame, and base_link frames to the appropriate frame names for your system.
# 1a. If your system does not have a map_frame, just remove it, and make sure “world_frame” is set to the value of odom_frame.
# 2. If you are fusing continuous position data such as wheel encoder odometry, visual odometry, or IMU data, set “world_frame”
# to your odom_frame value. This is the default behavior for robot_localization’s state estimation nodes.
# 3. If you are fusing global absolute position data that is subject to discrete jumps (e.g., GPS or position updates from landmark
# observations) then:
# 3a. Set your “world_frame” to your map_frame value
# 3b. MAKE SURE something else is generating the odom->base_link transform. Note that this can even be another state estimation node
# from robot_localization! However, that instance should *not* fuse the global data.
map_frame: map # Defaults to “map” if unspecified
odom_frame: odom # Defaults to “odom” if unspecified
base_link_frame: base_link # Defaults to “base_link” ifunspecified
world_frame: odom # Defaults to the value of odom_frame if unspecified
odom0: first_robot/odom
odom0_config: [true, true, false, # position of xyz
false, false, true, # angle of rpy
true, true, false, # velocity of xyz
false, false, true, # angular velocity of rpy
true, true, false,] # acceleraion of xyz
# imu0: first_robot/imu
# imu0_config: [true, true, false, # position of xyz
# false, false, true, # angle of rpy
# true, true, false, # velocity of xyz
# false, false, true, # angular velocity of rpy
# true, true, false] # acceleraion of xyz
ご返信ありがとうございます。
Node(
package=’robot_localization’,
executable=’ekf_node’,
name=’ekf_filter_node’,
output=’screen’,
parameters=[ekf_path, {‘use_sim_time’: use_sim_time}]),
やはり、このコードをランチファイルに記入するとgazeboが立ち上がりません。。
ターミナルのエラーに以下の文が記述されていました。
LookupError: Could not find the resource ‘robot_localization’ of type ‘packages
robot_localizationのパッケージがないみたいなのですが、何か解決策はありますでしょうか。
ご確認よろしくおねがいします。
エラー送付ありがとうございます。
sudo apt -y install ros-foxy-robot-localization
でインストール可能です。
返信が遅れて申し訳ございません。
ご返信ありがとうございます。
インストールしたら、robot_localizationでgazeboが立ち上がりました。
しかし、未だ、tfとmapが黄色になりエラーのままの状態です。
ロボットが二重で保存されるところがエラーの原因になっている可能性がありますか？
ご確認よろしくお願い致します。
ご回答ありがとうございます。
そうですね、ロボットが二重になっている部分が怪しいです。
ファイルをアップしておいたので、ご確認いただけたらと思います。
https://nutritionfoodtech.com/2024/01/23/imu-%e3%81%aa%e3%81%97-slam-%e3%81%ae%e3%81%9f%e3%82%81%e3%81%ae-localization-map%e4%bd%9c%e6%88%90-ros2-foxy/
ご返信ありがとうございます。
わざわざファイルまでアップロードしていただきありがとうございます。
当サイトに載っているファイルを比較し、一部変更しましたが、マッピングができないです。。
ロボットが二重になるエラーは、ロボットの名前をfirst_robotに変更したら、直りました。
考えられる原因を以下にいくつか記します。
・slam_toolbox
gazeboを起動し、salmを立ち上げるためのlaunch「first_slam.launch.py」を立ち上げると、rvizのtfが黄色になります。そのため、slam_toolboxの原因があると考えています。
「first_slam.launch.py」と「first_slam_settings.yaml」は当サイトをコピペしているため、同じ内容です。
・gazeboのワールド
私は、gazebo内に既存のmodelを配置しているのではなく、自身が作成したmodelを使用しています。modelは簡単なものであり、ただの棒をロボット周りに配置しているだけです。
この自作modelが原因になることもあるのでしょうか。
ご確認よろしくおねがいします。
rvizのtfワーニングは No map recieved といった表示がありますか？
ワールドのsdfファイルが原因かもしれませんね。
modelは一度作成して保存してから再配置していますよね？
sdfファイルを確認したいところですが…かなり長いですよね？
ご返信ありがとうございます。
tfのワーニングは以下のとおりです。
map
No transform from [map] to [base_footprint]
modelは作成して保存し、gazeboのinsertから配置しています。sdfファイルは結構長いです。。
ご確認よろしくおねがいします。
first_slam_settings.py を
https://nutritionfoodtech.com/2024/01/23/imu-%e3%81%aa%e3%81%97-slam-%e3%81%ae%e3%81%9f%e3%82%81%e3%81%ae-localization-map%e4%bd%9c%e6%88%90-ros2-foxy/
に変更してみてください。
また、CPUとメモリサイズを教えていただけますでしょうか。
上記で解決できなければ、sdfとurdfファイルの確認が必要になってきますね…
ご返信ありがとうございます。
first_slam.launch.pyは、同じソースコードでした。
PCスペック
CPU:intel core i5 の12世代
メモリ：16GB
PCのスペック的には問題ないかと思います。
エラーが複雑であるため、一旦このエラーは自身で解決策を模索したいと思います。進展がありましたら、また質問させていただきます。申し訳ございません。。
また、最後に別の質問させていただきたいです。
https://nutritionfoodtech.com/2022/12/20/slam%e3%81%a7%e4%bd%9c%e6%88%90%e3%81%97%e3%81%9fmap%e3%81%a7navigation2-%e3%83%a2%e3%83%87%e3%83%ab%e4%bf%ae%e6%ad%a3%e7%b7%a8-ros2-foxy/
上記のURL内のNavigation2を、現在タートルボットでシミュレーションしています。公式では、ロボットの中心付近にあるパーティクルが密集すると自己位置推定の精度が高いと記述されていましたが、このパーティクルの密集度を数値的に確認する方法（どれくらい精度が高いのかの確認）はありますでしょうか。
ご確認よろしくおねがいします。
おそらくですがsdfを見返せば解決できそうな気はします。
お力になれずすみません。
パーティクルの算出はNavigation2のamclパッケージで実施しています。
なのでこちらのソースコード
https://github.com/ros-planning/navigation2/blob/main/nav2_amcl/src/amcl_node.cpp
の970行目あたりが該当する部分です。
上記をcloneしてstdoutやloggerで出力するように変更し、colcon buildでリビルドすれば
密集度の計算は可能になるかと思われます。
ご返信ありがとうございます。
sdfを比較してみます。ご丁寧にありがとうございます。
C++に詳しくないので少し難しそうですね。。
教えていただきたありがとうございます。
もしくはamclトピックからサブスクライブするpythonファイルを作成するのも手だと思います。
ご返信ありがとうございます。
私には、少し難しそうなので数値的に自己位置推定の精度を確認する方法は断念することにします。。
教えていただきありがとうございます。
時間があれば私の方でもNavigation2を深堀した記事を作成しようと思います。
その際は是非、確認いただけたらと思います。