[pre-commit.ci] auto fixes from pre-commit.com hooks
for more information, see https://pre-commit.ci
This commit is contained in:
parent
130c16b43d
commit
d978053ebf
|
@ -113,6 +113,7 @@ class IntelRealSenseCameraConfig(CameraConfig):
|
|||
if self.rotation not in [-90, None, 90, 180]:
|
||||
raise ValueError(f"`rotation` must be in [-90, None, 90, 180] (got {self.rotation})")
|
||||
|
||||
|
||||
@CameraConfig.register_subclass("ros2")
|
||||
@dataclass
|
||||
class ROS2CameraConfig(CameraConfig):
|
||||
|
@ -123,6 +124,7 @@ class ROS2CameraConfig(CameraConfig):
|
|||
ROS2CameraConfig(topic="/image_raw", fps=30, width=640, height=480)
|
||||
```
|
||||
"""
|
||||
|
||||
# TODO(Yadunund): Consider converting inputs to lists to subscribe to multiple
|
||||
# topics with the same node.
|
||||
topic: str
|
||||
|
|
|
@ -8,26 +8,23 @@ import shutil
|
|||
import threading
|
||||
import time
|
||||
from pathlib import Path
|
||||
import time
|
||||
from threading import Thread
|
||||
|
||||
# TODO(Yadunund): Implement mock.
|
||||
import cv2
|
||||
import numpy as np
|
||||
import rclpy
|
||||
from cv_bridge import CvBridge
|
||||
from PIL import Image
|
||||
from sensor_msgs.msg import Image as ImageMsg
|
||||
|
||||
from lerobot.common.robot_devices.cameras.configs import ROS2CameraConfig
|
||||
from lerobot.common.robot_devices.utils import (
|
||||
RobotDeviceAlreadyConnectedError,
|
||||
RobotDeviceNotConnectedError,
|
||||
busy_wait,
|
||||
)
|
||||
from lerobot.common.utils.utils import capture_timestamp_utc
|
||||
|
||||
# TODO(Yadunund): Implement mock.
|
||||
import cv2
|
||||
from cv_bridge import CvBridge
|
||||
from sensor_msgs.msg import Image as ImageMsg
|
||||
import rclpy
|
||||
|
||||
|
||||
def save_image(img_array, camera_name, frame_index, images_dir):
|
||||
img = Image.fromarray(img_array)
|
||||
|
@ -48,9 +45,7 @@ def save_images_from_cameras(
|
|||
camera = ROS2Camera(config)
|
||||
camera.connect()
|
||||
print("Here 0")
|
||||
print(
|
||||
f"ROS2Camera({camera.node.get_name()}"
|
||||
)
|
||||
print(f"ROS2Camera({camera.node.get_name()}")
|
||||
cameras.append(camera)
|
||||
|
||||
images_dir = Path(images_dir)
|
||||
|
@ -167,12 +162,7 @@ class ROS2Camera:
|
|||
if self.is_connected:
|
||||
raise RobotDeviceAlreadyConnectedError(f"ROS2Camera({self.config.topic}) is already connected.")
|
||||
|
||||
self.sub = self.node.create_subscription(
|
||||
ImageMsg,
|
||||
self.config.topic,
|
||||
self.sub_cb,
|
||||
10
|
||||
)
|
||||
self.sub = self.node.create_subscription(ImageMsg, self.config.topic, self.sub_cb, 10)
|
||||
|
||||
while self.image_msg is None:
|
||||
print(f"Waiting to receive message over {self.config.topic}")
|
||||
|
@ -198,7 +188,6 @@ class ROS2Camera:
|
|||
f"ROS2Camera({self.node.get_name()}) is not connected. Try running `camera.connect()` first."
|
||||
)
|
||||
|
||||
|
||||
start_time = time.perf_counter()
|
||||
|
||||
desired_encoding = "passthrough" if temporary_color is None else temporary_color
|
||||
|
@ -219,7 +208,6 @@ class ROS2Camera:
|
|||
|
||||
return image
|
||||
|
||||
|
||||
def async_read(self):
|
||||
return self.read()
|
||||
|
||||
|
|
|
@ -22,7 +22,6 @@ from lerobot.common.robot_devices.cameras.configs import (
|
|||
CameraConfig,
|
||||
IntelRealSenseCameraConfig,
|
||||
OpenCVCameraConfig,
|
||||
ROS2CameraConfig,
|
||||
)
|
||||
from lerobot.common.robot_devices.motors.configs import (
|
||||
DynamixelMotorsBusConfig,
|
||||
|
|
Loading…
Reference in New Issue