diff --git a/launch/camera.launch.py b/launch/camera.launch.py index 119acd75..0e25b4f5 100644 --- a/launch/camera.launch.py +++ b/launch/camera.launch.py @@ -1,4 +1,6 @@ +from launch.actions import DeclareLaunchArgument from launch.launch_description import LaunchDescription +from launch.substitutions import LaunchConfiguration from launch_ros.actions import ComposableNodeContainer from launch_ros.descriptions import ComposableNode @@ -13,6 +15,32 @@ def generate_launch_description() -> LaunchDescription: LaunchDescription: the launch description """ + # parameters + camera_param_name = "camera" + camera_param_default = str(0) + camera_param = LaunchConfiguration( + camera_param_name, + default=camera_param_default, + ) + camera_launch_arg = DeclareLaunchArgument( + camera_param_name, + default_value=camera_param_default, + description="camera ID or name" + ) + + format_param_name = "format" + format_param_default = str() + format_param = LaunchConfiguration( + format_param_name, + default=format_param_default, + ) + format_launch_arg = DeclareLaunchArgument( + format_param_name, + default_value=format_param_default, + description="pixel format" + ) + + # composable nodes in single container container = ComposableNodeContainer( name='camera_container', namespace='', @@ -23,9 +51,10 @@ def generate_launch_description() -> LaunchDescription: package='camera_ros', plugin='camera::CameraNode', parameters=[{ - "camera": 0, + "camera": camera_param, "width": 640, "height": 480, + "format": format_param, }], extra_arguments=[{'use_intra_process_comms': True}], ), @@ -38,4 +67,8 @@ def generate_launch_description() -> LaunchDescription: ], ) - return LaunchDescription([container]) + return LaunchDescription([ + container, + camera_launch_arg, + format_launch_arg, + ]) diff --git a/src/CameraNode.cpp b/src/CameraNode.cpp index 9010eefe..4219c9e9 100644 --- a/src/CameraNode.cpp +++ b/src/CameraNode.cpp @@ -208,7 +208,7 @@ CameraNode::CameraNode(const rclcpp::NodeOptions &options) : Node("camera", opti RCLCPP_INFO_STREAM(get_logger(), camera_manager); RCLCPP_WARN_STREAM(get_logger(), "no camera selected, using default: \"" << camera->id() << "\""); - RCLCPP_WARN_STREAM(get_logger(), "set parameter 'camera' to silent this warning"); + RCLCPP_WARN_STREAM(get_logger(), "set parameter 'camera' to silence this warning"); break; case rclcpp::ParameterType::PARAMETER_INTEGER: { @@ -255,6 +255,7 @@ CameraNode::CameraNode(const rclcpp::NodeOptions &options) : Node("camera", opti const std::vector common_fmt = stream_formats.pixelformats(); // list all camera formats, including those not supported by the ROS message + RCLCPP_DEBUG_STREAM(get_logger(), "default stream configuration: \"" << scfg.toString() << "\""); RCLCPP_DEBUG_STREAM(get_logger(), scfg.formats()); if (common_fmt.empty()) @@ -265,13 +266,14 @@ CameraNode::CameraNode(const rclcpp::NodeOptions &options) : Node("camera", opti // check that the default pixel format is supported by the ROS encoding if (std::find(common_fmt.cbegin(), common_fmt.cend(), scfg.pixelFormat) == common_fmt.cend()) { // auto select first common pixel format + RCLCPP_WARN_STREAM(get_logger(), "default pixel format (" << scfg.pixelFormat << ") not supported"); scfg.pixelFormat = common_fmt.front(); } RCLCPP_INFO_STREAM(get_logger(), stream_formats); RCLCPP_WARN_STREAM(get_logger(), - "no pixel format selected, using default: \"" << scfg.pixelFormat << "\""); - RCLCPP_WARN_STREAM(get_logger(), "set parameter 'format' to silent this warning"); + "no pixel format selected, auto-selecting: \"" << scfg.pixelFormat << "\""); + RCLCPP_WARN_STREAM(get_logger(), "set parameter 'format' to silence this warning"); } else { // get pixel format from provided string @@ -293,7 +295,7 @@ CameraNode::CameraNode(const rclcpp::NodeOptions &options) : Node("camera", opti RCLCPP_INFO_STREAM(get_logger(), scfg); RCLCPP_WARN_STREAM(get_logger(), "no dimensions selected, auto-selecting: \"" << scfg.size << "\""); - RCLCPP_WARN_STREAM(get_logger(), "set parameter 'width' or 'height' to silent this warning"); + RCLCPP_WARN_STREAM(get_logger(), "set parameter 'width' or 'height' to silence this warning"); } else { scfg.size = size; diff --git a/src/format_mapping.cpp b/src/format_mapping.cpp index 7e9e3e3f..21cf348b 100644 --- a/src/format_mapping.cpp +++ b/src/format_mapping.cpp @@ -9,8 +9,10 @@ namespace cam = libcamera::formats; namespace ros = sensor_msgs::image_encodings; -//mapping of FourCC to ROS image encodings -// see 'include/uapi/drm/drm_fourcc.h' for a full FourCC list +// mapping of FourCC to ROS image encodings +// references: +// - full list of FourCC: 'include/uapi/drm/drm_fourcc.h' +// - V4L2 image formats: 'https://docs.kernel.org/userspace-api/media/v4l/pixfmt.html' // supported FourCC formats, without conversion static const std::unordered_map map_format_raw = {