add mpp hardware decode mjpeg

orbbec_camera/include/orbbec_camera/mjpeg_decoder.h

@@ -0,0 +1,24 @@
+#pragma once
+
+#include <string>
+#include <vector>
+#include "libobsensor/ObSensor.hpp"
+
+namespace orbbec_camera {
+class MjpegDecoder {
+ public:
+  MjpegDecoder(int width, int height);
+
+  virtual ~MjpegDecoder();
+
+  virtual bool decode(const std::shared_ptr<ob::ColorFrame> &frame, uint8_t *dest) = 0;
+
+  std::string getErrorMsg() const { return error_msg_; }
+
+ protected:
+  int width_ = 0;
+  int height_ = 0;
+  uint8_t *rgb_buffer_ = nullptr;
+  std::string error_msg_;
+};
+}  // namespace orbbec_camera

orbbec_camera/include/orbbec_camera/ob_camera_node.h

@@ -56,6 +56,7 @@
 #include "orbbec_camera/dynamic_params.h"
 #include "orbbec_camera/d2c_viewer.h"
 #include "magic_enum/magic_enum.hpp"
+#include "mjpeg_decoder.h"
 
 #define STREAM_NAME(sip)                                                                       \
   (static_cast<std::ostringstream&&>(std::ostringstream()                                      \
@@ -156,8 +157,8 @@ class OBCameraNode {
 
   void setupPublishers();
 
-  void publishStaticTF(const rclcpp::Time& t, const tf2::Vector3& trans,
-                       const tf2::Quaternion& q, const std::string& from, const std::string& to);
+  void publishStaticTF(const rclcpp::Time& t, const tf2::Vector3& trans, const tf2::Quaternion& q,
+                       const std::string& from, const std::string& to);
 
   void calcAndPublishStaticTransform();
 
@@ -254,6 +255,8 @@ class OBCameraNode {
 
   void onNewFrameSetCallback(const std::shared_ptr<ob::FrameSet>& frame_set);
 
+  std::shared_ptr<ob::Frame> softwareDecodeColorFrame(const std::shared_ptr<ob::Frame>& frame);
+
   void onNewFrameCallback(const std::shared_ptr<ob::Frame>& frame,
                           const stream_index_pair& stream_index);
 
@@ -398,5 +401,8 @@ class OBCameraNode {
   double angular_vel_cov_ = 0.0001;
   std::deque<IMUData> imu_history_;
   IMUData accel_data_{ACCEL, {0, 0, 0}, -1.0};
+  // mjpeg decoder
+  std::shared_ptr<MjpegDecoder> mjpeg_decoder_ = nullptr;
+  uint8_t* rgb_buffer_ = nullptr;
 };
 }  // namespace orbbec_camera

orbbec_camera/include/orbbec_camera/rk_mpp_decoder.h

@@ -0,0 +1,41 @@
+#pragma once
+
+#include "mjpeg_decoder.h"
+#include <rga/RgaApi.h>
+#include <rockchip/mpp_buffer.h>
+#include <rockchip/mpp_err.h>
+#include <rockchip/mpp_frame.h>
+#include <rockchip/mpp_log.h>
+#include <rockchip/mpp_packet.h>
+#include <rockchip/mpp_rc_defs.h>
+#include <rockchip/mpp_task.h>
+#include <rockchip/rk_mpi.h>
+#define MPP_ALIGN(x, a) (((x) + (a)-1) & ~((a)-1))
+
+namespace orbbec_camera {
+class RKMjpegDecoder : public MjpegDecoder {
+ public:
+  RKMjpegDecoder(int width, int height);
+
+  ~RKMjpegDecoder() override;
+
+  bool decode(const std::shared_ptr<ob::ColorFrame>& frame, uint8_t* dest) override;
+
+  bool mppFrame2RGB(const MppFrame frame, uint8_t* data);
+
+ private:
+  MppCtx mpp_ctx_ = nullptr;
+  MppApi* mpp_api_ = nullptr;
+  MppPacket mpp_packet_ = nullptr;
+  MppFrame mpp_frame_ = nullptr;
+  MppDecCfg mpp_dec_cfg_ = nullptr;
+  MppBuffer mpp_frame_buffer_ = nullptr;
+  MppBuffer mpp_packet_buffer_ = nullptr;
+  uint8_t* data_buffer_ = nullptr;
+  MppBufferGroup mpp_frame_group_ = nullptr;
+  MppBufferGroup mpp_packet_group_ = nullptr;
+  MppTask mpp_task_ = nullptr;
+  uint32_t need_split_ = 0;
+};
+
+}  // namespace orbbec_camera