diff --git a/CMakeLists.txt b/CMakeLists.txt
index 5c50555..5eabbdc 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -34,6 +34,7 @@ endforeach()
 
 install(PROGRAMS
   scripts/move_mss
+  scripts/move_sun.py
   DESTINATION lib/${PROJECT_NAME}
 )
 
@@ -42,7 +43,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 4bd725e..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:
@@ -197,8 +200,36 @@
       stopped_velocity_tolerance: 0.1
       goal_time: 0.0
 
-/gz_ros_control:
+/camera_joint_trajectory_controller:
   ros__parameters:
-    # P-gain across all joints in gazebo's position interface
-    # https://control.ros.org/humble/doc/gz_ros2_control/doc/index.html#notes-on-the-command-interface
-    position_proportional_gain: 0.05
+    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..b282e39 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,64 @@ 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,
   )
 
-
-  image_bridge = Node(
-    package="ros_gz_image",
-    executable="image_bridge",
-    arguments=["/image_raw", "/image_raw"],
-    output="screen"
+  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:
+    camera_bridges.append(
+      Node(
+        package="ros_gz_image",
+        executable="image_bridge",
+        name=f"{camera_name}_image_bridge",
+        arguments=[f"/{camera_name}/image_raw"],
+        output="screen",
+        parameters=[{
+            "use_sim_time": True,
+        }],
+      )
+    )
+    # 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}_info_bridge",
+          parameters=[{
+                'use_sim_time': True,
+          }]
+      )
+    )
+
+
   return LaunchDescription( launch_args + [
     mss_robot_state_publisher,
     mss_spawn,
     mss_move,
-    image_bridge,
+    ] + camera_bridges + [
     RegisterEventHandler(
       OnProcessExit(
         target_action=mss_spawn,
@@ -151,14 +206,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 @@
   <xacro:include filename="$(find iss_description)/urdf/canadarm2.urdf.xacro"/>
   <xacro:include filename="$(find iss_description)/urdf/dextre.urdf.xacro"/>
   <xacro:include filename="$(find iss_description)/urdf/solar_panels.urdf.xacro"/>
-  
+  <xacro:include filename="$(find iss_description)/urdf/etvcg_cameras.urdf.xacro"/>
+
   <!-- Gazebo -->
   <xacro:include filename="$(find iss_description)/urdf/mobile_servicing_system.gazebo.xacro"/>
   <xacro:mobile_servicing_system_gazebo command_interface="position"/>
@@ -13,12 +14,15 @@
   <!-- ISS and Mobile Base System -->
   <link name="world"/>
   <xacro:mobile_base_system parent="world"/>
-  
+
   <!-- Solar Panels -->
   <xacro:solar_panels parent="link_14_s0_truss"/>
-    
+
   <!-- Canadarm2 and Dextre -->
   <xacro:dextre parent="canadarm2_link_7"/>
   <xacro:canadarm2 parent="mobile_base_system"/>
-  
+
+  <!-- ETV-CG Cameras -->
+  <xacro:etvcg_cameras parent="root"/>
+
 </robot>
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: <Fixed 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/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 @@
 <?xml version="1.0"?>
 <robot name="canadarm2" xmlns:xacro="http://www.ros.org/wiki/xacro">
 
-  <!-- CANADARM2 -->
+  <xacro:include filename="$(find iss_description)/urdf/pt_camera.urdf.xacro"/>
+  <xacro:include filename="$(find iss_description)/urdf/pt_camera.gazebo.xacro"/>
+
   <xacro:macro name="canadarm2" params=" parent:='' ">
 
   <link name="pdgf"/>
@@ -12,12 +14,11 @@
       <child link="pdgf_forward"/>
   </joint>
 
-  <!-- ******* Link Base *******  -->
   <link name="canadarm2_base">
    <inertial>
      <origin xyz="0 0 0.0" rpy="0 0 0"/>
-     <mass value="1"/>
-     <inertia ixx="100"  ixy="0"  ixz="0" iyy="100" iyz="0" izz="100" />
+     <mass value="200.0"/>
+     <inertia ixx="10.0"  ixy="0"  ixz="0" iyy="10.0" iyz="0" izz="10.0" />
    </inertial>
 
    <visual>
@@ -35,16 +36,15 @@
    </collision>
   </link>
 
-  <!-- ******* Link 1 *******  -->
   <link name="canadarm2_link_1">
     <inertial>
-      <origin xyz="0 0 0.0" rpy="0 0 0"/>
-      <mass value="1"/>
-      <inertia ixx="100"  ixy="0"  ixz="0" iyy="100" iyz="0" izz="100" />
+      <origin xyz="0 -0.5 0.0" rpy="0 0 0"/>
+      <mass value="140.0"/>
+      <inertia ixx="8.0"  ixy="0"  ixz="0" iyy="5.0" iyz="0" izz="8.0" />
     </inertial>
 
     <visual>
-      <origin xyz="0 0 0" rpy="0.0 -0.488692 0.663225" /> <!-- Blender: 0 -28 38-->
+      <origin xyz="0 0 0" rpy="0.0 -0.488692 0.663225" />
       <geometry>
         <mesh filename="package://iss_description/meshes/canadarm2/link_1.dae" scale="1 1 1"/>
       </geometry>
@@ -58,16 +58,15 @@
     </collision>
    </link>
 
-  <!-- ******* Link 2 *******  -->
   <link name="canadarm2_link_2">
     <inertial>
       <origin xyz="0 0 0.0" rpy="0 0 0"/>
-      <mass value="1"/>
-      <inertia ixx="100"  ixy="0"  ixz="0" iyy="100" iyz="0" izz="100" />
+      <mass value="120.0"/>
+      <inertia ixx="8.0"  ixy="0"  ixz="0" iyy="8.0" iyz="0" izz="5.0" />
     </inertial>
 
     <visual>
-      <origin xyz="0 0 0" rpy="0 -0.49131018 0" /> <!-- Blender: 0.0 -28.15 0.0-->
+      <origin xyz="0 0 0" rpy="0 -0.49131018 0" />
       <geometry>
         <mesh filename="package://iss_description/meshes/canadarm2/link_2.dae" scale="1 1 1"/>
       </geometry>
@@ -80,13 +79,12 @@
       </geometry>
     </collision>
    </link>
-   
-  <!-- ******* Link 3 *******  -->
+
   <link name="canadarm2_link_3">
     <inertial>
-      <origin xyz="0 0 0.0" rpy="0 0 0"/>
-      <mass value="1"/>
-      <inertia ixx="100"  ixy="0"  ixz="0" iyy="100" iyz="0" izz="100" />
+      <origin xyz="-3.0 0 0.0" rpy="0 0 0"/>
+      <mass value="100.0"/>
+      <inertia ixx="5.0"  ixy="0"  ixz="0" iyy="200.0" iyz="0" izz="200.0" />
     </inertial>
 
     <visual>
@@ -104,13 +102,11 @@
     </collision>
    </link>
 
-
-  <!-- ******* Link 4 *******  -->
   <link name="canadarm2_link_4">
     <inertial>
-      <origin xyz="0 0 0.0" rpy="0 0 0"/>
-      <mass value="1"/>
-      <inertia ixx="100"  ixy="0"  ixz="0" iyy="100" iyz="0" izz="100" />
+      <origin xyz="0 -3.5 0.0" rpy="0 0 0"/>
+      <mass value="150.0"/>
+      <inertia ixx="615.0"  ixy="0"  ixz="0" iyy="5.0" iyz="0" izz="615.0" />
     </inertial>
 
     <visual>
@@ -128,12 +124,11 @@
     </collision>
    </link>
 
-  <!-- ******* Link 5 *******  -->
   <link name="canadarm2_link_5">
     <inertial>
-      <origin xyz="0 0 0.0" rpy="0 0 0"/>
-      <mass value="1"/>
-      <inertia ixx="100"  ixy="0"  ixz="0" iyy="100" iyz="0" izz="100" />
+      <origin xyz="0 0 3.5" rpy="0 0 0"/>
+      <mass value="150.0"/>
+      <inertia ixx="615.0"  ixy="0"  ixz="0" iyy="615.0" iyz="0" izz="5.0" />
     </inertial>
 
     <visual>
@@ -150,13 +145,12 @@
       </geometry>
     </collision>
    </link>
-   
-  <!-- ******* Link 6 *******  -->
+
   <link name="canadarm2_link_6">
     <inertial>
       <origin xyz="0 0 0.0" rpy="0 0 0"/>
-      <mass value="1"/>
-      <inertia ixx="100"  ixy="0"  ixz="0" iyy="100" iyz="0" izz="100" />
+      <mass value="120.0"/>
+      <inertia ixx="8.0"  ixy="0"  ixz="0" iyy="8.0" iyz="0" izz="5.0" />
     </inertial>
 
     <visual>
@@ -174,12 +168,11 @@
     </collision>
    </link>
 
-  <!-- ******* Link 7 *******  -->
   <link name="canadarm2_link_7">
     <inertial>
       <origin xyz="0 0 0.0" rpy="0 0 0"/>
-      <mass value="1"/>
-      <inertia ixx="100"  ixy="0"  ixz="0" iyy="100" iyz="0" izz="100" />
+      <mass value="200.0"/>
+      <inertia ixx="10.0"  ixy="0"  ixz="0" iyy="10.0" iyz="0" izz="10.0" />
     </inertial>
 
     <visual>
@@ -195,84 +188,102 @@
         <mesh filename="package://iss_description/meshes/canadarm2/link_7.dae" scale="1 1 1"/>
       </geometry>
     </collision>
-   </link>   
+   </link>
 
   <joint name="joint_pdgf" type="fixed">
       <origin rpy="0 0 0" xyz="-1.4347 -0.19847 1.5901"/>
       <parent link="${parent}"/>
       <child link="pdgf"/>
   </joint>
-  
 
   <joint name="joint_canadarm2_base" type="fixed">
-      <origin rpy="-0.35691983 0.0 -0.572468" xyz="0 0 0"/> 
+      <origin rpy="-0.35691983 0.0 -0.572468" xyz="0 0 0"/>
       <parent link="pdgf"/>
       <child link="canadarm2_base"/>
   </joint>
 
-  <!-- joint 1 -->
   <joint name="joint_canadarm2_1" type="revolute">
-      <origin rpy="0 0 0" xyz="0.0 -1.2388 0.0"/> <!-- -0.66927 -0.97994 0.3556 -->
+      <origin rpy="0 0 0" xyz="0.0 -1.2388 0.0"/>
       <parent link="canadarm2_base"/>
       <child link="canadarm2_link_1"/>
       <axis xyz="0 1 0"/>
-      <limit effort="1000.0" lower="-3.1416" upper="3.1416" velocity="0.5"/>
+      <limit effort="10000.0" lower="-3.1416" upper="3.1416" velocity="0.04"/>
   </joint>
 
-  <!-- joint 2 -->
   <joint name="joint_canadarm2_2" type="revolute">
       <origin rpy="0 0 0" xyz="-0.005874 -0.22963 0.35801"/>
       <parent link="canadarm2_link_1"/>
       <child link="canadarm2_link_2"/>
       <axis xyz="0 0 1"/>
-      <limit effort="1000.0" lower="-3.1416" upper="3.1416" velocity="0.5"/>
+      <limit effort="10000.0" lower="-3.1416" upper="3.1416" velocity="0.04"/>
   </joint>
 
-  <!-- joint 3 -->
   <joint name="joint_canadarm2_3" type="revolute">
       <origin rpy="0 0 0" xyz="0.13387 -0.31415 0.25363"/>
       <parent link="canadarm2_link_2"/>
       <child link="canadarm2_link_3"/>
       <axis xyz="0 1 0"/>
-      <limit effort="1000.0" lower="-3.1416" upper="3.1416" velocity="0.5"/>
+      <limit effort="10000.0" lower="-3.1416" upper="3.1416" velocity="0.04"/>
   </joint>
-  
-  <!-- joint 4 -->
+
   <joint name="joint_canadarm2_4" type="revolute">
       <origin rpy="0 0 0" xyz="-7.3435 -0.57517 0.004392"/>
       <parent link="canadarm2_link_3"/>
       <child link="canadarm2_link_4"/>
       <axis xyz="0 1 0"/>
-      <limit effort="1000.0" lower="-3.1416" upper="3.1416" velocity="0.5"/>
-  </joint>  
-  
-  <!-- joint 5 -->
+      <limit effort="10000.0" lower="-3.1416" upper="3.1416" velocity="0.04"/>
+  </joint>
+
   <joint name="joint_canadarm2_5" type="revolute">
       <origin rpy="0 0 0" xyz="0.004395 -0.57517 7.3435"/>
       <parent link="canadarm2_link_4"/>
       <child link="canadarm2_link_5"/>
       <axis xyz="0 1 0"/>
-      <limit effort="1000.0" lower="-3.1416" upper="3.1416" velocity="0.5"/>
-  </joint>   
-  
-  <!-- joint 6 -->
+      <limit effort="5000.0" lower="-3.1416" upper="3.1416" velocity="0.04"/>
+  </joint>
+
   <joint name="joint_canadarm2_6" type="revolute">
       <origin rpy="0 0 0" xyz="-0.28758 -0.3134 0.002192"/>
       <parent link="canadarm2_link_5"/>
       <child link="canadarm2_link_6"/>
       <axis xyz="1 0 0"/>
-      <limit effort="1000.0" lower="-3.1416" upper="3.1416" velocity="0.5"/>
-  </joint> 
-  
-  <!-- joint 7 -->
+      <limit effort="5000.0" lower="-3.1416" upper="3.1416" velocity="0.04"/>
+  </joint>
+
   <joint name="joint_canadarm2_7" type="revolute">
       <origin rpy="0 0 0" xyz="-0.31341 -0.002199 0.28759"/>
       <parent link="canadarm2_link_6"/>
       <child link="canadarm2_link_7"/>
       <axis xyz="0 0 1"/>
-      <limit effort="1000.0" lower="-3.1416" upper="3.1416" velocity="0.5"/>
-  </joint> 
-  
- </xacro:macro> 
-    
-</robot>
+      <limit effort="5000.0" lower="-3.1416" upper="3.1416" velocity="0.04"/>
+  </joint>
+
+  <xacro:pt_camera prefix="boom_a_clpa" parent_link="canadarm2_link_3">
+    <origin xyz="-4.591 0.259 0.0" rpy="1.5708 0.0 1.5708"/>
+  </xacro:pt_camera>
+
+  <xacro:pt_camera_gazebo
+    prefix="boom_a_clpa"
+    image_width="1024"
+    image_height="1024"
+    horizontal_fov="0.896"
+    update_rate="30.0"
+    enable_lens_flare="true"
+    lens_flare_scale="5.0"/>
+
+  <xacro:pt_camera prefix="boom_b_clpa" parent_link="canadarm2_link_4">
+    <origin xyz="0.142 -0.847 2.735" rpy="0.0 0.0 0.0"/>
+  </xacro:pt_camera>
+
+  <xacro:pt_camera_gazebo
+    prefix="boom_b_clpa"
+    image_width="1024"
+    image_height="1024"
+    horizontal_fov="0.896"
+    update_rate="30.0"
+    enable_lens_flare="true"
+    lens_flare_scale="5.0"/>
+
+ </xacro:macro>
+
+</robot>
\ 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 @@
 <?xml version="1.0"?>
 <robot name="dextre" xmlns:xacro="http://www.ros.org/wiki/xacro">
 
+  <xacro:include filename="$(find iss_description)/urdf/pt_camera.urdf.xacro"/>
+  <xacro:include filename="$(find iss_description)/urdf/pt_camera.gazebo.xacro"/>
+
   <!-- DEXTRE -->
   <xacro:macro name="dextre" params=" parent:='' ">
 
@@ -99,7 +102,7 @@
 
   <!-- **************** -->
   <!-- NEUTRAL JOINTS   -->
-  <!-- **************** -->  
+  <!-- **************** -->
 
   <!-- joint base -->
   <joint name="joint_dextre_base" type="fixed">
@@ -107,14 +110,14 @@
       <parent link="${parent}"/>
       <child link="dextre_base"/>
   </joint>
-  
+
   <!-- joint tool platform -->
   <joint name="joint_dextre_tool_platform" type="fixed">
       <origin rpy="0 0 0" xyz="-0.000003 -0.005982 1.9619"/>
       <parent link="dextre_base"/>
       <child link="dextre_tool_platform"/>
   </joint>
-  
+
   <!-- joint latching ee -->
   <joint name="joint_dextre_latching_end_effector" type="fixed">
       <origin rpy="0 0 0" xyz="0.0 0.0 0.96287"/>
@@ -134,7 +137,7 @@
   <!-- *********************  -->
   <!-- ******* ARM 1 *******  -->
   <!-- *********************  -->
-  
+
   <!-- ******* Link Arm 1 Shoulder Roll *******  -->
   <link name="dextre_arm_1_shoulder_roll">
    <inertial>
@@ -156,7 +159,7 @@
        <mesh filename="package://iss_description/meshes/dextre/arm_1/shoulder_roll.dae" scale="1 1 1"/>
      </geometry>
    </collision>
-  </link>  
+  </link>
 
   <!-- ******* Link Arm 1 Shoulder Yaw *******  -->
   <link name="dextre_arm_1_shoulder_yaw">
@@ -179,7 +182,7 @@
        <mesh filename="package://iss_description/meshes/dextre/arm_1/shoulder_yaw.dae" scale="1 1 1"/>
      </geometry>
    </collision>
-  </link>  
+  </link>
 
   <!-- ******* Link Arm 1 Shoulder Pitch *******  -->
   <link name="dextre_arm_1_shoulder_pitch">
@@ -202,7 +205,7 @@
        <mesh filename="package://iss_description/meshes/dextre/arm_1/shoulder_pitch.dae" scale="1 1 1"/>
      </geometry>
    </collision>
-  </link>  
+  </link>
 
   <!-- ******* Link Arm 1 Elbow Pitch *******  -->
   <link name="dextre_arm_1_elbow_pitch">
@@ -225,7 +228,7 @@
        <mesh filename="package://iss_description/meshes/dextre/arm_1/elbow_pitch.dae" scale="1 1 1"/>
      </geometry>
    </collision>
-  </link>  
+  </link>
 
   <!-- ******* Link Arm 1 Wrist Pitch Yaw *******  -->
   <link name="dextre_arm_1_wrist_pitch_yaw">
@@ -248,7 +251,7 @@
        <mesh filename="package://iss_description/meshes/dextre/arm_1/wrist_pitch_yaw.dae" scale="1 1 1"/>
      </geometry>
    </collision>
-  </link>  
+  </link>
 
   <!-- ******* Link Arm 1 Wrist Roll *******  -->
   <link name="dextre_arm_1_wrist_roll">
@@ -271,12 +274,12 @@
        <mesh filename="package://iss_description/meshes/dextre/arm_1/wrist_roll.dae" scale="1 1 1"/>
      </geometry>
    </collision>
-  </link> 
+  </link>
 
 
   <!-- **************** -->
   <!-- ARM 1 JOINTS     -->
-  <!-- **************** -->  
+  <!-- **************** -->
 
   <!-- joint arm 1 shoulder roll -->
   <joint name="joint_dextre_arm_1_shoulder_roll" type="revolute">
@@ -335,7 +338,7 @@
   <!-- *********************  -->
   <!-- ******* ARM 2 *******  -->
   <!-- *********************  -->
-  
+
   <!-- ******* Link Arm 2 Shoulder Roll *******  -->
   <link name="dextre_arm_2_shoulder_roll">
    <inertial>
@@ -357,7 +360,7 @@
        <mesh filename="package://iss_description/meshes/dextre/arm_2/shoulder_roll.dae" scale="1 1 1"/>
      </geometry>
    </collision>
-  </link>  
+  </link>
 
   <!-- ******* Link Arm 2 Shoulder Yaw *******  -->
   <link name="dextre_arm_2_shoulder_yaw">
@@ -380,7 +383,7 @@
        <mesh filename="package://iss_description/meshes/dextre/arm_2/shoulder_yaw.dae" scale="1 1 1"/>
      </geometry>
    </collision>
-  </link>  
+  </link>
 
  <!-- ******* Link Arm 2 Shoulder Pitch *******  -->
   <link name="dextre_arm_2_shoulder_pitch">
@@ -404,7 +407,7 @@
      </geometry>
    </collision>
   </link>
-   
+
 
  <!-- ******* Link Arm 2 Elbow Pitch *******  -->
   <link name="dextre_arm_2_elbow_pitch">
@@ -427,8 +430,8 @@
        <mesh filename="package://iss_description/meshes/dextre/arm_2/elbow_pitch.dae" scale="1 1 1"/>
      </geometry>
    </collision>
-  </link>   
-    
+  </link>
+
   <!-- ******* Link Arm 2 Wrist Pitch Yaw *******  -->
   <link name="dextre_arm_2_wrist_pitch_yaw">
    <inertial>
@@ -450,7 +453,7 @@
        <mesh filename="package://iss_description/meshes/dextre/arm_2/wrist_pitch_yaw.dae" scale="1 1 1"/>
      </geometry>
    </collision>
-  </link>  
+  </link>
 
   <!-- ******* Link Arm 2 Wrist Roll *******  -->
   <link name="dextre_arm_2_wrist_roll">
@@ -474,11 +477,11 @@
      </geometry>
    </collision>
   </link>
-    
+
 
   <!-- **************** -->
   <!-- ARM 2 JOINTS     -->
-  <!-- **************** -->  
+  <!-- **************** -->
 
   <!-- joint arm 2 shoulder roll -->
   <joint name="joint_dextre_arm_2_shoulder_roll" type="revolute">
@@ -498,7 +501,7 @@
       <axis xyz="0 0 1"/>
       <limit effort="1000.0" lower="-3.1416" upper="3.1416" velocity="0.5"/>
   </joint>
-  
+
   <!-- joint arm 2 shoulder pitch -->
   <joint name="joint_dextre_arm_2_shoulder_pitch" type="revolute">
       <origin rpy="0 0 0" xyz="0.30367 -0.14278 0.11332"/>
@@ -534,8 +537,36 @@
       <axis xyz="0 0 1"/>
       <limit effort="1000.0" lower="-3.1416" upper="3.1416" velocity="0.5"/>
   </joint>
-  
+
+  <!-- Outrigger 1 CLPA Camera on tool_platform -->
+  <xacro:pt_camera prefix="outrigger_1_clpa" parent_link="dextre_tool_platform">
+    <origin xyz="1.368 0.300 0.314" rpy="1.5708 0.0 0.0"/>
+  </xacro:pt_camera>
+
+  <xacro:pt_camera_gazebo
+    prefix="outrigger_1_clpa"
+    image_width="1024"
+    image_height="1024"
+    horizontal_fov="0.896"
+    update_rate="30.0"
+    enable_lens_flare="true"
+    lens_flare_scale="5.0"/>
+
+  <!-- Outrigger 2 CLPA Camera on tool_platform -->
+  <xacro:pt_camera prefix="outrigger_2_clpa" parent_link="dextre_tool_platform">
+    <origin xyz="-1.368 0.300 0.314" rpy="1.5708 0.0 0.0"/>
+  </xacro:pt_camera>
+
+  <xacro:pt_camera_gazebo
+    prefix="outrigger_2_clpa"
+    image_width="1024"
+    image_height="1024"
+    horizontal_fov="0.896"
+    update_rate="30.0"
+    enable_lens_flare="true"
+    lens_flare_scale="5.0"/>
+
   </xacro:macro>
-  
+
 
 </robot>
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 @@
+<?xml version="1.0"?>
+<robot name="etvcg_cameras" xmlns:xacro="http://www.ros.org/wiki/xacro">
+
+  <xacro:include filename="$(find iss_description)/urdf/pt_camera.urdf.xacro"/>
+  <xacro:include filename="$(find iss_description)/urdf/pt_camera.gazebo.xacro"/>
+
+  <!--
+    ETV-CG Cameras Macro
+
+    Creates 4 pan-tilt cameras mounted on the ISS structure (ETV-CG camera positions).
+    These cameras are attached to the root/world frame of the ISS.
+
+    Parameters:
+      - parent: Parent link to attach cameras to (typically 'root')
+  -->
+  <xacro:macro name="etvcg_cameras" params="parent:=''">
+
+    <!-- CP3: Camera Position 3 -->
+    <xacro:pt_camera prefix="etvcg_cp3" parent_link="${parent}">
+      <origin xyz="-15.528 -6.700 3.407" rpy="0.6435 0.0 0.0"/>
+    </xacro:pt_camera>
+
+    <xacro:pt_camera_gazebo
+      prefix="etvcg_cp3"
+      image_width="1024"
+      image_height="1024"
+      horizontal_fov="0.896"
+      update_rate="30.0"
+      enable_lens_flare="true"
+      lens_flare_scale="5.0"/>
+
+    <!-- CP8: Camera Position 8 -->
+    <xacro:pt_camera prefix="etvcg_cp8" parent_link="${parent}">
+      <origin xyz="19.207 -4.983 8.827" rpy="1.0297 -1.0192 1.0472"/>
+    </xacro:pt_camera>
+
+    <xacro:pt_camera_gazebo
+      prefix="etvcg_cp8"
+      image_width="1024"
+      image_height="1024"
+      horizontal_fov="0.896"
+      update_rate="30.0"
+      enable_lens_flare="true"
+      lens_flare_scale="5.0"/>
+
+    <!-- CP9: Camera Position 9 -->
+    <xacro:pt_camera prefix="etvcg_cp9" parent_link="${parent}">
+      <origin xyz="19.304 -5.129 1.685" rpy="1.0123 -1.0472 -1.0297"/>
+    </xacro:pt_camera>
+
+    <xacro:pt_camera_gazebo
+      prefix="etvcg_cp9"
+      image_width="1024"
+      image_height="1024"
+      horizontal_fov="0.896"
+      update_rate="30.0"
+      enable_lens_flare="true"
+      lens_flare_scale="5.0"/>
+
+    <!-- CP13: Camera Position 13 -->
+    <xacro:pt_camera prefix="etvcg_cp13" parent_link="${parent}">
+      <origin xyz="-4.055 -10.533 -0.403" rpy="0.4712 0.4487 -1.4311"/>
+    </xacro:pt_camera>
+
+    <xacro:pt_camera_gazebo
+      prefix="etvcg_cp13"
+      image_width="1024"
+      image_height="1024"
+      horizontal_fov="0.896"
+      update_rate="30.0"
+      enable_lens_flare="true"
+      lens_flare_scale="5.0"/>
+
+  </xacro:macro>
+
+</robot>
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 @@
   <xacro:include filename="$(find iss_description)/urdf/canadarm2.gazebo.xacro"/>
   <xacro:include filename="$(find iss_description)/urdf/dextre.gazebo.xacro"/>
   <xacro:include filename="$(find iss_description)/urdf/solar_panels.gazebo.xacro"/>
-    
+  <xacro:include filename="$(find iss_description)/urdf/pt_camera.gazebo.xacro"/>
+
   <xacro:macro name="mobile_servicing_system_gazebo" params="command_interface:=position">
 
     <ros2_control name="GazeboSystem" type="system">
@@ -13,10 +14,20 @@
           <plugin>gz_ros2_control/GazeboSimSystem</plugin>
       </hardware>
 
-      <xacro:mobile_base_system_ros2_control_joints command_interface="${command_interface}"/>      
+      <xacro:mobile_base_system_ros2_control_joints command_interface="${command_interface}"/>
       <xacro:canadarm2_ros2_control_joints command_interface="${command_interface}"/>
       <xacro:dextre_ros2_control_joints command_interface="${command_interface}"/>
-      <xacro:solar_panels_ros2_control_joints command_interface="${command_interface}"/>       
+      <xacro:solar_panels_ros2_control_joints command_interface="${command_interface}"/>
+
+      <!-- Pan-Tilt Camera Joints -->
+      <xacro:pt_camera_ros2_control prefix="boom_a_clpa"/>
+      <xacro:pt_camera_ros2_control prefix="boom_b_clpa"/>
+      <xacro:pt_camera_ros2_control prefix="outrigger_1_clpa"/>
+      <xacro:pt_camera_ros2_control prefix="outrigger_2_clpa"/>
+      <xacro:pt_camera_ros2_control prefix="etvcg_cp3"/>
+      <xacro:pt_camera_ros2_control prefix="etvcg_cp8"/>
+      <xacro:pt_camera_ros2_control prefix="etvcg_cp9"/>
+      <xacro:pt_camera_ros2_control prefix="etvcg_cp13"/>
     </ros2_control>
 
   <gazebo>
@@ -28,42 +39,7 @@
         </ros>
       </plugin>
   </gazebo>
-  
-  <!-- Simulate camera at the end of big arm-->
-  <link name="dextre_arm_2_dummy_camera_link"/>
-
-  <joint name="dextre_2_camera_joint" type="fixed">
-    <parent link="dextre_arm_2_wrist_roll"/>
-    <child link="dextre_arm_2_dummy_camera_link"/>
-    <origin xyz="0 0 -0.96" rpy="0.0 0.0 0.0"/>
-  </joint>
-  
-  <gazebo reference="dextre_arm_2_dummy_camera_link">
-      <sensor type="camera" name="camera_sensor">
-          <visualize>1</visualize>
-          <update_rate>10.0</update_rate>
-          <camera>
-              <pose>1.0 0 0 0 0 -1.57</pose>
-              <horizontal_fov>1.3962634</horizontal_fov>
-              <image>
-                  <width>800</width>
-                  <height>800</height>
-                  <format>R8G8B8</format>
-              </image>
-              <clip>
-              <near>0.01</near>
-              <far>100</far>
-              </clip>
-              <noise>
-              <type>gaussian</type>
-              <stddev>0.007</stddev>
-              </noise>
-          </camera>
-          <always_on>1</always_on>
-          <topic>image_raw</topic>
-      </sensor>
-  </gazebo>
 
   </xacro:macro>
 
-</robot>  
+</robot>
diff --git a/urdf/pt_camera.gazebo.xacro b/urdf/pt_camera.gazebo.xacro
new file mode 100644
index 0000000..2cda977
--- /dev/null
+++ b/urdf/pt_camera.gazebo.xacro
@@ -0,0 +1,111 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
+
+  <!--
+    Pan-Tilt Camera Gazebo Configuration
+
+    Adds Gazebo camera sensor with optional lens flare plugin.
+
+    Parameters:
+      - prefix: Camera instance identifier (must match pt_camera macro)
+      - image_width: Image width in pixels (default: 1024)
+      - image_height: Image height in pixels (default: 1024)
+      - image_format: Image format (default: R8G8B8)
+      - horizontal_fov: Horizontal field of view in radians (default: 0.896)
+      - clip_near: Near clipping plane (default: 0.1)
+      - clip_far: Far clipping plane (default: 100.0)
+      - update_rate: Camera update rate in Hz (default: 30.0)
+      - enable_lens_flare: Enable lens flare plugin (default: true)
+      - lens_flare_scale: Lens flare intensity (default: 5.0)
+      - lens_flare_color: Lens flare RGB color (default: '1.0 1.0 0.8')
+      - lens_flare_occlusion_steps: Occlusion detection steps (default: 10)
+      - sun_light_name: Name of sun light in world (default: 'sun_model::link::sun_light')
+  -->
+  <xacro:macro name="pt_camera_gazebo" params="
+    prefix
+    image_width:=1024
+    image_height:=1024
+    image_format:=R8G8B8
+    horizontal_fov:=0.896
+    clip_near:=0.1
+    clip_far:=100.0
+    update_rate:=30.0
+    enable_lens_flare:=true
+    lens_flare_scale:=5.0
+    lens_flare_color:='1.0 1.0 0.8'
+    lens_flare_occlusion_steps:=10
+    sun_light_name:='sun_model::link::sun_light'
+    ">
+
+    <gazebo reference="${prefix}_camera_link">
+      <sensor name="${prefix}_camera" type="camera">
+        <camera>
+          <horizontal_fov>${horizontal_fov}</horizontal_fov>
+          <image>
+            <width>${image_width}</width>
+            <height>${image_height}</height>
+            <format>${image_format}</format>
+          </image>
+          <clip>
+            <near>${clip_near}</near>
+            <far>${clip_far}</far>
+          </clip>
+        </camera>
+        <always_on>1</always_on>
+        <update_rate>${update_rate}</update_rate>
+        <visualize>true</visualize>
+        <topic>${prefix}/image_raw</topic>
+        <gz_frame_id>${prefix}_optical_frame</gz_frame_id>
+        <!-- Lens Flare Plugin (optional, realistic sun effects) -->
+        <xacro:if value="${enable_lens_flare}">
+          <plugin
+            filename="gz-sim-lens-flare-system"
+            name="gz::sim::systems::LensFlare">
+            <light_name>${sun_light_name}</light_name>
+            <scale>${lens_flare_scale}</scale>
+            <color>${lens_flare_color}</color>
+            <occlusion_steps>${lens_flare_occlusion_steps}</occlusion_steps>
+          </plugin>
+        </xacro:if>
+      </sensor>
+    </gazebo>
+
+    <!-- Gazebo material for camera body -->
+    <gazebo reference="${prefix}_camera_link">
+      <material>Gazebo/Black</material>
+    </gazebo>
+
+    <!-- Gazebo material for pan link -->
+    <gazebo reference="${prefix}_pan_link">
+      <material>Gazebo/DarkGrey</material>
+    </gazebo>
+
+    <!-- Gazebo material for tilt link -->
+    <gazebo reference="${prefix}_tilt_link">
+      <material>Gazebo/Grey</material>
+    </gazebo>
+
+  </xacro:macro>
+
+  <!--
+    Pan-Tilt Camera ros2_control Configuration
+
+    Adds ros2_control interfaces for pan and tilt joints.
+
+    Parameters:
+      - prefix: Camera instance identifier (must match pt_camera macro)
+  -->
+  <xacro:macro name="pt_camera_ros2_control" params="prefix">
+    <joint name="${prefix}_pan_joint">
+      <command_interface name="position"/>
+      <state_interface name="position"/>
+      <state_interface name="velocity"/>
+    </joint>
+    <joint name="${prefix}_tilt_joint">
+      <command_interface name="position"/>
+      <state_interface name="position"/>
+      <state_interface name="velocity"/>
+    </joint>
+  </xacro:macro>
+
+</robot>
diff --git a/urdf/pt_camera.urdf.xacro b/urdf/pt_camera.urdf.xacro
new file mode 100644
index 0000000..a2025ce
--- /dev/null
+++ b/urdf/pt_camera.urdf.xacro
@@ -0,0 +1,159 @@
+<?xml version="1.0"?>
+<robot name="pt_camera" xmlns:xacro="http://www.ros.org/wiki/xacro">
+
+  <!--
+    Pan-Tilt Camera Macro
+
+    Creates a pan-tilt camera unit with controllable joints and optical frame.
+
+    Parameters:
+      - prefix: Unique identifier for this camera instance
+      - parent_link: Link to attach the camera base to
+      - *origin: Transform from parent_link to camera base
+      - pan_lower: Lower limit for pan joint (default: -π)
+      - pan_upper: Upper limit for pan joint (default: π)
+      - tilt_lower: Lower limit for tilt joint (default: -π/2)
+      - tilt_upper: Upper limit for tilt joint (default: π/2)
+      - camera_mass: Mass of camera body (default: 0.1 kg)
+      - camera_size: Size of camera box visual (default: 0.05 m)
+  -->
+  <xacro:macro name="pt_camera" params="
+    prefix
+    parent_link
+    *origin
+    pan_lower:=-3.14159265359
+    pan_upper:=3.14159265359
+    tilt_lower:=-1.57079632679
+    tilt_upper:=1.57079632679
+    camera_mass:=0.1
+    camera_size:=0.05
+    ">
+
+    <!-- Base Link: Attachment point to parent -->
+    <link name="${prefix}_base_link">
+      <inertial>
+        <origin xyz="0 0 0" rpy="0 0 0"/>
+        <mass value="0.01"/>
+        <inertia ixx="0.0001" ixy="0" ixz="0" iyy="0.0001" iyz="0" izz="0.0001"/>
+      </inertial>
+    </link>
+
+    <!-- Base Joint: Fixed connection to parent -->
+    <joint name="${prefix}_base_joint" type="fixed">
+      <xacro:insert_block name="origin"/>
+      <parent link="${parent_link}"/>
+      <child link="${prefix}_base_link"/>
+    </joint>
+
+    <!-- Pan Link: Rotates about Z-axis -->
+    <link name="${prefix}_pan_link">
+      <inertial>
+        <origin xyz="0 0 0" rpy="0 0 0"/>
+        <mass value="0.05"/>
+        <inertia ixx="0.0001" ixy="0" ixz="0" iyy="0.0001" iyz="0" izz="0.0001"/>
+      </inertial>
+      <visual>
+        <origin xyz="0 0 0.01" rpy="0 0 0"/>
+        <geometry>
+          <cylinder radius="0.02" length="0.02"/>
+        </geometry>
+        <material name="${prefix}_pan_material">
+          <color rgba="0.3 0.3 0.3 1.0"/>
+        </material>
+      </visual>
+      <collision>
+        <origin xyz="0 0 0.01" rpy="0 0 0"/>
+        <geometry>
+          <cylinder radius="0.02" length="0.02"/>
+        </geometry>
+      </collision>
+    </link>
+
+    <!-- Pan Joint: Revolute about Z-axis -->
+    <joint name="${prefix}_pan_joint" type="revolute">
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <parent link="${prefix}_base_link"/>
+      <child link="${prefix}_pan_link"/>
+      <axis xyz="0 0 1"/>
+      <limit effort="10.0" lower="${pan_lower}" upper="${pan_upper}" velocity="1.0"/>
+      <dynamics damping="0.1" friction="0.1"/>
+    </joint>
+
+    <!-- Tilt Link: Rotates about X-axis (local after pan) -->
+    <link name="${prefix}_tilt_link">
+      <inertial>
+        <origin xyz="0 0 0" rpy="0 0 0"/>
+        <mass value="0.05"/>
+        <inertia ixx="0.0001" ixy="0" ixz="0" iyy="0.0001" iyz="0" izz="0.0001"/>
+      </inertial>
+      <visual>
+        <origin xyz="0.01 0 0" rpy="0 1.5708 0"/>
+        <geometry>
+          <cylinder radius="0.015" length="0.02"/>
+        </geometry>
+        <material name="${prefix}_tilt_material">
+          <color rgba="0.4 0.4 0.4 1.0"/>
+        </material>
+      </visual>
+      <collision>
+        <origin xyz="0.01 0 0" rpy="0 1.5708 0"/>
+        <geometry>
+          <cylinder radius="0.015" length="0.02"/>
+        </geometry>
+      </collision>
+    </link>
+
+    <!-- Tilt Joint: Revolute about Y-axis -->
+    <joint name="${prefix}_tilt_joint" type="revolute">
+      <origin xyz="0 0 0.02" rpy="0 0 0"/>
+      <parent link="${prefix}_pan_link"/>
+      <child link="${prefix}_tilt_link"/>
+      <axis xyz="0 1 0"/>
+      <limit effort="10.0" lower="${tilt_lower}" upper="${tilt_upper}" velocity="1.0"/>
+      <dynamics damping="0.1" friction="0.1"/>
+    </joint>
+
+    <!-- Camera Link: Physical camera body -->
+    <link name="${prefix}_camera_link">
+      <inertial>
+        <origin xyz="0 0 0" rpy="0 0 0"/>
+        <mass value="${camera_mass}"/>
+        <inertia ixx="0.0001" ixy="0" ixz="0" iyy="0.0001" iyz="0" izz="0.0001"/>
+      </inertial>
+      <visual>
+        <origin xyz="0 0 0" rpy="0 0 0"/>
+        <geometry>
+          <box size="${camera_size} ${camera_size} ${camera_size}"/>
+        </geometry>
+        <material name="${prefix}_camera_material">
+          <color rgba="0.1 0.1 0.1 1.0"/>
+        </material>
+      </visual>
+      <collision>
+        <origin xyz="0 0 0" rpy="0 0 0"/>
+        <geometry>
+          <box size="${camera_size} ${camera_size} ${camera_size}"/>
+        </geometry>
+      </collision>
+    </link>
+
+    <!-- Camera Joint: Fixed connection from tilt to camera -->
+    <joint name="${prefix}_camera_joint" type="fixed">
+      <origin xyz="0.02 0 0" rpy="0 0 0"/>
+      <parent link="${prefix}_tilt_link"/>
+      <child link="${prefix}_camera_link"/>
+    </joint>
+
+    <!-- Optical Frame: ROS camera convention (X-right, Y-down, Z-forward) -->
+    <link name="${prefix}_optical_frame"/>
+
+    <!-- Optical Joint: Transform to ROS camera convention -->
+    <joint name="${prefix}_optical_joint" type="fixed">
+      <origin xyz="0 0 0" rpy="-1.5707963267948966 0 -1.5707963267948966"/>
+      <parent link="${prefix}_camera_link"/>
+      <child link="${prefix}_optical_frame"/>
+    </joint>
+
+  </xacro:macro>
+
+</robot>
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 @@
+<?xml version="1.0" ?>
 <sdf version="1.8">
   <world name="default">
 
     <scene>
       <ambient>0.4 0.4 0.4 1</ambient>
-      <background>0.0 0.0 0.0 1</background>
+      <background>0 0 0 1</background>
+      <shadows>true</shadows>
       <grid>false</grid>
-    </scene>   
+    </scene>
+
+    <model name="sun_model">
+      <static>true</static>
+      <pose>0 0 10000 0 0 0</pose>
+      <link name="link">
+
+        <light type="directional" name="sun_light">
+          <pose>0 0 0 0 0 0</pose>
+          <cast_shadows>true</cast_shadows>
+          <direction>0 0 -1</direction>
+          <diffuse>1.0 1.0 1.0 1</diffuse>
+          <specular>1.0 1.0 1.0 1</specular>
+          <intensity>2.0</intensity>
+        </light>
+
+      </link>
+    </model>
 
     <gui fullscreen="0">
- 
+
       <!-- 3D scene -->
       <plugin filename="MinimalScene" name="3D View">
         <gz-gui>
@@ -16,7 +35,6 @@
           <property type="bool" key="showTitleBar">false</property>
           <property type="string" key="state">docked</property>
         </gz-gui>
- 
         <engine>ogre2</engine>
         <scene>scene</scene>
         <camera_pose>-10.0 -12 8.0 0 0.5 0.3</camera_pose>
@@ -39,66 +57,29 @@
           <property key="showTitleBar" type="bool">false</property>
         </gz-gui>
       </plugin>
- 
+
       <plugin filename="ImageDisplay" name="Image Display">
         <gz-gui>
           <property key="state" type="string">floating</property>
         </gz-gui>
-      </plugin> 
- 
+      </plugin>
+
     </gui>
 
-    <physics name='default_physics' default='1' type='ignored'>
+    <physics name='default_physics' default='1' type='ode'>
       <max_step_size>0.01</max_step_size>
       <real_time_factor>1</real_time_factor>
       <real_time_update_rate>100</real_time_update_rate>
     </physics>
 
-    <plugin
-      filename="gz-sim-physics-system"
-      name="gz::sim::systems::Physics">
-    </plugin>
-
-    <plugin
-      filename="gz-sim-user-commands-system"
-      name="gz::sim::systems::UserCommands">
-    </plugin>
-
-    <plugin
-      filename="gz-sim-scene-broadcaster-system"
-      name="gz::sim::systems::SceneBroadcaster">
-    </plugin> 
-
-    <plugin
-      filename="gz-sim-sensors-system"
-      name="gz::sim::systems::Sensors">
+    <plugin filename="gz-sim-physics-system" name="gz::sim::systems::Physics"></plugin>
+    <plugin filename="gz-sim-user-commands-system" name="gz::sim::systems::UserCommands"></plugin>
+    <plugin filename="gz-sim-scene-broadcaster-system" name="gz::sim::systems::SceneBroadcaster"></plugin>
+    <plugin filename="gz-sim-sensors-system" name="gz::sim::systems::Sensors">
       <render_engine>ogre2</render_engine>
     </plugin>
 
-
-    <light type="directional" name="sun">
-      <cast_shadows>true</cast_shadows>
-      <pose>0 -10 10 0 0 0</pose>
-      <diffuse>0.8 0.8 0.8 1</diffuse>
-      <specular>0.2 0.2 0.2 1</specular>
-      <attenuation>
-        <range>1000</range>
-        <constant>0.9</constant>
-        <linear>0.01</linear>
-        <quadratic>0.001</quadratic>
-      </attenuation>
-      <direction>10 10 -0.9</direction>
-    </light>
     <gravity>0 0 0</gravity>
 
-    <!-- Moon -->
-    <!--<include>
-      <uri>model://moon</uri>      
-      <name>moon</name>
-      <pose>170 0 -50 0 0 0.0</pose>
-    </include>-->
-
-
   </world>
-   
-</sdf>
+</sdf>
\ No newline at end of file