Update camera settings
This commit is contained in:
Даниил Грабарь
@@ -178,12 +178,19 @@ def build_ros_urdf(cfg: CameraConfig) -> str:
|
||||
devices.append(
|
||||
f' <device reference="{cfg.name}" type="Camera">\n'
|
||||
f' <ros>\n'
|
||||
f' <enabled>false</enabled>\n'
|
||||
f' <topicName>{ros.topic}</topicName>\n'
|
||||
f' <updateRate>{ros.update_rate}</updateRate>\n'
|
||||
f' <alwaysOn>True</alwaysOn>\n'
|
||||
f' <frameName>{cfg.name}_link</frameName>\n'
|
||||
f' </ros>\n'
|
||||
f' </device>'
|
||||
f' </device>\n'
|
||||
f' <plugin type="iiwa_utils.webots_camera.WebotsCamera">\n'
|
||||
f' <device>{cfg.name}</device>\n'
|
||||
f' <topicName>{ros.topic}</topicName>\n'
|
||||
f' <updateRate>{ros.update_rate}</updateRate>\n'
|
||||
f' <frameName>{cfg.name}_link</frameName>\n'
|
||||
f' </plugin>'
|
||||
)
|
||||
|
||||
if cfg.range_finder is not None:
|
||||
@@ -191,12 +198,20 @@ def build_ros_urdf(cfg: CameraConfig) -> str:
|
||||
devices.append(
|
||||
f' <device reference="{cfg.name}_depth" type="RangeFinder">\n'
|
||||
f' <ros>\n'
|
||||
f' <enabled>false</enabled>\n'
|
||||
f' <topicName>{ros.topic}</topicName>\n'
|
||||
f' <updateRate>{ros.update_rate}</updateRate>\n'
|
||||
f' <alwaysOn>True</alwaysOn>\n'
|
||||
f' <frameName>{cfg.name}_link</frameName>\n'
|
||||
f' </ros>\n'
|
||||
f' </device>'
|
||||
f' </device>\n'
|
||||
f' <plugin type="iiwa_utils.webots_camera.WebotsCamera">\n'
|
||||
f' <device>{cfg.name}_depth</device>\n'
|
||||
f' <depth>true</depth>\n'
|
||||
f' <topicName>{ros.topic}</topicName>\n'
|
||||
f' <updateRate>{ros.update_rate}</updateRate>\n'
|
||||
f' <frameName>{cfg.name}_link</frameName>\n'
|
||||
f' </plugin>'
|
||||
)
|
||||
|
||||
devices_str = "\n".join(devices)
|
||||
|
||||
@@ -0,0 +1,114 @@
|
||||
"""Publish Webots camera frames without accumulating timestep rounding drift."""
|
||||
import array
|
||||
import math
|
||||
import sys
|
||||
|
||||
import numpy as np
|
||||
import rclpy
|
||||
from rclpy.qos import QoSProfile, ReliabilityPolicy
|
||||
from builtin_interfaces.msg import Time
|
||||
from sensor_msgs.msg import CameraInfo, Image, PointCloud2, PointField
|
||||
|
||||
|
||||
class FrameSchedule:
|
||||
"""Keep deadlines on the requested cadence, with at most one frame per step."""
|
||||
|
||||
def __init__(self, rate, now):
|
||||
if not math.isfinite(rate) or rate <= 0:
|
||||
raise ValueError('Camera update rate must be finite and positive')
|
||||
self.period = 1.0 / rate
|
||||
self.last_time = now
|
||||
self.next_time = now + self.period
|
||||
|
||||
def due(self, now):
|
||||
if now < self.last_time:
|
||||
self.next_time = now + self.period
|
||||
self.last_time = now
|
||||
if now + 1e-9 < self.next_time:
|
||||
return False
|
||||
elapsed = max(0, math.floor((now - self.next_time + 1e-9) / self.period))
|
||||
self.next_time += (elapsed + 1) * self.period
|
||||
return True
|
||||
|
||||
|
||||
class WebotsCamera:
|
||||
"""One RGB or depth stream; preserves the stock driver's topic suffixes."""
|
||||
|
||||
def init(self, webots_node, properties):
|
||||
self._robot = webots_node.robot
|
||||
name = properties['device']
|
||||
self._device = self._robot.getDevice(name)
|
||||
if self._device is None:
|
||||
raise ValueError(f'Camera device not found: {name}')
|
||||
self._depth = properties.get('depth', 'false') == 'true'
|
||||
rate = float(properties.get('updateRate', '30'))
|
||||
self._schedule = FrameSchedule(rate, self._robot.getTime())
|
||||
timestep = int(self._robot.getBasicTimeStep())
|
||||
if timestep <= 0 or rate * timestep > 1000:
|
||||
raise ValueError('Camera rate exceeds the Webots simulation step rate')
|
||||
self._device.enable(timestep)
|
||||
if not rclpy.ok():
|
||||
rclpy.init(args=None)
|
||||
self._node = rclpy.create_node(f'{name}_publisher')
|
||||
qos = QoSProfile(depth=5, reliability=ReliabilityPolicy.RELIABLE)
|
||||
topic = properties['topicName']
|
||||
suffix = '/image' if self._depth else '/image_color'
|
||||
self._publisher = self._node.create_publisher(Image, topic + suffix, qos)
|
||||
self._info_publisher = self._node.create_publisher(CameraInfo, topic + '/camera_info', qos)
|
||||
self._info = CameraInfo()
|
||||
self._info.header.frame_id = properties['frameName']
|
||||
self._info.width = self._device.getWidth()
|
||||
self._info.height = self._device.getHeight()
|
||||
focal = self._info.width / (2 * math.tan(self._device.getFov() / 2))
|
||||
cx, cy = self._info.width / 2, self._info.height / 2
|
||||
self._info.distortion_model = 'plumb_bob'
|
||||
self._info.d = [0.0] * 5
|
||||
self._info.k = [focal, 0.0, cx, 0.0, focal, cy, 0.0, 0.0, 1.0]
|
||||
self._info.r = [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0]
|
||||
self._info.p = [focal, 0.0, cx, 0.0, 0.0, focal, cy, 0.0, 0.0, 0.0, 1.0, 0.0]
|
||||
if self._depth:
|
||||
self._cloud_publisher = self._node.create_publisher(
|
||||
PointCloud2, topic + '/point_cloud', qos)
|
||||
u, v = np.meshgrid(np.arange(self._info.width), np.arange(self._info.height))
|
||||
self._rays = np.stack(((u - cx) / focal, (v - cy) / focal,
|
||||
np.ones_like(u)), axis=-1).astype(np.float32)
|
||||
|
||||
def step(self):
|
||||
now = self._robot.getTime()
|
||||
if not self._schedule.due(now):
|
||||
return
|
||||
if self._depth:
|
||||
data = array.array('f', self._device.getRangeImage()).tobytes()
|
||||
else:
|
||||
data = self._device.getImage()
|
||||
if data is None:
|
||||
return
|
||||
nanoseconds = round(now * 1_000_000_000)
|
||||
self._info.header.stamp = Time(
|
||||
sec=nanoseconds // 1_000_000_000, nanosec=nanoseconds % 1_000_000_000)
|
||||
message = Image()
|
||||
message.header = self._info.header
|
||||
message.width, message.height = self._info.width, self._info.height
|
||||
message.encoding = '32FC1' if self._depth else 'bgra8'
|
||||
message.is_bigendian = int(sys.byteorder == 'big') if self._depth else 0
|
||||
message.step = message.width * 4
|
||||
message.data = data
|
||||
self._publisher.publish(message)
|
||||
self._info_publisher.publish(self._info)
|
||||
if self._depth and self._cloud_publisher.get_subscription_count():
|
||||
depth = np.frombuffer(data, dtype=np.float32).reshape(message.height, message.width)
|
||||
# Optical coordinates: x right, y down, z forward, all in metres.
|
||||
valid = np.isfinite(depth) & (depth > 0)
|
||||
points = self._rays * np.where(valid, depth, 0.0)[..., None]
|
||||
points[~valid] = np.nan
|
||||
cloud = PointCloud2()
|
||||
cloud.header = message.header
|
||||
cloud.width, cloud.height = message.width, message.height
|
||||
cloud.fields = [PointField(name=name, offset=4 * i,
|
||||
datatype=PointField.FLOAT32, count=1)
|
||||
for i, name in enumerate(('x', 'y', 'z'))]
|
||||
cloud.is_bigendian = bool(message.is_bigendian)
|
||||
cloud.point_step, cloud.row_step = 12, message.width * 12
|
||||
cloud.is_dense = bool(valid.all())
|
||||
cloud.data = points.tobytes()
|
||||
self._cloud_publisher.publish(cloud)
|
||||
@@ -11,6 +11,9 @@
|
||||
<depend>xacro</depend>
|
||||
<depend>ament_index_python</depend>
|
||||
<depend>webots_ros2_msgs</depend>
|
||||
<exec_depend>webots_ros2_driver</exec_depend>
|
||||
<exec_depend>sensor_msgs</exec_depend>
|
||||
<exec_depend>builtin_interfaces</exec_depend>
|
||||
<depend>python3-yaml</depend>
|
||||
<depend>iiwa_msgs</depend>
|
||||
<depend>python3-numpy</depend>
|
||||
|
||||
@@ -0,0 +1,33 @@
|
||||
"""Camera cadence must not halve when 30 Hz is sampled on 32 ms steps."""
|
||||
import pytest
|
||||
|
||||
from iiwa_utils.webots_camera import FrameSchedule
|
||||
|
||||
|
||||
def test_30_hz_on_webots_32_ms_steps():
|
||||
schedule = FrameSchedule(30, 0.0)
|
||||
stamps = [tick * 0.032 for tick in range(1, 3126)
|
||||
if schedule.due(tick * 0.032)]
|
||||
assert len(stamps) == 3000
|
||||
assert all(b > a for a, b in zip(stamps, stamps[1:]))
|
||||
assert max(b - a for a, b in zip(stamps, stamps[1:])) <= 0.064001
|
||||
|
||||
|
||||
def test_delayed_step_does_not_publish_catchup_burst():
|
||||
schedule = FrameSchedule(30, 0.0)
|
||||
assert schedule.due(1.0)
|
||||
assert not schedule.due(1.0)
|
||||
assert not schedule.due(1.001)
|
||||
assert schedule.due(1.034)
|
||||
|
||||
|
||||
def test_time_reset_restarts_schedule():
|
||||
schedule = FrameSchedule(30, 10.0)
|
||||
assert not schedule.due(0.0)
|
||||
assert schedule.due(0.034)
|
||||
|
||||
|
||||
@pytest.mark.parametrize('rate', [0, -1, float('nan'), float('inf')])
|
||||
def test_invalid_rates_rejected(rate):
|
||||
with pytest.raises(ValueError):
|
||||
FrameSchedule(rate, 0.0)
|
||||
@@ -0,0 +1,66 @@
|
||||
"""Check published pixels, units and calibration with a small device adapter."""
|
||||
import array
|
||||
import copy
|
||||
import math
|
||||
from types import SimpleNamespace
|
||||
|
||||
import pytest
|
||||
import numpy as np
|
||||
|
||||
from iiwa_utils import webots_camera as camera
|
||||
|
||||
|
||||
@pytest.mark.parametrize('depth', [False, True])
|
||||
def test_messages_match_device_and_share_timestamp(monkeypatch, depth):
|
||||
messages = {}
|
||||
now = [0.0]
|
||||
device = SimpleNamespace(
|
||||
enable=lambda period: None,
|
||||
getWidth=lambda: 2, getHeight=lambda: 1,
|
||||
getFov=lambda: math.pi / 2,
|
||||
getImage=lambda: bytes([1, 2, 3, 255, 4, 5, 6, 255]),
|
||||
getRangeImage=lambda: [1.25, float('inf')],
|
||||
)
|
||||
robot = SimpleNamespace(getDevice=lambda name: device,
|
||||
getBasicTimeStep=lambda: 32, getTime=lambda: now[0])
|
||||
|
||||
def publisher(message_type, topic, qos):
|
||||
messages[topic] = []
|
||||
return SimpleNamespace(publish=lambda msg: messages[topic].append(copy.deepcopy(msg)),
|
||||
get_subscription_count=lambda: 1)
|
||||
|
||||
monkeypatch.setattr(camera.rclpy, 'ok', lambda: True)
|
||||
monkeypatch.setattr(camera.rclpy, 'create_node',
|
||||
lambda name: SimpleNamespace(create_publisher=publisher))
|
||||
plugin = camera.WebotsCamera()
|
||||
plugin.init(SimpleNamespace(robot=robot), dict(
|
||||
device='test_camera', depth=str(depth).lower(), updateRate='30',
|
||||
topicName='/test', frameName='test_optical_frame'))
|
||||
for tick in range(1, 101):
|
||||
now[0] = tick * 0.032
|
||||
plugin.step()
|
||||
images = messages['/test/image' if depth else '/test/image_color']
|
||||
infos = messages['/test/camera_info']
|
||||
assert len(images) == len(infos) == 96
|
||||
image, info = images[-1], infos[-1]
|
||||
assert image.header == info.header
|
||||
assert image.header.frame_id == 'test_optical_frame'
|
||||
assert (image.width, image.height, image.step) == (2, 1, 8)
|
||||
assert image.header.stamp.sec == 3
|
||||
assert image.header.stamp.nanosec == 200_000_000
|
||||
assert info.k[0] == pytest.approx(1.0)
|
||||
assert info.k[2] == 1.0
|
||||
assert info.k[5] == 0.5
|
||||
if depth:
|
||||
assert image.encoding == '32FC1'
|
||||
values = array.array('f', image.data.tobytes())
|
||||
assert values[0] == 1.25
|
||||
assert math.isinf(values[1])
|
||||
cloud = messages['/test/point_cloud'][-1]
|
||||
assert cloud.header == image.header
|
||||
xyz = np.frombuffer(cloud.data, dtype=np.float32).reshape(-1, 3)
|
||||
np.testing.assert_allclose(xyz[0], [-1.25, -0.625, 1.25])
|
||||
assert np.isnan(xyz[1]).all()
|
||||
else:
|
||||
assert image.encoding == 'bgra8'
|
||||
assert image.data.tobytes() == device.getImage()
|
||||
Reference in New Issue
Block a user