turtlebot3のLiderを用いて指定位置まで自律移動させる

turtlebot3を自律移動させるパッケージとしてnavigationがあります。
navigationを利用して指定位置までturtlebot3を動かすノードを作ってみます。

mapの作成

まず環境マップを作成する。 今回は例としてシミュレーション環境のturtlebot3_worldを用いる。

端末を開き

$ roslaunch turtlebot3_gazebo turtlebot3_world.launch

シミュレーション環境が構築される。

slamを参考にしてmap.pgmとmap.yamlを作成する。

移動先を指定するノードの調査

端末を開き

$ roslaunch turtlebot3_gazebo turtlebot3_world.launch

navigationを参考にしてmapファイルを読み込み、rivzで表示する。

rvizの2D Pose Estimateを選択してturtlebot3の初期位置を設定する。同様に2D Nav Goalを選択するとturtlebot3の移動先を設定できる。
この状態でのノード・トピックの関係を確認するため、他の端末を開き

$ rqt_graph

/move_base/goalが移動先を設定するトピックだと考えられる。 /move_base/goalについて調査する。 アクティブなトピックをリスト表示する。

$ rostopic list
/amcl/parameter_descriptions
...
/move_base/goal
...

/move_base/goalがトピックであると再度確認できた。 /move_base/goalのトピックの型を確認する。

$ rostopic type /move_base/goal
move_base_msgs/MoveBaseActionGoal

move_base_msgs/MoveBaseActionGoalのメッセージタイプの構造を確認する。

$ rosmsg show move_base_msgs/MoveBaseActionGoal
std_msgs/Header header
  uint32 seq
  time stamp
  string frame_id
actionlib_msgs/GoalID goal_id
  time stamp
  string id
move_base_msgs/MoveBaseGoal goal
  geometry_msgs/PoseStamped target_pose
    std_msgs/Header header
      uint32 seq
      time stamp
      string frame_id
    geometry_msgs/Pose pose
      geometry_msgs/Point position
        float64 x
        float64 y
        float64 z
      geometry_msgs/Quaternion orientation
        float64 x
        float64 y
        float64 z
        float64 w

実際に/move_base/goalにはどんなメッセージが送られているかを確認する。rostopic echoにてメッセージを待ち、rviz上で2D Nav Goalで移動先を設定すると以下のようなメッセージが確認できる。

$ rostopic echo /move_base/goal
header: 
  seq: 1
  stamp: 
    secs: 98
    nsecs:  87000000
  frame_id: ''
goal_id: 
  stamp: 
    secs: 0
    nsecs:         0
  id: ''
goal: 
  target_pose: 
    header: 
      seq: 1
      stamp: 
        secs: 98
        nsecs:  86000000
      frame_id: "map"
    pose: 
      position: 
        x: -1.93752658367
        y: -0.244830369949
        z: 0.0
      orientation: 
        x: 0.0
        y: 0.0
        z: 0.707669681949
        w: 0.706543431963

以上の情報を元に適当に指定した位置を/move_base/goalにpublishするプログラムを以下に示す。

#!/usr/bin/python
# coding: UTF-8

import rospy
from move_base_msgs.msg import MoveBaseActionGoal

rospy.init_node('topic_publisher')
pub = rospy.Publisher('/move_base/goal', MoveBaseActionGoal, queue_size=5)
rate = rospy.Rate(0.1)

target_pos=MoveBaseActionGoal()
target_pos.goal.target_pose.header.frame_id = "map"
target_pos.goal.target_pose.pose.position.x = -2.0
target_pos.goal.target_pose.pose.orientation.w = 0.999607289662

count = 0
while not rospy.is_shutdown():
    target_pos.header.stamp = rospy.Time.now()
    target_pos.goal.target_pose.header.stamp = rospy.Time.now()
    pub.publish(target_pos)
    rate.sleep()
    count += 1
    print count

指定位置まで移動するプログラムの実行

端末を開き

roslaunch turtlebot3_gazebo turtlebot3_world.launch

別の端末を開き

roslaunch turtlebot3_navigation turtlebot3_navigation.launch map_file:=$HOME/map.yaml

別の端末を開き

rosrun rviz rviz -d `rospack find turtlebot3_navigation`/rviz/turtlebot3_nav.rviz

別の端末を開き、先ほどのプログラムを実行