def test_negative_rotation_from_carla():
""" Test that Rotation throws a ValueError if incorrect carla_rot argument
is passed. """
DummyType = namedtuple("DummyType", "pitch, yaw, roll")
dummy_instance = DummyType(10, 20, 30)
with pytest.raises(ValueError):
Rotation.from_carla_rotation(dummy_instance)
def generate_waypoints(self, ego_transform, waypoint, road_option):
# Keep on driving in the same lane/straight if the next waypoint is
# far away.
if (road_option == pylot.utils.RoadOption.STRAIGHT
or road_option == pylot.utils.RoadOption.LANE_FOLLOW
or ego_transform.location.distance(
waypoint.location.distance) > 20):
for lane in self.lanes:
if lane.id == 0:
return lane.get_lane_center_transforms()
# Lane detector didn't find ego's lane. Keep on driving
# in the same direction.
output_wps = deque([])
for distance in range(1, 20):
wp = (ego_transform *
Transform(Location(x=distance), Rotation()))
output_wps.append(wp)
return output_wps
elif road_option == pylot.utils.RoadOption.LEFT:
output_wps = deque([])
wp = copy.deepcopy(ego_transform)
for distance in range(1, 11):
wp = wp * Transform(Location(x=1), Rotation(yaw=-9))
output_wps.append(wp)
return output_wps
elif road_option == pylot.utils.RoadOption.RIGHT:
output_wps = deque([])
wp = copy.deepcopy(ego_transform)
for distance in range(1, 11):
wp = wp * Transform(Location(x=1), Rotation(yaw=9))
output_wps.append(wp)
return output_wps
elif road_option == pylot.utils.RoadOption.CHANGE_LANE_LEFT:
for lane in self.lanes:
if lane.id == -1:
return lane.get_lane_center_transforms()
# Lane detector didn't find left lane.
output_wps = deque([])
wp = copy.deepcopy(ego_transform)
for distance in range(1, 11):
wp = wp * Transform(Location(x=1), Rotation(yaw=-4))
output_wps.append(wp)
return output_wps
elif road_option == pylot.utils.RoadOption.CHANGE_LANE_RIGHT:
for lane in self.lanes:
if lane.id == 1:
return lane.get_lane_center_transforms()
# Lane detector didn't find right lane.
output_wps = deque([])
wp = copy.deepcopy(ego_transform)
for distance in range(1, 11):
wp = wp * Transform(Location(x=1), Rotation(yaw=4))
output_wps.append(wp)
return output_wps
self._logger.debug('Unexpected road option {}'.format(road_option))
return deque([ego_transform])
def test_as_carla_rotation():
""" Test the as_carla_rotation instance method of Rotation """
rotation = Rotation(pitch=1, yaw=2, roll=3)
carla_rotation = rotation.as_carla_rotation()
assert isinstance(carla_rotation, carla.Rotation), "Returned instance is "
"not of the type carla.Rotation"
assert np.isclose(carla_rotation.pitch, rotation.pitch), "Returned "
"instance pitch value is not the same as the one in rotation."
assert np.isclose(carla_rotation.yaw, rotation.yaw), "Returned instance "
"yaw value is not the same as the one in rotation."
assert np.isclose(carla_rotation.roll, rotation.roll), "Returned instance "
"roll value is not the same as the one in location."
def _construct_waypoints(self, timestamp, path_x, path_y, speeds, success):
"""
Convert the optimal frenet path into a waypoints message.
"""
path_transforms = []
target_speeds = []
if not success:
self._logger.debug("@{}: Frenet Optimal Trajectory failed. "
"Sending emergency stop.".format(timestamp))
for wp in itertools.islice(self._prev_waypoints, 0,
DEFAULT_NUM_WAYPOINTS):
path_transforms.append(wp)
target_speeds.append(0)
else:
self._logger.debug("@{}: Frenet Optimal Trajectory succeeded."
.format(timestamp))
for point in zip(path_x, path_y, speeds):
if self._map is not None:
p_loc = self._map.get_closest_lane_waypoint(
Location(x=point[0], y=point[1], z=0)).location
else:
p_loc = Location(x=point[0], y=point[1], z=0)
path_transforms.append(
Transform(
location=Location(x=point[0], y=point[1], z=p_loc.z),
rotation=Rotation(),
))
target_speeds.append(point[2])
waypoints = deque(path_transforms)
self._prev_waypoints = waypoints
return WaypointsMessage(timestamp, waypoints, target_speeds)
def get_lane_center_transforms(self):
if len(self.left_markings) < len(self.right_markings):
anchor_markings = self.left_markings
other_markings = self.right_markings
else:
anchor_markings = self.right_markings
other_markings = self.left_markings
index_other = 0
center_markings = deque([])
for transform in anchor_markings:
dist = transform.location.distance(
other_markings[index_other].location)
while (index_other + 1 < len(other_markings)
and dist > transform.location.distance(
other_markings[index_other + 1].location)):
index_other += 1
dist = transform.location.distance(
other_markings[index_other].location)
if index_other < len(other_markings):
other_loc = other_markings[index_other].location
center_location = Location(
(transform.location.x + other_loc.x) / 2.0,
(transform.location.y + other_loc.y) / 2.0,
(transform.location.z + other_loc.z) / 2.0)
center_markings.append(Transform(center_location, Rotation()))
return center_markings
def create_center_lidar_setup(location, rotation_frequency=20):
""" Creates a LidarSetup instance with the given location.
The Rotation is set to (pitch=0, roll=0, yaw=0).
Args:
location (:py:class:`~pylot.utils.Location`): The location of the
LIDAR with respect to the center of the vehicle.
Returns:
:py:class:`~pylot.drivers.sensor_setup.LidarSetup`: A LidarSetup
with the given location.
"""
rotation = Rotation()
# Place the lidar in the same position as the camera.
lidar_transform = Transform(location, rotation)
return LidarSetup(
name='front_center_lidar',
lidar_type='sensor.lidar.ray_cast',
transform=lidar_transform,
range=5000, # in centimeters
rotation_frequency=rotation_frequency,
channels=32,
upper_fov=15,
lower_fov=-30,
points_per_second=250000)
def create_segmented_camera_setup(camera_name_prefix,
camera_location,
width,
height,
fov=90):
""" Creates a SegmentedCameraSetup instance with the given values.
The Rotation is set to (pitch=0, yaw=0, roll=0).
Args:
camera_name_prefix (str): The name of the camera instance. A suffix
of "_segmented" is appended to the name.
camera_location (:py:class:`~pylot.utils.Location`): The location of
the camera with respect to the center of the vehicle.
width (int): The width of the image returned by the camera.
height (int): The height of the image returned by the camera.
fov (float): The field of view of the image returned by the camera.
Returns:
:py:class:`~pylot.drivers.sensor_setup.SegmentedCameraSetup`: A
camera setup with the given parameters.
"""
transform = Transform(camera_location, Rotation())
return SegmentedCameraSetup(camera_name_prefix + '_segmented',
width,
height,
transform,
fov=fov)
def create_rgb_camera_setup(camera_name,
camera_location,
width,
height,
fov=90):
transform = Transform(camera_location, Rotation())
return RGBCameraSetup(camera_name, width, height, transform, fov)
def test_camera_unreal_transform(rotation, expected):
"""
Ensure that the camera space to unreal engine coordinate space conversion
is correct.
The camera space is defined as:
+x to the right, +y to down, +z into the screen.
The unreal engine coordinate space is defined as:
+x into the screen, +y to the right, +z to up.
"""
camera_rotation = Rotation(*rotation)
camera_setup = CameraSetup(name='camera_setup',
camera_type='sensor.camera.rgb',
width=1920,
height=1080,
transform=Transform(Location(),
camera_rotation))
transformed_rotation = camera_setup.get_unreal_transform().rotation
transformed_rotation = [
transformed_rotation.pitch, transformed_rotation.yaw,
transformed_rotation.roll
]
assert all(np.isclose(transformed_rotation, expected)), \
"The unreal transformation does not match the expected transform."
def test_lidar_unreal_transform(rotation, expected):
"""
Ensure that the LIDAR space to unreal engine coordinate space conversion
is correct.
The LIDAR space is defined as:
+x to the right, +y out of the screen, +z is down.
The unreal engine coordinate space is defined as:
+x into the screen, +y to the right, +z to up.
"""
lidar_rotation = Rotation(*rotation)
lidar_setup = LidarSetup(name='lidar_setup',
lidar_type='sensor.lidar.ray_cast',
transform=Transform(Location(), lidar_rotation),
range=6000.0,
rotation_frequency=3000.0,
channels=3,
upper_fov=90.0,
lower_fov=90.0,
points_per_second=100)
transformed_rotation = lidar_setup.get_unreal_transform().rotation
transformed_rotation = [
transformed_rotation.pitch, transformed_rotation.yaw,
transformed_rotation.roll
]
assert all(np.isclose(transformed_rotation, expected)), \
"The unreal transformation does not match the expected transform."
def test_imu_setup_initialization():
# Set up the required parameters for the initialization.
name = 'imu_sensor'
transform = Transform(Location(), Rotation())
imu_setup = IMUSetup(name, transform)
assert imu_setup.name == name, "The name in the setup is not the same."
assert imu_setup.transform == transform, "The transform is not the same."
def run(self):
# Read the vehicle ID from the vehicle ID stream.
vehicle_id_msg = self._vehicle_id_stream.read()
vehicle_id = vehicle_id_msg.data
self._logger.debug("@{}: Received Vehicle ID: {}".format(
vehicle_id_msg.timestamp, vehicle_id))
# Connect to the world.
_, world = get_world(self._flags.simulator_host,
self._flags.simulator_port,
self._flags.simulator_timeout)
self._vehicle = get_vehicle_handle(world, vehicle_id)
# Install the collision sensor.
collision_blueprint = world.get_blueprint_library().find(
'sensor.other.collision')
self._logger.debug("Spawning a collision sensor.")
self._collision_sensor = world.spawn_actor(
collision_blueprint,
Transform(Location(), Rotation()).as_simulator_transform(),
attach_to=self._vehicle)
# Register the callback on the collision sensor.
self._collision_sensor.listen(self.process_collision)
def _construct_waypoints(self, timestamp, path_x, path_y, speeds, success):
"""
Convert the rrt* path into a waypoints message.
"""
path_transforms = []
target_speeds = []
if not success:
self._logger.error("@{}: RRT* failed. "
"Sending emergency stop.".format(timestamp))
for wp in itertools.islice(self._prev_waypoints, 0,
DEFAULT_NUM_WAYPOINTS):
path_transforms.append(wp)
target_speeds.append(0)
else:
self._logger.debug("@{}: RRT* succeeded."
.format(timestamp))
for point in zip(path_x, path_y, speeds):
if self._map is not None:
p_loc = self._map.get_closest_lane_waypoint(
Location(x=point[0], y=point[1], z=0)).location
else:
# RRT* does not take into account the driveable region
# it constructs search space as a top down, minimum bounding rectangle
# with padding in each dimension
p_loc = Location(x=point[0], y=point[1], z=0)
path_transforms.append(
Transform(
location=Location(x=point[0], y=point[1], z=p_loc.z),
rotation=Rotation(),
))
target_speeds.append(point[2])
waypoints = deque(path_transforms)
self._prev_waypoints = waypoints
return WaypointsMessage(timestamp, waypoints, target_speeds)
def test_rotation_from_carla(pitch, yaw, roll):
""" Test that the Rotation is initialized correctly from a carla.Rotation
instance """
carla_rotation = carla.Rotation(pitch, yaw, roll)
rotation = Rotation.from_carla_rotation(carla_rotation)
assert np.isclose(rotation.pitch, pitch), "pitch values are not the same."
assert np.isclose(rotation.yaw, yaw), "yaw values are not the same."
assert np.isclose(rotation.roll, roll), "roll values are not the same."
def test_point_cloud_get_pixel_location(lidar_points, pixel, expected):
camera_setup = CameraSetup(
'test_setup',
'test_type',
801,
601, # width, height
Transform(location=Location(0, 0, 0), rotation=Rotation(0, 0, 0)),
fov=90)
point_cloud = PointCloud(lidar_points, Transform(Location(), Rotation()))
location = point_cloud.get_pixel_location(pixel, camera_setup)
assert np.isclose(location.x,
expected.x), 'Returned x value is not the same '
'as expected'
assert np.isclose(location.y,
expected.y), 'Returned y value is not the same '
'as expected'
assert np.isclose(location.z,
expected.z), 'Returned z value is not the same '
'as expected'
def test_imu_setup_failed_initialization():
# Set up the required parameters for the initialization.
name = 'imu_sensor'
transform = Transform(Location(), Rotation())
with pytest.raises(AssertionError):
imu_setup = IMUSetup(name=1, transform=transform)
with pytest.raises(AssertionError):
imu_setup = IMUSetup(name=name, transform=None)
def on_watermark(self, timestamp, waypoints_stream):
self._logger.debug('@{}: received watermark'.format(timestamp))
# get ego info
can_bus_msg = self._can_bus_msgs.popleft()
vehicle_transform = can_bus_msg.data.transform
# get obstacles
obstacle_map = self._build_obstacle_map(vehicle_transform)
# compute goals
target_location = self._compute_target_location(vehicle_transform)
# run rrt*
path, cost = self._run_rrt_star(vehicle_transform, target_location,
obstacle_map)
# convert to waypoints if path found, else use default waypoints
if cost is not None:
path_transforms = []
for point in path:
p_loc = self._carla_map.get_waypoint(
carla.Location(x=point[0], y=point[1], z=0),
project_to_road=True,
).transform.location # project to road to approximate Z
path_transforms.append(
Transform(
location=Location(x=point[0], y=point[1], z=p_loc.z),
rotation=Rotation(),
))
waypoints = deque(path_transforms)
waypoints.extend(
itertools.islice(self._waypoints, self._wp_index,
len(self._waypoints))
) # add the remaining global route for future
else:
waypoints = self._waypoints
# construct waypoints message
waypoints = collections.deque(
itertools.islice(waypoints, 0,
DEFAULT_NUM_WAYPOINTS)) # only take 50 meters
next_waypoint = waypoints[self._wp_index]
wp_steer_speed_vector, wp_steer_speed_angle = \
get_waypoint_vector_and_angle(
next_waypoint, vehicle_transform
)
output_msg = WaypointsMessage(timestamp,
waypoints=waypoints,
wp_angle=wp_steer_speed_angle,
wp_vector=wp_steer_speed_vector,
wp_angle_speed=wp_steer_speed_angle)
# send waypoints message
waypoints_stream.send(output_msg)
waypoints_stream.send(erdos.WatermarkMessage(timestamp))
def create_depth_camera_setup(camera_name_prefix,
camera_location,
width,
height,
fov=90):
transform = Transform(camera_location, Rotation())
return DepthCameraSetup(camera_name_prefix + '_depth',
width,
height,
transform,
fov=fov)
def create_segmented_camera_setup(camera_name_prefix,
camera_location,
width,
height,
fov=90):
transform = Transform(camera_location, Rotation())
return SegmentedCameraSetup(camera_name_prefix + '_segmented',
width,
height,
transform,
fov=fov)
def test_camera_setup_failed_initialization():
"""
Ensure that the CameraSetup constructor fails when wrong values or values
of the wrong type are provided.
"""
# Set up the correct values for all the constructor values.
name, camera_type = 'camera_setup', 'sensor.camera.rgb'
width, height = 1920, 1080
transform = Transform(Location(), Rotation())
# Ensure that a wrong name throws an error.
with pytest.raises(AssertionError):
camera_setup = CameraSetup(name=1,
camera_type=camera_type,
width=width,
height=height,
transform=transform)
# Ensure that a wrong camera_type throws an error.
with pytest.raises(AssertionError):
camera_setup = CameraSetup(name=name,
camera_type=None,
width=width,
height=height,
transform=transform)
# Ensure that a wrong width, height throws an error.
with pytest.raises(AssertionError):
camera_setup = CameraSetup(name=name,
camera_type=camera_type,
width=float(width),
height=float(height),
transform=transform)
# Ensure that a wrong transform throws an error.
with pytest.raises(AssertionError):
camera_setup = CameraSetup(name=name,
camera_type=camera_type,
width=width,
height=height,
transform=None)
# Ensure that a bad fov raises an error.
with pytest.raises(AssertionError):
camera_setup = CameraSetup(name=name,
camera_type=camera_type,
width=width,
height=height,
transform=transform,
fov=None)
def test_vector_to_camera_view(location, expected):
from pylot.drivers.sensor_setup import CameraSetup
camera_setup = CameraSetup('test_camera',
'sensor.camera.rgb',
width=1920,
height=1080,
transform=Transform(Location(), Rotation()))
location = Location(*location)
assert all(
np.isclose(
location.to_camera_view(
camera_setup.get_extrinsic_matrix(),
camera_setup.get_intrinsic_matrix()).as_numpy_array(),
expected)), "The camera transformation was not as expected."
def create_center_lidar_setup(location):
rotation = Rotation()
# Place the lidar in the same position as the camera.
lidar_transform = Transform(location, rotation)
return LidarSetup(
name='front_center_lidar',
lidar_type='sensor.lidar.ray_cast',
transform=lidar_transform,
range=5000, # in centimers
rotation_frequency=20,
channels=32,
upper_fov=15,
lower_fov=-30,
points_per_second=500000)
def build_output_waypoints(self, path_x, path_y, speeds):
wps = deque()
target_speeds = deque()
for point in zip(path_x, path_y, speeds):
if self._map is not None:
p_loc = self._map.get_closest_lane_waypoint(
Location(x=point[0], y=point[1], z=0)).location
else:
p_loc = Location(x=point[0], y=point[1], z=0)
wps.append(
Transform(
location=Location(x=point[0], y=point[1], z=p_loc.z),
rotation=Rotation(),
))
target_speeds.append(point[2])
return Waypoints(wps, target_speeds)
def get_closest_lane_waypoint(self, location):
if self.is_on_lane(location):
return Transform(location, Rotation())
closest_transform = None
min_dist = np.infty
for transform in self.left_markings:
dist = transform.location.distance(location)
if dist < min_dist:
min_dist = dist
closest_transform = transform
for transform in self.right_markings:
dist = transform.location.distance(location)
if dist < min_dist:
min_dist = dist
closest_transform = transform
return closest_transform
def generate_predicted_trajectories(self, msg, linear_prediction_stream):
self._logger.debug('@{}: received trajectories message'.format(
msg.timestamp))
obstacle_predictions_list = []
for obstacle in msg.obstacle_trajectories:
# Time step matrices used in regression.
num_steps = min(self._flags.prediction_num_past_steps,
len(obstacle.trajectory))
ts = np.zeros((num_steps, 2))
future_ts = np.zeros((self._flags.prediction_num_future_steps, 2))
for t in range(num_steps):
ts[t][0] = -t
ts[t][1] = 1
for i in range(self._flags.prediction_num_future_steps):
future_ts[i][0] = i + 1
future_ts[i][1] = 1
xy = np.zeros((num_steps, 2))
for t in range(num_steps):
# t-th most recent step
transform = obstacle.trajectory[-(t + 1)]
xy[t][0] = transform.location.x
xy[t][1] = transform.location.y
linear_model_params = np.linalg.lstsq(ts, xy)[0]
# Predict future steps and convert to list of locations.
predict_array = np.matmul(future_ts, linear_model_params)
predictions = []
for t in range(self._flags.prediction_num_future_steps):
predictions.append(
Transform(location=Location(x=predict_array[t][0],
y=predict_array[t][1]),
rotation=Rotation()))
# Get the current transform of the obstacle, which is the last
# trajectory value.
cur_transform = obstacle.trajectory[-1]
obstacle_predictions_list.append(
ObstaclePrediction(
obstacle.label,
obstacle.id,
cur_transform,
obstacle.bounding_box,
1.0, # probability
predictions))
linear_prediction_stream.send(
PredictionMessage(msg.timestamp, obstacle_predictions_list))
def test_get_pixel_locations(depth_frame, pixels, expected):
height, width = depth_frame.shape
camera_setup = CameraSetup('test_setup',
'test_type',
width,
height,
Transform(location=Location(0, 0, 0),
rotation=Rotation(0, 0, 0)),
fov=90)
depth_frame = DepthFrame(depth_frame, camera_setup)
locations = depth_frame.get_pixel_locations(pixels)
for i in range(len(pixels)):
assert np.isclose(locations[i].x, expected[i].x), 'Returned x '
'value is not the same as expected'
assert np.isclose(locations[i].y, expected[i].y), 'Returned y '
'value is not the same as expected'
assert np.isclose(locations[i].z, expected[i].z), 'Returned z '
'value is not the same as expected'
def _construct_waypoints(self, timestamp, pose_msg, path_x, path_y, speeds, success):
"""
Convert the rrt* path into a waypoints message.
"""
path_transforms = []
target_speeds = deque()
if not success:
self._logger.error("@{}: RRT* failed. "
"Sending emergency stop.".format(timestamp))
x = pose_msg.data.transform.location.x
y = pose_msg.data.transform.location.y
current_index = 0
min_dist = np.infty
for i, wp in enumerate(self._prev_waypoints):
dist = np.linalg.norm([wp.location.x - x, wp.location.y - y])
if dist <= min_dist:
current_index = i
min_dist = dist
for wp in itertools.islice(
self._prev_waypoints, current_index,
current_index + self._flags.num_waypoints_ahead):
path_transforms.append(wp)
target_speeds.append(0)
waypoints = deque(path_transforms)
else:
self._logger.debug("@{}: RRT* succeeded.".format(timestamp))
for point in zip(path_x, path_y, speeds):
if self._map is not None:
p_loc = self._map.get_closest_lane_waypoint(
Location(x=point[0], y=point[1], z=0)).location
else:
# RRT* does not take into account the driveable region it
# constructs search space as a top down, minimum bounding
# rectangle with padding in each dimension.
p_loc = Location(x=point[0], y=point[1], z=0)
path_transforms.append(
Transform(
location=Location(x=point[0], y=point[1], z=p_loc.z),
rotation=Rotation(),
))
target_speeds.append(point[2])
waypoints = deque(path_transforms)
self._prev_waypoints = waypoints
return WaypointsMessage(timestamp, waypoints, target_speeds)
def test_depth_to_point_cloud(depth_frame, expected):
height, width = depth_frame.shape
camera_setup = CameraSetup('test_setup',
'test_type',
width,
height,
Transform(location=Location(0, 0, 0),
rotation=Rotation(0, 0, 0)),
fov=90)
depth_frame = DepthFrame(depth_frame, camera_setup)
# Resulting unreal coordinates.
point_cloud = depth_frame.as_point_cloud()
for i in range(width * height):
assert np.isclose(point_cloud[i].x, expected[i].x), 'Returned x '
'value is not the same as expected'
assert np.isclose(point_cloud[i].y, expected[i].y), 'Returned y '
'value is not the same as expected'
assert np.isclose(point_cloud[i].z, expected[i].z), 'Returned z '
'value is not the same as expected'
def create_left_right_camera_setups(camera_name_prefix,
location,
width,
height,
camera_offset,
fov=90):
""" Creates a dual-RGB-camera setup with the center at the given location,
and the two cameras on either side of the center at a distance specified
by the camera_offset.
The Rotation is set to (pitch=0, yaw=0, roll=0).
Args:
camera_name_prefix (str): The name of the camera instance. A suffix
of "_left" and "_right" is appended to the name.
location (:py:class:`~pylot.utils.Location`): The location of the
center of the cameras with respect to the center of the vehicle.
width (int): The width of the image returned by the cameras.
height (int): The height of the image returned by the cameras.
camera_offset (float): The offset of the two cameras from the center.
fov (float): The field of view of the image returned by the cameras.
Returns:
tuple: A tuple containing two instances of
:py:class:`~pylot.drivers.sensor_setup.RGBCameraSetup` for the left
and right camera setups with the given parameters.
"""
rotation = Rotation()
left_loc = location + Location(0, -camera_offset, 0)
right_loc = location + Location(0, camera_offset, 0)
left_transform = Transform(left_loc, rotation)
right_transform = Transform(right_loc, rotation)
left_camera_setup = RGBCameraSetup(camera_name_prefix + '_left',
width,
height,
left_transform,
fov=fov)
right_camera_setup = RGBCameraSetup(camera_name_prefix + '_right',
width,
height,
right_transform,
fov=fov)
return (left_camera_setup, right_camera_setup)
def on_pose_update(self, data):
self._counter += 1
if self._counter % self._modulo_to_send != 0:
return
loc = Location(data.pose.position.x, data.pose.position.y,
data.pose.position.z)
quaternion = [
data.pose.orientation.x, data.pose.orientation.y,
data.pose.orientation.z, data.pose.orientation.w
]
roll, pitch, yaw = euler_from_quaternion(quaternion)
rotation = Rotation(np.degrees(pitch), np.degrees(yaw),
np.degrees(roll))
timestamp = erdos.Timestamp(coordinates=[self._msg_cnt])
pose = Pose(Transform(loc, rotation), self._forward_speed)
self._logger.debug('NDT localization {}'.format(pose))
self._pose_stream.send(erdos.Message(timestamp, pose))
self._pose_stream.send(erdos.WatermarkMessage(timestamp))
self._msg_cnt += 1
