Dev/material reflectivity - #2

 from launch import LaunchDescription
 from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription
 from launch.launch_description_sources import PythonLaunchDescriptionSource
 from launch_ros.actions import Node
 
+
 def generate_launch_description():
     # 1. Paths to your new assets
-    pkg_dave_worlds = get_package_share_directory('dave_worlds')
-    pkg_ros_gz_sim = get_package_share_directory('ros_gz_sim')
-    pkg_dave_demos = get_package_share_directory('dave_demos')
+    pkg_dave_worlds = get_package_share_directory("dave_worlds")
+    pkg_ros_gz_sim = get_package_share_directory("ros_gz_sim")
+    pkg_dave_demos = get_package_share_directory("dave_demos")
 
     # Path to your new world file
-    world_path = os.path.join(pkg_dave_worlds, 'worlds', 'lidar_3d.world')
+    world_path = os.path.join(pkg_dave_worlds, "worlds", "lidar_3d.world")
 
     # 2. Launch Gazebo with the new world
     # We include the standard Gazebo Sim launch but pass our custom world
     gz_sim = IncludeLaunchDescription(
         PythonLaunchDescriptionSource(
-            os.path.join(pkg_dave_demos, 'launch', "dave_sensor.launch.py")
+            os.path.join(pkg_dave_demos, "launch", "dave_sensor.launch.py")
         ),
         launch_arguments={
             "namespace": "lidar_3d",
             "world_name": "lidar_3d",
             "paused": "false",
             "debug": "true",
             "x": "5.8",
             "z": "2",
             "yaw": "3.14",
-            'gz_args': f'-r {world_path} --render-engine ogre2'
+            "gz_args": f"-r {world_path} --render-engine ogre2",
         }.items(),
     )
 
     # This replaces the DAVE multibeam bridge with one for your 3D LiDAR
     bridge = Node(
-        package='ros_gz_bridge',
-        executable='parameter_bridge',
+        package="ros_gz_bridge",
+        executable="parameter_bridge",
         arguments=[
             # LiDAR LaserScan
-            '/lidar@sensor_msgs/msg/LaserScan[gz.msgs.LaserScan',
+            "/lidar@sensor_msgs/msg/LaserScan[gz.msgs.LaserScan",
             # LiDAR PointCloud
-            '/lidar/points@sensor_msgs/msg/PointCloud2[gz.msgs.PointCloudPacked',
+            "/lidar/points@sensor_msgs/msg/PointCloud2[gz.msgs.PointCloudPacked",
             # Clock bridge (essential for TF and sensor timing)
-            '/clock@rosgraph_msgs/msg/Clock[gz.msgs.Clock'
+            "/clock@rosgraph_msgs/msg/Clock[gz.msgs.Clock",
         ],
-        remappings=[
-        ('/lidar', '/lidar_3d/lidar'),
-        ('/lidar/points', '/lidar_3d/lidar/points')
-        ],
-        output='screen'
+        remappings=[("/lidar", "/lidar_3d/lidar"), ("/lidar/points", "/lidar_3d/lidar/points")],
+        output="screen",
     )
 
     # Connects the sensor frame to the world frame for RViz visualization
     tf_node = Node(
-    package='tf2_ros',
-    executable='static_transform_publisher',
-    arguments=[
-        '--x', '0', '--y', '0', '--z', '0.5',
-        '--roll', '0', '--pitch', '0', '--yaw', '0',
-        '--frame-id', 'world',
-        '--child-frame-id', 'lidar_3d/lidar_3d_base_link/gpu_lidar'
-    ],
-)
+        package="tf2_ros",
+        executable="static_transform_publisher",
+        arguments=[
+            "--x",
+            "0",
+            "--y",
+            "0",
+            "--z",
+            "0.5",
+            "--roll",
+            "0",
+            "--pitch",
+            "0",
+            "--yaw",
+            "0",
+            "--frame-id",
+            "world",
+            "--child-frame-id",
+            "lidar_3d/lidar_3d_base_link/gpu_lidar",
+        ],
+    )
 
-    return LaunchDescription([
-        gz_sim,
-        bridge,
-        tf_node
-    ])
+    return LaunchDescription([gz_sim, bridge, tf_node])

 from launch_ros.actions import Node
 
 
 def generate_launch_description():
     pkg_dave_demos = get_package_share_directory("dave_demos")
-    
+
     # Launch DAVE sim with the 3d_sonar sensor
     multibeam_sonar_sim = IncludeLaunchDescription(
         PythonLaunchDescriptionSource(
             os.path.join(pkg_dave_demos, "launch", "dave_sensor.launch.py")
         ),

         }.items(),
     )
 
     # RViz Config
     pkg_dave_multibeam_sonar_demo = get_package_share_directory("dave_multibeam_sonar_demo")
-    
+
     # We load the newly generated rviz config which specifically includes
     # all 64 distinct point clouds overlaid.
     rviz = Node(
         package="rviz2",
         executable="rviz2",

 sonar_3d_vertical_start_rad = math.radians(
     -20.0 + 1.10
 )  # first sensor is -20 + 0.8 + 0.3 because we need to get the center point
 sonar_3d_vertical_end_rad = math.radians(20.0 - 1.10)
 
-multibeam_vertical_min_angle_rad = math.radians(
-    -sonar_3d_vertical_angular_resolution / 2.0
-)
-multibeam_vertical_max_angle_rad = math.radians(
-    sonar_3d_vertical_angular_resolution / 2.0
-)
+multibeam_vertical_min_angle_rad = math.radians(-sonar_3d_vertical_angular_resolution / 2.0)
+multibeam_vertical_max_angle_rad = math.radians(sonar_3d_vertical_angular_resolution / 2.0)
 multibeam_vertical_num_sensors = sonar_3d_vertical_resolution  # 64 sensors
 multibeam_vertical_fov_deg = sonar_3d_vertical_angular_resolution
 
 # ------------- horizontal ------------------------
 sonar_3d_horizontal_resolution = 256

 
 # Generate sensors
 for i in range(multibeam_vertical_num_sensors):
     # center around 0
     # start at sonar_3d_vertical_start_rad and step with sonar_3d_vertical_seperation_deg
-    pitch_rad = sonar_3d_vertical_start_rad + i * math.radians(
-        sonar_3d_vertical_seperation_deg
-    )
+    pitch_rad = sonar_3d_vertical_start_rad + i * math.radians(sonar_3d_vertical_seperation_deg)
     print(f"Generating sensor {i} with pitch {math.degrees(pitch_rad):.6f}")
     sensor_xml = f"""
       <sensor name="sonar_3d_{i}" type="custom" gz:type="multibeam_sonar">
         <pose>0 0 0 0 {pitch_rad:.6f} 0</pose>
         <always_on>true</always_on>

         output="screen",
     )
 
     return LaunchDescription(
         [
-            DeclareLaunchArgument(
-                "rviz", default_value="true", description="Open RViz."
-            ),
+            DeclareLaunchArgument("rviz", default_value="true", description="Open RViz."),
             tank_sim,
             bridge,
             tf_lidar,
             rviz,
         ]

 from launch.conditions import IfCondition
 from launch.substitutions import LaunchConfiguration
 
 from launch_ros.actions import Node
 
-
 SENSOR_RVIZ = {
     "sonar": "square_metal_demo.rviz",
     "lidar": "square_metal_demo_lidar.rviz",
 }
 

     sensor = LaunchConfiguration("sensor").perform(context)
     rviz_config_arg = LaunchConfiguration("rviz_config").perform(context)
     use_rviz = LaunchConfiguration("rviz")
 
     if sensor not in SENSOR_RVIZ:
-        raise RuntimeError(
-            f"Unknown sensor '{sensor}'. Options: {sorted(SENSOR_RVIZ)}"
-        )
+        raise RuntimeError(f"Unknown sensor '{sensor}'. Options: {sorted(SENSOR_RVIZ)}")
 
     pkg_sonar_3d_demo = get_package_share_directory("sonar_3d_demo")
-    rviz_config = rviz_config_arg or os.path.join(
-        pkg_sonar_3d_demo, "rviz", SENSOR_RVIZ[sensor]
-    )
+    rviz_config = rviz_config_arg or os.path.join(pkg_sonar_3d_demo, "rviz", SENSOR_RVIZ[sensor])
 
     nodes = []
     if sensor == "sonar":
         nodes.append(
             Node(

 
 def launch_setup(context, *args, **kwargs):
     sensor = LaunchConfiguration("sensor").perform(context)
 
     if sensor not in SENSOR_PROFILES:
-        raise RuntimeError(
-            f"Unknown sensor '{sensor}'. Options: {sorted(SENSOR_PROFILES)}"
-        )
+        raise RuntimeError(f"Unknown sensor '{sensor}'. Options: {sorted(SENSOR_PROFILES)}")
     profile = SENSOR_PROFILES[sensor]
     pkg_dave_demos = get_package_share_directory("dave_demos")
 
     tank_sim = IncludeLaunchDescription(
         PythonLaunchDescriptionSource(

     if not root.is_dir():
         print(f"ERROR: {root} does not exist", file=sys.stderr)
         return 1
 
     results: dict[str, list[str]] = {
-        "added": [], "skipped": [], "multi-visual": [], "no-visual": [], "missing": [],
+        "added": [],
+        "skipped": [],
+        "multi-visual": [],
+        "no-visual": [],
+        "missing": [],
     }
     for model_name, retro in RETRO_VALUES.items():
         sdf = root / model_name / "model.sdf"
         if not sdf.is_file():
             results["missing"].append(model_name)

 
     Returns True on success, False on failure (timeout, service missing, etc).
     """
     req = (
         f'name: "{name}", '
-        f'position: {{x: {x}, y: {y}, z: {z}}}, '
-        f'orientation: {{x: {qx}, y: {qy}, z: {qz}, w: {qw}}}'
+        f"position: {{x: {x}, y: {y}, z: {z}}}, "
+        f"orientation: {{x: {qx}, y: {qy}, z: {qz}, w: {qw}}}"
     )
     cmd = [
-        "gz", "service",
-        "-s", f"/world/{world}/set_pose",
-        "--reqtype", "gz.msgs.Pose",
-        "--reptype", "gz.msgs.Boolean",
-        "--timeout", str(timeout_ms),
-        "--req", req,
+        "gz",
+        "service",
+        "-s",
+        f"/world/{world}/set_pose",
+        "--reqtype",
+        "gz.msgs.Pose",
+        "--reptype",
+        "gz.msgs.Boolean",
+        "--timeout",
+        str(timeout_ms),
+        "--req",
+        req,
     ]
     try:
         out = subprocess.run(cmd, capture_output=True, text=True, check=False)
     except FileNotFoundError:
         print("ERROR: 'gz' CLI not found in PATH.", file=sys.stderr)

 
 
 def main():
     ap = argparse.ArgumentParser(description=__doc__)
     ap.add_argument("--csv", required=True, help="Per-bag CSV from extract_object_poses.py")
-    ap.add_argument("--model_name", required=True,
-                    help="Gazebo entity name to drive (same name passed to ros_gz_sim create).")
+    ap.add_argument(
+        "--model_name",
+        required=True,
+        help="Gazebo entity name to drive (same name passed to ros_gz_sim create).",
+    )
     ap.add_argument("--world", default="default")
     ap.add_argument("--sensor_x", type=float, default=0.0)
     ap.add_argument("--sensor_y", type=float, default=0.0)
     ap.add_argument("--sensor_z", type=float, default=0.0)
-    ap.add_argument("--rate", type=float, default=10.0,
-                    help="Target set_pose rate in Hz. CSV is downsampled to this.")
-    ap.add_argument("--use_csv_orientation", action="store_true",
-                    help="Use rotation from CSV. Default keeps spawn rotation (identity here).")
-    ap.add_argument("--qx", type=float, default=0.0,
-                    help="Fixed quaternion x when --use_csv_orientation is off.")
+    ap.add_argument(
+        "--rate",
+        type=float,
+        default=10.0,
+        help="Target set_pose rate in Hz. CSV is downsampled to this.",
+    )
+    ap.add_argument(
+        "--use_csv_orientation",
+        action="store_true",
+        help="Use rotation from CSV. Default keeps spawn rotation (identity here).",
+    )
+    ap.add_argument(
+        "--qx",
+        type=float,
+        default=0.0,
+        help="Fixed quaternion x when --use_csv_orientation is off.",
+    )
     ap.add_argument("--qy", type=float, default=0.0)
     ap.add_argument("--qz", type=float, default=0.0)
     ap.add_argument("--qw", type=float, default=1.0)
-    ap.add_argument("--time_rate", type=float, default=1.0,
-                    help="Playback speed multiplier (>1 = faster than real time).")
-    ap.add_argument("--startup_delay_s", type=float, default=2.0,
-                    help="Wait this long before the first set_pose (lets the spawn settle).")
+    ap.add_argument(
+        "--time_rate",
+        type=float,
+        default=1.0,
+        help="Playback speed multiplier (>1 = faster than real time).",
+    )
+    ap.add_argument(
+        "--startup_delay_s",
+        type=float,
+        default=2.0,
+        help="Wait this long before the first set_pose (lets the spawn settle).",
+    )
     args = ap.parse_args()
 
     if shutil.which("gz") is None:
         print("ERROR: 'gz' CLI not on PATH. Source the Gazebo env first.", file=sys.stderr)
         sys.exit(2)

         print("ERROR: CSV is empty.", file=sys.stderr)
         sys.exit(2)
 
     keep = reject_outliers(xyz)
     t_rel, xyz, quat = t_rel[keep], xyz[keep], quat[keep]
-    print(f"Loaded {len(t_rel)} inlier frames "
-          f"(from {len(keep)} total, dropped {(~keep).sum()} outliers).")
+    print(
+        f"Loaded {len(t_rel)} inlier frames "
+        f"(from {len(keep)} total, dropped {(~keep).sum()} outliers)."
+    )
 
     # Downsample to target rate.
     if args.rate > 0:
         keep_idx = [0]
         last_t = t_rel[0]

             qx, qy, qz, qw = quat[i]
         else:
             qx, qy, qz, qw = args.qx, args.qy, args.qz, args.qw
 
         ok = gz_set_pose(
-            args.world, args.model_name,
-            xyz[i, 0], xyz[i, 1], xyz[i, 2],
-            qx, qy, qz, qw,
+            args.world,
+            args.model_name,
+            xyz[i, 0],
+            xyz[i, 1],
+            xyz[i, 2],
+            qx,
+            qy,
+            qz,
+            qw,
         )
         if not ok:
             failures += 1
             if failures == 1:
                 print("WARN: first set_pose failed — is the model spawned yet?", file=sys.stderr)

 )
 from launch.launch_description_sources import PythonLaunchDescriptionSource
 from launch.substitutions import LaunchConfiguration
 from launch_ros.actions import Node
 
-
 BLENDER_MODELS_DIR = os.path.expanduser("~/blender_models")
 WORLD_NAME = "tank_with_float"
 POSE_KEYS = ("x", "y", "z", "roll", "pitch", "yaw")
 
-DEFAULT_POSES_DIR = (
-    "/media/aki/2C76C6780AEDB4DB1/wl_wetlab_apr22_processed/object_poses"
-)
+DEFAULT_POSES_DIR = "/media/aki/2C76C6780AEDB4DB1/wl_wetlab_apr22_processed/object_poses"
 POSE_SOURCES = ("yaml", "real_median", "real_trajectory")
 
 # Map the user-facing `sensor` arg to (namespace, YAML key).
 # The namespace is what dave_sensor.launch.py uses to look up the model
 # under dave_sensor_models/description/<namespace>/model.sdf, and what gets

     return Node(
         package="ros_gz_sim",
         executable="create",
         name=f"spawn_{name}",
         arguments=[
-            "-world", "default",
-            "-name", name,
-            "-file", sdf_path,
-            "-x", f"{pose['x']}",
-            "-y", f"{pose['y']}",
-            "-z", f"{pose['z']}",
-            "-R", f"{pose['roll']}",
-            "-P", f"{pose['pitch']}",
-            "-Y", f"{pose['yaw']}",
+            "-world",
+            "default",
+            "-name",
+            name,
+            "-file",
+            sdf_path,
+            "-x",
+            f"{pose['x']}",
+            "-y",
+            f"{pose['y']}",
+            "-z",
+            f"{pose['z']}",
+            "-R",
+            f"{pose['roll']}",
+            "-P",
+            f"{pose['pitch']}",
+            "-Y",
+            f"{pose['yaw']}",
         ],
         output="both",
         parameters=[{"use_sim_time": True}],
     )
 

     )
     tf = Node(
         package="tf2_ros",
         executable="static_transform_publisher",
         arguments=[
-            "--x", f"{pose['x']}",
-            "--y", f"{pose['y']}",
-            "--z", f"{pose['z']}",
-            "--roll", f"{pose['roll']}",
-            "--pitch", f"{pose['pitch']}",
-            "--yaw", f"{pose['yaw']}",
-            "--frame-id", "world",
-            "--child-frame-id", "lidar_3d/lidar_3d_base_link/gpu_lidar",
+            "--x",
+            f"{pose['x']}",
+            "--y",
+            f"{pose['y']}",
+            "--z",
+            f"{pose['z']}",
+            "--roll",
+            f"{pose['roll']}",
+            "--pitch",
+            f"{pose['pitch']}",
+            "--yaw",
+            f"{pose['yaw']}",
+            "--frame-id",
+            "world",
+            "--child-frame-id",
+            "lidar_3d/lidar_3d_base_link/gpu_lidar",
         ],
     )
     return [bridge, tf]
 
 

     sensor = LaunchConfiguration("sensor").perform(context)
     pose_source = LaunchConfiguration("pose_source").perform(context)
     poses_dir = LaunchConfiguration("poses_dir").perform(context)
 
     if sensor not in SENSOR_PROFILES:
-        raise RuntimeError(
-            f"Unknown sensor '{sensor}'. Options: {sorted(SENSOR_PROFILES)}"
-        )
+        raise RuntimeError(f"Unknown sensor '{sensor}'. Options: {sorted(SENSOR_PROFILES)}")
     if pose_source not in POSE_SOURCES:
-        raise RuntimeError(
-            f"Unknown pose_source '{pose_source}'. Options: {list(POSE_SOURCES)}"
-        )
+        raise RuntimeError(f"Unknown pose_source '{pose_source}'. Options: {list(POSE_SOURCES)}")
     profile = SENSOR_PROFILES[sensor]
 
-    pose_overrides = {
-        k: LaunchConfiguration(k).perform(context) for k in POSE_KEYS
-    }
+    pose_overrides = {k: LaunchConfiguration(k).perform(context) for k in POSE_KEYS}
 
     config_path = os.path.join(
         get_package_share_directory("sonar_3d_demo"), "config", "targets.yaml"
     )
     with open(config_path) as f:
         manifest = yaml.safe_load(f)
 
     if target not in manifest["targets"]:
-        raise RuntimeError(
-            f"Unknown target '{target}'. Available: {sorted(manifest['targets'])}"
-        )
+        raise RuntimeError(f"Unknown target '{target}'. Available: {sorted(manifest['targets'])}")
 
     scene = manifest["scene"]
     if profile["yaml_key"] not in scene:
         raise RuntimeError(
             f"Missing 'scene.{profile['yaml_key']}' in targets.yaml "

     sensor_pose = scene[profile["yaml_key"]]
     float_pose = dict(scene["float"])  # mutable copy — x/y are overridden below
     target_yaml = manifest["targets"][target]
 
     # YAML baseline pose (rotations always come from here — see module docstring).
-    target_pose = {
-        k: _resolve(pose_overrides[k], target_yaml[k]) for k in POSE_KEYS
-    }
+    target_pose = {k: _resolve(pose_overrides[k], target_yaml[k]) for k in POSE_KEYS}
 
     # Optionally overwrite XYZ with the real median pose. We compose
     # sim_world_T_object = sim_world_T_sonar (translation) + sonar_T_object (CSV),
     # which assumes sensor_pose has identity rotation — true for the current
     # targets.yaml. CLI overrides on x/y/z (non-empty pose_overrides) still win.

     cli_fy = LaunchConfiguration("float_y").perform(context).strip()
     cli_fz = LaunchConfiguration("float_z").perform(context).strip()
     yaml_fx = target_yaml.get("float_x")
     yaml_fy = target_yaml.get("float_y")
     yaml_fz = target_yaml.get("float_z")
-    float_pose["x"] = float(cli_fx) if cli_fx else (
-        float(yaml_fx) if yaml_fx is not None else target_pose["x"]
-    )
-    float_pose["y"] = float(cli_fy) if cli_fy else (
-        float(yaml_fy) if yaml_fy is not None else target_pose["y"]
+    float_pose["x"] = (
+        float(cli_fx) if cli_fx else (float(yaml_fx) if yaml_fx is not None else target_pose["x"])
+    )
+    float_pose["y"] = (
+        float(cli_fy) if cli_fy else (float(yaml_fy) if yaml_fy is not None else target_pose["y"])
     )
     if cli_fz:
         float_pose["z"] = float(cli_fz)
     elif yaml_fz is not None:
         float_pose["z"] = float(yaml_fz)

         player = Node(
             package="sonar_3d_demo",
             executable="play_object_trajectory.py",
             name="play_object_trajectory",
             arguments=[
-                "--csv", csv_path,
-                "--model_name", target,
-                "--world", "default",
-                "--sensor_x", str(sensor_pose["x"]),
-                "--sensor_y", str(sensor_pose["y"]),
-                "--sensor_z", str(sensor_pose["z"]),
-                "--rate", LaunchConfiguration("rate").perform(context),
-                "--startup_delay_s", "8.0",
+                "--csv",
+                csv_path,
+                "--model_name",
+                target,
+                "--world",
+                "default",
+                "--sensor_x",
+                str(sensor_pose["x"]),
+                "--sensor_y",
+                str(sensor_pose["y"]),
+                "--sensor_z",
+                str(sensor_pose["z"]),
+                "--rate",
+                LaunchConfiguration("rate").perform(context),
+                "--startup_delay_s",
+                "8.0",
             ],
             output="screen",
         )
         # Start after the spawner timer (period=7s).
         nodes.append(TimerAction(period=9.0, actions=[player]))

     def __init__(self):
         super().__init__("keyboard_joy_publisher")
 
         self.declare_parameter("output_topic", "/keyboard/joy")
         output_topic = (
-            self.get_parameter("output_topic").get_parameter_value().string_value
-            or "/keyboard/joy"
+            self.get_parameter("output_topic").get_parameter_value().string_value or "/keyboard/joy"
         )
 
         self.pub = self.create_publisher(Joy, output_topic, 10)
         self.axes = [0.0] * NUM_AXES
         self.pending_buttons = set()