diff --git a/psdk_interfaces/srv/CameraSetupStreaming.srv b/psdk_interfaces/srv/CameraSetupStreaming.srv index 2d98aee5..40cb5d0d 100644 --- a/psdk_interfaces/srv/CameraSetupStreaming.srv +++ b/psdk_interfaces/srv/CameraSetupStreaming.srv @@ -2,6 +2,7 @@ uint8 payload_index 1 # please refer to enum E_DjiLiveViewCameraPosition in dji_liveview.h uint8 camera_source 0 # please refer to enum E_DjiLiveViewCameraSource in dji_liveview.h bool start_stop # 0 for stop and 1 for start +bool decoded_output 1 # 0 for encoded output and 1 for decoded output --- #response bool success \ No newline at end of file diff --git a/psdk_wrapper/include/psdk_wrapper/psdk_wrapper.hpp b/psdk_wrapper/include/psdk_wrapper/psdk_wrapper.hpp index c25baf52..76e04ed0 100644 --- a/psdk_wrapper/include/psdk_wrapper/psdk_wrapper.hpp +++ b/psdk_wrapper/include/psdk_wrapper/psdk_wrapper.hpp @@ -1547,6 +1547,7 @@ class PSDKWrapper : public rclcpp_lifecycle::LifecycleNode * @param request CameraSetupStreaming service request. The camera * mounted position for which the request is made needs to be specified as * well as the camera source (e.g. using the wide or the zoom camera). + * Moreover, the user can choose to stream the images raw or decoded. * @param response CameraSetupStreaming service response. */ void camera_setup_streaming_cb( @@ -1830,7 +1831,7 @@ class PSDKWrapper : public rclcpp_lifecycle::LifecycleNode void set_local_altitude_reference(const float altitude); /** - * @brief Starts the main camera streaming. + * @brief Starts the camera streaming. * @param callback function to be executed when a frame is received * @param user_data unused parameter * @param payload_index select which camera to use to retrieve the streaming. @@ -1841,7 +1842,7 @@ class PSDKWrapper : public rclcpp_lifecycle::LifecycleNode * @return true/false Returns true if the streaming has been started * correctly and False otherwise. */ - bool start_main_camera_stream(CameraImageCallback callback, void* user_data, + bool start_camera_stream(CameraImageCallback callback, void* user_data, const E_DjiLiveViewCameraPosition payload_index, const E_DjiLiveViewCameraSource camera_source); /** @@ -1862,12 +1863,31 @@ class PSDKWrapper : public rclcpp_lifecycle::LifecycleNode * @param user_data unused parameter */ void publish_main_camera_images(CameraRGBImage rgb_img, void* user_data); + + /** + * @brief Publishes the raw (not decoded) main camera streaming to a ROS 2 + * topic + * @param buffer raw buffer retrieved from the camera + * @param buffer_length length of the buffer + */ + void publish_main_camera_images(const uint8_t* buffer, + uint32_t buffer_length); + /** * @brief Publishes the FPV camera streaming to a ROS 2 topic * @param rgb_img decoded RGB frame retrieved from the camera * @param user_data unused parameter */ void publish_fpv_camera_images(CameraRGBImage rgb_img, void* user_data); + + /** + * @brief Publishes the raw (not decoded) FPV camera streaming to a ROS 2 + * topic + * @param buffer raw buffer retrieved from the camera + * @param buffer_length length of the buffer + */ + void publish_fpv_camera_images(const uint8_t* buffer, uint32_t buffer_length); + /** * @brief Get the optical frame id for a certain lens * @return string with the optical frame id name @@ -1896,6 +1916,7 @@ class PSDKWrapper : public rclcpp_lifecycle::LifecycleNode E_DjiCameraType attached_camera_type_; E_DjiLiveViewCameraSource selected_camera_source_; bool publish_camera_transforms_{false}; + bool decode_stream_{true}; }; /** diff --git a/psdk_wrapper/src/modules/liveview.cpp b/psdk_wrapper/src/modules/liveview.cpp index 3c0e6625..2bff29ef 100644 --- a/psdk_wrapper/src/modules/liveview.cpp +++ b/psdk_wrapper/src/modules/liveview.cpp @@ -38,6 +38,7 @@ PSDKWrapper::init_liveview() {DJI_LIVEVIEW_CAMERA_POSITION_NO_2, (new DJICameraStreamDecoder())}, {DJI_LIVEVIEW_CAMERA_POSITION_NO_3, (new DJICameraStreamDecoder())}, }; + decode_stream_ = true; return true; } @@ -58,8 +59,12 @@ c_LiveviewConvertH264ToRgbCallback(E_DjiLiveViewCameraPosition position, const uint8_t *buffer, uint32_t buffer_length) { - return global_ptr_->LiveviewConvertH264ToRgbCallback(position, buffer, - buffer_length); + if (global_ptr_->decode_stream_) + { + return global_ptr_->LiveviewConvertH264ToRgbCallback(position, buffer, + buffer_length); + } + return global_ptr_->publish_main_camera_images(buffer, buffer_length); } void @@ -95,10 +100,12 @@ PSDKWrapper::camera_setup_streaming_cb( static_cast(request->payload_index); selected_camera_source_ = static_cast(request->camera_source); - RCLCPP_INFO( - get_logger(), - "Setting up camera streaming for payload index %d and camera source %d", - payload_index, selected_camera_source_); + decode_stream_ = request->decoded_output; + + RCLCPP_INFO(get_logger(), + "Setting up camera streaming for payload index %d and camera " + "source %d. Output decoded: %d", + payload_index, selected_camera_source_); if (request->start_stop) { @@ -107,15 +114,15 @@ PSDKWrapper::camera_setup_streaming_cb( if (payload_index == DJI_LIVEVIEW_CAMERA_POSITION_NO_1) { char main_camera_name[] = "MAIN_CAMERA"; - streaming_result = start_main_camera_stream( - &c_publish_main_streaming_callback, &main_camera_name, payload_index, + streaming_result = start_camera_stream(&c_publish_main_streaming_callback, + &main_camera_name, payload_index, selected_camera_source_); } else if (payload_index == DJI_LIVEVIEW_CAMERA_POSITION_FPV) { char fpv_camera_name[] = "FPV_CAMERA"; - streaming_result = start_main_camera_stream( - &c_publish_fpv_streaming_callback, &fpv_camera_name, payload_index, + streaming_result = start_camera_stream(&c_publish_fpv_streaming_callback, + &fpv_camera_name, payload_index, selected_camera_source_); } @@ -147,40 +154,39 @@ PSDKWrapper::camera_setup_streaming_cb( } bool -PSDKWrapper::start_main_camera_stream(CameraImageCallback callback, - void *user_data, +PSDKWrapper::start_camera_stream(CameraImageCallback callback, void *user_data, E_DjiLiveViewCameraPosition payload_index, E_DjiLiveViewCameraSource camera_source) { - auto decoder = stream_decoder.find(payload_index); - - if ((decoder != stream_decoder.end()) && decoder->second) + if (decode_stream_) { - decoder->second->init(); - decoder->second->registerCallback(callback, user_data); - - T_DjiReturnCode return_code = DjiLiveview_StartH264Stream( - payload_index, camera_source, c_LiveviewConvertH264ToRgbCallback); - - if (return_code != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS) + auto decoder = stream_decoder.find(payload_index); + if ((decoder != stream_decoder.end()) && decoder->second) { - RCLCPP_ERROR(get_logger(), - "Failed to start camera streaming, error code: %ld.", - return_code); - return false; + decoder->second->init(); + decoder->second->registerCallback(callback, user_data); } else { - RCLCPP_INFO(get_logger(), "Successfully started the camera streaming."); - return true; + RCLCPP_ERROR(get_logger(), "Failed to set-up the decoder"); + return false; } } - else + + T_DjiReturnCode return_code = DjiLiveview_StartH264Stream( + payload_index, camera_source, c_LiveviewConvertH264ToRgbCallback); + if (return_code != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS) { - RCLCPP_ERROR(get_logger(), "Failed to decode streaming, error code: %ld", - DJI_ERROR_SYSTEM_MODULE_CODE_NOT_FOUND); + RCLCPP_ERROR(get_logger(), + "Failed to start camera streaming, error code: %ld.", + return_code); return false; } + else + { + RCLCPP_INFO(get_logger(), "Successfully started the camera streaming."); + return true; + } } bool @@ -209,35 +215,59 @@ PSDKWrapper::stop_main_camera_stream( } } +void +PSDKWrapper::publish_main_camera_images(const uint8_t *buffer, + uint32_t buffer_length) +{ + auto img = std::make_unique(); + img->encoding = "h264"; + img->data = std::vector(buffer, buffer + buffer_length); + img->header.stamp = this->get_clock()->now(); + img->header.frame_id = get_optical_frame_id(); + main_camera_stream_pub_->publish(std::move(img)); +} + +void +PSDKWrapper::publish_fpv_camera_images(const uint8_t *buffer, + uint32_t buffer_length) +{ + auto img = std::make_unique(); + img->encoding = "h264"; + img->data = std::vector(buffer, buffer + buffer_length); + img->header.stamp = this->get_clock()->now(); + img->header.frame_id = "fpv_camera_link"; + main_camera_stream_pub_->publish(std::move(img)); +} + void PSDKWrapper::publish_main_camera_images(CameraRGBImage rgb_img, void *user_data) { (void)user_data; - sensor_msgs::msg::Image img; - img.height = rgb_img.height; - img.width = rgb_img.width; - img.step = rgb_img.width * 3; - img.encoding = "rgb8"; - img.data = rgb_img.rawData; - - img.header.stamp = this->get_clock()->now(); - img.header.frame_id = get_optical_frame_id(); - main_camera_stream_pub_->publish(img); + auto img = std::make_unique(); + img->height = rgb_img.height; + img->width = rgb_img.width; + img->step = rgb_img.width * 3; + img->encoding = "rgb8"; + img->data = rgb_img.rawData; + + img->header.stamp = this->get_clock()->now(); + img->header.frame_id = get_optical_frame_id(); + main_camera_stream_pub_->publish(std::move(img)); } void PSDKWrapper::publish_fpv_camera_images(CameraRGBImage rgb_img, void *user_data) { (void)user_data; - sensor_msgs::msg::Image img; - img.height = rgb_img.height; - img.width = rgb_img.width; - img.step = rgb_img.width * 3; - img.encoding = "rgb8"; - img.data = rgb_img.rawData; - - img.header.stamp = this->get_clock()->now(); - img.header.frame_id = "fpv_camera_link"; - fpv_camera_stream_pub_->publish(img); + auto img = std::make_unique(); + img->height = rgb_img.height; + img->width = rgb_img.width; + img->step = rgb_img.width * 3; + img->encoding = "rgb8"; + img->data = rgb_img.rawData; + + img->header.stamp = this->get_clock()->now(); + img->header.frame_id = "fpv_camera_link"; + fpv_camera_stream_pub_->publish(std::move(img)); } } // namespace psdk_ros2 diff --git a/psdk_wrapper/src/psdk_wrapper.cpp b/psdk_wrapper/src/psdk_wrapper.cpp index 6d1f6a9b..45ee9ffa 100644 --- a/psdk_wrapper/src/psdk_wrapper.cpp +++ b/psdk_wrapper/src/psdk_wrapper.cpp @@ -21,9 +21,9 @@ using namespace std::placeholders; // NOLINT namespace psdk_ros2 { - PSDKWrapper::PSDKWrapper(const std::string &node_name) - : rclcpp_lifecycle::LifecycleNode(node_name, "", rclcpp::NodeOptions()) + : rclcpp_lifecycle::LifecycleNode( + node_name, "", rclcpp::NodeOptions().use_intra_process_comms(true)) { RCLCPP_INFO(get_logger(), "Creating Constructor PSDKWrapper"); declare_parameter("app_name", rclcpp::ParameterValue("")); @@ -787,9 +787,9 @@ PSDKWrapper::initialize_ros_elements() create_publisher( "psdk_ros2/acceleration_body_raw", 10); main_camera_stream_pub_ = create_publisher( - "psdk_ros2/main_camera_stream", 10); + "psdk_ros2/main_camera_stream", rclcpp::SensorDataQoS()); fpv_camera_stream_pub_ = create_publisher( - "psdk_ros2/fpv_camera_stream", 10); + "psdk_ros2/fpv_camera_stream", rclcpp::SensorDataQoS()); control_mode_pub_ = create_publisher( "psdk_ros2/control_mode", 10); home_point_pub_ =