diff --git a/.docs/aegis_station.png:Zone.Identifier b/.docs/aegis_station.png:Zone.Identifier new file mode 100644 index 00000000..d6c1ec68 Binary files /dev/null and b/.docs/aegis_station.png:Zone.Identifier differ diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index 23904f71..12c632a7 100644 --- a/.pre-commit-config.yaml +++ b/.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.9 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.3 hooks: - id: uv-lock diff --git a/aegis_control/CHANGELOG.md b/aegis_control/CHANGELOG.md index ee7ef138..73e1a684 100644 --- a/aegis_control/CHANGELOG.md +++ b/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. diff --git a/aegis_control/config/cameras/depthai_cameras.yaml b/aegis_control/config/cameras/depthai_cameras.yaml index e8a51cce..269fccb6 100644 --- a/aegis_control/config/cameras/depthai_cameras.yaml +++ b/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: RGB # RGBD, CamArray i_nn_type: None # rgb, spatial rgb: # TODO(issue#113) Unify ROI configuration for all cameras @@ -14,19 +14,22 @@ 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 + # TODO(issue#116) Enable right and left scene camera. # stereo: + # i_publish_topic: true # i_subpixel: true - # spatial_bb_node: - # ros__parameters: - # desqueeze: true - # nn: - # i_disable_resize: true - # i_enable_passthrough: true + # right: + # i_publish_topic: true + # left: + # i_publish_topic: true + # depth: + # i_publish_topic: true /cam_tool_front: ros__parameters: camera: diff --git a/aegis_description/CHANGELOG.md b/aegis_description/CHANGELOG.md index 5a2f2dee..a3a10988 100644 --- a/aegis_description/CHANGELOG.md +++ b/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. diff --git a/aegis_description/urdf/modules/camera_scene_luxonis.xacro b/aegis_description/urdf/modules/camera_scene_luxonis.xacro index 37759c68..e5907d51 100644 --- a/aegis_description/urdf/modules/camera_scene_luxonis.xacro +++ b/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}" diff --git a/aegis_director/CHANGELOG.md b/aegis_director/CHANGELOG.md index 9126fb60..c944dfe1 100644 --- a/aegis_director/CHANGELOG.md +++ b/aegis_director/CHANGELOG.md @@ -18,6 +18,7 @@ and this project adheres to [Semantic Versioning](https://semver.org/spec/v2.0.0 ### Added +* [PR-115](https://github.com/AGH-CEAI/aegis_ros/pull/115) - Added method for logging movement data. * [PR-75](https://github.com/AGH-CEAI/aegis_ros/pull/75) - Added methods for controlling MoveIt2 Servo (Twist & Jog). ### Changed diff --git a/aegis_director/aegis_director/robot_director.py b/aegis_director/aegis_director/robot_director.py index 410d824c..62f10db2 100644 --- a/aegis_director/aegis_director/robot_director.py +++ b/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: diff --git a/aegis_moveit_config/config/object_pose_detector_config.yaml b/aegis_moveit_config/config/object_pose_detector_config.yaml new file mode 100644 index 00000000..6da943c4 --- /dev/null +++ b/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" diff --git a/aegis_moveit_config/launch/move_group.launch.py b/aegis_moveit_config/launch/move_group.launch.py index 7358c38e..9be128d9 100644 --- a/aegis_moveit_config/launch/move_group.launch.py +++ b/aegis_moveit_config/launch/move_group.launch.py @@ -52,6 +52,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 +271,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], ) diff --git a/aegis_utils/CHANGELOG.md b/aegis_utils/CHANGELOG.md index c0bcf1bf..14dc1df5 100644 --- a/aegis_utils/CHANGELOG.md +++ b/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. diff --git a/aegis_utils/aegis_utils/measure_camera_error.py b/aegis_utils/aegis_utils/measure_camera_error.py index 8aded839..f5060546 100644 --- a/aegis_utils/aegis_utils/measure_camera_error.py +++ b/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( diff --git a/aegis_utils/aegis_utils/measure_camera_error_ros_nodes.py b/aegis_utils/aegis_utils/measure_camera_error_ros_nodes.py index a1950c6c..8ebc4d2d 100644 --- a/aegis_utils/aegis_utils/measure_camera_error_ros_nodes.py +++ b/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, diff --git a/aegis_utils/config/scene_extrinsics.yaml b/aegis_utils/config/scene_extrinsics.yaml index 18ccefdf..c8c4a7c3 100644 --- a/aegis_utils/config/scene_extrinsics.yaml +++ b/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] diff --git a/aegis_utils/config/scene_intrinsics.yaml b/aegis_utils/config/scene_intrinsics.yaml index fd2a6c46..455f8cd8 100644 --- a/aegis_utils/config/scene_intrinsics.yaml +++ b/aegis_utils/config/scene_intrinsics.yaml @@ -4,12 +4,12 @@ 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 @@ -17,4 +17,4 @@ rectification_matrix: 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]