From 7ca5b90f9b5ea77cb2a9d6ff7ae81058e4df142e Mon Sep 17 00:00:00 2001 From: Mike Lanighan Date: Thu, 5 Feb 2026 17:35:42 +0000 Subject: [PATCH 1/5] Adds pan tilt cameras to sim model, creates bridges for cameras, updates candarm2 model to have more realistic physics properties, adds positionable sun in world with script to change zenith angle, ensure sim time used in gz contollers, updates move example to use action servers, and adds test example launch arg (default false) --- CMakeLists.txt | 4 +- config/mobile_servicing_system_control.yaml | 60 +++- .../spawn_mobile_servicing_system.launch.py | 84 ++++- robots/mobile_servicing_system.urdf.xacro | 12 +- rviz/iss.rviz | 336 +++++++++++++++++- scripts/camera_info_frame_remapper.py | 105 ++++++ scripts/move_mss | 194 +++++----- scripts/move_sun.py | 63 ++++ urdf/canadarm2.urdf.xacro | 139 ++++---- urdf/dextre.urdf.xacro | 77 ++-- urdf/etvcg_cameras.urdf.xacro | 76 ++++ urdf/mobile_servicing_system.gazebo.xacro | 54 +-- urdf/pt_camera.gazebo.xacro | 111 ++++++ urdf/pt_camera.urdf.xacro | 159 +++++++++ worlds/leo.sdf | 81 ++--- 15 files changed, 1238 insertions(+), 317 deletions(-) create mode 100755 scripts/camera_info_frame_remapper.py create mode 100644 scripts/move_sun.py create mode 100644 urdf/etvcg_cameras.urdf.xacro create mode 100644 urdf/pt_camera.gazebo.xacro create mode 100644 urdf/pt_camera.urdf.xacro diff --git a/CMakeLists.txt b/CMakeLists.txt index 5c50555..1a809c4 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -34,6 +34,8 @@ endforeach() install(PROGRAMS scripts/move_mss + scripts/move_sun.py + scripts/camera_info_frame_remapper.py DESTINATION lib/${PROJECT_NAME} ) @@ -42,7 +44,7 @@ install(PROGRAMS #) install( - DIRECTORY + DIRECTORY launch rviz meshes robots urdf config worlds DESTINATION share/${PROJECT_NAME} ) diff --git a/config/mobile_servicing_system_control.yaml b/config/mobile_servicing_system_control.yaml index 902975a..3d12c6a 100644 --- a/config/mobile_servicing_system_control.yaml +++ b/config/mobile_servicing_system_control.yaml @@ -29,6 +29,9 @@ starboard_bga_joint_trajectory_controller: type: joint_trajectory_controller/JointTrajectoryController + camera_joint_trajectory_controller: + type: joint_trajectory_controller/JointTrajectoryController + /dextre_body_joint_trajectory_controller: ros__parameters: @@ -76,7 +79,7 @@ - joint_canadarm2_4 - joint_canadarm2_5 - joint_canadarm2_6 - - joint_canadarm2_7 + - joint_canadarm2_7 interface_name: position command_interfaces: - position @@ -89,7 +92,7 @@ constraints: stopped_velocity_tolerance: 0.1 goal_time: 0.0 - + /dextre_arm_1_joint_trajectory_controller: ros__parameters: allow_nonzero_velocity_at_trajectory_end: false @@ -112,7 +115,7 @@ constraints: stopped_velocity_tolerance: 0.1 goal_time: 0.0 - + /dextre_arm_2_joint_trajectory_controller: ros__parameters: allow_nonzero_velocity_at_trajectory_end: false @@ -134,8 +137,8 @@ allow_partial_joints_goal: false constraints: stopped_velocity_tolerance: 0.1 - goal_time: 0.0 - + goal_time: 0.0 + /sarj_joint_trajectory_controller: ros__parameters: allow_nonzero_velocity_at_trajectory_end: false @@ -153,8 +156,8 @@ allow_partial_joints_goal: false constraints: stopped_velocity_tolerance: 0.1 - goal_time: 0.0 - + goal_time: 0.0 + /port_bga_joint_trajectory_controller: ros__parameters: allow_nonzero_velocity_at_trajectory_end: false @@ -162,7 +165,7 @@ - joint_port_bga_1 - joint_port_bga_2 - joint_port_bga_3 - - joint_port_bga_4 + - joint_port_bga_4 interface_name: position command_interfaces: - position @@ -174,7 +177,7 @@ allow_partial_joints_goal: false constraints: stopped_velocity_tolerance: 0.1 - goal_time: 0.0 + goal_time: 0.0 /starboard_bga_joint_trajectory_controller: ros__parameters: @@ -183,7 +186,7 @@ - joint_starboard_bga_1 - joint_starboard_bga_2 - joint_starboard_bga_3 - - joint_starboard_bga_4 + - joint_starboard_bga_4 interface_name: position command_interfaces: - position @@ -195,5 +198,38 @@ allow_partial_joints_goal: false constraints: stopped_velocity_tolerance: 0.1 - goal_time: 0.0 - + goal_time: 0.0 + +/camera_joint_trajectory_controller: + ros__parameters: + allow_nonzero_velocity_at_trajectory_end: false + joints: + - boom_a_clpa_pan_joint + - boom_a_clpa_tilt_joint + - boom_b_clpa_pan_joint + - boom_b_clpa_tilt_joint + - outrigger_1_clpa_pan_joint + - outrigger_1_clpa_tilt_joint + - outrigger_2_clpa_pan_joint + - outrigger_2_clpa_tilt_joint + - etvcg_cp3_pan_joint + - etvcg_cp3_tilt_joint + - etvcg_cp8_pan_joint + - etvcg_cp8_tilt_joint + - etvcg_cp9_pan_joint + - etvcg_cp9_tilt_joint + - etvcg_cp13_pan_joint + - etvcg_cp13_tilt_joint + interface_name: position + command_interfaces: + - position + state_interfaces: + - position + - velocity + state_publish_rate: 100.0 + action_monitor_rate: 20.0 + allow_partial_joints_goal: true + constraints: + stopped_velocity_tolerance: 0.1 + goal_time: 0.0 + diff --git a/launch/spawn_mobile_servicing_system.launch.py b/launch/spawn_mobile_servicing_system.launch.py index 02cc27a..94f57ff 100644 --- a/launch/spawn_mobile_servicing_system.launch.py +++ b/launch/spawn_mobile_servicing_system.launch.py @@ -14,9 +14,12 @@ def generate_launch_description(): launch_args = [ - DeclareLaunchArgument(name="move_demo", default_value="True"), + DeclareLaunchArgument(name="move_demo", default_value="False"), ] + # Common parameters for all nodes + sim_time_params = [{"use_sim_time": True}] + # URDF mss_urdf = os.path.join(get_package_share_directory("iss_description"), "robots", "mobile_servicing_system.urdf.xacro") mss_doc = xacro.process_file(mss_urdf) @@ -53,6 +56,7 @@ def generate_launch_description(): package="iss_description", executable="move_mss", output="screen", + parameters=sim_time_params, # Ensure this uses sim time too condition=IfCondition(LaunchConfiguration('move_demo')) ) @@ -63,6 +67,7 @@ def generate_launch_description(): arguments=["joint_state_broadcaster", "--controller-manager", "/controller_manager", "--switch-timeout", "100.0"], name="start_joint_state_broadcaster", output="screen", + parameters=sim_time_params, # ) canadarm2_joint_controller_spawner = Node( @@ -71,6 +76,7 @@ def generate_launch_description(): arguments=["canadarm2_joint_trajectory_controller", "-c", "/controller_manager", "--switch-timeout", "100.0"], name="start_canadarm2_joint_trajectory_controller", output="screen", + parameters=sim_time_params, ) dextre_arm_1_joint_controller_spawner = Node( @@ -79,6 +85,7 @@ def generate_launch_description(): arguments=["dextre_arm_1_joint_trajectory_controller", "-c", "/controller_manager", "--switch-timeout", "100.0"], name="start_dextre_arm_1_joint_trajectory_controller", output="screen", + parameters=sim_time_params, ) dextre_arm_2_joint_controller_spawner = Node( @@ -87,6 +94,7 @@ def generate_launch_description(): arguments=["dextre_arm_2_joint_trajectory_controller", "-c", "/controller_manager", "--switch-timeout", "100.0"], name="start_dextre_arm_2_joint_trajectory_controller", output="screen", + parameters=sim_time_params, ) mobile_base_system_joint_controller_spawner = Node( @@ -95,6 +103,7 @@ def generate_launch_description(): arguments=["mobile_base_system_joint_trajectory_controller", "-c", "/controller_manager", "--switch-timeout", "100.0"], name="start_mobile_base_system_joint_trajectory_controller", output="screen", + parameters=sim_time_params, ) dextre_body_joint_controller_spawner = Node( @@ -103,6 +112,7 @@ def generate_launch_description(): arguments=["dextre_body_joint_trajectory_controller", "-c", "/controller_manager", "--switch-timeout", "100.0"], name="start_dextre_body_joint_trajectory_controller", output="screen", + parameters=sim_time_params, ) sarj_joint_controller_spawner = Node( @@ -111,6 +121,7 @@ def generate_launch_description(): arguments=["sarj_joint_trajectory_controller", "-c", "/controller_manager", "--switch-timeout", "100.0"], name="start_sarj_joint_trajectory_controller", output="screen", + parameters=sim_time_params, ) port_bga_joint_controller_spawner = Node( @@ -119,6 +130,7 @@ def generate_launch_description(): arguments=["port_bga_joint_trajectory_controller", "-c", "/controller_manager", "--switch-timeout", "100.0"], name="start_port_bga_joint_trajectory_controller", output="screen", + parameters=sim_time_params, ) starboard_bga_joint_controller_spawner = Node( @@ -127,21 +139,72 @@ def generate_launch_description(): arguments=["starboard_bga_joint_trajectory_controller", "-c", "/controller_manager", "--switch-timeout", "100.0"], name="start_starboard_bga_joint_trajectory_controller", output="screen", + parameters=sim_time_params, ) + camera_joint_controller_spawner = Node( + package="controller_manager", + executable="spawner", + arguments=["camera_joint_trajectory_controller", "-c", "/controller_manager", "--switch-timeout", "100.0"], + name="start_camera_joint_trajectory_controller", + output="screen", + parameters=sim_time_params, + ) + + # Bridge nodes for pan-tilt cameras + camera_names = [ + "boom_a_clpa", + "boom_b_clpa", + "outrigger_1_clpa", + "outrigger_2_clpa", + "etvcg_cp3", + "etvcg_cp8", + "etvcg_cp9", + "etvcg_cp13" + ] + + camera_bridges = [] + for camera_name in camera_names: + # Bridge image_raw topic to temporary topic (frame_id will be wrong) + camera_bridges.append( + Node( + package="ros_gz_image", + executable="image_bridge", + arguments=[f"/{camera_name}/image_raw"], + output="screen", + name=f"{camera_name}_image_bridge", + remappings=[(f"/{camera_name}/image_raw", f"/{camera_name}/image_raw_gz")], + parameters=sim_time_params, # Ensure bridges use sim time + ) + ) + # Bridge camera_info topic to temporary topic (frame_id will be wrong) + camera_bridges.append( + Node( + package="ros_gz_bridge", + executable="parameter_bridge", + arguments=[f"/{camera_name}/camera_info@sensor_msgs/msg/CameraInfo[gz.msgs.CameraInfo"], + output="screen", + name=f"{camera_name}_camera_info_bridge", + remappings=[(f"/{camera_name}/camera_info", f"/{camera_name}/camera_info_gz")], + parameters=sim_time_params, + ) + ) - image_bridge = Node( - package="ros_gz_image", - executable="image_bridge", - arguments=["/image_raw", "/image_raw"], - output="screen" + # Frame ID remapper node to fix camera_info and image_raw frame_id + camera_frame_remapper = Node( + package="iss_description", + executable="camera_info_frame_remapper.py", + output="screen", + name="camera_frame_remapper", + parameters=sim_time_params, ) return LaunchDescription( launch_args + [ mss_robot_state_publisher, mss_spawn, mss_move, - image_bridge, + camera_frame_remapper, + ] + camera_bridges + [ RegisterEventHandler( OnProcessExit( target_action=mss_spawn, @@ -151,14 +214,15 @@ def generate_launch_description(): RegisterEventHandler( OnProcessExit( target_action=joint_state_broadcaster_spawner, - on_exit=[canadarm2_joint_controller_spawner, + on_exit=[canadarm2_joint_controller_spawner, dextre_arm_1_joint_controller_spawner, dextre_arm_2_joint_controller_spawner, mobile_base_system_joint_controller_spawner, dextre_body_joint_controller_spawner, sarj_joint_controller_spawner, starboard_bga_joint_controller_spawner, - port_bga_joint_controller_spawner], + port_bga_joint_controller_spawner, + camera_joint_controller_spawner], ) ) - ]) + ]) \ No newline at end of file diff --git a/robots/mobile_servicing_system.urdf.xacro b/robots/mobile_servicing_system.urdf.xacro index 91e4386..5948e43 100644 --- a/robots/mobile_servicing_system.urdf.xacro +++ b/robots/mobile_servicing_system.urdf.xacro @@ -5,7 +5,8 @@ - + + @@ -13,12 +14,15 @@ - + - + - + + + + diff --git a/rviz/iss.rviz b/rviz/iss.rviz index 71de629..fc78877 100644 --- a/rviz/iss.rviz +++ b/rviz/iss.rviz @@ -6,8 +6,10 @@ Panels: Expanded: - /Global Options1 - /Status1 + - /Camera1 + - /CameraInfo1 Splitter Ratio: 0.5 - Tree Height: 746 + Tree Height: 752 - Class: rviz_common/Selection Name: Selection - Class: rviz_common/Tool Properties @@ -65,7 +67,48 @@ Visualization Manager: Expand Link Details: false Expand Tree: false Link Tree Style: Links in Alphabetic Order - body: + boom_a_clpa_base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + boom_a_clpa_camera_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + boom_a_clpa_optical_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + boom_a_clpa_pan_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + boom_a_clpa_tilt_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + boom_b_clpa_base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + boom_b_clpa_camera_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + boom_b_clpa_optical_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + boom_b_clpa_pan_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + boom_b_clpa_tilt_link: Alpha: 1 Show Axes: false Show Trail: false @@ -190,10 +233,116 @@ Visualization Manager: Show Axes: false Show Trail: false Value: true + etvcg_cp13_base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + etvcg_cp13_camera_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + etvcg_cp13_optical_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + etvcg_cp13_pan_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + etvcg_cp13_tilt_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + etvcg_cp3_base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + etvcg_cp3_camera_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + etvcg_cp3_optical_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + etvcg_cp3_pan_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + etvcg_cp3_tilt_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + etvcg_cp8_base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + etvcg_cp8_camera_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + etvcg_cp8_optical_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + etvcg_cp8_pan_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + etvcg_cp8_tilt_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + etvcg_cp9_base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + etvcg_cp9_camera_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + etvcg_cp9_optical_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + etvcg_cp9_pan_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + etvcg_cp9_tilt_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true link_02_unity_node: Alpha: 1 Show Axes: false Show Trail: false + link_08_p6_truss: + Alpha: 1 + Show Axes: false + Show Trail: false + link_08_p6_truss_01: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link_08_p6_truss_02: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true link_09_destiny: Alpha: 1 Show Axes: false @@ -202,11 +351,125 @@ Visualization Manager: Alpha: 1 Show Axes: false Show Trail: false + link_16_s1_truss: + Alpha: 1 + Show Axes: false + Show Trail: false + link_17_p1_truss: + Alpha: 1 + Show Axes: false + Show Trail: false + link_19_p3_truss: + Alpha: 1 + Show Axes: false + Show Trail: false + link_20_p4_truss: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link_20_p4_truss_01: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link_20_p4_truss_02: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link_21_p5_truss: + Alpha: 1 + Show Axes: false + Show Trail: false + link_22_s3_truss: + Alpha: 1 + Show Axes: false + Show Trail: false + link_23_s4_truss: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link_23_s4_truss_01: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link_23_s4_truss_02: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link_24_s5_truss: + Alpha: 1 + Show Axes: false + Show Trail: false + link_32_s6_truss: + Alpha: 1 + Show Axes: false + Show Trail: false + link_32_s6_truss_01: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link_32_s6_truss_02: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true mobile_base_system: Alpha: 1 Show Axes: false Show Trail: false Value: true + outrigger_1_clpa_base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + outrigger_1_clpa_camera_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + outrigger_1_clpa_optical_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + outrigger_1_clpa_pan_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + outrigger_1_clpa_tilt_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + outrigger_2_clpa_base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + outrigger_2_clpa_camera_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + outrigger_2_clpa_optical_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + outrigger_2_clpa_pan_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + outrigger_2_clpa_tilt_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true pdgf: Alpha: 1 Show Axes: false @@ -215,6 +478,15 @@ Visualization Manager: Alpha: 1 Show Axes: false Show Trail: false + root: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + world: + Alpha: 1 + Show Axes: false + Show Trail: false Mass Properties: Inertia: false Mass: false @@ -237,6 +509,44 @@ Visualization Manager: Radius: 0.05000000074505806 Reference Frame: dextre_arm_2_shoulder_roll Value: true + - Class: rviz_default_plugins/Camera + Enabled: false + Far Plane Distance: 100 + Image Rendering: background and overlay + Name: Camera + Overlay Alpha: 0.5 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /boom_a_clpa/image_raw + Value: false + Visibility: + Axes: true + CameraInfo: true + Grid: true + RobotModel: true + Value: true + Zoom Factor: 1 + - Alpha: 0.5 + Class: rviz_default_plugins/CameraInfo + Color: 85; 255; 255 + Edge color: 85; 255; 255 + Enabled: true + Far clip: 1 + Name: CameraInfo + Not show side polygons: true + Show edges: true + Show polygons: true + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /boom_a_clpa/camera_info + Value: true Enabled: true Global Options: Background Color: 0; 0; 0 @@ -283,33 +593,35 @@ Visualization Manager: Views: Current: Class: rviz_default_plugins/Orbit - Distance: 97.93163299560547 + Distance: 7.807682037353516 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 Swap Stereo Eyes: false Value: false Focal Point: - X: -6.481820583343506 - Y: -4.4895853996276855 - Z: 11.229458808898926 + X: -8.9642915725708 + Y: -9.175496101379395 + Z: 7.597785949707031 Focal Shape Fixed Size: true Focal Shape Size: 0.05000000074505806 Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 - Pitch: 0.5647977590560913 + Pitch: 0.22479797899723053 Target Frame: Value: Orbit (rviz) - Yaw: 4.506348133087158 + Yaw: 3.3663418292999268 Saved: ~ Window Geometry: - Displays: + Camera: collapsed: false + Displays: + collapsed: true Height: 1043 - Hide Left Dock: false + Hide Left Dock: true Hide Right Dock: true - QMainWindow State: 000000ff00000000fd00000004000000000000015600000375fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d00000375000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000375fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d00000375000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000073e0000003efc0100000002fb0000000800540069006d006501000000000000073e0000026f00fffffffb0000000800540069006d00650100000000000004500000000000000000000005e20000037500000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + QMainWindow State: 000000ff00000000fd00000004000000000000015600000379fc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073000000003b00000379000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000c00430061006d006500720061020000008500000118000002a90000021b000000010000010f00000375fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d00000375000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000073e0000003efc0100000002fb0000000800540069006d006501000000000000073e0000025300fffffffb0000000800540069006d006501000000000000045000000000000000000000073e0000037900000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Selection: collapsed: false Time: @@ -320,4 +632,4 @@ Window Geometry: collapsed: true Width: 1854 X: 66 - Y: 0 + Y: 32 diff --git a/scripts/camera_info_frame_remapper.py b/scripts/camera_info_frame_remapper.py new file mode 100755 index 0000000..24b95bd --- /dev/null +++ b/scripts/camera_info_frame_remapper.py @@ -0,0 +1,105 @@ +#!/usr/bin/env python3 +""" +Camera Frame ID Remapper + +Subscribes to camera_info and image_raw topics from Gazebo (which use scoped frame names), +remaps the frame_id to match the TF tree frame names, and republishes. + +This is necessary because Gazebo Harmonic uses scoped frame names like: + 'mobile_servicing_system/boom_a_clpa_tilt_link/boom_a_clpa_camera' +But the TF tree uses simple names like: + 'boom_a_clpa_optical_frame' +""" + +import rclpy +from rclpy.node import Node +from sensor_msgs.msg import CameraInfo, Image + + +class CameraInfoFrameRemapper(Node): + def __init__(self): + super().__init__('camera_frame_remapper') + + # Camera names to remap + self.camera_names = [ + 'boom_a_clpa', + 'boom_b_clpa', + 'outrigger_1_clpa', + 'outrigger_2_clpa', + 'etvcg_cp3', + 'etvcg_cp8', + 'etvcg_cp9', + 'etvcg_cp13' + ] + + # Create subscribers and publishers for each camera + self.info_subs = {} + self.info_pubs = {} + self.image_subs = {} + self.image_pubs = {} + + for camera_name in self.camera_names: + # Subscribe to the bridged camera_info (with wrong frame_id) + self.info_subs[camera_name] = self.create_subscription( + CameraInfo, + f'/{camera_name}/camera_info_gz', # Renamed topic from bridge + lambda msg, name=camera_name: self.camera_info_callback(msg, name), + 10 + ) + + # Publish corrected camera_info + self.info_pubs[camera_name] = self.create_publisher( + CameraInfo, + f'/{camera_name}/camera_info', + 10 + ) + + # Subscribe to the bridged image_raw (with wrong frame_id) + self.image_subs[camera_name] = self.create_subscription( + Image, + f'/{camera_name}/image_raw_gz', # Renamed topic from bridge + lambda msg, name=camera_name: self.image_callback(msg, name), + 10 + ) + + # Publish corrected image_raw + self.image_pubs[camera_name] = self.create_publisher( + Image, + f'/{camera_name}/image_raw', + 10 + ) + + self.get_logger().info(f'Camera frame remapper started for {len(self.camera_names)} cameras') + + def camera_info_callback(self, msg: CameraInfo, camera_name: str): + """Remap frame_id and republish""" + # Change frame_id to the optical frame name + msg.header.frame_id = f'{camera_name}_optical_frame' + + # Republish with corrected frame_id + self.info_pubs[camera_name].publish(msg) + + def image_callback(self, msg: Image, camera_name: str): + """Remap frame_id and republish""" + # Change frame_id to the optical frame name + msg.header.frame_id = f'{camera_name}_optical_frame' + + # Republish with corrected frame_id + self.image_pubs[camera_name].publish(msg) + + +def main(args=None): + rclpy.init(args=args) + node = CameraInfoFrameRemapper() + + try: + rclpy.spin(node) + except KeyboardInterrupt: + pass + finally: + node.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() diff --git a/scripts/move_mss b/scripts/move_mss index 02d88cc..926d7cc 100755 --- a/scripts/move_mss +++ b/scripts/move_mss @@ -1,111 +1,101 @@ #!/usr/bin/env python3 +import sys import rclpy +from rclpy.action import ActionClient from rclpy.node import Node from builtin_interfaces.msg import Duration - -from std_msgs.msg import String, Float64 from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint -from std_srvs.srv import Empty - -class MoveArm(Node): - - def __init__(self): - super().__init__('arm_node') - self.canadarm2_publisher_ = self.create_publisher(JointTrajectory, '/canadarm2_joint_trajectory_controller/joint_trajectory', 10) - self.mbs_publisher_ = self.create_publisher(JointTrajectory, '/mobile_base_system_joint_trajectory_controller/joint_trajectory', 10) - - self.canadarm2_pose_0_srv = self.create_service(Empty, 'canadarm2_pose_0', self.canadarm2_pose_0_callback) - self.canadarm2_pose_1_srv = self.create_service(Empty, 'canadarm2_pose_1', self.canadarm2_pose_1_callback) - - self.mbs_pose_0_srv = self.create_service(Empty, 'mbs_pose_0', self.mbs_pose_0_callback) - self.mbs_pose_1_srv = self.create_service(Empty, 'mbs_pose_1', self.mbs_pose_1_callback) - self.mbs_pose_2_srv = self.create_service(Empty, 'mbs_pose_2', self.mbs_pose_1_callback) - - self.canadarm2_joints = ["joint_canadarm2_1", "joint_canadarm2_2", "joint_canadarm2_3", "joint_canadarm2_4", "joint_canadarm2_5", "joint_canadarm2_6", "joint_canadarm2_7"] - - self.mbs_joints = ["joint_mbs"] - - def canadarm2_pose_0_callback(self, request, response): - traj = JointTrajectory() - traj.joint_names = self.canadarm2_joints - - point1 = JointTrajectoryPoint() - point1.positions = [0.0, 0.628, -0.187, 0.0, 0.0, 0.0, 0.0] - point1.time_from_start = Duration(sec=30) - - traj.points.append(point1) - self.canadarm2_publisher_.publish(traj) - - return response - - def canadarm2_pose_1_callback(self, request, response): - traj = JointTrajectory() - traj.joint_names = self.canadarm2_joints - - point1 = JointTrajectoryPoint() - point1.positions = [0.51, 1.48, -0.677, 0.534, -0.062, -0.645, -0.755] - point1.time_from_start = Duration(sec=30) - - traj.points.append(point1) - self.canadarm2_publisher_.publish(traj) - - return response - - def mbs_pose_0_callback(self, request, response): - traj = JointTrajectory() - traj.joint_names = self.mbs_joints - - point1 = JointTrajectoryPoint() - point1.positions = [0.0] - point1.time_from_start = Duration(sec=30) - - traj.points.append(point1) - self.mbs_publisher_.publish(traj) - - return response - - def mbs_pose_1_callback(self, request, response): - traj = JointTrajectory() - traj.joint_names = self.mbs_joints - - point1 = JointTrajectoryPoint() - point1.positions = [8.0] - point1.time_from_start = Duration(sec=30) - - traj.points.append(point1) - self.mbs_publisher_.publish(traj) - - return response - - def mbs_pose_2_callback(self, request, response): - traj = JointTrajectory() - traj.joint_names = self.mbs_joints - - point1 = JointTrajectoryPoint() - point1.positions = [-4.0] - point1.time_from_start = Duration(sec=30) - - traj.points.append(point1) - self.mbs_publisher_.publish(traj) - - return response - - - - -def main(args=None): - rclpy.init(args=args) - - arm_node = MoveArm() - - rclpy.spin(arm_node) - # Destroy the node explicitly - # (optional - otherwise it will be done automatically - # when the garbage collector destroys the node object) - arm_node.destroy_node() +from control_msgs.action import FollowJointTrajectory +from itertools import zip_longest + +# Helper function to send a goal and return the result future (non-blocking) +def start_trajectory(node, client, joint_names, positions): + if positions is None: + return None + + traj = JointTrajectory() + traj.joint_names = joint_names + point = JointTrajectoryPoint() + point.positions = positions + point.time_from_start = Duration(sec=30) + traj.points.append(point) + + goal_msg = FollowJointTrajectory.Goal() + goal_msg.trajectory = traj + + # Send goal + print(f"Sending goal to {client._action_name}...") + send_goal_future = client.send_goal_async(goal_msg) + + # Wait strictly for the server to ACCEPT the goal (fast) + rclpy.spin_until_future_complete(node, send_goal_future) + goal_handle = send_goal_future.result() + + if not goal_handle.accepted: + print(f"Goal rejected for {client._action_name}") + return None + + # Return the future that tracks the actual execution (slow) + return goal_handle.get_result_async() + +def main(): + rclpy.init() + node = rclpy.create_node('simple_arm_mover') + + # 1. Setup Data + canadarm2_joints = [ + "joint_canadarm2_1", "joint_canadarm2_2", "joint_canadarm2_3", + "joint_canadarm2_4", "joint_canadarm2_5", "joint_canadarm2_6", + "joint_canadarm2_7" + ] + mbs_joints = ["joint_mbs"] + + canadarm2_poses = [ + [0.51, 1.48, -0.677, 0.534, -0.062, -0.645, -0.755], + [0.0, 0.628, -0.187, 0.0, 0.0, 0.0, 0.0] + ] + mbs_poses = [ + [0.0], + [8.0], + [-4.0] + ] + + # 2. Setup Clients + arm_client = ActionClient( + node, FollowJointTrajectory, + '/canadarm2_joint_trajectory_controller/follow_joint_trajectory' + ) + mbs_client = ActionClient( + node, FollowJointTrajectory, + '/mobile_base_system_joint_trajectory_controller/follow_joint_trajectory' + ) + + print("Waiting for action servers...") + arm_client.wait_for_server() + mbs_client.wait_for_server() + print("Servers ready.") + + # 3. Iterate through poses (zipping ensures we handle different list lengths) + # zip_longest allows the shorter list to return 'None' while the longer continues + for i, (arm_pos, mbs_pos) in enumerate(zip_longest(canadarm2_poses, mbs_poses)): + + print(f"\n--- Starting Pose Set {i} ---") + + # Start both movements concurrently + future_arm = start_trajectory(node, arm_client, canadarm2_joints, arm_pos) + future_mbs = start_trajectory(node, mbs_client, mbs_joints, mbs_pos) + + # Wait until BOTH are done + while (future_arm and not future_arm.done()) or (future_mbs and not future_mbs.done()): + rclpy.spin_once(node, timeout_sec=0.1) + + print(f"Pose Set {i} complete.") + + # 4. Cleanup and Exit + print("\nAll poses finished. Exiting.") + node.destroy_node() rclpy.shutdown() - if __name__ == '__main__': - main() + main() \ No newline at end of file diff --git a/scripts/move_sun.py b/scripts/move_sun.py new file mode 100644 index 0000000..b956137 --- /dev/null +++ b/scripts/move_sun.py @@ -0,0 +1,63 @@ +#!/usr/bin/env python3 +import subprocess +import math +import argparse + +TARGET_NAME = "sun_model" +WORLD_NAME = "default" +ORBIT_RADIUS = 10000.0 + +def get_x_axis_rotation_quaternion(y, z): + """ + Calculates the 'Roll' (Rotation around X-axis) needed + for a light at (0, y, z) to point its -Z axis at (0,0,0). + """ + # 1. Calculate the angle from the vertical (Zenith) + # math.atan2(y, z) gives the angle of the vector relative to vertical. + # Since the light points Down (-Z), we negate the angle to rotate it correctly. + roll = -math.atan2(y, z) + + # 2. Convert Euler Roll to Quaternion + # Rotation around X-axis: q = [sin(theta/2), 0, 0, cos(theta/2)] + qx = math.sin(roll / 2.0) + qw = math.cos(roll / 2.0) + + # Y and Z components of the quaternion are 0 for pure X-axis rotation + return (qx, 0.0, 0.0, qw) + +def set_sun_pose(angle_degrees): + rad = math.radians(angle_degrees) + + # --- CHANGED: ORBIT IN Y-Z PLANE (Rotate around X) --- + x = 0 + y = ORBIT_RADIUS * math.sin(rad) + z = ORBIT_RADIUS * math.cos(rad) + + # Safety: don't go underground + if z < 100: z = 100 + + # Calculate Orientation (Roll instead of Pitch) + qx, qy, qz, qw = get_x_axis_rotation_quaternion(y, z) + + print(f"Moving {TARGET_NAME} (X-Axis Rot) -> Angle {angle_degrees}° | Pos [0, {y:.0f}, {z:.0f}]") + + req_str = ( + f'name: "{TARGET_NAME}", ' + f'position: {{x: {x:.1f}, y: {y:.1f}, z: {z:.1f}}}, ' + f'orientation: {{x: {qx:.4f}, y: {qy:.4f}, z: {qz:.4f}, w: {qw:.4f}}}' + ) + + cmd = [ + "gz", "service", + "-s", f"/world/{WORLD_NAME}/set_pose", + "--reqtype", "gz.msgs.Pose", + "--reptype", "gz.msgs.Boolean", + "--req", req_str + ] + subprocess.run(cmd) + +if __name__ == "__main__": + parser = argparse.ArgumentParser() + parser.add_argument("--angle", type=float, default=0.0) + args = parser.parse_args() + set_sun_pose(args.angle) \ No newline at end of file diff --git a/urdf/canadarm2.urdf.xacro b/urdf/canadarm2.urdf.xacro index d09cced..e58bf2f 100644 --- a/urdf/canadarm2.urdf.xacro +++ b/urdf/canadarm2.urdf.xacro @@ -1,7 +1,9 @@ - + + + @@ -12,12 +14,11 @@ - - - + + @@ -35,16 +36,15 @@ - - - - + + + - + @@ -58,16 +58,15 @@ - - - + + - + @@ -80,13 +79,12 @@ - - + - - - + + + @@ -104,13 +102,11 @@ - - - - - + + + @@ -128,12 +124,11 @@ - - - - + + + @@ -150,13 +145,12 @@ - - + - - + + @@ -174,12 +168,11 @@ - - - + + @@ -195,84 +188,102 @@ - + - - + - - + - + - - + - - + - - + - - - - + + + - - - - + + + - - - - + + + - - - - - - + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/urdf/dextre.urdf.xacro b/urdf/dextre.urdf.xacro index 2e6ca47..69b1d9e 100644 --- a/urdf/dextre.urdf.xacro +++ b/urdf/dextre.urdf.xacro @@ -1,6 +1,9 @@ + + + @@ -99,7 +102,7 @@ - + @@ -107,14 +110,14 @@ - + - + @@ -134,7 +137,7 @@ - + @@ -156,7 +159,7 @@ - + @@ -179,7 +182,7 @@ - + @@ -202,7 +205,7 @@ - + @@ -225,7 +228,7 @@ - + @@ -248,7 +251,7 @@ - + @@ -271,12 +274,12 @@ - + - + @@ -335,7 +338,7 @@ - + @@ -357,7 +360,7 @@ - + @@ -380,7 +383,7 @@ - + @@ -404,7 +407,7 @@ - + @@ -427,8 +430,8 @@ - - + + @@ -450,7 +453,7 @@ - + @@ -474,11 +477,11 @@ - + - + @@ -498,7 +501,7 @@ - + @@ -534,8 +537,36 @@ - + + + + + + + + + + + + + + + - + diff --git a/urdf/etvcg_cameras.urdf.xacro b/urdf/etvcg_cameras.urdf.xacro new file mode 100644 index 0000000..a03dd77 --- /dev/null +++ b/urdf/etvcg_cameras.urdf.xacro @@ -0,0 +1,76 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/urdf/mobile_servicing_system.gazebo.xacro b/urdf/mobile_servicing_system.gazebo.xacro index c65c5ea..9c17336 100644 --- a/urdf/mobile_servicing_system.gazebo.xacro +++ b/urdf/mobile_servicing_system.gazebo.xacro @@ -5,7 +5,8 @@ - + + @@ -13,10 +14,20 @@ gz_ros2_control/GazeboSimSystem - + - + + + + + + + + + + + @@ -28,42 +39,7 @@ - - - - - - - - - - - - - 1 - 10.0 - - 1.0 0 0 0 0 -1.57 - 1.3962634 - - 800 - 800 - R8G8B8 - - - 0.01 - 100 - - - gaussian - 0.007 - - - 1 - image_raw - - - + diff --git a/urdf/pt_camera.gazebo.xacro b/urdf/pt_camera.gazebo.xacro new file mode 100644 index 0000000..ebcdadc --- /dev/null +++ b/urdf/pt_camera.gazebo.xacro @@ -0,0 +1,111 @@ + + + + + + + + + + ${horizontal_fov} + + ${image_width} + ${image_height} + ${image_format} + + + ${clip_near} + ${clip_far} + + + 1 + ${update_rate} + true + ${prefix}/image_raw + + + + + ${sun_light_name} + ${lens_flare_scale} + ${lens_flare_color} + ${lens_flare_occlusion_steps} + + + + + + + + Gazebo/Black + + + + + Gazebo/DarkGrey + + + + + Gazebo/Grey + + + + + + + + + + + + + + + + + + + diff --git a/urdf/pt_camera.urdf.xacro b/urdf/pt_camera.urdf.xacro new file mode 100644 index 0000000..5150337 --- /dev/null +++ b/urdf/pt_camera.urdf.xacro @@ -0,0 +1,159 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/worlds/leo.sdf b/worlds/leo.sdf index 036622b..d9a197e 100644 --- a/worlds/leo.sdf +++ b/worlds/leo.sdf @@ -1,14 +1,33 @@ + 0.4 0.4 0.4 1 - 0.0 0.0 0.0 1 + 0 0 0 1 + true false - + + + + true + 0 0 10000 0 0 0 + + + + 0 0 0 0 0 0 + true + 0 0 -1 + 1.0 1.0 1.0 1 + 1.0 1.0 1.0 1 + 2.0 + + + + - + @@ -16,7 +35,6 @@ false docked - ogre2 scene -10.0 -12 8.0 0 0.5 0.3 @@ -39,66 +57,29 @@ false - + floating - - + + - + 0.01 1 100 - - - - - - - - - - + + + + ogre2 - - - true - 0 -10 10 0 0 0 - 0.8 0.8 0.8 1 - 0.2 0.2 0.2 1 - - 1000 - 0.9 - 0.01 - 0.001 - - 10 10 -0.9 - 0 0 0 - - - - - - + \ No newline at end of file From 77c90e892bb0e6ce17c3d931d1afdbe1972b8b9a Mon Sep 17 00:00:00 2001 From: Mike Lanighan Date: Fri, 13 Feb 2026 22:16:47 +0000 Subject: [PATCH 2/5] fix tilt joint --- urdf/pt_camera.urdf.xacro | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/urdf/pt_camera.urdf.xacro b/urdf/pt_camera.urdf.xacro index 5150337..a2025ce 100644 --- a/urdf/pt_camera.urdf.xacro +++ b/urdf/pt_camera.urdf.xacro @@ -103,12 +103,12 @@ - + - + From 15d32b303f96b2f0572d946c6f5a15ee52b2970a Mon Sep 17 00:00:00 2001 From: Mike Lanighan Date: Tue, 17 Feb 2026 15:57:14 +0000 Subject: [PATCH 3/5] simplify camera bridging --- CMakeLists.txt | 1 - .../spawn_mobile_servicing_system.launch.py | 43 +++---- scripts/camera_info_frame_remapper.py | 105 ------------------ 3 files changed, 23 insertions(+), 126 deletions(-) delete mode 100755 scripts/camera_info_frame_remapper.py diff --git a/CMakeLists.txt b/CMakeLists.txt index 1a809c4..5eabbdc 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -35,7 +35,6 @@ endforeach() install(PROGRAMS scripts/move_mss scripts/move_sun.py - scripts/camera_info_frame_remapper.py DESTINATION lib/${PROJECT_NAME} ) diff --git a/launch/spawn_mobile_servicing_system.launch.py b/launch/spawn_mobile_servicing_system.launch.py index 94f57ff..5ec3dbe 100644 --- a/launch/spawn_mobile_servicing_system.launch.py +++ b/launch/spawn_mobile_servicing_system.launch.py @@ -170,40 +170,43 @@ def generate_launch_description(): Node( package="ros_gz_image", executable="image_bridge", + name=f"{camera_name}_image_bridge", arguments=[f"/{camera_name}/image_raw"], output="screen", - name=f"{camera_name}_image_bridge", - remappings=[(f"/{camera_name}/image_raw", f"/{camera_name}/image_raw_gz")], - parameters=sim_time_params, # Ensure bridges use sim time + parameters=[{ + "use_sim_time": True, + "frame_id": f"{camera_name}_optical_frame" + }], ) ) - # Bridge camera_info topic to temporary topic (frame_id will be wrong) + # Bridge camera_info topic, frame is wrong but handled by static tf camera_bridges.append( Node( - package="ros_gz_bridge", - executable="parameter_bridge", - arguments=[f"/{camera_name}/camera_info@sensor_msgs/msg/CameraInfo[gz.msgs.CameraInfo"], - output="screen", - name=f"{camera_name}_camera_info_bridge", - remappings=[(f"/{camera_name}/camera_info", f"/{camera_name}/camera_info_gz")], - parameters=sim_time_params, + package="ros_gz_bridge", + executable="parameter_bridge", + arguments=[f"/{camera_name}/camera_info@sensor_msgs/msg/CameraInfo[gz.msgs.CameraInfo"], + output="screen", + name=f"{camera_name}_info_bridge", + ), + ) + # provide static transforms so info renders correctly + camera_bridges.append( + # 2. The TF Alias (The "Fix") + Node( + package='tf2_ros', + executable='static_transform_publisher', + arguments=['0', '0', '0', '0', '0', '0', '1', + f'{camera_name}_optical_frame', # Parent (The Real Frame) + f'{camera_name}_camera'], # Child (The Gazebo Frame) + name=f'tf_alias_{camera_name}' ) ) - # Frame ID remapper node to fix camera_info and image_raw frame_id - camera_frame_remapper = Node( - package="iss_description", - executable="camera_info_frame_remapper.py", - output="screen", - name="camera_frame_remapper", - parameters=sim_time_params, - ) return LaunchDescription( launch_args + [ mss_robot_state_publisher, mss_spawn, mss_move, - camera_frame_remapper, ] + camera_bridges + [ RegisterEventHandler( OnProcessExit( diff --git a/scripts/camera_info_frame_remapper.py b/scripts/camera_info_frame_remapper.py deleted file mode 100755 index 24b95bd..0000000 --- a/scripts/camera_info_frame_remapper.py +++ /dev/null @@ -1,105 +0,0 @@ -#!/usr/bin/env python3 -""" -Camera Frame ID Remapper - -Subscribes to camera_info and image_raw topics from Gazebo (which use scoped frame names), -remaps the frame_id to match the TF tree frame names, and republishes. - -This is necessary because Gazebo Harmonic uses scoped frame names like: - 'mobile_servicing_system/boom_a_clpa_tilt_link/boom_a_clpa_camera' -But the TF tree uses simple names like: - 'boom_a_clpa_optical_frame' -""" - -import rclpy -from rclpy.node import Node -from sensor_msgs.msg import CameraInfo, Image - - -class CameraInfoFrameRemapper(Node): - def __init__(self): - super().__init__('camera_frame_remapper') - - # Camera names to remap - self.camera_names = [ - 'boom_a_clpa', - 'boom_b_clpa', - 'outrigger_1_clpa', - 'outrigger_2_clpa', - 'etvcg_cp3', - 'etvcg_cp8', - 'etvcg_cp9', - 'etvcg_cp13' - ] - - # Create subscribers and publishers for each camera - self.info_subs = {} - self.info_pubs = {} - self.image_subs = {} - self.image_pubs = {} - - for camera_name in self.camera_names: - # Subscribe to the bridged camera_info (with wrong frame_id) - self.info_subs[camera_name] = self.create_subscription( - CameraInfo, - f'/{camera_name}/camera_info_gz', # Renamed topic from bridge - lambda msg, name=camera_name: self.camera_info_callback(msg, name), - 10 - ) - - # Publish corrected camera_info - self.info_pubs[camera_name] = self.create_publisher( - CameraInfo, - f'/{camera_name}/camera_info', - 10 - ) - - # Subscribe to the bridged image_raw (with wrong frame_id) - self.image_subs[camera_name] = self.create_subscription( - Image, - f'/{camera_name}/image_raw_gz', # Renamed topic from bridge - lambda msg, name=camera_name: self.image_callback(msg, name), - 10 - ) - - # Publish corrected image_raw - self.image_pubs[camera_name] = self.create_publisher( - Image, - f'/{camera_name}/image_raw', - 10 - ) - - self.get_logger().info(f'Camera frame remapper started for {len(self.camera_names)} cameras') - - def camera_info_callback(self, msg: CameraInfo, camera_name: str): - """Remap frame_id and republish""" - # Change frame_id to the optical frame name - msg.header.frame_id = f'{camera_name}_optical_frame' - - # Republish with corrected frame_id - self.info_pubs[camera_name].publish(msg) - - def image_callback(self, msg: Image, camera_name: str): - """Remap frame_id and republish""" - # Change frame_id to the optical frame name - msg.header.frame_id = f'{camera_name}_optical_frame' - - # Republish with corrected frame_id - self.image_pubs[camera_name].publish(msg) - - -def main(args=None): - rclpy.init(args=args) - node = CameraInfoFrameRemapper() - - try: - rclpy.spin(node) - except KeyboardInterrupt: - pass - finally: - node.destroy_node() - rclpy.shutdown() - - -if __name__ == '__main__': - main() From b60309bfbeb77f9af25cae11138ea6911a11d265 Mon Sep 17 00:00:00 2001 From: Mike Lanighan Date: Tue, 17 Feb 2026 16:27:59 +0000 Subject: [PATCH 4/5] handle frame mapping in xacro --- launch/spawn_mobile_servicing_system.launch.py | 16 +++------------- urdf/pt_camera.gazebo.xacro | 2 +- 2 files changed, 4 insertions(+), 14 deletions(-) diff --git a/launch/spawn_mobile_servicing_system.launch.py b/launch/spawn_mobile_servicing_system.launch.py index 5ec3dbe..dd4b24b 100644 --- a/launch/spawn_mobile_servicing_system.launch.py +++ b/launch/spawn_mobile_servicing_system.launch.py @@ -165,7 +165,6 @@ def generate_launch_description(): camera_bridges = [] for camera_name in camera_names: - # Bridge image_raw topic to temporary topic (frame_id will be wrong) camera_bridges.append( Node( package="ros_gz_image", @@ -187,18 +186,9 @@ def generate_launch_description(): arguments=[f"/{camera_name}/camera_info@sensor_msgs/msg/CameraInfo[gz.msgs.CameraInfo"], output="screen", name=f"{camera_name}_info_bridge", - ), - ) - # provide static transforms so info renders correctly - camera_bridges.append( - # 2. The TF Alias (The "Fix") - Node( - package='tf2_ros', - executable='static_transform_publisher', - arguments=['0', '0', '0', '0', '0', '0', '1', - f'{camera_name}_optical_frame', # Parent (The Real Frame) - f'{camera_name}_camera'], # Child (The Gazebo Frame) - name=f'tf_alias_{camera_name}' + parameters=[{ + 'use_sim_time': True, + }] ) ) diff --git a/urdf/pt_camera.gazebo.xacro b/urdf/pt_camera.gazebo.xacro index ebcdadc..2cda977 100644 --- a/urdf/pt_camera.gazebo.xacro +++ b/urdf/pt_camera.gazebo.xacro @@ -55,7 +55,7 @@ ${update_rate} true ${prefix}/image_raw - + ${prefix}_optical_frame Date: Tue, 17 Feb 2026 16:35:37 +0000 Subject: [PATCH 5/5] remove redundant param --- launch/spawn_mobile_servicing_system.launch.py | 1 - 1 file changed, 1 deletion(-) diff --git a/launch/spawn_mobile_servicing_system.launch.py b/launch/spawn_mobile_servicing_system.launch.py index dd4b24b..b282e39 100644 --- a/launch/spawn_mobile_servicing_system.launch.py +++ b/launch/spawn_mobile_servicing_system.launch.py @@ -174,7 +174,6 @@ def generate_launch_description(): output="screen", parameters=[{ "use_sim_time": True, - "frame_id": f"{camera_name}_optical_frame" }], ) )