645 lines
24 KiB
Python
645 lines
24 KiB
Python
"""ROS 2 image publisher for Hikrobot MVS USB3 Vision cameras."""
|
|
|
|
from __future__ import annotations
|
|
|
|
from ctypes import byref, cast, memset, POINTER, sizeof, string_at
|
|
from dataclasses import dataclass
|
|
import importlib
|
|
import os
|
|
from pathlib import Path
|
|
import sys
|
|
from threading import Event, Lock, Thread
|
|
from typing import Any, Sequence
|
|
from urllib.parse import unquote, urlparse
|
|
|
|
import rclpy
|
|
from rclpy.node import Node
|
|
from rclpy.qos import qos_profile_sensor_data
|
|
from sensor_msgs.msg import CameraInfo, Image
|
|
from sensor_msgs.srv import SetCameraInfo
|
|
import yaml
|
|
|
|
|
|
DEFAULT_MVS_PYTHON_PATH = "/opt/MVS/Samples/64/Python/MvImport"
|
|
DEFAULT_MVS_RUNTIME_PATH = "/opt/MVS/lib"
|
|
|
|
|
|
@dataclass(frozen=True)
|
|
class DeviceDescriptor:
|
|
"""Stable identity fields for one enumerated USB3 Vision camera."""
|
|
|
|
index: int
|
|
model: str
|
|
serial: str
|
|
guid: str
|
|
|
|
|
|
@dataclass(frozen=True)
|
|
class CameraCalibration:
|
|
"""Validated subset of a standard ROS camera calibration YAML file."""
|
|
|
|
camera_name: str
|
|
width: int
|
|
height: int
|
|
distortion_model: str
|
|
d: tuple[float, ...]
|
|
k: tuple[float, ...]
|
|
r: tuple[float, ...]
|
|
p: tuple[float, ...]
|
|
|
|
|
|
def decode_c_string(value: Any) -> str:
|
|
"""Decode a null-terminated MVS SDK character array."""
|
|
raw = bytes(value).split(b"\0", 1)[0]
|
|
for encoding in ("utf-8", "gbk", "latin-1"):
|
|
try:
|
|
return raw.decode(encoding)
|
|
except UnicodeDecodeError:
|
|
continue
|
|
return raw.decode("latin-1", errors="replace")
|
|
|
|
|
|
def select_device(
|
|
devices: Sequence[DeviceDescriptor],
|
|
selector: str,
|
|
expected_model: str,
|
|
) -> DeviceDescriptor:
|
|
"""Select exactly one camera by serial or GUID and check its model."""
|
|
if not devices:
|
|
raise RuntimeError("MVS did not enumerate any USB3 Vision cameras")
|
|
selector = selector.strip()
|
|
if selector:
|
|
matches = [
|
|
device
|
|
for device in devices
|
|
if selector in {device.serial, device.guid}
|
|
]
|
|
if not matches:
|
|
available = ", ".join(
|
|
f"{device.model}:{device.serial}:{device.guid}"
|
|
for device in devices
|
|
)
|
|
raise RuntimeError(
|
|
f"camera selector {selector!r} was not found; "
|
|
f"available cameras: {available}"
|
|
)
|
|
elif len(devices) == 1:
|
|
matches = [devices[0]]
|
|
else:
|
|
available = ", ".join(device.serial for device in devices)
|
|
raise RuntimeError(
|
|
"camera selector is required when multiple cameras are present; "
|
|
f"available serials: {available}"
|
|
)
|
|
if len(matches) != 1:
|
|
raise RuntimeError(f"camera selector {selector!r} is not unique")
|
|
selected = matches[0]
|
|
if expected_model and expected_model not in selected.model:
|
|
raise RuntimeError(
|
|
f"camera {selected.serial} is {selected.model!r}, expected a model "
|
|
f"containing {expected_model!r}"
|
|
)
|
|
return selected
|
|
|
|
|
|
def resolve_camera_info_path(url_or_path: str) -> Path | None:
|
|
"""Resolve a plain path or file:// URL used for CameraInfo persistence."""
|
|
value = url_or_path.strip()
|
|
if not value:
|
|
return None
|
|
parsed = urlparse(value)
|
|
if parsed.scheme not in {"", "file"}:
|
|
raise ValueError(
|
|
"camera_info_url must be a filesystem path or file:// URL"
|
|
)
|
|
if parsed.scheme == "file":
|
|
if parsed.netloc not in {"", "localhost"}:
|
|
raise ValueError("remote file:// camera_info_url is not supported")
|
|
value = unquote(parsed.path)
|
|
return Path(value).expanduser().resolve()
|
|
|
|
|
|
def _matrix_data(mapping: dict[str, Any], name: str, length: int) -> tuple[float, ...]:
|
|
section = mapping.get(name)
|
|
if not isinstance(section, dict):
|
|
raise ValueError(f"camera calibration is missing {name}")
|
|
data = section.get("data")
|
|
if not isinstance(data, list) or len(data) != length:
|
|
raise ValueError(f"{name}.data must contain exactly {length} values")
|
|
result = tuple(float(value) for value in data)
|
|
if not all(value == value and abs(value) != float("inf") for value in result):
|
|
raise ValueError(f"{name}.data contains a non-finite value")
|
|
return result
|
|
|
|
|
|
def load_camera_calibration(path: Path) -> CameraCalibration:
|
|
"""Load and validate a standard ROS camera calibration YAML file."""
|
|
mapping = yaml.safe_load(path.read_text(encoding="utf-8"))
|
|
if not isinstance(mapping, dict):
|
|
raise ValueError("camera calibration YAML root must be a mapping")
|
|
width = int(mapping.get("image_width", 0))
|
|
height = int(mapping.get("image_height", 0))
|
|
if width <= 0 or height <= 0:
|
|
raise ValueError("camera calibration image dimensions must be positive")
|
|
distortion = mapping.get("distortion_coefficients")
|
|
if not isinstance(distortion, dict) or not isinstance(
|
|
distortion.get("data"), list
|
|
):
|
|
raise ValueError(
|
|
"camera calibration is missing distortion_coefficients.data"
|
|
)
|
|
d = tuple(float(value) for value in distortion["data"])
|
|
calibration = CameraCalibration(
|
|
camera_name=str(mapping.get("camera_name", "hikrobot_camera")),
|
|
width=width,
|
|
height=height,
|
|
distortion_model=str(mapping.get("distortion_model", "plumb_bob")),
|
|
d=d,
|
|
k=_matrix_data(mapping, "camera_matrix", 9),
|
|
r=_matrix_data(mapping, "rectification_matrix", 9),
|
|
p=_matrix_data(mapping, "projection_matrix", 12),
|
|
)
|
|
if calibration.k[0] <= 0.0 or calibration.k[4] <= 0.0:
|
|
raise ValueError("camera calibration focal lengths must be positive")
|
|
if calibration.p[0] <= 0.0 or calibration.p[5] <= 0.0:
|
|
raise ValueError("camera calibration projection focal lengths must be positive")
|
|
return calibration
|
|
|
|
|
|
def calibration_to_mapping(
|
|
camera_name: str, camera_info: CameraInfo
|
|
) -> dict[str, Any]:
|
|
"""Convert CameraInfo into the standard ROS YAML representation."""
|
|
return {
|
|
"image_width": int(camera_info.width),
|
|
"image_height": int(camera_info.height),
|
|
"camera_name": camera_name,
|
|
"camera_matrix": {
|
|
"rows": 3,
|
|
"cols": 3,
|
|
"data": [float(value) for value in camera_info.k],
|
|
},
|
|
"distortion_model": camera_info.distortion_model,
|
|
"distortion_coefficients": {
|
|
"rows": 1,
|
|
"cols": len(camera_info.d),
|
|
"data": [float(value) for value in camera_info.d],
|
|
},
|
|
"rectification_matrix": {
|
|
"rows": 3,
|
|
"cols": 3,
|
|
"data": [float(value) for value in camera_info.r],
|
|
},
|
|
"projection_matrix": {
|
|
"rows": 3,
|
|
"cols": 4,
|
|
"data": [float(value) for value in camera_info.p],
|
|
},
|
|
}
|
|
|
|
|
|
def configure_fastdds_large_image_transport() -> Path:
|
|
"""Select the package's large-image SHM profile before creating ROS nodes."""
|
|
from ament_index_python.packages import get_package_share_directory
|
|
|
|
profile = (
|
|
Path(get_package_share_directory("linkerhand_calibration"))
|
|
/ "config"
|
|
/ "fastdds_large_images.xml"
|
|
)
|
|
if not profile.is_file():
|
|
raise RuntimeError(f"Fast DDS large-image profile not found: {profile}")
|
|
os.environ.setdefault("FASTRTPS_DEFAULT_PROFILES_FILE", str(profile))
|
|
return profile
|
|
|
|
|
|
class HikrobotCameraNode(Node):
|
|
"""Publish synchronized Mono8 Image and CameraInfo messages from MVS."""
|
|
|
|
def __init__(self) -> None:
|
|
super().__init__("hikrobot_camera")
|
|
self._declare_parameters()
|
|
self._stop_event = Event()
|
|
self._camera_info_lock = Lock()
|
|
self._grab_thread: Thread | None = None
|
|
self._camera: Any | None = None
|
|
self._mvs: Any | None = None
|
|
self._sdk_initialized = False
|
|
self._handle_created = False
|
|
self._camera_opened = False
|
|
self._grabbing = False
|
|
self._closed = False
|
|
self._frame_count = 0
|
|
self._timeout_count = 0
|
|
|
|
self.selector = str(self.get_parameter("serial_number").value)
|
|
self.expected_model = str(self.get_parameter("expected_model").value)
|
|
self.frame_id = str(self.get_parameter("frame_id").value)
|
|
self.camera_name = str(self.get_parameter("camera_name").value)
|
|
self.image_width = int(self.get_parameter("image_width").value)
|
|
self.image_height = int(self.get_parameter("image_height").value)
|
|
if self.image_width <= 0 or self.image_height <= 0:
|
|
raise ValueError("image_width and image_height must be positive")
|
|
if float(self.get_parameter("frame_rate").value) <= 0.0:
|
|
raise ValueError("frame_rate must be positive")
|
|
if int(self.get_parameter("grab_timeout_ms").value) <= 0:
|
|
raise ValueError("grab_timeout_ms must be positive")
|
|
self.camera_info_path = resolve_camera_info_path(
|
|
str(self.get_parameter("camera_info_url").value)
|
|
)
|
|
self._calibration = self._load_calibration()
|
|
|
|
self.image_publisher = self.create_publisher(
|
|
Image, "image_raw", qos_profile_sensor_data
|
|
)
|
|
self.camera_info_publisher = self.create_publisher(
|
|
CameraInfo, "camera_info", qos_profile_sensor_data
|
|
)
|
|
self.create_service(
|
|
SetCameraInfo, "set_camera_info", self._set_camera_info_callback
|
|
)
|
|
|
|
try:
|
|
self._open_camera()
|
|
self._grab_thread = Thread(
|
|
target=self._grab_loop,
|
|
name="hikrobot_mvs_grab",
|
|
daemon=True,
|
|
)
|
|
self._grab_thread.start()
|
|
except Exception:
|
|
self.close()
|
|
raise
|
|
|
|
def _declare_parameters(self) -> None:
|
|
self.declare_parameter("serial_number", "")
|
|
self.declare_parameter("expected_model", "MV-CS020-10UM")
|
|
self.declare_parameter("camera_name", "hikrobot_front")
|
|
self.declare_parameter("frame_id", "camera_color_optical_frame")
|
|
self.declare_parameter("image_width", 1624)
|
|
self.declare_parameter("image_height", 1240)
|
|
self.declare_parameter("frame_rate", 30.0)
|
|
self.declare_parameter("exposure_time_us", 5000.0)
|
|
self.declare_parameter("gain_db", 0.0)
|
|
self.declare_parameter("auto_exposure", False)
|
|
self.declare_parameter("camera_info_url", "")
|
|
self.declare_parameter("mvs_python_path", DEFAULT_MVS_PYTHON_PATH)
|
|
self.declare_parameter("mvs_runtime_path", DEFAULT_MVS_RUNTIME_PATH)
|
|
self.declare_parameter("grab_timeout_ms", 1000)
|
|
|
|
def _load_calibration(self) -> CameraCalibration | None:
|
|
if self.camera_info_path is None or not self.camera_info_path.exists():
|
|
target = self.camera_info_path or "an unset camera_info_url"
|
|
self.get_logger().warning(
|
|
f"No camera calibration found at {target}; publishing "
|
|
"uncalibrated CameraInfo. Trajectory calibration will remain "
|
|
"locked until valid intrinsics are installed."
|
|
)
|
|
return None
|
|
calibration = load_camera_calibration(self.camera_info_path)
|
|
self._validate_calibration_dimensions(calibration)
|
|
self.get_logger().info(
|
|
f"Loaded camera intrinsics from {self.camera_info_path}"
|
|
)
|
|
return calibration
|
|
|
|
def _validate_calibration_dimensions(
|
|
self, calibration: CameraCalibration
|
|
) -> None:
|
|
if (
|
|
calibration.width != self.image_width
|
|
or calibration.height != self.image_height
|
|
):
|
|
raise ValueError(
|
|
"camera calibration dimensions "
|
|
f"{calibration.width}x{calibration.height} do not match "
|
|
f"configured stream {self.image_width}x{self.image_height}"
|
|
)
|
|
|
|
def _import_mvs(self) -> Any:
|
|
python_path = Path(
|
|
str(self.get_parameter("mvs_python_path").value)
|
|
).expanduser()
|
|
runtime_path = Path(
|
|
str(self.get_parameter("mvs_runtime_path").value)
|
|
).expanduser()
|
|
if not python_path.is_dir():
|
|
raise RuntimeError(f"MVS Python bindings not found: {python_path}")
|
|
if not runtime_path.is_dir():
|
|
raise RuntimeError(f"MVS runtime not found: {runtime_path}")
|
|
os.environ.setdefault("MVCAM_COMMON_RUNENV", str(runtime_path))
|
|
if str(python_path) not in sys.path:
|
|
sys.path.insert(0, str(python_path))
|
|
return importlib.import_module("MvCameraControl_class")
|
|
|
|
def _enumerate_usb_cameras(
|
|
self, mvs: Any
|
|
) -> tuple[Any, list[DeviceDescriptor]]:
|
|
device_list = mvs.MV_CC_DEVICE_INFO_LIST()
|
|
result = mvs.MvCamera.MV_CC_EnumDevices(
|
|
mvs.MV_USB_DEVICE, device_list
|
|
)
|
|
self._require_ok("enumerate USB3 Vision devices", result)
|
|
descriptors: list[DeviceDescriptor] = []
|
|
for index in range(device_list.nDeviceNum):
|
|
device_info = cast(
|
|
device_list.pDeviceInfo[index], POINTER(mvs.MV_CC_DEVICE_INFO)
|
|
).contents
|
|
usb_info = device_info.SpecialInfo.stUsb3VInfo
|
|
descriptors.append(
|
|
DeviceDescriptor(
|
|
index=index,
|
|
model=decode_c_string(usb_info.chModelName),
|
|
serial=decode_c_string(usb_info.chSerialNumber),
|
|
guid=decode_c_string(usb_info.chDeviceGUID),
|
|
)
|
|
)
|
|
return device_list, descriptors
|
|
|
|
def _open_camera(self) -> None:
|
|
mvs = self._import_mvs()
|
|
self._mvs = mvs
|
|
self._require_ok("initialize MVS SDK", mvs.MvCamera.MV_CC_Initialize())
|
|
self._sdk_initialized = True
|
|
device_list, descriptors = self._enumerate_usb_cameras(mvs)
|
|
selected = select_device(descriptors, self.selector, self.expected_model)
|
|
device_info = cast(
|
|
device_list.pDeviceInfo[selected.index],
|
|
POINTER(mvs.MV_CC_DEVICE_INFO),
|
|
).contents
|
|
|
|
camera = mvs.MvCamera()
|
|
self._require_ok("create camera handle", camera.MV_CC_CreateHandle(device_info))
|
|
self._camera = camera
|
|
self._handle_created = True
|
|
self._require_ok(
|
|
"open camera",
|
|
camera.MV_CC_OpenDevice(mvs.MV_ACCESS_Exclusive, 0),
|
|
)
|
|
self._camera_opened = True
|
|
|
|
self._set_required_enum("AcquisitionMode", "Continuous")
|
|
self._set_required_enum("TriggerMode", "Off")
|
|
self._set_required_enum("PixelFormat", "Mono8")
|
|
self._set_required_integer("OffsetX", 0)
|
|
self._set_required_integer("OffsetY", 0)
|
|
self._set_required_integer("Width", self.image_width)
|
|
self._set_required_integer("Height", self.image_height)
|
|
self._set_optional_bool("AcquisitionFrameRateEnable", True)
|
|
self._set_optional_float(
|
|
"AcquisitionFrameRate",
|
|
float(self.get_parameter("frame_rate").value),
|
|
)
|
|
if bool(self.get_parameter("auto_exposure").value):
|
|
self._set_required_enum("ExposureAuto", "Continuous")
|
|
else:
|
|
self._set_required_enum("ExposureAuto", "Off")
|
|
self._set_optional_float(
|
|
"ExposureTime",
|
|
float(self.get_parameter("exposure_time_us").value),
|
|
)
|
|
self._set_required_enum("GainAuto", "Off")
|
|
self._set_optional_float(
|
|
"Gain", float(self.get_parameter("gain_db").value)
|
|
)
|
|
|
|
self._require_ok(
|
|
"set latest-frame grab strategy",
|
|
camera.MV_CC_SetGrabStrategy(mvs.MV_GrabStrategy_LatestImagesOnly),
|
|
)
|
|
self._require_ok("start image acquisition", camera.MV_CC_StartGrabbing())
|
|
self._grabbing = True
|
|
self.get_logger().info(
|
|
"Opened Hikrobot camera "
|
|
f"model={selected.model} serial={selected.serial} "
|
|
f"guid={selected.guid} stream={self.image_width}x{self.image_height} "
|
|
"encoding=mono8"
|
|
)
|
|
|
|
def _set_required_enum(self, name: str, value: str) -> None:
|
|
assert self._camera is not None
|
|
self._require_ok(
|
|
f"set {name}={value}",
|
|
self._camera.MV_CC_SetEnumValueByString(name, value),
|
|
)
|
|
|
|
def _set_required_integer(self, name: str, value: int) -> None:
|
|
assert self._camera is not None
|
|
self._require_ok(
|
|
f"set {name}={value}",
|
|
self._camera.MV_CC_SetIntValueEx(name, value),
|
|
)
|
|
|
|
def _set_optional_bool(self, name: str, value: bool) -> None:
|
|
assert self._camera is not None
|
|
result = self._camera.MV_CC_SetBoolValue(name, value)
|
|
if result != 0:
|
|
self.get_logger().warning(
|
|
f"MVS could not set {name}={value}: 0x{result:08x}"
|
|
)
|
|
|
|
def _set_optional_float(self, name: str, value: float) -> None:
|
|
assert self._camera is not None
|
|
result = self._camera.MV_CC_SetFloatValue(name, value)
|
|
if result != 0:
|
|
self.get_logger().warning(
|
|
f"MVS could not set {name}={value}: 0x{result:08x}"
|
|
)
|
|
|
|
@staticmethod
|
|
def _require_ok(operation: str, result: int) -> None:
|
|
if result != 0:
|
|
hint = ""
|
|
if result == 0x80000203:
|
|
hint = (
|
|
"; access denied: stop acquisition and close the MVS "
|
|
"viewer or any other process using this camera"
|
|
)
|
|
raise RuntimeError(
|
|
f"MVS failed to {operation}: 0x{result:08x}{hint}"
|
|
)
|
|
|
|
def _grab_loop(self) -> None:
|
|
assert self._camera is not None
|
|
assert self._mvs is not None
|
|
timeout_ms = int(self.get_parameter("grab_timeout_ms").value)
|
|
while not self._stop_event.is_set():
|
|
frame = self._mvs.MV_FRAME_OUT()
|
|
memset(byref(frame), 0, sizeof(frame))
|
|
result = self._camera.MV_CC_GetImageBuffer(frame, timeout_ms)
|
|
if result != 0:
|
|
self._timeout_count += 1
|
|
if self._timeout_count == 1 or self._timeout_count % 10 == 0:
|
|
self.get_logger().warning(
|
|
"MVS image acquisition timed out or failed: "
|
|
f"0x{result:08x}; consecutive={self._timeout_count}"
|
|
)
|
|
continue
|
|
try:
|
|
self._timeout_count = 0
|
|
self._publish_frame(frame)
|
|
except Exception as error:
|
|
self.get_logger().error(f"Failed to publish MVS frame: {error}")
|
|
finally:
|
|
free_result = self._camera.MV_CC_FreeImageBuffer(frame)
|
|
if free_result != 0:
|
|
self.get_logger().error(
|
|
f"MVS failed to release frame: 0x{free_result:08x}"
|
|
)
|
|
|
|
def _publish_frame(self, frame: Any) -> None:
|
|
assert self._mvs is not None
|
|
info = frame.stFrameInfo
|
|
width = int(info.nWidth)
|
|
height = int(info.nHeight)
|
|
if width != self.image_width or height != self.image_height:
|
|
raise RuntimeError(
|
|
f"received {width}x{height}, expected "
|
|
f"{self.image_width}x{self.image_height}"
|
|
)
|
|
if int(info.enPixelType) != int(self._mvs.PixelType_Gvsp_Mono8):
|
|
raise RuntimeError(
|
|
f"received pixel type 0x{int(info.enPixelType):x}, expected Mono8"
|
|
)
|
|
expected_size = width * height
|
|
if int(info.nFrameLen) < expected_size:
|
|
raise RuntimeError(
|
|
f"frame contains {info.nFrameLen} bytes, expected {expected_size}"
|
|
)
|
|
stamp = self.get_clock().now().to_msg()
|
|
image = Image()
|
|
image.header.stamp = stamp
|
|
image.header.frame_id = self.frame_id
|
|
image.height = height
|
|
image.width = width
|
|
image.encoding = "mono8"
|
|
image.is_bigendian = 0
|
|
image.step = width
|
|
image.data = string_at(frame.pBufAddr, expected_size)
|
|
|
|
camera_info = self._make_camera_info(width, height)
|
|
camera_info.header.stamp = stamp
|
|
camera_info.header.frame_id = self.frame_id
|
|
self.image_publisher.publish(image)
|
|
self.camera_info_publisher.publish(camera_info)
|
|
self._frame_count += 1
|
|
|
|
def _make_camera_info(self, width: int, height: int) -> CameraInfo:
|
|
message = CameraInfo()
|
|
message.width = width
|
|
message.height = height
|
|
with self._camera_info_lock:
|
|
calibration = self._calibration
|
|
if calibration is None:
|
|
message.distortion_model = "plumb_bob"
|
|
message.d = []
|
|
message.k = [0.0] * 9
|
|
message.r = [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0]
|
|
message.p = [0.0] * 12
|
|
return message
|
|
message.distortion_model = calibration.distortion_model
|
|
message.d = list(calibration.d)
|
|
message.k = list(calibration.k)
|
|
message.r = list(calibration.r)
|
|
message.p = list(calibration.p)
|
|
return message
|
|
|
|
def _set_camera_info_callback(
|
|
self,
|
|
request: SetCameraInfo.Request,
|
|
response: SetCameraInfo.Response,
|
|
) -> SetCameraInfo.Response:
|
|
try:
|
|
if self.camera_info_path is None:
|
|
raise ValueError(
|
|
"camera_info_url is empty; set it before saving calibration"
|
|
)
|
|
message = request.camera_info
|
|
if (
|
|
int(message.width) != self.image_width
|
|
or int(message.height) != self.image_height
|
|
):
|
|
raise ValueError(
|
|
f"calibration is {message.width}x{message.height}, expected "
|
|
f"{self.image_width}x{self.image_height}"
|
|
)
|
|
mapping = calibration_to_mapping(self.camera_name, message)
|
|
self.camera_info_path.parent.mkdir(parents=True, exist_ok=True)
|
|
temporary_path = self.camera_info_path.with_suffix(
|
|
self.camera_info_path.suffix + ".tmp"
|
|
)
|
|
temporary_path.write_text(
|
|
yaml.safe_dump(mapping, sort_keys=False), encoding="utf-8"
|
|
)
|
|
temporary_path.replace(self.camera_info_path)
|
|
calibration = load_camera_calibration(self.camera_info_path)
|
|
self._validate_calibration_dimensions(calibration)
|
|
with self._camera_info_lock:
|
|
self._calibration = calibration
|
|
response.success = True
|
|
response.status_message = (
|
|
f"saved camera calibration to {self.camera_info_path}"
|
|
)
|
|
self.get_logger().info(response.status_message)
|
|
except Exception as error:
|
|
response.success = False
|
|
response.status_message = str(error)
|
|
self.get_logger().error(
|
|
f"Rejected camera calibration: {response.status_message}"
|
|
)
|
|
return response
|
|
|
|
def close(self) -> None:
|
|
if self._closed:
|
|
return
|
|
self._closed = True
|
|
self._stop_event.set()
|
|
if self._grab_thread is not None:
|
|
self._grab_thread.join(timeout=2.0)
|
|
if self._camera is not None and self._camera_opened:
|
|
if self._grabbing:
|
|
result = self._camera.MV_CC_StopGrabbing()
|
|
if result != 0:
|
|
self.get_logger().warning(
|
|
f"MVS failed to stop grabbing: 0x{result:08x}"
|
|
)
|
|
self._grabbing = False
|
|
result = self._camera.MV_CC_CloseDevice()
|
|
if result != 0:
|
|
self.get_logger().warning(
|
|
f"MVS failed to close camera: 0x{result:08x}"
|
|
)
|
|
self._camera.MV_CC_DestroyHandle()
|
|
elif self._camera is not None and self._handle_created:
|
|
self._camera.MV_CC_DestroyHandle()
|
|
self._camera = None
|
|
self._handle_created = False
|
|
self._camera_opened = False
|
|
if self._sdk_initialized and self._mvs is not None:
|
|
self._mvs.MvCamera.MV_CC_Finalize()
|
|
self._sdk_initialized = False
|
|
|
|
def destroy_node(self) -> None:
|
|
self.close()
|
|
super().destroy_node()
|
|
|
|
|
|
def main(args: list[str] | None = None) -> None:
|
|
configure_fastdds_large_image_transport()
|
|
rclpy.init(args=args)
|
|
node: HikrobotCameraNode | None = None
|
|
try:
|
|
node = HikrobotCameraNode()
|
|
rclpy.spin(node)
|
|
except KeyboardInterrupt:
|
|
pass
|
|
finally:
|
|
if node is not None:
|
|
node.destroy_node()
|
|
if rclpy.ok():
|
|
rclpy.shutdown()
|
|
|
|
|
|
if __name__ == "__main__":
|
|
main()
|