392 lines
15 KiB
Python
392 lines
15 KiB
Python
"""Launch the complete front-camera G20 thumb calibration stack."""
|
|
|
|
from __future__ import annotations
|
|
|
|
from datetime import datetime
|
|
from pathlib import Path
|
|
import re
|
|
|
|
from ament_index_python.packages import get_package_share_directory
|
|
from launch import LaunchDescription
|
|
from launch.actions import (
|
|
DeclareLaunchArgument,
|
|
ExecuteProcess,
|
|
LogInfo,
|
|
OpaqueFunction,
|
|
SetEnvironmentVariable,
|
|
)
|
|
from launch.conditions import IfCondition
|
|
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
|
|
|
|
|
|
def _launch_stack(context):
|
|
serial_number = LaunchConfiguration("serial_number").perform(context)
|
|
if not serial_number or serial_number == "UNSET":
|
|
raise RuntimeError(
|
|
"serial_number is required, for example serial_number:=G20_LEFT_001"
|
|
)
|
|
if (
|
|
re.fullmatch(r"[A-Za-z0-9_.-]+", serial_number) is None
|
|
or serial_number in {".", ".."}
|
|
):
|
|
raise RuntimeError(
|
|
"serial_number may contain only letters, digits, dot, underscore and dash"
|
|
)
|
|
requested_session = LaunchConfiguration("session_dir").perform(context)
|
|
output_root = Path(LaunchConfiguration("output_root").perform(context)).resolve()
|
|
if requested_session:
|
|
session_dir = Path(requested_session).expanduser().resolve()
|
|
else:
|
|
timestamp = datetime.now().strftime("%Y%m%d_%H%M%S")
|
|
session_dir = output_root / serial_number / timestamp
|
|
session_dir.mkdir(parents=True, exist_ok=True)
|
|
bag_path = session_dir / "rosbag"
|
|
|
|
calibration_config = LaunchConfiguration("calibration_config").perform(context)
|
|
tag_config = LaunchConfiguration("tag_config").perform(context)
|
|
use_roi_text = LaunchConfiguration("use_roi").perform(context).strip().lower()
|
|
if use_roi_text not in {"true", "false"}:
|
|
raise RuntimeError("use_roi must be true or false")
|
|
use_roi = use_roi_text == "true"
|
|
|
|
roi_values = {}
|
|
for name in ("roi_x", "roi_y", "roi_width", "roi_height"):
|
|
text = LaunchConfiguration(name).perform(context)
|
|
try:
|
|
roi_values[name] = int(text)
|
|
except ValueError as error:
|
|
raise RuntimeError(f"{name} must be an integer") from error
|
|
if roi_values["roi_x"] < 0 or roi_values["roi_y"] < 0:
|
|
raise RuntimeError("roi_x and roi_y must be non-negative")
|
|
if roi_values["roi_width"] <= 0 or roi_values["roi_height"] <= 0:
|
|
raise RuntimeError("roi_width and roi_height must be positive")
|
|
|
|
try:
|
|
image_width = int(LaunchConfiguration("image_width").perform(context))
|
|
image_height = int(LaunchConfiguration("image_height").perform(context))
|
|
except ValueError as error:
|
|
raise RuntimeError("image_width and image_height must be integers") from error
|
|
if image_width <= 0 or image_height <= 0:
|
|
raise RuntimeError("image_width and image_height must be positive")
|
|
if use_roi:
|
|
if (
|
|
roi_values["roi_x"] + roi_values["roi_width"] > image_width
|
|
or roi_values["roi_y"] + roi_values["roi_height"] > image_height
|
|
):
|
|
raise RuntimeError(
|
|
"ROI lies outside camera image "
|
|
f"{image_width}x{image_height}"
|
|
)
|
|
|
|
camera = Node(
|
|
package="linkerhand_calibration",
|
|
executable="hikrobot_camera_node",
|
|
name="hikrobot_camera",
|
|
namespace="/camera/camera/color",
|
|
output="screen",
|
|
emulate_tty=True,
|
|
condition=IfCondition(LaunchConfiguration("start_camera")),
|
|
parameters=[
|
|
{
|
|
"serial_number": LaunchConfiguration("camera_serial_number"),
|
|
"expected_model": LaunchConfiguration("camera_model"),
|
|
"camera_name": LaunchConfiguration("camera_name"),
|
|
"frame_id": LaunchConfiguration("camera_frame_id"),
|
|
"image_width": ParameterValue(
|
|
LaunchConfiguration("image_width"), value_type=int
|
|
),
|
|
"image_height": ParameterValue(
|
|
LaunchConfiguration("image_height"), value_type=int
|
|
),
|
|
"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": ParameterValue(
|
|
LaunchConfiguration("auto_exposure"), value_type=bool
|
|
),
|
|
"camera_info_url": LaunchConfiguration("camera_info_url"),
|
|
}
|
|
],
|
|
)
|
|
|
|
vision_components = []
|
|
if use_roi:
|
|
processed_image_raw_topic = "/g20_thumb_roi/image_raw"
|
|
processed_camera_info_topic = "/g20_thumb_roi/camera_info"
|
|
processed_image_rect_topic = "/g20_thumb_roi/image_rect"
|
|
vision_components.append(
|
|
ComposableNode(
|
|
package="image_proc",
|
|
plugin="image_proc::CropDecimateNode",
|
|
name="crop_color_roi",
|
|
namespace="/g20_thumb_roi",
|
|
remappings=[
|
|
("in/image_raw", "/camera/camera/color/image_raw"),
|
|
("in/camera_info", "/camera/camera/color/camera_info"),
|
|
("out/image_raw", processed_image_raw_topic),
|
|
("out/camera_info", processed_camera_info_topic),
|
|
],
|
|
parameters=[
|
|
{
|
|
"queue_size": 5,
|
|
"decimation_x": 1,
|
|
"decimation_y": 1,
|
|
"offset_x": roi_values["roi_x"],
|
|
"offset_y": roi_values["roi_y"],
|
|
"width": roi_values["roi_width"],
|
|
"height": roi_values["roi_height"],
|
|
}
|
|
],
|
|
extra_arguments=[{"use_intra_process_comms": True}],
|
|
)
|
|
)
|
|
rectifier_namespace = "/g20_thumb_roi"
|
|
rectifier_name = "rectify_color_roi"
|
|
else:
|
|
processed_image_raw_topic = "/camera/camera/color/image_raw"
|
|
processed_camera_info_topic = "/camera/camera/color/camera_info"
|
|
processed_image_rect_topic = "/camera/camera/color/image_rect"
|
|
rectifier_namespace = "/camera/camera/color"
|
|
rectifier_name = "rectify_color"
|
|
|
|
vision_components.append(
|
|
ComposableNode(
|
|
package="image_proc",
|
|
plugin="image_proc::RectifyNode",
|
|
name=rectifier_name,
|
|
namespace=rectifier_namespace,
|
|
remappings=[
|
|
("image", processed_image_raw_topic),
|
|
("camera_info", processed_camera_info_topic),
|
|
("image_rect", processed_image_rect_topic),
|
|
],
|
|
parameters=[{"queue_size": 1}],
|
|
extra_arguments=[{"use_intra_process_comms": True}],
|
|
)
|
|
)
|
|
|
|
vision_components.append(
|
|
ComposableNode(
|
|
package="apriltag_ros",
|
|
plugin="AprilTagNode",
|
|
name="apriltag",
|
|
namespace="/apriltag",
|
|
parameters=[
|
|
tag_config,
|
|
{
|
|
"detector.decimate": ParameterValue(
|
|
LaunchConfiguration("apriltag_decimate"),
|
|
value_type=float,
|
|
)
|
|
},
|
|
],
|
|
remappings=[
|
|
("image_rect", processed_image_rect_topic),
|
|
("camera_info", processed_camera_info_topic),
|
|
],
|
|
extra_arguments=[{"use_intra_process_comms": True}],
|
|
)
|
|
)
|
|
|
|
vision_container = ComposableNodeContainer(
|
|
name="g20_thumb_vision_container",
|
|
namespace="/",
|
|
package="rclcpp_components",
|
|
executable="component_container_mt",
|
|
composable_node_descriptions=vision_components,
|
|
output="screen",
|
|
emulate_tty=True,
|
|
)
|
|
|
|
sdk = Node(
|
|
package="linker_hand_ros2_sdk",
|
|
executable="linker_hand_sdk",
|
|
name="linker_hand_sdk",
|
|
output="screen",
|
|
condition=IfCondition(LaunchConfiguration("start_sdk")),
|
|
parameters=[
|
|
{
|
|
"hand_type": "left",
|
|
"hand_joint": "G20",
|
|
"can": LaunchConfiguration("can_interface"),
|
|
"modbus": "None",
|
|
"topic_prefix": "/g20",
|
|
"move_on_startup": False,
|
|
"startup_speed": ParameterValue(
|
|
LaunchConfiguration("calibration_speed"),
|
|
value_type=int,
|
|
),
|
|
"startup_torque": 80,
|
|
"state_poll_rate": 10.0,
|
|
"repeat_position_commands": False,
|
|
"is_touch": False,
|
|
}
|
|
],
|
|
)
|
|
|
|
calibration = Node(
|
|
package="linkerhand_calibration",
|
|
executable="calibration_node",
|
|
name="g20_thumb_calibration",
|
|
output="screen",
|
|
parameters=[
|
|
calibration_config,
|
|
tag_config,
|
|
{
|
|
"serial_number": serial_number,
|
|
"session_dir": str(session_dir),
|
|
"commands_enabled": LaunchConfiguration("commands_enabled"),
|
|
"calibration_speed": ParameterValue(
|
|
LaunchConfiguration("calibration_speed"),
|
|
value_type=int,
|
|
),
|
|
"continuous_motion_mode": ParameterValue(
|
|
LaunchConfiguration("continuous_motion_mode"),
|
|
value_type=str,
|
|
),
|
|
"angle_estimation_mode": ParameterValue(
|
|
LaunchConfiguration("angle_estimation_mode"),
|
|
value_type=str,
|
|
),
|
|
"camera_serial_number": LaunchConfiguration("camera_serial_number"),
|
|
"rosbag_path": str(bag_path),
|
|
"camera_info_topic": processed_camera_info_topic,
|
|
"image_topic": processed_image_rect_topic,
|
|
"publish_debug_image": LaunchConfiguration(
|
|
"publish_debug_image"
|
|
),
|
|
},
|
|
],
|
|
)
|
|
|
|
bag = ExecuteProcess(
|
|
condition=IfCondition(LaunchConfiguration("record_bag")),
|
|
cmd=[
|
|
"ros2",
|
|
"bag",
|
|
"record",
|
|
"--storage",
|
|
"mcap",
|
|
"--storage-preset-profile",
|
|
"zstd_fast",
|
|
"--max-bag-size",
|
|
"10737418240",
|
|
"--output",
|
|
str(bag_path),
|
|
processed_image_raw_topic,
|
|
processed_camera_info_topic,
|
|
"/apriltag/detections",
|
|
"/tf",
|
|
"/g20/cb_left_hand_control_cmd",
|
|
"/g20/cb_left_hand_state",
|
|
"/g20/cb_left_hand_info",
|
|
"/g20_thumb_calibration/status",
|
|
],
|
|
output="screen",
|
|
)
|
|
|
|
actions = [
|
|
LogInfo(msg=f"G20 thumb calibration session: {session_dir}"),
|
|
LogInfo(
|
|
msg=(
|
|
"G20 thumb image ROI: "
|
|
f"x={roi_values['roi_x']}, y={roi_values['roi_y']}, "
|
|
f"width={roi_values['roi_width']}, "
|
|
f"height={roi_values['roi_height']}"
|
|
if use_roi
|
|
else "G20 thumb image ROI: disabled"
|
|
)
|
|
),
|
|
camera,
|
|
vision_container,
|
|
]
|
|
actions.extend([sdk, calibration, bag])
|
|
return actions
|
|
|
|
|
|
def generate_launch_description() -> LaunchDescription:
|
|
package_share = Path(
|
|
get_package_share_directory("linkerhand_calibration")
|
|
)
|
|
default_output = str(Path.cwd() / "calibration_output")
|
|
return LaunchDescription(
|
|
[
|
|
SetEnvironmentVariable(
|
|
name="FASTRTPS_DEFAULT_PROFILES_FILE",
|
|
value=str(
|
|
package_share / "config" / "fastdds_large_images.xml"
|
|
),
|
|
),
|
|
DeclareLaunchArgument("serial_number", default_value="UNSET"),
|
|
DeclareLaunchArgument(
|
|
"camera_serial_number", default_value="DB2163742"
|
|
),
|
|
DeclareLaunchArgument(
|
|
"camera_model", default_value="MV-CS020-10UM"
|
|
),
|
|
DeclareLaunchArgument(
|
|
"camera_name", default_value="hikrobot_front_DB2163742"
|
|
),
|
|
DeclareLaunchArgument(
|
|
"camera_frame_id", default_value="camera_color_optical_frame"
|
|
),
|
|
DeclareLaunchArgument("image_width", default_value="1624"),
|
|
DeclareLaunchArgument("image_height", default_value="1240"),
|
|
DeclareLaunchArgument("camera_frame_rate", default_value="30.0"),
|
|
DeclareLaunchArgument("exposure_time_us", default_value="5000.0"),
|
|
DeclareLaunchArgument("gain_db", default_value="0.0"),
|
|
DeclareLaunchArgument("auto_exposure", default_value="false"),
|
|
DeclareLaunchArgument(
|
|
"camera_info_url",
|
|
default_value=str(
|
|
Path.home()
|
|
/ ".ros"
|
|
/ "camera_info"
|
|
/ "hikrobot_DB2163742.yaml"
|
|
),
|
|
),
|
|
DeclareLaunchArgument(
|
|
"publish_debug_image", default_value="false"
|
|
),
|
|
DeclareLaunchArgument("use_roi", default_value="false"),
|
|
DeclareLaunchArgument("roi_x", default_value="128"),
|
|
DeclareLaunchArgument("roi_y", default_value="192"),
|
|
DeclareLaunchArgument("roi_width", default_value="1024"),
|
|
DeclareLaunchArgument("roi_height", default_value="528"),
|
|
DeclareLaunchArgument("can_interface", default_value="can0"),
|
|
DeclareLaunchArgument("calibration_speed", default_value="15"),
|
|
DeclareLaunchArgument(
|
|
"continuous_motion_mode", default_value="endpoint"
|
|
),
|
|
DeclareLaunchArgument(
|
|
"angle_estimation_mode",
|
|
default_value="trajectory_center_3d",
|
|
),
|
|
DeclareLaunchArgument("apriltag_decimate", default_value="1.5"),
|
|
DeclareLaunchArgument("commands_enabled", default_value="true"),
|
|
DeclareLaunchArgument("start_camera", default_value="true"),
|
|
DeclareLaunchArgument("start_sdk", default_value="true"),
|
|
DeclareLaunchArgument("record_bag", default_value="false"),
|
|
DeclareLaunchArgument("output_root", default_value=default_output),
|
|
DeclareLaunchArgument("session_dir", default_value=""),
|
|
DeclareLaunchArgument(
|
|
"calibration_config",
|
|
default_value=str(package_share / "config" / "calibration.yaml"),
|
|
),
|
|
DeclareLaunchArgument(
|
|
"tag_config",
|
|
default_value=str(package_share / "config" / "front_tags.yaml"),
|
|
),
|
|
OpaqueFunction(function=_launch_stack),
|
|
]
|
|
)
|