はじめに
前回の記事では、 Navigation2 の設定ファイルを作成しました。
今回の記事では、残りの必要なファイルの作成方法について説明していきます。
前提条件
前提条件は、以下の通りです。
- Map 作成が完了している
- Navigation2 の設定ファイルの作成が完了している
Map 作成はこちらの記事を、Navigation2 はこちらの記事を参考にしてください。
自立走行に必要なファイル
Navigation2 を使用した自立走行に必要なファイルは以下の二つです。
- Navigation2 の設定ファイル (.yaml) 作成済
- Navigation2 の起動ファイル (.launch.py)
- Navigation2 の rviz ファイル (.rviz)
今回は起動ファイルと rviz ファイルを作成していきます。
rviz ファイルの作成
Navigation2 用の rviz ファイルを作成していきます。
cd ros_ws/src/ros2_first_test
touch rviz/nav2_default_view.rviznav2_default_view.rviz は Navigation2 の公式githubにも記載があります。
以前まで使用していた first_robot.rviz は、使用できません。
rviz の設定を適切にしないと、Navigation2 は動作しません。
コードは、以下のようになります。コピペして nav_default_view.rviz に貼付してください。
global_costmap と local_costmap の連携や、ボクセルマップ、パーティクルフィルタの表示、連携等を追加してあります。
Panels:
- Class: rviz_common/Displays
Help Height: 0
Name: Displays
Property Tree Widget:
Expanded:
- /Global Options1
- /TF1/Frames1
- /TF1/Tree1
Splitter Ratio: 0.5833333134651184
Tree Height: 606
- Class: rviz_common/Selection
Name: Selection
- Class: rviz_common/Tool Properties
Expanded:
- /Publish Point1
Name: Tool Properties
Splitter Ratio: 0.5886790156364441
- Class: rviz_common/Views
Expanded:
- /Current View1
Name: Views
Splitter Ratio: 0.5
- Class: nav2_rviz_plugins/Navigation 2
Name: Navigation 2
Visualization Manager:
Class: ""
Displays:
- Alpha: 0.5
Cell Size: 1
Class: rviz_default_plugins/Grid
Color: 160; 160; 164
Enabled: true
Line Style:
Line Width: 0.029999999329447746
Value: Lines
Name: Grid
Normal Cell Count: 0
Offset:
X: 0
Y: 0
Z: 0
Plane: XY
Plane Cell Count: 10
Reference Frame: <Fixed Frame>
Value: true
- Alpha: 1
Class: rviz_default_plugins/RobotModel
Collision Enabled: false
Description File: ""
Description Source: Topic
Description Topic:
Depth: 5
Durability Policy: Volatile
History Policy: Keep Last
Reliability Policy: Reliable
Value: /robot_description
Enabled: false
Links:
All Links Enabled: true
Expand Joint Details: false
Expand Link Details: false
Expand Tree: false
Link Tree Style: ""
Name: RobotModel
TF Prefix: ""
Update Interval: 0
Value: false
Visual Enabled: true
- Class: rviz_default_plugins/TF
Enabled: true
Frame Timeout: 15
Frames:
All Enabled: false
base_footprint:
Value: true
base_link:
Value: true
base_scan:
Value: true
camera_depth_frame:
Value: true
camera_depth_optical_frame:
Value: true
camera_link:
Value: true
camera_rgb_frame:
Value: true
camera_rgb_optical_frame:
Value: true
caster_back_left_link:
Value: true
caster_back_right_link:
Value: true
imu_link:
Value: true
map:
Value: true
odom:
Value: true
wheel_left_link:
Value: true
wheel_right_link:
Value: true
Marker Scale: 1
Name: TF
Show Arrows: true
Show Axes: true
Show Names: false
Tree:
map:
odom:
base_footprint:
base_link:
base_scan:
{}
camera_link:
camera_depth_frame:
camera_depth_optical_frame:
{}
camera_rgb_frame:
camera_rgb_optical_frame:
{}
caster_back_left_link:
{}
caster_back_right_link:
{}
imu_link:
{}
wheel_left_link:
{}
wheel_right_link:
{}
Update Interval: 0
Value: true
- Alpha: 1
Autocompute Intensity Bounds: true
Autocompute Value Bounds:
Max Value: 10
Min Value: -10
Value: true
Axis: Z
Channel Name: intensity
Class: rviz_default_plugins/LaserScan
Color: 255; 255; 255
Color Transformer: Intensity
Decay Time: 0
Enabled: true
Invert Rainbow: false
Max Color: 255; 255; 255
Max Intensity: 0
Min Color: 0; 0; 0
Min Intensity: 0
Name: LaserScan
Position Transformer: XYZ
Selectable: true
Size (Pixels): 3
Size (m): 0.009999999776482582
Style: Flat Squares
Topic:
Depth: 5
Durability Policy: Volatile
History Policy: Keep Last
Reliability Policy: Best Effort
Value: /scan
Use Fixed Frame: true
Use rainbow: true
Value: true
- Alpha: 1
Autocompute Intensity Bounds: true
Autocompute Value Bounds:
Max Value: 10
Min Value: -10
Value: true
Axis: Z
Channel Name: intensity
Class: rviz_default_plugins/PointCloud2
Color: 255; 255; 255
Color Transformer: ""
Decay Time: 0
Enabled: true
Invert Rainbow: false
Max Color: 255; 255; 255
Max Intensity: 4096
Min Color: 0; 0; 0
Min Intensity: 0
Name: Bumper Hit
Position Transformer: ""
Selectable: true
Size (Pixels): 3
Size (m): 0.07999999821186066
Style: Spheres
Topic:
Depth: 5
Durability Policy: Volatile
History Policy: Keep Last
Reliability Policy: Best Effort
Value: /mobile_base/sensors/bumper_pointcloud
Use Fixed Frame: true
Use rainbow: true
Value: true
- Alpha: 1
Class: rviz_default_plugins/Map
Color Scheme: map
Draw Behind: true
Enabled: true
Name: Map
Topic:
Depth: 1
Durability Policy: Transient Local
History Policy: Keep Last
Reliability Policy: Reliable
Value: /map
Use Timestamp: false
Value: true
- Alpha: 1
Arrow Length: 0.019999999552965164
Axes Length: 0.30000001192092896
Axes Radius: 0.009999999776482582
Class: rviz_default_plugins/PoseArray
Color: 0; 180; 0
Enabled: true
Head Length: 0.07000000029802322
Head Radius: 0.029999999329447746
Name: Amcl Particle Swarm
Shaft Length: 0.23000000417232513
Shaft Radius: 0.009999999776482582
Shape: Arrow (Flat)
Topic:
Depth: 5
Durability Policy: Volatile
History Policy: Keep Last
Reliability Policy: Best Effort
Value: /particlecloud
Value: true
- Class: rviz_common/Group
Displays:
- Alpha: 0.3
Class: rviz_default_plugins/Map
Color Scheme: costmap
Draw Behind: false
Enabled: true
Name: Global Costmap
Topic:
Depth: 1
Durability Policy: Transient Local
History Policy: Keep Last
Reliability Policy: Reliable
Value: /global_costmap/costmap
Use Timestamp: false
Value: true
- Alpha: 0.3
Class: rviz_default_plugins/Map
Color Scheme: costmap
Draw Behind: false
Enabled: true
Name: Downsampled Costmap
Topic:
Depth: 1
Durability Policy: Transient Local
History Policy: Keep Last
Reliability Policy: Reliable
Value: /downsampled_costmap
Use Timestamp: false
Value: true
- Alpha: 1
Buffer Length: 1
Class: rviz_default_plugins/Path
Color: 255; 0; 0
Enabled: true
Head Diameter: 0.019999999552965164
Head Length: 0.019999999552965164
Length: 0.30000001192092896
Line Style: Lines
Line Width: 0.029999999329447746
Name: Path
Offset:
X: 0
Y: 0
Z: 0
Pose Color: 255; 85; 255
Pose Style: Arrows
Radius: 0.029999999329447746
Shaft Diameter: 0.004999999888241291
Shaft Length: 0.019999999552965164
Topic:
Depth: 5
Durability Policy: Volatile
History Policy: Keep Last
Reliability Policy: Reliable
Value: /plan
Value: true
- Alpha: 1
Autocompute Intensity Bounds: true
Autocompute Value Bounds:
Max Value: 10
Min Value: -10
Value: true
Axis: Z
Channel Name: intensity
Class: rviz_default_plugins/PointCloud
Color: 125; 125; 125
Color Transformer: FlatColor
Decay Time: 0
Enabled: true
Invert Rainbow: false
Max Color: 255; 255; 255
Max Intensity: 4096
Min Color: 0; 0; 0
Min Intensity: 0
Name: VoxelGrid
Position Transformer: XYZ
Selectable: true
Size (Pixels): 3
Size (m): 0.05000000074505806
Style: Boxes
Topic:
Depth: 5
Durability Policy: Volatile
History Policy: Keep Last
Reliability Policy: Reliable
Value: /global_costmap/voxel_marked_cloud
Use Fixed Frame: true
Use rainbow: true
Value: true
- Alpha: 1
Class: rviz_default_plugins/Polygon
Color: 25; 255; 0
Enabled: false
Name: Polygon
Topic:
Depth: 5
Durability Policy: Volatile
History Policy: Keep Last
Reliability Policy: Reliable
Value: /global_costmap/published_footprint
Value: false
Enabled: true
Name: Global Planner
- Class: rviz_common/Group
Displays:
- Alpha: 0.699999988079071
Class: rviz_default_plugins/Map
Color Scheme: costmap
Draw Behind: false
Enabled: true
Name: Local Costmap
Topic:
Depth: 1
Durability Policy: Transient Local
History Policy: Keep Last
Reliability Policy: Reliable
Value: /local_costmap/costmap
Use Timestamp: false
Value: true
- Alpha: 1
Buffer Length: 1
Class: rviz_default_plugins/Path
Color: 0; 12; 255
Enabled: true
Head Diameter: 0.30000001192092896
Head Length: 0.20000000298023224
Length: 0.30000001192092896
Line Style: Lines
Line Width: 0.029999999329447746
Name: Local Plan
Offset:
X: 0
Y: 0
Z: 0
Pose Color: 255; 85; 255
Pose Style: None
Radius: 0.029999999329447746
Shaft Diameter: 0.10000000149011612
Shaft Length: 0.10000000149011612
Topic:
Depth: 5
Durability Policy: Volatile
History Policy: Keep Last
Reliability Policy: Reliable
Value: /local_plan
Value: true
- Class: rviz_default_plugins/MarkerArray
Enabled: false
Name: Trajectories
Namespaces:
{}
Topic:
Depth: 5
Durability Policy: Volatile
History Policy: Keep Last
Reliability Policy: Reliable
Value: /marker
Value: false
- Alpha: 1
Class: rviz_default_plugins/Polygon
Color: 25; 255; 0
Enabled: true
Name: Polygon
Topic:
Depth: 5
Durability Policy: Volatile
History Policy: Keep Last
Reliability Policy: Reliable
Value: /local_costmap/published_footprint
Value: true
- Alpha: 1
Autocompute Intensity Bounds: true
Autocompute Value Bounds:
Max Value: 10
Min Value: -10
Value: true
Axis: Z
Channel Name: intensity
Class: rviz_default_plugins/PointCloud
Color: 255; 255; 255
Color Transformer: RGB8
Decay Time: 0
Enabled: true
Invert Rainbow: false
Max Color: 255; 255; 255
Max Intensity: 4096
Min Color: 0; 0; 0
Min Intensity: 0
Name: VoxelGrid
Position Transformer: XYZ
Selectable: true
Size (Pixels): 3
Size (m): 0.009999999776482582
Style: Flat Squares
Topic:
Depth: 5
Durability Policy: Volatile
History Policy: Keep Last
Reliability Policy: Reliable
Value: /local_costmap/voxel_marked_cloud
Use Fixed Frame: true
Use rainbow: true
Value: true
Enabled: true
Name: Controller
- Class: rviz_common/Group
Displays:
- Class: rviz_default_plugins/Image
Enabled: true
Max Value: 1
Median window: 5
Min Value: 0
Name: RealsenseCamera
Normalize Range: true
Topic:
Depth: 5
Durability Policy: Volatile
History Policy: Keep Last
Reliability Policy: Reliable
Value: /intel_realsense_r200_depth/image_raw
Value: true
- Alpha: 1
Autocompute Intensity Bounds: true
Autocompute Value Bounds:
Max Value: 10
Min Value: -10
Value: true
Axis: Z
Channel Name: intensity
Class: rviz_default_plugins/PointCloud2
Color: 255; 255; 255
Color Transformer: RGB8
Decay Time: 0
Enabled: true
Invert Rainbow: false
Max Color: 255; 255; 255
Max Intensity: 4096
Min Color: 0; 0; 0
Min Intensity: 0
Name: RealsenseDepthImage
Position Transformer: XYZ
Selectable: true
Size (Pixels): 3
Size (m): 0.009999999776482582
Style: Flat Squares
Topic:
Depth: 5
Durability Policy: Volatile
History Policy: Keep Last
Reliability Policy: Reliable
Value: /intel_realsense_r200_depth/points
Use Fixed Frame: true
Use rainbow: true
Value: true
Enabled: false
Name: Realsense
- Class: rviz_default_plugins/MarkerArray
Enabled: true
Name: MarkerArray
Namespaces:
{}
Topic:
Depth: 5
Durability Policy: Volatile
History Policy: Keep Last
Reliability Policy: Reliable
Value: /waypoints
Value: true
Enabled: true
Global Options:
Background Color: 48; 48; 48
Fixed Frame: map
Frame Rate: 30
Name: root
Tools:
- Class: rviz_default_plugins/MoveCamera
- Class: rviz_default_plugins/Select
- Class: rviz_default_plugins/FocusCamera
- Class: rviz_default_plugins/Measure
Line color: 128; 128; 0
- Class: rviz_default_plugins/SetInitialPose
Topic:
Depth: 5
Durability Policy: Volatile
History Policy: Keep Last
Reliability Policy: Reliable
Value: /initialpose
- Class: rviz_default_plugins/PublishPoint
Single click: true
Topic:
Depth: 5
Durability Policy: Volatile
History Policy: Keep Last
Reliability Policy: Reliable
Value: /clicked_point
- Class: nav2_rviz_plugins/GoalTool
Transformation:
Current:
Class: rviz_default_plugins/TF
Value: true
Views:
Current:
Angle: -1.6150002479553223
Class: rviz_default_plugins/TopDownOrtho
Enable Stereo Rendering:
Stereo Eye Separation: 0.05999999865889549
Stereo Focal Distance: 1
Swap Stereo Eyes: false
Value: false
Invert Z Axis: false
Name: Current View
Near Clip Distance: 0.009999999776482582
Scale: 127.88431549072266
Target Frame: <Fixed Frame>
Value: TopDownOrtho (rviz_default_plugins)
X: -0.044467076659202576
Y: -0.38726311922073364
Saved: ~
Window Geometry:
Displays:
collapsed: false
Height: 932
Hide Left Dock: false
Hide Right Dock: true
Navigation 2:
collapsed: false
QMainWindow State: 000000ff00000000fd00000004000000000000016a0000034afc020000000afb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d0000029b000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb00000018004e0061007600690067006100740069006f006e0020003201000002de000000a90000008100fffffffb0000001e005200650061006c00730065006e0073006500430061006d00650072006100000002c6000000c10000002800ffffff000000010000010f0000034afc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d0000034a000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d00650100000000000004500000000000000000000004990000034a00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
RealsenseCamera:
collapsed: false
Selection:
collapsed: false
Tool Properties:
collapsed: false
Views:
collapsed: true
Width: 1545
X: 696
Y: 229Navigation2 の起動ファイル
Navigation2 の起動ファイルを作成していきます。
cd ros_ws/src/ros2_first_test
touch launch/first_navigation.launch.py作成した first_navigation.launch.py に以下のコードを入力してください。
import os
from ament_index_python.packages import get_package_share_directory
import launch
from launch import LaunchDescription
from launch_ros.actions import Node
from launch.substitutions import LaunchConfiguration
from launch.actions import IncludeLaunchDescription
from launch.launch_description_sources import PythonLaunchDescriptionSource
def generate_launch_description():
package_dir = get_package_share_directory("ros2_first_test")
urdf = os.path.join(package_dir, "urdf" , "first_robot.urdf")
use_sim_time = LaunchConfiguration('use_sim_time', default='true')
world_path = os.path.join(package_dir, 'world', 'first_robot.sdf')
map_path = os.path.join(package_dir, "map", 'map.yaml')
nav_params_path = os.path.join(package_dir, "config", 'first_navigation.yaml')
nav2_launch_file_dir = os.path.join(get_package_share_directory('nav2_bringup'), 'launch')
#rviz = os.path.join(package_dir, "rviz" , "first_robot.rviz")
rviz = os.path.join(package_dir, "rviz" , "nav2_default_view.rviz")
ekf_path = os.path.join(package_dir, "config", 'first_ekf.yaml')
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"]),
IncludeLaunchDescription(
PythonLaunchDescriptionSource([nav2_launch_file_dir, '/bringup_launch.py']),
launch_arguments={
'map': map_path,
'use_sim_time': use_sim_time,
'params_file': nav_params_path}.items(),
),
Node(
package='rviz2',
executable='rviz2',
name='rviz2',
arguments=['-d', rviz],
parameters=[{'use_sim_time': use_sim_time}],
output='screen'),
])新規部分のみ説明していきます。
nav2_launch_file_dir = os.path.join(get_package_share_directory('nav2_bringup'), 'launch')Navigation2 のパッケージを使用するので、nav2_bringup パッケージの launch フォルダまでのパスを取得します。
IncludeLaunchDescription(
PythonLaunchDescriptionSource([nav2_launch_file_dir, '/bringup_launch.py']),
launch_arguments={
'map': map_path,
'use_sim_time': use_sim_time,
'params_file': nav_params_path
}.items(),
),今までは Node を使用して、executable：python や C++ で作成された実行可能ファイル のみを launch ファイルで起動していました。
今回は launch ファイル内から navigation2 の launch ファイルを起動します。
IncludeLaunchDescription() 内に launch ファイルのパス、引数を渡します。
PythonLaunchDescriptionSource() は、launch ファイルのパスです。
launch_arguments は、引数を渡します。辞書形式で記述し、 .items() を使用して渡します。
おわりに
今回は rviz ファイルと launch ファイルの作成方法について説明しました。
特に rviz ファイルは必須なので忘れないようにしてください。
次回はいよいよ Navigation2 を実行します。
残る工程は setup.py の編集と rviz2 上での少しの操作となります。
コメント
はじめまして。
こちらの記事とても参考にさせてもらっています！
一方で、自分もfoxyでnavigationの設定をしているのですが、うまく動作せず困っており、質問させていただきたいです。
以下のエラーがでて、経路が生成されないのですが、何かご存じな事ありませんでしょうか？
> [controller_server-4] [INFO] [1674959250.384528300] [controller_server]: Received a goal, begin computing control effort.
> [controller_server-4] [ERROR] [1674959250.386237900] [DWBLocalPlanner]: No valid trajectories out of 0!
> [controller_server-4] [ERROR] [1674959250.386578300] [controller_server]: No valid trajectories out of 0!
> [controller_server-4] [WARN] [1674959250.386707500]
> [controller_server_rclcpp_node]: [follow_path] [ActionServer] Aborting handle.
ご連絡ありがとうございます。
ロボット自身が現在地を把握しておらず、軌道生成できないようなエラーだと思います。
もしくは、ロボットがマップ外にいると認識しているかもしれません。
Rviz2 上で 2D Pose Estimate は正常に実行できていますでしょうか？
もし、エラーが出ている場合は教えていただけますでしょうか。
ご返信ありがとうございます！
2D Pose Estimateは正常に動作していました！
ですが、別のノードでも自己位置を生成していたため、それが干渉していたのかも知れません。（自己位置がガタガタ移動するため）
また、ホイールオドメトリから生成される自己位置も正常じゃない印象でした。
地図をリアルがタイムに生成しながらナビゲーションをしたかったので、以下を行ったところうまく動作しました。ご助言がヒントになり動作しましたので、ありがとうございますmm
・amclを停止
・urdfを修正
・以下を参考にノードを起動
・https://navigation.ros.org/tutorials/docs/navigation2_with_slam.html
そうですね、ホイールオドメトリはかなり不安定です。ノイズが効いているのか長時間動かすとずれてしまいます。
上手くいってよかったです！
私は地図生成＋ナビゲーションを実行しようとして挫折しました…
amclが悪さしていたんですね。
ありがとうございます、取り組んでみようと思います。
> 私は地図生成＋ナビゲーションを実行しようとして挫折しました…
なるほど、、ナビゲーションスタック内に複数のノードがあって、どれが影響してるか把握するのも一苦労ですもんね、、
> amclが悪さしていたんですね。
はい、自分はros localizationを使ってwheel odometryとimuを組み合わせた自己位置のみを使用するようにしました。ですが、wheel odometryだけでもうまくいくかもしれません
Kame-SanSunさんも動作するのを願っています。
こちらこそありがとうございました！