From 6601ad986bf05d75ef784dafe57f2fc9a15eb707 Mon Sep 17 00:00:00 2001 From: Woojin Wie Date: Sat, 28 Oct 2023 18:30:38 +0900 Subject: [PATCH 1/6] Update CameraNode.cpp --- src/CameraNode.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/CameraNode.cpp b/src/CameraNode.cpp index 1b22a385..f09cb7dd 100644 --- a/src/CameraNode.cpp +++ b/src/CameraNode.cpp @@ -394,6 +394,8 @@ CameraNode::declareParameters() // dynamic camera configuration ParameterMap parameters_init; for (const auto &[id, info] : camera->controls()) { + if (id->name() == "AfWindows") + continue; // store control id with name parameter_ids[id->name()] = id; From 6b1d368f65d2fa6822594d64e84056f2631149e2 Mon Sep 17 00:00:00 2001 From: Woojin Wie Date: Sat, 28 Oct 2023 18:39:24 +0900 Subject: [PATCH 2/6] provide qos_override options --- src/CameraNode.cpp | 15 ++++++++++++--- 1 file changed, 12 insertions(+), 3 deletions(-) diff --git a/src/CameraNode.cpp b/src/CameraNode.cpp index f09cb7dd..6e8e363b 100644 --- a/src/CameraNode.cpp +++ b/src/CameraNode.cpp @@ -164,11 +164,20 @@ CameraNode::CameraNode(const rclcpp::NodeOptions &options) : Node("camera", opti // camera ID declare_parameter("camera", rclcpp::ParameterValue {}, param_descr_ro.set__dynamic_typing(true)); + auto qos_override_options = rclcpp::QosOverridingOptions({ + rclcpp::QosPolicyKind::Depth, + rclcpp::QosPolicyKind::History, + rclcpp::QosPolicyKind::Reliability, + rclcpp::QosPolicyKind::Durability, + }); + + rclcpp::PublisherOptions pub_options; + pub_options.qos_overriding_options = qos_override_options; // publisher for raw and compressed image - pub_image = this->create_publisher("~/image_raw", 1); + pub_image = this->create_publisher("~/image_raw", 1, pub_options); pub_image_compressed = - this->create_publisher("~/image_raw/compressed", 1); - pub_ci = this->create_publisher("~/camera_info", 1); + this->create_publisher("~/image_raw/compressed", 1, pub_options); + pub_ci = this->create_publisher("~/camera_info", 1, pub_options); // start camera manager and check for cameras camera_manager.start(); From 17d361f3d156c56b2ea3cd0c84da0f29cdd23760 Mon Sep 17 00:00:00 2001 From: Woojin Wie Date: Sat, 28 Oct 2023 18:49:35 +0900 Subject: [PATCH 3/6] make launch file Update camera_launch.py --- CMakeLists.txt | 6 ++++++ launch/camera_launch.py | 32 ++++++++++++++++++++++++++++++++ 2 files changed, 38 insertions(+) create mode 100644 launch/camera_launch.py diff --git a/CMakeLists.txt b/CMakeLists.txt index ed9555ee..62c75918 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -60,6 +60,12 @@ target_link_libraries(camera_component ${libcamera_LINK_LIBRARIES} utils) install(TARGETS camera_component DESTINATION lib) +# install the launch directory +install(DIRECTORY + launch + DESTINATION share/${PROJECT_NAME}/ +) + if(BUILD_TESTING) set(ament_cmake_clang_format_CONFIG_FILE "${CMAKE_SOURCE_DIR}/.clang-format") find_package(ament_lint_auto REQUIRED) diff --git a/launch/camera_launch.py b/launch/camera_launch.py new file mode 100644 index 00000000..4111dbff --- /dev/null +++ b/launch/camera_launch.py @@ -0,0 +1,32 @@ +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, LogInfo +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node + + +def generate_launch_description(): + return LaunchDescription( + [ + DeclareLaunchArgument("width", default_value="1280", description="Camera image width"), + DeclareLaunchArgument("height", default_value="720", description="Camera image height"), + DeclareLaunchArgument("log_level", default_value="debug", description="Log level (e.g., debug)"), + # You can add other launch arguments as needed. + # Run the camera_node with the specified parameters. + Node( + package="camera_ros", + executable="camera_node", + name="camera", + output="screen", # Adjust the output as needed. + parameters=[ + {"width": LaunchConfiguration("width")}, + {"height": LaunchConfiguration("height")}, + {"qos_overrides./camera/image_raw.publisher.reliability": "best_effort"}, + {"qos_overrides./camera/image_raw/compressed.publisher.reliability": "best_effort"}, + {"qos_overrides./camera/camera_info.publisher.reliability": "best_effort"}, + ], + remappings=[], # Add remappings if necessary. + arguments=["--log-level", LaunchConfiguration("log_level")], + ), + # You can add additional nodes or actions here as needed. + ] + ) From c98c5d8d879431dce598f2d2e7af2f522ddd40f6 Mon Sep 17 00:00:00 2001 From: Woojin Wie Date: Sat, 28 Oct 2023 21:23:33 +0900 Subject: [PATCH 4/6] Update camera_launch.py --- launch/camera_launch.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/launch/camera_launch.py b/launch/camera_launch.py index 4111dbff..8fafb7ef 100644 --- a/launch/camera_launch.py +++ b/launch/camera_launch.py @@ -22,7 +22,7 @@ def generate_launch_description(): {"height": LaunchConfiguration("height")}, {"qos_overrides./camera/image_raw.publisher.reliability": "best_effort"}, {"qos_overrides./camera/image_raw/compressed.publisher.reliability": "best_effort"}, - {"qos_overrides./camera/camera_info.publisher.reliability": "best_effort"}, + # {"qos_overrides./camera/camera_info.publisher.reliability": "best_effort"}, ], remappings=[], # Add remappings if necessary. arguments=["--log-level", LaunchConfiguration("log_level")], From 91ac144e6a4964770a79ab5d4b2f17cf7d46b616 Mon Sep 17 00:00:00 2001 From: Woojin Wie Date: Mon, 30 Oct 2023 03:26:45 +0900 Subject: [PATCH 5/6] add raw role for uncropped image --- launch/camera_launch.py | 2 ++ src/CameraNode.cpp | 30 +++++++++++++++++++++++++++++- 2 files changed, 31 insertions(+), 1 deletion(-) diff --git a/launch/camera_launch.py b/launch/camera_launch.py index 8fafb7ef..11ad9d6a 100644 --- a/launch/camera_launch.py +++ b/launch/camera_launch.py @@ -20,6 +20,8 @@ def generate_launch_description(): parameters=[ {"width": LaunchConfiguration("width")}, {"height": LaunchConfiguration("height")}, + {"mode_width": 2304}, + {"mode_height": 1296}, {"qos_overrides./camera/image_raw.publisher.reliability": "best_effort"}, {"qos_overrides./camera/image_raw/compressed.publisher.reliability": "best_effort"}, # {"qos_overrides./camera/camera_info.publisher.reliability": "best_effort"}, diff --git a/src/CameraNode.cpp b/src/CameraNode.cpp index 6e8e363b..28bdcdca 100644 --- a/src/CameraNode.cpp +++ b/src/CameraNode.cpp @@ -21,6 +21,7 @@ #include #include #include +#include #include #include #include @@ -161,6 +162,9 @@ CameraNode::CameraNode(const rclcpp::NodeOptions &options) : Node("camera", opti declare_parameter("width", {}, param_descr_ro); declare_parameter("height", {}, param_descr_ro); + declare_parameter("mode_width", {}, param_descr_ro); + declare_parameter("mode_height", {}, param_descr_ro); + // camera ID declare_parameter("camera", rclcpp::ParameterValue {}, param_descr_ro.set__dynamic_typing(true)); @@ -225,9 +229,18 @@ CameraNode::CameraNode(const rclcpp::NodeOptions &options) : Node("camera", opti if (camera->acquire()) throw std::runtime_error("failed to acquire camera"); + std::vector roles; + + roles.push_back(get_role(get_parameter("role").as_string())); + const libcamera::Size mode_size(get_parameter("mode_width").as_int(), get_parameter("mode_height").as_int()); + + if(!mode_size.isNull()) { + roles.push_back(libcamera::StreamRole::Raw); + } + // configure camera stream std::unique_ptr cfg = - camera->generateConfiguration({get_role(get_parameter("role").as_string())}); + camera->generateConfiguration({roles}); if (!cfg) throw std::runtime_error("failed to generate configuration"); @@ -275,6 +288,7 @@ CameraNode::CameraNode(const rclcpp::NodeOptions &options) : Node("camera", opti } const libcamera::Size size(get_parameter("width").as_int(), get_parameter("height").as_int()); + if (size.isNull()) { RCLCPP_INFO_STREAM(get_logger(), scfg); scfg.size = scfg.formats().sizes(scfg.pixelFormat).back(); @@ -285,6 +299,20 @@ CameraNode::CameraNode(const rclcpp::NodeOptions &options) : Node("camera", opti scfg.size = size; } + if(!mode_size.isNull()) { + RCLCPP_INFO_STREAM(get_logger(), mode_size); + libcamera::StreamConfiguration &modecfg = cfg->at(1); + modecfg.size = mode_size; + + modecfg.pixelFormat = libcamera::formats::SBGGR12_CSI2P; + + RCLCPP_INFO_STREAM(get_logger(), "modecfg "); + RCLCPP_INFO_STREAM(get_logger(), modecfg); + + set_parameter(rclcpp::Parameter("mode_width", int64_t(modecfg.size.width))); + set_parameter(rclcpp::Parameter("mode_height", int64_t(modecfg.size.height))); + } + // store selected stream configuration const libcamera::StreamConfiguration selected_scfg = scfg; From 014dbb8ce0e0ba4ca1f8ac41edc3fa26a8dda048 Mon Sep 17 00:00:00 2001 From: Woojin Wie Date: Mon, 30 Oct 2023 03:30:11 +0900 Subject: [PATCH 6/6] Update camera_launch.py --- launch/camera_launch.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/launch/camera_launch.py b/launch/camera_launch.py index 11ad9d6a..66bd9965 100644 --- a/launch/camera_launch.py +++ b/launch/camera_launch.py @@ -7,8 +7,8 @@ def generate_launch_description(): return LaunchDescription( [ - DeclareLaunchArgument("width", default_value="1280", description="Camera image width"), - DeclareLaunchArgument("height", default_value="720", description="Camera image height"), + DeclareLaunchArgument("width", default_value="640", description="Camera image width"), + DeclareLaunchArgument("height", default_value="320", description="Camera image height"), DeclareLaunchArgument("log_level", default_value="debug", description="Log level (e.g., debug)"), # You can add other launch arguments as needed. # Run the camera_node with the specified parameters.