Update camera settings
This commit is contained in:
Даниил Грабарь
@@ -17,7 +17,9 @@ rotation: "0 1 0 1.5708"
|
||||
camera:
|
||||
width: 640
|
||||
height: 480
|
||||
fieldOfView: 1.047 # ~60° в радианах
|
||||
# VGA RGB: приближение центрального 4:3 crop сенсора 1280x800 с HFOV 90°.
|
||||
# 2*atan(tan(90°/2)*(4/3)/(1280/800)) = 79.61°; уточнить по калибровке.
|
||||
fieldOfView: 1.3894765524
|
||||
near: 0.1
|
||||
far: 0.0 # 0 = без ограничений
|
||||
exposure: 1.0
|
||||
@@ -40,13 +42,13 @@ camera:
|
||||
range_finder:
|
||||
width: 640
|
||||
height: 480
|
||||
fieldOfView: 1.047
|
||||
minRange: 0.1
|
||||
maxRange: 5.0
|
||||
resolution: -1.0 # -1 = без ограничений по дискретизации
|
||||
fieldOfView: 1.308996939 # D455 VGA depth: HFOV 75°, не HD 87°
|
||||
minRange: 0.4 # Выбранное ограничение симуляции, не точный Min-Z VGA
|
||||
maxRange: 6.0
|
||||
resolution: 0.001 # Шаг глубины 1 мм, не погрешность измерения
|
||||
noise: 0.0
|
||||
|
||||
# Параметры публикации в ROS2
|
||||
ros:
|
||||
topic: /d455_top/depth/image_raw
|
||||
update_rate: 15
|
||||
update_rate: 30
|
||||
|
||||
@@ -17,6 +17,7 @@
|
||||
<xacro:include filename="$(find iiwa_description)/urdf/links.xacro"/>
|
||||
<xacro:include filename="$(find iiwa_description)/urdf/joints.xacro"/>
|
||||
|
||||
<xacro:property name="tool_has_camera" value="false"/>
|
||||
<xacro:include filename="$(find iiwa_description)/urdf/tools/tool_active.xacro"/>
|
||||
|
||||
<xacro:if value="$(arg simulate)">
|
||||
@@ -26,6 +27,7 @@
|
||||
|
||||
<device reference="camera_rgb" type="Camera">
|
||||
<ros>
|
||||
<enabled>false</enabled>
|
||||
<topicName>/camera/color</topicName>
|
||||
<updateRate>30</updateRate>
|
||||
<alwaysOn>True</alwaysOn>
|
||||
@@ -35,6 +37,7 @@
|
||||
|
||||
<device reference="camera_depth" type="RangeFinder">
|
||||
<ros>
|
||||
<enabled>false</enabled>
|
||||
<topicName>/camera/depth</topicName>
|
||||
<updateRate>30</updateRate>
|
||||
<alwaysOn>True</alwaysOn>
|
||||
@@ -42,6 +45,23 @@
|
||||
</ros>
|
||||
</device>
|
||||
|
||||
<!-- Keep the 30 Hz deadlines independent of Webots' 32 ms steps. -->
|
||||
<xacro:if value="${tool_has_camera}">
|
||||
<plugin type="iiwa_utils.webots_camera.WebotsCamera">
|
||||
<device>camera_rgb</device>
|
||||
<topicName>/camera/color</topicName>
|
||||
<updateRate>30</updateRate>
|
||||
<frameName>camera_optical_frame</frameName>
|
||||
</plugin>
|
||||
<plugin type="iiwa_utils.webots_camera.WebotsCamera">
|
||||
<device>camera_depth</device>
|
||||
<depth>true</depth>
|
||||
<topicName>/camera/depth</topicName>
|
||||
<updateRate>30</updateRate>
|
||||
<frameName>camera_optical_frame</frameName>
|
||||
</plugin>
|
||||
</xacro:if>
|
||||
|
||||
</webots>
|
||||
|
||||
<ros2_control name="iiwaGazeboControl" type="system">
|
||||
|
||||
@@ -1,6 +1,8 @@
|
||||
<?xml version="1.0"?>
|
||||
<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="tool_patron">
|
||||
|
||||
<xacro:property name="tool_has_camera" value="true"/>
|
||||
|
||||
<link name="camera_holder">
|
||||
<inertial>
|
||||
<origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>
|
||||
@@ -134,7 +136,8 @@
|
||||
<gazebo reference="camera_optical_frame">
|
||||
<sensor type="camera" name="camera_rgb">
|
||||
<camera>
|
||||
<horizontal_fov>1.3089969</horizontal_fov>
|
||||
<!-- VGA RGB central-crop approximation: 79.61 deg; calibrate per device. -->
|
||||
<horizontal_fov>1.3894765524</horizontal_fov>
|
||||
<image>
|
||||
<width>640</width>
|
||||
<height>480</height>
|
||||
@@ -146,7 +149,7 @@
|
||||
</clip>
|
||||
<noise>
|
||||
<type>gaussian</type>
|
||||
<stddev>0.005</stddev>
|
||||
<stddev>0.0</stddev>
|
||||
</noise>
|
||||
</camera>
|
||||
</sensor>
|
||||
@@ -159,18 +162,20 @@
|
||||
<height>480</height>
|
||||
</image>
|
||||
<clip>
|
||||
<near>0.52</near>
|
||||
<near>0.01</near>
|
||||
<far>6.0</far>
|
||||
</clip>
|
||||
</camera>
|
||||
<range>
|
||||
<min>0.52</min>
|
||||
<!-- Conservative simulation cutoff, not calibrated VGA Min-Z. -->
|
||||
<min>0.4</min>
|
||||
<max>6.0</max>
|
||||
<resolution>0.001</resolution>
|
||||
</range>
|
||||
<noise>
|
||||
<type>gaussian</type>
|
||||
<stddev>0.02</stddev>
|
||||
<!-- No arbitrary Gaussian noise: this is an ideal depth model. -->
|
||||
<stddev>0.0</stddev>
|
||||
</noise>
|
||||
</sensor>
|
||||
</gazebo>
|
||||
|
||||
@@ -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