From b4c27a7f55a5e6724587bb5cec3ed6e7b4a55ad8 Mon Sep 17 00:00:00 2001 From: m-brl <103381146+m-brl@users.noreply.github.com> Date: Thu, 4 Jun 2026 11:13:13 +0200 Subject: [PATCH 01/10] evol(config): add camera into yaml config evol(config): evol gazebo config (split config) feat(bridge): add generated bridge config evol(config): add generated file fix(config): fix active config Signed-off-by: m-brl <103381146+m-brl@users.noreply.github.com> --- config/gazebo_bridge.yaml | 23 -- config/hardware/active.yaml | 37 ++ config/hardware/configs/default.yaml | 37 ++ description/gazebo/gazebo.xacro | 371 ++++++++++++++++++ description/gazebo/gazebo_bridge.yaml | 37 ++ .../ros2_control/inmoov_gz_ros2_control.xacro | 17 - .../ros2_control/inmoov_ros2_control.xacro | 76 ++-- description/urdf/inmoov.urdf.xacro | 7 +- launch/gazebo.launch.py | 2 +- worlds/default.sdf | 32 -- 10 files changed, 518 insertions(+), 121 deletions(-) delete mode 100644 config/gazebo_bridge.yaml create mode 100644 description/gazebo/gazebo.xacro create mode 100644 description/gazebo/gazebo_bridge.yaml delete mode 100644 description/ros2_control/inmoov_gz_ros2_control.xacro diff --git a/config/gazebo_bridge.yaml b/config/gazebo_bridge.yaml deleted file mode 100644 index 6f6f708..0000000 --- a/config/gazebo_bridge.yaml +++ /dev/null @@ -1,23 +0,0 @@ -- ros_topic_name: "/clock" - gz_topic_name: "/world/default/clock" - ros_type_name: "rosgraph_msgs/msg/Clock" - gz_type_name: "gz.msgs.Clock" - direction: GZ_TO_ROS - -- ros_topic_name: "/camera/gazebo/raw" - gz_topic_name: "/world/default/camera" - ros_type_name: "sensor_msgs/msg/Image" - gz_type_name: "gz.msgs.Image" - direction: GZ_TO_ROS - -- ros_topic_name: "/sensor_right_arm/mock_sensor_data" - gz_topic_name: "/sensor_right_arm/mock_sensor_data" - ros_type_name: "ros_gz_interfaces/msg/Float32Array" - gz_type_name: "gz.msgs.Float_V" - direction: GZ_TO_ROS - -- ros_topic_name: "/sensor_left_arm/mock_sensor_data" - gz_topic_name: "/sensor_left_arm/mock_sensor_data" - ros_type_name: "ros_gz_interfaces/msg/Float32Array" - gz_type_name: "gz.msgs.Float_V" - direction: GZ_TO_ROS diff --git a/config/hardware/active.yaml b/config/hardware/active.yaml index 2c974d4..97d1cf3 100644 --- a/config/hardware/active.yaml +++ b/config/hardware/active.yaml @@ -43,6 +43,43 @@ ignore_urdf_joints: - i01.rightHand.thumb1_link_joint - i01.rightHand.thumb3_link_joint - i01.rightHand.wrist.001_link_joint + +cameras: + - id: 'ext_camera' + name: 'External USB Webcam' + topic: '/camera/external/ext1/jpg' + message_type: 'sensor_msgs/msg/CompressedImage' + external: true + + - realsense-rgb: + name: 'Realsense RGB' + topic: '/realsense/realsense2_camera/color/image_raw/compressed' + message_type: 'sensor_msgs/msg/CompressedImage' + external: false + link: 'i01.head.eyeLeft_link' + + - realsense-aligned-depth: + name: 'Realsense Aligned Depth' + topic: '/realsense/realsense2_camera/aligned_depth_to_color/image_raw/compressed' + message_type: 'sensor_msgs/msg/CompressedImage' + external: false + link: 'i01.head.eyeLeft_link' + + - left_eye_camera: + name: 'Left eye webcam' + topic: '/camera/internal/left_eye/jpg' + message_type: 'sensor_msgs/msg/CompressedImage' + external: false + link: 'i01.head.eyeRight_link' + + - right_eye_camera: + name: 'Right eye webcam' + topic: '/camera/internal/right_eye/jpg' + message_type: 'sensor_msgs/msg/CompressedImage' + external: false + link: 'i01.head.eyeLeft_link' + + firmware: source_dir: micro_ros_raspberrypi_pico_sdk build_dir: build diff --git a/config/hardware/configs/default.yaml b/config/hardware/configs/default.yaml index 2c974d4..97d1cf3 100644 --- a/config/hardware/configs/default.yaml +++ b/config/hardware/configs/default.yaml @@ -43,6 +43,43 @@ ignore_urdf_joints: - i01.rightHand.thumb1_link_joint - i01.rightHand.thumb3_link_joint - i01.rightHand.wrist.001_link_joint + +cameras: + - id: 'ext_camera' + name: 'External USB Webcam' + topic: '/camera/external/ext1/jpg' + message_type: 'sensor_msgs/msg/CompressedImage' + external: true + + - realsense-rgb: + name: 'Realsense RGB' + topic: '/realsense/realsense2_camera/color/image_raw/compressed' + message_type: 'sensor_msgs/msg/CompressedImage' + external: false + link: 'i01.head.eyeLeft_link' + + - realsense-aligned-depth: + name: 'Realsense Aligned Depth' + topic: '/realsense/realsense2_camera/aligned_depth_to_color/image_raw/compressed' + message_type: 'sensor_msgs/msg/CompressedImage' + external: false + link: 'i01.head.eyeLeft_link' + + - left_eye_camera: + name: 'Left eye webcam' + topic: '/camera/internal/left_eye/jpg' + message_type: 'sensor_msgs/msg/CompressedImage' + external: false + link: 'i01.head.eyeRight_link' + + - right_eye_camera: + name: 'Right eye webcam' + topic: '/camera/internal/right_eye/jpg' + message_type: 'sensor_msgs/msg/CompressedImage' + external: false + link: 'i01.head.eyeLeft_link' + + firmware: source_dir: micro_ros_raspberrypi_pico_sdk build_dir: build diff --git a/description/gazebo/gazebo.xacro b/description/gazebo/gazebo.xacro new file mode 100644 index 0000000..388a337 --- /dev/null +++ b/description/gazebo/gazebo.xacro @@ -0,0 +1,371 @@ + + + + + + gz_ros2_control/GazeboSimSystem + + + 0 + 270 + 0.0 + 1 + 1.0 + 0.0 + 270.0 + 135.0 + + + + + 1 + 270 + 0.0 + 1 + 1.0 + 0.0 + 270.0 + 135.0 + + + + + 2 + 270 + 0.0 + 1 + 1.0 + 0.0 + 270.0 + 90.0 + + + + + 3 + 300 + 0.0 + 1 + 1.0 + 0.0 + 150.0 + 50.0 + + + + + 4 + 300 + 0.0 + 1 + 1.0 + 0.0 + 150.0 + 50.0 + + + + + 5 + 300 + 0.0 + 1 + 1.0 + 0.0 + 150.0 + 50.0 + + + + + 6 + 300 + 0.0 + 1 + 1.0 + 0.0 + 150.0 + 50.0 + + + + + 7 + 300 + 0.0 + 1 + 1.0 + 0.0 + 150.0 + 50.0 + + + + + 8 + 300 + 0.0 + 1 + 1.0 + 0.0 + 150.0 + 50.0 + + + + + + + gz_ros2_control/GazeboSimSystem + + + 0 + 270 + 0.0 + 1 + 1.0 + 155.0 + 165.0 + 160.0 + + + + + 1 + 270 + 0.0 + 1 + 1.0 + 140.0 + 180.0 + 180.0 + + + + + 2 + 270 + 0.0 + 1 + 1.0 + 100.0 + 110.0 + 100.0 + + + + + 3 + 300 + 0.0 + 1 + 1.0 + 0.0 + 230.0 + 150.0 + + + + + 4 + 300 + 0.0 + 1 + 1.0 + 36.0 + 150.0 + 36.0 + + + + + 5 + 300 + 0.0 + 1 + 1.0 + 16.0 + 150.0 + 16.0 + + + + + 6 + 300 + 0.0 + 1 + 1.0 + 21.0 + 150.0 + 21.0 + + + + + 7 + 300 + 0.0 + 1 + 1.0 + 33.0 + 150.0 + 33.0 + + + + + 8 + 300 + 0.0 + 1 + 1.0 + 30.0 + 150.0 + 30.0 + + + + + + + gz_ros2_control/GazeboSimSystem + + + 0 + 270 + 0.0 + 1 + 1.0 + 0.0 + 270.0 + 0.0 + + + + + 1 + 270 + 0.0 + 1 + 1.0 + 0.0 + 270.0 + 0.0 + + + + + + + + 10 + sensors/left_arm + + + + + + + + + + 10 + sensors/right_arm + + + + + + + + + + + true + 30.0 + true + //realsense/realsense2_camera/color/image_raw/compressed + + 1.047 + + 640 + 480 + R8G8B8 + + + 0.1 + 100.0 + + + + + + + true + 30.0 + true + //realsense/realsense2_camera/aligned_depth_to_color/image_raw/compressed + + 1.047 + + 640 + 480 + R8G8B8 + + + 0.1 + 100.0 + + + + + + + true + 30.0 + true + //camera/internal/left_eye/jpg + + 1.047 + + 640 + 480 + R8G8B8 + + + 0.1 + 100.0 + + + + + + + true + 30.0 + true + //camera/internal/right_eye/jpg + + 1.047 + + 640 + 480 + R8G8B8 + + + 0.1 + 100.0 + + + + + + + + $(arg controller_config) + + + + + \ No newline at end of file diff --git a/description/gazebo/gazebo_bridge.yaml b/description/gazebo/gazebo_bridge.yaml new file mode 100644 index 0000000..c528fb8 --- /dev/null +++ b/description/gazebo/gazebo_bridge.yaml @@ -0,0 +1,37 @@ +# Generated by lucy_config_generator — do not edit. +- ros_topic_name: "/clock" + gz_topic_name: "/world/default/clock" + ros_type_name: "rosgraph_msgs/msg/Clock" + gz_type_name: "gz.msgs.Clock" + direction: GZ_TO_ROS + +- ros_topic_name: "/realsense/realsense2_camera/color/image_raw/compressed" + gz_topic_name: "/realsense/realsense2_camera/color/image_raw/compressed" + ros_type_name: "sensor_msgs/msg/Image" + gz_type_name: "gz.msgs.Image" + direction: GZ_TO_ROS +- ros_topic_name: "/realsense/realsense2_camera/aligned_depth_to_color/image_raw/compressed" + gz_topic_name: "/realsense/realsense2_camera/aligned_depth_to_color/image_raw/compressed" + ros_type_name: "sensor_msgs/msg/Image" + gz_type_name: "gz.msgs.Image" + direction: GZ_TO_ROS +- ros_topic_name: "/camera/internal/left_eye/jpg" + gz_topic_name: "/camera/internal/left_eye/jpg" + ros_type_name: "sensor_msgs/msg/Image" + gz_type_name: "gz.msgs.Image" + direction: GZ_TO_ROS +- ros_topic_name: "/camera/internal/right_eye/jpg" + gz_topic_name: "/camera/internal/right_eye/jpg" + ros_type_name: "sensor_msgs/msg/Image" + gz_type_name: "gz.msgs.Image" + direction: GZ_TO_ROS +- ros_topic_name: "sensors/left_arm" + gz_topic_name: "sensors/left_arm" + ros_type_name: "ros_gz_interfaces/msg/Float32Array" + gz_type_name: "gz.msgs.Float_V" + direction: GZ_TO_ROS +- ros_topic_name: "sensors/right_arm" + gz_topic_name: "sensors/right_arm" + ros_type_name: "ros_gz_interfaces/msg/Float32Array" + gz_type_name: "gz.msgs.Float_V" + direction: GZ_TO_ROS diff --git a/description/ros2_control/inmoov_gz_ros2_control.xacro b/description/ros2_control/inmoov_gz_ros2_control.xacro deleted file mode 100644 index 72e1680..0000000 --- a/description/ros2_control/inmoov_gz_ros2_control.xacro +++ /dev/null @@ -1,17 +0,0 @@ - - - - - - $(arg controller_config) - - - - - diff --git a/description/ros2_control/inmoov_ros2_control.xacro b/description/ros2_control/inmoov_ros2_control.xacro index f2a044d..6b4c8b1 100644 --- a/description/ros2_control/inmoov_ros2_control.xacro +++ b/description/ros2_control/inmoov_ros2_control.xacro @@ -1,25 +1,19 @@ - - - - gz_ros2_control/GazeboSimSystem - - - lucy_ros2_control/LucySystemHardware - - false - - - actuators/left_arm - lucy_hardware_interface_left_arm - - - + + lucy_ros2_control/LucySystemHardware + + false + + + actuators/left_arm + lucy_hardware_interface_left_arm + + 0 270 @@ -157,21 +151,16 @@ - - - gz_ros2_control/GazeboSimSystem - - - lucy_ros2_control/LucySystemHardware - - false - - - actuators/right_arm - lucy_hardware_interface_right_arm - - - + + lucy_ros2_control/LucySystemHardware + + false + + + actuators/right_arm + lucy_hardware_interface_right_arm + + 0 270 @@ -309,21 +298,16 @@ - - - gz_ros2_control/GazeboSimSystem - - - lucy_ros2_control/LucySystemHardware - - false - - - actuators/torso - lucy_hardware_interface_torso_head - - - + + lucy_ros2_control/LucySystemHardware + + false + + + actuators/torso + lucy_hardware_interface_torso_head + + 0 270 diff --git a/description/urdf/inmoov.urdf.xacro b/description/urdf/inmoov.urdf.xacro index 9df9a3b..ca1443b 100644 --- a/description/urdf/inmoov.urdf.xacro +++ b/description/urdf/inmoov.urdf.xacro @@ -19,7 +19,10 @@ - + + + + - + diff --git a/launch/gazebo.launch.py b/launch/gazebo.launch.py index 8ce279d..97d678c 100644 --- a/launch/gazebo.launch.py +++ b/launch/gazebo.launch.py @@ -130,7 +130,7 @@ def generate_launch_description(): ) bridge_config_path = os.path.join( - get_package_share_directory("thais_urdf"), "config", "gazebo_bridge.yaml" + get_package_share_directory("thais_urdf"), "description", "gazebo", "gazebo_bridge.yaml" ) bridge = Node( package="ros_gz_bridge", diff --git a/worlds/default.sdf b/worlds/default.sdf index 043d5c8..2a33a53 100644 --- a/worlds/default.sdf +++ b/worlds/default.sdf @@ -99,37 +99,5 @@ - - - true - -10 -10 20 0 0.4636 0.7853 - - 10 - - 5.0 - 0 - thumb - - - - - - - - - - true - -10 -10 20 0 0.4636 0.7853 - - 10.0 - - - - - - - - - From 7fc7b49a0b1444f847bcde018f15656c7f7ea147 Mon Sep 17 00:00:00 2001 From: m-brl <103381146+m-brl@users.noreply.github.com> Date: Wed, 10 Jun 2026 22:35:24 +0200 Subject: [PATCH 02/10] feat(config): regenerate config Signed-off-by: m-brl <103381146+m-brl@users.noreply.github.com> --- description/gazebo/gazebo.xacro | 333 ++++++++++++++++++++++---------- 1 file changed, 227 insertions(+), 106 deletions(-) diff --git a/description/gazebo/gazebo.xacro b/description/gazebo/gazebo.xacro index 388a337..d3e386a 100644 --- a/description/gazebo/gazebo.xacro +++ b/description/gazebo/gazebo.xacro @@ -1,6 +1,7 @@ + gz_ros2_control/GazeboSimSystem @@ -8,108 +9,108 @@ 0 270 - 0.0 + 90 1 - 1.0 - 0.0 - 270.0 - 135.0 + 1 + 0 + 180 + 90 1 270 - 0.0 + 0 1 - 1.0 - 0.0 - 270.0 - 135.0 + 1 + 0 + 180 + 0 2 270 - 0.0 + 0 1 - 1.0 - 0.0 - 270.0 - 90.0 + 1 + 0 + 180 + 0 3 300 - 0.0 + 150 1 - 1.0 - 0.0 - 150.0 - 50.0 + 1 + 0 + 300 + 150 4 300 - 0.0 + 0 1 - 1.0 - 0.0 - 150.0 - 50.0 + 1 + 0 + 180 + 0 5 300 - 0.0 + 0 1 - 1.0 - 0.0 - 150.0 - 50.0 + 1 + 0 + 180 + 0 6 300 - 0.0 + 0 1 - 1.0 - 0.0 - 150.0 - 50.0 + 1 + 0 + 180 + 0 7 300 - 0.0 + 0 1 - 1.0 - 0.0 - 150.0 - 50.0 + 1 + 0 + 180 + 0 8 300 - 0.0 + 0 1 - 1.0 - 0.0 - 150.0 - 50.0 + 0.25 + 0 + 180 + 0 @@ -121,108 +122,108 @@ 0 270 - 0.0 - 1 - 1.0 - 155.0 - 165.0 - 160.0 + 90 + -1 + 1 + 0 + 180 + 90 1 270 - 0.0 + 0 1 - 1.0 - 140.0 - 180.0 - 180.0 + 1 + 0 + 180 + 0 2 270 - 0.0 + 0 1 - 1.0 - 100.0 - 110.0 - 100.0 + 1 + 0 + 180 + 0 3 300 - 0.0 - 1 - 1.0 - 0.0 - 230.0 - 150.0 + 150 + -1 + 1 + 0 + 300 + 150 4 300 - 0.0 + 0 1 - 1.0 - 36.0 - 150.0 - 36.0 + 1 + 0 + 180 + 0 5 300 - 0.0 + 0 1 - 1.0 - 16.0 - 150.0 - 16.0 + 1 + 0 + 180 + 0 6 300 - 0.0 + 0 1 - 1.0 - 21.0 - 150.0 - 21.0 + 1 + 0 + 180 + 0 7 300 - 0.0 + 0 1 - 1.0 - 33.0 - 150.0 - 33.0 + 1 + 0 + 180 + 0 8 300 - 0.0 + 0 1 - 1.0 - 30.0 - 150.0 - 30.0 + 1 + 0 + 180 + 0 @@ -234,24 +235,144 @@ 0 270 - 0.0 + 0 1 - 1.0 - 0.0 - 270.0 - 0.0 + 1 + 0 + 180 + 0 1 270 - 0.0 + 0 + 1 + 1 + 0 + 180 + 0 + + + + + 2 + 270 + 135 + 1 + 1 + 0 + 270 + 135 + + + + + 3 + 270 + 90 + 1 + 1 + 0 + 180 + 90 + + + + + 4 + 270 + 90 + 1 + 1 + 0 + 180 + 90 + + + + + 5 + 270 + 90 + 1 + 1 + 0 + 180 + 90 + + + + + 6 + 300 + 90 + 1 + 1 + 0 + 180 + 90 + + + + + 7 + 300 + 90 + 1 + 1 + 0 + 180 + 90 + + + + + 8 + 180 + 90 + 1 + 1 + 0 + 180 + 90 + + + + + 9 + 180 + 90 + 1 + 1 + 0 + 180 + 90 + + + + + 10 + 180 + 90 + 1 + 1 + 0 + 180 + 90 + + + + + 11 + 180 + 90 1 - 1.0 - 0.0 - 270.0 - 0.0 + 1 + 0 + 180 + 90 @@ -285,7 +406,7 @@ true 30.0 true - //realsense/realsense2_camera/color/image_raw/compressed + /realsense/realsense2_camera/color/image_raw/compressed 1.047 @@ -305,7 +426,7 @@ true 30.0 true - //realsense/realsense2_camera/aligned_depth_to_color/image_raw/compressed + /realsense/realsense2_camera/aligned_depth_to_color/image_raw/compressed 1.047 @@ -325,7 +446,7 @@ true 30.0 true - //camera/internal/left_eye/jpg + /camera/internal/left_eye/jpg 1.047 @@ -345,7 +466,7 @@ true 30.0 true - //camera/internal/right_eye/jpg + /camera/internal/right_eye/jpg 1.047 From 777a7ab574ab8075d021d4d7f1edca6f448a51c5 Mon Sep 17 00:00:00 2001 From: Charles Madjeri <80175305+charlesmadjeri@users.noreply.github.com> Date: Thu, 11 Jun 2026 09:12:28 +0200 Subject: [PATCH 03/10] feat(gazebo): compress sim camera streams and simulate external camera (#22) Signed-off-by: Charles Madjeri <80175305+charlesmadjeri@users.noreply.github.com> --- config/hardware/active.yaml | 3 +- config/hardware/configs/default.yaml | 3 +- description/gazebo/gazebo.xacro | 8 +- description/gazebo/gazebo_bridge.yaml | 22 +++-- .../ros2_control/inmoov_ros2_control.xacro | 2 +- launch/gazebo.launch.py | 83 ++++++++++++++++--- 6 files changed, 94 insertions(+), 27 deletions(-) diff --git a/config/hardware/active.yaml b/config/hardware/active.yaml index 97d1cf3..2e7dc6f 100644 --- a/config/hardware/active.yaml +++ b/config/hardware/active.yaml @@ -47,9 +47,10 @@ ignore_urdf_joints: cameras: - id: 'ext_camera' name: 'External USB Webcam' - topic: '/camera/external/ext1/jpg' + topic: '/ext_camera/jpg' message_type: 'sensor_msgs/msg/CompressedImage' external: true + sim_gz_topic: '/world/default/camera' - realsense-rgb: name: 'Realsense RGB' diff --git a/config/hardware/configs/default.yaml b/config/hardware/configs/default.yaml index 97d1cf3..2e7dc6f 100644 --- a/config/hardware/configs/default.yaml +++ b/config/hardware/configs/default.yaml @@ -47,9 +47,10 @@ ignore_urdf_joints: cameras: - id: 'ext_camera' name: 'External USB Webcam' - topic: '/camera/external/ext1/jpg' + topic: '/ext_camera/jpg' message_type: 'sensor_msgs/msg/CompressedImage' external: true + sim_gz_topic: '/world/default/camera' - realsense-rgb: name: 'Realsense RGB' diff --git a/description/gazebo/gazebo.xacro b/description/gazebo/gazebo.xacro index d3e386a..365717e 100644 --- a/description/gazebo/gazebo.xacro +++ b/description/gazebo/gazebo.xacro @@ -406,7 +406,7 @@ true 30.0 true - /realsense/realsense2_camera/color/image_raw/compressed + /realsense/realsense2_camera/color/image_raw 1.047 @@ -426,7 +426,7 @@ true 30.0 true - /realsense/realsense2_camera/aligned_depth_to_color/image_raw/compressed + /realsense/realsense2_camera/aligned_depth_to_color/image_raw 1.047 @@ -446,7 +446,7 @@ true 30.0 true - /camera/internal/left_eye/jpg + /camera/internal/left_eye/jpg/raw 1.047 @@ -466,7 +466,7 @@ true 30.0 true - /camera/internal/right_eye/jpg + /camera/internal/right_eye/jpg/raw 1.047 diff --git a/description/gazebo/gazebo_bridge.yaml b/description/gazebo/gazebo_bridge.yaml index c528fb8..1c1e8bd 100644 --- a/description/gazebo/gazebo_bridge.yaml +++ b/description/gazebo/gazebo_bridge.yaml @@ -5,23 +5,29 @@ gz_type_name: "gz.msgs.Clock" direction: GZ_TO_ROS -- ros_topic_name: "/realsense/realsense2_camera/color/image_raw/compressed" - gz_topic_name: "/realsense/realsense2_camera/color/image_raw/compressed" +# Raw frames; gazebo.launch.py republishes -> compressed topic (CompressedImage) for the LCP. +- ros_topic_name: "/ext_camera/jpg/raw" + gz_topic_name: "/world/default/camera" ros_type_name: "sensor_msgs/msg/Image" gz_type_name: "gz.msgs.Image" direction: GZ_TO_ROS -- ros_topic_name: "/realsense/realsense2_camera/aligned_depth_to_color/image_raw/compressed" - gz_topic_name: "/realsense/realsense2_camera/aligned_depth_to_color/image_raw/compressed" +- ros_topic_name: "/realsense/realsense2_camera/color/image_raw" + gz_topic_name: "/realsense/realsense2_camera/color/image_raw" ros_type_name: "sensor_msgs/msg/Image" gz_type_name: "gz.msgs.Image" direction: GZ_TO_ROS -- ros_topic_name: "/camera/internal/left_eye/jpg" - gz_topic_name: "/camera/internal/left_eye/jpg" +- ros_topic_name: "/realsense/realsense2_camera/aligned_depth_to_color/image_raw" + gz_topic_name: "/realsense/realsense2_camera/aligned_depth_to_color/image_raw" ros_type_name: "sensor_msgs/msg/Image" gz_type_name: "gz.msgs.Image" direction: GZ_TO_ROS -- ros_topic_name: "/camera/internal/right_eye/jpg" - gz_topic_name: "/camera/internal/right_eye/jpg" +- ros_topic_name: "/camera/internal/left_eye/jpg/raw" + gz_topic_name: "/camera/internal/left_eye/jpg/raw" + ros_type_name: "sensor_msgs/msg/Image" + gz_type_name: "gz.msgs.Image" + direction: GZ_TO_ROS +- ros_topic_name: "/camera/internal/right_eye/jpg/raw" + gz_topic_name: "/camera/internal/right_eye/jpg/raw" ros_type_name: "sensor_msgs/msg/Image" gz_type_name: "gz.msgs.Image" direction: GZ_TO_ROS diff --git a/description/ros2_control/inmoov_ros2_control.xacro b/description/ros2_control/inmoov_ros2_control.xacro index 6b4c8b1..e86cb70 100644 --- a/description/ros2_control/inmoov_ros2_control.xacro +++ b/description/ros2_control/inmoov_ros2_control.xacro @@ -490,4 +490,4 @@ - \ No newline at end of file + diff --git a/launch/gazebo.launch.py b/launch/gazebo.launch.py index 97d678c..a9ac1c6 100644 --- a/launch/gazebo.launch.py +++ b/launch/gazebo.launch.py @@ -93,6 +93,54 @@ def _default_controllers_yaml(pkg_share: str, controllers_basename: str) -> str: return os.path.join(pkg_share, "config", controllers_basename) +def _gazebo_camera_raw_topic(topic: str) -> str: + """ + Raw-image topic the gz camera/bridge use in sim (mirrors lucy_config_generator). + + gz-sim + ros_gz_bridge only emit raw ``sensor_msgs/msg/Image``; the LCP consumes + JPEG ``CompressedImage`` on ``topic``. The gz camera renders to this raw topic and + we republish it (raw -> compressed) onto ``topic``. + """ + suffix = "/compressed" + if topic.endswith(suffix): + return topic[: -len(suffix)] + return topic + "/raw" + + +def _sim_camera_topics(pkg_share: str) -> list[str]: + """LCP-facing (compressed) topics for every camera simulated in Gazebo.""" + candidates = [ + Path.cwd() / "src" / "thais_urdf" / "config" / "hardware" / "active.yaml", + Path(pkg_share) / "config" / "hardware" / "active.yaml", + ] + for active in candidates: + if not active.is_file(): + continue + try: + data = yaml.safe_load(active.read_text(encoding="utf-8")) + except (OSError, yaml.YAMLError): + return [] + cameras = data.get("cameras") if isinstance(data, dict) else None + if not isinstance(cameras, list): + return [] + topics: list[str] = [] + for cam in cameras: + if not isinstance(cam, dict): + continue + topic = cam.get("topic") + if not isinstance(topic, str) or not topic.strip(): + continue + if cam.get("external"): + gz_topic = cam.get("sim_gz_topic") + if not isinstance(gz_topic, str) or not gz_topic.strip(): + continue + elif not cam.get("link"): + continue + topics.append(topic.strip()) + return topics + return [] + + def generate_launch_description(): pkg_share = get_package_share_directory("thais_urdf") default_base = os.path.join(pkg_share, "description") @@ -140,17 +188,28 @@ def generate_launch_description(): output="screen", ) - camera_compressor = Node( - package="image_transport", - executable="republish", - arguments=["raw", "compressed"], - remappings=[ - ("in", "/camera/gazebo/raw"), - ("out/compressed", "/ext_camera/jpg"), - ], - parameters=[{"use_sim_time": True}], - output="screen", - ) + # ros_gz_bridge can only emit raw Image; the LCP wants JPEG CompressedImage. + # For every simulated camera (robot-mounted + external world camera), republish + # the bridged raw topic -> the compressed topic the LCP subscribes to. + def _camera_compressor(compressed_topic: str) -> Node: + raw_topic = _gazebo_camera_raw_topic(compressed_topic) + safe = "".join(c if c.isalnum() else "_" for c in compressed_topic).strip("_") + return Node( + package="image_transport", + executable="republish", + name="camera_compressor_" + safe, + arguments=["raw", "compressed"], + remappings=[ + ("in", raw_topic), + ("out/compressed", compressed_topic), + ], + parameters=[{"use_sim_time": True}], + output="screen", + ) + + camera_compressors = [ + _camera_compressor(topic) for topic in _sim_camera_topics(pkg_share) + ] ros_lib = _gz_ros2_control_plugin_path() gz_plugin_path = os.pathsep.join( @@ -246,6 +305,6 @@ def spawner_actions_from_yaml(context, *args, **kwargs): spawn_robot, gz_sim_launch, bridge, - camera_compressor, + *camera_compressors, ] ) From 9a3dad224fe723b332150fcb701bc8b7810989b4 Mon Sep 17 00:00:00 2001 From: m-brl <103381146+m-brl@users.noreply.github.com> Date: Thu, 11 Jun 2026 11:19:19 +0200 Subject: [PATCH 04/10] feat(topic): edit topic name raw_topic = user defined topic compressed_topic = raw_topic + "/compressed" Signed-off-by: m-brl <103381146+m-brl@users.noreply.github.com> --- config/hardware/active.yaml | 4 +-- config/hardware/configs/default.yaml | 4 +-- description/gazebo/gazebo.xacro | 8 ++++-- description/gazebo/gazebo_bridge.yaml | 17 +++++++----- launch/gazebo.launch.py | 40 ++++++++++++--------------- 5 files changed, 37 insertions(+), 36 deletions(-) diff --git a/config/hardware/active.yaml b/config/hardware/active.yaml index 2e7dc6f..ae41ac7 100644 --- a/config/hardware/active.yaml +++ b/config/hardware/active.yaml @@ -54,14 +54,14 @@ cameras: - realsense-rgb: name: 'Realsense RGB' - topic: '/realsense/realsense2_camera/color/image_raw/compressed' + topic: '/realsense/realsense2_camera/color/image_raw' message_type: 'sensor_msgs/msg/CompressedImage' external: false link: 'i01.head.eyeLeft_link' - realsense-aligned-depth: name: 'Realsense Aligned Depth' - topic: '/realsense/realsense2_camera/aligned_depth_to_color/image_raw/compressed' + topic: '/realsense/realsense2_camera/aligned_depth_to_color/image_raw' message_type: 'sensor_msgs/msg/CompressedImage' external: false link: 'i01.head.eyeLeft_link' diff --git a/config/hardware/configs/default.yaml b/config/hardware/configs/default.yaml index 2e7dc6f..ae41ac7 100644 --- a/config/hardware/configs/default.yaml +++ b/config/hardware/configs/default.yaml @@ -54,14 +54,14 @@ cameras: - realsense-rgb: name: 'Realsense RGB' - topic: '/realsense/realsense2_camera/color/image_raw/compressed' + topic: '/realsense/realsense2_camera/color/image_raw' message_type: 'sensor_msgs/msg/CompressedImage' external: false link: 'i01.head.eyeLeft_link' - realsense-aligned-depth: name: 'Realsense Aligned Depth' - topic: '/realsense/realsense2_camera/aligned_depth_to_color/image_raw/compressed' + topic: '/realsense/realsense2_camera/aligned_depth_to_color/image_raw' message_type: 'sensor_msgs/msg/CompressedImage' external: false link: 'i01.head.eyeLeft_link' diff --git a/description/gazebo/gazebo.xacro b/description/gazebo/gazebo.xacro index 365717e..e0f6368 100644 --- a/description/gazebo/gazebo.xacro +++ b/description/gazebo/gazebo.xacro @@ -406,6 +406,7 @@ true 30.0 true + /realsense/realsense2_camera/color/image_raw 1.047 @@ -426,6 +427,7 @@ true 30.0 true + /realsense/realsense2_camera/aligned_depth_to_color/image_raw 1.047 @@ -446,7 +448,8 @@ true 30.0 true - /camera/internal/left_eye/jpg/raw + + /camera/internal/left_eye/jpg 1.047 @@ -466,7 +469,8 @@ true 30.0 true - /camera/internal/right_eye/jpg/raw + + /camera/internal/right_eye/jpg 1.047 diff --git a/description/gazebo/gazebo_bridge.yaml b/description/gazebo/gazebo_bridge.yaml index 1c1e8bd..5915ca3 100644 --- a/description/gazebo/gazebo_bridge.yaml +++ b/description/gazebo/gazebo_bridge.yaml @@ -4,30 +4,33 @@ ros_type_name: "rosgraph_msgs/msg/Clock" gz_type_name: "gz.msgs.Clock" direction: GZ_TO_ROS - -# Raw frames; gazebo.launch.py republishes -> compressed topic (CompressedImage) for the LCP. -- ros_topic_name: "/ext_camera/jpg/raw" +# Raw frames; gazebo.launch.py republishes /ext_camera/jpg -> /ext_camera/jpg (CompressedImage) for the LCP. +- ros_topic_name: "/ext_camera/jpg" gz_topic_name: "/world/default/camera" ros_type_name: "sensor_msgs/msg/Image" gz_type_name: "gz.msgs.Image" direction: GZ_TO_ROS +# Raw frames; gazebo.launch.py republishes /realsense/realsense2_camera/color/image_raw -> /realsense/realsense2_camera/color/image_raw (CompressedImage) for the LCP. - ros_topic_name: "/realsense/realsense2_camera/color/image_raw" gz_topic_name: "/realsense/realsense2_camera/color/image_raw" ros_type_name: "sensor_msgs/msg/Image" gz_type_name: "gz.msgs.Image" direction: GZ_TO_ROS +# Raw frames; gazebo.launch.py republishes /realsense/realsense2_camera/aligned_depth_to_color/image_raw -> /realsense/realsense2_camera/aligned_depth_to_color/image_raw (CompressedImage) for the LCP. - ros_topic_name: "/realsense/realsense2_camera/aligned_depth_to_color/image_raw" gz_topic_name: "/realsense/realsense2_camera/aligned_depth_to_color/image_raw" ros_type_name: "sensor_msgs/msg/Image" gz_type_name: "gz.msgs.Image" direction: GZ_TO_ROS -- ros_topic_name: "/camera/internal/left_eye/jpg/raw" - gz_topic_name: "/camera/internal/left_eye/jpg/raw" +# Raw frames; gazebo.launch.py republishes /camera/internal/left_eye/jpg -> /camera/internal/left_eye/jpg (CompressedImage) for the LCP. +- ros_topic_name: "/camera/internal/left_eye/jpg" + gz_topic_name: "/camera/internal/left_eye/jpg" ros_type_name: "sensor_msgs/msg/Image" gz_type_name: "gz.msgs.Image" direction: GZ_TO_ROS -- ros_topic_name: "/camera/internal/right_eye/jpg/raw" - gz_topic_name: "/camera/internal/right_eye/jpg/raw" +# Raw frames; gazebo.launch.py republishes /camera/internal/right_eye/jpg -> /camera/internal/right_eye/jpg (CompressedImage) for the LCP. +- ros_topic_name: "/camera/internal/right_eye/jpg" + gz_topic_name: "/camera/internal/right_eye/jpg" ros_type_name: "sensor_msgs/msg/Image" gz_type_name: "gz.msgs.Image" direction: GZ_TO_ROS diff --git a/launch/gazebo.launch.py b/launch/gazebo.launch.py index a9ac1c6..95261a5 100644 --- a/launch/gazebo.launch.py +++ b/launch/gazebo.launch.py @@ -85,28 +85,12 @@ def _active_generated_files(pkg_share: str) -> dict[str, str]: def _default_controllers_yaml(pkg_share: str, controllers_basename: str) -> str: - cwd_candidate = ( - Path.cwd() / "src" / "thais_urdf" / "config" / controllers_basename - ) + cwd_candidate = Path.cwd() / "src" / "thais_urdf" / "config" / controllers_basename if cwd_candidate.is_file(): return str(cwd_candidate.resolve()) return os.path.join(pkg_share, "config", controllers_basename) -def _gazebo_camera_raw_topic(topic: str) -> str: - """ - Raw-image topic the gz camera/bridge use in sim (mirrors lucy_config_generator). - - gz-sim + ros_gz_bridge only emit raw ``sensor_msgs/msg/Image``; the LCP consumes - JPEG ``CompressedImage`` on ``topic``. The gz camera renders to this raw topic and - we republish it (raw -> compressed) onto ``topic``. - """ - suffix = "/compressed" - if topic.endswith(suffix): - return topic[: -len(suffix)] - return topic + "/raw" - - def _sim_camera_topics(pkg_share: str) -> list[str]: """LCP-facing (compressed) topics for every camera simulated in Gazebo.""" candidates = [ @@ -145,7 +129,9 @@ def generate_launch_description(): pkg_share = get_package_share_directory("thais_urdf") default_base = os.path.join(pkg_share, "description") generated = _active_generated_files(pkg_share) - default_controllers = _default_controllers_yaml(pkg_share, generated["controllers_yaml"]) + default_controllers = _default_controllers_yaml( + pkg_share, generated["controllers_yaml"] + ) base_path_arg = DeclareLaunchArgument( "base_path", @@ -178,7 +164,10 @@ def generate_launch_description(): ) bridge_config_path = os.path.join( - get_package_share_directory("thais_urdf"), "description", "gazebo", "gazebo_bridge.yaml" + get_package_share_directory("thais_urdf"), + "description", + "gazebo", + "gazebo_bridge.yaml", ) bridge = Node( package="ros_gz_bridge", @@ -191,19 +180,24 @@ def generate_launch_description(): # ros_gz_bridge can only emit raw Image; the LCP wants JPEG CompressedImage. # For every simulated camera (robot-mounted + external world camera), republish # the bridged raw topic -> the compressed topic the LCP subscribes to. - def _camera_compressor(compressed_topic: str) -> Node: - raw_topic = _gazebo_camera_raw_topic(compressed_topic) + def _camera_compressor(raw_topic: str) -> Node: + compressed_topic = raw_topic + "/compressed" safe = "".join(c if c.isalnum() else "_" for c in compressed_topic).strip("_") return Node( package="image_transport", executable="republish", name="camera_compressor_" + safe, - arguments=["raw", "compressed"], remappings=[ ("in", raw_topic), ("out/compressed", compressed_topic), ], - parameters=[{"use_sim_time": True}], + parameters=[ + { + "use_sim_time": True, + "in_transport": "raw", + "out_transport": "compressed", + } + ], output="screen", ) From 6e92fbe4364a1edb13f46138042e21f8f3a9ea2d Mon Sep 17 00:00:00 2001 From: m-brl <103381146+m-brl@users.noreply.github.com> Date: Thu, 11 Jun 2026 11:34:18 +0200 Subject: [PATCH 05/10] feat(republish): add node depending on version Signed-off-by: m-brl <103381146+m-brl@users.noreply.github.com> --- launch/gazebo.launch.py | 49 +++++++++++++++++++++++++++-------------- 1 file changed, 32 insertions(+), 17 deletions(-) diff --git a/launch/gazebo.launch.py b/launch/gazebo.launch.py index 95261a5..180b341 100644 --- a/launch/gazebo.launch.py +++ b/launch/gazebo.launch.py @@ -126,6 +126,7 @@ def _sim_camera_topics(pkg_share: str) -> list[str]: def generate_launch_description(): + ros_distro = os.environ.get("ROS_DISTRO", "humble").lower() pkg_share = get_package_share_directory("thais_urdf") default_base = os.path.join(pkg_share, "description") generated = _active_generated_files(pkg_share) @@ -183,23 +184,37 @@ def generate_launch_description(): def _camera_compressor(raw_topic: str) -> Node: compressed_topic = raw_topic + "/compressed" safe = "".join(c if c.isalnum() else "_" for c in compressed_topic).strip("_") - return Node( - package="image_transport", - executable="republish", - name="camera_compressor_" + safe, - remappings=[ - ("in", raw_topic), - ("out/compressed", compressed_topic), - ], - parameters=[ - { - "use_sim_time": True, - "in_transport": "raw", - "out_transport": "compressed", - } - ], - output="screen", - ) + if ros_distro == "humble": + return Node( + package="image_transport", + executable="republish", + name="camera_compressor_" + safe, + arguments=["raw", "compressed"], + remappings=[ + ("in", raw_topic), + ("out/compressed", compressed_topic), + ], + parameters=[{"use_sim_time": True}], + output="screen", + ) + else: + return Node( + package="image_transport", + executable="republish", + name="camera_compressor_" + safe, + remappings=[ + ("in", raw_topic), + ("out/compressed", compressed_topic), + ], + parameters=[ + { + "use_sim_time": True, + "in_transport": "raw", + "out_transport": "compressed", + } + ], + output="screen", + ) camera_compressors = [ _camera_compressor(topic) for topic in _sim_camera_topics(pkg_share) From 0ffb39aa3296c1ae5ba498cb1e758921cae5ed5b Mon Sep 17 00:00:00 2001 From: m-brl <103381146+m-brl@users.noreply.github.com> Date: Thu, 11 Jun 2026 15:59:22 +0200 Subject: [PATCH 06/10] feat(compressed topic): add defined compressed topic Signed-off-by: m-brl <103381146+m-brl@users.noreply.github.com> --- config/hardware/active.yaml | 5 +++++ config/hardware/configs/default.yaml | 5 +++++ launch/gazebo.launch.py | 25 ++++++++++++++----------- 3 files changed, 24 insertions(+), 11 deletions(-) diff --git a/config/hardware/active.yaml b/config/hardware/active.yaml index ae41ac7..d95810b 100644 --- a/config/hardware/active.yaml +++ b/config/hardware/active.yaml @@ -48,6 +48,7 @@ cameras: - id: 'ext_camera' name: 'External USB Webcam' topic: '/ext_camera/jpg' + compressed_topic: '/ext_camera/jpg/compressed' message_type: 'sensor_msgs/msg/CompressedImage' external: true sim_gz_topic: '/world/default/camera' @@ -55,6 +56,7 @@ cameras: - realsense-rgb: name: 'Realsense RGB' topic: '/realsense/realsense2_camera/color/image_raw' + compressed_topic: '/realsense/realsense2_camera/color/image_raw/compressed' message_type: 'sensor_msgs/msg/CompressedImage' external: false link: 'i01.head.eyeLeft_link' @@ -62,6 +64,7 @@ cameras: - realsense-aligned-depth: name: 'Realsense Aligned Depth' topic: '/realsense/realsense2_camera/aligned_depth_to_color/image_raw' + compressed_topic: '/realsense/realsense2_camera/aligned_depth_to_color/image_raw/compressed' message_type: 'sensor_msgs/msg/CompressedImage' external: false link: 'i01.head.eyeLeft_link' @@ -69,6 +72,7 @@ cameras: - left_eye_camera: name: 'Left eye webcam' topic: '/camera/internal/left_eye/jpg' + compressed_topic: '/camera/internal/left_eye/jpg/compressed' message_type: 'sensor_msgs/msg/CompressedImage' external: false link: 'i01.head.eyeRight_link' @@ -76,6 +80,7 @@ cameras: - right_eye_camera: name: 'Right eye webcam' topic: '/camera/internal/right_eye/jpg' + compressed_topic: '/camera/internal/right_eye/jpg/compressed' message_type: 'sensor_msgs/msg/CompressedImage' external: false link: 'i01.head.eyeLeft_link' diff --git a/config/hardware/configs/default.yaml b/config/hardware/configs/default.yaml index ae41ac7..d95810b 100644 --- a/config/hardware/configs/default.yaml +++ b/config/hardware/configs/default.yaml @@ -48,6 +48,7 @@ cameras: - id: 'ext_camera' name: 'External USB Webcam' topic: '/ext_camera/jpg' + compressed_topic: '/ext_camera/jpg/compressed' message_type: 'sensor_msgs/msg/CompressedImage' external: true sim_gz_topic: '/world/default/camera' @@ -55,6 +56,7 @@ cameras: - realsense-rgb: name: 'Realsense RGB' topic: '/realsense/realsense2_camera/color/image_raw' + compressed_topic: '/realsense/realsense2_camera/color/image_raw/compressed' message_type: 'sensor_msgs/msg/CompressedImage' external: false link: 'i01.head.eyeLeft_link' @@ -62,6 +64,7 @@ cameras: - realsense-aligned-depth: name: 'Realsense Aligned Depth' topic: '/realsense/realsense2_camera/aligned_depth_to_color/image_raw' + compressed_topic: '/realsense/realsense2_camera/aligned_depth_to_color/image_raw/compressed' message_type: 'sensor_msgs/msg/CompressedImage' external: false link: 'i01.head.eyeLeft_link' @@ -69,6 +72,7 @@ cameras: - left_eye_camera: name: 'Left eye webcam' topic: '/camera/internal/left_eye/jpg' + compressed_topic: '/camera/internal/left_eye/jpg/compressed' message_type: 'sensor_msgs/msg/CompressedImage' external: false link: 'i01.head.eyeRight_link' @@ -76,6 +80,7 @@ cameras: - right_eye_camera: name: 'Right eye webcam' topic: '/camera/internal/right_eye/jpg' + compressed_topic: '/camera/internal/right_eye/jpg/compressed' message_type: 'sensor_msgs/msg/CompressedImage' external: false link: 'i01.head.eyeLeft_link' diff --git a/launch/gazebo.launch.py b/launch/gazebo.launch.py index 180b341..79df8cf 100644 --- a/launch/gazebo.launch.py +++ b/launch/gazebo.launch.py @@ -24,9 +24,8 @@ import os from pathlib import Path -from ament_index_python.packages import get_package_share_directory import yaml -from launch import LaunchDescription +from ament_index_python.packages import get_package_prefix, get_package_share_directory from launch.actions import ( DeclareLaunchArgument, IncludeLaunchDescription, @@ -41,7 +40,8 @@ ) from launch_ros.actions import Node from lucy_control_supervisor.controllers_spawn import controllers_to_spawn -from ament_index_python.packages import get_package_prefix + +from launch import LaunchDescription def _gz_ros2_control_plugin_path(): @@ -91,7 +91,7 @@ def _default_controllers_yaml(pkg_share: str, controllers_basename: str) -> str: return os.path.join(pkg_share, "config", controllers_basename) -def _sim_camera_topics(pkg_share: str) -> list[str]: +def _sim_camera_topics(pkg_share: str) -> list[tuple[str, str]]: """LCP-facing (compressed) topics for every camera simulated in Gazebo.""" candidates = [ Path.cwd() / "src" / "thais_urdf" / "config" / "hardware" / "active.yaml", @@ -107,20 +107,23 @@ def _sim_camera_topics(pkg_share: str) -> list[str]: cameras = data.get("cameras") if isinstance(data, dict) else None if not isinstance(cameras, list): return [] - topics: list[str] = [] + topics: list[tuple[str, str]] = [] for cam in cameras: if not isinstance(cam, dict): continue topic = cam.get("topic") if not isinstance(topic, str) or not topic.strip(): continue + compressed_topic = cam.get("compressed_topic") + if not isinstance(compressed_topic, str) or not compressed_topic.strip(): + continue if cam.get("external"): gz_topic = cam.get("sim_gz_topic") if not isinstance(gz_topic, str) or not gz_topic.strip(): continue elif not cam.get("link"): continue - topics.append(topic.strip()) + topics.append((topic.strip(), compressed_topic.strip())) return topics return [] @@ -181,8 +184,7 @@ def generate_launch_description(): # ros_gz_bridge can only emit raw Image; the LCP wants JPEG CompressedImage. # For every simulated camera (robot-mounted + external world camera), republish # the bridged raw topic -> the compressed topic the LCP subscribes to. - def _camera_compressor(raw_topic: str) -> Node: - compressed_topic = raw_topic + "/compressed" + def _camera_compressor(topic: str, compressed_topic: str) -> Node: safe = "".join(c if c.isalnum() else "_" for c in compressed_topic).strip("_") if ros_distro == "humble": return Node( @@ -191,7 +193,7 @@ def _camera_compressor(raw_topic: str) -> Node: name="camera_compressor_" + safe, arguments=["raw", "compressed"], remappings=[ - ("in", raw_topic), + ("in", topic), ("out/compressed", compressed_topic), ], parameters=[{"use_sim_time": True}], @@ -203,7 +205,7 @@ def _camera_compressor(raw_topic: str) -> Node: executable="republish", name="camera_compressor_" + safe, remappings=[ - ("in", raw_topic), + ("in", topic), ("out/compressed", compressed_topic), ], parameters=[ @@ -217,7 +219,8 @@ def _camera_compressor(raw_topic: str) -> Node: ) camera_compressors = [ - _camera_compressor(topic) for topic in _sim_camera_topics(pkg_share) + _camera_compressor(topic, compressed_topic) + for topic, compressed_topic in _sim_camera_topics(pkg_share) ] ros_lib = _gz_ros2_control_plugin_path() From 2b38ab1ecc802c71463dcceb1163a1eea8f02c90 Mon Sep 17 00:00:00 2001 From: Charles Madjeri <80175305+charlesmadjeri@users.noreply.github.com> Date: Thu, 11 Jun 2026 21:28:57 +0200 Subject: [PATCH 07/10] evol(config): sync hardware config and urdf from inmoov_urdf Signed-off-by: Charles Madjeri <80175305+charlesmadjeri@users.noreply.github.com> --- config/hardware/active.yaml | 34 +++++++------------ config/hardware/configs/default.yaml | 34 +++++++------------ .../urdf/robot_description.urdf.xacro | 10 +++--- .../ros2_control/inmoov_ros2_control.xacro | 22 ++++++------ 4 files changed, 40 insertions(+), 60 deletions(-) diff --git a/config/hardware/active.yaml b/config/hardware/active.yaml index d95810b..ef878d7 100644 --- a/config/hardware/active.yaml +++ b/config/hardware/active.yaml @@ -1,18 +1,8 @@ -# Hardware mapping — single source of truth for lucy_config_generator. -# URDF joint names must match description/robot_description (processed URDF). -# left_shoulder_y_link_joint / right_shoulder_y_link_joint: torso board. - version: 1 robot_name: inmoov - -# Generated-artifact filenames (bare basenames; directories are fixed by repo -# convention: description/ros2_control/ and config/). Consumed by -# lucy_config_generator, lucy_config_pipeline, inmoov.urdf.xacro and the -# thais_urdf launches so every layer agrees on the names. generated_files: ros2_control_xacro: inmoov_ros2_control.xacro controllers_yaml: controllers.yaml - passive_urdf_joints: [] ignore_urdf_joints: - Empty.001_link_joint @@ -386,7 +376,7 @@ actuators: servo_max_deg: 180 servo_default_deg: 0 enabled: false - - id: act_left_arm_9 + - id: torso_stom_mid urdf_joint: i01.torso.midStom_link_joint board: rp2040_torso_head virtual_pin: 2 @@ -399,7 +389,7 @@ actuators: servo_max_deg: 270 servo_default_deg: 135 enabled: false - - id: act_left_arm_10 + - id: torso_stom_top urdf_joint: i01.torso.topStom_link_joint board: rp2040_torso_head virtual_pin: 3 @@ -412,7 +402,7 @@ actuators: servo_max_deg: 180 servo_default_deg: 90 enabled: false - - id: act_left_arm_11 + - id: head_rot urdf_joint: i01.head.rothead_link_joint board: rp2040_torso_head virtual_pin: 4 @@ -425,7 +415,7 @@ actuators: servo_max_deg: 180 servo_default_deg: 90 enabled: false - - id: act_left_arm_12 + - id: head_neck_roll urdf_joint: i01.head.rollNeck_link_joint board: rp2040_torso_head virtual_pin: 5 @@ -438,7 +428,7 @@ actuators: servo_max_deg: 180 servo_default_deg: 90 enabled: false - - id: act_left_arm_13 + - id: head_neck urdf_joint: i01.head.neck.001_link_joint board: rp2040_torso_head virtual_pin: 6 @@ -451,20 +441,20 @@ actuators: servo_max_deg: 180 servo_default_deg: 90 enabled: false - - id: act_left_arm_14 + - id: head_jaw urdf_joint: i01.head.jaw_link_joint board: rp2040_torso_head virtual_pin: 7 physical_pin: 12 servo_type: '300' - offset_deg: 90 + offset_deg: 0 direction: 1 scale: 1 servo_min_deg: 0 servo_max_deg: 180 - servo_default_deg: 90 + servo_default_deg: 0 enabled: false - - id: act_left_arm_15 + - id: head_eye_right_y urdf_joint: i01.head.eyeRight_link_joint board: rp2040_torso_head virtual_pin: 8 @@ -477,7 +467,7 @@ actuators: servo_max_deg: 180 servo_default_deg: 90 enabled: false - - id: act_left_arm_16 + - id: head_eye_right_x urdf_joint: i01.head.eyeRight.001_link_joint board: rp2040_torso_head virtual_pin: 9 @@ -490,7 +480,7 @@ actuators: servo_max_deg: 180 servo_default_deg: 90 enabled: false - - id: act_left_arm_17 + - id: head_eye_left_y urdf_joint: i01.head.eyeLeft_link_joint board: rp2040_torso_head virtual_pin: 10 @@ -503,7 +493,7 @@ actuators: servo_max_deg: 180 servo_default_deg: 90 enabled: false - - id: act_right_arm_9 + - id: head_eye_left_x urdf_joint: i01.head.eyeLeft.001_link_joint board: rp2040_torso_head virtual_pin: 11 diff --git a/config/hardware/configs/default.yaml b/config/hardware/configs/default.yaml index d95810b..ef878d7 100644 --- a/config/hardware/configs/default.yaml +++ b/config/hardware/configs/default.yaml @@ -1,18 +1,8 @@ -# Hardware mapping — single source of truth for lucy_config_generator. -# URDF joint names must match description/robot_description (processed URDF). -# left_shoulder_y_link_joint / right_shoulder_y_link_joint: torso board. - version: 1 robot_name: inmoov - -# Generated-artifact filenames (bare basenames; directories are fixed by repo -# convention: description/ros2_control/ and config/). Consumed by -# lucy_config_generator, lucy_config_pipeline, inmoov.urdf.xacro and the -# thais_urdf launches so every layer agrees on the names. generated_files: ros2_control_xacro: inmoov_ros2_control.xacro controllers_yaml: controllers.yaml - passive_urdf_joints: [] ignore_urdf_joints: - Empty.001_link_joint @@ -386,7 +376,7 @@ actuators: servo_max_deg: 180 servo_default_deg: 0 enabled: false - - id: act_left_arm_9 + - id: torso_stom_mid urdf_joint: i01.torso.midStom_link_joint board: rp2040_torso_head virtual_pin: 2 @@ -399,7 +389,7 @@ actuators: servo_max_deg: 270 servo_default_deg: 135 enabled: false - - id: act_left_arm_10 + - id: torso_stom_top urdf_joint: i01.torso.topStom_link_joint board: rp2040_torso_head virtual_pin: 3 @@ -412,7 +402,7 @@ actuators: servo_max_deg: 180 servo_default_deg: 90 enabled: false - - id: act_left_arm_11 + - id: head_rot urdf_joint: i01.head.rothead_link_joint board: rp2040_torso_head virtual_pin: 4 @@ -425,7 +415,7 @@ actuators: servo_max_deg: 180 servo_default_deg: 90 enabled: false - - id: act_left_arm_12 + - id: head_neck_roll urdf_joint: i01.head.rollNeck_link_joint board: rp2040_torso_head virtual_pin: 5 @@ -438,7 +428,7 @@ actuators: servo_max_deg: 180 servo_default_deg: 90 enabled: false - - id: act_left_arm_13 + - id: head_neck urdf_joint: i01.head.neck.001_link_joint board: rp2040_torso_head virtual_pin: 6 @@ -451,20 +441,20 @@ actuators: servo_max_deg: 180 servo_default_deg: 90 enabled: false - - id: act_left_arm_14 + - id: head_jaw urdf_joint: i01.head.jaw_link_joint board: rp2040_torso_head virtual_pin: 7 physical_pin: 12 servo_type: '300' - offset_deg: 90 + offset_deg: 0 direction: 1 scale: 1 servo_min_deg: 0 servo_max_deg: 180 - servo_default_deg: 90 + servo_default_deg: 0 enabled: false - - id: act_left_arm_15 + - id: head_eye_right_y urdf_joint: i01.head.eyeRight_link_joint board: rp2040_torso_head virtual_pin: 8 @@ -477,7 +467,7 @@ actuators: servo_max_deg: 180 servo_default_deg: 90 enabled: false - - id: act_left_arm_16 + - id: head_eye_right_x urdf_joint: i01.head.eyeRight.001_link_joint board: rp2040_torso_head virtual_pin: 9 @@ -490,7 +480,7 @@ actuators: servo_max_deg: 180 servo_default_deg: 90 enabled: false - - id: act_left_arm_17 + - id: head_eye_left_y urdf_joint: i01.head.eyeLeft_link_joint board: rp2040_torso_head virtual_pin: 10 @@ -503,7 +493,7 @@ actuators: servo_max_deg: 180 servo_default_deg: 90 enabled: false - - id: act_right_arm_9 + - id: head_eye_left_x urdf_joint: i01.head.eyeLeft.001_link_joint board: rp2040_torso_head virtual_pin: 11 diff --git a/description/robot_description/urdf/robot_description.urdf.xacro b/description/robot_description/urdf/robot_description.urdf.xacro index 16c2833..2f4f082 100644 --- a/description/robot_description/urdf/robot_description.urdf.xacro +++ b/description/robot_description/urdf/robot_description.urdf.xacro @@ -102,7 +102,7 @@ - + @@ -190,7 +190,7 @@ - + @@ -278,7 +278,7 @@ - + @@ -366,7 +366,7 @@ - + @@ -438,7 +438,7 @@ - + diff --git a/description/ros2_control/inmoov_ros2_control.xacro b/description/ros2_control/inmoov_ros2_control.xacro index e86cb70..92369b8 100644 --- a/description/ros2_control/inmoov_ros2_control.xacro +++ b/description/ros2_control/inmoov_ros2_control.xacro @@ -348,8 +348,8 @@ 270 135 - -0.50615 - 0.50615 + -1.5708 + 1.5708 @@ -363,8 +363,8 @@ 180 90 - -0.12217 - 0.12217 + -0.2618 + 0.2618 @@ -378,8 +378,8 @@ 180 90 - -0.08727 - 0.04363 + -1.5708 + 1.5708 @@ -393,8 +393,8 @@ 180 90 - -1.5708 - 1.5708 + -0.34907 + 0.34907 @@ -409,19 +409,19 @@ 90 -0.34907 - 0.0 + 0.34907 7 300 - 90 + 0 1 1 0 180 - 90 + 0 0.0 0.17453 From 65ed9691075be963556dc46e626f6a6932c1f622 Mon Sep 17 00:00:00 2001 From: m-brl <103381146+m-brl@users.noreply.github.com> Date: Sat, 13 Jun 2026 13:27:10 +0200 Subject: [PATCH 08/10] fix(color): add unique white color Signed-off-by: m-brl <103381146+m-brl@users.noreply.github.com> --- .../urdf/robot_description.urdf.xacro | 739 +++++++----------- 1 file changed, 290 insertions(+), 449 deletions(-) diff --git a/description/robot_description/urdf/robot_description.urdf.xacro b/description/robot_description/urdf/robot_description.urdf.xacro index 2f4f082..9148ca0 100644 --- a/description/robot_description/urdf/robot_description.urdf.xacro +++ b/description/robot_description/urdf/robot_description.urdf.xacro @@ -533,7 +533,7 @@ - + @@ -562,21 +562,21 @@ - + - + - + @@ -629,49 +629,49 @@ - + - + - + - + - + - + - + @@ -766,98 +766,98 @@ - + - + - + - + - + - + - + - + - + - + - + - + - + - + @@ -994,147 +994,147 @@ - + - + - + - + - + - + - + - + - + - + - + - + - + - + - + - + - + - + - + - + - + @@ -1223,91 +1223,91 @@ - + - + - + - + - + - + - + - + - + - + - + - + - + @@ -1324,7 +1324,7 @@ - + @@ -1341,7 +1341,7 @@ - + @@ -1358,7 +1358,7 @@ - + @@ -1429,70 +1429,70 @@ - + - + - + - + - + - + - + - + - + - + @@ -1513,7 +1513,7 @@ - + @@ -1530,7 +1530,7 @@ - + @@ -1547,7 +1547,7 @@ - + @@ -1564,7 +1564,7 @@ - + @@ -1581,7 +1581,7 @@ - + @@ -1598,7 +1598,7 @@ - + @@ -1615,7 +1615,7 @@ - + @@ -1632,7 +1632,7 @@ - + @@ -1649,7 +1649,7 @@ - + @@ -1666,7 +1666,7 @@ - + @@ -1851,203 +1851,203 @@ - + - + - + - + - + - + - + - + - + - + - + - + - + - + - + - + - + - + - + - + - + - + - + - + - + - + - + - + - + @@ -2068,7 +2068,7 @@ - + @@ -2085,7 +2085,7 @@ - + @@ -2102,7 +2102,7 @@ - + @@ -2119,7 +2119,7 @@ - + @@ -2136,7 +2136,7 @@ - + @@ -2153,7 +2153,7 @@ - + @@ -2206,49 +2206,49 @@ - + - + - + - + - + - + - + @@ -2331,84 +2331,84 @@ - + - + - + - + - + - + - + - + - + - + - + - + @@ -2539,140 +2539,140 @@ - + - + - + - + - + - + - + - + - + - + - + - + - + - + - + - + - + - + - + - + @@ -2761,91 +2761,91 @@ - + - + - + - + - + - + - + - + - + - + - + - + - + @@ -3336,560 +3336,560 @@ - + - + - + - + - + - + - + - + - + - + - + - + - + - + - + - + - + - + - + - + - + - + - + - + - + - + - + - + - + - + - + - + - + - + - + - + - + - + - + - + - + - + - + - + - + - + - + - + - + - + - + - + - + - + - + - + - + - + - + - + - + - + - + - + - + - + - + - + - + - + - + - + - + - + - + - + - + - + - + - + @@ -3910,7 +3910,7 @@ - + @@ -3927,7 +3927,7 @@ - + @@ -3944,7 +3944,7 @@ - + @@ -3961,7 +3961,7 @@ - + @@ -3978,7 +3978,7 @@ - + @@ -3995,7 +3995,7 @@ - + @@ -4012,7 +4012,7 @@ - + @@ -4029,7 +4029,7 @@ - + @@ -4046,7 +4046,7 @@ - + @@ -4063,7 +4063,7 @@ - + @@ -4083,7 +4083,7 @@ - + @@ -4100,7 +4100,7 @@ - + @@ -4117,7 +4117,7 @@ - + @@ -4134,7 +4134,7 @@ - + @@ -4151,7 +4151,7 @@ - + @@ -4168,7 +4168,7 @@ - + @@ -4185,7 +4185,7 @@ - + @@ -4202,7 +4202,7 @@ - + @@ -4219,7 +4219,7 @@ - + @@ -4254,28 +4254,28 @@ - + - + - + - + @@ -4382,112 +4382,112 @@ - + - + - + - + - + - + - + - + - + - + - + - + - + - + - + - + @@ -4507,7 +4507,7 @@ - + @@ -4515,166 +4515,7 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - From 71d32625686877dc01f083885748bf1641619351 Mon Sep 17 00:00:00 2001 From: Mathieu <103381146+m-brl@users.noreply.github.com> Date: Sat, 13 Jun 2026 15:26:00 +0200 Subject: [PATCH 09/10] feat(ci/cd): add release action (#32) Signed-off-by: m-brl <103381146+m-brl@users.noreply.github.com> --- .github/workflows/release.yml | 58 +++++++++++++++++++++++++++++++++++ 1 file changed, 58 insertions(+) create mode 100644 .github/workflows/release.yml diff --git a/.github/workflows/release.yml b/.github/workflows/release.yml new file mode 100644 index 0000000..d4b56ad --- /dev/null +++ b/.github/workflows/release.yml @@ -0,0 +1,58 @@ +name: Create release + +on: + workflow_dispatch: + inputs: + version: + description: 'Version number' + required: true + type: string + +permissions: + contents: write + pull-requests: write + +jobs: + create-release: + runs-on: ubuntu-latest + steps: + - name: Checkout code + uses: actions/checkout@v4 + with: + ref: dev + fetch-depth: 0 + + - name: Configure Git + run: | + git config --global user.name "github-actions[bot]" + git config --global user.email "github-actions[bot]@users.noreply.github.com" + + - name: Create Release Branch + run: | + git checkout -b release/${{ inputs.version }} + + echo "${{ inputs.version }}" > version.txt + + git add version.txt + git commit -m "Migrating to ${{ inputs.version }}" + git push origin release/${{ inputs.version }} + + - name: Create PR to Master + env: + GH_TOKEN: ${{ secrets.GITHUB_TOKEN }} + run: | + gh pr create \ + --title "Release ${{ inputs.version }} -> Master" \ + --body "Merge release ${{ inputs.version }} to master." \ + --base master \ + --head release/${{ inputs.version }} + + - name: Create PR to Develop + env: + GH_TOKEN: ${{ secrets.GITHUB_TOKEN }} + run: | + gh pr create \ + --title "Release ${{ inputs.version }} -> Dev" \ + --body "Merge release ${{ inputs.version }} to dev." \ + --base dev \ + --head release/${{ inputs.version }} From 69b187e2be5c7f4c65253cefd5ca351861f6e765 Mon Sep 17 00:00:00 2001 From: m-brl <103381146+m-brl@users.noreply.github.com> Date: Sun, 14 Jun 2026 08:59:42 +0200 Subject: [PATCH 10/10] evol(release CI): centralise CI release pipeline Signed-off-by: m-brl <103381146+m-brl@users.noreply.github.com> --- .github/workflows/release.yml | 51 +++-------------------------------- 1 file changed, 4 insertions(+), 47 deletions(-) diff --git a/.github/workflows/release.yml b/.github/workflows/release.yml index d4b56ad..5875d91 100644 --- a/.github/workflows/release.yml +++ b/.github/workflows/release.yml @@ -1,4 +1,4 @@ -name: Create release +name: Trigger Release on: workflow_dispatch: @@ -8,51 +8,8 @@ on: required: true type: string -permissions: - contents: write - pull-requests: write - jobs: create-release: - runs-on: ubuntu-latest - steps: - - name: Checkout code - uses: actions/checkout@v4 - with: - ref: dev - fetch-depth: 0 - - - name: Configure Git - run: | - git config --global user.name "github-actions[bot]" - git config --global user.email "github-actions[bot]@users.noreply.github.com" - - - name: Create Release Branch - run: | - git checkout -b release/${{ inputs.version }} - - echo "${{ inputs.version }}" > version.txt - - git add version.txt - git commit -m "Migrating to ${{ inputs.version }}" - git push origin release/${{ inputs.version }} - - - name: Create PR to Master - env: - GH_TOKEN: ${{ secrets.GITHUB_TOKEN }} - run: | - gh pr create \ - --title "Release ${{ inputs.version }} -> Master" \ - --body "Merge release ${{ inputs.version }} to master." \ - --base master \ - --head release/${{ inputs.version }} - - - name: Create PR to Develop - env: - GH_TOKEN: ${{ secrets.GITHUB_TOKEN }} - run: | - gh pr create \ - --title "Release ${{ inputs.version }} -> Dev" \ - --body "Merge release ${{ inputs.version }} to dev." \ - --base dev \ - --head release/${{ inputs.version }} + uses: Sentience-Robotics/.github/.github/workflows/release.yaml@master + with: + version: ${{ inputs.version }}