Enable encoded and decoded stream publishing

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

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};
 };
 
 /**

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<E_DjiLiveViewCameraPosition>(request->payload_index);
   selected_camera_source_ =
       static_cast<E_DjiLiveViewCameraSource>(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<sensor_msgs::msg::Image>();
+  img->encoding = "h264";
+  img->data = std::vector<uint8_t>(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<sensor_msgs::msg::Image>();
+  img->encoding = "h264";
+  img->data = std::vector<uint8_t>(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<sensor_msgs::msg::Image>();
+  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<sensor_msgs::msg::Image>();
+  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

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<geometry_msgs::msg::AccelStamped>(
           "psdk_ros2/acceleration_body_raw", 10);
   main_camera_stream_pub_ = create_publisher<sensor_msgs::msg::Image>(
-      "psdk_ros2/main_camera_stream", 10);
+      "psdk_ros2/main_camera_stream", rclcpp::SensorDataQoS());
   fpv_camera_stream_pub_ = create_publisher<sensor_msgs::msg::Image>(
-      "psdk_ros2/fpv_camera_stream", 10);
+      "psdk_ros2/fpv_camera_stream", rclcpp::SensorDataQoS());
   control_mode_pub_ = create_publisher<psdk_interfaces::msg::ControlMode>(
       "psdk_ros2/control_mode", 10);
   home_point_pub_ =