<で説明した制御方法2です）
- ロボットから目的地までの角度と距離を計算します
- 注1：これを計算するやり方はいくつかあります。いずれを使っても大丈夫です。
- → ロボットのポーズ（位置と向き）を /odom トピックから読んで、それに基づいて計算する
- → TFを利用して目的地の位置をロボットの座標系（フレーム/base_link）に変換する
- 注2：ROSでは角度が通常「クォータニオン」として使われています。クォータニオンから普通の角度（オイラー角度）へ変換するため、TFのAPIのメソッド `tf.transformations.euler_from_quaternion()` が利用できます。
- 目的地までの角度とロボットの現在の向きの差を計算します。
- 角速度の計算：
- 角速度 = K * 角度の差
- → Kの適切な値を試行錯誤で決定します。
- ロボットが速く回転しすぎないように、角速度を制限します（0.5 rad/s以下）
- 直線速度の計算：
- 角度の差（の絶対値）が45度より大きい場合、速度は 0 にするのがお勧めです。
（ロボットがだいたい目的地に向かっていることを待ちます）
- それ以外の場合、線形速度は一定にします（最大 0.25 m/s）
- ロボットがターゲットに近づいたら、速度を下げます
### （7）
課題のパッケージのlaunchファイルに上記のプログラムを新しいノードとして追加しましょう。
launchファイルを実行し、RVizで目的地を与えたらロボットがそちらに移動することを確認してください。
## （任意）自分でオドメトリを計算する
### (8)
簡単なオドメトリ計算プログラムを自分で実装してみましょう。以下の手順に従い、新しいROSノードを作成してください。
- ロボットの線速度 v と角速度 ω をトピック（例：/cmd_vel）から取得します。
- オドメトリの初期値として、位置 x=0、y=0、向き θ=0 を設定します。
- [週2のイントロ](v2_intro.html)で紹介した式を使って、時間間隔 T ごとに位置と向きを更新します。
- 計算したオドメトリを/my_odomトピックに配信し、nav_msgs/OdometryメッセージとしてRVizで可視化できるようにします（詳細なやり方はこの下に書いています）。
-トピック /odom RVizで可視化して、ロボットを動かしながら/odom と /my_odom のデータを比較してください。
#### Odometryメッセージの作成
トピック /my_odom を可視化するため nav_msgs/Odometry メッセージとして配信したほうがいいです（/odom トピックと同様）。
以下は計算した x,y,θ を nav_msgs/Odometry メッセージに変換するコードの例です。
```python
from nav_msgs.msg import Odometry
import rospy
from tf.transformations import quaternion_from_euler
odom_msg = Odometry()
odom_msg.header.stamp = rospy.Time.now()
odom_msg.header.frame_id = "odom"
# Pose
odom_msg.pose.pose.position.x = x
odom_msg.pose.pose.position.y = y
quaternion = quaternion_from_euler(0, 0, theta)
odom_msg.pose.pose.orientation.x = quaternion[0]
odom_msg.pose.pose.orientation.y = quaternion[1]
odom_msg.pose.pose.orientation.z = quaternion[2]
odom_msg.pose.pose.orientation.w = quaternion[3]
# Twist
odom_msg.twist.twist.linear.x = v
odom_msg.twist.twist.angular.z = omega
```
---
## 提出するビデオで何を示せばいいのか？
（すべての段階の結果がビデオにはっきりと表示されていることに注意してください！）
- launchファイルを実行し、Rvizが起動するのを待ちます（タスク(８)をできた場合、トピック/my_odomも表示して）
- Rvizでは、2D NavGoalボタンを使用して移動目的地を設定します
- ロボットが与えられた目的地まで移動するのを待ちます
- あと2つぐらいの目的地を与えてください
---
リマインダー：提出するのは
- 上記のビデオ（スクリーンキャスト）
- この課題で作ったパッケージ （robot2\_task\_名字 フォルダ）
が入った圧縮ファイルです。
---
### ヒント
1周目の実習では、メッセージの購読と配信を説明しました。しかし、同じノードで配信と購読の両方を行う必要がある場合が多いです（この課題もそうです）。
以下はそれを行うノードの基本的な例です。
```python
#!/usr/bin/env python
import rospy
from std_msgs.msg import String
def callbackTopic1(message):
# input_topic1に届いたメッセージの処理
# ...
def callbackTopic2(message):
# input_topic1に届いたメッセージの処理
# ...
# コールバック関数内でメッセージを発信する場合がある
pub2.publish(pub_msg2)
def hello_all():
rospy.init_node('hello_all')
pub1 = rospy.Publisher('output_topic1', OutputTopicType1)
pub2 = rospy.Publisher('output_topic2', OutputTopicType2)
rospy.Subscriber("input_topic1", InputTopicType1, callbackTopic1)
rospy.Subscriber("input_topic2", InputTopicType2, callbackTopic2)
# rospy.spin() は使いません
# （spinは無限ループなので、ここで使ったら以下のループに入れなくなります）
r = rospy.Rate(XX) # XXhz
while not rospy.is_shutdown():
# ...
pub1.publish(pub_msg1)
r.sleep()
if __name__ == '__main__':
hello_all()
```
---
← [実習その2](v2_2.html)
↑ [ホームページ](index.html)
]]>