Add PT cameras and supporting features

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}
 )

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
+

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],
       )
     )
-  ])
+  ])

robots/mobile_servicing_system.urdf.xacro

@@ -5,20 +5,24 @@
   <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"/>
 
   <!-- 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>