utils.py 9.68 KB
Newer Older
Brian Hou's avatar
Brian Hou committed
1
2
3
#!/usr/bin/env python

"""ROS geometry and map utility functions."""
4
from __future__ import division
Brian Hou's avatar
Brian Hou committed
5
6

import heapq
Brian Hou's avatar
Brian Hou committed
7
8
9
10
11
12
13
14
15
16
17
18
import numpy as np
import rospy
from geometry_msgs.msg import (
    Pose,
    PoseStamped,
    Transform,
    TransformStamped,
    Quaternion,
    Point32,
)
from nav_msgs.srv import GetMap
from std_msgs.msg import Header
Brian Hou's avatar
Brian Hou committed
19
from tf_conversions import transformations
Brian Hou's avatar
Brian Hou committed
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


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
Brian Hou's avatar
Brian Hou committed
114
    g = transformations.quaternion_matrix(q)
Brian Hou's avatar
Brian Hou committed
115
116
117
118
119
120
121
122
123
124
125
126
127
    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
    """
Brian Hou's avatar
Brian Hou committed
128
    return Quaternion(*transformations.quaternion_from_euler(0, 0, angle))
Brian Hou's avatar
Brian Hou committed
129
130
131
132
133
134
135
136
137
138
139


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)
    """
Brian Hou's avatar
Brian Hou committed
140
    _, _, yaw = transformations.euler_from_quaternion((q.x, q.y, q.z, q.w))
Brian Hou's avatar
Brian Hou committed
141
142
143
144
145
146
147
148
149
150
151
152
153
    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)
Brian Hou's avatar
Brian Hou committed
154
    return np.array([[c, -s], [s, c]])
Brian Hou's avatar
Brian Hou committed
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


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


Brian Hou's avatar
Brian Hou committed
270
def map_to_world(poses, map_info, out=None):
Brian Hou's avatar
Brian Hou committed
271
272
273
274
275
    """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)
Brian Hou's avatar
Brian Hou committed
276
      out: Optional output buffer to store results in. Defaults to the poses array
Brian Hou's avatar
Brian Hou committed
277
    """
Brian Hou's avatar
Brian Hou committed
278
279
280
    poses = np.atleast_2d(poses)
    if out is None:
        out = poses
Brian Hou's avatar
Brian Hou committed
281
282
283
284
    scale = map_info.resolution
    angle = quaternion_to_angle(map_info.origin.orientation)

    # Rotation
Brian Hou's avatar
Brian Hou committed
285
286
    out[:, [0, 1]] = np.matmul(poses[:, [0, 1]], rotation_matrix(angle))
    out[:, 2] = poses[:, 2] + angle
Brian Hou's avatar
Brian Hou committed
287
288

    # Scale
Brian Hou's avatar
Brian Hou committed
289
    out[:, :2] *= float(scale)
Brian Hou's avatar
Brian Hou committed
290
291

    # Translate
Brian Hou's avatar
Brian Hou committed
292
293
    out[:, 0] += map_info.origin.position.x
    out[:, 1] += map_info.origin.position.y
Brian Hou's avatar
Brian Hou committed
294

Brian Hou's avatar
Brian Hou committed
295
    return out
Brian Hou's avatar
Brian Hou committed
296
297


Brian Hou's avatar
Brian Hou committed
298
def world_to_map(poses, map_info, out=None):
Brian Hou's avatar
Brian Hou committed
299
300
301
302
303
    """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)
Brian Hou's avatar
Brian Hou committed
304
      out: Optional output buffer to store results in. Defaults to the poses array
Brian Hou's avatar
Brian Hou committed
305
    """
Brian Hou's avatar
Brian Hou committed
306
    poses = np.atleast_2d(poses)
Brian Hou's avatar
Brian Hou committed
307
308
    if out is None:
        out = poses
Brian Hou's avatar
Brian Hou committed
309
310
311
312
    scale = map_info.resolution
    angle = -quaternion_to_angle(map_info.origin.orientation)

    # Translation
Brian Hou's avatar
Brian Hou committed
313
314
    out[:, 0] = poses[:, 0] - map_info.origin.position.x
    out[:, 1] = poses[:, 1] - map_info.origin.position.y
Brian Hou's avatar
Brian Hou committed
315
316

    # Scale
Brian Hou's avatar
Brian Hou committed
317
    out[:, :2] = out[:, :2] * (1.0 / float(scale))
Brian Hou's avatar
Brian Hou committed
318

Brian Hou's avatar
Brian Hou committed
319
320
321
    # Rotate poses
    out[:, [0, 1]] = np.matmul(out[:, [0, 1]], rotation_matrix(angle))
    out[:, 2] = poses[:, 2] + angle
Brian Hou's avatar
Brian Hou committed
322

Brian Hou's avatar
Brian Hou committed
323
324
    return out

Brian Hou's avatar
Brian Hou committed
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344

def estimation_error(estimates, references):
    """Compute the error between the estimated and ground truth states.

    Args:
        estimates: Estimated states (from particle filter)
        references: Ground truth states

    Returns:
        position_errors: Positional errors at each timestep
        abs_angular_error: Absolute angular errors at each timestep
    """
    position_errors = np.linalg.norm(estimates[:, :2] - references[:, :2], axis=1)
    estimate_angles = estimates[:, 2]
    reference_angles = references[:, 2]
    angular_errors = np.arctan2(
        np.sin(reference_angles - estimate_angles),
        np.cos(reference_angles - estimate_angles),
    )
    return position_errors, np.abs(angular_errors)
Brian Hou's avatar
Brian Hou committed
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368


class PriorityQueue(object):
    def __init__(self):
        self.min_heap = []
        self.pop_counter = 0

    def __len__(self):
        return len(self.min_heap)

    def push(self, elem):
        heapq.heappush(self.min_heap, elem)

    def peek(self):
        if not self.min_heap:
            raise IndexError("no elements to peek")
        return self.min_heap[0][1]

    def pop(self):
        if not self.min_heap:
            raise IndexError("no elements to pop")
        elem = heapq.heappop(self.min_heap)
        self.pop_counter += 1
        return elem