utils.py 8.28 KB
Newer Older
Brian Hou's avatar
Brian Hou committed
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
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
#!/usr/bin/env python

"""ROS geometry and map utility functions."""

import numpy as np
import rospy
import tf.transformations as tr

from geometry_msgs.msg import (
    Pose,
    PoseStamped,
    Transform,
    TransformStamped,
    Quaternion,
    Point32,
)
from nav_msgs.srv import GetMap
from std_msgs.msg import Header


def pose_to_pq(msg):
    """Convert a C{geometry_msgs/Pose} into position/quaternion np.arrays.

    Args:
      msg: ROS message to be converted

    Returns:
      p: position as a np.array
      q: quaternion as a np.array (order = [x, y, z, w])
    """
    p = np.array([msg.position.x, msg.position.y, msg.position.z])
    q = np.array(
        [msg.orientation.x, msg.orientation.y, msg.orientation.z, msg.orientation.w]
    )
    return p, q


def pose_stamped_to_pq(msg):
    """Convert a C{geometry_msgs/PoseStamped} into position/quaternion np.arrays.

    Args:
      msg: ROS message to be converted

    Returns:
      p: position as a np.array
      q: quaternion as a np.array (order = [x, y, z, w])
    """
    return pose_to_pq(msg.pose)


def transform_to_pq(msg):
    """Convert a C{geometry_msgs/Transform} into position/quaternion np.arrays.

    Args:
      msg: ROS message to be converted

    Returns:
      p: position as a np.array
      q: quaternion as a np.array (order = [x, y, z, w])
    """
    p = np.array([msg.translation.x, msg.translation.y, msg.translation.z])
    q = np.array([msg.rotation.x, msg.rotation.y, msg.rotation.z, msg.rotation.w])
    return p, q


def transform_stamped_to_pq(msg):
    """Convert a C{geometry_msgs/TransformStamped} into position/quaternion np.arrays.

    Args:
      msg: ROS message to be converted

    Returns:
      p: position as a np.array
      q: quaternion as a np.array (order = [x, y, z, w])
    """
    return transform_to_pq(msg.transform)


def msg_to_se3(msg):
    """Convert geometric ROS messages into SE(3).

    Args:
      msg: ROS message to be converted. Acceptable types include
        C{geometry_msgs/Pose}, C{geometry_msgs/PoseStamped},
        C{geometry_msgs/Transform}, or C{geometry_msgs/TransformStamped}

    Returns:
      A 4x4 SE(3) matrix as a np.array

    Raises:
      TypeError if we receive an incorrect type.

    """
    if isinstance(msg, Pose):
        p, q = pose_to_pq(msg)
    elif isinstance(msg, PoseStamped):
        p, q = pose_stamped_to_pq(msg)
    elif isinstance(msg, Transform):
        p, q = transform_to_pq(msg)
    elif isinstance(msg, TransformStamped):
        p, q = transform_stamped_to_pq(msg)
    else:
        raise TypeError("Invalid type for conversion to SE(3)")
    norm = np.linalg.norm(q)
    if np.abs(norm - 1.0) > 1e-3:
        raise ValueError(
            "Received un-normalized quaternion (q = {0:s} ||q|| = {1:3.6f})".format(
                str(q), np.linalg.norm(q)
            )
        )
    elif np.abs(norm - 1.0) > 1e-6:
        q = q / norm
    g = tr.quaternion_matrix(q)
    g[0:3, -1] = p
    return g


def angle_to_quaternion(angle):
    """Convert yaw angle into a Quaternion ROS message.

    Args:
      angle: Yaw angle (radians)

    Returns:
      An equivalent C{geometry_msgs/Quaternion} message
    """
    return Quaternion(*tr.quaternion_from_euler(0, 0, angle))


def quaternion_to_angle(q):
    """Convert a C{geometry_msgs/Quaternion} into a yaw angle.

    Args:
      q: ROS message to be converted

    Returns:
      The equivalent yaw angle (radians)
    """
    _, _, yaw = tr.euler_from_quaternion((q.x, q.y, q.z, q.w))
    return yaw


def rotation_matrix(theta):
    """Construct a rotation matrix from a given angle.

    Args:
      theta: Angle (radians)

    Returns:
      The equivalent 2x2 rotation matrix as a np.array
    """
    c, s = np.cos(theta), np.sin(theta)
    return np.matrix([[c, -s], [s, c]])


def particle_to_pose(particle):
    """Convert a particle to a C{geometry_msgs/Pose} message.

    Args:
      particle: The particle to convert [x, y, theta]

    Returns:
      An equivalent C{geometry_msgs/Pose} message
    """
    pose = Pose()
    pose.position.x = particle[0]
    pose.position.y = particle[1]
    pose.orientation = angle_to_quaternion(particle[2])
    return pose


def pose_to_particle(msg):
    """Convert a C{geometry_msgs/Pose} into a particle.

    Args:
      msg: ROS message to be converted.

    Returns:
      A particle [x, y, theta]
    """
    x = msg.position.x
    y = msg.position.y
    theta = quaternion_to_angle(msg.orientation)
    return [x, y, theta]


def particles_to_poses(particles):
    """Convert a list of particles to a list of C{geometry_msgs/Pose} messages.

    Args:
      particles: A list of particles, where each element is itself a list of the form [x, y, theta]

    Returns:
      A list of equivalent C{geometry_msgs/Pose} messages
    """
    return list(map(particle_to_pose, particles))


def make_header(frame_id, stamp=None):
    """Create a header with the given frame_id and stamp.

    The default value of stamp is None, which results in a stamp denoting the
    time at which this function was called.

    Args:
      frame_id: The desired coordinate frame
      stamp: The desired stamp

    Returns:
      The resulting header
    """
    if stamp is None:
        stamp = rospy.Time.now()
    header = Header()
    header.stamp = stamp
    header.frame_id = frame_id
    return header


def point(npt):
    """Convert x/y coordinates into a C{geometry_msgs/Point32} message.

    Args:
      npt: A list of length two containing x and y coordinates

    Returns:
      A C{geometry_msgs/Point32} message
    """
    pt = Point32()
    pt.x, pt.y = npt[:2]
    return pt


def points(arr):
    """Convert a list of coordinates into a list of equivalent point messages.

    Args:
      arr: A list of coordinates, where each element is itself a list of the form [x, y]

    Returns:
      A list of C{geometry_msgs/Point32} messages
    """
    return list(map(point, arr))


def get_map(map_topic):
    """Get the map from the map server.

    Args:
      map_topic: The service topic that will provide the map

    Returns:
      map_img: A np.array with dimensions (map_info.height, map_info.width).
        A zero at a particular location indicates that it is impermissible.
        A one indicates that the location is permissible.
      map_info: Info about the map, see official docs for more info.
        http://docs.ros.org/noetic/api/nav_msgs/html/msg/MapMetaData.html
    """
    rospy.wait_for_service(map_topic)
    map_msg = rospy.ServiceProxy(map_topic, GetMap)().map
    array_255 = np.array(map_msg.data).reshape(
        (map_msg.info.height, map_msg.info.width)
    )
    map_img = np.zeros_like(array_255, dtype=bool)
    map_img[array_255 == 0] = 1
    return map_img, map_msg.info


def map_to_world(poses, map_info):
    """Convert an array of pixel locations in the map to poses in the world.

    Args:
      poses: Pixel poses in the map, converted in place. Should be nx3 np.array.
      map_info: Info about the map (returned by get_map)

    Returns:
      Poses in the world as a np.array
    """
    scale = map_info.resolution
    angle = quaternion_to_angle(map_info.origin.orientation)

    # Rotation
    c, s = np.cos(angle), np.sin(angle)

    # Store the x coordinates since they will be overwritten
    temp = np.copy(poses[:, 0])
    poses[:, 0] = c * poses[:, 0] - s * poses[:, 1]
    poses[:, 1] = s * temp + c * poses[:, 1]

    # Scale
    poses[:, :2] *= float(scale)

    # Translate
    poses[:, 0] += map_info.origin.position.x
    poses[:, 1] += map_info.origin.position.y
    poses[:, 2] += angle

    return poses


def world_to_map(poses, map_info):
    """Convert an array of poses in the world to pixel locations in the map image.

    Args:
      poses: Poses in the world, converted in place. Should be nx3 np.array.
      map_info: Info about the map (returned by get_map)
    """
    scale = map_info.resolution
    angle = -quaternion_to_angle(map_info.origin.orientation)

    # Translation
    poses[:, 0] -= map_info.origin.position.x
    poses[:, 1] -= map_info.origin.position.y

    # Scale
    poses[:, :2] *= 1.0 / float(scale)

    # Rotation
    c, s = np.cos(angle), np.sin(angle)

    # Store the x coordinates since they will be overwritten
    temp = np.copy(poses[:, 0])
    poses[:, 0] = c * poses[:, 0] - s * poses[:, 1]
    poses[:, 1] = s * temp + c * poses[:, 1]
    poses[:, 2] += angle