RobotinoでROS（その2）

RobotinoでROSの続きです．

Robotino のodometryを(x, y, z)で公開していましたが，zはz軸を表す（つまり，高さ）ため，回転角のところで表す方が適切です．
とすると，zは0もしくはロボットの高さとし，向きはquaternion で扱った方が良いハズ．

    # pose.position = point
    # set Pose
    pose.position.x = float(udp.view3Recv[2]) / 10
    pose.position.y = float(udp.view3Recv[3]) / 10
    pose.position.z = 0
    # set quaternion
    theta = math.radians(float(udp.view3Recv[4]) / 10)
    pose.orientation.x = math.cos(theta / 2.0)
    pose.orientation.y = math.sin(theta / 2.0)
    pose.orientation.z = math.sin(theta / 2.0)
    pose.orientation.w = 0

となります．
import mathも必要になってくるかな．