odom_publisher.py

 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
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
#!/usr/bin/env python

import rospy
from nav_msgs.msg import Odometry
from geometry_msgs.msg import PoseStamped, TwistStamped, Quaternion
import tf

# Callback for local position
local_pos = PoseStamped()
def local_pos_cb(msg):
    # Need global declaration in order to assign global variable
    global local_pos
    local_pos = msg

# Callback for local velocity
local_vel = TwistStamped()
def local_vel_cb(msg):
    # Need global declaration in order to assign global variable
    global local_vel
    local_vel = msg

def odom_publisher():
    rospy.init_node("odom_publisher")

    odom_pub = rospy.Publisher("odom", Odometry, queue_size=50)

    local_pos_sub = rospy.Subscriber("mavros/local_position/pose",
                                      PoseStamped,
                                      callback=local_pos_cb)
    local_vel_sub = rospy.Subscriber("mavros/local_position/velocity",
                                      TwistStamped,
                                      callback=local_vel_cb)

    rate = rospy.Rate(20)

    while not rospy.is_shutdown():

        pose = local_pos.pose
        twist = local_vel.twist

        # Create quaternion from euler angle of local position
        odom_quat = tf.transformations.quaternion_from_euler(pose.orientation.x,
                                                             pose.orientation.y,
                                                             pose.orientation.z)

        odom = Odometry()
        odom.header.stamp = rospy.Time.now()
        # Global frame is "odom"
        odom.header.frame_id = "odom"

        odom.pose.pose.position.x = pose.position.x
        odom.pose.pose.position.y = pose.position.y
        odom.pose.pose.position.z = 0
        odom.pose.pose.orientation = odom_quat

        # Set robot's base frame as odometry child frame
        odom.child_frame_id = "base_link"
        odom.twist.twist.linear.x = twist.linear.x
        odom.twist.twist.linear.y = twist.linear.y
        odom.twist.twist.angular.z = twist.angular.z

        odom_pub.publish(odom)
        rate.sleep()

# Only run if executed as top level script
if __name__ == "__main__":
    try:
        odom_publisher()
    except rospy.ROSInterruptException:
        pass

解説

#!/usr/bin/env python

ROSノードとして実行するために必要な記述です。 詳しくは""python shebang"などで検索してください。

# Callback for local position
local_pos = PoseStamped()
def local_pos_cb(msg):
    # Need global declaration in order to assign global variable
    global local_pos
    local_pos = msg

# Callback for local velocity
local_vel = TwistStamped()
def local_vel_cb(msg):
    # Need global declaration in order to assign global variable
    global local_vel
    local_vel = msg

mavros/local_position/pose トピックと mavros/local_position/velocity トピックのサブスクライバです。 どちらもグローバル変数に値を格納するだけの関数です。

odom_quat = tf.transformations.quaternion_from_euler(pose.orientation.x,
                                                      pose.orientation.y,
                                                      pose.orientation.z)

オイラー角からクォータニオンへの変換を行っています。

odom = Odometry()
odom.header.stamp = rospy.Time.now()
# Global frame is "odom"
odom.header.frame_id = "odom"

odom.pose.pose.position.x = pose.position.x
odom.pose.pose.position.y = pose.position.y
odom.pose.pose.position.z = 0
odom.pose.pose.orientation = odom_quat

# Set robot's base frame as odometry child frame
odom.child_frame_id = "base_link"
odom.twist.twist.linear.x = twist.linear.x
odom.twist.twist.linear.y = twist.linear.y
odom.twist.twist.angular.z = twist.angular.z

odom_pub.publish(odom)

Odometry 型の変数を作成し、それぞれのフィールドに姿勢や速度などの値を格納しています。 今回はグローバル座標は odom で、ロボットのベース座標は base_link になっています。

if __name__ == "__main__":
  try:
      odom_publisher()
  except rospy.ROSInterruptException:
      pass

一行目は、プログラムがトップレベルスクリプトとして実行された時のみ実行されるようにするための記述です。 try-except 節は、Ctrl-Cなどでノードが停止された時に送出される ROSInterruptException 例外をキャッチするための部分です。