test_msgconv.py 2.27 KB
Newer Older
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
71
72
73
74
75
76
77
78
79
80
81
import pytest
from geometry_msgs.msg import Twist, Vector3, Point32, Point, Quaternion, Pose
from sensor_msgs.msg import PointCloud, LaserScan
from std_msgs.msg import Header

from msg_utils.msg_conversion import msg_to_dict, dict_to_msg

def test_vector3():
    vec = Vector3(x=1.0, y=2.0, z=3.0)

    result = msg_to_dict(vec)

    assert result["x"] == 1.0
    assert result["y"] == 2.0
    assert result["z"] == 3.0

def test_pose():
    p = Point(x=1.0, y=2.0, z=3.0)
    q = Quaternion(x=0.1, y=0.2, z=0.3, w=0.4)

    pose = Pose(position=p, orientation=q)

    result = msg_to_dict(pose)

    assert result["position"]["x"] == 1.0
    assert result["position"]["y"] == 2.0
    assert result["position"]["z"] == 3.0
    assert result["orientation"]["x"] == 0.1
    assert result["orientation"]["y"] == 0.2
    assert result["orientation"]["z"] == 0.3
    assert result["orientation"]["w"] == 0.4

def test_laserscan():
    pc = PointCloud(
        points=[Point32(x=float(n), y=float(n), z=float(n)) for n in range(5)]
    )

    _fields_and_field_types = {
        "header": "std_msgs/Header",
        "angle_min": "float",
        "angle_max": "float",
        "angle_increment": "float",
        "time_increment": "float",
        "scan_time": "float",
        "range_min": "float",
        "range_max": "float",
        "ranges": "sequence<float>",
        "intensities": "sequence<float>",
    }

        # ls = LaserScan(header=h, angle_min=0.1, angle_max=6.0, angle_increment=0.05, time_increment=0.01, scan_time=0.5, range_min=0.1, range_max=10.0)


# class TestDictToRosMessage(unittest.TestCase):
def test_vector3_back():
    vec = Vector3(x=1.0, y=2.0, z=3.0)

    msgdict = msg_to_dict(vec)
    result = dict_to_msg(msgdict, Vector3)

    assert result.x == 1.0
    assert result.y == 2.0
    assert result.z == 3.0

def test_pose_back():
    p = Point(x=1.0, y=2.0, z=3.0)
    q = Quaternion(x=0.1, y=0.2, z=0.3, w=0.4)

    pose = Pose(position=p, orientation=q)

    msgdict = msg_to_dict(pose)

    result = dict_to_msg(msgdict, Pose)

    assert result.position.x == 1.0
    assert result.position.y == 2.0
    assert result.position.z == 3.0
    assert result.orientation.x == 0.1
    assert result.orientation.y == 0.2
    assert result.orientation.z == 0.3
    assert result.orientation.w == 0.4