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..66bd9965 --- /dev/null +++ b/launch/camera_launch.py @@ -0,0 +1,34 @@ +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="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. + Node( + package="camera_ros", + executable="camera_node", + name="camera", + output="screen", # Adjust the output as needed. + 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"}, + ], + remappings=[], # Add remappings if necessary. + arguments=["--log-level", LaunchConfiguration("log_level")], + ), + # You can add additional nodes or actions here as needed. + ] + ) diff --git a/src/CameraNode.cpp b/src/CameraNode.cpp index 1b22a385..28bdcdca 100644 --- a/src/CameraNode.cpp +++ b/src/CameraNode.cpp @@ -21,6 +21,7 @@ #include #include #include +#include #include #include #include @@ -161,14 +162,26 @@ 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)); + 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(); @@ -216,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"); @@ -266,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(); @@ -276,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; @@ -394,6 +431,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;