Feature/object detector with scene objects mamager

.pre-commit-config.yaml

@@ -35,7 +35,8 @@ repos:
         args: [-w]
 
 -   repo: https://github.com/astral-sh/ruff-pre-commit
-    rev: v0.15.5
+
+    rev: v0.15.8
     hooks:
     -   id: ruff-check
         args: [--fix, --exit-non-zero-on-fix]
@@ -48,7 +49,7 @@ repos:
     -   id: cmake-lint
 
 -   repo: https://github.com/pre-commit/mirrors-clang-format
-    rev: v22.1.0
+    rev: v22.1.2
     hooks:
     -   id: clang-format
         types_or: [c++, proto]
@@ -62,7 +63,7 @@ repos:
     -   id: sort-package-xml
 
 -   repo: https://github.com/astral-sh/uv-pre-commit
-    rev: 0.10.9
+    rev: 0.11.2
     hooks:
     -   id: uv-lock
 

aegis_control/CHANGELOG.md

@@ -24,6 +24,7 @@ and this project adheres to [Semantic Versioning](https://semver.org/spec/v2.0.0
 
 ### Changed
 
+* [PR-115](https://github.com/AGH-CEAI/aegis_ros/pull/115) - Setup scene camera setup.
 * [PR-114](https://github.com/AGH-CEAI/aegis_ros/pull/114) - Updated docs before release.
 * [PR-112](https://github.com/AGH-CEAI/aegis_ros/pull/112) - Cropped scene camera image output.
 * [PR-108](https://github.com/AGH-CEAI/aegis_ros/pull/108) - Reduced DepthAI pipeline for scene camera to RGB-only.

aegis_control/config/cameras/depthai_cameras.yaml

@@ -3,7 +3,7 @@
   ros__parameters:
     camera:
       i_mx_id: 184430108157970F00
-      i_pipeline_type: RGB # RGB, RGBD
+      i_pipeline_type: CamArray # RGB, RGBD, CamArray
       i_nn_type: None # rgb, spatial
     rgb:
       # TODO(issue#113) Unify ROI configuration for all cameras
@@ -14,19 +14,25 @@
       i_height: 704 # 720
       i_width: 704 # 1280
       i_output_isp: false
+      i_publish_topic: true
       # -------------
       i_enable_preview: false
       # i_enable_passthrough: true
       # i_keep_preview_aspect_ratio: true
       # i_preview_size: 416
-    # stereo:
-    #   i_subpixel: true
-    # spatial_bb_node:
-    #   ros__parameters:
-    #     desqueeze: true
-    # nn:
-    #   i_disable_resize: true
-    #   i_enable_passthrough: true
+    stereo:
+      i_publish_topic: true
+      i_subpixel: true
+    right:
+      i_publish_topic: true
+      i_height: 720
+      i_width: 1280
+    left:
+      i_publish_topic: true
+      i_height: 720
+      i_width: 1280
+    depth:
+      i_publish_topic: true
 /cam_tool_front:
   ros__parameters:
     camera:

aegis_description/CHANGELOG.md

@@ -23,6 +23,7 @@ and this project adheres to [Semantic Versioning](https://semver.org/spec/v2.0.0
 
 ### Changed
 
+* [PR-115](https://github.com/AGH-CEAI/aegis_ros/pull/115) - Changed the camera position.
 * [PR-114](https://github.com/AGH-CEAI/aegis_ros/pull/114) - Updated docs before release.
 * [PR-77](https://github.com/AGH-CEAI/aegis_ros/pull/77) - Changed `ur_base` frame to the `world` frame (simulation simplification).
 * [PR-62](https://github.com/AGH-CEAI/aegis_ros/pull/62) - Replaced cell collision mesh with primitive shapes.

aegis_description/urdf/modules/camera_scene_luxonis.xacro

@@ -14,8 +14,8 @@
       camera_model="$(arg scene_camera_model)"
       base_frame="$(arg scene_camera_base_frame)"
       cam_pos_x="0.33"
-      cam_pos_y="0.014"
-      cam_pos_z="1.166"
+      cam_pos_y="-0.016"
+      cam_pos_z="1.160"
       cam_roll="0"
       cam_pitch="${pi / 2}"
       cam_yaw="${pi}"

aegis_director/aegis_director/robot_director.py

@@ -217,9 +217,14 @@ def pose_move(
         self.moveit2.cartesian_avoid_collisions = cartesian_avoid_collisions
         self.moveit2.cartesian_jump_threshold = cartesian_jump_threshold
 
-        self.node.get_logger().info(
-            f"Moving to {{position: {position}, quat: {quat_xyzw}}}, max_vel: {max_vel:.2f}, max_accel: {max_accel:.2f}}}"
+        self._print_pose_move_info(
+            position=position,
+            quat_xyzw=quat_xyzw,
+            pose=pose,
+            max_vel=max_vel,
+            max_accel=max_accel,
         )
+
         self.moveit2.move_to_pose(
             pose=pose,
             position=position,
@@ -230,6 +235,39 @@ def pose_move(
         )
         self._wait_for_move_execution(cancel_after_secs)
 
+    def _print_pose_move_info(
+        self,
+        position: Optional[
+            Union[Point, tuple[float, float, float], dict[str, float]]
+        ] = None,
+        quat_xyzw: Optional[
+            Union[Quaternion, tuple[float, float, float, float], dict[str, float]]
+        ] = None,
+        pose: Optional[Union[PoseStamped, Pose]] = None,
+        max_vel: float = 1.0,
+        max_accel: float = 1.0,
+    ) -> None:
+        pos_str = None
+        quat_str = None
+
+        if pose is not None:
+            p = pose.pose if hasattr(pose, "pose") else pose
+            pos_str = (p.position.x, p.position.y, p.position.z)
+            quat_str = (
+                p.orientation.x,
+                p.orientation.y,
+                p.orientation.z,
+                p.orientation.w,
+            )
+        else:
+            pos_str = position
+            quat_str = quat_xyzw
+
+        self.node.get_logger().info(
+            f"Moving to {{pos: {pos_str}, quat: {quat_str}}}, "
+            f"max_vel: {max_vel:.2f}, max_accel: {max_accel:.2f}"
+        )
+
     def gripper_move(
         self, width: Optional[float] = None, action: Optional[str] = None
     ) -> None:

aegis_moveit_config/config/object_pose_detector_config.yaml

@@ -0,0 +1,7 @@
+object_pose_detector:
+  ros__parameters:
+    aruco_size: 0.02
+    image_topic: "/cam_scene/rgb/image_rect"
+    cam_info_topic: "/cam_scene/rgb/camera_info"
+    output_frame: "base_link"
+    cam_frame: "cam_scene_rgb_camera_optical_frame"

aegis_moveit_config/launch/move_group.launch.py

@@ -36,6 +36,10 @@ def __init__(self):
         self.description_cfg_pkg_name = "aegis_description"
         self.description_cfg_pkg = FindPackageShare(self.description_cfg_pkg_name)
 
+        # TODO if working then remove lines
+        # self.scene_objects_cfg_pkg_name = "scene_objects_manager"
+        # self.scene_objects_cfg_pkg = FindPackageShare(self.scene_objects_cfg_pkg_name)
+
         self.kinematics_cfg = PathJoinSubstitution(
             [self.moveit_cfg_pkg, "config", "move_group", "kinematics.yaml"]
         )
@@ -52,6 +56,10 @@ def __init__(self):
             [self.moveit_cfg_pkg, "config", "scene_objects.yaml"]
         )
 
+        self.object_pose_detector_cfg = PathJoinSubstitution(
+            [self.moveit_cfg_pkg, "config", "object_pose_detector_config.yaml"]
+        )
+
         self.rviz_cfg = PathJoinSubstitution(
             [self.moveit_cfg_pkg, "config", "moveit.rviz"]
         )
@@ -267,10 +275,10 @@ def prepare_static_tf_node(base_link: str, child_link: str) -> Node:
 def prepare_scene_objects_manager_node(paths: AegisPathsCfg) -> Node:
     return Node(
         package="scene_objects_manager",
-        executable="scene_objects_manager",
-        name="scene_objects_manager",
+        executable="scene_objects_manager_node",
         output="screen",
         arguments=["--cfg", paths.scene_objects_cfg, "--frame", "world"],
+        parameters=[paths.object_pose_detector_cfg],
     )
 
 

aegis_utils/CHANGELOG.md

@@ -25,6 +25,7 @@ and this project adheres to [Semantic Versioning](https://semver.org/spec/v2.0.0
 
 ### Changed
 
+* [PR-115](https://github.com/AGH-CEAI/aegis_ros/pull/115) - Updated cam scene intrinsics and extrinsics.
 * [PR-114](https://github.com/AGH-CEAI/aegis_ros/pull/114) - Updated docs before release.
 * [PR-99](https://github.com/AGH-CEAI/aegis_ros/pull/99) - T_base2cam was measured and set experimentally.
 * [PR-64](https://github.com/AGH-CEAI/aegis_ros/pull/64) - Switched calibration configuration from JSON to YAML and updated formatting style.

aegis_utils/aegis_utils/measure_camera_error.py

@@ -352,7 +352,7 @@ def parse_args() -> argparse.Namespace:
         "--tool_offset",
         type=float,
         nargs=3,
-        default=[0.00077, 0.00053, 0.26455],
+        default=[0.0, 0.0, 0.264],
         help="Offset of the calibration tool from tool0 frame in meters (x y z)",
     )
     parser.add_argument(

aegis_utils/aegis_utils/measure_camera_error_ros_nodes.py

@@ -186,7 +186,7 @@ def _republish_last_tf(self):
         if self.last_object_tf is None:
             return
         rvec, tvec = self.last_object_tf
-        tf_camera_name = "cam_scene_rgb_camera_optical_frame"
+        tf_camera_name = "cam_scene_rgb_camera_optical_frame_cal"
         self._publish_detected_object_tf(
             rvec=rvec,
             tvec=tvec,

aegis_utils/config/scene_extrinsics.yaml

@@ -1,23 +1,10 @@
 T_base2cam:
-- [0.0, 1.0, 0.0, 0.339468918]
-- [1.0, 0.0, 0.0, -0.029242202]
-- [0.0, 0.0, -1.0, 1.170050432]
+- [0.0, 1.0, 0.0, 0.329395049]
+- [1.0, 0.0, 0.0, -0.020626899]
+- [0.0, 0.0, -1.0, 1.159807969]
 - [0.0, 0.0, 0.0, 1.0]
 T_cam2base:
-- [0.0, 1.0, 0.0, 0.0292422]
-- [1.0, 0.0, 0.0, -0.33946892]
+- [0.0, 1.0, 0.0, 0.020626899]
+- [1.0, 0.0, 0.0, -0.329395049]
+- [0.0, 0.0, -1.0, 1.159807969]
 - [0.0, 0.0, 0.0, 1.0]
-- [0.0, 0.0, 0.0, 1.0]
-
-# TODO(issue#100) Calibration needs to be corrected. The visible matrices are calculated based on position error without taking camera rotation into account
-
-# T_base2cam:
-# - [0.019393463529861155, 0.9996413820847947, 0.018466206863277983, 0.3240708510322008]
-# - [0.999024067443758, -0.018641790273284053, -0.04004243153875939, -0.007406087495627406]
-# - [-0.0396838284499529, 0.0192247465265037, -0.9990273283952478, 1.1742893794290938]
-# - [0.0, 0.0, 0.0, 1.0]
-# T_cam2base:
-# - [0.019393463529861016, 0.9990240674437583, -0.039683828449952906, 0.047714301707007856]
-# - [0.9996413820847948, -0.0186417902732842, 0.019224746526503706, -0.34666811181735363]
-# - [0.018466206863277983, -0.04004243153875939, -0.9990273283952479, 1.1668662643689283]
-# - [0.0, 0.0, 0.0, 1.0]

aegis_utils/config/scene_intrinsics.yaml

@@ -4,17 +4,17 @@ camera_name: scene
 camera_matrix:
   rows: 3
   cols: 3
-  data: [1045.253469873161, 0.0, 616.7886604727472, 0.0, 1043.2430632943033, 386.8198966768913, 0.0, 0.0, 1.0]
-distortion_model: plumb_bob
+  data: [1035.415283203125, 0.0, 623.8995361328125, 0.0, 1034.0101318359375, 377.0184020996094, 0.0, 0.0, 1.0]
+distortion_model: rational_polynomial
 distortion_coefficients:
   rows: 1
-  cols: 5
-  data: [0.08083301537530861, -0.04332893272558069, 0.003319516691409084, -0.0016267198557977577, -0.2800871865529448]
+  cols: 8
+  data: [14.950403213500977, -144.93594360351562, -5.830940790474415e-05, -0.0007103129755705595, 366.5677185058594, 14.665616035461426, -143.06814575195312, 362.23809814453125]
 rectification_matrix:
   rows: 3
   cols: 3
   data: [1, 0, 0, 0, 1, 0, 0, 0, 1]
 projection_matrix:
   rows: 3
   cols: 4
-  data: [1045.253469873161, 0, 616.7886604727472, 0, 0, 1043.2430632943033, 386.8198966768913, 0, 0, 0, 1, 0]
+  data: [1035.415283203125, 0.0, 623.8995361328125, 0.0, 0.0, 1034.0101318359375, 377.0184020996094, 0.0, 0.0, 0.0, 1.0, 0.0]