はじめに
前回の記事では,Cartographerでを使って地図を作成しました.今回は,その地図で自己位置推定を実装する方法を紹介します.
動作環境
| 項目 | バージョン |
|---|---|
| ubuntu | 22.04 |
| ROS | ROS2 humble |
| Lidar | RPLidar A1 M8 |
↓RPLidar A1 M8
自己位置推定の手法
自己位置推定の手法として,EMCL（Extended Monte Carlo Localization）を用いました.
EMCLは,AMCL（Adaptive Monte Carlo Localization）を基盤に上田隆一先生が改良したパーティクルフィルタによる自己位置推定手法です.AMCLの課題として,ロボットが停止すると推定も停止する点や,C++とCで書かれたコードの複雑さが挙げられます.EMCLでは,ロボットの動作に関係なく一定周期で,tfからオドメトリデータを取得し,パーティクルの位置を更新.センサデータは非同期に取り込まれ,オドメトリ処理後に重みを更新してリサンプリングを行います.尤度場を用いて重みを更新し,重みの総和がしきい値を下回ると「膨張リセット」を実施します.このアプローチにより,ロボットが止まってもパーティクルが動作を続け,初期位置の微調整が行われます.ただし,実機での運用ではセンサーノイズやタイムスタンプ,tfの位置関係の管理に注意が必要です.
実装方法
マップの作成
マップの作成については,前回の記事を御覧ください.
emcl2のインストール
以下の手順に従って,ソースコードをクローンしてインストールしてください.
cd ~/ros2_ws/src
git clone https://github.com/Arcanain/emcl2.git
rvizファイルの作成
可視化用のrvizファイルを作成します.mapやロボット,ridarの点群が可視化できます.
Panels:
- Class: rviz_common/Displays
Help Height: 0
Name: Displays
Property Tree Widget:
Expanded:
- /Global Options1
- /TF1
- /TF1/Frames1
- /TF1/Tree1
- /LaserScan1
- /PointCloud21
- /Path1
- /Path1/Offset1
- /Marker1
- /Marker2
- /Marker2/Status1
- /Marker2/Topic1
- /Marker2/Namespaces1
Splitter Ratio: 0.5833333134651184
Tree Height: 550
- 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: true
Links:
All Links Enabled: true
Expand Joint Details: false
Expand Link Details: false
Expand Tree: false
Link Tree Style: Links in Alphabetic Order
base_footprint:
Alpha: 1
Show Axes: false
Show Trail: false
base_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
drivewhl_l_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
drivewhl_r_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
dummy_link:
Alpha: 1
Show Axes: false
Show Trail: false
front_caster:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
Mass Properties:
Inertia: false
Mass: false
Name: RobotModel
TF Prefix: ""
Update Interval: 0
Value: true
Visual Enabled: true
- Class: rviz_default_plugins/TF
Enabled: true
Frame Timeout: 15
Frames:
All Enabled: false
base_footprint:
Value: false
base_link:
Value: true
drivewhl_l_link:
Value: false
drivewhl_r_link:
Value: false
dummy_link:
Value: false
front_caster:
Value: false
laser_frame:
Value: false
map:
Value: true
odom:
Value: true
Marker Scale: 1
Name: TF
Show Arrows: true
Show Axes: true
Show Names: true
Tree:
map:
odom:
base_link:
base_footprint:
{}
drivewhl_l_link:
{}
drivewhl_r_link:
{}
front_caster:
{}
laser_frame:
{}
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: 47
Min Color: 0; 0; 0
Min Intensity: 47
Name: LaserScan
Position Transformer: XYZ
Selectable: true
Size (Pixels): 3
Size (m): 0.05000000074505806
Style: Flat Squares
Topic:
Depth: 5
Durability Policy: Volatile
Filter size: 10
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
Filter size: 10
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
Filter size: 10
History Policy: Keep Last
Reliability Policy: Reliable
Value: /map
Update Topic:
Depth: 5
Durability Policy: Volatile
History Policy: Keep Last
Reliability Policy: Reliable
Value: /map_updates
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
Filter size: 10
History Policy: Keep Last
Reliability Policy: Best Effort
Value: /particlecloud
Value: true
- Class: rviz_common/Group
Displays:
- Alpha: 0.30000001192092896
Class: rviz_default_plugins/Map
Color Scheme: costmap
Draw Behind: false
Enabled: true
Name: Global Costmap
Topic:
Depth: 1
Durability Policy: Transient Local
Filter size: 10
History Policy: Keep Last
Reliability Policy: Reliable
Value: /global_costmap/costmap
Update Topic:
Depth: 5
Durability Policy: Volatile
History Policy: Keep Last
Reliability Policy: Reliable
Value: /global_costmap/costmap_updates
Use Timestamp: false
Value: true
- Alpha: 0.30000001192092896
Class: rviz_default_plugins/Map
Color Scheme: costmap
Draw Behind: false
Enabled: true
Name: Downsampled Costmap
Topic:
Depth: 1
Durability Policy: Transient Local
Filter size: 10
History Policy: Keep Last
Reliability Policy: Reliable
Value: /downsampled_costmap
Update Topic:
Depth: 5
Durability Policy: Volatile
History Policy: Keep Last
Reliability Policy: Reliable
Value: /downsampled_costmap_updates
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
Filter size: 10
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
Filter size: 10
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
Filter size: 10
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
Filter size: 10
History Policy: Keep Last
Reliability Policy: Reliable
Value: /local_costmap/costmap
Update Topic:
Depth: 5
Durability Policy: Volatile
History Policy: Keep Last
Reliability Policy: Reliable
Value: /local_costmap/costmap_updates
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
Filter size: 10
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
Filter size: 10
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
Filter size: 10
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
Filter size: 10
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
- Alpha: 1
Axes Length: 1
Axes Radius: 0.10000000149011612
Class: rviz_default_plugins/PoseWithCovariance
Color: 255; 25; 0
Covariance:
Orientation:
Alpha: 0.5
Color: 255; 255; 127
Color Style: Unique
Frame: Local
Offset: 1
Scale: 1
Value: true
Position:
Alpha: 0.30000001192092896
Color: 204; 51; 204
Scale: 1
Value: true
Value: true
Enabled: true
Head Length: 0.30000001192092896
Head Radius: 0.10000000149011612
Name: PoseWithCovariance
Shaft Length: 1
Shaft Radius: 0.05000000074505806
Shape: Arrow
Topic:
Depth: 5
Durability Policy: Volatile
Filter size: 10
History Policy: Keep Last
Reliability Policy: Reliable
Value: /amcl_pose
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: Intensity
Decay Time: 1
Enabled: true
Invert Rainbow: false
Max Color: 255; 255; 255
Max Intensity: 4096
Min Color: 0; 0; 0
Min Intensity: 0
Name: PointCloud2
Position Transformer: XYZ
Selectable: true
Size (Pixels): 3
Size (m): 0.009999999776482582
Style: Flat Squares
Topic:
Depth: 5
Durability Policy: Volatile
Filter size: 10
History Policy: Keep Last
Reliability Policy: Reliable
Value: /particle_cloud
Use Fixed Frame: true
Use rainbow: true
Value: true
- Alpha: 3
Buffer Length: 5
Class: rviz_default_plugins/Path
Color: 0; 255; 0
Enabled: true
Head Diameter: 0.30000001192092896
Head Length: 0.20000000298023224
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: None
Radius: 0.029999999329447746
Shaft Diameter: 0.10000000149011612
Shaft Length: 0.10000000149011612
Topic:
Depth: 5
Durability Policy: Volatile
Filter size: 10
History Policy: Keep Last
Reliability Policy: Reliable
Value: /visualize_tgt_path
Value: true
- Class: rviz_default_plugins/Marker
Enabled: true
Name: Marker
Namespaces:
target_circle: true
Topic:
Depth: 5
Durability Policy: Volatile
Filter size: 10
History Policy: Keep Last
Reliability Policy: Reliable
Value: /look_ahead_range_marker
Value: true
- Class: rviz_default_plugins/Marker
Enabled: true
Name: Marker
Namespaces:
target_point: true
Topic:
Depth: 5
Durability Policy: Volatile
Filter size: 10
History Policy: Keep Last
Reliability Policy: Reliable
Value: /target_point_marker
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
Covariance x: 0.25
Covariance y: 0.25
Covariance yaw: 0.06853891909122467
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.5799998044967651
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: 27.15713882446289
Target Frame: base_link
Value: TopDownOrtho (rviz_default_plugins)
X: 0
Y: 0
Saved: ~
Window Geometry:
Displays:
collapsed: true
Height: 704
Hide Left Dock: true
Hide Right Dock: true
Navigation 2:
collapsed: false
QMainWindow State: 000000ff00000000fd00000004000000000000016a00000264fc020000000afb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073000000003e00000264000000ca00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb00000018004e0061007600690067006100740069006f006e00200032000000015e000001440000014400fffffffb0000001e005200650061006c00730065006e0073006500430061006d00650072006100000002c6000000c10000002800ffffff000000010000010f00000264fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003e00000264000000a600fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d00650100000000000004500000000000000000000002880000026400000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
RealsenseCamera:
collapsed: false
Selection:
collapsed: false
Tool Properties:
collapsed: false
Views:
collapsed: true
Width: 648
X: 718
Y: 27
launchファイルの作成
import os
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument, GroupAction
from launch.substitutions import LaunchConfiguration, TextSubstitution
from launch_ros.actions import Node, SetParameter
from launch.actions import IncludeLaunchDescription
from launch.launch_description_sources import PythonLaunchDescriptionSource
def generate_launch_description():
package_dir = get_package_share_directory("emcl2")
emcl_params_file = os.path.join(package_dir, "config", 'emcl2_params.yaml')
map_file = os.path.join(package_dir, "map", 'map.yaml')
print('\nMAP',map_file)
rviz = os.path.join(package_dir, "rviz" , "nav2_default_view.rviz")
print('\nRVIZ2 PATH',rviz)
lifecycle_nodes = ['map_server']
return LaunchDescription([
# map server node
Node(
package = 'nav2_map_server',
executable = 'map_server',
name = 'map_server',
output = 'screen',
parameters = [{'yaml_filename': map_file,
'use_sim_time' : False
}],
),
# emcl node
Node(
package = 'emcl2',
name = 'emcl2',
executable = 'emcl2_node',
output = 'screen',
parameters = [emcl_params_file],
),
# static TF conversion (base_link -> laser_frame)
Node(
package = 'tf2_ros',
executable = 'static_transform_publisher',
output = 'screen',
arguments = ['0.0', '0.0', '0.2', '0.0', '0.0', '0.0', 'base_link', 'laser_frame'],
),
# rviz node
Node(
package = 'rviz2',
executable = 'rviz2',
name = 'rviz2',
arguments = ['-d', rviz],
parameters = [{'use_sim_time': False}],
output = 'screen'
),
# life cycle node: manages the life cycle states of nodes
Node(
package = 'nav2_lifecycle_manager',
executable = 'lifecycle_manager',
name = 'lifecycle_manager_localization',
output = 'screen',
parameters = [{'autostart': True},
{'node_names': lifecycle_nodes}]
),
])
このlaunchファイルは,次のノードを立ち上げています.
・map_server: 地図データを提供するノード.
・emcl2: 位置推定を行うEMCLノード.
・tf2_ros: base_linkとlaser_frameを位置関係を固定し,座標変換を提供するノード.
・rviz2: RVizによる可視化ツール.
・nav2_lifecycle_manager: ノードのライフサイクルを管理するノード.
# static TF conversion (base_link -> laser_frame)
Node(
package = 'tf2_ros',
executable = 'static_transform_publisher',
output = 'screen',
arguments = ['0.0', '0.0', '0.2', '0.0', '0.0', '0.0', 'base_link', 'laser_frame'],
),
「laser_frame」の名前は,使うLiDARのフレームIDに適宜変更してください.
オドメトリの計算
emcl2は,tfからオドメトリの移動量を考慮して自己位置を推定しているので,オドメトリを計算する必要があります.高い精度で移動量を推定するには,モーターのエンコーダーからホイールオドメトリを算出しまする必要がありますが,以下のコードでは,簡易的に実装するため速度指令値(cmd_vel)を積算させてオドメトリの値を計算しています.
#include "rclcpp/rclcpp.hpp"
#include "geometry_msgs/msg/transform_stamped.hpp"
#include "geometry_msgs/msg/twist.hpp"
#include "nav_msgs/msg/odometry.hpp"
#include "nav_msgs/msg/path.hpp" // Pathを使用するために追加
#include "nav_msgs/msg/occupancy_grid.hpp"
#include "tf2/LinearMath/Quaternion.h" // tf2::Quaternionを使用するために追加
#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
#include "tf2_ros/static_transform_broadcaster.h"
#include "tf2_ros/transform_broadcaster.h"
#include "visualization_msgs/msg/marker.hpp"
using namespace std::chrono_literals;
class OdometryPublisher : public rclcpp::Node
{
public:
OdometryPublisher()
: Node("odometry_pub")
{
odom_pub = this->create_publisher<nav_msgs::msg::Odometry>("odom", 50);
path_pub = this->create_publisher<nav_msgs::msg::Path>("odom_path", 50);
localmap_pub = this->create_publisher<nav_msgs::msg::OccupancyGrid>("local_map", 10);
laser_range_pub = this->create_publisher<visualization_msgs::msg::Marker>(
"laser_range_marker",
10);
odom_broadcaster = std::make_shared<tf2_ros::TransformBroadcaster>(*this);
// cmd_velサブスクライバを追加
cmd_vel_subscriber = this->create_subscription<geometry_msgs::msg::Twist>(
"cmd_vel", 10, std::bind(&OdometryPublisher::cmd_vel_callback, this, std::placeholders::_1));
x = 0.0;
y = 0.0;
th = 0.0;
current_time = this->get_clock()->now();
last_time = this->get_clock()->now();
path.header.frame_id = "odom"; // パスのフレームIDを設定
timer_ = this->create_wall_timer(50ms, std::bind(&OdometryPublisher::timer_callback, this));
private:
void cmd_vel_callback(const geometry_msgs::msg::Twist::SharedPtr msg)
{
// cmd_velメッセージから速度を更新
vx = msg->linear.x;
vth = msg->angular.z;
}
void timer_callback()
{
/**
*******************************************************************************************
* Odometry Calculation
*******************************************************************************************
*/
current_time = this->get_clock()->now();
double dt = (current_time - last_time).seconds();
double delta_x = vx * cos(th) * dt;
double delta_y = vx * sin(th) * dt;
double delta_th = vth * dt;
x += delta_x;
y += delta_y;
th += delta_th;
tf2::Quaternion odom_quat;
odom_quat.setRPY(0, 0, th); // ロール,ピッチ,ヨーをセット
geometry_msgs::msg::Quaternion odom_quat_msg =
tf2::toMsg(odom_quat); // tf2::Quaternionからgeometry_msgs::msg::Quaternionに変換
geometry_msgs::msg::TransformStamped odom_trans;
odom_trans.header.stamp = current_time;
odom_trans.header.frame_id = "odom";
odom_trans.child_frame_id = "base_link";
odom_trans.transform.translation.x = x;
odom_trans.transform.translation.y = y;
odom_trans.transform.translation.z = 0.0;
odom_trans.transform.rotation = odom_quat_msg;
odom_broadcaster->sendTransform(odom_trans);
nav_msgs::msg::Odometry odom;
odom.header.stamp = current_time;
odom.header.frame_id = "odom";
odom.child_frame_id = "base_link";
odom.pose.pose.position.x = x;
odom.pose.pose.position.y = y;
odom.pose.pose.position.z = 0.0;
odom.pose.pose.orientation = odom_quat_msg;
odom.twist.twist.linear.x = vx;
odom.twist.twist.linear.y = 0.0;
odom.twist.twist.angular.z = vth;
odom_pub->publish(odom);
/**
*******************************************************************************************
* Odometry Path Publish
*******************************************************************************************
*/
// パスに現在の位置を追加
geometry_msgs::msg::PoseStamped this_pose_stamped;
this_pose_stamped.pose.position.x = x;
this_pose_stamped.pose.position.y = y;
this_pose_stamped.pose.orientation = odom_quat_msg;
this_pose_stamped.header.stamp = current_time;
this_pose_stamped.header.frame_id = "odom";
path.poses.push_back(this_pose_stamped);
// パスを公開
path_pub->publish(path);
/**
*******************************************************************************************
* local costmap Publish
*******************************************************************************************
*/
// localmapの設定とpublish
auto map = nav_msgs::msg::OccupancyGrid();
map.header.stamp = current_time;
map.header.frame_id = "base_link";
map.info.resolution = 0.1; // メートル/ピクセル
map.info.width = 40; // 4m x 4mの地図
map.info.height = 40;
map.info.origin.position.x = -(map.info.height * map.info.resolution) / 2.0;
map.info.origin.position.y = -(map.info.width * map.info.resolution) / 2.0;
map.info.origin.position.z = 0.0;
map.info.origin.orientation.w = 1.0;
// 地図データの初期化（すべてを未知(-1)に設定）
map.data = std::vector<int8_t>(map.info.width * map.info.height, -1);
// ローカルマップをパブリッシュ
localmap_pub->publish(map);
/**
*******************************************************************************************
* laser range visualize publish
*******************************************************************************************
*/
// 円の線を描くマーカーを作成
visualization_msgs::msg::Marker line_strip_marker;
line_strip_marker.header.frame_id = "base_link";
line_strip_marker.header.stamp = current_time;
line_strip_marker.ns = "circle_line";
line_strip_marker.id = 1;
line_strip_marker.type = visualization_msgs::msg::Marker::LINE_STRIP;
line_strip_marker.action = visualization_msgs::msg::Marker::ADD;
line_strip_marker.pose.orientation.w = 1.0;
line_strip_marker.scale.x = 0.05; // 線の太さ
line_strip_marker.color.r = 1.0;
line_strip_marker.color.g = 0.0;
line_strip_marker.color.b = 0.0;
line_strip_marker.color.a = 1.0; // 不透明
int num_points = 100;
// 円周上の点を追加
double radius = 1.0;
for (int i = 0; i <= num_points; ++i) {
double angle = i * 2.0 * M_PI / num_points;
geometry_msgs::msg::Point p;
p.x = radius * cos(angle);
p.y = radius * sin(angle);
p.z = 0.0;
line_strip_marker.points.push_back(p);
}
// マーカーをパブリッシュth
// 時刻の保存
last_time = current_time;
}
rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr odom_pub;
rclcpp::Publisher<nav_msgs::msg::Path>::SharedPtr path_pub;
rclcpp::Publisher<nav_msgs::msg::OccupancyGrid>::SharedPtr localmap_pub;
rclcpp::Publisher<visualization_msgs::msg::Marker>::SharedPtr laser_range_pub;
rclcpp::Subscription<geometry_msgs::msg::Twist>::SharedPtr cmd_vel_subscriber;
std::shared_ptr<tf2_ros::TransformBroadcaster> odom_broadcaster;
rclcpp::TimerBase::SharedPtr timer_;
nav_msgs::msg::Path path; // Pathメッセージのメンバ変数を追加
double x, y, th, vx, vth;
rclcpp::Time current_time, last_time;
};
int main(int argc, char ** argv)
{
rclcpp::init(argc, argv);
rclcpp::spin(std::make_shared<OdometryPublisher>());
rclcpp::shutdown();
return 0;
}
githubで公開しているのでインストールするか,ros2ファイルを新しく作成してください.
cd ~/ros2_ws/src
git clone https://github.com/Arcanain/arcanain_simulator.git
lidarを起動
source ~/ros2_ws/install/setup.bash
ros2 launch sllidar_ros2 sllidar_a1_launch.py
オドメトリの起動
source ~/ros2_ws/install/setup.bash
ros2 launch arcanain_simulator odometory_pub.py
emclを起動
source ~/ros2_ws/install/setup.bash
ros2 launch emcl2 emcl2_navigation_launch.py
emclを用いた経路追従制御
経路追従制御(pur pursuit法)の自己位置推定にemclを用いて実験しました.基本的には,精度良くマッチングしてくれましたが,初期座標,オドメトリの移動量,回転とセンサーによる位置推定にずれが生じると,自己位置が破綻しそうになる箇所がありました.EMCLは,適切なパラメータ設定と情報の組み合わせによって高精度な位置推定が可能です.しかし,自己位置推定の精度は,初期位置の設定,オドメトリの信頼性,センサーの精度,およびパーティクルの重み付けによって大きく左右されます.実験結果を基に,これらのパラメータを適切に調整し,オドメトリやセンサーの誤差が自己位置推定に与える影響を軽減することで,精度をさらに向上させることができそうです.
パラメータの設定
パラメータはLuaファイルで設定されていましたが,なぜかその設定が反映されていませんでした.そこで,直接コード内(src/emcl2_node.cpp)のパラメータを変更しました.ロボットの挙動に応じて,適宜調整してください.
// グローバル座標系の名前を設定します.通常は「map」として設定され,地図上の座標系.
this->declare_parameter("global_frame_id", std::string("map"));
// ロボットのフットプリント座標系の名前を設定します.通常は「base_footprint」で,ロボットの基準となる座標系.
this->declare_parameter("footprint_frame_id", std::string("base_footprint"));
// オドメトリの座標系の名前を設定します.通常は「odom」で,ロボットの移動履歴を追跡する座標系.
this->declare_parameter("odom_frame_id", std::string("odom"));
// ロボット本体の座標系の名前を設定します.通常は「base_link」で,ロボットの基準座標.
this->declare_parameter("base_frame_id", std::string("base_link"));
// オドメトリ情報の更新頻度 .
this->declare_parameter("odom_freq", 20);
// トランスフォームの許容誤差.これにより,座標系間の変換の信頼性が決まります.
this->declare_parameter("transform_tolerance", 0.2);
// レーザーセンサーの最小測定距離を設定します.0.0は測定距離の制限なしを意味します.
this->declare_parameter("laser_min_range", 0.0);
// レーザーセンサーの最大測定距離を設定します.
this->declare_parameter("laser_max_range", 100000000.0);
// スキャンの増分を設定します.センサーのスキャン解像度に関連します.
this->declare_parameter("scan_increment", 1);
// 初期位置のx座標を設定します.ロボットのスタート位置のx座標.
this->declare_parameter("initial_pose_x", 0.0);
// 初期位置のy座標を設定します.ロボットのスタート位置のy座標.
this->declare_parameter("initial_pose_y", 0.0);
// 初期位置の角度（ラジアン単位）を設定します.ロボットのスタート位置の向き.
this->declare_parameter("initial_pose_a", 0.0);
// EMCLで使用するパーティクルの数.
this->declare_parameter("num_particles", 500);
// パーティクルの重みの閾値.重みがこの閾値以下の場合,再サンプリングが行われます.
this->declare_parameter("alpha_threshold", 0.5);
// 位置の拡張半径.パーティクル群の位置更新の際に使用されます.
this->declare_parameter("expansion_radius_position", 0.1);
// 向きの拡張半径.パーティクル群の向き更新の際に使用されます.
this->declare_parameter("expansion_radius_orientation", 0.2);
// パーティクルの抽出率.再サンプリングの頻度を調整します.
this->declare_parameter("extraction_rate", 0.1);
// センサーの情報を元に位置推定の精度がどの程度低いとみなされるかを決定する範囲の閾値を設定します.
this->declare_parameter("range_threshold", 0.1);
// センサーのリセットを有効にするかどうか.デフォルトは無効です.
this->declare_parameter("sensor_reset", false);
// オドメトリの前進移動時の誤差.ロボットが前進した距離に応じた誤差です.
this->declare_parameter("odom_fw_dev_per_fw", 0.19);
// オドメトリの回転時の誤差.ロボットが回転した角度に応じた誤差です.
this->declare_parameter("odom_fw_dev_per_rot", 0.0001);
// オドメトリの回転時の誤差を前進移動量に基づいて設定します.
this->declare_parameter("odom_rot_dev_per_fw", 0.13);
// オドメトリの回転時の誤差を回転角度に基づいて設定します.
this->declare_parameter("odom_rot_dev_per_rot", 0.2);
// レーザーセンサーの信頼性を最大距離.この距離内でセンサーの情報が信頼されます.
this->declare_parameter("laser_likelihood_max_dist", 0.2);
参考文献
・上田 隆一(2019), 詳解 確率ロボティクス Pythonによる基礎アルゴリズムの実装
謝辞
この取り組みは, GxP(グロースエクスパートナーズ)株式会社様のサポートを受けて実施しています. 貴重なアドバイスや, ロボットに必要な機材の支援をいただきました. 心より感謝申し上げます.