210 lines
8.9 KiB
Python
210 lines
8.9 KiB
Python
"""Launch three Hikrobot cameras for one-time checkerboard extrinsics."""
|
|
|
|
from pathlib import Path
|
|
|
|
from ament_index_python.packages import get_package_share_directory
|
|
from launch import LaunchDescription
|
|
from launch.actions import DeclareLaunchArgument, OpaqueFunction, SetEnvironmentVariable
|
|
from launch.substitutions import LaunchConfiguration
|
|
from launch_ros.actions import ComposableNodeContainer, Node
|
|
from launch_ros.descriptions import ComposableNode
|
|
from launch_ros.parameter_descriptions import ParameterValue
|
|
|
|
|
|
VIEWS = ("front", "side", "top")
|
|
|
|
|
|
def _launch(context):
|
|
cameras = []
|
|
rectifiers = []
|
|
serials = {}
|
|
for view in VIEWS:
|
|
serial = LaunchConfiguration(f"{view}_camera_serial").perform(context)
|
|
if not serial:
|
|
raise RuntimeError(f"{view}_camera_serial is required")
|
|
serials[view] = serial
|
|
namespace = f"/g20_extrinsics/{view}/camera"
|
|
cameras.append(
|
|
Node(
|
|
package="linkerhand_calibration",
|
|
executable="hikrobot_camera_node",
|
|
name="hikrobot_camera",
|
|
namespace=namespace,
|
|
output="screen",
|
|
emulate_tty=True,
|
|
parameters=[
|
|
{
|
|
"serial_number": serial,
|
|
"expected_model": LaunchConfiguration("camera_model"),
|
|
"camera_name": LaunchConfiguration(
|
|
f"{view}_camera_name"
|
|
),
|
|
"frame_id": f"g20_extrinsics_{view}_optical_frame",
|
|
"image_width": 1624,
|
|
"image_height": 1240,
|
|
"frame_rate": ParameterValue(
|
|
LaunchConfiguration("camera_frame_rate"),
|
|
value_type=float,
|
|
),
|
|
"exposure_time_us": ParameterValue(
|
|
LaunchConfiguration("exposure_time_us"),
|
|
value_type=float,
|
|
),
|
|
"gain_db": ParameterValue(
|
|
LaunchConfiguration("gain_db"), value_type=float
|
|
),
|
|
"auto_exposure": False,
|
|
"camera_info_url": LaunchConfiguration(
|
|
f"{view}_camera_info_url"
|
|
),
|
|
}
|
|
],
|
|
)
|
|
)
|
|
rectifiers.append(
|
|
ComposableNode(
|
|
package="image_proc",
|
|
plugin="image_proc::RectifyNode",
|
|
name=f"rectify_{view}",
|
|
namespace=namespace,
|
|
remappings=[
|
|
("image", f"{namespace}/image_raw"),
|
|
("camera_info", f"{namespace}/camera_info"),
|
|
("image_rect", f"{namespace}/image_rect"),
|
|
],
|
|
parameters=[{"queue_size": 1}],
|
|
extra_arguments=[{"use_intra_process_comms": True}],
|
|
)
|
|
)
|
|
container = ComposableNodeContainer(
|
|
name="g20_extrinsics_vision",
|
|
namespace="/",
|
|
package="rclcpp_components",
|
|
executable="component_container_mt",
|
|
composable_node_descriptions=rectifiers,
|
|
output="screen",
|
|
)
|
|
solver = Node(
|
|
package="linkerhand_calibration",
|
|
executable="three_camera_extrinsics_node",
|
|
name="g20_camera_extrinsics",
|
|
output="screen",
|
|
emulate_tty=True,
|
|
parameters=[
|
|
{
|
|
"output_file": LaunchConfiguration("output_file"),
|
|
"checkerboard_columns": ParameterValue(
|
|
LaunchConfiguration("checkerboard_columns"), value_type=int
|
|
),
|
|
"checkerboard_rows": ParameterValue(
|
|
LaunchConfiguration("checkerboard_rows"), value_type=int
|
|
),
|
|
"square_size_m": ParameterValue(
|
|
LaunchConfiguration("square_size_m"), value_type=float
|
|
),
|
|
"enable_gui": ParameterValue(
|
|
LaunchConfiguration("enable_gui"), value_type=bool
|
|
),
|
|
"gui_refresh_hz": ParameterValue(
|
|
LaunchConfiguration("gui_refresh_hz"), value_type=float
|
|
),
|
|
"maximum_reprojection_rms_px": ParameterValue(
|
|
LaunchConfiguration("maximum_reprojection_rms_px"),
|
|
value_type=float,
|
|
),
|
|
"maximum_candidate_pair_reprojection_rms_px": ParameterValue(
|
|
LaunchConfiguration(
|
|
"maximum_candidate_pair_reprojection_rms_px"
|
|
),
|
|
value_type=float,
|
|
),
|
|
"maximum_single_camera_reprojection_rms_px": ParameterValue(
|
|
LaunchConfiguration(
|
|
"maximum_single_camera_reprojection_rms_px"
|
|
),
|
|
value_type=float,
|
|
),
|
|
"auto_capture_default": ParameterValue(
|
|
LaunchConfiguration("auto_capture_default"),
|
|
value_type=bool,
|
|
),
|
|
"auto_capture_stable_seconds": ParameterValue(
|
|
LaunchConfiguration("auto_capture_stable_seconds"),
|
|
value_type=float,
|
|
),
|
|
**{
|
|
f"{view}_camera_serial": serials[view]
|
|
for view in VIEWS
|
|
},
|
|
}
|
|
],
|
|
)
|
|
return [*cameras, container, solver]
|
|
|
|
|
|
def generate_launch_description() -> LaunchDescription:
|
|
package_share = Path(
|
|
get_package_share_directory("linkerhand_calibration")
|
|
)
|
|
camera_info = Path.home() / ".ros" / "camera_info"
|
|
return LaunchDescription(
|
|
[
|
|
# Keep the large-image transport deterministic even when the
|
|
# calling shell selected another ROS 2 RMW implementation.
|
|
SetEnvironmentVariable(
|
|
name="RMW_IMPLEMENTATION",
|
|
value="rmw_fastrtps_cpp",
|
|
),
|
|
SetEnvironmentVariable(
|
|
name="FASTDDS_DEFAULT_PROFILES_FILE",
|
|
value=str(
|
|
package_share / "config" / "fastdds_large_images.xml"
|
|
),
|
|
),
|
|
SetEnvironmentVariable(
|
|
name="FASTRTPS_DEFAULT_PROFILES_FILE",
|
|
value=str(
|
|
package_share / "config" / "fastdds_large_images.xml"
|
|
),
|
|
),
|
|
DeclareLaunchArgument("front_camera_serial", default_value="DB2163742"),
|
|
DeclareLaunchArgument("side_camera_serial", default_value="DB2163749"),
|
|
DeclareLaunchArgument("top_camera_serial", default_value="DB2163739"),
|
|
DeclareLaunchArgument("camera_model", default_value="MV-CS020-10U"),
|
|
DeclareLaunchArgument("front_camera_name", default_value="hikrobot_front_DB2163742"),
|
|
DeclareLaunchArgument("side_camera_name", default_value="hikrobot_side_DB2163749"),
|
|
DeclareLaunchArgument("top_camera_name", default_value="hikrobot_top_DB2163739"),
|
|
DeclareLaunchArgument("front_camera_info_url", default_value=str(camera_info / "hikrobot_DB2163742.yaml")),
|
|
DeclareLaunchArgument("side_camera_info_url", default_value=str(camera_info / "hikrobot_DB2163749.yaml")),
|
|
DeclareLaunchArgument("top_camera_info_url", default_value=str(camera_info / "hikrobot_DB2163739.yaml")),
|
|
DeclareLaunchArgument("camera_frame_rate", default_value="15.0"),
|
|
DeclareLaunchArgument("exposure_time_us", default_value="5000.0"),
|
|
DeclareLaunchArgument("gain_db", default_value="0.0"),
|
|
DeclareLaunchArgument("checkerboard_columns", default_value="8"),
|
|
DeclareLaunchArgument("checkerboard_rows", default_value="5"),
|
|
DeclareLaunchArgument("square_size_m", default_value="0.027"),
|
|
DeclareLaunchArgument("enable_gui", default_value="true"),
|
|
DeclareLaunchArgument("gui_refresh_hz", default_value="2.0"),
|
|
DeclareLaunchArgument(
|
|
"maximum_reprojection_rms_px", default_value="1.2"
|
|
),
|
|
DeclareLaunchArgument(
|
|
"maximum_candidate_pair_reprojection_rms_px",
|
|
default_value="1.5",
|
|
),
|
|
DeclareLaunchArgument(
|
|
"maximum_single_camera_reprojection_rms_px",
|
|
default_value="1.5",
|
|
),
|
|
DeclareLaunchArgument("auto_capture_default", default_value="false"),
|
|
DeclareLaunchArgument(
|
|
"auto_capture_stable_seconds", default_value="1.0"
|
|
),
|
|
DeclareLaunchArgument(
|
|
"output_file",
|
|
default_value=str(Path.cwd() / "config" / "g20_three_camera_extrinsics.yaml"),
|
|
),
|
|
OpaqueFunction(function=_launch),
|
|
]
|
|
)
|