プログラムからナビゲーションを実行
プログラムからナビゲーションを実行する方法を学びます．
目的
前回の講義では，ROSで用意されているパッケージを利用して地図の作成とナビゲーションを行いました．
ナビゲーションの際には，データを可視化するツールであるrviz2から目標位置を設定することで，経路計画及び経路に沿った走行が行われることを確認しました．
rviz2を用いた方法ではGUI操作が必須となります．
今回は目標位置をプログラムから与えることで，人間の手を必要としない自律走行を試します．
rviz2から目標位置を設定
前回の講義で学んだ内容を簡単に復習します．
一部に設定ファイルを編集する必要がありますので，設定していない人は前回の講義資料を参考にしてください．
ナビゲーションを実行するために下記を順番に実行します．
まずはstageシミュレーターを起動します．
1
|
$ ros2 launch stage_ros2 stage.launch.py world:=cave
|
次にナビゲーションに必要なノードを実行します．
地図の絶対パスを指定します．
下記の例をパスに含まれるアカウント名を修正して実行してください．
1
|
$ ros2 launch nav2_bringup bringup_launch.py use_sim_time:=True map:=/home/yuu/stage_ws/src/stage_ros2/map/map.yaml params_file:=/opt/ros/humble/share/nav2_bringup/params/nav2_params_stage.yaml
|
static_transform_publisherにより，フレームodomとワールド座標系を表すmapを繋ぎます．
1
|
$ ros2 run tf2_ros static_transform_publisher --x 0 --y 0 --z 0 --roll 0 --pitch 0 --yaw 0 --frame-id map --child-frame-id odom
|
rviz2を起動します．
1
|
$ ros2 launch stage_ros2 rviz.launch.py config:=cave
|
rviz2の起動後，Displaysの左下にあるAddボタンをクリックし，可視化したい項目のメニューを表示します．
項目の中からMapを選びOKをクリックしてください．
DisplaysにMapが追加されるのでダブルクリックし，Topicにて/mapを入力します．
作成した地図を表示する設定が完了しました(まだ地図は表示されません)． 下記のコマンドを実行して地図を読み込みます．
map_urlにて地図の絶対パスを与えます，アカウント名が含まれますので修正して実行してください．
1
|
$ ros2 service call /map_server/load_map nav2_msgs/srv/LoadMap "{map_url: /home/yuu/stage_ws/src/stage_ros2/map/map.yaml}"
|
これで準備が整いました．
rviz2の画面上部にある2D Goal Poseというボタンにてナビゲーションの目標位置・姿勢を設定することができます．
rviz2から配信されるトピックの確認
上記の方法でナビゲーションの目標位置・姿勢を設定するとrviz2からトピックが配信されます．
どのようなトピックが配信されるか確認しましょう．
まずはrviz2が配信するトピックを確認します．
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
|
$ ros2 node info /rviz2
/rviz2
Subscribers:
/base_scan: sensor_msgs/msg/LaserScan
/clock: rosgraph_msgs/msg/Clock
/map: nav_msgs/msg/OccupancyGrid
/map_updates: map_msgs/msg/OccupancyGridUpdate
/parameter_events: rcl_interfaces/msg/ParameterEvent
Publishers:
/clicked_point: geometry_msgs/msg/PointStamped
/goal_pose: geometry_msgs/msg/PoseStamped
/initialpose: geometry_msgs/msg/PoseWithCovarianceStamped
/parameter_events: rcl_interfaces/msg/ParameterEvent
/rosout: rcl_interfaces/msg/Log
Service Servers:
/rviz2/describe_parameters: rcl_interfaces/srv/DescribeParameters
/rviz2/get_parameter_types: rcl_interfaces/srv/GetParameterTypes
/rviz2/get_parameters: rcl_interfaces/srv/GetParameters
/rviz2/list_parameters: rcl_interfaces/srv/ListParameters
/rviz2/set_parameters: rcl_interfaces/srv/SetParameters
/rviz2/set_parameters_atomically: rcl_interfaces/srv/SetParametersAtomically
Service Clients:
Action Servers:
Action Clients:
|
Publishersの項目を見ると5つのトピックを配信することがわかります．
この中の/goal_poseが目標位置・姿勢のトピックとなります．
メッセージはgeometry_msgs/msg/PoseStampedであることがわかります．
具体的にどのようなデータを配信しているか確認してみましょう．
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
|
$ ros2 interface show geometry_msgs/msg/PoseStamped
# A Pose with reference coordinate frame and timestamp
std_msgs/Header header
builtin_interfaces/Time stamp
int32 sec
uint32 nanosec
string frame_id
Pose pose
Point position
float64 x
float64 y
float64 z
Quaternion orientation
float64 x 0
float64 y 0
float64 z 0
float64 w 1
|
情報量が多いので分かりにくいですが，時間とフレームid，ロボットの目標位置・姿勢の情報を配信します．
具体的な例を見るためにechoコマンドで確認してみましょう．
まずは下記のコマンドを実行します．
1
|
$ ros2 topic echo /goal_pose
|
配信されていない時は何の情報も表示されます．
この状態でrviz2の2D Goal Poseからロボットの目標位置・姿勢を設定してみましょう．
設定するとechoコマンドを実行したターミナルに下記のような情報が表示されました．
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
|
header:
stamp:
sec: 4712
nanosec: 100000000
frame_id: map
pose:
position:
x: 1.9941736459732056
y: 0.6447784304618835
z: 0.0
orientation:
x: 0.0
y: 0.0
z: 0.02481465068905508
w: 0.9996920691448843
---
|
stampはROS内部のシステム上の時間，frame_idは基準となるtfフレーム，
poseのpositionは位置，orientationは姿勢の情報を表しています．
これらの情報をプログラムから配信することで，rviz2を用いたGUI操作を必要としない自律走行を実現することができます．
プログラミング
下記のプログラムを入力して実行してください．
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
|
import rclpy
from rclpy.node import Node
from geometry_msgs.msg import PoseStamped
import time
class Goal(Node):
def __init__(self):
super().__init__('set_goal_node')
self.ps_pub = self.create_publisher(PoseStamped, 'goal_pose', 1)
time.sleep(1.0)
def setGoal(self, px, py, pz, oz, ow):
goal_point = PoseStamped()
goal_point.pose.position.x = px
goal_point.pose.position.y = py
goal_point.pose.position.z = pz
goal_point.pose.orientation.z = oz
goal_point.pose.orientation.w = ow
goal_point.header.stamp = self.get_clock().now().to_msg()
goal_point.header.frame_id = 'map'
self.ps_pub.publish(goal_point)
def main(args=None):
rclpy.init(args=args)
node = Goal()
node.setGoal(3.0, 0.0, 0.0, 0.0, 1.0)
rclpy.spin(node)
node.destroy_node()
rclpy.shutdown()
if __name__ == '__main__':
main()
|
プログラムの内容
トピック通信の実装にて使用したサンプルプログラムをベースとしたプログラムになっています．
目新しい内容のみ解説します．
12
13
14
15
16
17
18
19
20
21
22
|
def setGoal(self, px, py, pz, oz, ow):
goal_point = PoseStamped()
goal_point.pose.position.x = px
goal_point.pose.position.y = py
goal_point.pose.position.z = pz
goal_point.pose.orientation.z = oz
goal_point.pose.orientation.w = ow
goal_point.header.stamp = self.get_clock().now().to_msg()
goal_point.header.frame_id = 'map'
self.ps_pub.publish(goal_point)
|
setGoalメソッドにてロボットの目標位置・姿勢のトピックを配信しています．
ロボットの位置はgoal_point.pose.positionのx,y,zに代入しています．
姿勢はクォータニオン(四元数)で表現されます．
3次元ベクトルで姿勢を表現するオイラー角が有名ですが，計算する上で幾つかの問題を抱えているため，
ROS2では姿勢を4次元ベクトル(x, y, z, w)で表現するクォータニオンが用いられます．
4つのパラメータで表現すると姿勢をイメージしにくいですが，
こちらの3D Rotation Converterがとても便利です．
stageシミュレータは2次元平面上を走行する2輪車をモデルとしています．
そのため，ロボットの姿勢はオイラー角のz軸まわり(ヨー角)に限定されます．
(x軸とy軸まわりにロボットを回転したら車体が傾いてしまいます．)
クォータニオンで表現した場合，ヨー角を回転させるとzとwのみが変化します．
そのため，プログラムではzとwのみを与えるようにしています．
goal_point.header.stampにはROS内部のシステム上の時間を与えます．
ROS内部のシステム上の時間は，self.get_clock().now().to_msg()で取得します．
24
25
26
27
|
def main(args=None):
rclpy.init(args=args)
node = Goal()
node.setGoal(3.0, 0.0, 0.0, 0.0, 1.0)
|
27行目でロボットの位置・姿勢を配信するsetGoalメソッドを実行しています．
第1，2，3引数は，それぞれロボットの座標x，y，z，
第4，5引数はクォータニオンで表現されるロボットの姿勢z，wを表しています．
課題
- ロボットの目標位置・姿勢を変えてサンプルプログラムを実行しなさい．
- 予め決めた2つの目標位置・姿勢を往復するようなプログラムを作成しなさい．