TurtleBot3 のシミュレーション環境でmy_robotを使ってみます。
（4/5）TurtleBot3のシミュレーション環境を使ってみる
この環境は自律走行シミュレーターとしては非常に使いやすいです。
この環境に自作ロボットを組み込む手っ取り早い方法として、Waffle モデルをmy_robotに置き換えることにします。
で、ナビゲーションをやってみます。
Goalを設定する方法は、Rvizの２D Nav Goal と、Python の外部コマンドで設定する方法の２種類です。
my_robotで作成したロボットのモデルは以下のフォルダーにあります（あるはずです）。
~/catkin_ws/src/my_robot/urdf/my_robot.urdf.xacro
TurtleBot3 のシミュレーション環境で使うWaffle モデルは以下のフォルダーにあります。
/opt/ros/melodic/share/turtlebot3_description/urdf/turtlebot3_waffle.urdf.xacro
元のモデルは改名保存しておいて、置き換えます。
|
1 2 3 4 5 |
cd /opt/ros/melodic/share/turtlebot3_description/urdf sudo mv turtlebot3_waffle.urdf.xacro ORG-turtlebot3_waffle.urdf.xacro sudo cp ~/catkin_ws/src/my_robot/urdf/my_robot.urdf.xacro turtlebot3_waffle.urdf.xacro |
これでWaffle としてmy_robotが使えます。
１：２D Nav Goal でナビゲーション
独自空間（my_room.world）でmy_robotを使ってみましょう。
以下のlaunchファイルについては（4/5）参照
|
1 |
roslaunch turtlebot3_gazebo turtlebot3_my_room.launch |
hector slam で作った地図を使ってナビゲーションしてみます（gmappingの場合、２D Pose Estimation による初期位置補正が必要になりますが、hector は不要なので扱いやすいです）。
navigationを起動します。
roslaunch turtlebot3_navigation turtlebot3_navigation.launch map_file:=$HOME/maps/my_room_hector_map.yaml
メニュの「２D Nav Goal」を選択して、隣室中央でクリックしてGoalを設定してみました。
最適経路が策定されて自律走行を開始します。
２：ナビゲーションのGoal設定を外部コマンドでやってみる
上記のナビゲーションでは２D Nav Goal とマウスクリックでGoalを設定しましたが、ここではPythonで実行コマンドを作成してみます。
Pythonのコードを使ったコマンドをROSのパッケージにします。
ROSではGoalを指示するtopicは/move_base_simple/goalで、メッセージの型は以下のようになっています。
|
1 |
rostopic type /move_base_simple/goal |
geometry_msgs/PoseStamped
コマンド用のROSパッケージの依存パッケージは以下の２つです。
rospy、geometry_msgs
パッケージ名をnav_goal、Pythonファイル名をgo2goal.pyとします。
ワークスペースに移動してパッケージを作成
|
1 2 3 |
cd ~/catkin_ws/src catkin_create_pkg nav_goal rospy geometry_msgs |
Pythonを使うのでscriptsフォルダーを作って移動し、新規のファイルを開いておきます。
|
1 2 3 4 5 |
mkdir ~/catkin_ws/src/nav_goal/scripts cd ~/catkin_ws/src/nav_goal/scripts sudo nano go2goal.py |
コードはこんな感じ。
Goalの座標値を引数で渡して、移動させます。
|
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 |
#! /usr/bin/env python3 import sys import rospy from geometry_msgs.msg import PoseStamped args = sys.argv x = float(args[1]) y = float(args[2]) class Goal: def __init__(self): rospy.init_node('nav_goal', anonymous=True) self.ps_pub = rospy.Publisher('move_base_simple/goal',PoseStamped,queue_size=1) def setGoal(self, px, py, pz): rospy.sleep(1.0) now = rospy.Time.now() 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.w = 1.0 goal_point.header.stamp = now goal_point.header.frame_id = 'map' self.ps_pub.publish(goal_point) if __name__ == '__main__': try: goal_ob = Goal() goal_ob.setGoal(x, y, 0.0) rospy.spin() except rospy.ROSInterruptException: pass |
権限を与えておきます。
sudo chmod +x go2goal.py
source ~/catkin_ws/devel/setup.bash
ところで座標はどうなっているかというと…。
Gazeboの座標はこうなっていますが、ここが原点ではありません。
RvizのBot3の初期位置、ここが原点です。
で、移動先を指定しますが、ｘ軸方向に2、ｙ軸方向に−２移動するとします。
こういうコマンドを実行します。
|
1 |
rosrun nav_goal go2goal.py 2.0 -2.0 |
右下の別ターミナルでコマンドを実行しています。これで命令がPublishされますが、実行までに少々タイムラグがあります（５秒ほど）。実行されると指定の位置に矢印が現れて最適経路が表示され、Bot3が移動を開始します。また実行が開始されればコマンドはCtrl + C で終了しても可です。
原点は初期位置で固定されているので、以下のようなコマンドの場合
rosrun nav_goal go2goal.py -1.0 0.0
ここに移動します。
これでロボットに対して「玄関へ行って」という音声コマンドも送れるようになります。
Leave a Reply