Merge pull request #115 from lbr-stack/feature/roboreg-110/remove_ros_registry

remove ROS package registry from CLI 110

Author: mhubii

README.md

@@ -128,15 +128,20 @@ Next:
     ```
 
 ## Command Line Interface
-> [!NOTE]
-> In these examples, the [lbr_fri_ros2_stack](https://github.com/lbr-stack/lbr_fri_ros2_stack/) is used. Make sure to follow [Quick Start](https://github.com/lbr-stack/lbr_fri_ros2_stack/#quick-start) first. However, you can also use your own robot description files.
+> [!TIP]
+> Examples use sample data under [test/assets/lbr_med7_r800](test/assets/lbr_med7_r800). Data is stored via Git Large File Storage (LFS). Data is cloned automatically when `git-lfs` is installed. To clone in retrospect:
+> ```shell
+> sudo apt install git-lfs
+> git lfs fetch --all
+> git lfs checkout
+> ```
 
 ### Segment
 This is a required step to generate robot masks.
 
 ```shell
 rr-sam2 \
-    --path test/assets/lbr_med7/zed2i \
+    --path test/assets/lbr_med7_r800/samples \
     --pattern "left_image_*.png" \
     --n-positive-samples 5 \
     --n-negative-samples 5 \
@@ -148,13 +153,12 @@ The Hydra robust ICP implements a point-to-plane ICP registration on a Lie algeb
 
 ```shell
 rr-hydra \
-    --camera-info-file test/assets/lbr_med7/zed2i/left_camera_info.yaml \
-    --path test/assets/lbr_med7/zed2i \
+    --camera-info-file test/assets/lbr_med7_r800/samples/left_camera_info.yaml \
+    --path test/assets/lbr_med7_r800/samples \
     --mask-pattern mask_sam2_left_image_*.png \
     --depth-pattern depth_*.npy \
     --joint-states-pattern joint_states_*.npy \
-    --ros-package lbr_description \
-    --xacro-path urdf/med7/med7.xacro \
+    --urdf-path test/assets/lbr_med7_r800/description/lbr_med7_r800.urdf \
     --root-link-name lbr_link_0 \
     --end-link-name lbr_link_7 \
     --number-of-points 5000 \
@@ -181,15 +185,14 @@ rr-cam-swarm \
     --c2 1.5 \
     --max-iterations 100 \
     --display-progress \
-    --ros-package lbr_description \
-    --xacro-path urdf/med7/med7.xacro \
+    --urdf-path test/assets/lbr_med7_r800/description/lbr_med7_r800.urdf \
     --root-link-name lbr_link_0 \
     --end-link-name lbr_link_7 \
     --target-reduction 0.8 \
     --scale 0.1 \
     --n-samples 1 \
-    --camera-info-file test/assets/lbr_med7/zed2i/left_camera_info.yaml \
-    --path test/assets/lbr_med7/zed2i \
+    --camera-info-file test/assets/lbr_med7_r800/samples/left_camera_info.yaml \
+    --path test/assets/lbr_med7_r800/samples \
     --image-pattern left_image_*.png \
     --joint-states-pattern joint_states_*.npy \
     --mask-pattern mask_sam2_left_image_*.png \
@@ -210,13 +213,12 @@ rr-mono-dr \
     --lr 0.01 \
     --max-iterations 100 \
     --display-progress \
-    --ros-package lbr_description \
-    --xacro-path urdf/med7/med7.xacro \
+    --urdf-path test/assets/lbr_med7_r800/description/lbr_med7_r800.urdf \
     --root-link-name lbr_link_0 \
     --end-link-name lbr_link_7 \
-    --camera-info-file test/assets/lbr_med7/zed2i/left_camera_info.yaml \
-    --extrinsics-file test/assets/lbr_med7/zed2i/HT_hydra_robust.npy \
-    --path test/assets/lbr_med7/zed2i \
+    --camera-info-file test/assets/lbr_med7_r800/samples/left_camera_info.yaml \
+    --extrinsics-file test/assets/lbr_med7_r800/samples/HT_hydra_robust.npy \
+    --path test/assets/lbr_med7_r800/samples \
     --image-pattern left_image_*.png \
     --joint-states-pattern joint_states_*.npy \
     --mask-pattern mask_sam2_left_image_*.png \
@@ -237,15 +239,14 @@ rr-stereo-dr \
     --lr 0.01 \
     --max-iterations 100 \
     --display-progress \
-    --ros-package lbr_description \
-    --xacro-path urdf/med7/med7.xacro \
+    --urdf-path test/assets/lbr_med7_r800/description/lbr_med7_r800.urdf \
     --root-link-name lbr_link_0 \
     --end-link-name lbr_link_7 \
-    --left-camera-info-file test/assets/lbr_med7/zed2i/left_camera_info.yaml \
-    --right-camera-info-file test/assets/lbr_med7/zed2i/right_camera_info.yaml \
-    --left-extrinsics-file test/assets/lbr_med7/zed2i/HT_hydra_robust.npy \
-    --right-extrinsics-file test/assets/lbr_med7/zed2i/HT_right_to_left.npy \
-    --path test/assets/lbr_med7/zed2i \
+    --left-camera-info-file test/assets/lbr_med7_r800/samples/left_camera_info.yaml \
+    --right-camera-info-file test/assets/lbr_med7_r800/samples/right_camera_info.yaml \
+    --left-extrinsics-file test/assets/lbr_med7_r800/samples/HT_hydra_robust.npy \
+    --right-extrinsics-file test/assets/lbr_med7_r800/samples/HT_right_to_left.npy \
+    --path test/assets/lbr_med7_r800/samples \
     --left-image-pattern left_image_*.png \
     --right-image-pattern right_image_*.png \
     --joint-states-pattern joint_states_*.npy \
@@ -267,17 +268,16 @@ Generate renders using the obtained extrinsics:
 rr-render \
     --batch-size 1 \
     --num-workers 0 \
-    --ros-package lbr_description \
-    --xacro-path urdf/med7/med7.xacro \
+    --urdf-path test/assets/lbr_med7_r800/description/lbr_med7_r800.urdf \
     --root-link-name lbr_link_0 \
     --end-link-name lbr_link_7 \
-    --camera-info-file test/assets/lbr_med7/zed2i/left_camera_info.yaml \
-    --extrinsics-file test/assets/lbr_med7/zed2i/HT_left_dr.npy \
-    --images-path test/assets/lbr_med7/zed2i \
-    --joint-states-path test/assets/lbr_med7/zed2i \
+    --camera-info-file test/assets/lbr_med7_r800/samples/left_camera_info.yaml \
+    --extrinsics-file test/assets/lbr_med7_r800/samples/HT_left_dr.npy \
+    --images-path test/assets/lbr_med7_r800/samples \
+    --joint-states-path test/assets/lbr_med7_r800/samples \
     --image-pattern left_image_*.png \
     --joint-states-pattern joint_states_*.npy \
-    --output-path test/assets/lbr_med7/zed2i
+    --output-path /tmp/renders/lbr_med7_r800
 ```
 
 ## Testing
@@ -288,13 +288,12 @@ To run Hydra robust ICP on provided `xarm` and `realsense` data, run
 
 ```shell
 rr-hydra \
-    --camera-info-file test/assets/xarm/realsense/camera_info.yaml \
-    --path test/assets/xarm/realsense \
+    --camera-info-file test/assets/xarm_7/samples/camera_info.yaml \
+    --path test/assets/xarm_7/samples \
     --mask-pattern mask_*.png \
     --depth-pattern depth_*.npy \
     --joint-states-pattern joint_state_*.npy \
-    --ros-package xarm_description \
-    --xacro-path  urdf/xarm_device.urdf.xacro \
+    --urdf-path test/assets/xarm_7/description/xarm_7.urdf \
     --root-link-name link_base \
     --end-link-name link7 \
     --number-of-points 5000 \
@@ -308,17 +307,16 @@ Generate renders using the obtained extrinsics:
 rr-render \
     --batch-size 1 \
     --num-workers 0 \
-    --ros-package xarm_description \
-    --xacro-path urdf/xarm_device.urdf.xacro \
+    --urdf-path test/assets/xarm_7/description/xarm_7.urdf \
     --root-link-name link_base \
     --end-link-name link7 \
-    --camera-info-file test/assets/xarm/realsense/camera_info.yaml \
-    --extrinsics-file test/assets/xarm/realsense/HT_hydra_robust.npy \
-    --images-path test/assets/xarm/realsense \
-    --joint-states-path test/assets/xarm/realsense \
+    --camera-info-file test/assets/xarm_7/samples/camera_info.yaml \
+    --extrinsics-file test/assets/xarm_7/samples/HT_hydra_robust.npy \
+    --images-path test/assets/xarm_7/samples \
+    --joint-states-path test/assets/xarm_7/samples \
     --image-pattern img_*.png \
     --joint-states-pattern joint_state_*.npy \
-    --output-path test/assets/xarm/realsense
+    --output-path /tmp/renders/xarm_7
 ```
 
 ## Acknowledgements

cli/rr_cam_swarm.py

@@ -6,8 +6,21 @@
 import numpy as np
 import torch
 
-from roboreg import differentiable as rrd
-from roboreg.io import URDFParser, find_files, parse_camera_info, parse_mono_data
+from roboreg.core import (
+    NVDiffRastRenderer,
+    Robot,
+    RobotScene,
+    TorchKinematics,
+    TorchMeshContainer,
+    VirtualCamera,
+)
+from roboreg.io import (
+    find_files,
+    load_robot_data_from_ros_xacro,
+    load_robot_data_from_urdf_file,
+    parse_camera_info,
+    parse_mono_data,
+)
 from roboreg.losses import soft_dice_loss
 from roboreg.optim import LinearParticleSwarm, ParticleSwarmOptimizer
 from roboreg.util import (
@@ -17,6 +30,8 @@
     random_fov_eye_space_coordinates,
 )
 
+from .util.validate import validate_urdf_source
+
 
 def args_factory() -> argparse.Namespace:
     parser = argparse.ArgumentParser(
@@ -84,17 +99,26 @@ def args_factory() -> argparse.Namespace:
         action="store_true",
         help="Display optimization progress.",
     )
+    parser.add_argument(
+        "--urdf-path",
+        type=str,
+        default="test/assets/lbr_med7_r800/description/lbr_med7_r800.urdf",
+        help="Path to URDF file. Meshes resolved relative to this file. "
+        "Mutually exclusive with --ros-package/--xacro-path.",
+    )
     parser.add_argument(
         "--ros-package",
         type=str,
-        default="lbr_description",
-        help="Package where the URDF is located.",
+        default=None,
+        help="ROS package containing robot description. "
+        "Requires --xacro-path. Mutually exclusive with --urdf-path.",
     )
     parser.add_argument(
         "--xacro-path",
         type=str,
-        default="urdf/med7/med7.xacro",
-        help="Path to the xacro file, relative to --ros-package.",
+        default=None,
+        help="Path to xacro file relative to --ros-package. "
+        "Requires --ros-package. Mutually exclusive with --urdf-path.",
     )
     parser.add_argument(
         "--root-link-name",
@@ -168,6 +192,7 @@ def args_factory() -> argparse.Namespace:
         default=2,
         help="Number of concurrent compilation jobs for nvdiffrast. Only relevant on first run.",
     )
+    validate_urdf_source(parser, parser.parse_args())
     return parser.parse_args()
 
 
@@ -295,31 +320,52 @@ def main() -> None:
         n_joint_states * args.n_cameras
     )  # (each camera observes n_joint_states joint states)
     camera_name = "camera"
-    camera = rrd.VirtualCamera(
+    camera = VirtualCamera(
         resolution=(height, width),
         intrinsics=intrinsics,
         extrinsics=torch.eye(4, device=device).unsqueeze(0).expand(batch_size, -1, -1),
         device=device,
     )
 
-    urdf_parser = URDFParser.from_ros_xacro(
-        ros_package=args.ros_package, xacro_path=args.xacro_path
-    )
-    robot = rrd.Robot.from_urdf_parser(
-        urdf_parser=urdf_parser,
-        root_link_name=args.root_link_name,
-        end_link_name=args.end_link_name,
-        collision=args.collision_meshes,
+    # instantiate robot
+    if args.urdf_path is not None:
+        robot_data = load_robot_data_from_urdf_file(
+            urdf_path=args.urdf_path,
+            root_link_name=args.root_link_name,
+            end_link_name=args.end_link_name,
+            collision=args.collision_meshes,
+            target_reduction=args.target_reduction,
+        )
+    else:
+        robot_data = load_robot_data_from_ros_xacro(
+            ros_package=args.ros_package,
+            xacro_path=args.xacro_path,
+            root_link_name=args.root_link_name,
+            end_link_name=args.end_link_name,
+            collision=args.collision_meshes,
+            target_reduction=args.target_reduction,
+        )
+    mesh_container = TorchMeshContainer(
+        meshes=robot_data.meshes,
         batch_size=batch_size,
         device=device,
-        target_reduction=args.target_reduction,  # reduce mesh vertex count for memory reduction
+    )
+    kinematics = TorchKinematics(
+        urdf=robot_data.urdf,
+        root_link_name=robot_data.root_link_name,
+        end_link_name=robot_data.end_link_name,
+        device=device,
+    )
+    robot = Robot(
+        mesh_container=mesh_container,
+        kinematics=kinematics,
     )
 
-    renderer = rrd.NVDiffRastRenderer(device=device)
-    scene = rrd.RobotScene(
+    # instantiate scene
+    scene = RobotScene(
         cameras={camera_name: camera},
         robot=robot,
-        renderer=renderer,
+        renderer=NVDiffRastRenderer(device=device),
     )
 
     # repeat joint states and masks for each camera